Circle shape implementation. 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/circle.hpp>

Inheritance diagram for nav2_collision_monitor::Circle:

[legend]

Collaboration diagram for nav2_collision_monitor::Circle:

[legend]

Public Member Functions
	Circle (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)
	Circle class constructor. More...

	~Circle ()
	Circle class destructor.

void	getPolygon (std::vector< Point > &poly) const override
	Gets polygon points, approximated to the circle. To be used in visualization purposes. More...

int	getPointsInside (const std::vector< Point > &points) const override
	Gets number of points inside circle. More...

bool	isShapeSet () override
	Returns true if circle radius is set. Otherwise, prints a warning and returns false.

Public Member Functions inherited from 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. 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...

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...

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

virtual void	updatePolygon (const Velocity &)
	Updates polygon from footprint subscriber (if any)

virtual int	getPointsInside (const std::unordered_map< std::string, std::vector< Point >> &sources_collision_points_map) 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) 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	getParameters (std::string &polygon_sub_topic, std::string &polygon_pub_topic, std::string &footprint_topic) override
	Supporting routine obtaining polygon-specific ROS-parameters. More...

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

void	updatePolygon (double radius)
	Updates polygon from radius value. More...

void	radiusCallback (std_msgs::msg::Float32::ConstSharedPtr msg)
	Dynamic circle radius callback. 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...

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...

rcl_interfaces::msg::SetParametersResult	dynamicParametersCallback (std::vector< rclcpp::Parameter > parameters)
	Callback executed when a parameter change is detected. More...

bool	isPointInside (const Point &point) const
	Checks if point is inside polygon. 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
double	radius_
	Radius of the circle.

double	radius_squared_ = -1.0
	(radius * radius) value. Stored for optimization.

rclcpp::Subscription< std_msgs::msg::Float32 >::SharedPtr	radius_sub_
	Radius subscription.

Protected Attributes inherited from nav2_collision_monitor::Polygon
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	min_points_
	Minimum number of data readings within a zone to trigger the action.

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.

rclcpp::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.

std::shared_ptr< tf2_ros::Buffer >	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.

rclcpp_lifecycle::LifecyclePublisher< 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

Circle shape implementation. For STOP/SLOWDOWN/LIMIT model it represents zone around the robot while for APPROACH model it represents robot footprint.

Definition at line 34 of file circle.hpp.

Constructor & Destructor Documentation

◆ Circle()

nav2_collision_monitor::Circle::Circle	(	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
	)

Circle class constructor.

Parameters

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

Definition at line 26 of file circle.cpp.

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

Member Function Documentation

◆ createSubscription()

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

overrideprotectedvirtual

Creates polygon or radius topic subscription.

Parameters

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

Reimplemented from nav2_collision_monitor::Polygon.

Definition at line 129 of file circle.cpp.

References nav2_collision_monitor::Polygon::logger_, nav2_collision_monitor::Polygon::node_, nav2_collision_monitor::Polygon::polygon_name_, nav2_collision_monitor::Polygon::polygon_subscribe_transient_local_, radius_sub_, and radiusCallback().

Here is the call graph for this function:

◆ getParameters()

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

overrideprotectedvirtual

Supporting routine obtaining polygon-specific ROS-parameters.

Parameters

polygon_sub_topic	Input name of polygon subscription topic
polygon_pub_topic	Output name of polygon or radius publishing topic
footprint_topic	Output name of footprint topic. For Circle returns empty string, there is no footprint subscription in this class.