Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007 #include <ros/ros.h>
00008 #include "octomap/octomap.h"
00009 #include "pcl_to_octree/octree/OcTreeNodePCL.h"
00010 #include "pcl_to_octree/octree/OcTreePCL.h"
00011 #include "pcl_to_octree/octree/OcTreeServerPCL.h"
00012
00013 #include <pcl/point_types.h>
00014 #include <pcl/io/pcd_io.h>
00015 #include <visualization_msgs/MarkerArray.h>
00016
00017
00018 #include <vector>
00019 #include <iostream>
00020
00021 class OctreeClient
00022 {
00023 public:
00024 OctreeClient();
00025 ~OctreeClient();
00026 void OctreeCallback(const octomap_ros::OctomapBinary& mapMsg);
00027 void run();
00028
00029 private:
00030 ros::NodeHandle nh_;
00031 std::string octree_topic_;
00032 std::string frame_id_;
00033 bool visualize_octree_, visualize_only_occupied_cells_;
00034
00035 ros::Publisher octree_marker_array_publisher_;
00036
00037
00038
00039
00040
00041 ros::Publisher octree_marker_publisher_;
00042
00043
00044 ros::Subscriber octree_subscriber_;
00045
00046
00047 visualization_msgs::MarkerArray octree_marker_array_msg_;
00048 };
00049
00050
00051 OctreeClient::OctreeClient() : nh_("~")
00052 {
00053 nh_.param("octree_topic", octree_topic_, std::string("/pcl_to_octree/octree_binary"));
00054 nh_.param("visualize_octree", visualize_octree_, true);
00055 nh_.param("visualize_only_occupied_cells", visualize_only_occupied_cells_, true);
00056 nh_.param("frame_id", frame_id_, std::string("/map"));
00057 ROS_INFO("octree_client node is up and running.");
00058 run();
00059 }
00060
00061
00062 void OctreeClient::run()
00063 {
00064 octree_marker_array_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
00065
00066 octree_marker_publisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 100);
00067
00068 octree_subscriber_ = nh_.subscribe(octree_topic_, 100, &OctreeClient::OctreeCallback, this);
00069
00070 ros::spin();
00071 }
00072
00073 OctreeClient::~OctreeClient()
00074 {
00075 ROS_INFO("Shutting octree_client node!");
00076 octree_marker_array_publisher_.shutdown();
00077 octree_marker_publisher_.shutdown();
00078 }
00079
00080 void OctreeClient::OctreeCallback(const octomap_ros::OctomapBinary& mapMsg)
00081 {
00082 ROS_INFO("Received an octree.");
00083 octomap::OcTreePCL* octree = new octomap::OcTreePCL(0);
00084 octomap_server::octomapMsgToMap(mapMsg, *octree);
00085 ROS_INFO("OctomapBinary converted to OctreePCL");
00086
00087
00088
00089
00090
00091
00092 if (visualize_octree_)
00093 {
00094
00095 octree_marker_array_msg_.markers.resize(16);
00096 double lowestRes = octree->getResolution();
00097 ROS_INFO_STREAM("lowest resolution: " << lowestRes);
00098 std::list<octomap::OcTreeVolume>::iterator it;
00099
00100 for(unsigned int i = 0; i < 16; i++)
00101 {
00102 std::list<octomap::OcTreeVolume> all_cells;
00103
00104 if(visualize_only_occupied_cells_ == true) {
00105 octree->getOccupied(all_cells, i);
00106 }
00107 else {
00108 octree->getLeafNodes(all_cells, i);
00109 }
00110 for (it = all_cells.begin(); it != all_cells.end(); ++it)
00111 {
00112
00113 int idx = int(log2(it->second / lowestRes) +0.5);
00114 assert (idx >= 0 && unsigned(idx) < octree_marker_array_msg_.markers.size());
00115 geometry_msgs::Point cube_center;
00116 cube_center.x = it->first.x();
00117 cube_center.y = it->first.y();
00118 cube_center.z = it->first.z();
00119 octree_marker_array_msg_.markers[idx].points.push_back(cube_center);
00120 }
00121 }
00122
00123 for (unsigned i = 0; i < octree_marker_array_msg_.markers.size(); ++i)
00124 {
00125 octree_marker_array_msg_.markers[i].header.frame_id = frame_id_;
00126 octree_marker_array_msg_.markers[i].header.stamp = ros::Time::now();
00127
00128 double size = lowestRes * pow(2,i);
00129
00130 std::stringstream ss;
00131 ss <<"Level "<<i;
00132 octree_marker_array_msg_.markers[i].ns = ss.str();
00133 octree_marker_array_msg_.markers[i].id = i;
00134 octree_marker_array_msg_.markers[i].lifetime = ros::Duration::Duration();
00135 octree_marker_array_msg_.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00136 octree_marker_array_msg_.markers[i].scale.x = size;
00137 octree_marker_array_msg_.markers[i].scale.y = size;
00138 octree_marker_array_msg_.markers[i].scale.z = size;
00139 octree_marker_array_msg_.markers[i].color.r = 1.0f;
00140 octree_marker_array_msg_.markers[i].color.g = 0.0f;
00141 octree_marker_array_msg_.markers[i].color.b = 0.0f;
00142 octree_marker_array_msg_.markers[i].color.a = 0.5f;
00143
00144
00145 if (octree_marker_array_msg_.markers[i].points.size() > 0)
00146 octree_marker_array_msg_.markers[i].action = visualization_msgs::Marker::ADD;
00147 else
00148 octree_marker_array_msg_.markers[i].action = visualization_msgs::Marker::DELETE;
00149
00150 }
00151
00152 octree_marker_array_publisher_.publish(octree_marker_array_msg_);
00153
00154
00155 for (unsigned int i = 0; i < octree_marker_array_msg_.markers.size(); i++)
00156 {
00157 if (!octree_marker_array_msg_.markers[i].points.empty())
00158 {
00159 octree_marker_array_msg_.markers[i].points.clear();
00160 }
00161 }
00162 octree_marker_array_msg_.markers.clear();
00163 }
00164
00165 free(octree);
00166
00167 }
00168
00169 int main(int argc, char** argv)
00170 {
00171 ros::init(argc, argv, "octree_client");
00172 OctreeClient octree_client;
00173 return (0);
00174 }