Fix: Added missing startNavigation call; further renaming

include/homer_navigation/homer_navigation_node.h

@@ -88,7 +88,7 @@ private:
 
     float getMinLaserDistance();
     void followPath();
-    void avoidingCollision();
+    void avoidCollision();
     void performFinalTurn();
     bool updateNextWaypoint();
     bool updateSpeeds();
@@ -177,7 +177,7 @@ private:
 
 
     /** @brief calcs the maximal move distance from Laser and DepthData */
-    void calcMaxMoveDist();
+    void updateMaxMoveDistance();
 
     void publishObstacleMarker(const cv::Vec3f& color);
     void resetObstacleMarker();

src/homer_navigation_node.cpp

@@ -208,6 +208,8 @@ void HomerNavigationNode::idleProcess()
     // ---
 
     // ---
+    // TODO: Use full state machine class which automatically prints to terminal, if needed?
+
     // Continue to follow path, avoid obstacles or final turning
     if (m_state == IDLE)
         return;
@@ -424,28 +426,6 @@ void HomerNavigationNode::startNavigation()
 
     updateExplorerMap();
 
-    /*
-    // check if there still exists a path to the original target
-    if (m_avoided_collision && m_initial_path_reaches_target && m_stop_before_obstacle)
-    {
-        m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
-        // m_explorer->setTarget(map_tools::toMapCoords(m_target_point, m_map));
-        m_explorer->setTarget(
-            map_tools::toMapCoords(m_last_path.poses.back().pose.position, m_map));
-
-        bool success;
-        std::vector<Eigen::Vector2i> tmp_pixel_path = m_explorer->getPath(success);
-        if (!success)
-        {
-            ROS_INFO_STREAM("Initial path would have reached target, new path does not. "
-                << "Sending target unreachable.");
-            sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
-            return;
-        }
-    }
-    */
-
-    // m_explorer->setTarget(new_target_approx);
     m_state = FOLLOWING_PATH;
     ROS_INFO_STREAM("Switched to state FOLLOWING_PATH");
 
@@ -1253,7 +1233,7 @@ void HomerNavigationNode::poseCallback(const geometry_msgs::PoseStamped::ConstPt
     m_last_pose_time = msg->header.stamp; // ros::Time::now();
 }
 
-void HomerNavigationNode::calcMaxMoveDist()
+void HomerNavigationNode::updateMaxMoveDistance()
 {
     m_max_move_distance = 99;
 
@@ -1285,7 +1265,7 @@ void HomerNavigationNode::calcMaxMoveDist()
 void HomerNavigationNode::maxDepthMoveDistanceCallback(const std_msgs::Float32::ConstPtr& msg)
 {
     m_max_move_distances["depth"] = msg->data;
-    calcMaxMoveDist();
+    updateMaxMoveDistance();
 }
 
 void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
@@ -1314,7 +1294,7 @@ void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstP
         return;
     m_max_move_distances[msg->header.frame_id]
         = map_tools::get_max_move_distance(points, m_min_x, m_min_y);
-    calcMaxMoveDist();
+    updateMaxMoveDistance();
 
     // Make adding current scan to map easier
     m_last_scan_points = map_tools::laser_msg_to_points(msg, m_transform_listener, "map");
@@ -1335,6 +1315,8 @@ void HomerNavigationNode::updateNavigationTarget(
     publishTargetMarker();
 }
 
+//----------------------------------------------------------------------------------------------------------------------
+//----------------------------------------------------------------------------------------------------------------------
 void HomerNavigationNode::startNavigationCallback(
     const homer_mapnav_msgs::StartNavigation::ConstPtr& msg)
 {
@@ -1415,6 +1397,8 @@ void HomerNavigationNode::moveBaseSimpleGoalCallback(
                 q.y = tmp.y();
                 q.z = tmp.z();
                 q.w = tmp.w();
+                
+                updateNavigationTarget(pos, q);
             }
             catch (tf::TransformException ex)
             {
@@ -1437,8 +1421,8 @@ void HomerNavigationNode::moveBaseSimpleGoalCallback(
     ROS_INFO_STREAM(" - desired distance to target: " << m_desired_distance);
     ROS_INFO_STREAM(" - frame_id: " << msg->header.frame_id);
 
-    startNavigation();
     ROS_INFO_STREAM("Callback finished and took: " << (ros::Time::now() - start).toSec());
+    startNavigation();
 }
 
 void HomerNavigationNode::navigateToPOICallback(
@@ -1469,6 +1453,8 @@ void HomerNavigationNode::navigateToPOICallback(
     updateNavigationTarget(poi->pose.position, poi->pose.orientation);
     startNavigation();
 }
+//----------------------------------------------------------------------------------------------------------------------
+//----------------------------------------------------------------------------------------------------------------------
 
 void HomerNavigationNode::stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg)
 {