Basic polygon shape class. For STOP/SLOWDOWN/LIMIT model it represents zone around the robot while for APPROACH model it represents robot footprint. More...

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

Inheritance diagram for nav2_collision_monitor::Polygon:

[legend]

Collaboration diagram for nav2_collision_monitor::Polygon:

[legend]

Public Member Functions
	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 void	updatePolygon (const Velocity &)
	Updates polygon from footprint subscriber (if any)

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	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 bool	getParameters (std::string &polygon_sub_topic, std::string &polygon_pub_topic, std::string &footprint_topic)
	Supporting routine obtaining polygon-specific ROS-parameters. 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
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

Basic polygon shape class. For STOP/SLOWDOWN/LIMIT model it represents zone around the robot while for APPROACH model it represents robot footprint.

Definition at line 44 of file polygon.hpp.

Constructor & Destructor Documentation

◆ Polygon()

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.

Parameters

node	Collision Monitor node pointer
polygon_name	Name of polygon
tf_buffer	Shared pointer to a TF buffer
base_frame_id	Robot base frame ID
transform_tolerance	Transform tolerance

Definition at line 36 of file polygon.cpp.

References logger_, and polygon_name_.

Member Function Documentation

◆ configure()

bool nav2_collision_monitor::Polygon::configure ( )

Shape configuration routine. Obtains ROS-parameters related to shape object and creates polygon lifecycle publisher.

Returns: True in case of everything is configured correctly, or false otherwise

Definition at line 69 of file polygon.cpp.

References base_frame_id_, createSubscription(), footprint_sub_, getParameters(), getPolygon(), logger_, node_, node_clock_, polygon_, polygon_name_, polygon_pub_, tf_buffer_, transform_tolerance_, updateParametersCallback(), validateParameterUpdatesCallback(), and visualize_.

Here is the call graph for this function:

◆ createSubscription()

void nav2_collision_monitor::Polygon::createSubscription ( std::string & polygon_sub_topic )

protectedvirtual

Creates polygon or radius topic subscription.

Parameters

polygon_sub_topic Output name of polygon or radius subscription topic. Empty, if no polygon subscription.

Reimplemented in nav2_collision_monitor::Circle.

Definition at line 579 of file polygon.cpp.

References logger_, node_, polygon_name_, polygon_sub_, polygon_subscribe_transient_local_, and polygonCallback().

Referenced by configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getActionType()

ActionType nav2_collision_monitor::Polygon::getActionType ( ) const

Obtains polygon action type.

Returns: Action type for current polygon

Definition at line 146 of file polygon.cpp.

References action_type_.

◆ getAngularLimit()

double nav2_collision_monitor::Polygon::getAngularLimit ( ) const

Obtains speed angular z limit for current polygon. Applicable for LIMIT model.

Returns: Speed angular limit

Definition at line 213 of file polygon.cpp.

References angular_limit_.

◆ getCollisionTime()

double nav2_collision_monitor::Polygon::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.

Parameters

collision_points	Input 2D obstacle points
velocity	Simulated robot velocity
out_triggering_points	Output vector receiving the original points responsible for the collision (populated only on a triggering step)

Returns: Estimated time before a collision. If there is no collision, return value will be negative.

Definition at line 339 of file polygon.cpp.

References getPointsInside(), getSourcesNames(), min_points_, simulation_time_step_, and time_before_collision_.

Here is the call graph for this function:

◆ getCommonParameters()

bool nav2_collision_monitor::Polygon::getCommonParameters	(	std::string &	polygon_sub_topic,
		std::string &	polygon_pub_topic,
		std::string &	footprint_topic,
		bool	use_dynamic_sub = `false`
	)

protected

Supporting routine obtaining ROS-parameters common for all shapes.

Parameters

polygon_pub_topic	Output name of polygon or radius subscription topic. Empty, if no polygon subscription.
polygon_sub_topic	Output name of polygon publishing topic
footprint_topic	Output name of footprint topic. Empty, if no footprint subscription.
use_dynamic_sub	If false, the parameter polygon_sub_topic or footprint_topic will not be declared

Returns: True if all parameters were obtained or false in failure case

Definition at line 407 of file polygon.cpp.

References action_type_, angular_limit_, enabled_, getName(), linear_limit_, logger_, min_points_, node_, polygon_name_, polygon_subscribe_transient_local_, release_consecutive_points_, resetTriggerState(), simulation_time_step_, slowdown_ratio_, sources_names_, time_before_collision_, trigger_consecutive_points_, and visualize_.

Referenced by nav2_collision_monitor::VelocityPolygon::getParameters(), getParameters(), and nav2_collision_monitor::Circle::getParameters().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getEnabled()

bool nav2_collision_monitor::Polygon::getEnabled ( ) const

Obtains polygon enabled state.

Returns: Whether polygon is enabled

Definition at line 151 of file polygon.cpp.

References enabled_, and mutex_.

◆ getLinearLimit()

double nav2_collision_monitor::Polygon::getLinearLimit ( ) const

Obtains speed linear limit for current polygon. Applicable for LIMIT model.

Returns: Speed linear limit

Definition at line 208 of file polygon.cpp.

References linear_limit_.

◆ getMinPoints()

int nav2_collision_monitor::Polygon::getMinPoints ( ) const

Obtains polygon minimum points to enter inside polygon causing the action.

Returns: Minimum number of data readings within a zone to trigger the action

Definition at line 157 of file polygon.cpp.

References min_points_.

◆ getName()

std::string nav2_collision_monitor::Polygon::getName ( ) const

Returns the name of polygon.

Returns: Polygon name

Definition at line 141 of file polygon.cpp.

References polygon_name_.

Referenced by getCommonParameters().

Here is the caller graph for this function:

◆ getParameters()

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

protectedvirtual

Supporting routine obtaining polygon-specific ROS-parameters.

Parameters

polygon_sub_topic	Output name of polygon or radius subscription topic. Empty, if no polygon subscription.
polygon_pub_topic	Output name of polygon publishing topic
footprint_topic	Output name of footprint topic. Empty, if no footprint subscription.

Returns: True if all parameters were obtained or false in failure case

Reimplemented in nav2_collision_monitor::Circle, and nav2_collision_monitor::VelocityPolygon.

Definition at line 535 of file polygon.cpp.

References getCommonParameters(), getPolygonFromString(), logger_, node_, poly_, and polygon_name_.

Referenced by configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getPointsInside() [1/3]

int nav2_collision_monitor::Polygon::getPointsInside	(	const std::unordered_map< std::string, std::vector< Point >> &	sources_collision_points_map,
		std::vector< Point > &	out_triggering_points
	)		const

virtual

Gets number of points inside given polygon.

Parameters

sources_collision_points_map	Map containing source name as key, and input array of source's points to be checked as value
out_triggering_points	Output array of triggering points.

Returns: Number of points inside polygon, for sources in map that are associated with current polygon. If there are no points, returns zero value.

Definition at line 321 of file polygon.cpp.

References getPointsInside(), and getSourcesNames().

Here is the call graph for this function:

◆ getPointsInside() [2/3]

int nav2_collision_monitor::Polygon::getPointsInside	(	const std::vector< Point > &	points,
		std::vector< Point > &	out_triggering_points
	)		const

virtual

Gets number of points inside given polygon.

Parameters

points	Input array of points to be checked
out_triggering_points	Output array of triggering points.

Returns: Number of points inside polygon. If there are no points, returns zero value.

Reimplemented in nav2_collision_monitor::Circle.

Definition at line 293 of file polygon.cpp.

References poly_.

Referenced by getCollisionTime(), getPointsInside(), and isTriggered().

Here is the caller graph for this function:

◆ getPointsInside() [3/3]

int nav2_collision_monitor::Polygon::getPointsInside	(	const std::vector< Point > &	points,
		std::vector< std::size_t > &	out_triggering_indices
	)		const

virtual

Gets indices of points inside given polygon.

Parameters

points	Input array of points to be checked
out_triggering_indices	Output array of triggering points indices

Returns: Number of points inside polygon. If there are no points, returns zero value.

Reimplemented in nav2_collision_monitor::Circle.

Definition at line 307 of file polygon.cpp.

References poly_.

◆ getPolygon()

void nav2_collision_monitor::Polygon::getPolygon ( std::vector< Point > & poly ) const

virtual

Gets polygon points.

Parameters

poly	Output polygon points (vertices)

Reimplemented in nav2_collision_monitor::Circle.

Definition at line 228 of file polygon.cpp.

References poly_.

Referenced by configure().

Here is the caller graph for this function:

◆ getPolygonFromString()

bool nav2_collision_monitor::Polygon::getPolygonFromString	(	std::string &	poly_string,
		std::vector< Point > &	polygon
	)

protected

Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...].

Parameters

poly_string	Input String containing the verteceis of the polygon
polygon	Output Point vector with all the vertices of the polygon

Returns: True if all parameters were obtained or false in failure case

Definition at line 704 of file polygon.cpp.

References logger_, and polygon_name_.

Referenced by nav2_collision_monitor::VelocityPolygon::getParameters(), and getParameters().

Here is the caller graph for this function:

◆ getSlowdownRatio()

double nav2_collision_monitor::Polygon::getSlowdownRatio ( ) const

Obtains speed slowdown ratio for current polygon. Applicable for SLOWDOWN model.

Returns: Speed slowdown ratio

Definition at line 203 of file polygon.cpp.

References slowdown_ratio_.

◆ getSourcesNames()

std::vector< std::string > nav2_collision_monitor::Polygon::getSourcesNames ( ) const

Obtains the name of the observation sources for current polygon.

Returns: Names of the observation sources

Definition at line 223 of file polygon.cpp.

References sources_names_.

Referenced by getCollisionTime(), and getPointsInside().

Here is the caller graph for this function:

◆ getTimeBeforeCollision()

double nav2_collision_monitor::Polygon::getTimeBeforeCollision ( ) const

Obtains required time before collision for current polygon. Applicable for APPROACH model.

Returns: Time before collision in seconds

Definition at line 218 of file polygon.cpp.

References time_before_collision_.

◆ isTriggered()

bool nav2_collision_monitor::Polygon::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.

Parameters

points	Input array of points to be checked.
out_triggering_points	Output array of triggering points.

Returns: true if trigger should be considered active after debounce/hold logic.

Definition at line 162 of file polygon.cpp.

References getPointsInside().

Here is the call graph for this function:

◆ polygonCallback()

void nav2_collision_monitor::Polygon::polygonCallback ( geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg )

protected

Dynamic polygon callback.

Parameters

msg	Shared pointer to the polygon message

Definition at line 693 of file polygon.cpp.

References logger_, node_clock_, polygon_name_, and updatePolygon().

Referenced by createSubscription().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateParametersCallback()

void nav2_collision_monitor::Polygon::updateParametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

protected

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.

Parameters

parameters List of parameters that have been updated.

Definition at line 650 of file polygon.cpp.

References enabled_, min_points_, mutex_, polygon_name_, release_consecutive_points_, resetTriggerState(), and trigger_consecutive_points_.

Referenced by configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updatePolygon()

void nav2_collision_monitor::Polygon::updatePolygon ( geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg )

protected

Updates polygon from geometry_msgs::msg::PolygonStamped message.

Parameters

msg	Message to update polygon from

Definition at line 602 of file polygon.cpp.

References base_frame_id_, logger_, poly_, polygon_, polygon_name_, resetTriggerState(), tf_buffer_, and transform_tolerance_.

Here is the call graph for this function:

◆ validateParameterUpdatesCallback()

rcl_interfaces::msg::SetParametersResult nav2_collision_monitor::Polygon::validateParameterUpdatesCallback ( const std::vector< rclcpp::Parameter > & parameters )

protected

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.

Parameters

parameters List of parameters that are being updated.

Returns: rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted.

Definition at line 642 of file polygon.cpp.

Referenced by configure().

Here is the caller graph for this function:

The documentation for this class was generated from the following files:

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
nav2_collision_monitor/src/polygon.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Polygon()

Member Function Documentation

◆ configure()

◆ createSubscription()

◆ getActionType()

◆ getAngularLimit()

◆ getCollisionTime()

◆ getCommonParameters()

◆ getEnabled()

◆ getLinearLimit()

◆ getMinPoints()

◆ getName()

◆ getParameters()

◆ getPointsInside() [1/3]

◆ getPointsInside() [2/3]

◆ getPointsInside() [3/3]

◆ getPolygon()

◆ getPolygonFromString()

◆ getSlowdownRatio()

◆ getSourcesNames()

◆ getTimeBeforeCollision()

◆ isTriggered()

◆ polygonCallback()

◆ updateParametersCallback()

◆ updatePolygon()

◆ validateParameterUpdatesCallback()