Collision Monitor ROS2 node. More...

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

Inheritance diagram for nav2_collision_monitor::CollisionDetector:

Collaboration diagram for nav2_collision_monitor::CollisionDetector:

[legend]

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

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

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

	LifecycleNode (const std::string &node_name, const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
	A lifecycle node constructor with no namespace. More...

template<typename ParameterT >
ParameterT	declare_or_get_parameter (const std::string &parameter_name, const ParameterDescriptor &parameter_descriptor=ParameterDescriptor())
	Declares or gets a parameter with specified type (not value). If the parameter is already declared, returns its value; otherwise declares it with the specified type. More...

template<typename ParamType >
ParamType	declare_or_get_parameter (const std::string &parameter_name, const ParamType &default_value, const ParameterDescriptor &parameter_descriptor=ParameterDescriptor())
	Declares or gets a parameter. If the parameter is already declared, returns its value; otherwise declares it and returns the default value. More...

template<typename MessageT , typename CallbackT >
nav2::Subscription< MessageT >::SharedPtr	create_subscription (const std::string &topic_name, CallbackT &&callback, const rclcpp::QoS &qos=nav2::qos::StandardTopicQoS(), const rclcpp::CallbackGroup::SharedPtr &callback_group=nullptr)
	Create a subscription to a topic using Nav2 QoS profiles and SubscriptionOptions. More...

template<typename MessageT >
nav2::Publisher< MessageT >::SharedPtr	create_publisher (const std::string &topic_name, const rclcpp::QoS &qos=nav2::qos::StandardTopicQoS(), const rclcpp::CallbackGroup::SharedPtr &callback_group=nullptr)
	Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions. More...

template<typename ServiceT >
nav2::ServiceClient< ServiceT >::SharedPtr	create_client (const std::string &service_name, bool use_internal_executor=false)
	Create a ServiceClient to interface with a service. More...

template<typename ServiceT >
nav2::ServiceServer< ServiceT >::SharedPtr	create_service (const std::string &service_name, typename nav2::ServiceServer< ServiceT >::CallbackType cb, rclcpp::CallbackGroup::SharedPtr callback_group=nullptr)
	Create a ServiceServer to host with a service. More...

template<typename DurationRepT , typename DurationT , typename CallbackT >
rclcpp::GenericTimer< CallbackT >::SharedPtr	create_timer (std::chrono::duration< DurationRepT, DurationT > period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group=nullptr)
	Create a sim-time-aware timer for Nav2 lifecycle nodes. More...

template<typename ActionT >
nav2::SimpleActionServer< ActionT >::SharedPtr	create_action_server (const std::string &action_name, typename nav2::SimpleActionServer< ActionT >::ExecuteCallback execute_callback, typename nav2::SimpleActionServer< ActionT >::GoalReceivedCallback goal_received_callback=nullptr, typename nav2::SimpleActionServer< ActionT >::CompletionCallback compl_cb=nullptr, std::chrono::milliseconds server_timeout=std::chrono::milliseconds(500), bool spin_thread=false, const bool realtime=false)
	Create a SimpleActionServer to host with an action. More...

template<typename ActionT >
nav2::ActionClient< ActionT >::SharedPtr	create_action_client (const std::string &action_name, rclcpp::CallbackGroup::SharedPtr callback_group=nullptr)
	Create a ActionClient to call an action using. More...

nav2::LifecycleNode::SharedPtr	shared_from_this ()
	Get a shared pointer of this.

nav2::LifecycleNode::WeakPtr	weak_from_this ()
	Get a shared pointer of this.

nav2::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::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::CallbackReturn	on_activate (const rclcpp_lifecycle::State &state) override
	: Activates LifecyclePublishers, polygons and main processor, creates bond connection More...

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

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

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

bool	getParameters ()
	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 ()
	Main processing routine.

void	publishVisualizations () const
	Publishes all visualization topics (polygons and exclusion zones).

void	publishTriggeringPoints (const std::unordered_map< std::string, std::vector< Point >> &all_triggering_points)
	Publishes the points inside each detected polygon as markers, bucketed by polygon name and per-point source. More...

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

void	register_rcl_preshutdown_callback ()

void	runCleanups ()

Protected Attributes
nav2::TransformBuffer::SharedPtr	tf_buffer_
	TF buffer.

nav2::TransformListener::SharedPtr	tf_listener_
	TF listener.

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

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

nav2::Publisher< nav2_msgs::msg::CollisionDetectorState >::SharedPtr	state_pub_
	collision monitor state publisher

nav2::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	collision_points_marker_pub_
	Collision points marker publisher.

nav2::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	triggering_points_pub_
	Triggering points marker publisher (points inside each detected polygon)

rclcpp::TimerBase::SharedPtr	timer_
	timer that runs actions

double	frequency_
	main loop frequency

bool	collision_points_marker_3d_
	Whether to include z in the collision_points_marker.

std::string	base_frame_id_
	Robot base frame ID.

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

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

double	bond_heartbeat_period {0.1}

rclcpp::TimerBase::SharedPtr	autostart_timer_

Additional Inherited Members
Public Types inherited from nav2::LifecycleNode
using	SharedPtr = std::shared_ptr< nav2::LifecycleNode >

using	WeakPtr = std::weak_ptr< nav2::LifecycleNode >

using	SharedConstPointer = std::shared_ptr< const nav2::LifecycleNode >

Detailed Description

Collision Monitor ROS2 node.

Definition at line 50 of file collision_detector_node.hpp.

Constructor & Destructor Documentation

◆ CollisionDetector()

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

explicit

Constructor for the nav2_collision_monitor::CollisionDetector.

Parameters

options Additional options to control creation of the node.

Definition at line 29 of file collision_detector_node.cpp.

Member Function Documentation

◆ configurePolygons()

bool nav2_collision_monitor::CollisionDetector::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 184 of file collision_detector_node.cpp.

References polygons_, nav2::LifecycleNode::shared_from_this(), and tf_buffer_.

Referenced by getParameters().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ configureSources()

bool nav2_collision_monitor::CollisionDetector::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 243 of file collision_detector_node.cpp.

References nav2::LifecycleNode::shared_from_this(), sources_, and tf_buffer_.

Referenced by getParameters().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getParameters()

bool nav2_collision_monitor::CollisionDetector::getParameters ( )

protected

Supporting routine obtaining all ROS-parameters.

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

Definition at line 152 of file collision_detector_node.cpp.

References base_frame_id_, collision_points_marker_3d_, configurePolygons(), configureSources(), frequency_, and nav2::LifecycleNode::shared_from_this().

Referenced by on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_activate()

nav2::CallbackReturn nav2_collision_monitor::CollisionDetector::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 68 of file collision_detector_node.cpp.

References collision_points_marker_pub_, nav2::LifecycleNode::create_timer(), nav2::LifecycleNode::createBond(), frequency_, polygons_, process(), sources_, state_pub_, timer_, and triggering_points_pub_.

Here is the call graph for this function:

◆ on_cleanup()

nav2::CallbackReturn nav2_collision_monitor::CollisionDetector::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 128 of file collision_detector_node.cpp.

References collision_points_marker_pub_, polygons_, sources_, state_pub_, tf_buffer_, tf_listener_, and triggering_points_pub_.

Referenced by on_configure().

Here is the caller graph for this function:

◆ on_configure()

nav2::CallbackReturn nav2_collision_monitor::CollisionDetector::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 41 of file collision_detector_node.cpp.

References collision_points_marker_pub_, getParameters(), on_cleanup(), state_pub_, tf_buffer_, tf_listener_, and triggering_points_pub_.

Here is the call graph for this function:

◆ on_deactivate()

nav2::CallbackReturn nav2_collision_monitor::CollisionDetector::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 99 of file collision_detector_node.cpp.

References collision_points_marker_pub_, nav2::LifecycleNode::destroyBond(), polygons_, sources_, state_pub_, timer_, and triggering_points_pub_.

Here is the call graph for this function:

◆ on_shutdown()

nav2::CallbackReturn nav2_collision_monitor::CollisionDetector::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 146 of file collision_detector_node.cpp.

◆ publishTriggeringPoints()

void nav2_collision_monitor::CollisionDetector::publishTriggeringPoints ( const std::unordered_map< std::string, std::vector< Point >> & all_triggering_points )

protected

Publishes the points inside each detected polygon as markers, bucketed by polygon name and per-point source.

Parameters

all_triggering_points Map from polygon name to its triggering points.

Definition at line 411 of file collision_detector_node.cpp.

References base_frame_id_, and triggering_points_pub_.

Referenced by process().

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/collision_detector_node.hpp
nav2_collision_monitor/src/collision_detector_node.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CollisionDetector()

Member Function Documentation

◆ configurePolygons()

◆ configureSources()

◆ getParameters()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_shutdown()

◆ publishTriggeringPoints()