obstacles_detection_old.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
30 #include <nodelet/nodelet.h>
31 
32 #include <pcl/point_cloud.h>
33 #include <pcl/point_types.h>
35 
36 #include <tf/transform_listener.h>
37 
38 #include <sensor_msgs/PointCloud2.h>
39 
41 
42 #include "rtabmap/core/util3d.h"
46 
47 namespace rtabmap_ros
48 {
49 
51 {
52 public:
54  frameId_("base_link"),
55  normalKSearch_(20),
56  groundNormalAngle_(M_PI_4),
57  clusterRadius_(0.05),
58  minClusterSize_(20),
59  maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
60  maxGroundHeight_(0.0), // if<=0.0 -> disabled, used only if detect_flat_obstacles is true
64  projVoxelSize_(0.01)
65  {}
66 
68  {}
69 
70 private:
71  virtual void onInit()
72  {
75 
76  int queueSize = 10;
77  pnh.param("queue_size", queueSize, queueSize);
78  pnh.param("frame_id", frameId_, frameId_);
79  pnh.param("normal_k", normalKSearch_, normalKSearch_);
80  pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
81  if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius"))
82  {
83  NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed "
84  "to \"cluster_radius\"! Your value is still copied to "
85  "corresponding parameter. Instead of normal radius, nearest neighbors count "
86  "\"normal_k\" is used instead (default 20).");
87  pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_);
88  }
89  else
90  {
91  pnh.param("cluster_radius", clusterRadius_, clusterRadius_);
92  }
93  pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
94  pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
95  pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_);
96  pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_);
97  pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
98  pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
99  pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_);
100 
101  cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetectionOld::callback, this);
102 
103  groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
104  obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
105  projObstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("proj_obstacles", 1);
106  }
107 
108 
109 
110  void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
111  {
113 
115  {
116  // no one wants the results
117  return;
118  }
119 
120  rtabmap::Transform localTransform;
121  try
122  {
124  {
125  if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
126  {
127  NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
128  return;
129  }
130  }
132  tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
133  localTransform = rtabmap_ros::transformFromTF(tmp);
134  }
135  catch(tf::TransformException & ex)
136  {
137  NODELET_ERROR("%s",ex.what());
138  return;
139  }
140 
141  pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
142  pcl::fromROSMsg(*cloudMsg, *originalCloud);
143 
144  //Common variables for all strategies
145  pcl::IndicesPtr ground, obstacles;
146  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
147  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
148  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
149 
150  if(originalCloud->size())
151  {
152  originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
153  if(maxObstaclesHeight_ > 0)
154  {
155  // std::numeric_limits<float>::lowest() exists only for c++11
157  }
158 
159  if(originalCloud->size())
160  {
162  {
163  // This is the default strategy
164  pcl::IndicesPtr flatObstacles(new std::vector<int>);
165  rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
166  originalCloud,
167  ground,
168  obstacles,
175  &flatObstacles);
176 
178  ground.get() && ground->size())
179  {
180  pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
181  }
182 
184  obstacles.get() && obstacles->size())
185  {
186  // remove flat obstacles from obstacles
187  std::set<int> flatObstaclesSet;
189  {
190  flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
191  }
192 
193  obstaclesCloud->resize(obstacles->size());
194  obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
195 
196  int oi=0;
197  for(unsigned int i=0; i<obstacles->size(); ++i)
198  {
199  obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
200  if(flatObstaclesSet.size() == 0 ||
201  flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
202  {
203  obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
204  obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
205  ++oi;
206  }
207 
208  }
209 
210  obstaclesCloudWithoutFlatSurfaces->resize(oi);
211  if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
212  {
213  obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_);
214  }
215  }
216  }
217  else
218  {
219  // in this case optimizeForCloseObject_ is true:
220  // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m
221  // one for potential floor points further away than 1m.
222  // For the points at closer range, we use a smaller normal estimation radius and ground normal angle,
223  // which allows to detect smaller objects, without increasing the number of false positive.
224  // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the
225  // grond normal angle (* 2.).
226 
227  pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
228  pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
229 
230  // Part 1: segment floor and obstacles near the robot
231  rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
232  originalCloud_near,
233  ground,
234  obstacles,
241 
242  if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
243  {
244  pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud);
245  ground->clear();
246  }
247 
248  if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
249  {
250  pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
251  obstacles->clear();
252  }
253 
254  // Part 2: segment floor and obstacles far from the robot
255  rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
256  originalCloud_far,
257  ground,
258  obstacles,
261  3.*clusterRadius_,
265 
266  if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
267  {
268  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
269  pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2);
270  *groundCloud += *groundCloud2;
271  }
272 
273 
274  if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
275  {
276  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
277  pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
278  *obstaclesCloud += *obstacles2;
279  }
280  }
281 
282  if(!localTransform.isIdentity())
283  {
284  //transform back in topic frame
285  rtabmap::Transform localTransformInv = localTransform.inverse();
286  if(groundCloud->size())
287  {
288  groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv);
289  }
290  if(obstaclesCloud->size())
291  {
292  obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv);
293  }
294  }
295  }
296  }
297 
299  {
300  sensor_msgs::PointCloud2 rosCloud;
301  pcl::toROSMsg(*groundCloud, rosCloud);
302  rosCloud.header = cloudMsg->header;
303 
304  //publish the message
305  groundPub_.publish(rosCloud);
306  }
307 
309  {
310  sensor_msgs::PointCloud2 rosCloud;
311  pcl::toROSMsg(*obstaclesCloud, rosCloud);
312  rosCloud.header = cloudMsg->header;
313 
314  //publish the message
315  obstaclesPub_.publish(rosCloud);
316  }
317 
319  {
320  sensor_msgs::PointCloud2 rosCloud;
321  pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud);
322  rosCloud.header.stamp = cloudMsg->header.stamp;
323  rosCloud.header.frame_id = frameId_;
324 
325  //publish the message
326  projObstaclesPub_.publish(rosCloud);
327  }
328 
329  NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
330  }
331 
332 private:
333  std::string frameId_;
344 
346 
350 
352 };
353 
355 }
356 
357 
#define NODELET_ERROR(...)
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void callback(const sensor_msgs::PointCloud2ConstPtr &cloudMsg)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
pcl::IndicesPtr RTABMAP_EXP passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
bool isIdentity() const
ros::NodeHandle & getPrivateNodeHandle() const
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
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
#define false
uint32_t getNumSubscribers() const
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
static WallTime now()
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
Transform inverse() const
bool hasParam(const std::string &key) const
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
#define NODELET_DEBUG(...)
rtabmap::Transform transformFromTF(const tf::Transform &transform)


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:04