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

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -70,8 +70,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

@@ -70,8 +70,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

@@ -70,8 +70,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

@@ -303,14 +303,25 @@ 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_) {
-    source->getData(curr_time, collision_points);
+    if (source->getEnabled()) {
+      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());
+      }
+    }
   }
 
-  std::unique_ptr<nav2_msgs::msg::CollisionDetectorState> state_msg =
-    std::make_unique<nav2_msgs::msg::CollisionDetectorState>();
-
   for (std::shared_ptr<Polygon> polygon : polygons_) {
     state_msg->polygons.push_back(polygon->getName());
     state_msg->detections.push_back(

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -355,18 +355,27 @@ 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.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;
+      }
     }
   }
 
-  // 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;
@@ -462,6 +471,15 @@ bool CollisionMonitor::processApproach(
 void CollisionMonitor::printAction(
   const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const
 {
+  if (action_polygon == nullptr) {
+    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");
+    return;
+  }
+
   if (robot_action.action_type == STOP) {
     RCLCPP_INFO(
       get_logger(),

nav2_collision_monitor/src/pointcloud.cpp

@@ -64,17 +64,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;
@@ -87,7 +87,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
@@ -98,7 +98,7 @@ void PointCloud::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -117,6 +117,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

@@ -64,17 +64,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
@@ -83,7 +83,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;
@@ -96,7 +96,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
@@ -107,7 +107,7 @@ void Range::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -141,6 +141,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

@@ -62,17 +62,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;
@@ -85,7 +85,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
@@ -96,7 +96,7 @@ void Scan::getData(
         data_->header.frame_id, base_frame_id_,
         transform_tolerance_, tf_buffer_, tf_transform))
     {
-      return;
+      return false;
     }
   }
 
@@ -116,6 +116,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

@@ -72,6 +72,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(
@@ -81,7 +87,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. "
@@ -98,6 +104,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)