octree_pa_node.cpp
Go to the documentation of this file.
1 /******************************************************************************
2 * *
3 * octree_pa_node.cpp *
4 * ================== *
5 * *
6 *******************************************************************************
7 * *
8 * github repository *
9 * https://github.com/TUC-ProAut/ros_octomap *
10 * *
11 * Chair of Automation Technology, Technische Universität Chemnitz *
12 * https://www.tu-chemnitz.de/etit/proaut *
13 * *
14 *******************************************************************************
15 * *
16 * New BSD License *
17 * *
18 * Copyright (c) 2015-2018, Peter Weissig, Technische Universität Chemnitz *
19 * All rights reserved. *
20 * *
21 * Redistribution and use in source and binary forms, with or without *
22 * modification, are permitted provided that the following conditions are met: *
23 * * Redistributions of source code must retain the above copyright *
24 * notice, this list of conditions and the following disclaimer. *
25 * * Redistributions in binary form must reproduce the above copyright *
26 * notice, this list of conditions and the following disclaimer in the *
27 * documentation and/or other materials provided with the distribution. *
28 * * Neither the name of the Technische Universität Chemnitz nor the *
29 * names of its contributors may be used to endorse or promote products *
30 * derived from this software without specific prior written permission. *
31 * *
32 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
33 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
34 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
35 * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY *
36 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
37 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
38 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
39 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
40 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
41 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH *
42 * DAMAGE. *
43 * *
44 ******************************************************************************/
45 
46 // local headers
48 
49 // ros headers
51 
52 // standard headers
53 #include <string>
54 
55 //**************************[main]*********************************************
56 int main(int argc, char **argv) {
57 
58  ros::init(argc, argv, "octree_pa_node");
60 
61  ros::spin();
62 
63  return 0;
64 }
65 
66 //**************************[cOctreePaNode]************************************
68  cOctreePaRos(0.1), tf_listener_(ros::Duration(20), true) {
69 
70  cParameterPaRos paramloader;
71 
72  paramloader.load("~/output_frame", rosparams_base_.output_frame_);
73 
74  // octomap parameter
75  double temp;
76  temp = 0.1 ;
77  paramloader.load("~/map_resolution" , temp); setResolution (temp);
78  temp = 0.5 ;
79  paramloader.load("~/map_prob_threshold", temp); setOccupancyThres (temp);
80  temp = 0.12;
81  paramloader.load("~/map_clamp_min" , temp); setClampingThresMin(temp);
82  temp = 0.97;
83  paramloader.load("~/map_clamp_max" , temp); setClampingThresMax(temp);
84 
85  // pointcloud insertion parameter
86  paramloader.load("~/map_prob_hit" , addparams_.map_prob_hit_ );
87  paramloader.load("~/map_prob_miss" , addparams_.map_prob_miss_ );
88  paramloader.load("~/pcd_voxel_active" , addparams_.pcd_voxel_active_ );
89  paramloader.load("~/pcd_voxel_explicit", addparams_.pcd_voxel_explicit_);
90  paramloader.load("~/pcd_voxel_explicit_relative_resolution",
92  paramloader.load("~/pcd_explicit_transform",
94 
95  // topics in
96  paramloader.loadTopic("~/topic_in_cloud" ,
98  paramloader.loadTopic("~/topic_in_cloud_old",
100  paramloader.loadTopic("~/topic_in_laser" ,
102 
103  // topics out
104  paramloader.loadTopic("~/topic_out_octomap" ,
106  paramloader.loadTopic("~/topic_out_octomap_full" ,
108  paramloader.loadTopic("~/topic_out_cloud_free" ,
110  paramloader.loadTopic("~/topic_out_cloud_occupied",
112 
113 
114  // Subscriber for pointclouds
115  if (nodeparams_.topic_in_cloud_ != "") {
116  sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(
119  }
120 
121  // Subscriber for pointclouds (old format)
122  if (nodeparams_.topic_in_cloud_old_ != "") {
123  sub_cloud_old_ = nh_.subscribe<sensor_msgs::PointCloud>(
126  }
127  // Subscriber for laserscans
128  if (nodeparams_.topic_in_laser_ != "") {
129  sub_laser_ = nh_.subscribe<sensor_msgs::LaserScan>(
132  }
133 
134 
135  // puplisher for the octomap (binary data)
136  pub_octomap_ = nh_.advertise<octomap_msgs::Octomap>(
137  nodeparams_.topic_out_octomap_, 10, true);
138  // puplisher for the octomap (full data)
139  pub_octomap_full_ = nh_.advertise<octomap_msgs::Octomap>(
141 
142  // puplisher for free voxels as pointcloud
143  pub_cloud_free_ = nh_.advertise<sensor_msgs::PointCloud2>(
145  // puplisher for occupied voxels as pointcloud
146  pub_cloud_occupied_ = nh_.advertise<sensor_msgs::PointCloud2>(
148 
149  std::string str_service("~/");
150  str_service = paramloader.resolveRessourcename(str_service);
151  // service for clearing the octomap
152  srv_clear_ = nh_.advertiseService(str_service + "clear",
154  // service for receiving the size of the octomap
155  srv_getsize_ = nh_.advertiseService(str_service + "getsize",
157  // service for saving the octomap
158  srv_save_ = nh_.advertiseService(str_service + "save",
160  // service for loading a octomap
161  srv_load_ = nh_.advertiseService(str_service + "load",
163 
164  // count number of inserted pointclouds
165  count_cloud_ = 0;
166  count_cloud_old_ = 0;
167  count_laser_ = 0;
168 }
169 
170 //**************************[~cOctreePaNode]***********************************
172 
173 }
174 
175 //**************************[publishOctomap]***********************************
177 
178  if (pub_octomap_.getNumSubscribers() > 0) {
180  }
183  }
184 
187  }
190  }
191 }
192 
193 //**************************[addPointcloudCallbackSub]*************************
195  const sensor_msgs::PointCloud2ConstPtr &msg) {
196 
197  if (!updateTime(msg->header.stamp)) {
199  return;
200  }
201 
202  tf::StampedTransform transform;
203 
204  try {
206  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
208  msg->header.frame_id, msg->header.stamp, transform);
209  } catch (tf::TransformException &ex){
210  ROS_WARN("%s",ex.what());
211  return;
212  }
213 
214  if (addCloud(msg, addparams_, transform)) {
215  count_cloud_++;
216  publishOctomap();
217  }
218 }
219 
220 //**************************[addPointcloudCallbackSub]*************************
222  const sensor_msgs::PointCloudConstPtr &msg) {
223 
224  if (!updateTime(msg->header.stamp)) {
226  return;
227  }
228 
229  tf::StampedTransform transform;
230 
231  try {
233  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
235  msg->header.frame_id, msg->header.stamp, transform);
236  } catch (tf::TransformException &ex){
237  ROS_WARN("%s",ex.what());
238  return;
239  }
240 
241  if (addCloud(msg, addparams_, transform)) {
243  publishOctomap();
244  }
245 }
246 
247 //**************************[addLaserCallbackSub]******************************
249  const sensor_msgs::LaserScanConstPtr &msg) {
250 
251  if (!updateTime(msg->header.stamp)) {
253  return;
254  }
255 
256  tf::StampedTransform transform;
257 
258  try {
260  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
262  msg->header.frame_id, msg->header.stamp, transform);
263  } catch (tf::TransformException &ex){
264  ROS_WARN("%s",ex.what());
265  return;
266  }
267 
268  updateTime(msg->header.stamp);
269  if (addCloud(msg, addparams_, transform)) {
270  count_laser_++;
271  publishOctomap();
272  }
273 }
274 
275 
276 //**************************[clearCallbackSrv]*********************************
277 bool cOctreePaNode::clearCallbackSrv(std_srvs::Empty::Request &req,
278  std_srvs::Empty::Response &res) {
279 
280  ROS_INFO("cOctreePaNode::clear()");
281 
282  count_cloud_ = 0;
283  count_cloud_old_ = 0;
284  count_laser_ = 0;
285 
286  clear();
288 
289  return true;
290 }
291 
292 //**************************[getSizeCallbackSrv]*******************************
294  octomap_pa::OctomapPaGetSize::Request &req,
295  octomap_pa::OctomapPaGetSize::Response &res) {
296 
297  ROS_INFO("cOctreePaNode::getsize()");
298 
299  res.size = size();
300  res.memoryusage = (int64_t) memoryUsage();
301 
302  res.count_cloud = count_cloud_ ;
303  res.count_cloud_old = count_cloud_old_;
304  res.count_laser = count_laser_ ;
305 
306  return true;
307 }
308 
309 //**************************[saveCallbackSrv]**********************************
311  octomap_pa::OctomapPaFileName::Request &req,
312  octomap_pa::OctomapPaFileName::Response &res) {
313 
314  ROS_INFO_STREAM("cOctreePaNode::save(" << req.filename << ")");
315 
316  std::string filename;
317  filename = req.filename;
318  cParameterPaRos par;
319  par.replaceFindpack(filename);
320 
321  //res.ok = writeBinary(filename);
322  res.ok = this->write(filename);
323 
324  return res.ok;
325 }
326 
327 //**************************[loadCallbackSrv]**********************************
329  octomap_pa::OctomapPaFileName::Request &req,
330  octomap_pa::OctomapPaFileName::Response &res) {
331 
332  ROS_INFO_STREAM("cOctreePaNode::load(" << req.filename << ")");
333 
334  std::string filename;
335  filename = req.filename;
336  cParameterPaRos par;
337  par.replaceFindpack(filename);
338 
339  // res.ok = readBinary(filename);
340  res.ok = readFull(filename);
341 
342  publishOctomap();
343  return res.ok;
344 }
sensor_msgs::PointCloud2Ptr getOctomapPcdFree(const int tree_depth=0, const bool expand=false) const
similar to getOctomapPcd, but only returning just empty voxels
std::string topic_out_octomap_full_
name of the topic for publishing the octomap ("~out_octomap_full")
ros::Publisher pub_octomap_
puplisher for the octomap (binary data)
void addPointcloudOldCallbackSub(const sensor_msgs::PointCloudConstPtr &msg)
callback function for receiving a pointcloud (old format)
std::string output_frame_
name of the output frame ("map")
octomap_msgs::OctomapPtr getOctomapFull(void) const
function for getting the full octomap
filename
double map_prob_miss_
octomap parameter: probability for a "miss"
ros::ServiceServer srv_load_
service for loading a octomap
int count_cloud_old_
number of inserted pointclouds (old format)
ros::ServiceServer srv_clear_
service for clearing the octomap
void publish(const boost::shared_ptr< M > &message) const
virtual void clear(void)
clear local timestamps with octomap
ros::Subscriber sub_cloud_old_
subscriber for a pointcloud (old format)
void setClampingThresMax(double thresProb)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool updateTime(const ros::Time &time)
bool clearCallbackSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Subscriber sub_cloud_
subscriber for a pointcloud
octomap_msgs::OctomapPtr getOctomap(void) const
function for getting the binary octomap
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
int count_laser_
number of inserted laser scans
bool loadTopic(const std::string name, std::string &value, const bool print_default=true) const
#define ROS_WARN(...)
int main(int argc, char **argv)
cAddCloudParameter addparams_
bool load(const std::string name, bool &value, const bool print_default=true) const
ROSCPP_DECL void spin(Spinner &spinner)
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
static bool replaceFindpack(std::string &path)
double map_prob_hit_
octomap parameter: probability for a "hit"
bool addCloud(const sensor_msgs::PointCloud2ConstPtr &cloud, const cAddCloudParameter &params, const tf::Transform transform=tf::Transform::getIdentity())
bool pcd_voxel_active_
pointcloud insertion parameter: use voxel-filter (speeds up)
#define ROS_INFO(...)
ros::ServiceServer srv_getsize_
service for receiving the size of the octomap
bool write(const std::string &filename) const
bool saveCallbackSrv(octomap_pa::OctomapPaFileName::Request &req, octomap_pa::OctomapPaFileName::Response &res)
bool loadCallbackSrv(octomap_pa::OctomapPaFileName::Request &req, octomap_pa::OctomapPaFileName::Response &res)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
cOctreePaNode()
default constructor
cOctreeBasePaRosParameter rosparams_base_
parameters
static std::string resolveRessourcename(const std::string name)
std::string topic_out_octomap_
name of the topic for publishing the octomap ("~out_octomap")
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
ros::ServiceServer srv_save_
service for saving the octomap
sensor_msgs::PointCloud2Ptr getOctomapPcd(const int tree_depth=0, const bool expand=false) const
function for getting the pointcloud equivalent of the octomap
void setOccupancyThres(double prob)
tf::TransformListener tf_listener_
transformation of different frames
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
std::string topic_in_laser_
name of the topic for subscribing a laserscan ("~in_laser")
~cOctreePaNode()
default destructor
void addPointcloudCallbackSub(const sensor_msgs::PointCloud2ConstPtr &msg)
callback function for receiving a pointcloud
ros::Publisher pub_octomap_full_
puplisher for the octomap (full data)
ros::Subscriber sub_laser_
subscriber for a laserscan
bool readFull(const std::string &filename)
trying to read the given file into the current OcTree
void publishOctomap(void)
function for publishing the octomap
ros::Publisher pub_cloud_occupied_
puplisher for occupied voxels as pointcloud
cOctreeBasePaNodeParameter nodeparams_
parameters
int count_cloud_
number of inserted pointclouds
bool getSizeCallbackSrv(octomap_pa::OctomapPaGetSize::Request &req, octomap_pa::OctomapPaGetSize::Response &res)
void addLaserCallbackSub(const sensor_msgs::LaserScanConstPtr &msg)
callback function for receiving a laserscan
ros::NodeHandle nh_
node handler for topic subscription and advertising
double pcd_voxel_explicit_relative_resolution_
pointcloud insertion parameter: relative resolution of pcl-filter
ros::Publisher pub_cloud_free_
puplisher for free voxels as pointcloud
void setClampingThresMin(double thresProb)


octomap_pa
Author(s):
autogenerated on Fri Jun 7 2019 22:04:53