Introduce parameter to change behaviour after collision avoidance

homer_navigation/include/homer_navigation/homer_navigation_node.h

@@ -271,6 +271,10 @@ class HomerNavigationNode {
    * check_path_max_distance to robot */
   float m_check_path_max_distance;
 
+  /** do not replan if lisa avoids an obstacle, instead send target
+   * unreachable*/
+  bool m_no_replanning_on_collision;
+
   bool m_avoided_collision;
 
   float m_min_turn_angle;

homer_navigation/src/homer_navigation_node.cpp

@@ -97,6 +97,8 @@ void HomerNavigationNode::loadParameters() {
                     m_collision_distance_near_target, (float)0.2);
   ros::param::param("/homer_navigation/backward_collision_distance",
                     m_backward_collision_distance, (float)0.5);
+  ros::param::param("/homer_navigation/no_replanning_on_collision",
+                    m_no_replanning_on_collision, (bool)false);
 
   // cmd_vel config values
   ros::param::param("/homer_navigation/min_turn_angle", m_min_turn_angle,
@@ -266,6 +268,13 @@ void HomerNavigationNode::startNavigation() {
     targetPositionReached();
     return;
   }
+  if (m_initial_path_reaches_target && m_no_replanning_on_collision) {
+    ROS_INFO_STREAM(
+        "Collision avoided and option no_replanning_on_collision is set.");
+    ROS_INFO_STREAM("Sending target unreachable.");
+    sendTargetUnreachableMsg(
+        homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
+  }
   ROS_INFO_STREAM("Distance to target still too large ("
                   << m_distance_to_target
                   << "m; requested: " << m_desired_distance << "m)");