Call performNextMove in idleProcess instead of pose callbacks; better update...

Call performNextMove in idleProcess instead of pose callbacks; better update of current m_distance_to_target;Added target marker

include/homer_navigation/homer_navigation_node.h

@@ -13,11 +13,12 @@
 #include <opencv2/opencv.hpp>
 #include <tf/transform_listener.h>
 
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Twist.h>
 #include <homer_mapnav_msgs/NavigateToPOI.h>
 #include <homer_mapnav_msgs/StartNavigation.h>
 #include <homer_mapnav_msgs/TargetUnreachable.h>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/Path.h>
 #include <sensor_msgs/LaserScan.h>
@@ -29,7 +30,6 @@
 #include <homer_nav_libs/Explorer/Explorer.h>
 #include <homer_nav_libs/tools.h>
 
-class Explorer;
 /**
  * @class  HomerNavigationNode
  * @author Malte Knauf, Stephan Wirth, David Gossow (RX), Florian Polster
@@ -46,7 +46,9 @@ public:
         IDLE,
         FOLLOWING_PATH,
         AVOIDING_COLLISION,
-        FINAL_TURN
+        FINAL_TURN,
+        TIMEOUT_POSE,
+        TIMEOUT_LASER
     };
     ProcessState m_state;
 
@@ -90,7 +92,6 @@ private:
     bool updateSpeeds();
     bool checkForObstacles();
 
-    geometry_msgs::Point calculateMeanPoint(const std::vector<geometry_msgs::Point>& points);
     /** @brief Check if obstacles are blocking the way in last_map_data_ */
     bool obstacleOnPath();
 
@@ -123,7 +124,6 @@ private:
     /** @brief Finishes navigation or starts turning to target direction if the
      * target position has been reached */
     bool isTargetPositionReached();
-    bool m_new_target;
 
     /** @return Angle from robot_pose_ to point in degrees */
     int angleToPointDeg(const geometry_msgs::Point& point);
@@ -181,9 +181,10 @@ private:
 
     void publishObstacleMarker(const cv::Vec3f& color);
     void resetObstacleMarker();
+    void publishTargetMarker();
 
-    //---------------------------------------------------------
-    ros::NodeHandle m_nh;
+        //---------------------------------------------------------
+        ros::NodeHandle m_nh;
 
     /// @brief Worker instances
     std::unique_ptr<Explorer> m_explorer;

src/homer_navigation_node.cpp

@@ -11,7 +11,8 @@
 
 #define MARKER_ID_OBSTACLE 0
 #define MARKER_ID_OBSTACLE_TEXT 1
-
+#define MARKER_ID_TARGET 2
+#define MARKER_ID_TARGET_TEXT 3
 
 //----------------------------------------------------------------------------------------------------------------------
 std::string to_string(const geometry_msgs::Point& p)
@@ -194,18 +195,42 @@ void HomerNavigationNode::idleProcess()
     if (m_state == IDLE)
         return;
 
-    const auto dt_laser = ros::Time::now() - m_last_laser_time;
-    if (dt_laser > ros::Duration(m_callback_error_duration))
+    // ---
+    // Check if data timed out
+    const auto now = ros::Time::now();
+    const auto dt_laser = now - m_last_laser_time;
+    const bool is_laser_timeout = dt_laser > ros::Duration(m_callback_error_duration);
+    const auto dt_pose = now - m_last_pose_time;
+    const bool is_pose_timeout = dt_pose > ros::Duration(m_callback_error_duration);
+    if (is_laser_timeout)
     {
         ROS_ERROR_THROTTLE(1, "Laser data timeout! Last msg received: %fs", dt_laser.toSec());
+        m_state = TIMEOUT_LASER;
         stopRobot();
     }
-    const auto dt_pose = ros::Time::now() - m_last_pose_time;
-    if (dt_pose > ros::Duration(m_callback_error_duration))
+    if (is_pose_timeout)
     {
         ROS_ERROR_THROTTLE(1, "Pose timeout! Last msg received: %fs", dt_pose.toSec());
+        m_state = TIMEOUT_POSE;
         stopRobot();
     }
+
+    if (m_state == TIMEOUT_LASER || m_state == TIMEOUT_POSE)
+    {
+        if (!is_laser_timeout && !is_pose_timeout)
+        {
+            ROS_INFO_STREAM("Exiting pose and/or laser timeout state. Recalculating path...");
+            m_state = FOLLOWING_PATH;
+        }
+        else
+            return;
+    }
+    // ---
+
+    // ---
+    // Continue to follow path, avoid obstacles or final turning
+    performNextMove();
+    // ---
 }
 
 void HomerNavigationNode::setExplorerMap()
@@ -325,6 +350,7 @@ bool HomerNavigationNode::planPath(
     return true;
 }
 
+// TODO: Always calculate path to a set target point, remove input arguments?
 void HomerNavigationNode::calculatePath(const geometry_msgs::Point& target, bool set_map)
 {
     if (!m_explorer || isTargetPositionReached())
@@ -348,6 +374,7 @@ void HomerNavigationNode::calculatePath(const geometry_msgs::Point& target, bool
     if (!success)
     {
         ROS_WARN_STREAM("no path to target possible - drive to obstacle");
+        // TODO: Rename to something like m_path_blocked?
         m_obstacle_on_path = true;
         return;
     }
@@ -382,11 +409,16 @@ void HomerNavigationNode::calculatePath(const geometry_msgs::Point& target, bool
     m_waypoints = path.poses;
     ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << m_waypoints.size());
     if (!m_waypoints.empty())
+    {
         sendPathData();
+        m_state = FOLLOWING_PATH;
+    }
     else
         sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
 
     // TODO (DM): This doesn't really make sense?
+    // It updates stamp which is used for checking for pose/laser timeouts, spin not called often
+    // enough to really perform update concurrently
     m_last_laser_time = ros::Time::now();
     m_last_pose_time = ros::Time::now();
 }
@@ -420,6 +452,8 @@ void HomerNavigationNode::startNavigation()
         << m_distance_to_target << "m --> requested: " << m_desired_distance << "m");
 
     setExplorerMap();
+
+    /*
     // check if there still exists a path to the original target
     if (m_avoided_collision && m_initial_path_reaches_target && m_stop_before_obstacle)
     {
@@ -438,6 +472,7 @@ void HomerNavigationNode::startNavigation()
             return;
         }
     }
+    */
 
     // m_explorer->setTarget(new_target_approx);
     m_state = FOLLOWING_PATH;
@@ -523,10 +558,12 @@ void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason)
 
 bool HomerNavigationNode::isTargetPositionReached()
 {
-    if (m_distance_to_target >= m_desired_distance || m_new_target)
+    if (m_distance_to_target >= m_desired_distance)
         return false;
     ROS_INFO_STREAM("Target position reached. Distance to target: "
         << m_distance_to_target << "m (desired:" << m_desired_distance << "m)");
+    ROS_INFO_STREAM("robot position: " << to_string(m_robot_pose.position));
+    ROS_INFO_STREAM("target position: " << to_string(m_target_point));
     stopRobot();
     m_waypoints.clear();
     m_state = FINAL_TURN;
@@ -534,22 +571,6 @@ bool HomerNavigationNode::isTargetPositionReached()
     return true;
 }
 
-geometry_msgs::Point HomerNavigationNode::calculateMeanPoint(
-    const std::vector<geometry_msgs::Point>& points)
-{
-    geometry_msgs::Point mean_point;
-    if (points.empty())
-        return mean_point;
-    for (auto const& point : points)
-    {
-        mean_point.x += point.x;
-        mean_point.y += point.y;
-    }
-    mean_point.x /= points.size();
-    mean_point.y /= points.size();
-    return mean_point;
-}
-
 bool HomerNavigationNode::obstacleOnPath()
 {
     m_last_check_path_time = ros::Time::now();
@@ -840,6 +861,7 @@ bool HomerNavigationNode::checkWaypoints()
     }
     if (nearestWaypoint != 0)
     {
+        /*
         // if the target is nearer than the waypoint don't recalculate
         if (m_waypoints.size() != 2)
         {
@@ -854,6 +876,9 @@ bool HomerNavigationNode::checkWaypoints()
         {
             m_waypoints.erase(m_waypoints.begin());
         }
+        */
+        ROS_WARN_STREAM("Skipped " << nearestWaypoint << " waypoints.");
+        m_waypoints.erase(m_waypoints.begin(), m_waypoints.begin() + nearestWaypoint);
     }
 
     Eigen::Vector2i waypointPixel = map_tools::toMapCoords(m_waypoints[0].pose.position, m_map);
@@ -1106,8 +1131,7 @@ void HomerNavigationNode::avoidingCollision()
     {
         float min_distance_to_last_waypoint = 0.3;
         m_nh.getParamCached("min_distance_to_last_waypoint", min_distance_to_last_waypoint);
-        if (map_tools::distance(
-                m_robot_pose.position, m_waypoints[m_waypoints.size() - 1].pose.position)
+        if (map_tools::distance(m_robot_pose.position, m_waypoints.back().pose.position)
             < min_distance_to_last_waypoint)
         {
             ROS_INFO_STREAM_THROTTLE(1,
@@ -1224,6 +1248,9 @@ void HomerNavigationNode::finalTurn()
 
 void HomerNavigationNode::performNextMove()
 {
+    if (m_state == IDLE)
+        return;
+
     if (m_state == FOLLOWING_PATH)
     {
         ROS_DEBUG_STREAM("performNextMove: followPath");
@@ -1379,9 +1406,7 @@ void HomerNavigationNode::poseCallback(const geometry_msgs::PoseStamped::ConstPt
     m_robot_pose = msg->pose;
     // TODO: Use message's stamp?
     m_last_pose_time = ros::Time::now();
-    m_new_target = false;
     m_distance_to_target = map_tools::distance(m_robot_pose.position, m_target_point);
-    performNextMove();
 }
 
 void HomerNavigationNode::calcMaxMoveDist()
@@ -1434,22 +1459,19 @@ void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstP
     }
     m_scan_map[msg->header.frame_id] = tmp_scan;
 
+    m_last_laser_time = ros::Time::now();
     if (m_state == IDLE)
-    {
-        m_last_laser_time = ros::Time::now();
         return;
-    }
 
     std::vector<geometry_msgs::Point> points
         = map_tools::laser_msg_to_points(msg, m_transform_listener, "base_link");
-    if (points.size() > 0)
-    {
-        m_last_laser_time = ros::Time::now();
-    }
+    if (points.size() < 0)
+        return;
     m_max_move_distances[msg->header.frame_id]
         = map_tools::get_max_move_distance(points, m_min_x, m_min_y);
     calcMaxMoveDist();
 
+    // Make adding current scan to map easier
     m_last_scan_points = map_tools::laser_msg_to_points(msg, m_transform_listener, "map");
 }
 
@@ -1457,8 +1479,11 @@ void HomerNavigationNode::initNewTarget()
 {
     m_initial_path_reaches_target = false;
     m_avoided_collision = false;
-    m_new_target = true;
+    m_distance_to_target = map_tools::distance(m_robot_pose.position, m_target_point);
+
+    publishTargetMarker();
 }
+
 void HomerNavigationNode::startNavigationCallback(
     const homer_mapnav_msgs::StartNavigation::ConstPtr& msg)
 {
@@ -1473,6 +1498,7 @@ void HomerNavigationNode::startNavigationCallback(
     ROS_INFO_STREAM(" - (x, y): (" << m_target_point.x << ", " << m_target_point.y << ")");
     ROS_INFO_STREAM(" - orientation: " << m_target_orientation);
     ROS_INFO_STREAM(" - desired distance to target: " << m_desired_distance);
+
     initNewTarget();
     startNavigation();
 }
@@ -1574,6 +1600,7 @@ void HomerNavigationNode::navigateToPOICallback(
     ROS_INFO_STREAM(" - (x, y): (" << m_target_point.x << ", " << m_target_point.y << ")");
     ROS_INFO_STREAM(" - orientation: " << m_target_orientation);
     ROS_INFO_STREAM(" - desired distance to target: " << m_desired_distance);
+
     initNewTarget();
     startNavigation();
 }
@@ -1610,6 +1637,7 @@ void HomerNavigationNode::publishObstacleMarker(const cv::Vec3f& color)
     m_marker_pub.publish(m_text);
     ROS_INFO_STREAM("Published obstacle marker.");
 }
+
 void HomerNavigationNode::resetObstacleMarker()
 {
     visualization_msgs::Marker m;
@@ -1619,6 +1647,23 @@ void HomerNavigationNode::resetObstacleMarker()
     m.id = MARKER_ID_OBSTACLE_TEXT;
     m_marker_pub.publish(m);
 }
+
+void HomerNavigationNode::publishTargetMarker()
+{
+    geometry_msgs::PointStamped ps;
+    ps.point = m_target_point;
+    ps.point.z = 0.1;
+    ps.header.frame_id = "map";
+    ps.header.stamp = ros::Time::now();
+    const auto m = homer_utils::Marker::Sphere(ps, MARKER_ID_TARGET, 0.1, cv::Vec3f(0, 1, 0));
+    m_marker_pub.publish(m);
+
+    ps.point.z += 0.1;
+    const auto m_text = homer_utils::Marker::Text(
+        "navigation\ntarget", ps, MARKER_ID_TARGET_TEXT, 0.07, cv::Vec3f(0, 0, 0));
+    m_marker_pub.publish(m_text);
+    ROS_INFO_STREAM("Published target marker.");
+}
 //----------------------------------------------------------------------------------------------------------------------
 //----------------------------------------------------------------------------------------------------------------------
 int main(int argc, char** argv)