mrpt_local_obstacles_node.cpp
Go to the documentation of this file.
1 /***********************************************************************************
2  * Revised BSD License *
3  * Copyright (c) 2014-2015, Jose-Luis Blanco <jlblanco@ual.es> *
4  * All rights reserved. *
5  * *
6  * Redistribution and use in source and binary forms, with or without *
7  * modification, are permitted provided that the following conditions are met: *
8  * * Redistributions of source code must retain the above copyright *
9  * notice, this list of conditions and the following disclaimer. *
10  * * Redistributions in binary form must reproduce the above copyright *
11  * notice, this list of conditions and the following disclaimer in the *
12  * documentation and/or other materials provided with the distribution. *
13  * * Neither the name of the Vienna University of Technology nor the *
14  * names of its contributors may be used to endorse or promote products *
15  * derived from this software without specific prior written permission. *
16  * *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  *AND *
19  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20  **
21  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
22  * DISCLAIMED. IN NO EVENT SHALL Markus Bader BE LIABLE FOR ANY *
23  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
24  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25  **
26  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
27  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
28  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29  **
30  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
31  ***********************************************************************************/
32 
33 #include <ros/ros.h>
34 
35 #include <sensor_msgs/LaserScan.h>
36 #include <sensor_msgs/PointCloud.h>
37 #include <nav_msgs/Odometry.h>
38 
39 #include <tf/transform_listener.h>
40 
41 #include <mrpt_bridge/pose.h>
42 #include <mrpt_bridge/laser_scan.h>
43 #include <mrpt_bridge/point_cloud.h>
44 
45 #include <map>
46 
47 #include <mrpt/version.h>
48 #include <mrpt/obs/CSensoryFrame.h>
49 #include <mrpt/maps/CSimplePointsMap.h>
50 #include <mrpt/maps/COccupancyGridMap2D.h>
51 #include <mrpt/obs/CObservation2DRangeScan.h>
52 using namespace mrpt::maps;
53 using namespace mrpt::obs;
54 
55 #include <mrpt/system/string_utils.h>
56 #include <mrpt/gui/CDisplayWindow3D.h>
57 #include <mrpt/opengl/CGridPlaneXY.h>
58 #include <mrpt/opengl/CPointCloud.h>
59 #include <mrpt/opengl/stock_objects.h>
60 
61 #include <mrpt/version.h>
62 #if MRPT_VERSION>=0x199
63 #include <mrpt/system/CTimeLogger.h>
64 #include <mrpt/config/CConfigFile.h>
65 using namespace mrpt::system;
66 using namespace mrpt::config;
67 using namespace mrpt::img;
68 #else
69 #include <mrpt/utils/CTimeLogger.h>
70 #include <mrpt/utils/CConfigFile.h>
71 using namespace mrpt::utils;
72 #endif
73 
74 
75 // The ROS node
76 class LocalObstaclesNode
77 {
78  private:
79  struct TAuxInitializer
80  {
81  TAuxInitializer(int argc, char** argv)
82  {
83  ros::init(argc, argv, "mrpt_local_obstacles_node");
84  }
85  };
86 
87  CTimeLogger m_profiler;
88 
89  TAuxInitializer m_auxinit;
90  ros::NodeHandle m_nh;
91  ros::NodeHandle m_localn;
92  bool m_show_gui;
93  std::string m_frameid_reference;
94  std::string m_frameid_robot;
95  std::string
96  m_topic_local_map_pointcloud;
97  std::string m_source_topics_2dscan;
98  double m_time_window;
99  double m_publish_period;
101 
103  ros::Timer m_timer_publish;
104 
105  // Sensor data:
106  struct TInfoPerTimeStep
107  {
108  CObservation::Ptr observation;
109  mrpt::poses::CPose3D robot_pose;
110  };
111  typedef std::multimap<double, TInfoPerTimeStep> TListObservations;
112  TListObservations m_hist_obs;
113  boost::mutex m_hist_obs_mtx;
115 
117  CSimplePointsMap m_localmap_pts;
118  // COccupancyGridMap2D m_localmap_grid;
119 
120  mrpt::gui::CDisplayWindow3D::Ptr m_gui_win;
121 
124  ros::Publisher m_pub_local_map_pointcloud;
125  std::vector<ros::Subscriber>
126  m_subs_2dlaser;
127  tf::TransformListener m_tf_listener;
128 
136  template <typename CALLBACK_METHOD_TYPE>
137  size_t subscribeToMultipleTopics(
138  const std::string& lstTopics, std::vector<ros::Subscriber>& subs,
139  CALLBACK_METHOD_TYPE cb)
140  {
141  std::vector<std::string> lstSources;
142  mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources);
143  subs.resize(lstSources.size());
144  for (size_t i = 0; i < lstSources.size(); i++)
145  subs[i] = m_nh.subscribe(lstSources[i], 1, cb, this);
146  return lstSources.size();
147  }
148 
151  void onNewSensor_Laser2D(const sensor_msgs::LaserScanConstPtr& scan)
152  {
153  CTimeLoggerEntry tle(m_profiler, "onNewSensor_Laser2D");
154 
155  // Get the relative position of the sensor wrt the robot:
156  tf::StampedTransform sensorOnRobot;
157  try
158  {
159  CTimeLoggerEntry tle2(
160  m_profiler, "onNewSensor_Laser2D.lookupTransform_sensor");
161  m_tf_listener.lookupTransform(
162  m_frameid_robot, scan->header.frame_id, ros::Time(0),
163  sensorOnRobot);
164  }
165  catch (tf::TransformException& ex)
166  {
167  ROS_ERROR("%s", ex.what());
168  return;
169  }
170 
171  // Convert data to MRPT format:
172  mrpt::poses::CPose3D sensorOnRobot_mrpt;
173  mrpt_bridge::convert(sensorOnRobot, sensorOnRobot_mrpt);
174  // In MRPT, CObservation2DRangeScan holds both: sensor data + relative
175  // pose:
176  auto obsScan = CObservation2DRangeScan::Create();
177  mrpt_bridge::convert(*scan, sensorOnRobot_mrpt, *obsScan);
178 
179  ROS_DEBUG(
180  "[onNewSensor_Laser2D] %u rays, sensor pose on robot %s",
181  static_cast<unsigned int>(obsScan->scan.size()),
182  sensorOnRobot_mrpt.asString().c_str());
183 
184  // Get sensor timestamp:
185  const double timestamp = scan->header.stamp.toSec();
186 
187  // Get robot pose at that time in the reference frame, typ: /odom ->
188  // /base_link
189  mrpt::poses::CPose3D robotPose;
190  try
191  {
192  CTimeLoggerEntry tle3(
193  m_profiler, "onNewSensor_Laser2D.lookupTransform_robot");
194  tf::StampedTransform tx;
195 
196  try
197  {
198  m_tf_listener.lookupTransform(
199  m_frameid_reference, m_frameid_robot, scan->header.stamp,
200  tx);
201  }
202  catch (tf::ExtrapolationException&)
203  {
204  // if we need a "too much " recent robot pose,be happy with the
205  // latest one:
206  m_tf_listener.lookupTransform(
207  m_frameid_reference, m_frameid_robot, ros::Time(0), tx);
208  }
209  mrpt_bridge::convert(tx, robotPose);
210  ROS_DEBUG(
211  "[onNewSensor_Laser2D] robot pose %s",
212  robotPose.asString().c_str());
213  }
214  catch (tf::TransformException& ex)
215  {
216  ROS_ERROR("%s", ex.what());
217  return;
218  }
219 
220  // Insert into the observation history:
221  TInfoPerTimeStep ipt;
222  ipt.observation = obsScan;
223  ipt.robot_pose = robotPose;
224 
225  m_hist_obs_mtx.lock();
226  m_hist_obs.insert(
227  m_hist_obs.end(), TListObservations::value_type(timestamp, ipt));
228  m_hist_obs_mtx.unlock();
229 
230  } // end onNewSensor_Laser2D
231 
233  void onDoPublish(const ros::TimerEvent&)
234  {
235  CTimeLoggerEntry tle(m_profiler, "onDoPublish");
236 
237  // Purge old observations & latch a local copy:
238  TListObservations obs;
239  {
240  CTimeLoggerEntry tle(
241  m_profiler, "onDoPublish.removingOld");
242  m_hist_obs_mtx.lock();
243 
244  // Purge old obs:
245  if (!m_hist_obs.empty())
246  {
247  const double last_time = m_hist_obs.rbegin()->first;
248  TListObservations::iterator it_first_valid =
249  m_hist_obs.lower_bound(last_time - m_time_window);
250  const size_t nToRemove =
251  std::distance(m_hist_obs.begin(), it_first_valid);
252  ROS_DEBUG(
253  "[onDoPublish] Removing %u old entries, last_time=%lf",
254  static_cast<unsigned int>(nToRemove), last_time);
255  m_hist_obs.erase(m_hist_obs.begin(), it_first_valid);
256  }
257  // Local copy in this thread:
258  obs = m_hist_obs;
259  m_hist_obs_mtx.unlock();
260  }
261 
262  ROS_DEBUG(
263  "Building local map with %u observations.",
264  static_cast<unsigned int>(obs.size()));
265  if (obs.empty()) return;
266 
267  // Build local map(s):
268  // -----------------------------------------------
269  m_localmap_pts.clear();
270  mrpt::poses::CPose3D curRobotPose;
271  {
272  CTimeLoggerEntry tle2(
273  m_profiler, "onDoPublish.buildLocalMap");
274 
275  // Get the latest robot pose in the reference frame (typ: /odom ->
276  // /base_link)
277  // so we can build the local map RELATIVE to it:
278  try
279  {
280  tf::StampedTransform tx;
281  m_tf_listener.lookupTransform(
282  m_frameid_reference, m_frameid_robot, ros::Time(0), tx);
283  mrpt_bridge::convert(tx, curRobotPose);
284  ROS_DEBUG(
285  "[onDoPublish] Building local map relative to latest robot "
286  "pose: %s",
287  curRobotPose.asString().c_str());
288  }
289  catch (tf::TransformException& ex)
290  {
291  ROS_ERROR("%s", ex.what());
292  return;
293  }
294 
295  // For each observation: compute relative robot pose & insert obs
296  // into map:
297  for (TListObservations::const_iterator it = obs.begin();
298  it != obs.end(); ++it)
299  {
300  const TInfoPerTimeStep& ipt = it->second;
301 
302  // Relative pose in the past:
303  mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE);
304  relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose);
305 
306  // Insert obs:
307  m_localmap_pts.insertObservationPtr(ipt.observation, &relPose);
308 
309  } // end for
310  }
311 
312  // Publish them:
313  if (m_pub_local_map_pointcloud.getNumSubscribers() > 0)
314  {
315  sensor_msgs::PointCloudPtr msg_pts =
316  sensor_msgs::PointCloudPtr(new sensor_msgs::PointCloud);
317  msg_pts->header.frame_id = m_frameid_robot;
318  msg_pts->header.stamp = ros::Time(obs.rbegin()->first);
319  mrpt_bridge::point_cloud::mrpt2ros(
320  m_localmap_pts, msg_pts->header, *msg_pts);
321  m_pub_local_map_pointcloud.publish(msg_pts);
322  }
323 
324  // Show gui:
325  if (m_show_gui)
326  {
327  if (!m_gui_win)
328  {
329  m_gui_win = mrpt::gui::CDisplayWindow3D::Create(
330  "LocalObstaclesNode", 800, 600);
331  mrpt::opengl::COpenGLScene::Ptr& scene =
332  m_gui_win->get3DSceneAndLock();
333  scene->insert(mrpt::opengl::CGridPlaneXY::Create());
334  scene->insert(
335  mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 4.0));
336 
337  auto gl_obs = mrpt::opengl::CSetOfObjects::Create();
338  gl_obs->setName("obstacles");
339  scene->insert(gl_obs);
340 
341  auto gl_pts = mrpt::opengl::CPointCloud::Create();
342  gl_pts->setName("points");
343  gl_pts->setPointSize(2.0);
344  gl_pts->setColor_u8(TColor(0x0000ff));
345  scene->insert(gl_pts);
346 
347  m_gui_win->unlockAccess3DScene();
348  }
349 
350  auto& scene = m_gui_win->get3DSceneAndLock();
351  auto gl_obs = mrpt::ptr_cast<mrpt::opengl::CSetOfObjects>::from(
352  scene->getByName("obstacles"));
353  ROS_ASSERT(!!gl_obs);
354  gl_obs->clear();
355 
356  auto gl_pts = mrpt::ptr_cast<mrpt::opengl::CPointCloud>::from(
357  scene->getByName("points"));
358 
359  for (const auto &o :obs)
360  {
361  const TInfoPerTimeStep& ipt = o.second;
362  // Relative pose in the past:
363  mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE);
364  relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose);
365 
366  mrpt::opengl::CSetOfObjects::Ptr gl_axis =
367  mrpt::opengl::stock_objects::CornerXYZSimple(0.9, 2.0);
368  gl_axis->setPose(relPose);
369  gl_obs->insert(gl_axis);
370  } // end for
371 
372  gl_pts->loadFromPointsMap(&m_localmap_pts);
373 
374  m_gui_win->unlockAccess3DScene();
375  m_gui_win->repaint();
376  }
377 
378  } // onDoPublish
379 
380  public:
382  LocalObstaclesNode(int argc, char** argv)
383  : m_auxinit(argc, argv),
384  m_nh(),
385  m_localn("~"),
386  m_show_gui(true),
387  m_frameid_reference("odom"),
388  m_frameid_robot("base_link"),
389  m_topic_local_map_pointcloud("local_map_pointcloud"),
390  m_source_topics_2dscan("scan,laser1"),
391  m_time_window(0.2),
392  m_publish_period(0.05)
393  {
394  // Load params:
395  m_localn.param("show_gui", m_show_gui, m_show_gui);
396  m_localn.param(
397  "frameid_reference", m_frameid_reference, m_frameid_reference);
398  m_localn.param("frameid_robot", m_frameid_robot, m_frameid_robot);
399  m_localn.param(
400  "topic_local_map_pointcloud", m_topic_local_map_pointcloud,
401  m_topic_local_map_pointcloud);
402  m_localn.param(
403  "source_topics_2dscan", m_source_topics_2dscan,
404  m_source_topics_2dscan);
405  m_localn.param("time_window", m_time_window, m_time_window);
406  m_localn.param("publish_period", m_publish_period, m_publish_period);
407 
408  ROS_ASSERT(m_time_window > m_publish_period);
409  ROS_ASSERT(m_publish_period > 0);
410 
411  // Init ROS publishers:
412  m_pub_local_map_pointcloud = m_nh.advertise<sensor_msgs::PointCloud>(
413  m_topic_local_map_pointcloud, 10);
414 
415  // Init ROS subs:
416  // Subscribe to one or more laser sources:
417  size_t nSubsTotal = 0;
418  nSubsTotal += this->subscribeToMultipleTopics(
419  m_source_topics_2dscan, m_subs_2dlaser,
420  &LocalObstaclesNode::onNewSensor_Laser2D);
421 
422  ROS_INFO(
423  "Total number of sensor subscriptions: %u\n",
424  static_cast<unsigned int>(nSubsTotal));
425  ROS_ASSERT_MSG(
426  nSubsTotal > 0,
427  "*Error* It is mandatory to set at least one source topic for "
428  "sensory information!");
429 
430  // Local map params:
431  m_localmap_pts.insertionOptions.minDistBetweenLaserPoints = 0;
432  m_localmap_pts.insertionOptions.also_interpolate = false;
433 
434  // Init timers:
435  m_timer_publish = m_nh.createTimer(
436  ros::Duration(m_publish_period), &LocalObstaclesNode::onDoPublish,
437  this);
438 
439  } // end ctor
440 
441 }; // end class
442 
443 int main(int argc, char** argv)
444 {
445  LocalObstaclesNode the_node(argc, argv);
446  ros::spin();
447  return 0;
448 }
LocalObstaclesNode::m_auxinit
TAuxInitializer m_auxinit
Just to make sure ROS is init first.
Definition: mrpt_local_obstacles_node.cpp:89
transform_listener.h
ros::NodeHandle
LocalObstaclesNode::m_topic_local_map_pointcloud
std::string m_topic_local_map_pointcloud
Default: "local_map_pointcloud".
Definition: mrpt_local_obstacles_node.cpp:96
LocalObstaclesNode::m_subs_2dlaser
std::vector< ros::Subscriber > m_subs_2dlaser
Subscriber to 2D laser scans.
Definition: mrpt_local_obstacles_node.cpp:126
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
LocalObstaclesNode::m_frameid_reference
std::string m_frameid_reference
typ: "odom"
Definition: mrpt_local_obstacles_node.cpp:93
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
LocalObstaclesNode::LocalObstaclesNode
LocalObstaclesNode(int argc, char **argv)
Definition: mrpt_local_obstacles_node.cpp:382
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
LocalObstaclesNode::m_frameid_robot
std::string m_frameid_robot
typ: "base_link"
Definition: mrpt_local_obstacles_node.cpp:94
main
int main(int argc, char **argv)
Definition: mrpt_local_obstacles_node.cpp:443
LocalObstaclesNode::m_localn
ros::NodeHandle m_localn
"~"
Definition: mrpt_local_obstacles_node.cpp:91
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
LocalObstaclesNode::m_source_topics_2dscan
std::string m_source_topics_2dscan
Default: "scan,laser1".
Definition: mrpt_local_obstacles_node.cpp:97
LocalObstaclesNode::m_tf_listener
tf::TransformListener m_tf_listener
Use to retrieve TF data.
Definition: mrpt_local_obstacles_node.cpp:127
LocalObstaclesNode::m_gui_win
mrpt::gui::CDisplayWindow3D::Ptr m_gui_win
Definition: mrpt_local_obstacles_node.cpp:120
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROS_ASSERT_MSG
#define ROS_ASSERT_MSG(cond,...)
ros::NodeHandle::createTimer
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
LocalObstaclesNode::onDoPublish
void onDoPublish(const ros::TimerEvent &)
Definition: mrpt_local_obstacles_node.cpp:233
LocalObstaclesNode::subscribeToMultipleTopics
size_t subscribeToMultipleTopics(const std::string &lstTopics, std::vector< ros::Subscriber > &subs, CALLBACK_METHOD_TYPE cb)
Subscribe to a variable number of topics.
Definition: mrpt_local_obstacles_node.cpp:137
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
tf::Transformer::lookupTransform
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
LocalObstaclesNode::m_pub_local_map_pointcloud
ros::Publisher m_pub_local_map_pointcloud
Definition: mrpt_local_obstacles_node.cpp:124
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
LocalObstaclesNode::m_nh
ros::NodeHandle m_nh
The node handle.
Definition: mrpt_local_obstacles_node.cpp:90
ROS_ASSERT
#define ROS_ASSERT(cond)
LocalObstaclesNode::onNewSensor_Laser2D
void onNewSensor_Laser2D(const sensor_msgs::LaserScanConstPtr &scan)
Definition: mrpt_local_obstacles_node.cpp:151
LocalObstaclesNode::m_timer_publish
ros::Timer m_timer_publish
than m_time_window
Definition: mrpt_local_obstacles_node.cpp:103
ROS_ERROR
#define ROS_ERROR(...)
LocalObstaclesNode::TListObservations
std::multimap< double, TInfoPerTimeStep > TListObservations
Definition: mrpt_local_obstacles_node.cpp:111
ROS_DEBUG
#define ROS_DEBUG(...)

mrpt_local_obstacles
Author(s): Jose-Luis Blanco-Claraco
autogenerated on Thu Jun 6 2019 19:44:47