fix ignore invalid source behavior

ros-navigation · Oct 29, 2023 · 73e6cbb · 73e6cbb
1 parent e54293f
commit 73e6cbb
Show file tree

Hide file tree

Showing 7 changed files with 18 additions and 15 deletions.
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
@@ -87,6 +87,12 @@ class 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.

diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp
@@ -313,7 +313,9 @@ void CollisionDetector::process()
   // Fill collision_points array from different data sources
   for (std::shared_ptr<Source> source : sources_) {
     if (source->getEnabled()) {
-      if (!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."

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -386,7 +386,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
   // Fill collision_points array from different data sources
   for (std::shared_ptr<Source> source : sources_) {
     if (source->getEnabled()) {
-      if (!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;

diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp
@@ -78,10 +78,6 @@ bool PointCloud::getData(
     return false;
   }
   if (!sourceValid(data_->header.stamp, curr_time)) {
-    // Don't block if source_timeout == 0.0
-    if (source_timeout_.seconds() == 0.0) {
-      return true;
-    }
     return false;
   }
 

diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp
@@ -78,10 +78,6 @@ bool Range::getData(
     return false;
   }
   if (!sourceValid(data_->header.stamp, curr_time)) {
-    // Don't block if source_timeout == 0.0
-    if (source_timeout_.seconds() == 0.0) {
-      return true;
-    }
     return false;
   }
 

diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp
@@ -78,10 +78,6 @@ bool Scan::getData(
     return false;
   }
   if (!sourceValid(data_->header.stamp, curr_time)) {
-    // Don't block if source_timeout == 0.0
-    if (source_timeout_.seconds() == 0.0) {
-      return true;
-    }
     return false;
   }
 

diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp
@@ -84,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. "
@@ -106,6 +106,11 @@ 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)