octree_stamped_native_node.cpp
Go to the documentation of this file.
1 /******************************************************************************
2 * *
3 * octree_stamped_native_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_native_node");
60 
61  ros::spin();
62 
63  return 0;
64 }
65 
66 //**************************[cOctreeStampedNativeNode]*************************
68  cOctreeStampedNativeRos(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  // 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 
120  // Subscriber for pointclouds
121  if (nodeparams_.topic_in_cloud_ != "") {
122  sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(
125  }
126 
127  // Subscriber for pointclouds (old format)
128  if (nodeparams_.topic_in_cloud_old_ != "") {
129  sub_cloud_old_ = nh_.subscribe<sensor_msgs::PointCloud>(
132  }
133  // Subscriber for laserscans
134  if (nodeparams_.topic_in_laser_ != "") {
135  sub_laser_ = nh_.subscribe<sensor_msgs::LaserScan>(
138  }
139 
140 
141  // puplisher for the octomap (binary data)
142  pub_octomap_ = nh_.advertise<octomap_msgs::Octomap>(
143  nodeparams_.topic_out_octomap_, 10, true);
144  // puplisher for the octomap (full data)
145  pub_octomap_full_ = nh_.advertise<octomap_msgs::Octomap>(
147 
148  // puplisher for free voxels as pointcloud
149  pub_cloud_free_ = nh_.advertise<sensor_msgs::PointCloud2>(
151  // puplisher for occupied voxels as pointcloud
152  pub_cloud_occupied_ = nh_.advertise<sensor_msgs::PointCloud2>(
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  // count number of inserted pointclouds
171  count_cloud_ = 0;
172  count_cloud_old_ = 0;
173  count_laser_ = 0;
174 }
175 
176 //**************************[~cOctreeStampedNativeNode]************************
178 
179 }
180 
181 //**************************[publishOctomap]***********************************
183 
184  if (pub_octomap_.getNumSubscribers() > 0) {
186  }
189  }
190 
193  }
196  }
197 }
198 
199 //**************************[addPointcloudCallbackSub]*************************
201  const sensor_msgs::PointCloud2ConstPtr &msg) {
202 
203  if (!updateTime(msg->header.stamp)) {
205  return;
206  }
207 
208  tf::StampedTransform transform;
209 
210  try {
212  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
214  msg->header.frame_id, msg->header.stamp, transform);
215  } catch (tf::TransformException &ex){
216  ROS_WARN("%s",ex.what());
217  return;
218  }
219 
220  if (addCloud(msg, addparams_, transform)) {
221  count_cloud_++;
222  checkDegrading();
223  publishOctomap();
224  }
225 }
226 
227 //**************************[addPointcloudCallbackSub]*************************
229  const sensor_msgs::PointCloudConstPtr &msg) {
230 
231  if (!updateTime(msg->header.stamp)) {
233  return;
234  }
235 
236  tf::StampedTransform transform;
237 
238  try {
240  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
242  msg->header.frame_id, msg->header.stamp, transform);
243  } catch (tf::TransformException &ex){
244  ROS_WARN("%s",ex.what());
245  return;
246  }
247 
248  if (addCloud(msg, addparams_, transform)) {
250  checkDegrading();
251  publishOctomap();
252  }
253 }
254 
255 //**************************[addLaserCallbackSub]******************************
257  const sensor_msgs::LaserScanConstPtr &msg) {
258 
259  if (!updateTime(msg->header.stamp)) {
261  return;
262  }
263 
264  tf::StampedTransform transform;
265 
266  try {
268  msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
270  msg->header.frame_id, msg->header.stamp, transform);
271  } catch (tf::TransformException &ex){
272  ROS_WARN("%s",ex.what());
273  return;
274  }
275 
276  updateTime(msg->header.stamp);
277  if (addCloud(msg, addparams_, transform)) {
278  count_laser_++;
279  checkDegrading();
280  publishOctomap();
281  }
282 }
283 
284 
285 //**************************[clearCallbackSrv]*********************************
286 bool cOctreeStampedNativeNode::clearCallbackSrv(std_srvs::Empty::Request &req,
287  std_srvs::Empty::Response &res) {
288 
289  ROS_INFO("cOctreeStampedNativeNode::clear()");
290 
291  count_cloud_ = 0;
292  count_cloud_old_ = 0;
293  count_laser_ = 0;
294 
295  clear();
297 
298  return true;
299 }
300 
301 //**************************[getSizeCallbackSrv]*******************************
303  octomap_pa::OctomapPaGetSize::Request &req,
304  octomap_pa::OctomapPaGetSize::Response &res) {
305 
306  ROS_INFO("cOctreeStampedNativeNode::getsize()");
307 
308  res.size = size();
309  res.memoryusage = (int64_t) memoryUsage();
310 
311  res.count_cloud = count_cloud_ ;
312  res.count_cloud_old = count_cloud_old_;
313  res.count_laser = count_laser_ ;
314 
315  return true;
316 }
317 
318 //**************************[saveCallbackSrv]**********************************
320  octomap_pa::OctomapPaFileName::Request &req,
321  octomap_pa::OctomapPaFileName::Response &res) {
322 
323  ROS_INFO_STREAM("cOctreeStampedNativeNode::save(" << req.filename << ")");
324 
325  std::string filename;
326  filename = req.filename;
327  cParameterPaRos par;
328  par.replaceFindpack(filename);
329 
330  //res.ok = writeBinary(filename);
331  res.ok = this->write(filename);
332 
333  return res.ok;
334 }
335 
336 //**************************[loadCallbackSrv]**********************************
338  octomap_pa::OctomapPaFileName::Request &req,
339  octomap_pa::OctomapPaFileName::Response &res) {
340 
341  ROS_INFO_STREAM("cOctreeStampedNativeNode::load(" << req.filename << ")");
342 
343  std::string filename;
344  filename = req.filename;
345  cParameterPaRos par;
346  par.replaceFindpack(filename);
347 
348  // res.ok = readBinary(filename);
349  res.ok = readFull(filename);
350 
351  if (res.ok) { publishOctomap(); }
352  return res.ok;
353 }
sensor_msgs::PointCloud2Ptr getOctomapPcdFree(const int tree_depth=0, const bool expand=false) const
similar to getOctomapPcd, but only returning just empty voxels
ros::Subscriber sub_cloud_old_
subscriber for a pointcloud (old format)
std::string topic_out_octomap_full_
name of the topic for publishing the octomap ("~out_octomap_full")
ros::Subscriber sub_cloud_
subscriber for a pointcloud
ros::ServiceServer srv_save_
service for saving the octomap
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
ros::Publisher pub_cloud_free_
puplisher for free voxels as pointcloud
double map_prob_miss_
octomap parameter: probability for a "miss"
double degrading_time_
duration how long the outdated nodes will be kept (60s)
void publish(const boost::shared_ptr< M > &message) const
virtual void clear(void)
clear local timestamps with octomap
ros::Subscriber sub_laser_
subscriber for a laserscan
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)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int count_cloud_
number of inserted pointclouds
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)
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)
bool loadCallbackSrv(octomap_pa::OctomapPaFileName::Request &req, octomap_pa::OctomapPaFileName::Response &res)
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())
ros::Publisher pub_octomap_
puplisher for the octomap (binary data)
int main(int argc, char **argv)
ros::ServiceServer srv_clear_
service for clearing the octomap
ros::Publisher pub_cloud_occupied_
puplisher for occupied voxels as pointcloud
bool pcd_voxel_active_
pointcloud insertion parameter: use voxel-filter (speeds up)
~cOctreeStampedNativeNode()
default destructor
cOctreeStampedPaRosParameter rosparams_
parameters
#define ROS_INFO(...)
bool write(const std::string &filename) const
double auto_degrading_intervall_
intervall for automatic degrading (2.0s)
void addLaserCallbackSub(const sensor_msgs::LaserScanConstPtr &msg)
callback function for receiving a laserscan
bool getSizeCallbackSrv(octomap_pa::OctomapPaGetSize::Request &req, octomap_pa::OctomapPaGetSize::Response &res)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool clearCallbackSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
cOctreeBasePaRosParameter rosparams_base_
parameters
void checkDegrading(void)
helper function for automatic degrading
ros::ServiceServer srv_getsize_
service for receiving the size of the octomap
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
int count_cloud_old_
number of inserted pointclouds (old format)
cOctreeStampedNativeNode()
default constructor
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)
void publishOctomap(void)
function for publishing the octomap
uint32_t getNumSubscribers() const
ros::Publisher pub_octomap_full_
puplisher for the octomap (full data)
#define ROS_INFO_STREAM(args)
std::string topic_in_laser_
name of the topic for subscribing a laserscan ("~in_laser")
bool auto_degrading_
turns on automatic degrading (true)
cOctreeBasePaNodeParameter nodeparams_
parameters
bool readFull(const std::string &filename)
trying to read the given file into the current OcTree
tf::TransformListener tf_listener_
transformation of different frames
int count_laser_
number of inserted laser scans
ros::NodeHandle nh_
node handler for topic subscription and advertising
bool saveCallbackSrv(octomap_pa::OctomapPaFileName::Request &req, octomap_pa::OctomapPaFileName::Response &res)
double pcd_voxel_explicit_relative_resolution_
pointcloud insertion parameter: relative resolution of pcl-filter
ros::ServiceServer srv_load_
service for loading a octomap
void addPointcloudCallbackSub(const sensor_msgs::PointCloud2ConstPtr &msg)
callback function for receiving a pointcloud
void setClampingThresMin(double thresProb)


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