Classes | Public Member Functions | Private Member Functions | Private Attributes | List of all members
map_merge_3d::MapMerge3d Class Reference

ROS node class. More...

#include <map_merge_node.h>

Classes

struct  MapSubscription
 

Public Member Functions

void discovery ()
 Initiates discovery of new robots (maps) under current ROS core. More...
 
std::vector< PointCloudConstPtrgetMaps ()
 Get currently stored maps. More...
 
std::vector< Eigen::Matrix4f > getTransforms ()
 Get currently stored transforms. For each map there should be a transform between map and global reference frame. More...
 
void mapCompositing ()
 Composes and publishes the global map based on estimated transformations. More...
 
 MapMerge3d ()
 
void transformsEstimation ()
 Estimates transformations between maps. More...
 

Private Member Functions

bool isRobotMapTopic (const ros::master::TopicInfo &topic)
 
void mapUpdate (const PointCloud::ConstPtr &msg, MapSubscription &subscription)
 
void publishTF ()
 
std::string robotNameFromTopic (const std::string &topic)
 

Private Attributes

double compositing_rate_
 
ros::Timer compositing_timer_
 
double discovery_rate_
 
ros::Timer discovery_timer_
 
double estimation_rate_
 
ros::Timer estimation_timer_
 
MapMergingParams map_merge_params_
 
ros::Publisher merged_map_publisher_
 
ros::NodeHandle node_
 
std::string robot_map_topic_
 
std::string robot_namespace_
 
std::unordered_map< std::string, MapSubscription * > robots_
 
std::forward_list< MapSubscriptionsubscriptions_
 
std::mutex subscriptions_mutex_
 
size_t subscriptions_size_
 
std::atomic_flag tf_current_flag_
 
tf2_ros::TransformBroadcaster tf_publisher_
 
std::thread tf_thread_
 
std::vector< geometry_msgs::TransformStamped > tf_transforms_
 
std::vector< Eigen::Matrix4f > transforms_
 
std::mutex transforms_mutex_
 
std::string world_frame_
 

Detailed Description

ROS node class.

Runs robot discovery, transforms estimation and map compositing at predefined rates (from ROS parameters). Does not spin on its own.

Definition at line 32 of file map_merge_node.h.

Constructor & Destructor Documentation

◆ MapMerge3d()

map_merge_3d::MapMerge3d::MapMerge3d ( )

Member Function Documentation

◆ discovery()

void map_merge_3d::MapMerge3d::discovery ( )

Initiates discovery of new robots (maps) under current ROS core.

When new maps topics are found, there are added for merging. This function is thread-safe

◆ getMaps()

std::vector<PointCloudConstPtr> map_merge_3d::MapMerge3d::getMaps ( )

Get currently stored maps.

This function is thread-safe

Returns
all currently received maps

◆ getTransforms()

std::vector<Eigen::Matrix4f> map_merge_3d::MapMerge3d::getTransforms ( )

Get currently stored transforms. For each map there should be a transform between map and global reference frame.

This function is thread-safe

Returns
all currently estimated transforms. Zero Matrix if transform could not be estimated.

◆ isRobotMapTopic()

bool map_merge_3d::MapMerge3d::isRobotMapTopic ( const ros::master::TopicInfo topic)
private

◆ mapCompositing()

void map_merge_3d::MapMerge3d::mapCompositing ( )

Composes and publishes the global map based on estimated transformations.

This function is thread-safe

◆ mapUpdate()

void map_merge_3d::MapMerge3d::mapUpdate ( const PointCloud::ConstPtr &  msg,
MapSubscription subscription 
)
private

◆ publishTF()

void map_merge_3d::MapMerge3d::publishTF ( )
private

◆ robotNameFromTopic()

std::string map_merge_3d::MapMerge3d::robotNameFromTopic ( const std::string &  topic)
private

◆ transformsEstimation()

void map_merge_3d::MapMerge3d::transformsEstimation ( )

Estimates transformations between maps.

This function is thread-safe

Member Data Documentation

◆ compositing_rate_

double map_merge_3d::MapMerge3d::compositing_rate_
private

Definition at line 45 of file map_merge_node.h.

◆ compositing_timer_

ros::Timer map_merge_3d::MapMerge3d::compositing_timer_
private

Definition at line 57 of file map_merge_node.h.

◆ discovery_rate_

double map_merge_3d::MapMerge3d::discovery_rate_
private

Definition at line 46 of file map_merge_node.h.

◆ discovery_timer_

ros::Timer map_merge_3d::MapMerge3d::discovery_timer_
private

Definition at line 58 of file map_merge_node.h.

◆ estimation_rate_

double map_merge_3d::MapMerge3d::estimation_rate_
private

Definition at line 47 of file map_merge_node.h.

◆ estimation_timer_

ros::Timer map_merge_3d::MapMerge3d::estimation_timer_
private

Definition at line 59 of file map_merge_node.h.

◆ map_merge_params_

MapMergingParams map_merge_3d::MapMerge3d::map_merge_params_
private

Definition at line 52 of file map_merge_node.h.

◆ merged_map_publisher_

ros::Publisher map_merge_3d::MapMerge3d::merged_map_publisher_
private

Definition at line 55 of file map_merge_node.h.

◆ node_

ros::NodeHandle map_merge_3d::MapMerge3d::node_
private

Definition at line 42 of file map_merge_node.h.

◆ robot_map_topic_

std::string map_merge_3d::MapMerge3d::robot_map_topic_
private

Definition at line 48 of file map_merge_node.h.

◆ robot_namespace_

std::string map_merge_3d::MapMerge3d::robot_namespace_
private

Definition at line 49 of file map_merge_node.h.

◆ robots_

std::unordered_map<std::string, MapSubscription*> map_merge_3d::MapMerge3d::robots_
private

Definition at line 68 of file map_merge_node.h.

◆ subscriptions_

std::forward_list<MapSubscription> map_merge_3d::MapMerge3d::subscriptions_
private

Definition at line 70 of file map_merge_node.h.

◆ subscriptions_mutex_

std::mutex map_merge_3d::MapMerge3d::subscriptions_mutex_
private

Definition at line 72 of file map_merge_node.h.

◆ subscriptions_size_

size_t map_merge_3d::MapMerge3d::subscriptions_size_
private

Definition at line 71 of file map_merge_node.h.

◆ tf_current_flag_

std::atomic_flag map_merge_3d::MapMerge3d::tf_current_flag_
private

Definition at line 64 of file map_merge_node.h.

◆ tf_publisher_

tf2_ros::TransformBroadcaster map_merge_3d::MapMerge3d::tf_publisher_
private

Definition at line 62 of file map_merge_node.h.

◆ tf_thread_

std::thread map_merge_3d::MapMerge3d::tf_thread_
private

Definition at line 63 of file map_merge_node.h.

◆ tf_transforms_

std::vector<geometry_msgs::TransformStamped> map_merge_3d::MapMerge3d::tf_transforms_
private

Definition at line 61 of file map_merge_node.h.

◆ transforms_

std::vector<Eigen::Matrix4f> map_merge_3d::MapMerge3d::transforms_
private

Definition at line 74 of file map_merge_node.h.

◆ transforms_mutex_

std::mutex map_merge_3d::MapMerge3d::transforms_mutex_
private

Definition at line 75 of file map_merge_node.h.

◆ world_frame_

std::string map_merge_3d::MapMerge3d::world_frame_
private

Definition at line 50 of file map_merge_node.h.


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


map_merge_3d
Author(s): Jiri Horner
autogenerated on Mon Feb 28 2022 22:47:17