octree_stamped_pa_node.cpp
Go to the documentation of this file.
1 /******************************************************************************
2 * *
3 * octree_stamped_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_stamped_pa_node");
60 
61  ros::spin();
62 
63  return 0;
64 }
65 
66 //**************************[cOctreeStampedPaNode]*****************************
68  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  // degrading parameter
96  paramloader.load("~/degrading_time" , rosparams_.degrading_time_);
97  paramloader.load("~/auto_degrading" , rosparams_.auto_degrading_);
98  paramloader.load("~/auto_degrading_intervall",
100 
101  // topics in
102  paramloader.loadTopic("~/topic_in_cloud" ,
104  paramloader.loadTopic("~/topic_in_cloud_old",
106  paramloader.loadTopic("~/topic_in_laser" ,
108 
109  // topics out
110  paramloader.loadTopic("~/topic_out_octomap" ,
112  paramloader.loadTopic("~/topic_out_octomap_full" ,
114  paramloader.loadTopic("~/topic_out_cloud_free" ,
116  paramloader.loadTopic("~/topic_out_cloud_occupied",
118 
119  // Subscriber for pointclouds
120  if (nodeparams_.topic_in_cloud_ != "") {
121  sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(
124  }
125 
126  // Subscriber for pointclouds (old format)
127  if (nodeparams_.topic_in_cloud_old_ != "") {
128  sub_cloud_old_ = nh_.subscribe<sensor_msgs::PointCloud>(
131  }
132  // Subscriber for laserscans
133  if (nodeparams_.topic_in_laser_ != "") {
134  sub_laser_ = nh_.subscribe<sensor_msgs::LaserScan>(
137  }
138 
139 
140  // puplisher for the octomap (binary data)
141  pub_octomap_ = nh_.advertise<octomap_msgs::Octomap>(
142  nodeparams_.topic_out_octomap_, 10, true);
143  // puplisher for the octomap (full data)
144  pub_octomap_full_ = nh_.advertise<octomap_msgs::Octomap>(
146 
147  // puplisher for free voxels as pointcloud
148  pub_cloud_free_ = nh_.advertise<sensor_msgs::PointCloud2>(
150  // puplisher for occupied voxels as pointcloud
151  pub_cloud_occupied_ = nh_.advertise<sensor_msgs::PointCloud2>(
153 
154 
155  std::string str_service("~/");
156  str_service = paramloader.resolveRessourcename(str_service);
157  // service for clearing the octomap
158  srv_clear_ = nh_.advertiseService(str_service + "clear",
160  // service for receiving the size of the octomap
161  srv_getsize_ = nh_.advertiseService(str_service + "getsize",
163  // service for saving the octomap
164  srv_save_ = nh_.advertiseService(str_service + "save",
166  // service for loading a octomap
167  srv_load_ = nh_.advertiseService(str_service + "load",
169 
170 
171  // count number of inserted pointclouds
172  count_cloud_ = 0;
173  count_cloud_old_ = 0;
174  count_laser_ = 0;
175 }
176 
177 //**************************[~cOctreeStampedPaNode]****************************
179 
180 }
181 
182 //**************************[publishOctomap]***********************************
184 
185  if (pub_octomap_.getNumSubscribers() > 0) {
187  }
190  }
191 
194  }
197  }
198 }
199 
200 //**************************[addPointcloudCallbackSub]*************************
202  const sensor_msgs::PointCloud2ConstPtr &msg) {
203 
204  if (!updateTime(msg->header.stamp)) {
206  return;
207  }
208 
209  tf::StampedTransform transform;
210 
211  try {
213  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
215  msg->header.frame_id, msg->header.stamp, transform);
216  } catch (tf::TransformException &ex){
217  ROS_WARN("%s",ex.what());
218  return;
219  }
220 
221  if (addCloud(msg, addparams_, transform)) {
222  count_cloud_++;
223  checkDegrading();
224  publishOctomap();
225  }
226 }
227 
228 //**************************[addPointcloudCallbackSub]*************************
230  const sensor_msgs::PointCloudConstPtr &msg) {
231 
232  if (!updateTime(msg->header.stamp)) {
234  return;
235  }
236 
237  tf::StampedTransform transform;
238 
239  try {
241  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
243  msg->header.frame_id, msg->header.stamp, transform);
244  } catch (tf::TransformException &ex){
245  ROS_WARN("%s",ex.what());
246  return;
247  }
248 
249  if (addCloud(msg, addparams_, transform)) {
251  checkDegrading();
252  publishOctomap();
253  }
254 }
255 
256 //**************************[addLaserCallbackSub]******************************
258  const sensor_msgs::LaserScanConstPtr &msg) {
259 
260  if (!updateTime(msg->header.stamp)) {
262  return;
263  }
264 
265  tf::StampedTransform transform;
266 
267  try {
269  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
271  msg->header.frame_id, msg->header.stamp, transform);
272  } catch (tf::TransformException &ex){
273  ROS_WARN("%s",ex.what());
274  return;
275  }
276 
277  updateTime(msg->header.stamp);
278  if (addCloud(msg, addparams_, transform)) {
279  count_laser_++;
280  checkDegrading();
281  publishOctomap();
282  }
283 }
284 
285 
286 //**************************[clearCallbackSrv]*********************************
287 bool cOctreeStampedPaNode::clearCallbackSrv(std_srvs::Empty::Request &req,
288  std_srvs::Empty::Response &res) {
289 
290  ROS_INFO("cOctreeStampedPaNode::clear()");
291 
292  count_cloud_ = 0;
293  count_cloud_old_ = 0;
294  count_laser_ = 0;
295 
296  clear();
298 
299  return true;
300 }
301 
302 //**************************[getSizeCallbackSrv]*******************************
304  octomap_pa::OctomapPaGetSize::Request &req,
305  octomap_pa::OctomapPaGetSize::Response &res) {
306 
307  ROS_INFO("cOctreeStampedPaNode::getsize()");
308 
309  res.size = size();
310  res.memoryusage = (int64_t) memoryUsage();
311 
312  res.count_cloud = count_cloud_ ;
313  res.count_cloud_old = count_cloud_old_;
314  res.count_laser = count_laser_ ;
315 
316  return true;
317 }
318 
319 //**************************[saveCallbackSrv]**********************************
321  octomap_pa::OctomapPaFileName::Request &req,
322  octomap_pa::OctomapPaFileName::Response &res) {
323 
324  ROS_INFO_STREAM("cOctreeStampedPaNode::save(" << req.filename << ")");
325 
326  std::string filename;
327  filename = req.filename;
328  cParameterPaRos par;
329  par.replaceFindpack(filename);
330 
331  //res.ok = writeBinary(filename);
332  res.ok = this->write(filename);
333 
334  return res.ok;
335 }
336 
337 //**************************[loadCallbackSrv]**********************************
339  octomap_pa::OctomapPaFileName::Request &req,
340  octomap_pa::OctomapPaFileName::Response &res) {
341 
342  ROS_INFO_STREAM("cOctreeStampedPaNode::load(" << req.filename << ")");
343 
344  std::string filename;
345  filename = req.filename;
346  cParameterPaRos par;
347  par.replaceFindpack(filename);
348 
349  // res.ok = readBinary(filename);
350  res.ok = readFull(filename);
351 
352  if (res.ok) { publishOctomap(); }
353  return res.ok;
354 }
sensor_msgs::PointCloud2Ptr getOctomapPcdFree(const int tree_depth=0, const bool expand=false) const
similar to getOctomapPcd, but only returning just empty voxels
cOctreeStampedPaRosParameter rosparams_
parameters
bool clearCallbackSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ros::ServiceServer srv_load_
service for loading a octomap
ros::Publisher pub_cloud_free_
puplisher for free voxels as pointcloud
std::string topic_out_octomap_full_
name of the topic for publishing the octomap ("~out_octomap_full")
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"
void addPointcloudOldCallbackSub(const sensor_msgs::PointCloudConstPtr &msg)
callback function for receiving a pointcloud (old format)
bool saveCallbackSrv(octomap_pa::OctomapPaFileName::Request &req, octomap_pa::OctomapPaFileName::Response &res)
double degrading_time_
duration how long the outdated nodes will be kept (60s)
~cOctreeStampedPaNode()
default destructor
void publish(const boost::shared_ptr< M > &message) const
virtual void clear(void)
clear local timestamps with octomap
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)
void addPointcloudCallbackSub(const sensor_msgs::PointCloud2ConstPtr &msg)
callback function for receiving a pointcloud
bool loadCallbackSrv(octomap_pa::OctomapPaFileName::Request &req, octomap_pa::OctomapPaFileName::Response &res)
cAddCloudParameter addparams_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
octomap_msgs::OctomapPtr getOctomap(void) const
function for getting the binary octomap
int count_laser_
number of inserted laser scans
void checkDegrading(void)
helper function for automatic degrading
int count_cloud_
number of inserted pointclouds
ros::Publisher pub_octomap_
puplisher for the octomap (binary data)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool loadTopic(const std::string name, std::string &value, const bool print_default=true) const
#define ROS_WARN(...)
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)
int main(int argc, char **argv)
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())
cOctreeStampedPaNode()
default constructor
int count_cloud_old_
number of inserted pointclouds (old format)
bool pcd_voxel_active_
pointcloud insertion parameter: use voxel-filter (speeds up)
ros::Subscriber sub_cloud_old_
subscriber for a pointcloud (old format)
bool getSizeCallbackSrv(octomap_pa::OctomapPaGetSize::Request &req, octomap_pa::OctomapPaGetSize::Response &res)
#define ROS_INFO(...)
ros::NodeHandle nh_
node handler for topic subscription and advertising
double auto_degrading_intervall_
intervall for automatic degrading (2.0s)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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
sensor_msgs::PointCloud2Ptr getOctomapPcd(const int tree_depth=0, const bool expand=false) const
function for getting the pointcloud equivalent of the octomap
void addLaserCallbackSub(const sensor_msgs::LaserScanConstPtr &msg)
callback function for receiving a laserscan
ros::Subscriber sub_laser_
subscriber for a laserscan
uint32_t getNumSubscribers() const
ros::Subscriber sub_cloud_
subscriber for a pointcloud
#define ROS_INFO_STREAM(args)
std::string topic_in_laser_
name of the topic for subscribing a laserscan ("~in_laser")
ros::ServiceServer srv_save_
service for saving the octomap
bool auto_degrading_
turns on automatic degrading (true)
bool readFull(const std::string &filename)
trying to read the given file into the current OcTree
cOctreeBasePaNodeParameter nodeparams_
parameters
void publishOctomap(void)
function for publishing the octomap
ros::Publisher pub_cloud_occupied_
puplisher for occupied voxels as pointcloud
ros::Publisher pub_octomap_full_
puplisher for the octomap (full data)
ros::ServiceServer srv_getsize_
service for receiving the size of the octomap
tf::TransformListener tf_listener_
transformation of different frames
double pcd_voxel_explicit_relative_resolution_
pointcloud insertion parameter: relative resolution of pcl-filter
ros::ServiceServer srv_clear_
service for clearing the octomap


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