Velocity polygon class. This class contains all the points of the polygon and the expected condition of the velocity based polygon. More...

#include <nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp>

Inheritance diagram for nav2_collision_monitor::VelocityPolygon:

[legend]

Collaboration diagram for nav2_collision_monitor::VelocityPolygon:

[legend]

Classes
struct	SubPolygonParameter
	Custom struct to store the parameters of the sub-polygon. More...

Public Member Functions
	VelocityPolygon (const nav2::LifecycleNode::WeakPtr &node, const std::string &polygon_name, const nav2::TransformBuffer::SharedPtr tf_buffer, const std::string &base_frame_id, const tf2::Duration &transform_tolerance)
	VelocityPolygon constructor. More...

virtual	~VelocityPolygon ()
	VelocityPolygon destructor.

bool	getParameters (std::string &, std::string &polygon_pub_topic, std::string &) override
	Overridden getParameters function for VelocityPolygon parameters. More...

void	updatePolygon (const Velocity &cmd_vel_in) override
	Overridden updatePolygon function for VelocityPolygon. More...

Public Member Functions inherited from nav2_collision_monitor::Polygon
	Polygon (const nav2::LifecycleNode::WeakPtr &node, const std::string &polygon_name, const nav2::TransformBuffer::SharedPtr tf_buffer, const std::string &base_frame_id, const tf2::Duration &transform_tolerance)
	Polygon constructor. More...

virtual	~Polygon ()
	Polygon destructor.

bool	configure ()
	Shape configuration routine. Obtains ROS-parameters related to shape object and creates polygon lifecycle publisher. More...

void	activate ()
	Activates polygon lifecycle publisher.

void	deactivate ()
	Deactivates polygon lifecycle publisher.

std::string	getName () const
	Returns the name of polygon. More...

ActionType	getActionType () const
	Obtains polygon action type. More...

bool	getEnabled () const
	Obtains polygon enabled state. More...

int	getMinPoints () const
	Obtains polygon minimum points to enter inside polygon causing the action. More...

bool	isTriggered (const std::unordered_map< std::string, std::vector< Point >> &sources_collision_points_map, std::vector< Point > &out_triggering_points)
	Temporal debounce for min_points trigger. More...

void	resetTriggerState ()
	Reset temporal debounce state.

double	getSlowdownRatio () const
	Obtains speed slowdown ratio for current polygon. Applicable for SLOWDOWN model. More...

double	getLinearLimit () const
	Obtains speed linear limit for current polygon. Applicable for LIMIT model. More...

double	getAngularLimit () const
	Obtains speed angular z limit for current polygon. Applicable for LIMIT model. More...

double	getTimeBeforeCollision () const
	Obtains required time before collision for current polygon. Applicable for APPROACH model. More...

virtual void	getPolygon (std::vector< Point > &poly) const
	Gets polygon points. More...

std::vector< std::string >	getSourcesNames () const
	Obtains the name of the observation sources for current polygon. More...

virtual bool	isShapeSet ()
	Returns true if polygon points were set. Otherwise, prints a warning and returns false.

virtual int	getPointsInside (const std::vector< Point > &points, std::vector< Point > &out_triggering_points) const
	Gets number of points inside given polygon. More...

virtual int	getPointsInside (const std::vector< Point > &points, std::vector< std::size_t > &out_triggering_indices) const
	Gets indices of points inside given polygon. More...

virtual int	getPointsInside (const std::unordered_map< std::string, std::vector< Point >> &sources_collision_points_map, std::vector< Point > &out_triggering_points) const
	Gets number of points inside given polygon. More...

double	getCollisionTime (const std::unordered_map< std::string, std::vector< Point >> &sources_collision_points_map, const Velocity &velocity, std::vector< Point > &out_triggering_points) const
	Obtains estimated (simulated) time before a collision. Applicable for APPROACH model. More...

void	publish ()
	Publishes polygon message into a its own topic.

Protected Member Functions
bool	isInRange (const Velocity &cmd_vel_in, const SubPolygonParameter &sub_polygon_param)
	Check if the velocities and direction is in expected range. More...

Protected Member Functions inherited from nav2_collision_monitor::Polygon
bool	getCommonParameters (std::string &polygon_sub_topic, std::string &polygon_pub_topic, std::string &footprint_topic, bool use_dynamic_sub=false)
	Supporting routine obtaining ROS-parameters common for all shapes. More...

virtual void	createSubscription (std::string &polygon_sub_topic)
	Creates polygon or radius topic subscription. More...

void	updatePolygon (geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
	Updates polygon from geometry_msgs::msg::PolygonStamped message. More...

void	polygonCallback (geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
	Dynamic polygon callback. More...

void	updateParametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Apply parameter updates after validation This callback is executed when parameters have been successfully updated. It updates the internal configuration of the node with the new parameter values. More...

rcl_interfaces::msg::SetParametersResult	validateParameterUpdatesCallback (const std::vector< rclcpp::Parameter > &parameters)
	Validate incoming parameter updates before applying them. This callback is triggered when one or more parameters are about to be updated. It checks the validity of parameter values and rejects updates that would lead to invalid or inconsistent configurations. More...

bool	getPolygonFromString (std::string &poly_string, std::vector< Point > &polygon)
	Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...]. More...

Protected Attributes
rclcpp::Clock::SharedPtr	clock_

bool	holonomic_
	Flag to indicate if the robot is holonomic.

std::vector< SubPolygonParameter >	sub_polygons_
	Vector to store the parameters of the sub-polygon.

Protected Attributes inherited from nav2_collision_monitor::Polygon
nav2::LifecycleNode::WeakPtr	node_
	Collision Monitor node.

rclcpp::Logger	logger_ {rclcpp::get_logger("collision_monitor")}
	Collision monitor node logger stored for further usage.

std::mutex	mutex_
	Dynamic parameters handler.

rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr	post_set_params_handler_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	on_set_params_handler_

std::string	polygon_name_
	Name of polygon.

ActionType	action_type_
	Action type for the polygon.

int	min_points_
	Minimum number of data readings within a zone to trigger the action.

int	trigger_consecutive_points_
	Number of consecutive hits required to trigger action.

int	release_consecutive_points_
	Number of consecutive misses required to release action.

int	trigger_hits_
	Current consecutive hit counter.

int	release_hits_
	Current consecutive miss counter.

bool	trigger_active_
	Latched trigger state after temporal debounce.

double	slowdown_ratio_
	Robot slowdown (share of its actual speed)

double	linear_limit_
	Robot linear limit.

double	angular_limit_
	Robot angular limit.

double	time_before_collision_
	Time before collision in seconds.

double	simulation_time_step_
	Time step for robot movement simulation.

bool	enabled_
	Whether polygon is enabled.

bool	polygon_subscribe_transient_local_
	Whether the subscription to polygon topic has transient local QoS durability.

nav2::Subscription< geometry_msgs::msg::PolygonStamped >::SharedPtr	polygon_sub_
	Polygon subscription.

std::unique_ptr< nav2_costmap_2d::FootprintSubscriber >	footprint_sub_
	Footprint subscriber.

std::vector< std::string >	sources_names_
	Name of the observation sources to check for polygon.

nav2::TransformBuffer::SharedPtr	tf_buffer_
	TF buffer.

std::string	base_frame_id_
	Base frame ID.

tf2::Duration	transform_tolerance_
	Transform tolerance.

rclcpp::Clock::SharedPtr	node_clock_
	Collision monitor node's clock.

bool	visualize_
	Whether to publish the polygon.

geometry_msgs::msg::PolygonStamped	polygon_
	Polygon, used for: 1. visualization; 2. storing latest dynamic polygon message.

nav2::Publisher< geometry_msgs::msg::PolygonStamped >::SharedPtr	polygon_pub_
	Polygon publisher for visualization purposes.

std::vector< Point >	poly_
	Polygon points (vertices) in a base_frame_id_.

Detailed Description

Velocity polygon class. This class contains all the points of the polygon and the expected condition of the velocity based polygon.

Definition at line 37 of file velocity_polygon.hpp.

Constructor & Destructor Documentation

◆ VelocityPolygon()

nav2_collision_monitor::VelocityPolygon::VelocityPolygon	(	const nav2::LifecycleNode::WeakPtr &	node,
		const std::string &	polygon_name,
		const nav2::TransformBuffer::SharedPtr	tf_buffer,
		const std::string &	base_frame_id,
		const tf2::Duration &	transform_tolerance
	)

VelocityPolygon constructor.

Parameters

node	Collision Monitor node pointer
polygon_name	Name of main polygon

Definition at line 23 of file velocity_polygon.cpp.

References nav2_collision_monitor::Polygon::logger_, and nav2_collision_monitor::Polygon::polygon_name_.

Member Function Documentation

◆ getParameters()

bool nav2_collision_monitor::VelocityPolygon::getParameters	(	std::string &	polygon_sub_topic,
		std::string &	polygon_pub_topic,
		std::string &	footprint_topic
	)