diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h index 45b7d3947aacaf4b3f927ef9b6a468ab7a3dddf9..8239a574a406e14cde7a6b30272cfbbe3f86268c 100644 --- a/homer_navigation/include/homer_navigation/homer_navigation_node.h +++ b/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; diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp index bc4f5a181aff8ccac5e7170d225e0cd17474fb87..e56dca631c484a434ca3d2172571b2fa36840dc8 100644 --- a/homer_navigation/src/homer_navigation_node.cpp +++ b/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)");