From 2d268cde86fbdfebdc7c577d35494166b1c99c50 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 31 Oct 2023 18:16:00 +0000 Subject: [PATCH 1/2] [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 --- .../nav2_collision_monitor/pointcloud.hpp | 3 ++- .../include/nav2_collision_monitor/range.hpp | 3 ++- .../include/nav2_collision_monitor/scan.hpp | 3 ++- .../include/nav2_collision_monitor/source.hpp | 15 ++++++++++++- .../src/collision_detector_node.cpp | 19 +++++++++++++---- .../src/collision_monitor_node.cpp | 21 +++++++++++++------ nav2_collision_monitor/src/pointcloud.cpp | 11 +++++----- nav2_collision_monitor/src/range.cpp | 14 +++++++------ nav2_collision_monitor/src/scan.cpp | 11 +++++----- nav2_collision_monitor/src/source.cpp | 18 +++++++++++++++- 10 files changed, 87 insertions(+), 31 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index 8e1d1caf43a..3016ffe6f3e 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/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 & data) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 1deb80a448b..5d5d44bf31e 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/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 & data) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index 690a22564e5..a120ea96334 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/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 & data) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 30d58d53bc1..865a3c023b9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/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 & 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. diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index e4ecf653f01..88232a4ec44 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/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 collision_points; + std::unique_ptr state_msg = + std::make_unique(); + // Fill collision_points array from different data sources for (std::shared_ptr 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 state_msg = - std::make_unique(); - for (std::shared_ptr polygon : polygons_) { state_msg->polygons.push_back(polygon->getName()); state_msg->detections.push_back( diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 21569b96673..3a5b6a338da 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/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 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 action_polygon; + // Fill collision_points array from different data sources for (std::shared_ptr 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 action_polygon; - for (std::shared_ptr polygon : polygons_) { if (!polygon->getEnabled()) { continue; diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 7e84d8890a0..8c9581652d0 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/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 & 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) diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 8f750490962..aba1df54fc9 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/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 & 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) diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 8ac45904a21..c09735ec28c 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/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 & 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) diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index 41b6f647ee8..f67b7a56fb3 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/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 parameters) From 2924f7f781c20623cc2d1889c3b2380fe68e1778 Mon Sep 17 00:00:00 2001 From: emilnovak Date: Thu, 21 Nov 2024 14:08:43 +0000 Subject: [PATCH 2/2] Fix nullptr error --- nav2_collision_monitor/src/collision_monitor_node.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 3a5b6a338da..d7dc6e1e083 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -471,6 +471,15 @@ bool CollisionMonitor::processApproach( void CollisionMonitor::printAction( const Action & robot_action, const std::shared_ptr 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(),