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  , point_cloud_subscriber_(NULL)
56  , point_cloud_filter_(NULL)
57 {
58 }
59 
61 {
62  stopHelper();
63 }
64 
66 {
67  try
68  {
69  if (!params.hasMember("point_cloud_topic"))
70  return false;
71  point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);
72 
73  readXmlParam(params, "max_range", &max_range_);
74  readXmlParam(params, "padding_offset", &padding_);
75  readXmlParam(params, "padding_scale", &scale_);
76  readXmlParam(params, "point_subsample", &point_subsample_);
77  if (params.hasMember("filtered_cloud_topic"))
78  filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
79  }
80  catch (XmlRpc::XmlRpcException& ex)
81  {
82  ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
83  return false;
84  }
85 
86  return true;
87 }
88 
90 {
93  shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2));
94  if (!filtered_cloud_topic_.empty())
95  filtered_cloud_publisher_ = private_nh_.advertise<sensor_msgs::PointCloud2>(filtered_cloud_topic_, 10, false);
96  return true;
97 }
98 
100 {
102  return;
103  /* subscribe to point cloud topic using tf filter*/
105  if (tf_ && !monitor_->getMapFrame().empty())
106  {
110  ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(),
112  }
113  else
114  {
116  ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str());
117  }
118 }
119 
121 {
122  delete point_cloud_filter_;
124 }
125 
127 {
128  stopHelper();
129  point_cloud_filter_ = NULL;
131 }
132 
134 {
135  ShapeHandle h = 0;
136  if (shape_mask_)
137  h = shape_mask_->addShape(shape, scale_, padding_);
138  else
139  ROS_ERROR("Shape filter not yet initialized!");
140  return h;
141 }
142 
144 {
145  if (shape_mask_)
146  shape_mask_->removeShape(handle);
147 }
148 
149 bool PointCloudOctomapUpdater::getShapeTransform(ShapeHandle h, Eigen::Affine3d& transform) const
150 {
151  ShapeTransformCache::const_iterator it = transform_cache_.find(h);
152  if (it == transform_cache_.end())
153  {
154  ROS_ERROR("Internal error. Shape filter handle %u not found", h);
155  return false;
156  }
157  transform = it->second;
158  return true;
159 }
160 
161 void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin,
162  std::vector<int>& mask)
163 {
164 }
165 
166 void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
167 {
168  ROS_DEBUG("Received a new point cloud message");
170 
171  if (monitor_->getMapFrame().empty())
172  monitor_->setMapFrame(cloud_msg->header.frame_id);
173 
174  /* get transform for cloud into map frame */
175  tf::StampedTransform map_H_sensor;
176  if (monitor_->getMapFrame() == cloud_msg->header.frame_id)
177  map_H_sensor.setIdentity();
178  else
179  {
180  if (tf_)
181  {
182  try
183  {
184  tf_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp,
185  map_H_sensor);
186  }
187  catch (tf::TransformException& ex)
188  {
189  ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << "; quitting callback");
190  return;
191  }
192  }
193  else
194  return;
195  }
196 
197  /* compute sensor origin in map frame */
198  const tf::Vector3& sensor_origin_tf = map_H_sensor.getOrigin();
199  octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
200  Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
201 
202  if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp))
203  {
204  ROS_ERROR_THROTTLE(1, "Transform cache was not updated. Self-filtering may fail.");
205  return;
206  }
207 
208  /* mask out points on the robot */
209  shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, max_range_, mask_);
210  updateMask(*cloud_msg, sensor_origin_eigen, mask_);
211 
212  octomap::KeySet free_cells, occupied_cells, model_cells, clip_cells;
213  std::unique_ptr<sensor_msgs::PointCloud2> filtered_cloud;
214 
215  // We only use these iterators if we are creating a filtered_cloud for
216  // publishing. We cannot default construct these, so we use unique_ptr's
217  // to defer construction
218  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_x;
219  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_y;
220  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float> > iter_filtered_z;
221 
222  if (!filtered_cloud_topic_.empty())
223  {
224  filtered_cloud.reset(new sensor_msgs::PointCloud2());
225  filtered_cloud->header = cloud_msg->header;
226  sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
227  pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
228  pcd_modifier.resize(cloud_msg->width * cloud_msg->height);
229 
230  // we have created a filtered_out, so we can create the iterators now
231  iter_filtered_x.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "x"));
232  iter_filtered_y.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "y"));
233  iter_filtered_z.reset(new sensor_msgs::PointCloud2Iterator<float>(*filtered_cloud, "z"));
234  }
235  size_t filtered_cloud_size = 0;
236 
237  tree_->lockRead();
238 
239  try
240  {
241  /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
242  * should be occupied */
243  for (unsigned int row = 0; row < cloud_msg->height; row += point_subsample_)
244  {
245  unsigned int row_c = row * cloud_msg->width;
246  sensor_msgs::PointCloud2ConstIterator<float> pt_iter(*cloud_msg, "x");
247  // set iterator to point at start of the current row
248  pt_iter += row_c;
249 
250  for (unsigned int col = 0; col < cloud_msg->width; col += point_subsample_, pt_iter += point_subsample_)
251  {
252  // if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
253  // continue;
254 
255  /* check for NaN */
256  if (!std::isnan(pt_iter[0]) && !std::isnan(pt_iter[1]) && !std::isnan(pt_iter[2]))
257  {
258  /* transform to map frame */
259  tf::Vector3 point_tf = map_H_sensor * tf::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]);
260 
261  /* occupied cell at ray endpoint if ray is shorter than max range and this point
262  isn't on a part of the robot*/
264  model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
265  else if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
266  clip_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
267  else
268  {
269  occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
270  // build list of valid points if we want to publish them
271  if (filtered_cloud)
272  {
273  **iter_filtered_x = pt_iter[0];
274  **iter_filtered_y = pt_iter[1];
275  **iter_filtered_z = pt_iter[2];
276  ++filtered_cloud_size;
277  ++*iter_filtered_x;
278  ++*iter_filtered_y;
279  ++*iter_filtered_z;
280  }
281  }
282  }
283  }
284  }
285 
286  /* compute the free cells along each ray that ends at an occupied cell */
287  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
288  if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
289  free_cells.insert(key_ray_.begin(), key_ray_.end());
290 
291  /* compute the free cells along each ray that ends at a model cell */
292  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
293  if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
294  free_cells.insert(key_ray_.begin(), key_ray_.end());
295 
296  /* compute the free cells along each ray that ends at a clipped cell */
297  for (octomap::KeySet::iterator it = clip_cells.begin(), end = clip_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  catch (...)
302  {
303  tree_->unlockRead();
304  return;
305  }
306 
307  tree_->unlockRead();
308 
309  /* cells that overlap with the model are not occupied */
310  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
311  occupied_cells.erase(*it);
312 
313  /* occupied cells are not free */
314  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
315  free_cells.erase(*it);
316 
317  tree_->lockWrite();
318 
319  try
320  {
321  /* mark free cells only if not seen occupied in this cloud */
322  for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it)
323  tree_->updateNode(*it, false);
324 
325  /* now mark all occupied cells */
326  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
327  tree_->updateNode(*it, true);
328 
329  // set the logodds to the minimum for the cells that are part of the model
330  const float lg = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
331  for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
332  tree_->updateNode(*it, lg);
333  }
334  catch (...)
335  {
336  ROS_ERROR("Internal error while updating octree");
337  }
338  tree_->unlockWrite();
339  ROS_DEBUG("Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
340  tree_->triggerUpdateCallback();
341 
342  if (filtered_cloud)
343  {
344  sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
345  pcd_modifier.resize(filtered_cloud_size);
346  filtered_cloud_publisher_.publish(*filtered_cloud);
347  }
348 }
349 }
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
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 Mon Jan 15 2018 03:51:11