Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007 #include <ros/ros.h>
00008 #include <sensor_msgs/PointCloud.h>
00009 #include <sensor_msgs/PointCloud2.h>
00010 #include <sensor_msgs/point_cloud_conversion.h>
00011 #include "octomap/octomap.h"
00012
00013
00014
00015 #include <octomap_ros/conversions.h>
00016 #include <octomap/octomap.h>
00017 #include <pcl/point_types.h>
00018 #include <pcl/io/pcd_io.h>
00019 #include <pcl/ros/conversions.h>
00020 #include <visualization_msgs/MarkerArray.h>
00021
00022 #include <vector>
00023 #include <iostream>
00024
00025 class PclToOctree
00026 {
00027 public:
00028 PclToOctree();
00029 ~PclToOctree();
00030 void pclToOctreeCallback(const sensor_msgs::PointCloud& msg);
00031 void run();
00032
00033 private:
00034 ros::NodeHandle nh_;
00035 double laser_offset_, octree_res_;
00036 int octree_maxrange_, level_;
00037 std::string point_cloud_topic_, frame_id_;
00038 bool visualize_octree_;
00039
00040 ros::Publisher octree_marker_array_publisher_;
00041
00042
00043
00044
00045
00046 ros::Publisher octree_marker_publisher_;
00047
00048 ros::Publisher octree_binary_publisher_;
00049
00050
00051 ros::Subscriber pointcloud_subscriber_;
00052
00053
00054 visualization_msgs::MarkerArray octree_marker_array_msg_;
00055 };
00056
00057
00058
00059
00060 PclToOctree::PclToOctree() : nh_("~")
00061 {
00062 nh_.param("laser_offset", laser_offset_, 1.5);
00063 nh_.param("octree_resolution", octree_res_, 0.5);
00064 nh_.param("octree_maxrange", octree_maxrange_, -1);
00065 nh_.param("point_cloud_topic", point_cloud_topic_, std::string("/point_cloud"));
00066 nh_.param("frame_id", frame_id_, std::string("/map"));
00067 nh_.param("level", level_, 0);
00068 nh_.param("visualize_octree", visualize_octree_, false);
00069 ROS_INFO("pcl_to_octree node is up and running.");
00070 run();
00071 }
00072
00073
00074 void PclToOctree::run()
00075 {
00076 octree_binary_publisher_ = nh_.advertise<octomap_ros::OctomapBinary>("octree_binary", 100);
00077
00078 pointcloud_subscriber_ = nh_.subscribe(point_cloud_topic_, 100, &PclToOctree::pclToOctreeCallback, this);
00079
00080 ros::spin();
00081 }
00082
00083 PclToOctree::~PclToOctree()
00084 {
00085 ROS_INFO("Shutting down pcl_to_octree node!");
00086 }
00087
00088 void PclToOctree::pclToOctreeCallback(const sensor_msgs::PointCloud& pointcloud_msg)
00089 {
00090 ROS_INFO("Received a point cloud.");
00091 sensor_msgs::PointCloud2 pointcloud2_msg;
00092 octomap_ros::OctomapBinary octree_msg;
00093
00094
00095 sensor_msgs::convertPointCloudToPointCloud2(pointcloud_msg, pointcloud2_msg);
00096 pcl::PointCloud<pcl::PointXYZ> pointcloud2_pcl;
00097
00098 octomap::point3d octomap_3d_point;
00099 octomap::Pointcloud octomap_pointcloud;
00100
00101
00102 pcl::fromROSMsg(pointcloud2_msg, pointcloud2_pcl);
00103
00104
00105 for(unsigned int i =0; i < pointcloud2_pcl.points.size(); i++)
00106 {
00107 octomap_3d_point(0) = pointcloud2_pcl.points[i].x;
00108 octomap_3d_point(1) = pointcloud2_pcl.points[i].y;
00109 octomap_3d_point(2) = pointcloud2_pcl.points[i].z;
00110 octomap_pointcloud.push_back(octomap_3d_point);
00111 }
00112
00113
00114 octomap::pose6d offset_trans(0,0,-laser_offset_,0,0,0);
00115 octomap::pose6d laser_pose(0,0,laser_offset_,0,0,0);
00116 octomap_pointcloud.transform(offset_trans);
00117
00118
00119 octomap::ScanGraph* octomap_graph = new octomap::ScanGraph();
00120 octomap_graph->addNode(&octomap_pointcloud, laser_pose);
00121
00122 ROS_INFO("Number of points in graph: %d", octomap_graph->getNumPoints());
00123
00124
00125 octomap::OcTree* octree = new octomap::OcTree(octree_res_);
00126 for (octomap::ScanGraph::iterator scan_it = octomap_graph->begin(); scan_it != octomap_graph->end(); scan_it++)
00127 {
00128 octree->insertScan(**scan_it, octree_maxrange_, false);
00129 }
00130
00131
00132
00133 octomap::octomapMapToMsg(*octree, octree_msg);
00134 octree_binary_publisher_.publish(octree_msg);
00135 ROS_INFO("OctreeBinary built and published");
00136 }
00137
00138 int main(int argc, char** argv)
00139 {
00140 ros::init(argc, argv, "pcl_to_octree_vanilla");
00141 PclToOctree pcl_to_octree;
00142 return (0);
00143 }