octree_client_vanilla.cpp
Go to the documentation of this file.
00001 /*
00002  * Based on the original code by Kai M. Wurm and Armin Hornung
00003  * (http://octomap.sourceforge.net)
00004  * Author: Hozefa Indorewala
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 #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         // Publishes the octree in MarkerArray format so that it can be visualized in rviz
00035   ros::Publisher octree_marker_array_publisher_;
00036         
00037         /*
00038          * The following publisher, even though not required, is used because otherwise rviz 
00039          * cannot visualize the MarkerArray format without advertising the Marker format
00040          */
00041   ros::Publisher octree_marker_publisher_;
00042         
00043         // Subscribes to the Octree
00044         ros::Subscriber octree_subscriber_;
00045         
00046         // Marker array to visualize the octree. It displays the occuplied cells of the octree
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   //create new OcTree with arbitrary resolution - 0.25 in this case
00082   //the resolution gets overridden from the incoming octree
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   //ROS_INFO("Octree Node List size: %ld",octree->octree_node_list.size());
00088 
00089   //**********************************************************************************
00090   //Visualization of Octree
00091   //**********************************************************************************
00092   if (visualize_octree_)
00093   {
00094     // each array stores all cubes of a different size, one for each depth level:
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       //getting the occupied cells at different depths of the octree
00103       //    octree->getOccupied(all_cells, i);
00104       octree->getLeafNodes(all_cells, i);
00105       for (it = all_cells.begin(); it != all_cells.end(); ++it)
00106       {
00107         // which array to store cubes in?
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_to_octree
Author(s): Hozefa Indorewala, Dejan Pangercic
autogenerated on Thu May 23 2013 08:28:50