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  ROS_ERROR("Internal error. Shape filter handle %u not found", h);
158  return false;
159  }
160  transform = it->second;
161  return true;
162 }
163 
164 void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin,
165  std::vector<int>& mask)
166 {
167 }
168 
169 void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
170 {
171  ROS_DEBUG("Received a new point cloud message");
173 
174  if (max_update_rate_ > 0)
175  {
176  // ensure we are not updating the octomap representation too often
178  return;
180  }
181 
182  if (monitor_->getMapFrame().empty())
183  monitor_->setMapFrame(cloud_msg->header.frame_id);
184 
185  /* get transform for cloud into map frame */
186  tf::StampedTransform map_H_sensor;
187  if (monitor_->getMapFrame() == cloud_msg->header.frame_id)
188  map_H_sensor.setIdentity();
189  else
190  {
191  if (tf_)
192  {
193  try
194  {
195  tf_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp,
196  map_H_sensor);
197  }
198  catch (tf::TransformException& ex)
199  {
200  ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << "; quitting callback");
201  return;
202  }
203  }
204  else
205  return;
206  }
207 
208  /* compute sensor origin in map frame */
209  const tf::Vector3& sensor_origin_tf = map_H_sensor.getOrigin();
210  octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
211  Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
212 
213  if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp))
214  {
215  ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail.");
216  return;
217  }
218 
219  /* mask out points on the robot */
220  shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, max_range_, mask_);
221  updateMask(*cloud_msg, sensor_origin_eigen, mask_);
222 
223  octomap::KeySet free_cells, occupied_cells, model_cells, clip_cells;
224  std::unique_ptr<sensor_msgs::PointCloud2> filtered_cloud;
225 
226  // We only use these iterators if we are creating a filtered_cloud for
227  // publishing. We cannot default construct these, so we use unique_ptr's
228  // to defer construction
229  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_x;
230  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_y;
231  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_z;
232 
233  if (!filtered_cloud_topic_.empty())
234  {
235  filtered_cloud.reset(new sensor_msgs::PointCloud2());
236  filtered_cloud->header = cloud_msg->header;
237  sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
238  pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
239  pcd_modifier.resize(cloud_msg->width * cloud_msg->height);
240 
241  // we have created a filtered_out, so we can create the iterators now
242  iter_filtered_x.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "x"));
243  iter_filtered_y.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "y"));
244  iter_filtered_z.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "z"));
245  }
246  size_t filtered_cloud_size = 0;
247 
248  tree_->lockRead();
249 
250  try
251  {
252  /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
253  * should be occupied */
254  for (unsigned int row = 0; row < cloud_msg->height; row += point_subsample_)
255  {
256  unsigned int row_c = row * cloud_msg->width;
257  sensor_msgs::PointCloud2ConstIterator<float> pt_iter(*cloud_msg, "x");
258  // set iterator to point at start of the current row
259  pt_iter += row_c;
260 
261  for (unsigned int col = 0; col < cloud_msg->width; col += point_subsample_, pt_iter += point_subsample_)
262  {
263  // if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
264  // continue;
265 
266  /* check for NaN */
267  if (!std::isnan(pt_iter[0]) && !std::isnan(pt_iter[1]) && !std::isnan(pt_iter[2]))
268  {
269  /* transform to map frame */
270  tf::Vector3 point_tf = map_H_sensor * tf::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]);
271 
272  /* occupied cell at ray endpoint if ray is shorter than max range and this point
273  isn't on a part of the robot*/
275  model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
276  else if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
277  clip_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
278  else
279  {
280  occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
281  // build list of valid points if we want to publish them
282  if (filtered_cloud)
283  {
284  **iter_filtered_x = pt_iter[0];
285  **iter_filtered_y = pt_iter[1];
286  **iter_filtered_z = pt_iter[2];
287  ++filtered_cloud_size;
288  ++*iter_filtered_x;
289  ++*iter_filtered_y;
290  ++*iter_filtered_z;
291  }
292  }
293  }
294  }
295  }
296 
297  /* compute the free cells along each ray that ends at an occupied cell */
298  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
299  if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
300  free_cells.insert(key_ray_.begin(), key_ray_.end());
301 
302  /* compute the free cells along each ray that ends at a model cell */
303  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
304  if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
305  free_cells.insert(key_ray_.begin(), key_ray_.end());
306 
307  /* compute the free cells along each ray that ends at a clipped cell */
308  for (octomap::KeySet::iterator it = clip_cells.begin(), end = clip_cells.end(); it != end; ++it)
309  if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
310  free_cells.insert(key_ray_.begin(), key_ray_.end());
311  }
312  catch (...)
313  {
314  tree_->unlockRead();
315  return;
316  }
317 
318  tree_->unlockRead();
319 
320  /* cells that overlap with the model are not occupied */
321  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
322  occupied_cells.erase(*it);
323 
324  /* occupied cells are not free */
325  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
326  free_cells.erase(*it);
327 
328  tree_->lockWrite();
329 
330  try
331  {
332  /* mark free cells only if not seen occupied in this cloud */
333  for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it)
334  tree_->updateNode(*it, false);
335 
336  /* now mark all occupied cells */
337  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
338  tree_->updateNode(*it, true);
339 
340  // set the logodds to the minimum for the cells that are part of the model
341  const float lg = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
342  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
343  tree_->updateNode(*it, lg);
344  }
345  catch (...)
346  {
347  ROS_ERROR("Internal error while updating octree");
348  }
349  tree_->unlockWrite();
350  ROS_DEBUG("Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
351  tree_->triggerUpdateCallback();
352 
353  if (filtered_cloud)
354  {
355  sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
356  pcd_modifier.resize(filtered_cloud_size);
357  filtered_cloud_publisher_.publish(*filtered_cloud);
358  }
359 }
360 }
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 Tue Jun 12 2018 02:48:04