9 #include <sensor_msgs/PointCloud2.h>
11 #include <nav_msgs/Odometry.h>
17 #include <pcl/point_cloud.h>
18 #include <pcl/point_types.h>
43 double map_resolution = 0.4;
59 ROS_INFO(
"Exact Synchronization Policy chosen for Mapping");
64 ROS_INFO(
"Approximate Synchronization Policy chosen for Mapping");
72 const nav_msgs::OdometryConstPtr & odom,
73 const sensor_msgs::PointCloud2ConstPtr & cloud)
75 pcl::PointCloud<pcl::PointXYZL>::Ptr pointcloud_in(
new pcl::PointCloud<pcl::PointXYZL>());
77 ros::Time pointcloud_time = cloud->header.stamp;
79 Eigen::Isometry3d current_pose = Eigen::Isometry3d::Identity();
83 odom->pose.pose.orientation.w,
84 odom->pose.pose.orientation.x,
85 odom->pose.pose.orientation.y,
86 odom->pose.pose.orientation.z));
88 current_pose.pretranslate(
90 odom->pose.pose.position.x,
91 odom->pose.pose.position.y,
92 odom->pose.pose.position.z));
95 pcl::PointCloud<pcl::PointXYZL>::Ptr pc_map(
new pcl::PointCloud<pcl::PointXYZL>());
97 sensor_msgs::PointCloud2 PointsMsg;
99 PointsMsg.header.stamp = pointcloud_time;
100 PointsMsg.header.frame_id =
"map";