Merge branch 'feature/obstacle_avoidance' into 'master'

Clear obstacle if it moves away

See merge request !3

homer_navigation/src/homer_navigation_node.cpp

@@ -84,10 +84,6 @@ void HomerNavigationNode::loadParameters() {
                       (double)1.2);
     ros::param::param("/homer_navigation/waypoint_sampling_threshold",
                       m_waypoint_sampling_threshold, (float)1.5);
-    m_AllowedObstacleDistance.first;
-    m_AllowedObstacleDistance.second;
-    m_SafeObstacleDistance.first;
-    m_SafeObstacleDistance.second;
 
     // check path
     ros::param::param("/homer_navigation/check_path", m_check_path, (bool)true);
@@ -516,6 +512,7 @@ void HomerNavigationNode::performNextMove() {
                 m_seen_obstacle_before = true;
             } else {
                 m_seen_obstacle_before = false;
+                m_obstacle_on_path = false;
             }
         }
 
@@ -722,8 +719,8 @@ void HomerNavigationNode::performNextMove() {
     } else if (m_MainMachine.state() == AVOIDING_COLLISION) {
         if (isTargetPositionReached()) {
             return;
-        } else if (m_max_move_distance <= m_collision_distance &&
-                       m_waypoints.size() > 1 ||
+        } else if ((m_max_move_distance <= m_collision_distance &&
+                    m_waypoints.size() > 1) ||
                    m_max_move_distance <= m_collision_distance_near_target) {
             ROS_WARN_STREAM("Maximum driving distance too short ("
                             << m_max_move_distance << "m)! Moving back.");
@@ -1026,6 +1023,7 @@ float HomerNavigationNode::minTurnAngle(float angle1, float angle2) {
 
 void HomerNavigationNode::refreshParamsCallback(
     const std_msgs::Empty::ConstPtr& msg) {
+    (void) msg;
     ROS_INFO_STREAM("Refreshing Parameters");
     loadParameters();
 }
@@ -1063,10 +1061,10 @@ void HomerNavigationNode::calcMaxMoveDist() {
     for (auto d : m_max_move_distances) {
         m_max_move_distance = std::min(m_max_move_distance, d.second);
     }
-    if (m_max_move_distance <= m_collision_distance &&
-            std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1 ||
-        m_max_move_distance <= m_collision_distance_near_target &&
-            std::fabs(m_act_speed) > 0.1 && m_waypoints.size() == 1 ||
+    if ((m_max_move_distance <= m_collision_distance &&
+         std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1) ||
+        (m_max_move_distance <= m_collision_distance_near_target &&
+         std::fabs(m_act_speed) > 0.1 && m_waypoints.size() == 1) ||
         m_max_move_distance <= 0.1) {
         if (m_MainMachine.state() == FOLLOWING_PATH) {
             stopRobot();
@@ -1226,6 +1224,7 @@ void HomerNavigationNode::navigateToPOICallback(
 
 void HomerNavigationNode::stopNavigationCallback(
     const std_msgs::Empty::ConstPtr& msg) {
+    (void) msg;
     ROS_INFO_STREAM("Stopping navigation.");
     m_MainMachine.setState(IDLE);
     stopRobot();