OctomapServer.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010-2013, A. Hornung, University of Freiburg
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the University of Freiburg nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef OCTOMAP_SERVER_OCTOMAPSERVER_H
31 #define OCTOMAP_SERVER_OCTOMAPSERVER_H
32 
33 #include <ros/ros.h>
34 #include <visualization_msgs/MarkerArray.h>
35 #include <nav_msgs/OccupancyGrid.h>
36 #include <std_msgs/ColorRGBA.h>
37 
38 // #include <moveit_msgs/CollisionObject.h>
39 // #include <moveit_msgs/CollisionMap.h>
40 #include <sensor_msgs/PointCloud2.h>
41 #include <std_srvs/Empty.h>
42 #include <dynamic_reconfigure/server.h>
43 #include <octomap_server/OctomapServerConfig.h>
44 
45 #include <pcl/point_types.h>
46 #include <pcl/conversions.h>
47 #include <pcl_ros/transforms.h>
48 #include <pcl/sample_consensus/method_types.h>
49 
50 #pragma GCC diagnostic push
51 #pragma GCC diagnostic ignored "-Wdeprecated-declarations" // pcl::SAC_SAMPLE_SIZE is protected since PCL 1.8.0
52 #include <pcl/sample_consensus/model_types.h>
53 #pragma GCC diagnostic pop
54 
55 #include <pcl/segmentation/sac_segmentation.h>
56 #include <pcl/io/pcd_io.h>
57 #include <pcl/filters/extract_indices.h>
58 #include <pcl/filters/passthrough.h>
60 
61 
62 #include <tf/transform_listener.h>
63 #include <tf/message_filter.h>
65 #include <octomap_msgs/Octomap.h>
66 #include <octomap_msgs/GetOctomap.h>
67 #include <octomap_msgs/BoundingBoxQuery.h>
69 
71 #include <octomap/octomap.h>
72 #include <octomap/OcTreeKey.h>
73 
74 //#define COLOR_OCTOMAP_SERVER // switch color here - easier maintenance, only maintain OctomapServer. Two targets are defined in the cmake, octomap_server_color and octomap_server. One has this defined, and the other doesn't
75 
76 #ifdef COLOR_OCTOMAP_SERVER
77 #include <octomap/ColorOcTree.h>
78 #endif
79 
80 namespace octomap_server {
82 
83 public:
84 #ifdef COLOR_OCTOMAP_SERVER
85  typedef pcl::PointXYZRGB PCLPoint;
86  typedef pcl::PointCloud<pcl::PointXYZRGB> PCLPointCloud;
88 #else
89  typedef pcl::PointXYZ PCLPoint;
90  typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;
92 #endif
93  typedef octomap_msgs::GetOctomap OctomapSrv;
94  typedef octomap_msgs::BoundingBoxQuery BBXSrv;
95 
96  OctomapServer(const ros::NodeHandle private_nh_ = ros::NodeHandle("~"), const ros::NodeHandle &nh_ = ros::NodeHandle());
97  virtual ~OctomapServer();
98  virtual bool octomapBinarySrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res);
99  virtual bool octomapFullSrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res);
100  bool clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp);
101  bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);
102 
103  virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud);
104  virtual bool openFile(const std::string& filename);
105 
106 protected:
107  inline static void updateMinKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& min) {
108  for (unsigned i = 0; i < 3; ++i)
109  min[i] = std::min(in[i], min[i]);
110  };
111 
112  inline static void updateMaxKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& max) {
113  for (unsigned i = 0; i < 3; ++i)
114  max[i] = std::max(in[i], max[i]);
115  };
116 
118  inline bool isInUpdateBBX(const OcTreeT::iterator& it) const {
119  // 2^(tree_depth-depth) voxels wide:
120  unsigned voxelWidth = (1 << (m_maxTreeDepth - it.getDepth()));
121  octomap::OcTreeKey key = it.getIndexKey(); // lower corner of voxel
122  return (key[0] + voxelWidth >= m_updateBBXMin[0]
123  && key[1] + voxelWidth >= m_updateBBXMin[1]
124  && key[0] <= m_updateBBXMax[0]
125  && key[1] <= m_updateBBXMax[1]);
126  }
127 
128  void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level);
129  void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const;
130  void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const;
131  void publishProjected2DMap(const ros::Time& rostime = ros::Time::now());
132  virtual void publishAll(const ros::Time& rostime = ros::Time::now());
133 
142  virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground);
143 
145  void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const;
146 
152  bool isSpeckleNode(const octomap::OcTreeKey& key) const;
153 
155  virtual void handlePreNodeTraversal(const ros::Time& rostime);
156 
158  virtual void handleNode(const OcTreeT::iterator& it) {};
159 
161  virtual void handleNodeInBBX(const OcTreeT::iterator& it) {};
162 
164  virtual void handleOccupiedNode(const OcTreeT::iterator& it);
165 
167  virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator& it);
168 
170  virtual void handleFreeNode(const OcTreeT::iterator& it);
171 
173  virtual void handleFreeNodeInBBX(const OcTreeT::iterator& it);
174 
176  virtual void handlePostNodeTraversal(const ros::Time& rostime);
177 
179  virtual void update2DMap(const OcTreeT::iterator& it, bool occupied);
180 
181  inline unsigned mapIdx(int i, int j) const {
182  return m_gridmap.info.width * j + i;
183  }
184 
185  inline unsigned mapIdx(const octomap::OcTreeKey& key) const {
186  return mapIdx((key[0] - m_paddedMinKey[0]) / m_multires2DScale,
187  (key[1] - m_paddedMinKey[1]) / m_multires2DScale);
188 
189  }
190 
197  void adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const;
198 
199  inline bool mapChanged(const nav_msgs::MapMetaData& oldMapInfo, const nav_msgs::MapMetaData& newMapInfo) {
200  return ( oldMapInfo.height != newMapInfo.height
201  || oldMapInfo.width != newMapInfo.width
202  || oldMapInfo.origin.position.x != newMapInfo.origin.position.x
203  || oldMapInfo.origin.position.y != newMapInfo.origin.position.y);
204  }
205 
206  static std_msgs::ColorRGBA heightMapColor(double h);
214  boost::recursive_mutex m_config_mutex;
215  dynamic_reconfigure::Server<OctomapServerConfig> m_reconfigureServer;
216 
217  OcTreeT* m_octree;
218  octomap::KeyRay m_keyRay; // temp storage for ray casting
221 
222  double m_minRange;
223  double m_maxRange;
224  std::string m_worldFrameId; // the map frame
225  std::string m_baseFrameId; // base of the robot for ground plane filtering
227  std_msgs::ColorRGBA m_color;
228  std_msgs::ColorRGBA m_colorFree;
230 
233 
234  double m_res;
235  unsigned m_treeDepth;
236  unsigned m_maxTreeDepth;
237 
246  double m_minSizeX;
247  double m_minSizeY;
249 
254 
256 
258 
259  // downprojected 2D map:
261  nav_msgs::OccupancyGrid m_gridmap;
268 };
269 }
270 
271 #endif
tf::TransformListener m_tfListener
virtual bool octomapBinarySrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res)
virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud)
virtual bool octomapFullSrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res)
bool isInUpdateBBX(const OcTreeT::iterator &it) const
Test if key is within update area of map (2D, ignores height)
octomap::OcTreeKey m_updateBBXMax
static void updateMaxKey(const octomap::OcTreeKey &in, octomap::OcTreeKey &max)
octomap::OcTreeKey m_updateBBXMin
virtual void handleFreeNodeInBBX(const OcTreeT::iterator &it)
hook that is called when traversing free nodes in the updated area (updates 2D map projection here) ...
bool mapChanged(const nav_msgs::MapMetaData &oldMapInfo, const nav_msgs::MapMetaData &newMapInfo)
OctomapServer(const ros::NodeHandle private_nh_=ros::NodeHandle("~"), const ros::NodeHandle &nh_=ros::NodeHandle())
ros::Publisher m_collisionObjectPub
tf::Vector3 Point
bool resetSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
void publishFullOctoMap(const ros::Time &rostime=ros::Time::now()) const
void reconfigureCallback(octomap_server::OctomapServerConfig &config, uint32_t level)
bool clearBBXSrv(BBXSrv::Request &req, BBXSrv::Response &resp)
void adjustMapData(nav_msgs::OccupancyGrid &map, const nav_msgs::MapMetaData &oldMapInfo) const
ros::ServiceServer m_octomapBinaryService
void publishProjected2DMap(const ros::Time &rostime=ros::Time::now())
static void updateMinKey(const octomap::OcTreeKey &in, octomap::OcTreeKey &min)
static std_msgs::ColorRGBA heightMapColor(double h)
virtual void handlePreNodeTraversal(const ros::Time &rostime)
hook that is called before traversing all nodes
octomap::OcTreeKey m_paddedMinKey
virtual void handleOccupiedNode(const OcTreeT::iterator &it)
hook that is called when traversing occupied nodes of the updated Octree
std_msgs::ColorRGBA m_color
virtual void insertScan(const tf::Point &sensorOrigin, const PCLPointCloud &ground, const PCLPointCloud &nonground)
update occupancy map with a scan labeled as ground and nonground. The scans should be in the global m...
void publishBinaryOctoMap(const ros::Time &rostime=ros::Time::now()) const
octomap_msgs::BoundingBoxQuery BBXSrv
Definition: OctomapServer.h:94
std_msgs::ColorRGBA m_colorFree
octomap_msgs::GetOctomap OctomapSrv
Definition: OctomapServer.h:93
tf::MessageFilter< sensor_msgs::PointCloud2 > * m_tfPointCloudSub
dynamic_reconfigure::Server< OctomapServerConfig > m_reconfigureServer
message_filters::Subscriber< sensor_msgs::PointCloud2 > * m_pointCloudSub
ros::ServiceServer m_octomapFullService
virtual void handleNode(const OcTreeT::iterator &it)
hook that is called when traversing all nodes of the updated Octree (does nothing here) ...
virtual void publishAll(const ros::Time &rostime=ros::Time::now())
unsigned mapIdx(int i, int j) const
unsigned mapIdx(const octomap::OcTreeKey &key) const
void filterGroundPlane(const PCLPointCloud &pc, PCLPointCloud &ground, PCLPointCloud &nonground) const
label the input cloud "pc" into ground and nonground. Should be in the robot&#39;s fixed frame (not world...
static Time now()
virtual void handlePostNodeTraversal(const ros::Time &rostime)
hook that is called after traversing all nodes
boost::recursive_mutex m_config_mutex
nav_msgs::OccupancyGrid m_gridmap
virtual void update2DMap(const OcTreeT::iterator &it, bool occupied)
updates the downprojected 2D map as either occupied or free
ros::ServiceServer m_resetService
pcl::PointCloud< pcl::PointXYZ > PCLPointCloud
Definition: OctomapServer.h:90
virtual void handleNodeInBBX(const OcTreeT::iterator &it)
hook that is called when traversing all nodes of the updated Octree in the updated area (does nothing...
bool isSpeckleNode(const octomap::OcTreeKey &key) const
Find speckle nodes (single occupied voxels with no neighbors). Only works on lowest resolution! ...
virtual void handleFreeNode(const OcTreeT::iterator &it)
hook that is called when traversing free nodes of the updated Octree
virtual bool openFile(const std::string &filename)
virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator &it)
hook that is called when traversing occupied nodes in the updated area (updates 2D map projection her...
ros::ServiceServer m_clearBBXService


octomap_server
Author(s): Armin Hornung
autogenerated on Tue Dec 27 2022 03:15:28