Replaced occurance of path planning with newly dedicated path planning function call

include/homer_navigation/homer_navigation_node.h

@@ -75,6 +75,9 @@ protected:
 
     void initNewTarget();
 
+    bool planPath(
+        const geometry_msgs::Point& A, const geometry_msgs::Point& B, nav_msgs::Path& path);
+
 private:
     /** @brief Start navigation to m_Target on  last_map_data_ */
     void startNavigation();
@@ -173,7 +176,8 @@ private:
     static float rad2Deg(float rad) { return rad / M_PI * 180.0; }
 
     bool drawPolygon(std::vector<geometry_msgs::Point> vertices);
-    void generateLine(std::vector<Eigen::Vector2i>& coordinates, const Eigen::Vector2i& start, const Eigen::Vector2i& end);
+    void generateLine(std::vector<Eigen::Vector2i>& coordinates, const Eigen::Vector2i& start,
+        const Eigen::Vector2i& end);
     void drawLine(std::vector<int>& data, int startX, int startY, int endX, int endY, int value);
     bool fillPolygon(std::vector<int>& data, int x, int y, int value);
 

src/homer_navigation_node.cpp

@@ -238,16 +238,19 @@ void HomerNavigationNode::setExplorerMap()
     m_explorer->setOccupancyMap(tmp_map);
 }
 
-void HomerNavigationNode::planPath(
-    const geometry_msgs::Point& A, const geometry_msgs::Point& B, nav_msgs::Path& msg)
+bool HomerNavigationNode::planPath(
+    const geometry_msgs::Point& A, const geometry_msgs::Point& B, nav_msgs::Path& path)
 {
-    msg.header.frame_id = "map";
-    msg.header.stamp = ros::Time::now();
+    path.header.frame_id = "map";
+    path.header.stamp = ros::Time::now();
 
     m_explorer->setStart(map_tools::toMapCoords(A, m_map));
     m_explorer->setTarget(map_tools::toMapCoords(B, m_map));
+
+    // NOTE (DM): Funny that bool is passed by reference but list of vectors is returned as copy? :D
     bool success;
     std::vector<Eigen::Vector2i> pixel_path = m_explorer->getPath(success);
+
     m_sample_path = false;
     m_nh.getParamCached("sample_path", m_sample_path);
     const auto& waypoint_pixels = (m_sample_path)
@@ -255,37 +258,30 @@ void HomerNavigationNode::planPath(
         : pixel_path;
 
     if (waypoint_pixels.empty())
-        return;
+        return false;
 
     geometry_msgs::PoseStamped ps;
-    ps.header.frame_id = "map";
+    ps.header = path.header;
     ps.pose.orientation.w = 1.0;
-
     for (const auto& wp : waypoint_pixels)
     {
         ps.pose.position = map_tools::fromMapCoords(wp, m_map);
-        waypoints.push_back(ps);
+        path.poses.push_back(ps);
     }
 
-    path_reaches_target
-        = map_tools::distance(new_target_approx_world, m_target_point) < m_desired_distance;
-
-    // TODO: Check locally!
-    std::vector<geometry_msgs::PoseStamped> waypoints;
+    Eigen::Vector2i new_target_approx
+        = m_explorer->getNearestAccessibleTarget(map_tools::toMapCoords(B, m_map));
+    geometry_msgs::Point new_target_approx_world
+        = map_tools::fromMapCoords(new_target_approx, m_map);
+    // Add goal point as last entry if path is close enough
+    const bool path_reaches_target
+        = map_tools::distance(new_target_approx_world, B) < m_desired_distance;
     if (path_reaches_target)
     {
-        ps.pose.position = m_target_point;
-        waypoints.push_back(ps);
-    }
-
-    if (m_waypoints.size() > 0)
-    {
-        msg.poses = m_waypoints;
-        geometry_msgs::PoseStamped pose_stamped;
-        pose_stamped.pose = m_robot_pose;
-        pose_stamped.header = msg.header;
-        msg.poses.insert(msg.poses.begin(), pose_stamped);
+        ps.pose.position = B;
+        path.poses.push_back(ps);
     }
+    return true;
 }
 
 void HomerNavigationNode::calculatePath(bool set_map)
@@ -298,10 +294,8 @@ void HomerNavigationNode::calculatePath(bool set_map)
     if (set_map)
         setExplorerMap();
 
-    m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
-
-    bool success;
-    std::vector<Eigen::Vector2i> tmp_pixel_path = m_explorer->getPath(success);
+    nav_msgs::Path path;
+    const bool success = planPath(m_robot_pose.position, m_target_point, path);
     if (!success)
     {
         ROS_WARN_STREAM("no path to target possible - drive to obstacle");
@@ -309,35 +303,12 @@ void HomerNavigationNode::calculatePath(bool set_map)
         return;
     }
 
-    ROS_DEBUG_STREAM("Found tmp path with " << tmp_pixel_path.size() << " points.");
-    m_obstacle_on_path = false;
-
-    m_sample_path = false;
-    m_nh.getParamCached("sample_path", m_sample_path);
-    const std::vector<Eigen::Vector2i>& waypoint_pixels = (m_sample_path)
-        ? m_explorer->sampleWaypointsFromPath(tmp_pixel_path, m_waypoint_sampling_threshold)
-        : tmp_pixel_path;
-
+    // NOTE: These are used when sending path msg in sendPathData...
     m_waypoints.clear();
-    ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << waypoint_pixels.size());
-    if (waypoint_pixels.size() > 0)
-    {
-        geometry_msgs::PoseStamped ps;
-        ps.header.frame_id = "map";
-        ps.pose.orientation.w = 1.0;
-
-        for (const auto& wp : waypoint_pixels)
-        {
-            ps.pose.position = map_tools::fromMapCoords(wp, m_map);
-            m_waypoints.push_back(ps);
-        }
-        if (m_path_reaches_target)
-        {
-            ps.pose.position = m_target_point;
-            m_waypoints.push_back(ps);
-        }
+    m_waypoints = path.poses;
+    ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << m_waypoints.size());
+    if (!m_waypoints.empty())
         sendPathData();
-    }
     else
         sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
 
@@ -391,6 +362,7 @@ void HomerNavigationNode::startNavigation()
         }
     }
 
+    // TODO: Replace with planPath
     m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
     Eigen::Vector2i new_target_approx
         = m_explorer->getNearestAccessibleTarget(map_tools::toMapCoords(m_target_point, m_map));