occupancy_map_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  * Author: Julius Kammerl (jkammerl@willowgarage.com)
00030  *
00031  */
00032 
00033 
00034 #include "octomap_rviz_plugins/occupancy_map_display.h"
00035 
00036 #include "rviz/visualization_manager.h"
00037 #include "rviz/properties/int_property.h"
00038 #include "rviz/properties/ros_topic_property.h"
00039 
00040 #include <octomap/octomap.h>
00041 #include <octomap_msgs/Octomap.h>
00042 #include <octomap_msgs/conversions.h>
00043 
00044 using namespace rviz;
00045 
00046 namespace octomap_rviz_plugin
00047 {
00048 
00049 static const std::size_t max_octree_depth_ = sizeof(unsigned short) * 8;
00050 
00051 OccupancyMapDisplay::OccupancyMapDisplay()
00052   : rviz::MapDisplay()
00053   , octree_depth_ (max_octree_depth_)
00054 {
00055 
00056   topic_property_->setName("Octomap Binary Topic");
00057   topic_property_->setMessageType(QString::fromStdString(ros::message_traits::datatype<octomap_msgs::Octomap>()));
00058   topic_property_->setDescription("octomap_msgs::OctomapBinary topic to subscribe to.");
00059 
00060   tree_depth_property_ = new IntProperty("Max. Octree Depth",
00061                                          octree_depth_,
00062                                          "Defines the maximum tree depth",
00063                                          this,
00064                                          SLOT (updateTreeDepth() ));
00065 }
00066 
00067 OccupancyMapDisplay::~OccupancyMapDisplay()
00068 {
00069   unsubscribe();
00070 }
00071 
00072 void OccupancyMapDisplay::onInitialize()
00073 {
00074   rviz::MapDisplay::onInitialize();
00075 }
00076 
00077 void OccupancyMapDisplay::updateTreeDepth()
00078 {
00079   octree_depth_ = tree_depth_property_->getInt();
00080 }
00081 
00082 void OccupancyMapDisplay::updateTopic()
00083 {
00084   unsubscribe();
00085   reset();
00086   subscribe();
00087   context_->queueRender();
00088 }
00089 
00090 void OccupancyMapDisplay::subscribe()
00091 {
00092   if (!isEnabled())
00093   {
00094     return;
00095   }
00096 
00097   try
00098   {
00099     unsubscribe();
00100 
00101     const std::string& topicStr = topic_property_->getStdString();
00102 
00103     if (!topicStr.empty())
00104     {
00105 
00106       sub_.reset(new message_filters::Subscriber<octomap_msgs::Octomap>());
00107 
00108       sub_->subscribe(threaded_nh_, topicStr, 5);
00109       sub_->registerCallback(boost::bind(&OccupancyMapDisplay::handleOctomapBinaryMessage, this, _1));
00110 
00111     }
00112   }
00113   catch (ros::Exception& e)
00114   {
00115     setStatus(StatusProperty::Error, "Topic", (std::string("Error subscribing: ") + e.what()).c_str());
00116   }
00117 }
00118 
00119 void OccupancyMapDisplay::unsubscribe()
00120 {
00121   clear();
00122 
00123   try
00124   {
00125     // reset filters
00126     sub_.reset();
00127   }
00128   catch (ros::Exception& e)
00129   {
00130     setStatus(StatusProperty::Error, "Topic", (std::string("Error unsubscribing: ") + e.what()).c_str());
00131   }
00132 }
00133 
00134 
00135 void OccupancyMapDisplay::handleOctomapBinaryMessage(const octomap_msgs::OctomapConstPtr& msg)
00136 {
00137 
00138   ROS_DEBUG("Received OctomapBinary message (size: %d bytes)", (int)msg->data.size());
00139 
00140   // creating octree
00141   octomap::OcTree* octomap = NULL;
00142   octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(*msg);
00143   if (tree){
00144     octomap = dynamic_cast<octomap::OcTree*>(tree);
00145   }
00146 
00147   if (!octomap)
00148   {
00149     this->setStatusStd(StatusProperty::Error, "Message", "Failed to create octree structure");
00150     return;
00151   }
00152 
00153   // get dimensions of octree
00154   double minX, minY, minZ, maxX, maxY, maxZ;
00155   octomap->getMetricMin(minX, minY, minZ);
00156   octomap->getMetricMax(maxX, maxY, maxZ);
00157   octomap::point3d minPt = octomap::point3d(minX, minY, minZ);
00158 
00159   unsigned int tree_depth = octomap->getTreeDepth();
00160 
00161   octomap::OcTreeKey paddedMinKey = octomap->coordToKey(minPt);
00162 
00163   nav_msgs::OccupancyGrid::Ptr occupancy_map (new nav_msgs::OccupancyGrid());
00164 
00165   unsigned int width, height;
00166   double res;
00167 
00168   unsigned int ds_shift = tree_depth-octree_depth_;
00169 
00170   occupancy_map->header = msg->header;
00171   occupancy_map->info.resolution = res = octomap->getNodeSize(octree_depth_);
00172   occupancy_map->info.width = width = (maxX-minX) / res + 1;
00173   occupancy_map->info.height = height = (maxY-minY) / res + 1;
00174   occupancy_map->info.origin.position.x = minX  - (res / (float)(1<<ds_shift) ) + res;
00175   occupancy_map->info.origin.position.y = minY  - (res / (float)(1<<ds_shift) );;
00176 
00177   occupancy_map->data.clear();
00178   occupancy_map->data.resize(width*height, -1);
00179 
00180     // traverse all leafs in the tree:
00181   unsigned int treeDepth = std::min<unsigned int>(octree_depth_, octomap->getTreeDepth());
00182   for (octomap::OcTree::iterator it = octomap->begin(treeDepth), end = octomap->end(); it != end; ++it)
00183   {
00184     bool occupied = octomap->isNodeOccupied(*it);
00185     int intSize = 1 << (octree_depth_ - it.getDepth());
00186 
00187     octomap::OcTreeKey minKey=it.getIndexKey();
00188 
00189     for (int dx = 0; dx < intSize; dx++)
00190     {
00191       for (int dy = 0; dy < intSize; dy++)
00192       {
00193         int posX = std::max<int>(0, minKey[0] + dx - paddedMinKey[0]);
00194         posX>>=ds_shift;
00195 
00196         int posY = std::max<int>(0, minKey[1] + dy - paddedMinKey[1]);
00197         posY>>=ds_shift;
00198 
00199         int idx = width * posY + posX;
00200 
00201         if (occupied)
00202           occupancy_map->data[idx] = 100;
00203         else if (occupancy_map->data[idx] == -1)
00204         {
00205           occupancy_map->data[idx] = 0;
00206         }
00207 
00208       }
00209     }
00210 
00211   }
00212 
00213   delete octomap;
00214 
00215   this->incomingMap(occupancy_map);
00216 }
00217 
00218 } // namespace rviz
00219 
00220 #include <pluginlib/class_list_macros.h>
00221 PLUGINLIB_EXPORT_CLASS( octomap_rviz_plugin::OccupancyMapDisplay, rviz::Display)


octomap_rviz_plugins
Author(s): Julius Kammerl
autogenerated on Mon Oct 6 2014 03:00:57