Added config param to disable path simplification (post-processing), new...

Added config param to disable path simplification (post-processing), new default is disabled simplification for navigating in complex surroundings

config/homer_navigation.yaml

@@ -41,3 +41,4 @@
 /homer_navigation/use_ptu:                        false # bool toggles if the ptu is being used to look at the next Waypoint during navigation
 /homer_navigation/unknown_threshold:              70   # obstacle strenght under which the obstacle is ignored by navigation
 /homer_navigation/waypoint_radius_factor:         0.5 # factor for includance of obstacle map
+/homer_navigation/sample_path:                     false # Downsample the amount of waypoints

include/homer_navigation/homer_navigation_node.h

@@ -315,6 +315,7 @@ private:
     int m_unknown_threshold;
 
     bool m_use_ptu;
+    bool m_sample_path = false;
 
     geometry_msgs::Twist m_cmd_vel;
 

src/homer_navigation_node.cpp

@@ -247,33 +247,27 @@ void HomerNavigationNode::calculatePath(bool setMap)
     {
         ROS_DEBUG_STREAM("Found tmp path with " << tmp_pixel_path.size() << " points.");
         m_obstacle_on_path = false;
-        std::vector<Eigen::Vector2i> waypoint_pixels
-            = m_explorer->sampleWaypointsFromPath(tmp_pixel_path, m_waypoint_sampling_threshold);
+        
+        m_sample_path = false;
+        ros::param::getCached("/homer_navigation/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;
+        
         m_waypoints.clear();
         ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << waypoint_pixels.size());
         if (waypoint_pixels.size() > 0)
         {
-            for (std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin();
-                 it != waypoint_pixels.end(); ++it)
+            geometry_msgs::PoseStamped poseStamped;
+            poseStamped.header.frame_id = "map";
+            poseStamped.pose.orientation.w = 1.0;
+            for(const auto& wp : waypoint_pixels)
             {
-                geometry_msgs::PoseStamped poseStamped;
-                poseStamped.header.frame_id = "map";
-                poseStamped.pose.position = map_tools::fromMapCoords(*it, m_map);
-                poseStamped.pose.orientation.x = 0.0;
-                poseStamped.pose.orientation.y = 0.0;
-                poseStamped.pose.orientation.z = 0.0;
-                poseStamped.pose.orientation.w = 1.0;
+                poseStamped.pose.position = map_tools::fromMapCoords(wp, m_map);
                 m_waypoints.push_back(poseStamped);
             }
             if (m_path_reaches_target)
             {
-                geometry_msgs::PoseStamped poseStamped;
-                poseStamped.header.frame_id = "map";
                 poseStamped.pose.position = m_target_point;
-                poseStamped.pose.orientation.x = 0;
-                poseStamped.pose.orientation.y = 0;
-                poseStamped.pose.orientation.z = 0;
-                poseStamped.pose.orientation.w = 1;
                 m_waypoints.push_back(poseStamped);
             }
             sendPathData();
@@ -416,7 +410,7 @@ void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason)
     }
     else if (ros::Time::now() - m_unreachable_delay > ros::Duration(unreachable_duration))
     {
-        bool m_use_ptu = false;
+        m_use_ptu = false;
         ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
         if (m_use_ptu)
         {
@@ -854,7 +848,7 @@ bool HomerNavigationNode::checkWaypoints()
 
     sendPathData();
 
-    bool m_use_ptu = false;
+    m_use_ptu = false;
     ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
     if (m_use_ptu)
     {
@@ -1121,7 +1115,7 @@ void HomerNavigationNode::avoidingCollision()
 
 void HomerNavigationNode::finalTurn()
 {
-    bool m_use_ptu = false;
+    m_use_ptu = false;
     ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
     if (m_use_ptu)
     {