pointcloud_octomap_updater.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Jon Binney, Ioan Sucan */
36 
37 #include <cmath>
42 #include <XmlRpcException.h>
43 
44 #include <memory>
45 
46 namespace occupancy_map_monitor
47 {
49  : OccupancyMapUpdater("PointCloudUpdater")
50  , private_nh_("~")
51  , scale_(1.0)
52  , padding_(0.0)
53  , max_range_(std::numeric_limits<double>::infinity())
54  , point_subsample_(1)
55  , max_update_rate_(0)
56  , point_cloud_subscriber_(NULL)
57  , point_cloud_filter_(NULL)
58 {
59 }
60 
62 {
63  stopHelper();
64 }
65 
67 {
68  try
69  {
70  if (!params.hasMember("point_cloud_topic"))
71  return false;
72  point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);
73 
74  readXmlParam(params, "max_range", &max_range_);
75  readXmlParam(params, "padding_offset", &padding_);
76  readXmlParam(params, "padding_scale", &scale_);
77  readXmlParam(params, "point_subsample", &point_subsample_);
78  if (params.hasMember("max_update_rate"))
79  readXmlParam(params, "max_update_rate", &max_update_rate_);
80  if (params.hasMember("filtered_cloud_topic"))
81  filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
82  }
83  catch (XmlRpc::XmlRpcException& ex)
84  {
85  ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
86  return false;
87  }
88 
89  return true;
90 }
91 
93 {
96  shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2));
97  if (!filtered_cloud_topic_.empty())
98  filtered_cloud_publisher_ = private_nh_.advertise<sensor_msgs::PointCloud2>(filtered_cloud_topic_, 10, false);
99  return true;
100 }
101 
103 {
105  return;
106  /* subscribe to point cloud topic using tf filter*/
108  if (tf_ && !monitor_->getMapFrame().empty())
109  {
113  ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(),
115  }
116  else
117  {
119  ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str());
120  }
121 }
122 
124 {
125  delete point_cloud_filter_;
127 }
128 
130 {
131  stopHelper();
132  point_cloud_filter_ = NULL;
134 }
135 
137 {
138  ShapeHandle h = 0;
139  if (shape_mask_)
140  h = shape_mask_->addShape(shape, scale_, padding_);
141  else
142  ROS_ERROR("Shape filter not yet initialized!");
143  return h;
144 }
145 
147 {
148  if (shape_mask_)
149  shape_mask_->removeShape(handle);
150 }
151 
152 bool PointCloudOctomapUpdater::getShapeTransform(ShapeHandle h, Eigen::Affine3d& transform) const
153 {
154  ShapeTransformCache::const_iterator it = transform_cache_.find(h);
155  if (it == transform_cache_.end())
156  {
157  return false;
158  }
159  transform = it->second;
160  return true;
161 }
162 
163 void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin,
164  std::vector<int>& mask)
165 {
166 }
167 
168 void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
169 {
170  ROS_DEBUG("Received a new point cloud message");
172 
173  if (max_update_rate_ > 0)
174  {
175  // ensure we are not updating the octomap representation too often
177  return;
179  }
180 
181  if (monitor_->getMapFrame().empty())
182  monitor_->setMapFrame(cloud_msg->header.frame_id);
183 
184  /* get transform for cloud into map frame */
185  tf::StampedTransform map_H_sensor;
186  if (monitor_->getMapFrame() == cloud_msg->header.frame_id)
187  map_H_sensor.setIdentity();
188  else
189  {
190  if (tf_)
191  {
192  try
193  {
194  tf_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp,
195  map_H_sensor);
196  }
197  catch (tf::TransformException& ex)
198  {
199  ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << "; quitting callback");
200  return;
201  }
202  }
203  else
204  return;
205  }
206 
207  /* compute sensor origin in map frame */
208  const tf::Vector3& sensor_origin_tf = map_H_sensor.getOrigin();
209  octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
210  Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
211 
212  if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp))
213  {
214  ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail.");
215  return;
216  }
217 
218  /* mask out points on the robot */
219  shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, max_range_, mask_);
220  updateMask(*cloud_msg, sensor_origin_eigen, mask_);
221 
222  octomap::KeySet free_cells, occupied_cells, model_cells, clip_cells;
223  std::unique_ptr<sensor_msgs::PointCloud2> filtered_cloud;
224 
225  // We only use these iterators if we are creating a filtered_cloud for
226  // publishing. We cannot default construct these, so we use unique_ptr's
227  // to defer construction
228  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_x;
229  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_y;
230  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_z;
231 
232  if (!filtered_cloud_topic_.empty())
233  {
234  filtered_cloud.reset(new sensor_msgs::PointCloud2());
235  filtered_cloud->header = cloud_msg->header;
236  sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
237  pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
238  pcd_modifier.resize(cloud_msg->width * cloud_msg->height);
239 
240  // we have created a filtered_out, so we can create the iterators now
241  iter_filtered_x.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "x"));
242  iter_filtered_y.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "y"));
243  iter_filtered_z.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "z"));
244  }
245  size_t filtered_cloud_size = 0;
246 
247  tree_->lockRead();
248 
249  try
250  {
251  /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
252  * should be occupied */
253  for (unsigned int row = 0; row < cloud_msg->height; row += point_subsample_)
254  {
255  unsigned int row_c = row * cloud_msg->width;
256  sensor_msgs::PointCloud2ConstIterator<float> pt_iter(*cloud_msg, "x");
257  // set iterator to point at start of the current row
258  pt_iter += row_c;
259 
260  for (unsigned int col = 0; col < cloud_msg->width; col += point_subsample_, pt_iter += point_subsample_)
261  {
262  // if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
263  // continue;
264 
265  /* check for NaN */
266  if (!std::isnan(pt_iter[0]) && !std::isnan(pt_iter[1]) && !std::isnan(pt_iter[2]))
267  {
268  /* transform to map frame */
269  tf::Vector3 point_tf = map_H_sensor * tf::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]);
270 
271  /* occupied cell at ray endpoint if ray is shorter than max range and this point
272  isn't on a part of the robot*/
274  model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
275  else if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
276  clip_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
277  else
278  {
279  occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
280  // build list of valid points if we want to publish them
281  if (filtered_cloud)
282  {
283  **iter_filtered_x = pt_iter[0];
284  **iter_filtered_y = pt_iter[1];
285  **iter_filtered_z = pt_iter[2];
286  ++filtered_cloud_size;
287  ++*iter_filtered_x;
288  ++*iter_filtered_y;
289  ++*iter_filtered_z;
290  }
291  }
292  }
293  }
294  }
295 
296  /* compute the free cells along each ray that ends at an occupied cell */
297  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
298  if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
299  free_cells.insert(key_ray_.begin(), key_ray_.end());
300 
301  /* compute the free cells along each ray that ends at a model cell */
302  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
303  if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
304  free_cells.insert(key_ray_.begin(), key_ray_.end());
305 
306  /* compute the free cells along each ray that ends at a clipped cell */
307  for (octomap::KeySet::iterator it = clip_cells.begin(), end = clip_cells.end(); it != end; ++it)
308  if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
309  free_cells.insert(key_ray_.begin(), key_ray_.end());
310  }
311  catch (...)
312  {
313  tree_->unlockRead();
314  return;
315  }
316 
317  tree_->unlockRead();
318 
319  /* cells that overlap with the model are not occupied */
320  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
321  occupied_cells.erase(*it);
322 
323  /* occupied cells are not free */
324  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
325  free_cells.erase(*it);
326 
327  tree_->lockWrite();
328 
329  try
330  {
331  /* mark free cells only if not seen occupied in this cloud */
332  for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it)
333  tree_->updateNode(*it, false);
334 
335  /* now mark all occupied cells */
336  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
337  tree_->updateNode(*it, true);
338 
339  // set the logodds to the minimum for the cells that are part of the model
340  const float lg = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
341  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
342  tree_->updateNode(*it, lg);
343  }
344  catch (...)
345  {
346  ROS_ERROR("Internal error while updating octree");
347  }
348  tree_->unlockWrite();
349  ROS_DEBUG("Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
350  tree_->triggerUpdateCallback();
351 
352  if (filtered_cloud)
353  {
354  sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
355  pcd_modifier.resize(filtered_cloud_size);
356  filtered_cloud_publisher_.publish(*filtered_cloud);
357  }
358 }
359 }
const std::string & getMessage() const
iterator begin()
virtual bool initialize()
Do any necessary setup (subscribe to ros topics, etc.). This call assumes setMonitor() and setParams(...
std::unique_ptr< point_containment_filter::ShapeMask > shape_mask_
void publish(const boost::shared_ptr< M > &message) const
Base class for classes which update the occupancy map.
message_filters::Subscriber< sensor_msgs::PointCloud2 > * point_cloud_subscriber_
TFSIMD_FORCE_INLINE const tfScalar & getY() const
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape)
bool getShapeTransform(ShapeHandle h, Eigen::Affine3d &transform) const
void setIdentity()
#define ROS_ERROR_THROTTLE(rate,...)
const boost::shared_ptr< tf::Transformer > & getTFClient() const
Computing a mask for a pointcloud that states which points are inside the robot.
Definition: shape_mask.h:55
#define ROS_INFO(...)
virtual void updateMask(const sensor_msgs::PointCloud2 &cloud, const Eigen::Vector3d &sensor_origin, std::vector< int > &mask)
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
iterator end()
bool hasMember(const std::string &name) const
static WallTime now()
static void readXmlParam(XmlRpc::XmlRpcValue &params, const std::string &param_name, double *value)
virtual bool setParams(XmlRpc::XmlRpcValue &params)
Set updater params using struct that comes from parsing a yaml string. This must be called after setM...
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static Time now()
std::string getTargetFramesString()
bool updateTransformCache(const std::string &target_frame, const ros::Time &target_time)
#define ROS_ERROR_STREAM(args)
void setPointCloud2FieldsByString(int n_fields,...)
#define ROS_ERROR(...)
Connection registerCallback(const C &callback)
#define ROS_DEBUG(...)
std::shared_ptr< const Shape > ShapeConstPtr
tf::MessageFilter< sensor_msgs::PointCloud2 > * point_cloud_filter_


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23