#include <rgbd_handler.h>

Inheritance diagram for cslam::RGBDHandler:

Public Member Functions
	RGBDHandler (std::shared_ptr< rclcpp::Node > &node)
	Initialization of parameters and ROS 2 objects. More...

	~RGBDHandler ()

virtual void	process_new_sensor_data ()
	Processes Latest received image. More...

void	local_descriptors_request (cslam_common_interfaces::msg::LocalDescriptorsRequest::ConstSharedPtr request)
	Service callback to publish local descriptors. More...

void	receive_local_keyframe_match (cslam_common_interfaces::msg::LocalKeyframeMatch::ConstSharedPtr msg)
	Receives a local match and tries to compute a local loop closure. More...

void	receive_local_image_descriptors (const std::shared_ptr< cslam_common_interfaces::msg::LocalImageDescriptors > msg)
	Message callback to receive descriptors and compute. More...

void	compute_local_descriptors (std::shared_ptr< rtabmap::SensorData > &frame_data)
	Computes local 3D descriptors from frame data and store them. More...

virtual void	local_descriptors_msg_to_sensor_data (const std::shared_ptr< cslam_common_interfaces::msg::LocalImageDescriptors > msg, rtabmap::SensorData &sensor_data)
	converts descriptors to sensore data More...

void	sensor_data_to_rgbd_msg (const std::shared_ptr< rtabmap::SensorData > sensor_data, rtabmap_msgs::msg::RGBDImage &msg_data)
	converts sensor data to descriptor msg More...

bool	generate_new_keyframe (std::shared_ptr< rtabmap::SensorData > &keyframe)
	Generate a new keyframe according to the policy. More...

void	send_keyframe (const std::pair< std::shared_ptr< rtabmap::SensorData >, std::shared_ptr< const nav_msgs::msg::Odometry >> &keypoints_data)
	Function to send the image to the python node. More...

void	send_keyframe (const std::pair< std::shared_ptr< rtabmap::SensorData >, std::shared_ptr< const nav_msgs::msg::Odometry >> &keypoints_data, const sensor_msgs::msg::NavSatFix &gps_data)
	Function to send the image to the python node. More...

virtual void	send_visualization (const std::pair< std::shared_ptr< rtabmap::SensorData >, std::shared_ptr< const nav_msgs::msg::Odometry >> &keypoints_data)
	Send keypoints for visualizations. More...

void	send_visualization_keypoints (const std::pair< std::shared_ptr< rtabmap::SensorData >, std::shared_ptr< const nav_msgs::msg::Odometry >> &keypoints_data)
	Send keypoints for visualizations. More...

void	send_visualization_pointcloud (const std::shared_ptr< rtabmap::SensorData > &sensor_data)
	Send colored pointcloud for visualizations. More...

void	rgbd_callback (const sensor_msgs::msg::Image::ConstSharedPtr image_rect_rgb, const sensor_msgs::msg::Image::ConstSharedPtr image_rect_depth, const sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_rgb, const nav_msgs::msg::Odometry::ConstSharedPtr odom)
	Callback receiving sync data from camera. More...

void	clear_sensor_data (std::shared_ptr< rtabmap::SensorData > &sensor_data)
	Clear images and large data fields in sensor data. More...

void	gps_callback (const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg)
	GPS data callback. More...

sensor_msgs::msg::PointCloud2	visualization_pointcloud_voxel_subsampling (const sensor_msgs::msg::PointCloud2 &input_cloud)
	Subsample pointcloud to reduce size for visualization. More...

Public Member Functions inherited from cslam::ISensorHandler
virtual	~ISensorHandler ()
	Virtual destructor. More...

Protected Attributes
std::deque< std::pair< std::shared_ptr< rtabmap::SensorData >, nav_msgs::msg::Odometry::ConstSharedPtr > >	received_data_queue_

std::shared_ptr< rtabmap::SensorData >	previous_keyframe_

std::map< int, std::shared_ptr< rtabmap::SensorData > >	local_descriptors_map_

std::shared_ptr< rclcpp::Node >	node_

unsigned int	min_inliers_

unsigned int	max_nb_robots_

unsigned int	robot_id_

unsigned int	max_queue_size_

unsigned int	nb_local_keyframes_

message_filters::Subscriber< nav_msgs::msg::Odometry >	sub_odometry_

rclcpp::Subscription< cslam_common_interfaces::msg::LocalDescriptorsRequest >::SharedPtr	send_local_descriptors_subscriber_

rclcpp::Publisher< cslam_common_interfaces::msg::LocalImageDescriptors >::SharedPtr	local_descriptors_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::LocalImageDescriptors >::SharedPtr	visualization_local_descriptors_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::KeyframeRGB >::SharedPtr	keyframe_data_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::KeyframeOdom >::SharedPtr	keyframe_odom_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::VizPointCloud >::SharedPtr	keyframe_pointcloud_publisher_

rclcpp::Subscription< cslam_common_interfaces::msg::LocalKeyframeMatch >::SharedPtr	local_keyframe_match_subscriber_

rclcpp::Subscription< cslam_common_interfaces::msg::LocalImageDescriptors >::SharedPtr	local_descriptors_subscriber_

rtabmap::RegistrationVis	registration_

rclcpp::Publisher< cslam_common_interfaces::msg::InterRobotLoopClosure >::SharedPtr	inter_robot_loop_closure_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::IntraRobotLoopClosure >::SharedPtr	intra_robot_loop_closure_publisher_

rclcpp::Publisher< diagnostic_msgs::msg::KeyValue >::SharedPtr	log_publisher_

unsigned int	log_total_local_descriptors_cumulative_communication_

bool	enable_logs_

std::shared_ptr< tf2_ros::Buffer >	tf_buffer_

std::shared_ptr< tf2_ros::TransformListener >	tf_listener_

std::string	base_frame_id_

float	keyframe_generation_ratio_threshold_

bool	generate_new_keyframes_based_on_inliers_ratio_

unsigned int	visualization_period_ms_

bool	enable_visualization_

float	visualization_voxel_size_

float	visualization_max_range_

bool	enable_gps_recording_

std::string	gps_topic_

rclcpp::Subscription< sensor_msgs::msg::NavSatFix >::SharedPtr	gps_subscriber_

sensor_msgs::msg::NavSatFix	latest_gps_fix_

std::deque< sensor_msgs::msg::NavSatFix >	received_gps_queue_

Detailed Description

Definition at line 56 of file rgbd_handler.h.

Constructor & Destructor Documentation

◆ RGBDHandler()

RGBDHandler::RGBDHandler ( std::shared_ptr< rclcpp::Node > & node )

Initialization of parameters and ROS 2 objects.

Parameters

node	ROS 2 node handle

Definition at line 16 of file rgbd_handler.cpp.

Here is the call graph for this function:

◆ ~RGBDHandler()

cslam::RGBDHandler::~RGBDHandler ( )

inline

Definition at line 65 of file rgbd_handler.h.

Member Function Documentation

◆ clear_sensor_data()

void RGBDHandler::clear_sensor_data ( std::shared_ptr< rtabmap::SensorData > & sensor_data )

Clear images and large data fields in sensor data.

Parameters

sensor_data frame data

Definition at line 619 of file rgbd_handler.cpp.

Here is the caller graph for this function:

◆ compute_local_descriptors()

void RGBDHandler::compute_local_descriptors ( std::shared_ptr< rtabmap::SensorData > & frame_data )

Computes local 3D descriptors from frame data and store them.

Parameters

frame_data Full frame data

Definition at line 266 of file rgbd_handler.cpp.

Here is the caller graph for this function:

◆ generate_new_keyframe()

bool RGBDHandler::generate_new_keyframe ( std::shared_ptr< rtabmap::SensorData > & keyframe )

Generate a new keyframe according to the policy.

Parameters

keyframe Sensor data

Returns: true A new keyframe is added to the map; false The frame is rejected

Definition at line 314 of file rgbd_handler.cpp.

Here is the caller graph for this function:

◆ gps_callback()

void RGBDHandler::gps_callback ( const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg )

GPS data callback.

Parameters

msg

Definition at line 684 of file rgbd_handler.cpp.

Here is the caller graph for this function:

◆ local_descriptors_msg_to_sensor_data()

void RGBDHandler::local_descriptors_msg_to_sensor_data	(	const std::shared_ptr< cslam_common_interfaces::msg::LocalImageDescriptors >	msg,
		rtabmap::SensorData &	sensor_data
	)

virtual

converts descriptors to sensore data

Parameters

msg	local descriptors

Returns: rtabmap::SensorData&

Reimplemented in cslam::StereoHandler.

Definition at line 471 of file rgbd_handler.cpp.

Here is the caller graph for this function:

◆ local_descriptors_request()

void RGBDHandler::local_descriptors_request ( cslam_common_interfaces::msg::LocalDescriptorsRequest::ConstSharedPtr request )

Service callback to publish local descriptors.

Parameters

request Image ID to send and matching info

Definition at line 404 of file rgbd_handler.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ process_new_sensor_data()

void RGBDHandler::process_new_sensor_data ( )

virtual

Processes Latest received image.

Implements cslam::ISensorHandler.

Definition at line 353 of file rgbd_handler.cpp.

Here is the call graph for this function:

◆ receive_local_image_descriptors()

void RGBDHandler::receive_local_image_descriptors ( const std::shared_ptr< cslam_common_interfaces::msg::LocalImageDescriptors > msg )

Message callback to receive descriptors and compute.

Parameters

msg	local descriptors

Definition at line 493 of file rgbd_handler.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ receive_local_keyframe_match()

void RGBDHandler::receive_local_keyframe_match ( cslam_common_interfaces::msg::LocalKeyframeMatch::ConstSharedPtr msg )

Receives a local match and tries to compute a local loop closure.

Parameters

msg

Definition at line 433 of file rgbd_handler.cpp.

Here is the caller graph for this function:

◆ rgbd_callback()

void RGBDHandler::rgbd_callback	(	const sensor_msgs::msg::Image::ConstSharedPtr	image_rect_rgb,
		const sensor_msgs::msg::Image::ConstSharedPtr	image_rect_depth,
		const sensor_msgs::msg::CameraInfo::ConstSharedPtr	camera_info_rgb,
		const nav_msgs::msg::Odometry::ConstSharedPtr	odom
	)

Callback receiving sync data from camera.

Parameters

image_rgb
image_depth
camera_info_rgb
camera_info_depth
odom

Definition at line 173 of file rgbd_handler.cpp.

Here is the caller graph for this function:

◆ send_keyframe() [1/2]

void RGBDHandler::send_keyframe ( const std::pair< std::shared_ptr< rtabmap::SensorData >, std::shared_ptr< const nav_msgs::msg::Odometry >> & keypoints_data )

Function to send the image to the python node.

Parameters

keypoints_data keyframe keypoints data

Definition at line 556 of file rgbd_handler.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_keyframe() [2/2]

void RGBDHandler::send_keyframe	(	const std::pair< std::shared_ptr< rtabmap::SensorData >, std::shared_ptr< const nav_msgs::msg::Odometry >> &	keypoints_data,
		const sensor_msgs::msg::NavSatFix &	gps_data
	)

Function to send the image to the python node.

Parameters

keypoints_data	keyframe keypoints data
gps_data	GPS data

Definition at line 584 of file rgbd_handler.cpp.

Here is the call graph for this function:

◆ send_visualization()

void RGBDHandler::send_visualization ( const std::pair< std::shared_ptr< rtabmap::SensorData >, std::shared_ptr< const nav_msgs::msg::Odometry >> & keypoints_data )

virtual

Send keypoints for visualizations.

Parameters

keypoints_data keyframe keypoints data

Reimplemented in cslam::StereoHandler.

Definition at line 613 of file rgbd_handler.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_visualization_keypoints()

void RGBDHandler::send_visualization_keypoints ( const std::pair< std::shared_ptr< rtabmap::SensorData >, std::shared_ptr< const nav_msgs::msg::Odometry >> & keypoints_data )

Send keypoints for visualizations.

Parameters

keypoints_data keyframe keypoints data

Definition at line 626 of file rgbd_handler.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_visualization_pointcloud()

void RGBDHandler::send_visualization_pointcloud ( const std::shared_ptr< rtabmap::SensorData > & sensor_data )

Send colored pointcloud for visualizations.

Parameters

sensor_data RGBD image

Definition at line 665 of file rgbd_handler.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sensor_data_to_rgbd_msg()

void RGBDHandler::sensor_data_to_rgbd_msg	(	const std::shared_ptr< rtabmap::SensorData >	sensor_data,
		rtabmap_msgs::msg::RGBDImage &	msg_data
	)

converts sensor data to descriptor msg

Parameters

sensor_data	local descriptors
msg_data	rtabmap_msgs::msg::RGBDImage&

Definition at line 396 of file rgbd_handler.cpp.

Here is the caller graph for this function:

◆ visualization_pointcloud_voxel_subsampling()

sensor_msgs::msg::PointCloud2 RGBDHandler::visualization_pointcloud_voxel_subsampling ( const sensor_msgs::msg::PointCloud2 & input_cloud )

Subsample pointcloud to reduce size for visualization.

Parameters

input_cloud

Returns: pcl::PointCloud<pcl::PointXYZRGB>&

Definition at line 640 of file rgbd_handler.cpp.

Here is the caller graph for this function:

Member Data Documentation

◆ base_frame_id_

std::string cslam::RGBDHandler::base_frame_id_

protected

Definition at line 276 of file rgbd_handler.h.

◆ enable_gps_recording_

bool cslam::RGBDHandler::enable_gps_recording_

protected

Definition at line 284 of file rgbd_handler.h.

◆ enable_logs_

bool cslam::RGBDHandler::enable_logs_

protected

Definition at line 270 of file rgbd_handler.h.

◆ enable_visualization_

bool cslam::RGBDHandler::enable_visualization_

protected

Definition at line 281 of file rgbd_handler.h.

◆ generate_new_keyframes_based_on_inliers_ratio_

bool cslam::RGBDHandler::generate_new_keyframes_based_on_inliers_ratio_

protected

Definition at line 278 of file rgbd_handler.h.

◆ gps_subscriber_

rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr cslam::RGBDHandler::gps_subscriber_

protected

Definition at line 286 of file rgbd_handler.h.

◆ gps_topic_

std::string cslam::RGBDHandler::gps_topic_

protected

Definition at line 285 of file rgbd_handler.h.

◆ inter_robot_loop_closure_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::InterRobotLoopClosure>::SharedPtr cslam::RGBDHandler::inter_robot_loop_closure_publisher_

protected

Definition at line 260 of file rgbd_handler.h.

◆ intra_robot_loop_closure_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::IntraRobotLoopClosure>::SharedPtr cslam::RGBDHandler::intra_robot_loop_closure_publisher_

protected

Definition at line 264 of file rgbd_handler.h.

◆ keyframe_data_publisher_

rclcpp::Publisher<cslam_common_interfaces::msg::KeyframeRGB>::SharedPtr cslam::RGBDHandler::keyframe_data_publisher_

protected

Definition at line 240 of file rgbd_handler.h.

◆ keyframe_generation_ratio_threshold_

float cslam::RGBDHandler::keyframe_generation_ratio_threshold_

protected

Definition at line 277 of file rgbd_handler.h.

◆ keyframe_odom_publisher_

rclcpp::Publisher<cslam_common_interfaces::msg::KeyframeOdom>::SharedPtr cslam::RGBDHandler::keyframe_odom_publisher_

protected

Definition at line 243 of file rgbd_handler.h.

◆ keyframe_pointcloud_publisher_

rclcpp::Publisher<cslam_common_interfaces::msg::VizPointCloud>::SharedPtr cslam::RGBDHandler::keyframe_pointcloud_publisher_

protected

Definition at line 246 of file rgbd_handler.h.

◆ latest_gps_fix_

sensor_msgs::msg::NavSatFix cslam::RGBDHandler::latest_gps_fix_

protected

Definition at line 287 of file rgbd_handler.h.

◆ local_descriptors_map_

std::map<int, std::shared_ptr<rtabmap::SensorData> > cslam::RGBDHandler::local_descriptors_map_

protected

Definition at line 221 of file rgbd_handler.h.

◆ local_descriptors_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::LocalImageDescriptors>::SharedPtr cslam::RGBDHandler::local_descriptors_publisher_

protected

Definition at line 236 of file rgbd_handler.h.

◆ local_descriptors_subscriber_

rclcpp::Subscription< cslam_common_interfaces::msg::LocalImageDescriptors>::SharedPtr cslam::RGBDHandler::local_descriptors_subscriber_

protected

Definition at line 254 of file rgbd_handler.h.

◆ local_keyframe_match_subscriber_

rclcpp::Subscription< cslam_common_interfaces::msg::LocalKeyframeMatch>::SharedPtr cslam::RGBDHandler::local_keyframe_match_subscriber_

protected

Definition at line 250 of file rgbd_handler.h.

◆ log_publisher_

rclcpp::Publisher< diagnostic_msgs::msg::KeyValue>::SharedPtr cslam::RGBDHandler::log_publisher_

protected

Definition at line 268 of file rgbd_handler.h.

◆ log_total_local_descriptors_cumulative_communication_

unsigned int cslam::RGBDHandler::log_total_local_descriptors_cumulative_communication_

protected

Definition at line 269 of file rgbd_handler.h.

◆ max_nb_robots_

unsigned int cslam::RGBDHandler::max_nb_robots_

protected

Definition at line 225 of file rgbd_handler.h.

◆ max_queue_size_

unsigned int cslam::RGBDHandler::max_queue_size_

protected

Definition at line 225 of file rgbd_handler.h.

◆ min_inliers_

unsigned int cslam::RGBDHandler::min_inliers_

protected

Definition at line 225 of file rgbd_handler.h.

◆ nb_local_keyframes_

unsigned int cslam::RGBDHandler::nb_local_keyframes_

protected

Definition at line 226 of file rgbd_handler.h.

◆ node_

std::shared_ptr<rclcpp::Node> cslam::RGBDHandler::node_

protected

Definition at line 223 of file rgbd_handler.h.

◆ previous_keyframe_

std::shared_ptr<rtabmap::SensorData> cslam::RGBDHandler::previous_keyframe_

protected

Definition at line 219 of file rgbd_handler.h.

◆ received_data_queue_

std::deque<std::pair<std::shared_ptr<rtabmap::SensorData>, nav_msgs::msg::Odometry::ConstSharedPtr> > cslam::RGBDHandler::received_data_queue_

protected

Definition at line 217 of file rgbd_handler.h.

◆ received_gps_queue_

std::deque<sensor_msgs::msg::NavSatFix> cslam::RGBDHandler::received_gps_queue_

protected

Definition at line 289 of file rgbd_handler.h.

◆ registration_

rtabmap::RegistrationVis cslam::RGBDHandler::registration_

protected

Definition at line 256 of file rgbd_handler.h.

◆ robot_id_

unsigned int cslam::RGBDHandler::robot_id_

protected

Definition at line 225 of file rgbd_handler.h.

◆ send_local_descriptors_subscriber_

rclcpp::Subscription< cslam_common_interfaces::msg::LocalDescriptorsRequest>::SharedPtr cslam::RGBDHandler::send_local_descriptors_subscriber_

protected

Definition at line 232 of file rgbd_handler.h.

◆ sub_odometry_

message_filters::Subscriber<nav_msgs::msg::Odometry> cslam::RGBDHandler::sub_odometry_

protected

Definition at line 228 of file rgbd_handler.h.

◆ tf_buffer_

std::shared_ptr<tf2_ros::Buffer> cslam::RGBDHandler::tf_buffer_

protected

Definition at line 273 of file rgbd_handler.h.

◆ tf_listener_

std::shared_ptr<tf2_ros::TransformListener> cslam::RGBDHandler::tf_listener_

protected

Definition at line 274 of file rgbd_handler.h.

◆ visualization_local_descriptors_publisher_

rclcpp::Publisher< cslam_common_interfaces::msg::LocalImageDescriptors>::SharedPtr cslam::RGBDHandler::visualization_local_descriptors_publisher_

protected

Definition at line 237 of file rgbd_handler.h.

◆ visualization_max_range_

float cslam::RGBDHandler::visualization_max_range_

protected

Definition at line 282 of file rgbd_handler.h.

◆ visualization_period_ms_

unsigned int cslam::RGBDHandler::visualization_period_ms_

protected

Definition at line 280 of file rgbd_handler.h.

◆ visualization_voxel_size_

float cslam::RGBDHandler::visualization_voxel_size_

protected

Definition at line 282 of file rgbd_handler.h.

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

/home/lajoiepy/Documents/phd/projects/C-SLAM/Swarm-SLAM/src/cslam/include/cslam/front_end/rgbd_handler.h
/home/lajoiepy/Documents/phd/projects/C-SLAM/Swarm-SLAM/src/cslam/src/front_end/rgbd_handler.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ RGBDHandler()

◆ ~RGBDHandler()

Member Function Documentation

◆ clear_sensor_data()

◆ compute_local_descriptors()

◆ generate_new_keyframe()

◆ gps_callback()

◆ local_descriptors_msg_to_sensor_data()

◆ local_descriptors_request()

◆ process_new_sensor_data()

◆ receive_local_image_descriptors()

◆ receive_local_keyframe_match()

◆ rgbd_callback()

◆ send_keyframe() [1/2]

◆ send_keyframe() [2/2]

◆ send_visualization()

◆ send_visualization_keypoints()

◆ send_visualization_pointcloud()

◆ sensor_data_to_rgbd_msg()

◆ visualization_pointcloud_voxel_subsampling()

Member Data Documentation

◆ base_frame_id_

◆ enable_gps_recording_

◆ enable_logs_

◆ enable_visualization_

◆ generate_new_keyframes_based_on_inliers_ratio_

◆ gps_subscriber_

◆ gps_topic_

◆ inter_robot_loop_closure_publisher_

◆ intra_robot_loop_closure_publisher_

◆ keyframe_data_publisher_

◆ keyframe_generation_ratio_threshold_

◆ keyframe_odom_publisher_

◆ keyframe_pointcloud_publisher_

◆ latest_gps_fix_

◆ local_descriptors_map_

◆ local_descriptors_publisher_

◆ local_descriptors_subscriber_

◆ local_keyframe_match_subscriber_

◆ log_publisher_

◆ log_total_local_descriptors_cumulative_communication_

◆ max_nb_robots_

◆ max_queue_size_

◆ min_inliers_

◆ nb_local_keyframes_

◆ node_

◆ previous_keyframe_

◆ received_data_queue_

◆ received_gps_queue_

◆ registration_

◆ robot_id_

◆ send_local_descriptors_subscriber_

◆ sub_odometry_

◆ tf_buffer_

◆ tf_listener_

◆ visualization_local_descriptors_publisher_

◆ visualization_max_range_

◆ visualization_period_ms_

◆ visualization_voxel_size_