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