Nav2 Navigation Stack - kilted  kilted
ROS 2 Navigation Stack
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
nav2_collision_monitor::Scan Class Reference

Implementation for laser scanner source. More...

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

Inheritance diagram for nav2_collision_monitor::Scan:
Inheritance graph
[legend]
Collaboration diagram for nav2_collision_monitor::Scan:
Collaboration graph
[legend]

Public Member Functions

 Scan (const nav2_util::LifecycleNode::WeakPtr &node, const std::string &source_name, const std::shared_ptr< tf2_ros::Buffer > 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)
 Scan constructor. More...
 
 ~Scan ()
 Scan destructor.
 
void configure ()
 Data source configuration routine. Obtains ROS-parameters and creates laser scanner subscriber.
 
bool getData (const rclcpp::Time &curr_time, std::vector< Point > &data)
 Adds latest data from laser scanner to the data array. More...
 
- Public Member Functions inherited from nav2_collision_monitor::Source
 Source (const nav2_util::LifecycleNode::WeakPtr &node, const std::string &source_name, const std::shared_ptr< tf2_ros::Buffer > 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 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 dataCallback (sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
 Laser scanner data callback. 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...
 
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback (std::vector< rclcpp::Parameter > parameters)
 Callback executed when a parameter change is detected. 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

rclcpp::Subscription< sensor_msgs::msg::LaserScan >::SharedPtr data_sub_
 Laser scanner data subscriber.
 
sensor_msgs::msg::LaserScan::ConstSharedPtr data_
 Latest data obtained from laser scanner.
 
- Protected Attributes inherited from nav2_collision_monitor::Source
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 source_name_
 Name of data source.
 
std::shared_ptr< tf2_ros::Buffer > 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.
 

Detailed Description

Implementation for laser scanner source.

Definition at line 32 of file scan.hpp.

Constructor & Destructor Documentation

◆ Scan()

nav2_collision_monitor::Scan::Scan ( const nav2_util::LifecycleNode::WeakPtr &  node,
const std::string &  source_name,
const std::shared_ptr< tf2_ros::Buffer >  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 
)

Scan constructor.

Parameters
nodeCollision Monitor node pointer
source_nameName of data source
tf_bufferShared pointer to a TF buffer
base_frame_idRobot base frame ID. The output data will be transformed into this frame.
global_frame_idGlobal frame ID for correct transform calculation
transform_toleranceTransform tolerance
source_timeoutMaximum time interval in which data is considered valid
base_shift_correctionWhether to correct source data towards to base frame movement, considering the difference between current time and latest source time

Definition at line 27 of file scan.cpp.

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

Member Function Documentation

◆ dataCallback()

void nav2_collision_monitor::Scan::dataCallback ( sensor_msgs::msg::LaserScan::ConstSharedPtr  msg)
protected

Laser scanner data callback.

Parameters
msgShared pointer to LaserScan message

Definition at line 106 of file scan.cpp.

References data_.

Referenced by configure().

Here is the caller graph for this function:

◆ getData()

bool nav2_collision_monitor::Scan::getData ( const rclcpp::Time &  curr_time,
std::vector< Point > &  data 
)
virtual

Adds latest data from laser scanner to the data array.

Parameters
curr_timeCurrent node time for data interpolation
dataArray 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 69 of file scan.cpp.

References data_, nav2_collision_monitor::Source::getTransform(), and nav2_collision_monitor::Source::sourceValid().

Here is the call graph for this function:

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