Implementation for pointcloud source. More...

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

Inheritance diagram for nav2_collision_monitor::PointCloud:

Collaboration diagram for nav2_collision_monitor::PointCloud:

[legend]

Public Member Functions
	PointCloud (const nav2::LifecycleNode::WeakPtr &node, const std::string &source_name, const nav2::TransformBuffer::SharedPtr tf_buffer, const std::string &base_frame_id, const std::string &global_frame_id, const tf2::Duration &transform_tolerance, const rclcpp::Duration &source_timeout, const bool base_shift_correction)
	PointCloud constructor. More...

	~PointCloud ()
	PointCloud destructor.

bool	configure ()
	Data source configuration routine. Obtains pointcloud related ROS-parameters and creates pointcloud subscriber. More...

bool	getSourceData (const rclcpp::Time &curr_time, std::vector< Point > &data) override
	Adds latest data from pointcloud source to the data array. More...

Public Member Functions inherited from nav2_collision_monitor::Source
	Source (const nav2::LifecycleNode::WeakPtr &node, const std::string &source_name, const nav2::TransformBuffer::SharedPtr tf_buffer, const std::string &base_frame_id, const std::string &global_frame_id, const tf2::Duration &transform_tolerance, const rclcpp::Duration &source_timeout, const bool base_shift_correction)
	Source constructor. More...

virtual	~Source ()
	Source destructor.

bool	getData (const rclcpp::Time &curr_time, std::vector< Point > &data)
	Adds latest data from source to the data array, then removes any points falling inside an enabled exclusion zone. More...

void	activate ()
	Activates the exclusion zone visualization publishers (if any)

void	deactivate ()
	Deactivates the exclusion zone visualization publishers (if any)

void	publishExclusionZones () const
	Publishes the source's exclusion zones for visualization.

bool	getEnabled () const
	Obtains source enabled state. More...

std::string	getSourceName () const
	Obtains the name of the data source. More...

rclcpp::Duration	getSourceTimeout () const
	Obtains the source_timeout parameter of the data source. More...

Protected Member Functions
void	getParameters (std::string &source_topic)
	Getting sensor-specific ROS-parameters. More...

void	dataCallback (sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
	PointCloud data callback. 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...

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

Protected Member Functions inherited from nav2_collision_monitor::Source
bool	configure ()
	Source configuration routine. More...

void	getCommonParameters (std::string &source_topic)
	Supporting routine obtaining ROS-parameters common for all data sources. More...

bool	sourceValid (const rclcpp::Time &source_time, const rclcpp::Time &curr_time) const
	Checks whether the source data might be considered as valid. More...

void	addExclusionZoneCallback (const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< nav2_msgs::srv::AddExclusionZone::Request > request, std::shared_ptr< nav2_msgs::srv::AddExclusionZone::Response > response)
	Service callback to add an exclusion zone at runtime.

void	removeExclusionZoneCallback (const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< nav2_msgs::srv::RemoveExclusionZone::Request > request, std::shared_ptr< nav2_msgs::srv::RemoveExclusionZone::Response > response)
	Service callback to remove an exclusion zone at runtime.

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

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

bool	getTransform (const rclcpp::Time &curr_time, const std_msgs::msg::Header &data_header, tf2::Transform &tf_transform) const
	Obtain the transform to get data from source frame and time where it was received to the base frame and current time (if base_shift_correction_ is true) or the transform without time shift considered which is less accurate but much more faster option not dependent on state estimation frames. More...

Protected Attributes
nav2::Subscription< sensor_msgs::msg::PointCloud2 >::SharedPtr	data_sub_
	PointCloud data subscriber.

std::string	transport_type_

double	min_height_

double	max_height_

double	min_range_

bool	use_global_height_

sensor_msgs::msg::PointCloud2::ConstSharedPtr	data_
	Latest data obtained from pointcloud.

Protected Attributes inherited from nav2_collision_monitor::Source
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	source_name_
	Name of data source.

nav2::TransformBuffer::SharedPtr	tf_buffer_
	TF buffer.

std::string	base_frame_id_
	Robot base frame ID.

std::string	global_frame_id_
	Global frame ID for correct transform calculation.

tf2::Duration	transform_tolerance_
	Transform tolerance.

rclcpp::Duration	source_timeout_
	Maximum time interval in which data is considered valid.

bool	base_shift_correction_
	Whether to correct source data towards to base frame movement, considering the difference between current time and latest source time.

bool	enabled_
	Whether source is enabled.

std::vector< std::shared_ptr< ExclusionZone > >	exclusion_zones_
	Exclusion zones masking out points from this source.

nav2::ServiceServer< nav2_msgs::srv::AddExclusionZone >::SharedPtr	add_ez_service_
	Service to add an exclusion zone at runtime.

nav2::ServiceServer< nav2_msgs::srv::RemoveExclusionZone >::SharedPtr	remove_ez_service_
	Service to remove an exclusion zone at runtime.

Detailed Description

Implementation for pointcloud source.

Definition at line 34 of file pointcloud.hpp.

Constructor & Destructor Documentation

◆ PointCloud()

nav2_collision_monitor::PointCloud::PointCloud	(	const nav2::LifecycleNode::WeakPtr &	node,
		const std::string &	source_name,
		const nav2::TransformBuffer::SharedPtr	tf_buffer,
		const std::string &	base_frame_id,
		const std::string &	global_frame_id,
		const tf2::Duration &	transform_tolerance,
		const rclcpp::Duration &	source_timeout,
		const bool	base_shift_correction
	)

PointCloud constructor.

Parameters

node	Collision Monitor node pointer
source_name	Name of data source
tf_buffer	Shared pointer to a TF buffer
base_frame_id	Robot base frame ID. The output data will be transformed into this frame.
global_frame_id	Global frame ID for correct transform calculation
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

Definition at line 29 of file pointcloud.cpp.

References nav2_collision_monitor::Source::logger_, and nav2_collision_monitor::Source::source_name_.

Member Function Documentation

◆ configure()

bool nav2_collision_monitor::PointCloud::configure ( )

Data source configuration routine. Obtains pointcloud related ROS-parameters and creates pointcloud subscriber.

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

Definition at line 65 of file pointcloud.cpp.

References nav2_collision_monitor::Source::configure(), data_sub_, dataCallback(), getParameters(), nav2_collision_monitor::Source::node_, updateParametersCallback(), and validateParameterUpdatesCallback().

Here is the call graph for this function:

◆ dataCallback()

void nav2_collision_monitor::PointCloud::dataCallback ( sensor_msgs::msg::PointCloud2::ConstSharedPtr msg )

protected

PointCloud data callback.

Parameters

msg	Shared pointer to PointCloud message

Definition at line 199 of file pointcloud.cpp.

References data_.

Referenced by configure().

Here is the caller graph for this function:

◆ getParameters()

void nav2_collision_monitor::PointCloud::getParameters ( std::string & source_topic )

protected

Getting sensor-specific ROS-parameters.

Parameters

source_topic Output name of source subscription topic

Definition at line 181 of file pointcloud.cpp.

References nav2_collision_monitor::Source::getCommonParameters(), nav2_collision_monitor::Source::node_, nav2_collision_monitor::Source::source_name_, and use_global_height_.

Referenced by configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getSourceData()

bool nav2_collision_monitor::PointCloud::getSourceData	(	const rclcpp::Time &	curr_time,
		std::vector< Point > &	data
	)

overridevirtual

Adds latest data from pointcloud source to the data array.

Parameters

curr_time	Current node time for data interpolation
data	Array where the data from source to be added. Added data is transformed to base_frame_id_ coordinate system at curr_time.

Returns: false if an invalid source should block the robot

Implements nav2_collision_monitor::Source.

Definition at line 107 of file pointcloud.cpp.

References data_, nav2_collision_monitor::Source::getTransform(), nav2_collision_monitor::Source::logger_, nav2_collision_monitor::Source::mutex_, nav2_collision_monitor::Source::source_name_, nav2_collision_monitor::Source::sourceValid(), and use_global_height_.

Here is the call graph for this function:

◆ updateParametersCallback()

void nav2_collision_monitor::PointCloud::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 228 of file pointcloud.cpp.

References nav2_collision_monitor::Source::enabled_, nav2_collision_monitor::Source::mutex_, nav2_collision_monitor::Source::source_name_, and use_global_height_.

Referenced by configure().

Here is the caller graph for this function:

◆ validateParameterUpdatesCallback()

rcl_interfaces::msg::SetParametersResult nav2_collision_monitor::PointCloud::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 204 of file pointcloud.cpp.

References nav2_collision_monitor::Source::logger_, and nav2_collision_monitor::Source::source_name_.

Referenced by configure().

Here is the caller graph for this function:

Member Data Documentation

◆ use_global_height_

bool nav2_collision_monitor::PointCloud::use_global_height_

protected

Changes height check from "z" field to "height" field for pipelines utilizing ground contouring

Definition at line 133 of file pointcloud.hpp.

Referenced by getParameters(), getSourceData(), and updateParametersCallback().

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

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
nav2_collision_monitor/src/pointcloud.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PointCloud()

Member Function Documentation

◆ configure()

◆ dataCallback()

◆ getParameters()

◆ getSourceData()

◆ updateParametersCallback()

◆ validateParameterUpdatesCallback()

Member Data Documentation

◆ use_global_height_