[Collision Monitor] Add a watchdog mechanism (#3880)

* Add block_if_invalid and allow sensor specific  source_timeout

* remove block_if_invalid param and make it default behavior

* allow unblocking if data not yet published

* log source name when invalid

* getData return true if invalid AND source_timeout == 0.0

* fixed logic without source feedback

* fix test

* rebase artefact

* format artefact

* better log

* move per sensor param source_timeout logic to source.cpp

* fix ignore invalid source behavior

* add source_timeout tests

* no needed anymore

* fix testSourceTimeoutOverride test

---------

Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

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/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

@@ -307,10 +307,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());
+      }
     }
   }
 
@@ -341,9 +353,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

@@ -378,10 +378,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;
+      }
     }
   }
 
@@ -412,11 +427,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;
@@ -545,10 +555,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

@@ -65,17 +65,17 @@ void PointCloud::configure()
     std::bind(&PointCloud::dataCallback, this, std::placeholders::_1));
 }
 
-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::Transform tf_transform;
@@ -88,7 +88,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
@@ -99,7 +99,7 @@ void PointCloud::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -118,6 +118,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/range.cpp

@@ -65,17 +65,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
@@ -84,7 +84,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::Transform tf_transform;
@@ -97,7 +97,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
@@ -108,7 +108,7 @@ void Range::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -142,6 +142,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

@@ -64,17 +64,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::Transform tf_transform;
@@ -87,7 +87,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
@@ -98,7 +98,7 @@ void Scan::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -118,6 +118,7 @@ void Scan::getData(
     }
     angle += data_->angle_increment;
   }
+  return true;
 }
 
 void Scan::dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg)

nav2_collision_monitor/src/source.cpp

@@ -69,6 +69,12 @@ void Source::getCommonParameters(std::string & source_topic)
   nav2_util::declare_parameter_if_not_declared(
     node, source_name_ + ".enabled", rclcpp::ParameterValue(true));
   enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, source_name_ + ".source_timeout",
+    rclcpp::ParameterValue(source_timeout_.seconds()));      // node source_timeout by default
+  source_timeout_ = rclcpp::Duration::from_seconds(
+    node->get_parameter(source_name_ + ".source_timeout").as_double());
 }
 
 bool Source::sourceValid(
@@ -78,7 +84,7 @@ bool Source::sourceValid(
   // Source is considered as not valid, if latest received data timestamp is earlier
   // than current time by source_timeout_ interval
   const rclcpp::Duration dt = curr_time - source_time;
-  if (dt > source_timeout_) {
+  if (source_timeout_.seconds() != 0.0 && dt > source_timeout_) {
     RCLCPP_WARN(
       logger_,
       "[%s]: Latest source and current collision monitor node timestamps differ on %f seconds. "
@@ -95,6 +101,16 @@ bool Source::getEnabled() const
   return enabled_;
 }
 
+std::string Source::getSourceName() const
+{
+  return source_name_;
+}
+
+rclcpp::Duration Source::getSourceTimeout() const
+{
+  return source_timeout_;
+}
+
 rcl_interfaces::msg::SetParametersResult
 Source::dynamicParametersCallback(
   std::vector<rclcpp::Parameter> parameters)