Basic polygon shape class. For STOP/SLOWDOWN 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_util::LifecycleNode::WeakPtr &node, const std::string &polygon_name, const std::shared_ptr< tf2_ros::Buffer > 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	getMaxPoints () const
	Obtains polygon maximum points to enter inside polygon causing no action. More...

double	getSlowdownRatio () const
	Obtains speed slowdown ratio for current polygon. Applicable for SLOWDOWN 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...

void	updatePolygon ()
	Updates polygon from footprint subscriber (if any)

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

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

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

Protected Member Functions
bool	getCommonParameters (std::string &polygon_pub_topic)
	Supporting routine obtaining ROS-parameters common for all shapes. More...

virtual bool	getParameters (std::string &polygon_pub_topic, std::string &footprint_topic)
	Supporting routine obtaining polygon-specific ROS-parameters. More...

rcl_interfaces::msg::SetParametersResult	dynamicParametersCallback (std::vector< rclcpp::Parameter > parameters)
	Checks if point is inside polygon. More...

bool	isPointInside (const Point &point) const

Protected Attributes
nav2_util::LifecycleNode::WeakPtr	node_
	Collision Monitor node.

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

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	dyn_params_handler_
	Dynamic parameters handler.

std::string	polygon_name_
	Name of polygon.

ActionType	action_type_
	Action type for the polygon.

int	max_points_
	Maximum number of data readings within a zone to not trigger the action.

double	slowdown_ratio_
	Robot slowdown (share of its actual speed)

double	time_before_collision_
	Time before collision in seconds.

double	simulation_time_step_
	Time step for robot movement simulation.

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

bool	enabled_
	Whether polygon is enabled.

std::shared_ptr< tf2_ros::Buffer >	tf_buffer_
	TF buffer.

std::string	base_frame_id_
	Base frame ID.

tf2::Duration	transform_tolerance_
	Transform tolerance.

bool	visualize_
	Whether to publish the polygon.

geometry_msgs::msg::Polygon	polygon_
	Polygon points stored for later publishing.

rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PolygonStamped >::SharedPtr	polygon_pub_
	Polygon publisher for visualization purposes.

std::vector< Point >	poly_
	Polygon points (vertices)

Detailed Description

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

Definition at line 42 of file polygon.hpp.

Constructor & Destructor Documentation

◆ Polygon()

nav2_collision_monitor::Polygon::Polygon	(	const nav2_util::LifecycleNode::WeakPtr &	node,
		const std::string &	polygon_name,
		const std::shared_ptr< tf2_ros::Buffer >	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 30 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 50 of file polygon.cpp.

References base_frame_id_, dyn_params_handler_, dynamicParametersCallback(), footprint_sub_, getParameters(), getPolygon(), node_, polygon_, polygon_pub_, tf_buffer_, transform_tolerance_, and visualize_.

Here is the call graph for this function:

◆ dynamicParametersCallback()

rcl_interfaces::msg::SetParametersResult nav2_collision_monitor::Polygon::dynamicParametersCallback ( std::vector< rclcpp::Parameter > parameters )

protected

Checks if point is inside polygon.

Parameters

point Given point to check

Returns: True if given point is inside polygon, otherwise false

Callback executed when a parameter change is detected

Parameters

event ParameterEvent message

Definition at line 368 of file polygon.cpp.

References enabled_, and polygon_name_.

Referenced by configure().

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 112 of file polygon.cpp.

References action_type_.

◆ getCollisionTime()

double nav2_collision_monitor::Polygon::getCollisionTime	(	const std::vector< Point > &	collision_points,
		const Velocity &	velocity
	)		const

Obtains estimated (simulated) time before a collision. Applicable for APPROACH model.

Parameters

collision_points	Array of 2D obstacle points
velocity	Simulated robot velocity

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

Definition at line 175 of file polygon.cpp.

References getPointsInside(), max_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_pub_topic )

protected

Supporting routine obtaining ROS-parameters common for all shapes.

Parameters

polygon_pub_topic Output name of polygon publishing topic

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

Definition at line 232 of file polygon.cpp.

References action_type_, enabled_, logger_, max_points_, node_, polygon_name_, simulation_time_step_, slowdown_ratio_, time_before_collision_, and visualize_.

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

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 117 of file polygon.cpp.

References enabled_.

◆ getMaxPoints()

int nav2_collision_monitor::Polygon::getMaxPoints ( ) const

Obtains polygon maximum points to enter inside polygon causing no action.

Returns: Maximum points to enter to current polygon and take no action

Definition at line 122 of file polygon.cpp.

References max_points_.

◆ getName()

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

Returns the name of polygon.

Returns: Polygon name

Definition at line 107 of file polygon.cpp.

References polygon_name_.

◆ getParameters()

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

protectedvirtual

Supporting routine obtaining polygon-specific ROS-parameters.

Parameters

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.

Definition at line 302 of file polygon.cpp.

References action_type_, getCommonParameters(), 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()

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

virtual

Gets number of points inside given polygon.

Parameters

points Input array of points to be checked

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

Reimplemented in nav2_collision_monitor::Circle.

Definition at line 164 of file polygon.cpp.

Referenced by getCollisionTime().

Here is the caller graph for this function:

◆ 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 137 of file polygon.cpp.

References poly_.

Referenced by configure().

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 127 of file polygon.cpp.

References slowdown_ratio_.

◆ 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 132 of file polygon.cpp.

References time_before_collision_.

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()

◆ dynamicParametersCallback()

◆ getActionType()

◆ getCollisionTime()

◆ getCommonParameters()

◆ getEnabled()

◆ getMaxPoints()

◆ getName()

◆ getParameters()

◆ getPointsInside()

◆ getPolygon()

◆ getSlowdownRatio()

◆ getTimeBeforeCollision()