Collision Monitor ROS2 node. More...

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

Inheritance diagram for nav2_collision_monitor::CollisionMonitor:

Collaboration diagram for nav2_collision_monitor::CollisionMonitor:

[legend]

Public Member Functions
	CollisionMonitor (const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	Constructor for the nav2_collision_monitor::CollisionMonitor. More...

	~CollisionMonitor ()
	Destructor for the nav2_collision_monitor::CollisionMonitor.

Public Member Functions inherited from nav2_util::LifecycleNode
	LifecycleNode (const std::string &node_name, const std::string &ns="", const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	A lifecycle node constructor. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has no integer or floating point range constraints. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const floating_point_range fp_range, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has a floating point range constraint. More...

void	add_parameter (const std::string &name, const rclcpp::ParameterValue &default_value, const integer_range int_range, const std::string &description="", const std::string &additional_constraints="", bool read_only=false)
	Declare a parameter that has an integer range constraint. More...

std::shared_ptr< nav2_util::LifecycleNode >	shared_from_this ()
	Get a shared pointer of this.

nav2_util::CallbackReturn	on_error (const rclcpp_lifecycle::State &)
	Abstracted on_error state transition callback, since unimplemented as of 2020 in the managed ROS2 node state machine. More...

void	autostart ()
	Automatically configure and active the node.

virtual void	on_rcl_preshutdown ()
	Perform preshutdown activities before our Context is shutdown. Note that this is related to our Context's shutdown sequence, not the lifecycle node state machine.

void	createBond ()
	Create bond connection to lifecycle manager.

void	destroyBond ()
	Destroy bond connection to lifecycle manager.

Protected Member Functions
nav2_util::CallbackReturn	on_configure (const rclcpp_lifecycle::State &state) override
	: Initializes and obtains ROS-parameters, creates main subscribers and publishers, creates polygons and data sources objects More...

nav2_util::CallbackReturn	on_activate (const rclcpp_lifecycle::State &state) override
	: Activates LifecyclePublishers, polygons and main processor, creates bond connection More...

nav2_util::CallbackReturn	on_deactivate (const rclcpp_lifecycle::State &state) override
	: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection More...

nav2_util::CallbackReturn	on_cleanup (const rclcpp_lifecycle::State &state) override
	: Resets all subscribers/publishers, polygons/data sources arrays More...

nav2_util::CallbackReturn	on_shutdown (const rclcpp_lifecycle::State &state) override
	Called in shutdown state. More...

void	cmdVelInCallbackStamped (geometry_msgs::msg::TwistStamped::SharedPtr msg)
	Callback for input cmd_vel. More...

void	cmdVelInCallbackUnstamped (geometry_msgs::msg::Twist::SharedPtr msg)

void	publishVelocity (const Action &robot_action, const std_msgs::msg::Header &header)
	Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds, quit to publish 0-velocity. More...

bool	getParameters (std::string &cmd_vel_in_topic, std::string &cmd_vel_out_topic, std::string &state_topic)
	Supporting routine obtaining all ROS-parameters. More...

bool	configurePolygons (const std::string &base_frame_id, const tf2::Duration &transform_tolerance)
	Supporting routine creating and configuring all polygons. More...

bool	configureSources (const std::string &base_frame_id, const std::string &odom_frame_id, const tf2::Duration &transform_tolerance, const rclcpp::Duration &source_timeout, const bool base_shift_correction)
	Supporting routine creating and configuring all data sources. More...

void	process (const Velocity &cmd_vel_in, const std_msgs::msg::Header &header)
	Main processing routine. More...

bool	processStopSlowdownLimit (const std::shared_ptr< Polygon > polygon, const std::unordered_map< std::string, std::vector< Point >> &sources_collision_points_map, const Velocity &velocity, Action &robot_action) const
	Processes the polygon of STOP, SLOWDOWN and LIMIT action type. More...

bool	processApproach (const std::shared_ptr< Polygon > polygon, const std::unordered_map< std::string, std::vector< Point >> &sources_collision_points_map, const Velocity &velocity, Action &robot_action) const
	Processes APPROACH action type. More...

void	notifyActionState (const Action &robot_action, const std::shared_ptr< Polygon > action_polygon) const
	Log and publish current robot action and polygon. More...

void	publishPolygons () const
	Polygons publishing routine. Made for visualization.

Protected Member Functions inherited from nav2_util::LifecycleNode
void	printLifecycleNodeNotification ()
	Print notifications for lifecycle node.

void	register_rcl_preshutdown_callback ()

void	runCleanups ()

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

std::shared_ptr< tf2_ros::TransformListener >	tf_listener_
	TF listener.

std::vector< std::shared_ptr< Polygon > >	polygons_
	Polygons array.

std::vector< std::shared_ptr< Source > >	sources_
	Data sources array.

std::unique_ptr< nav2_util::TwistSubscriber >	cmd_vel_in_sub_
	Input cmd_vel subscriber.

std::unique_ptr< nav2_util::TwistPublisher >	cmd_vel_out_pub_
	Output cmd_vel publisher.

rclcpp_lifecycle::LifecyclePublisher< nav2_msgs::msg::CollisionMonitorState >::SharedPtr	state_pub_
	CollisionMonitor state publisher.

rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray >::SharedPtr	collision_points_marker_pub_
	Collision points marker publisher.

bool	process_active_
	Whether main routine is active.

Action	robot_action_prev_
	Previous robot action.

rclcpp::Time	stop_stamp_
	Latest timestamp when robot has 0-velocity.

rclcpp::Duration	stop_pub_timeout_
	Timeout after which 0-velocity ceases to be published.

Protected Attributes inherited from nav2_util::LifecycleNode
std::unique_ptr< rclcpp::PreShutdownCallbackHandle >	rcl_preshutdown_cb_handle_ {nullptr}

std::shared_ptr< bond::Bond >	bond_ {nullptr}

double	bond_heartbeat_period

rclcpp::TimerBase::SharedPtr	autostart_timer_

Detailed Description

Collision Monitor ROS2 node.

Definition at line 53 of file collision_monitor_node.hpp.

Constructor & Destructor Documentation

◆ CollisionMonitor()

nav2_collision_monitor::CollisionMonitor::CollisionMonitor ( const rclcpp::NodeOptions & options = rclcpp::NodeOptions() )

explicit

Constructor for the nav2_collision_monitor::CollisionMonitor.

Parameters

options Additional options to control creation of the node.

Definition at line 31 of file collision_monitor_node.cpp.

Member Function Documentation

◆ cmdVelInCallbackStamped()

void nav2_collision_monitor::CollisionMonitor::cmdVelInCallbackStamped ( geometry_msgs::msg::TwistStamped::SharedPtr msg )

protected

Callback for input cmd_vel.

Parameters

msg	Input cmd_vel message

Definition at line 188 of file collision_monitor_node.cpp.

◆ configurePolygons()

bool nav2_collision_monitor::CollisionMonitor::configurePolygons	(	const std::string &	base_frame_id,
		const tf2::Duration &	transform_tolerance
	)

protected

Supporting routine creating and configuring all polygons.

Parameters

base_frame_id	Robot base frame ID
transform_tolerance	Transform tolerance

Returns: True if all polygons were configured successfully or false in failure case

Definition at line 289 of file collision_monitor_node.cpp.

◆ configureSources()

bool nav2_collision_monitor::CollisionMonitor::configureSources	(	const std::string &	base_frame_id,
		const std::string &	odom_frame_id,
		const tf2::Duration &	transform_tolerance,
		const rclcpp::Duration &	source_timeout,
		const bool	base_shift_correction
	)

protected

Supporting routine creating and configuring all data sources.

Parameters

base_frame_id	Robot base frame ID
odom_frame_id	Odometry frame ID. Used as global frame to get source->base time interpolated transform.
transform_tolerance	Transform tolerance
source_timeout	Maximum time interval in which data is considered valid
base_shift_correction	Whether to correct source data towards to base frame movement, considering the difference between current time and latest source time

Returns: True if all sources were configured successfully or false in failure case

Definition at line 339 of file collision_monitor_node.cpp.

◆ getParameters()

bool nav2_collision_monitor::CollisionMonitor::getParameters	(	std::string &	cmd_vel_in_topic,
		std::string &	cmd_vel_out_topic,
		std::string &	state_topic
	)

protected

Supporting routine obtaining all ROS-parameters.

Parameters

cmd_vel_in_topic	Output name of cmd_vel_in topic
cmd_vel_out_topic	Output name of cmd_vel_out topic is required.
state_topic	topic name for publishing collision monitor state

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

Definition at line 230 of file collision_monitor_node.cpp.

◆ notifyActionState()

void nav2_collision_monitor::CollisionMonitor::notifyActionState	(	const Action &	robot_action,
		const std::shared_ptr< Polygon >	action_polygon
	)		const

protected

Log and publish current robot action and polygon.

Parameters

robot_action	Robot action to notify
action_polygon	Pointer to a polygon causing a selected action

Definition at line 605 of file collision_monitor_node.cpp.

◆ on_activate()

nav2_util::CallbackReturn nav2_collision_monitor::CollisionMonitor::on_activate ( const rclcpp_lifecycle::State & state )

overrideprotected

: Activates LifecyclePublishers, polygons and main processor, creates bond connection

Parameters

state Lifecycle Node's state

Returns: Success or Failure

Definition at line 103 of file collision_monitor_node.cpp.

◆ on_cleanup()

nav2_util::CallbackReturn nav2_collision_monitor::CollisionMonitor::on_cleanup ( const rclcpp_lifecycle::State & state )

overrideprotected

: Resets all subscribers/publishers, polygons/data sources arrays

Parameters

state Lifecycle Node's state

Returns: Success or Failure

Definition at line 162 of file collision_monitor_node.cpp.

◆ on_configure()

nav2_util::CallbackReturn nav2_collision_monitor::CollisionMonitor::on_configure ( const rclcpp_lifecycle::State & state )

overrideprotected

: Initializes and obtains ROS-parameters, creates main subscribers and publishers, creates polygons and data sources objects

Parameters

state Lifecycle Node's state

Returns: Success or Failure

Definition at line 45 of file collision_monitor_node.cpp.

◆ on_deactivate()

nav2_util::CallbackReturn nav2_collision_monitor::CollisionMonitor::on_deactivate ( const rclcpp_lifecycle::State & state )

overrideprotected

: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection

Parameters

state Lifecycle Node's state

Returns: Success or Failure

Definition at line 133 of file collision_monitor_node.cpp.

◆ on_shutdown()

nav2_util::CallbackReturn nav2_collision_monitor::CollisionMonitor::on_shutdown ( const rclcpp_lifecycle::State & state )

overrideprotected

Called in shutdown state.

Parameters

state Lifecycle Node's state

Returns: Success or Failure

Definition at line 181 of file collision_monitor_node.cpp.

◆ process()

void nav2_collision_monitor::CollisionMonitor::process	(	const Velocity &	cmd_vel_in,
		const std_msgs::msg::Header &	header
	)

protected

Main processing routine.

Parameters

cmd_vel_in	Input desired robot velocity
header	Twist header

Definition at line 406 of file collision_monitor_node.cpp.

◆ processApproach()

bool nav2_collision_monitor::CollisionMonitor::processApproach	(	const std::shared_ptr< Polygon >	polygon,
		const std::unordered_map< std::string, std::vector< Point >> &	sources_collision_points_map,
		const Velocity &	velocity,
		Action &	robot_action
	)		const

protected

Processes APPROACH action type.

Parameters

polygon	Polygon to process
sources_collision_points_map	Map containing source name as key and array of source's 2D obstacle points as value
velocity	Desired robot velocity
robot_action	Output processed robot action

Returns: True if returned action is caused by current polygon, otherwise false

Definition at line 576 of file collision_monitor_node.cpp.

◆ processStopSlowdownLimit()

bool nav2_collision_monitor::CollisionMonitor::processStopSlowdownLimit	(	const std::shared_ptr< Polygon >	polygon,
		const std::unordered_map< std::string, std::vector< Point >> &	sources_collision_points_map,
		const Velocity &	velocity,
		Action &	robot_action
	)		const

protected

Processes the polygon of STOP, SLOWDOWN and LIMIT action type.

Parameters

polygon	Polygon to process
sources_collision_points_map	Map containing source name as key and array of source's 2D obstacle points as value
velocity	Desired robot velocity
robot_action	Output processed robot action

Returns: True if returned action is caused by current polygon, otherwise false

Definition at line 517 of file collision_monitor_node.cpp.

◆ publishVelocity()

void nav2_collision_monitor::CollisionMonitor::publishVelocity	(	const Action &	robot_action,
		const std_msgs::msg::Header &	header
	)

protected

Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds, quit to publish 0-velocity.

Parameters

robot_action	Robot action to publish
header	TwistStamped header to use

Definition at line 206 of file collision_monitor_node.cpp.

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

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
nav2_collision_monitor/src/collision_monitor_node.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CollisionMonitor()

Member Function Documentation

◆ cmdVelInCallbackStamped()

◆ configurePolygons()

◆ configureSources()

◆ getParameters()

◆ notifyActionState()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ process()

◆ processApproach()

◆ processStopSlowdownLimit()

◆ publishVelocity()