OctomapServer.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010-2013, A. Hornung, University of Freiburg
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 University of Freiburg 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 
00030 #ifndef OCTOMAP_SERVER_OCTOMAPSERVER_H
00031 #define OCTOMAP_SERVER_OCTOMAPSERVER_H
00032 
00033 #include <ros/ros.h>
00034 #include <visualization_msgs/MarkerArray.h>
00035 #include <nav_msgs/OccupancyGrid.h>
00036 #include <std_msgs/ColorRGBA.h>
00037 
00038 // #include <moveit_msgs/CollisionObject.h>
00039 // #include <moveit_msgs/CollisionMap.h>
00040 #include <sensor_msgs/PointCloud2.h>
00041 #include <std_srvs/Empty.h>
00042 #include <dynamic_reconfigure/server.h>
00043 #include <octomap_server/OctomapServerConfig.h>
00044 
00045 #include <pcl/point_types.h>
00046 #include <pcl/conversions.h>
00047 #include <pcl_ros/transforms.h>
00048 #include <pcl/sample_consensus/method_types.h>
00049 #include <pcl/sample_consensus/model_types.h>
00050 #include <pcl/segmentation/sac_segmentation.h>
00051 #include <pcl/io/pcd_io.h>
00052 #include <pcl/filters/extract_indices.h>
00053 #include <pcl/filters/passthrough.h>
00054 #include <pcl_conversions/pcl_conversions.h>
00055 
00056 
00057 #include <tf/transform_listener.h>
00058 #include <tf/message_filter.h>
00059 #include <message_filters/subscriber.h>
00060 #include <octomap_msgs/Octomap.h>
00061 #include <octomap_msgs/GetOctomap.h>
00062 #include <octomap_msgs/BoundingBoxQuery.h>
00063 #include <octomap_msgs/conversions.h>
00064 
00065 #include <octomap_ros/conversions.h>
00066 #include <octomap/octomap.h>
00067 #include <octomap/OcTreeKey.h>
00068 
00069 
00070 namespace octomap_server {
00071 class OctomapServer{
00072 
00073 public:
00074   typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;
00075   typedef octomap_msgs::GetOctomap OctomapSrv;
00076   typedef octomap_msgs::BoundingBoxQuery BBXSrv;
00077 
00078   typedef octomap::OcTree OcTreeT;
00079 
00080   OctomapServer(ros::NodeHandle private_nh_ = ros::NodeHandle("~"));
00081   virtual ~OctomapServer();
00082   virtual bool octomapBinarySrv(OctomapSrv::Request  &req, OctomapSrv::GetOctomap::Response &res);
00083   virtual bool octomapFullSrv(OctomapSrv::Request  &req, OctomapSrv::GetOctomap::Response &res);
00084   bool clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp);
00085   bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);
00086 
00087   virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud);
00088   virtual bool openFile(const std::string& filename);
00089 
00090 protected:
00091   inline static void updateMinKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& min){
00092     for (unsigned i=0; i<3; ++i)
00093       min[i] = std::min(in[i], min[i]);
00094   };
00095   
00096   inline static void updateMaxKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& max){
00097     for (unsigned i=0; i<3; ++i)
00098       max[i] = std::max(in[i], max[i]);
00099   };
00100  
00102   inline bool isInUpdateBBX(const octomap::OcTree::iterator& it) const{
00103     // 2^(tree_depth-depth) voxels wide:
00104     unsigned voxelWidth = (1 << (m_maxTreeDepth - it.getDepth()));
00105     octomap::OcTreeKey key = it.getIndexKey(); // lower corner of voxel
00106     return (key[0]+voxelWidth >= m_updateBBXMin[0]
00107          && key[1]+voxelWidth >= m_updateBBXMin[1]
00108          && key[0] <= m_updateBBXMax[0]
00109          && key[1] <= m_updateBBXMax[1]);
00110   }
00111 
00112   void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level);
00113   void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const;
00114   void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const;
00115   void publishAll(const ros::Time& rostime = ros::Time::now());
00116 
00125   virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground);
00126 
00128   void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const;
00129 
00135   bool isSpeckleNode(const octomap::OcTreeKey& key) const;
00136 
00138   virtual void handlePreNodeTraversal(const ros::Time& rostime);
00139 
00141   virtual void handleNode(const OcTreeT::iterator& it) {};
00142 
00144   virtual void handleNodeInBBX(const OcTreeT::iterator& it) {};
00145 
00147   virtual void handleOccupiedNode(const OcTreeT::iterator& it);
00148 
00150   virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator& it);
00151 
00153   virtual void handleFreeNode(const OcTreeT::iterator& it);
00154 
00156   virtual void handleFreeNodeInBBX(const OcTreeT::iterator& it);
00157 
00159   virtual void handlePostNodeTraversal(const ros::Time& rostime);
00160 
00162   virtual void update2DMap(const OcTreeT::iterator& it, bool occupied);
00163 
00164   inline unsigned mapIdx(int i, int j) const{
00165     return m_gridmap.info.width*j + i;
00166   }
00167 
00168   inline unsigned mapIdx(const octomap::OcTreeKey& key) const{
00169     return mapIdx((key[0] - m_paddedMinKey[0])/m_multires2DScale,
00170         (key[1] - m_paddedMinKey[1])/m_multires2DScale);
00171 
00172   }
00173 
00180   void adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const;
00181 
00182   inline bool mapChanged(const nav_msgs::MapMetaData& oldMapInfo, const nav_msgs::MapMetaData& newMapInfo){
00183     return (    oldMapInfo.height != newMapInfo.height
00184              || oldMapInfo.width !=newMapInfo.width
00185              || oldMapInfo.origin.position.x != newMapInfo.origin.position.x
00186              || oldMapInfo.origin.position.y != newMapInfo.origin.position.y);
00187   }
00188 
00189   static std_msgs::ColorRGBA heightMapColor(double h);
00190   ros::NodeHandle m_nh;
00191   ros::Publisher  m_markerPub, m_binaryMapPub, m_fullMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub, m_cmapPub, m_fmapPub, m_fmarkerPub;
00192   message_filters::Subscriber<sensor_msgs::PointCloud2>* m_pointCloudSub;
00193   tf::MessageFilter<sensor_msgs::PointCloud2>* m_tfPointCloudSub;
00194   ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService;
00195   tf::TransformListener m_tfListener;
00196   dynamic_reconfigure::Server<OctomapServerConfig> m_reconfigureServer;
00197 
00198   octomap::OcTree* m_octree;
00199   octomap::KeyRay m_keyRay;  // temp storage for ray casting
00200   octomap::OcTreeKey m_updateBBXMin;
00201   octomap::OcTreeKey m_updateBBXMax;
00202 
00203   double m_maxRange;
00204   std::string m_worldFrameId; // the map frame
00205   std::string m_baseFrameId; // base of the robot for ground plane filtering
00206   bool m_useHeightMap;
00207   std_msgs::ColorRGBA m_color;
00208   std_msgs::ColorRGBA m_colorFree;
00209   double m_colorFactor;
00210 
00211   bool m_latchedTopics;
00212   bool m_publishFreeSpace;
00213 
00214   double m_res;
00215   unsigned m_treeDepth;
00216   unsigned m_maxTreeDepth;
00217   double m_probHit;
00218   double m_probMiss;
00219   double m_thresMin;
00220   double m_thresMax;
00221 
00222   double m_pointcloudMinZ;
00223   double m_pointcloudMaxZ;
00224   double m_occupancyMinZ;
00225   double m_occupancyMaxZ;
00226   double m_minSizeX;
00227   double m_minSizeY;
00228   bool m_filterSpeckles;
00229 
00230   bool m_filterGroundPlane;
00231   double m_groundFilterDistance;
00232   double m_groundFilterAngle;
00233   double m_groundFilterPlaneDistance;
00234 
00235   bool m_compressMap;
00236 
00237   // downprojected 2D map:
00238   bool m_incrementalUpdate;
00239   nav_msgs::OccupancyGrid m_gridmap;
00240   bool m_publish2DMap;
00241   bool m_mapOriginChanged;
00242   octomap::OcTreeKey m_paddedMinKey;
00243   unsigned m_multires2DScale;
00244   bool m_projectCompleteMap;
00245 };
00246 }
00247 
00248 #endif


octomap_server
Author(s): Armin Hornung
autogenerated on Thu Aug 27 2015 14:14:07