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 template <typename OcTreeType>
00136 void TemplatedOccupancyMapDisplay<OcTreeType>::handleOctomapBinaryMessage(const octomap_msgs::OctomapConstPtr& msg)
00137 {
00138 
00139   ROS_DEBUG("Received OctomapBinary message (size: %d bytes)", (int)msg->data.size());
00140 
00141   // creating octree
00142   OcTreeType* octomap = NULL;
00143   octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(*msg);
00144   if (tree){
00145     octomap = dynamic_cast<OcTreeType*>(tree);
00146   }
00147 
00148   if (!octomap)
00149   {
00150     this->setStatusStd(StatusProperty::Error, "Message", "Failed to create octree structure");
00151     return;
00152   }
00153 
00154   // get dimensions of octree
00155   double minX, minY, minZ, maxX, maxY, maxZ;
00156   octomap->getMetricMin(minX, minY, minZ);
00157   octomap->getMetricMax(maxX, maxY, maxZ);
00158   octomap::point3d minPt = octomap::point3d(minX, minY, minZ);
00159 
00160   unsigned int tree_depth = octomap->getTreeDepth();
00161 
00162   octomap::OcTreeKey paddedMinKey = octomap->coordToKey(minPt);
00163 
00164   nav_msgs::OccupancyGrid::Ptr occupancy_map (new nav_msgs::OccupancyGrid());
00165 
00166   unsigned int width, height;
00167   double res;
00168 
00169   unsigned int ds_shift = tree_depth-octree_depth_;
00170 
00171   occupancy_map->header = msg->header;
00172   occupancy_map->info.resolution = res = octomap->getNodeSize(octree_depth_);
00173   occupancy_map->info.width = width = (maxX-minX) / res + 1;
00174   occupancy_map->info.height = height = (maxY-minY) / res + 1;
00175   occupancy_map->info.origin.position.x = minX  - (res / (float)(1<<ds_shift) ) + res;
00176   occupancy_map->info.origin.position.y = minY  - (res / (float)(1<<ds_shift) );
00177 
00178   occupancy_map->data.clear();
00179   occupancy_map->data.resize(width*height, -1);
00180 
00181     // traverse all leafs in the tree:
00182   unsigned int treeDepth = std::min<unsigned int>(octree_depth_, octomap->getTreeDepth());
00183   for (typename OcTreeType::iterator it = octomap->begin(treeDepth), end = octomap->end(); it != end; ++it)
00184   {
00185     bool occupied = octomap->isNodeOccupied(*it);
00186     int intSize = 1 << (octree_depth_ - it.getDepth());
00187 
00188     octomap::OcTreeKey minKey=it.getIndexKey();
00189 
00190     for (int dx = 0; dx < intSize; dx++)
00191     {
00192       for (int dy = 0; dy < intSize; dy++)
00193       {
00194         int posX = std::max<int>(0, minKey[0] + dx - paddedMinKey[0]);
00195         posX>>=ds_shift;
00196 
00197         int posY = std::max<int>(0, minKey[1] + dy - paddedMinKey[1]);
00198         posY>>=ds_shift;
00199 
00200         int idx = width * posY + posX;
00201 
00202         if (occupied)
00203           occupancy_map->data[idx] = 100;
00204         else if (occupancy_map->data[idx] == -1)
00205         {
00206           occupancy_map->data[idx] = 0;
00207         }
00208 
00209       }
00210     }
00211 
00212   }
00213 
00214   delete octomap;
00215 
00216   this->incomingMap(occupancy_map);
00217 }
00218 
00219 } // namespace rviz
00220 
00221 #include <pluginlib/class_list_macros.h>
00222 typedef octomap_rviz_plugin::TemplatedOccupancyMapDisplay<octomap::OcTree> OcTreeMapDisplay;
00223 typedef octomap_rviz_plugin::TemplatedOccupancyMapDisplay<octomap::OcTreeStamped> OcTreeStampedMapDisplay;
00224 
00225 PLUGINLIB_EXPORT_CLASS( OcTreeMapDisplay, rviz::Display)
00226 PLUGINLIB_EXPORT_CLASS( OcTreeStampedMapDisplay, rviz::Display)


octomap_rviz_plugins
Author(s): Julius Kammerl
autogenerated on Thu Jun 6 2019 18:26:16