lidar_mapping_node.cpp
Go to the documentation of this file.
1 
3 
4 // Original Author of FLOAM: Wang Han
5 // Email wh200720041@gmail.com
6 // Homepage https://wanghan.pro
7 
8 #include <ros/ros.h>
9 #include <sensor_msgs/PointCloud2.h>
11 #include <nav_msgs/Odometry.h>
12 #include <tf/transform_datatypes.h>
14 
15 //pcl lib
17 #include <pcl/point_cloud.h>
18 #include <pcl/point_types.h>
19 
20 //local lib
22 #include "floam/lidar_mapping.hpp"
23 
24 namespace floam
25 {
26 namespace lidar
27 {
28 
30 {
31  // constructor
32 }
33 
35 {
36  // destructor
37 }
38 
40 {
42 
43  double map_resolution = 0.4;
44 
45  m_nodeHandle.getParam("use_exact_sync", m_useExactSync);
46  m_nodeHandle.getParam("queue_size", m_queueSize);
47  m_nodeHandle.getParam("map_resolution", map_resolution);
48 
49  m_lidarMapping.init(map_resolution);
50 
51  // should these topic names be parameters instead of remapped?
52  m_subPoints.subscribe(m_nodeHandle, "points_filtered", 1000);
53  m_subOdom.subscribe(m_nodeHandle, "odom", 1000);
54 
55  m_pubMap = m_nodeHandle.advertise<sensor_msgs::PointCloud2>("map", 100);
56 
57  // initialize callbacks using sync policy
58  if (m_useExactSync) {
59  ROS_INFO("Exact Synchronization Policy chosen for Mapping");
61  m_exactSync->registerCallback(
62  std::bind(&LidarMappingNode::generateMap, this, std::placeholders::_1, std::placeholders::_2));
63  } else {
64  ROS_INFO("Approximate Synchronization Policy chosen for Mapping");
66  m_approximateSync->registerCallback(
67  std::bind(&LidarMappingNode::generateMap, this, std::placeholders::_1, std::placeholders::_2));
68  }
69 }
70 
72  const nav_msgs::OdometryConstPtr & odom,
73  const sensor_msgs::PointCloud2ConstPtr & cloud)
74 {
75  pcl::PointCloud<pcl::PointXYZL>::Ptr pointcloud_in(new pcl::PointCloud<pcl::PointXYZL>());
76  pcl::fromROSMsg(*cloud, *pointcloud_in);
77  ros::Time pointcloud_time = cloud->header.stamp;
78 
79  Eigen::Isometry3d current_pose = Eigen::Isometry3d::Identity();
80 
81  current_pose.rotate(
82  Eigen::Quaterniond(
83  odom->pose.pose.orientation.w,
84  odom->pose.pose.orientation.x,
85  odom->pose.pose.orientation.y,
86  odom->pose.pose.orientation.z));
87 
88  current_pose.pretranslate(
89  Eigen::Vector3d(
90  odom->pose.pose.position.x,
91  odom->pose.pose.position.y,
92  odom->pose.pose.position.z));
93 
94  m_lidarMapping.updateCurrentPointsToMap(pointcloud_in, current_pose);
95  pcl::PointCloud<pcl::PointXYZL>::Ptr pc_map(new pcl::PointCloud<pcl::PointXYZL>());
96  pc_map = m_lidarMapping.getMap();
97  sensor_msgs::PointCloud2 PointsMsg;
98  pcl::toROSMsg(*pc_map, PointsMsg);
99  PointsMsg.header.stamp = pointcloud_time;
100  PointsMsg.header.frame_id = "map";
101  m_pubMap.publish(PointsMsg);
102 }
103 
104 } // namespace lidar
105 } // namespace floam
106 
107 
108 #include <pluginlib/class_list_macros.h> // NO LINT
floam::lidar::LidarMapping::getMap
pcl::PointCloud< pcl::PointXYZL >::Ptr getMap(void)
Definition: lidar_mapping.cpp:208
floam::lidar::LidarMappingNode::m_queueSize
int m_queueSize
Definition: lidar_mapping_node.hpp:76
floam::lidar::LidarMappingNode::m_exactSync
std::shared_ptr< ExactSynchronizer > m_exactSync
Definition: lidar_mapping_node.hpp:67
floam::lidar::LidarMappingNode::m_useExactSync
bool m_useExactSync
Definition: lidar_mapping_node.hpp:75
floam::lidar::ApproximateSyncPolicy
message_filters::sync_policies::ApproximateTime< nav_msgs::Odometry, sensor_msgs::PointCloud2 > ApproximateSyncPolicy
Definition: lidar_mapping_node.hpp:34
lidar_mapping_node.hpp
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ros.h
floam::lidar::LidarMappingNode::onInit
void onInit()
Definition: lidar_mapping_node.cpp:39
floam::lidar::ExactSyncPolicy
message_filters::sync_policies::ExactTime< nav_msgs::Odometry, sensor_msgs::PointCloud2 > ExactSyncPolicy
Definition: lidar_mapping_node.hpp:33
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(floam::lidar::LidarMappingNode, nodelet::Nodelet)
floam::lidar::LidarMappingNode::~LidarMappingNode
~LidarMappingNode()
Definition: lidar_mapping_node.cpp:34
floam::lidar::LidarMappingNode::m_subPoints
message_filters::Subscriber< sensor_msgs::PointCloud2 > m_subPoints
Definition: lidar_mapping_node.hpp:63
floam::lidar::LidarMappingNode::generateMap
void generateMap(const nav_msgs::OdometryConstPtr &odom, const sensor_msgs::PointCloud2ConstPtr &cloud)
Definition: lidar_mapping_node.cpp:71
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
floam::lidar::LidarMappingNode::m_subOdom
message_filters::Subscriber< nav_msgs::Odometry > m_subOdom
Definition: lidar_mapping_node.hpp:64
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
transform_broadcaster.h
floam::lidar::LidarMappingNode::m_pubMap
ros::Publisher m_pubMap
Definition: lidar_mapping_node.hpp:58
class_list_macros.h
floam::lidar::LidarMappingNode::LidarMappingNode
LidarMappingNode()
Definition: lidar_mapping_node.cpp:29
subscriber.h
floam::lidar::LidarMappingNode
Definition: lidar_mapping_node.hpp:36
floam::lidar::LidarMappingNode::ApproximateSynchronizer
message_filters::Synchronizer< ApproximateSyncPolicy > ApproximateSynchronizer
Definition: lidar_mapping_node.hpp:60
message_filters::Subscriber::subscribe
void subscribe()
lidar_mapping.hpp
floam::lidar::LidarMapping::init
void init(const double &map_resolution)
Definition: lidar_mapping.cpp:21
nodelet::Nodelet
transform_datatypes.h
ros::Time
floam::lidar::LidarMappingNode::m_nodeHandle
ros::NodeHandle m_nodeHandle
Definition: lidar_mapping_node.hpp:57
floam::lidar::LidarMappingNode::m_lidarMapping
LidarMapping m_lidarMapping
Definition: lidar_mapping_node.hpp:73
pcl::toROSMsg
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
floam::lidar::LidarMapping::updateCurrentPointsToMap
void updateCurrentPointsToMap(const pcl::PointCloud< pcl::PointXYZL >::Ptr &pc_in, const Eigen::Isometry3d &pose_current)
Definition: lidar_mapping.cpp:171
ROS_INFO
#define ROS_INFO(...)
floam::lidar::LidarMappingNode::ExactSynchronizer
message_filters::Synchronizer< ExactSyncPolicy > ExactSynchronizer
Definition: lidar_mapping_node.hpp:61
floam::lidar::LidarMappingNode::m_approximateSync
std::shared_ptr< ApproximateSynchronizer > m_approximateSync
Definition: lidar_mapping_node.hpp:66
floam
Major rewrite Author: Evan Flynn.
Definition: lidar.hpp:15
pcl_conversions.h


floam
Author(s): Han Wang
autogenerated on Wed Mar 2 2022 00:23:09