Backport: [Collision Monitor] Add a watchdog mechanism

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -69,8 +69,9 @@ class PointCloud : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp

@@ -70,7 +70,7 @@ class PolygonSource : public Source
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

nav2_collision_monitor/include/nav2_collision_monitor/range.hpp

@@ -69,8 +69,9 @@ class Range : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp

@@ -69,8 +69,9 @@ class Scan : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const;
 

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -69,8 +69,9 @@ class Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  virtual void getData(
+  virtual bool getData(
     const rclcpp::Time & curr_time,
     std::vector<Point> & data) const = 0;
 
@@ -80,6 +81,18 @@ class Source
    */
   bool getEnabled() const;
 
+  /**
+   * @brief Obtains the name of the data source
+   * @return Name of the data source
+   */
+  std::string getSourceName() const;
+
+  /**
+   * @brief Obtains the source_timeout parameter of the data source
+   * @return source_timeout parameter value of the data source
+   */
+  rclcpp::Duration getSourceTimeout() const;
+
 protected:
   /**
    * @brief Source configuration routine.

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -315,10 +315,22 @@ void CollisionDetector::process()
   // Points array collected from different data sources in a robot base frame
   std::vector<Point> collision_points;
 
+  std::unique_ptr<nav2_msgs::msg::CollisionDetectorState> state_msg =
+    std::make_unique<nav2_msgs::msg::CollisionDetectorState>();
+
   // Fill collision_points array from different data sources
   for (std::shared_ptr<Source> source : sources_) {
     if (source->getEnabled()) {
-      source->getData(curr_time, collision_points);
+      if (!source->getData(curr_time, collision_points) &&
+        source->getSourceTimeout().seconds() != 0.0)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Invalid source %s detected."
+          " Either due to data not published yet, or to lack of new data received within the"
+          " sensor timeout, or if impossible to transform data to base frame",
+          source->getSourceName().c_str());
+      }
     }
   }
 
@@ -337,7 +349,6 @@ void CollisionDetector::process()
     marker.color.r = 1.0;
     marker.color.a = 1.0;
     marker.lifetime = rclcpp::Duration(0, 0);
-    marker.frame_locked = true;
 
     for (const auto & point : collision_points) {
       geometry_msgs::msg::Point p;
@@ -350,9 +361,6 @@ void CollisionDetector::process()
     collision_points_marker_pub_->publish(std::move(marker_array));
   }
 
-  std::unique_ptr<nav2_msgs::msg::CollisionDetectorState> state_msg =
-    std::make_unique<nav2_msgs::msg::CollisionDetectorState>();
-
   for (std::shared_ptr<Polygon> polygon : polygons_) {
     if (!polygon->getEnabled()) {
       continue;

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -399,10 +399,25 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
   // Points array collected from different data sources in a robot base frame
   std::vector<Point> collision_points;
 
+  // By default - there is no action
+  Action robot_action{DO_NOTHING, cmd_vel_in, ""};
+  // Polygon causing robot action (if any)
+  std::shared_ptr<Polygon> action_polygon;
+
   // Fill collision_points array from different data sources
   for (std::shared_ptr<Source> source : sources_) {
     if (source->getEnabled()) {
-      source->getData(curr_time, collision_points);
+      if (!source->getData(curr_time, collision_points) &&
+        source->getSourceTimeout().seconds() != 0.0)
+      {
+        action_polygon = nullptr;
+        robot_action.polygon_name = "invalid source";
+        robot_action.action_type = STOP;
+        robot_action.req_vel.x = 0.0;
+        robot_action.req_vel.y = 0.0;
+        robot_action.req_vel.tw = 0.0;
+        break;
+      }
     }
   }
 
@@ -421,7 +436,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     marker.color.r = 1.0;
     marker.color.a = 1.0;
     marker.lifetime = rclcpp::Duration(0, 0);
-    marker.frame_locked = true;
 
     for (const auto & point : collision_points) {
       geometry_msgs::msg::Point p;
@@ -434,11 +448,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     collision_points_marker_pub_->publish(std::move(marker_array));
   }
 
-  // By default - there is no action
-  Action robot_action{DO_NOTHING, cmd_vel_in, ""};
-  // Polygon causing robot action (if any)
-  std::shared_ptr<Polygon> action_polygon;
-
   for (std::shared_ptr<Polygon> polygon : polygons_) {
     if (!polygon->getEnabled()) {
       continue;
@@ -579,10 +588,18 @@ void CollisionMonitor::notifyActionState(
   const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const
 {
   if (robot_action.action_type == STOP) {
-    RCLCPP_INFO(
-      get_logger(),
-      "Robot to stop due to %s polygon",
-      action_polygon->getName().c_str());
+    if (robot_action.polygon_name == "invalid source") {
+      RCLCPP_WARN(
+        get_logger(),
+        "Robot to stop due to invalid source."
+        " Either due to data not published yet, or to lack of new data received within the"
+        " sensor timeout, or if impossible to transform data to base frame");
+    } else {
+      RCLCPP_INFO(
+        get_logger(),
+        "Robot to stop due to %s polygon",
+        action_polygon->getName().c_str());
+    }
   } else if (robot_action.action_type == SLOWDOWN) {
     RCLCPP_INFO(
       get_logger(),

nav2_collision_monitor/src/pointcloud.cpp

@@ -99,17 +99,17 @@ PointCloud::dynamicParametersCallback(
   return result;
 }
 
-void PointCloud::getData(
+bool PointCloud::getData(
   const rclcpp::Time & curr_time,
   std::vector<Point> & data) const
 {
   // Ignore data from the source if it is not being published yet or
   // not published for a long time
   if (data_ == nullptr) {
-    return;
+    return false;
   }
   if (!sourceValid(data_->header.stamp, curr_time)) {
-    return;
+    return false;
   }
 
   tf2::Stamped<tf2::Transform> tf_transform;
@@ -122,7 +122,7 @@ void PointCloud::getData(
         base_frame_id_, curr_time, global_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   } else {
     // Obtaining the transform to get data from source frame to base frame without time shift
@@ -133,7 +133,7 @@ void PointCloud::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -152,6 +152,7 @@ void PointCloud::getData(
       data.push_back({p_v3_b.x(), p_v3_b.y()});
     }
   }
+  return true;
 }
 
 void PointCloud::getParameters(std::string & source_topic)

nav2_collision_monitor/src/polygon_source.cpp

@@ -67,14 +67,14 @@ void PolygonSource::configure()
     std::bind(&PolygonSource::dataCallback, this, std::placeholders::_1));
 }
 
-void PolygonSource::getData(
+bool PolygonSource::getData(
   const rclcpp::Time & curr_time,
   std::vector<Point> & data) const
 {
   // Ignore data from the source if it is not being published yet or
   // not published for a long time
   if (data_ == nullptr || data_->polygons.empty()) {
-    return;
+    return false;
   }
   // get the oldest time stamp from the polygon array
   rclcpp::Time oldest_stamp = rclcpp::Time(data_->polygons[0].header.stamp);
@@ -84,7 +84,7 @@ void PolygonSource::getData(
     }
   }
   if (!sourceValid(oldest_stamp, curr_time)) {
-    return;
+    return false;
   }
 
   tf2::Stamped<tf2::Transform> tf_transform;
@@ -99,7 +99,7 @@ void PolygonSource::getData(
           base_frame_id_, curr_time, global_frame_id_,
           transform_tolerance_, tf_buffer_, tf_transform))
       {
-        return;
+        return false;
       }
     } else {
       // Obtaining the transform to get data from source frame to base frame without time shift
@@ -110,14 +110,15 @@ void PolygonSource::getData(
           polygon.header.frame_id, base_frame_id_,
           transform_tolerance_, tf_buffer_, tf_transform))
       {
-        return;
+        return false;
       }
     }
     geometry_msgs::msg::PolygonStamped poly_out;
     geometry_msgs::msg::TransformStamped tf = tf2::toMsg(tf_transform);
     tf2::doTransform(polygon, poly_out, tf);
     convertPolygonStampedToPoints(poly_out, data);
   }
+  return true;
 
 }
 void PolygonSource::convertPolygonStampedToPoints(

nav2_collision_monitor/src/range.cpp

@@ -67,17 +67,17 @@ void Range::configure()
     std::bind(&Range::dataCallback, this, std::placeholders::_1));
 }
 
-void Range::getData(
+bool Range::getData(
   const rclcpp::Time & curr_time,
   std::vector<Point> & data) const
 {
   // Ignore data from the source if it is not being published yet or
   // not being published for a long time
   if (data_ == nullptr) {
-    return;
+    return false;
   }
   if (!sourceValid(data_->header.stamp, curr_time)) {
-    return;
+    return false;
   }
 
   // Ignore data, if its range is out of scope of range sensor abilities
@@ -86,7 +86,7 @@ void Range::getData(
       logger_,
       "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...",
       source_name_.c_str(), data_->range, data_->min_range, data_->max_range);
-    return;
+    return false;
   }
 
   tf2::Stamped<tf2::Transform> tf_transform;
@@ -99,7 +99,7 @@ void Range::getData(
         base_frame_id_, curr_time, global_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   } else {
     // Obtaining the transform to get data from source frame to base frame without time shift
@@ -110,7 +110,7 @@ void Range::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -144,6 +144,8 @@ void Range::getData(
 
   // Refill data array
   data.push_back({p_v3_b.x(), p_v3_b.y()});
+
+  return true;
 }
 
 void Range::getParameters(std::string & source_topic)

nav2_collision_monitor/src/scan.cpp

@@ -66,17 +66,17 @@ void Scan::configure()
     std::bind(&Scan::dataCallback, this, std::placeholders::_1));
 }
 
-void Scan::getData(
+bool Scan::getData(
   const rclcpp::Time & curr_time,
   std::vector<Point> & data) const
 {
   // Ignore data from the source if it is not being published yet or
   // not being published for a long time
   if (data_ == nullptr) {
-    return;
+    return false;
   }
   if (!sourceValid(data_->header.stamp, curr_time)) {
-    return;
+    return false;
   }
 
   tf2::Stamped<tf2::Transform> tf_transform;
@@ -89,7 +89,7 @@ void Scan::getData(
         base_frame_id_, curr_time, global_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   } else {
     // Obtaining the transform to get data from source frame to base frame without time shift
@@ -100,7 +100,7 @@ void Scan::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -120,6 +120,7 @@ void Scan::getData(
     }
     angle += data_->angle_increment;
   }
+  return true;
 }
 
 void Scan::dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg)