OctomapServer.h
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright (c) 2010-2012, A. Hornung, University of Freiburg
00011  * All rights reserved.
00012  *
00013  * Redistribution and use in source and binary forms, with or without
00014  * modification, are permitted provided that the following conditions are met:
00015  *
00016  *     * Redistributions of source code must retain the above copyright
00017  *       notice, this list of conditions and the following disclaimer.
00018  *     * Redistributions in binary form must reproduce the above copyright
00019  *       notice, this list of conditions and the following disclaimer in the
00020  *       documentation and/or other materials provided with the distribution.
00021  *     * Neither the name of the University of Freiburg nor the names of its
00022  *       contributors may be used to endorse or promote products derived from
00023  *       this software without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00028  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00029  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00030  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00031  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00032  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00033  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00034  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  */
00037 
00038 #ifndef OCTOMAP_SERVER_OCTOMAPSERVER_H
00039 #define OCTOMAP_SERVER_OCTOMAPSERVER_H
00040 
00041 #include <ros/ros.h>
00042 #include <visualization_msgs/MarkerArray.h>
00043 #include <nav_msgs/OccupancyGrid.h>
00044 #include <std_msgs/ColorRGBA.h>
00045 #include <arm_navigation_msgs/CollisionObject.h>
00046 #include <arm_navigation_msgs/CollisionMap.h>
00047 #include <sensor_msgs/PointCloud2.h>
00048 #include <std_srvs/Empty.h>
00049 #include <dynamic_reconfigure/server.h>
00050 #include <octomap_server/OctomapServerConfig.h>
00051 
00052 #include <pcl/point_types.h>
00053 #include <pcl/ros/conversions.h>
00054 #include <pcl_ros/transforms.h>
00055 #include <pcl/sample_consensus/method_types.h>
00056 #include <pcl/sample_consensus/model_types.h>
00057 #include <pcl/segmentation/sac_segmentation.h>
00058 #include <pcl/io/pcd_io.h>
00059 #include <pcl/filters/extract_indices.h>
00060 #include <pcl/filters/passthrough.h>
00061 
00062 
00063 #include <tf/transform_listener.h>
00064 #include <tf/message_filter.h>
00065 #include <message_filters/subscriber.h>
00066 #if ROS_VERSION_MINIMUM(1,8,0) // test for Fuerte (new build system, separate stacks)
00067   #include <octomap_msgs/OctomapBinary.h>
00068   #include <octomap_msgs/GetOctomap.h>
00069   #include <octomap_msgs/BoundingBoxQuery.h>
00070 #else
00071   #include <octomap_ros/OctomapBinary.h>
00072   #include <octomap_ros/GetOctomap.h>
00073   #include <octomap_ros/ClearBBXRegion.h>
00074 #endif
00075 
00076 #include <octomap_ros/conversions.h>
00077 #include <octomap/octomap.h>
00078 #include <octomap/OcTreeKey.h>
00079 
00080 
00081 namespace octomap_server {
00082 class OctomapServer{
00083 
00084 public:
00085   typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;
00086 #if ROS_VERSION_MINIMUM(1,8,0)
00087   typedef octomap_msgs::GetOctomap OctomapSrv;
00088   typedef octomap_msgs::BoundingBoxQuery BBXSrv;
00089 #else
00090   typedef octomap_ros::GetOctomap OctomapSrv;
00091   typedef octomap_ros::ClearBBXRegion BBXSrv;
00092 #endif
00093 
00094   typedef octomap::OcTree OcTreeT;
00095 
00096   OctomapServer(ros::NodeHandle private_nh_ = ros::NodeHandle("~"));
00097   virtual ~OctomapServer();
00098   virtual bool octomapBinarySrv(OctomapSrv::Request  &req, OctomapSrv::GetOctomap::Response &res);
00099   virtual bool octomapFullSrv(OctomapSrv::Request  &req, OctomapSrv::GetOctomap::Response &res);
00100   bool clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp);
00101   bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);
00102 
00103   virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud);
00104   virtual bool openFile(const std::string& filename);
00105 
00106 protected:
00107   inline static void updateMinKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& min){
00108     for (unsigned i=0; i<3; ++i)
00109       min[i] = std::min(in[i], min[i]);
00110   };
00111   
00112   inline static void updateMaxKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& max){
00113     for (unsigned i=0; i<3; ++i)
00114       max[i] = std::max(in[i], max[i]);
00115   };
00116  
00118   inline bool isInUpdateBBX(const octomap::OcTree::iterator& it) const{
00119     // 2^(tree_depth-depth) voxels wide:
00120     unsigned voxelWidth = (1 << (m_maxTreeDepth - it.getDepth()));
00121     octomap::OcTreeKey key = it.getIndexKey(); // lower corner of voxel
00122     return (key[0]+voxelWidth >= m_updateBBXMin[0]
00123          && key[1]+voxelWidth >= m_updateBBXMin[1]
00124          && key[0] <= m_updateBBXMax[0]
00125          && key[1] <= m_updateBBXMax[1]);
00126   }
00127 
00128   void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level);
00129   void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const;
00130   void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const;
00131   void publishAll(const ros::Time& rostime = ros::Time::now());
00132 
00141   virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground);
00142 
00144   void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const;
00145 
00151   bool isSpeckleNode(const octomap::OcTreeKey& key) const;
00152 
00154   virtual void handlePreNodeTraversal(const ros::Time& rostime);
00155 
00157   virtual void handleNode(const OcTreeT::iterator& it) {};
00158 
00160   virtual void handleNodeInBBX(const OcTreeT::iterator& it) {};
00161 
00163   virtual void handleOccupiedNode(const OcTreeT::iterator& it);
00164 
00166   virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator& it);
00167 
00169   virtual void handleFreeNode(const OcTreeT::iterator& it);
00170 
00172   virtual void handleFreeNodeInBBX(const OcTreeT::iterator& it);
00173 
00175   virtual void handlePostNodeTraversal(const ros::Time& rostime);
00176 
00178   virtual void update2DMap(const OcTreeT::iterator& it, bool occupied);
00179 
00180   inline unsigned mapIdx(int i, int j) const{
00181     return m_gridmap.info.width*j + i;
00182   }
00183 
00184   inline unsigned mapIdx(const octomap::OcTreeKey& key) const{
00185     return mapIdx((key[0] - m_paddedMinKey[0])/m_multires2DScale,
00186         (key[1] - m_paddedMinKey[1])/m_multires2DScale);
00187 
00188   }
00189 
00196   void adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const;
00197 
00198   inline bool mapChanged(const nav_msgs::MapMetaData& oldMapInfo, const nav_msgs::MapMetaData& newMapInfo){
00199     return (    oldMapInfo.height != newMapInfo.height
00200              || oldMapInfo.width !=newMapInfo.width
00201              || oldMapInfo.origin.position.x != newMapInfo.origin.position.x
00202              || oldMapInfo.origin.position.y != newMapInfo.origin.position.y);
00203   }
00204 
00205   static std_msgs::ColorRGBA heightMapColor(double h);
00206   ros::NodeHandle m_nh;
00207   ros::Publisher  m_markerPub, m_binaryMapPub, m_fullMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub, m_cmapPub, m_fmapPub, m_fmarkerPub;
00208   message_filters::Subscriber<sensor_msgs::PointCloud2>* m_pointCloudSub;
00209   tf::MessageFilter<sensor_msgs::PointCloud2>* m_tfPointCloudSub;
00210   ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService;
00211   tf::TransformListener m_tfListener;
00212   dynamic_reconfigure::Server<OctomapServerConfig> m_reconfigureServer;
00213 
00214   octomap::OcTree* m_octree;
00215   octomap::KeyRay m_keyRay;  // temp storage for ray casting
00216   octomap::OcTreeKey m_updateBBXMin;
00217   octomap::OcTreeKey m_updateBBXMax;
00218 
00219   double m_maxRange;
00220   std::string m_worldFrameId; // the map frame
00221   std::string m_baseFrameId; // base of the robot for ground plane filtering
00222   bool m_useHeightMap;
00223   std_msgs::ColorRGBA m_color;
00224   std_msgs::ColorRGBA m_colorFree;
00225   double m_colorFactor;
00226 
00227   bool m_latchedTopics;
00228   bool m_publishFreeSpace;
00229 
00230   double m_res;
00231   unsigned m_treeDepth;
00232   unsigned m_maxTreeDepth;
00233   double m_probHit;
00234   double m_probMiss;
00235   double m_thresMin;
00236   double m_thresMax;
00237 
00238   double m_pointcloudMinZ;
00239   double m_pointcloudMaxZ;
00240   double m_occupancyMinZ;
00241   double m_occupancyMaxZ;
00242   double m_minSizeX;
00243   double m_minSizeY;
00244   bool m_filterSpeckles;
00245 
00246   bool m_filterGroundPlane;
00247   double m_groundFilterDistance;
00248   double m_groundFilterAngle;
00249   double m_groundFilterPlaneDistance;
00250 
00251   bool m_compressMap;
00252 
00253   // downprojected 2D map:
00254   bool m_incrementalUpdate;
00255   nav_msgs::OccupancyGrid m_gridmap;
00256   bool m_publish2DMap;
00257   bool m_mapOriginChanged;
00258   octomap::OcTreeKey m_paddedMinKey;
00259   unsigned m_multires2DScale;
00260   bool m_projectCompleteMap;
00261 };
00262 }
00263 
00264 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


octomap_server
Author(s): Armin Hornung
autogenerated on Tue Jul 9 2013 10:21:26