diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 64ced0c66d..865a3c023b 100644 --- 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 index 78e89eae93..b6c871c540 100644 --- 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 : 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 index e2fe6fa9e9..481cafbb65 100644 --- 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 : 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 index 19fac2f635..75af86f11a 100644 --- 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 index c544848405..4478cddf71 100644 --- 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 index 278b729f94..ede22d0125 100644 --- 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 index cac0bb9179..a27876f05b 100644 --- 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 parameters)