diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index fdc9e07b7a3..fe132fcd798 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -126,6 +126,12 @@ class Polygon */ virtual void getPolygon(std::vector & poly) const; + /** + * @brief Obtains the name of the observation sources for current polygon. + * @return Names of the observation sources + */ + std::vector getSourcesNames() const; + /** * @brief Returns true if polygon points were set. * Otherwise, prints a warning and returns false. @@ -269,6 +275,8 @@ class Polygon rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; + /// @brief Name of the observation sources to check for polygon + std::vector sources_names_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 766ed713cd8..20ff3924c0e 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -270,10 +270,6 @@ bool CollisionMonitor::getParameters( stop_pub_timeout_ = rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double()); - if (!configurePolygons(base_frame_id, transform_tolerance)) { - return false; - } - if ( !configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -281,6 +277,10 @@ bool CollisionMonitor::getParameters( return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } @@ -412,7 +412,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } // Points array collected from different data sources in a robot base frame - std::vector collision_points; + std::unordered_map> sources_collision_points_map; + + // Fill collision_points array from different data sources + for (std::shared_ptr source : sources_) { + std::vector collision_points; + source->getData(curr_time, collision_points); + sources_collision_points_map.insert({source->getSourceName(), collision_points}); + } // By default - there is no action Action robot_action{DO_NOTHING, cmd_vel_in, ""}; @@ -476,6 +483,28 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: // Update polygon coordinates polygon->updatePolygon(cmd_vel_in); + // Get collision points for sources associated to polygon + std::vector sources_names = polygon->getSourcesNames(); + std::vector collision_points; + for (auto source_name : sources_names) { + try { + // Get vector for source + auto source_collision_points = sources_collision_points_map.at(source_name); + // Concatenate vectors + collision_points.insert( + collision_points.end(), + source_collision_points.begin(), + source_collision_points.end()); + } catch (std::exception & e) { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), + *get_clock(), + 1000, + "Observation source [" << source_name << + "] configured for polygon [" << polygon->getName() << "] is not defined!"); + } + } + const ActionType at = polygon->getActionType(); if (at == STOP || at == SLOWDOWN || at == LIMIT) { // Process STOP/SLOWDOWN for the selected polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 523b446b25a..ddef394305e 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -157,6 +157,11 @@ double Polygon::getTimeBeforeCollision() const return time_before_collision_; } +std::vector Polygon::getSourcesNames() const +{ + return sources_names_; +} + void Polygon::getPolygon(std::vector & poly) const { poly = poly_; @@ -392,6 +397,27 @@ bool Polygon::getCommonParameters( node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); } } + + // By default, use all observation sources for polygon + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + std::vector source_names = + node->get_parameter("observation_sources").as_string_array(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(source_names)); + sources_names_ = node->get_parameter(polygon_name_ + ".sources_names").as_string_array(); + + // Check the observation sources configured for polygon are defined + for (auto source_name : sources_names_) { + if (std::find(source_names.begin(), source_names.end(), source_name) == source_names.end()) { + RCLCPP_ERROR_STREAM( + logger_, + "Observation source [" << source_name << + "] configured for polygon [" << getName() << + "] is not defined!"); + return false; + } + } } catch (const std::exception & ex) { RCLCPP_ERROR( logger_,