diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index d18f974b33..4ffa93a41a 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -128,11 +128,6 @@ class Polygon */ virtual bool isShapeSet(); - /** - * @brief Returns true if using velocity based polygon - */ - bool isUsingVelocityPolygonSelector(); - /** * @brief Updates polygon from footprint subscriber (if any) */ @@ -262,6 +257,9 @@ class Polygon /// @brief Polygon points (vertices) in a base_frame_id_ std::vector poly_; + + /// @brief Source for setting the polygon + PolygonSource polygon_source; }; // class Polygon } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index ac93b916be..a51f0ac830 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -79,6 +79,15 @@ struct Action std::string polygon_name; }; +/// @brief Source for setting the polygon +enum PolygonSource +{ + POLYGON_SOURCE_UNKNOWN = 0, // Default + STATIC_POINTS = 1, // Set from points + DYNAMIC_SUB = 2, // Set from topic subscription + VELOCITY_POLYGON = 3 // Set based on current twists +}; + } // namespace nav2_collision_monitor #endif // NAV2_COLLISION_MONITOR__TYPES_HPP_ diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 7c5d830545..fd5e4d8d8c 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -38,7 +38,8 @@ Polygon::Polygon( : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING), slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer), - base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) + base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance), + polygon_source(POLYGON_SOURCE_UNKNOWN) { RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str()); } @@ -169,89 +170,95 @@ bool Polygon::isShapeSet() return true; } -bool Polygon::isUsingVelocityPolygonSelector() -{ - return !velocity_polygons_.empty(); -} - void Polygon::updatePolygon(const Velocity & cmd_vel_in) { - if (isUsingVelocityPolygonSelector()) { - for (auto & velocity_polygon : velocity_polygons_) { - - if (velocity_polygon->isInRange(cmd_vel_in)) { - // Set the polygon that is within the speed range - poly_ = velocity_polygon->getPolygon(); - - // Update visualization polygon - polygon_.polygon.points.clear(); - for (const Point & p : poly_) { - geometry_msgs::msg::Point32 p_s; - p_s.x = p.x; - p_s.y = p.y; - // p_s.z will remain 0.0 - polygon_.polygon.points.push_back(p_s); + switch (polygon_source) { + case STATIC_POINTS: + { + if (!polygon_.header.frame_id.empty() && polygon_.header.frame_id != base_frame_id_) { + // Polygon is published in another frame: correct poly_ vertices to the latest frame state + std::size_t new_size = polygon_.polygon.points.size(); + + // Get the transform from PolygonStamped frame to base_frame_id_ + tf2::Transform tf_transform; + if ( + !nav2_util::getTransform( + polygon_.header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + + // Correct main poly_ vertices + poly_.resize(new_size); + for (std::size_t i = 0; i < new_size; i++) { + // Transform point coordinates from PolygonStamped frame -> to base frame + tf2::Vector3 p_v3_s(polygon_.polygon.points[i].x, polygon_.polygon.points[i].y, 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Fill poly_ array + poly_[i] = {p_v3_b.x(), p_v3_b.y()}; + } } - - return; + break; } - } - - // Log for uncovered velocity - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - RCLCPP_WARN_THROTTLE( - logger_, - *node->get_clock(), 2.0, "Velocity is not covered by any of the velocity polygons. x: %.3f y: %.3f tw: %.3f ", - cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw); - return; - } - - if (footprint_sub_ != nullptr) { - // Get latest robot footprint from footprint subscriber - std::vector footprint_vec; - std_msgs::msg::Header footprint_header; - footprint_sub_->getFootprintInRobotFrame(footprint_vec, footprint_header); - - std::size_t new_size = footprint_vec.size(); - poly_.resize(new_size); - polygon_.header.frame_id = base_frame_id_; - polygon_.polygon.points.resize(new_size); - - geometry_msgs::msg::Point32 p_s; - for (std::size_t i = 0; i < new_size; i++) { - poly_[i] = {footprint_vec[i].x, footprint_vec[i].y}; - p_s.x = footprint_vec[i].x; - p_s.y = footprint_vec[i].y; - polygon_.polygon.points[i] = p_s; - } - } else if (!polygon_.header.frame_id.empty() && polygon_.header.frame_id != base_frame_id_) { - // Polygon is published in another frame: correct poly_ vertices to the latest frame state - std::size_t new_size = polygon_.polygon.points.size(); - - // Get the transform from PolygonStamped frame to base_frame_id_ - tf2::Transform tf_transform; - if ( - !nav2_util::getTransform( - polygon_.header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } - - // Correct main poly_ vertices - poly_.resize(new_size); - for (std::size_t i = 0; i < new_size; i++) { - // Transform point coordinates from PolygonStamped frame -> to base frame - tf2::Vector3 p_v3_s(polygon_.polygon.points[i].x, polygon_.polygon.points[i].y, 0.0); - tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + case DYNAMIC_SUB: + { + // Get latest robot footprint from footprint subscriber + std::vector footprint_vec; + std_msgs::msg::Header footprint_header; + footprint_sub_->getFootprintInRobotFrame(footprint_vec, footprint_header); + + std::size_t new_size = footprint_vec.size(); + poly_.resize(new_size); + polygon_.header.frame_id = base_frame_id_; + polygon_.polygon.points.resize(new_size); + + geometry_msgs::msg::Point32 p_s; + for (std::size_t i = 0; i < new_size; i++) { + poly_[i] = {footprint_vec[i].x, footprint_vec[i].y}; + p_s.x = footprint_vec[i].x; + p_s.y = footprint_vec[i].y; + polygon_.polygon.points[i] = p_s; + } + break; + } + case VELOCITY_POLYGON: + { + for (auto & velocity_polygon : velocity_polygons_) { + if (velocity_polygon->isInRange(cmd_vel_in)) { + // Set the polygon that is within the speed range + poly_ = velocity_polygon->getPolygon(); + + // Update visualization polygon + polygon_.polygon.points.clear(); + for (const Point & p : poly_) { + geometry_msgs::msg::Point32 p_s; + p_s.x = p.x; + p_s.y = p.y; + // p_s.z will remain 0.0 + polygon_.polygon.points.push_back(p_s); + } + return; + } + } - // Fill poly_ array - poly_[i] = {p_v3_b.x(), p_v3_b.y()}; - } + // Log for uncovered velocity + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + RCLCPP_WARN_THROTTLE( + logger_, + *node->get_clock(), 2.0, "Velocity is not covered by any of the velocity polygons. x: %.3f y: %.3f tw: %.3f ", + cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw); + break; + } + case POLYGON_SOURCE_UNKNOWN: + default: + break; } + return; } int Polygon::getPointsInside(const std::vector & points) const @@ -438,6 +445,7 @@ bool Polygon::getParameters( polygon_name_.c_str()); return false; } + polygon_source = STATIC_POINTS; return true; } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( @@ -462,6 +470,7 @@ bool Polygon::getParameters( node, polygon_name_ + ".footprint_topic", rclcpp::PARAMETER_STRING); footprint_topic = node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); + polygon_source = DYNAMIC_SUB; return true; } } catch (const rclcpp::exceptions::ParameterUninitializedException &) { @@ -486,9 +495,8 @@ bool Polygon::getParameters( if (velocity_polygon->getParameters()) { velocity_polygons_.emplace_back(velocity_polygon); } - } - + polygon_source = VELOCITY_POLYGON; } catch (const std::exception & ex) { RCLCPP_ERROR( logger_,