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 //#define COLOR_OCTOMAP_SERVER // turned off here, turned on identical ColorOctomapServer.h - easier maintenance, only maintain OctomapServer and then copy and paste to ColorOctomapServer and change define. There are prettier ways to do this, but this works for now
00070 
00071 #ifdef COLOR_OCTOMAP_SERVER
00072 #include <octomap/ColorOcTree.h>
00073 #endif
00074 
00075 namespace octomap_server {
00076 class OctomapServer {
00077 
00078 public:
00079 #ifdef COLOR_OCTOMAP_SERVER
00080   typedef pcl::PointXYZRGB PCLPoint;
00081   typedef pcl::PointCloud<pcl::PointXYZRGB> PCLPointCloud;
00082   typedef octomap::ColorOcTree OcTreeT;
00083 #else
00084   typedef pcl::PointXYZ PCLPoint;
00085   typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;
00086   typedef octomap::OcTree OcTreeT;
00087 #endif
00088   typedef octomap_msgs::GetOctomap OctomapSrv;
00089   typedef octomap_msgs::BoundingBoxQuery BBXSrv;
00090 
00091   OctomapServer(ros::NodeHandle private_nh_ = ros::NodeHandle("~"));
00092   virtual ~OctomapServer();
00093   virtual bool octomapBinarySrv(OctomapSrv::Request  &req, OctomapSrv::GetOctomap::Response &res);
00094   virtual bool octomapFullSrv(OctomapSrv::Request  &req, OctomapSrv::GetOctomap::Response &res);
00095   bool clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp);
00096   bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);
00097 
00098   virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud);
00099   virtual bool openFile(const std::string& filename);
00100 
00101 protected:
00102   inline static void updateMinKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& min) {
00103     for (unsigned i = 0; i < 3; ++i)
00104       min[i] = std::min(in[i], min[i]);
00105   };
00106 
00107   inline static void updateMaxKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& max) {
00108     for (unsigned i = 0; i < 3; ++i)
00109       max[i] = std::max(in[i], max[i]);
00110   };
00111 
00113   inline bool isInUpdateBBX(const OcTreeT::iterator& it) const {
00114     // 2^(tree_depth-depth) voxels wide:
00115     unsigned voxelWidth = (1 << (m_maxTreeDepth - it.getDepth()));
00116     octomap::OcTreeKey key = it.getIndexKey(); // lower corner of voxel
00117     return (key[0] + voxelWidth >= m_updateBBXMin[0]
00118             && key[1] + voxelWidth >= m_updateBBXMin[1]
00119             && key[0] <= m_updateBBXMax[0]
00120             && key[1] <= m_updateBBXMax[1]);
00121   }
00122 
00123   void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level);
00124   void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const;
00125   void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const;
00126   virtual void publishAll(const ros::Time& rostime = ros::Time::now());
00127 
00136   virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground);
00137 
00139   void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const;
00140 
00146   bool isSpeckleNode(const octomap::OcTreeKey& key) const;
00147 
00149   virtual void handlePreNodeTraversal(const ros::Time& rostime);
00150 
00152   virtual void handleNode(const OcTreeT::iterator& it) {};
00153 
00155   virtual void handleNodeInBBX(const OcTreeT::iterator& it) {};
00156 
00158   virtual void handleOccupiedNode(const OcTreeT::iterator& it);
00159 
00161   virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator& it);
00162 
00164   virtual void handleFreeNode(const OcTreeT::iterator& it);
00165 
00167   virtual void handleFreeNodeInBBX(const OcTreeT::iterator& it);
00168 
00170   virtual void handlePostNodeTraversal(const ros::Time& rostime);
00171 
00173   virtual void update2DMap(const OcTreeT::iterator& it, bool occupied);
00174 
00175   inline unsigned mapIdx(int i, int j) const {
00176     return m_gridmap.info.width * j + i;
00177   }
00178 
00179   inline unsigned mapIdx(const octomap::OcTreeKey& key) const {
00180     return mapIdx((key[0] - m_paddedMinKey[0]) / m_multires2DScale,
00181                   (key[1] - m_paddedMinKey[1]) / m_multires2DScale);
00182 
00183   }
00184 
00191   void adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const;
00192 
00193   inline bool mapChanged(const nav_msgs::MapMetaData& oldMapInfo, const nav_msgs::MapMetaData& newMapInfo) {
00194     return (    oldMapInfo.height != newMapInfo.height
00195                 || oldMapInfo.width != newMapInfo.width
00196                 || oldMapInfo.origin.position.x != newMapInfo.origin.position.x
00197                 || oldMapInfo.origin.position.y != newMapInfo.origin.position.y);
00198   }
00199 
00200   static std_msgs::ColorRGBA heightMapColor(double h);
00201   ros::NodeHandle m_nh;
00202   ros::Publisher  m_markerPub, m_binaryMapPub, m_fullMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub, m_cmapPub, m_fmapPub, m_fmarkerPub;
00203   message_filters::Subscriber<sensor_msgs::PointCloud2>* m_pointCloudSub;
00204   tf::MessageFilter<sensor_msgs::PointCloud2>* m_tfPointCloudSub;
00205   ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService;
00206   tf::TransformListener m_tfListener;
00207   boost::recursive_mutex m_config_mutex;
00208   dynamic_reconfigure::Server<OctomapServerConfig> m_reconfigureServer;
00209 
00210   OcTreeT* m_octree;
00211   octomap::KeyRay m_keyRay;  // temp storage for ray casting
00212   octomap::OcTreeKey m_updateBBXMin;
00213   octomap::OcTreeKey m_updateBBXMax;
00214 
00215   double m_maxRange;
00216   std::string m_worldFrameId; // the map frame
00217   std::string m_baseFrameId; // base of the robot for ground plane filtering
00218   bool m_useHeightMap;
00219   std_msgs::ColorRGBA m_color;
00220   std_msgs::ColorRGBA m_colorFree;
00221   double m_colorFactor;
00222 
00223   bool m_latchedTopics;
00224   bool m_publishFreeSpace;
00225 
00226   double m_res;
00227   unsigned m_treeDepth;
00228   unsigned m_maxTreeDepth;
00229 
00230   double m_pointcloudMinX;
00231   double m_pointcloudMaxX;
00232   double m_pointcloudMinY;
00233   double m_pointcloudMaxY;
00234   double m_pointcloudMinZ;
00235   double m_pointcloudMaxZ;
00236   double m_occupancyMinZ;
00237   double m_occupancyMaxZ;
00238   double m_minSizeX;
00239   double m_minSizeY;
00240   bool m_filterSpeckles;
00241 
00242   bool m_filterGroundPlane;
00243   double m_groundFilterDistance;
00244   double m_groundFilterAngle;
00245   double m_groundFilterPlaneDistance;
00246 
00247   bool m_compressMap;
00248 
00249   bool m_initConfig;
00250 
00251   // downprojected 2D map:
00252   bool m_incrementalUpdate;
00253   nav_msgs::OccupancyGrid m_gridmap;
00254   bool m_publish2DMap;
00255   bool m_mapOriginChanged;
00256   octomap::OcTreeKey m_paddedMinKey;
00257   unsigned m_multires2DScale;
00258   bool m_projectCompleteMap;
00259   bool m_useColoredMap;
00260 };
00261 }
00262 
00263 #endif


octomap_server
Author(s): Armin Hornung
autogenerated on Sat Jun 8 2019 19:23:48