mrpt_local_obstacles_node.cpp
Go to the documentation of this file.
1 /***********************************************************************************
2  * Revised BSD License *
3  * Copyright (c) 2014-2023, 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 <mp2p_icp_filters/FilterDecimateVoxels.h>
34 #include <mrpt/config/CConfigFile.h>
35 #include <mrpt/gui/CDisplayWindow3D.h>
36 #include <mrpt/maps/COccupancyGridMap2D.h>
37 #include <mrpt/maps/CSimplePointsMap.h>
38 #include <mrpt/obs/CObservation2DRangeScan.h>
39 #include <mrpt/obs/CObservationPointCloud.h>
40 #include <mrpt/obs/CSensoryFrame.h>
41 #include <mrpt/opengl/CGridPlaneXY.h>
42 #include <mrpt/opengl/COpenGLScene.h>
43 #include <mrpt/opengl/CPointCloud.h>
44 #include <mrpt/opengl/stock_objects.h>
45 #include <mrpt/ros1bridge/laser_scan.h>
46 #include <mrpt/ros1bridge/point_cloud2.h>
47 #include <mrpt/ros1bridge/pose.h>
48 #include <mrpt/system/CTimeLogger.h>
49 #include <mrpt/system/string_utils.h>
50 #include <nav_msgs/Odometry.h>
51 #include <ros/ros.h>
52 #include <sensor_msgs/LaserScan.h>
53 #include <sensor_msgs/PointCloud2.h>
54 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
55 #include <tf2_ros/transform_listener.h>
56 
57 #include <map>
58 
59 using namespace mrpt::system;
60 using namespace mrpt::config;
61 using namespace mrpt::img;
62 using namespace mrpt::maps;
63 using namespace mrpt::obs;
64 
65 // The ROS node
66 class LocalObstaclesNode
67 {
68  private:
69  struct TAuxInitializer
70  {
71  TAuxInitializer(int argc, char** argv)
72  {
73  ros::init(argc, argv, "mrpt_local_obstacles_node");
74  }
75  };
76 
77  CTimeLogger m_profiler;
78 
79  TAuxInitializer m_auxinit;
80  ros::NodeHandle m_nh;
81  ros::NodeHandle m_localn;
82 
83  bool m_show_gui = true;
84 
85  std::string m_frameid_reference = "odom";
86  std::string m_frameid_robot = "base_link";
87 
88  std::string m_topic_local_map_pointcloud = "local_map_pointcloud";
89 
90  std::string m_source_topics_2dscan = "scan,laser1";
91  std::string m_source_topics_pointclouds = "";
92 
93  double m_time_window = 0.20;
94  double m_publish_period = 0.05;
95 
96  ros::Timer m_timer_publish;
97 
98  // Sensor data:
99  struct TInfoPerTimeStep
100  {
101  CObservation::Ptr observation;
102  mrpt::poses::CPose3D robot_pose;
103  };
104  typedef std::multimap<double, TInfoPerTimeStep> TListObservations;
105  TListObservations m_hist_obs;
106  boost::mutex m_hist_obs_mtx;
108 
110  CSimplePointsMap::Ptr m_localmap_pts = CSimplePointsMap::Create();
111  // COccupancyGridMap2D m_localmap_grid;
112 
115  mp2p_icp_filters::FilterPipeline m_filter_pipeline;
116 
117  std::string m_filter_output_layer_name;
118 
119  mrpt::gui::CDisplayWindow3D::Ptr m_gui_win;
120 
123  ros::Publisher m_pub_local_map_pointcloud;
124 
126  std::vector<ros::Subscriber> m_subs_2dlaser;
127 
129  std::vector<ros::Subscriber> m_subs_pointclouds;
130 
131  tf2_ros::Buffer m_tf_buffer;
132  tf2_ros::TransformListener m_tf_listener{m_tf_buffer};
133 
142  template <typename CALLBACK_METHOD_TYPE>
143  size_t subscribeToMultipleTopics(
144  const std::string& lstTopics, std::vector<ros::Subscriber>& subs,
145  CALLBACK_METHOD_TYPE cb)
146  {
147  std::vector<std::string> lstSources;
148  mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources);
149  subs.resize(lstSources.size());
150  for (size_t i = 0; i < lstSources.size(); i++)
151  subs[i] = m_nh.subscribe(lstSources[i], 1, cb, this);
152  return lstSources.size();
153  }
154 
157  void onNewSensor_Laser2D(const sensor_msgs::LaserScanConstPtr& scan)
158  {
159  CTimeLoggerEntry tle(m_profiler, "onNewSensor_Laser2D");
160 
161  ros::Duration timeout(1.0);
162 
163  // Get the relative position of the sensor wrt the robot:
164  geometry_msgs::TransformStamped sensorOnRobot;
165  try
166  {
167  CTimeLoggerEntry tle2(
168  m_profiler, "onNewSensor_Laser2D.lookupTransform_sensor");
169 
170  sensorOnRobot = m_tf_buffer.lookupTransform(
171  m_frameid_robot, scan->header.frame_id, scan->header.stamp,
172  timeout);
173  }
174  catch (const tf2::TransformException& ex)
175  {
176  ROS_ERROR("%s", ex.what());
177  return;
178  }
179 
180  // Convert data to MRPT format:
181  const mrpt::poses::CPose3D sensorOnRobot_mrpt = [&]() {
182  tf2::Transform tx;
183  tf2::fromMsg(sensorOnRobot.transform, tx);
184  return mrpt::ros1bridge::fromROS(tx);
185  }();
186 
187  // In MRPT, CObservation2DRangeScan holds both: sensor data +
188  // relative pose:
189  auto obsScan = CObservation2DRangeScan::Create();
190  mrpt::ros1bridge::fromROS(*scan, sensorOnRobot_mrpt, *obsScan);
191 
192  ROS_DEBUG(
193  "[onNewSensor_Laser2D] %u rays, sensor pose on robot %s",
194  static_cast<unsigned int>(obsScan->getScanSize()),
195  sensorOnRobot_mrpt.asString().c_str());
196 
197  // Get sensor timestamp:
198  const double timestamp = scan->header.stamp.toSec();
199 
200  // Get robot pose at that time in the reference frame, typ: /odom ->
201  // /base_link
202  mrpt::poses::CPose3D robotPose;
203  try
204  {
205  CTimeLoggerEntry tle3(
206  m_profiler, "onNewSensor_Laser2D.lookupTransform_robot");
207 
208  geometry_msgs::TransformStamped robotTfStamp;
209  try
210  {
211  robotTfStamp = m_tf_buffer.lookupTransform(
212  m_frameid_reference, m_frameid_robot, scan->header.stamp,
213  timeout);
214  }
215  catch (const tf2::ExtrapolationException& ex)
216  {
217  ROS_ERROR("%s", ex.what());
218  return;
219  }
220 
221  robotPose = [&]() {
222  tf2::Transform tx;
223  tf2::fromMsg(robotTfStamp.transform, tx);
224  return mrpt::ros1bridge::fromROS(tx);
225  }();
226 
227  ROS_DEBUG(
228  "[onNewSensor_Laser2D] robot pose %s",
229  robotPose.asString().c_str());
230  }
231  catch (const tf2::TransformException& ex)
232  {
233  ROS_ERROR("%s", ex.what());
234  return;
235  }
236 
237  // Insert into the observation history:
238  TInfoPerTimeStep ipt;
239  ipt.observation = obsScan;
240  ipt.robot_pose = robotPose;
241 
242  m_hist_obs_mtx.lock();
243  m_hist_obs.insert(
244  m_hist_obs.end(), TListObservations::value_type(timestamp, ipt));
245  m_hist_obs_mtx.unlock();
246 
247  } // end onNewSensor_Laser2D
248 
251  void onNewSensor_PointCloud(const sensor_msgs::PointCloud2::ConstPtr& pts)
252  {
253  CTimeLoggerEntry tle(m_profiler, "onNewSensor_PointCloud");
254 
255  ros::Duration timeout(1.0);
256  // Get the relative position of the sensor wrt the robot:
257  geometry_msgs::TransformStamped sensorOnRobot;
258  try
259  {
260  CTimeLoggerEntry tle2(
261  m_profiler, "onNewSensor_PointCloud.lookupTransform_sensor");
262 
263  sensorOnRobot = m_tf_buffer.lookupTransform(
264  m_frameid_robot, pts->header.frame_id, pts->header.stamp,
265  timeout);
266  }
267  catch (const tf2::TransformException& ex)
268  {
269  ROS_ERROR("%s", ex.what());
270  return;
271  }
272 
273  // Convert data to MRPT format:
274  const mrpt::poses::CPose3D sensorOnRobot_mrpt = [&]() {
275  tf2::Transform tx;
276  tf2::fromMsg(sensorOnRobot.transform, tx);
277  return mrpt::ros1bridge::fromROS(tx);
278  }();
279 
280  // In MRPT, CObservation2DRangeScan holds both: sensor data +
281  // relative pose:
282  auto obsPts = CObservationPointCloud::Create();
283  const auto ptsMap = mrpt::maps::CSimplePointsMap::Create();
284  obsPts->pointcloud = ptsMap;
285  obsPts->sensorPose = sensorOnRobot_mrpt;
286  mrpt::ros1bridge::fromROS(*pts, *ptsMap);
287 
288  ROS_DEBUG(
289  "[onNewSensor_PointCloud] %u points, sensor pose on robot %s",
290  static_cast<unsigned int>(ptsMap->size()),
291  sensorOnRobot_mrpt.asString().c_str());
292 
293  // Get sensor timestamp:
294  const double timestamp = pts->header.stamp.toSec();
295 
296  // Get robot pose at that time in the reference frame, typ: /odom ->
297  // /base_link
298  mrpt::poses::CPose3D robotPose;
299  try
300  {
301  CTimeLoggerEntry tle3(
302  m_profiler, "onNewSensor_PointCloud.lookupTransform_robot");
303 
304  geometry_msgs::TransformStamped robotTfStamp;
305  try
306  {
307  robotTfStamp = m_tf_buffer.lookupTransform(
308  m_frameid_reference, m_frameid_robot, pts->header.stamp,
309  timeout);
310  }
311  catch (const tf2::ExtrapolationException& ex)
312  {
313  ROS_ERROR("%s", ex.what());
314  return;
315  }
316 
317  robotPose = [&]() {
318  tf2::Transform tx;
319  tf2::fromMsg(robotTfStamp.transform, tx);
320  return mrpt::ros1bridge::fromROS(tx);
321  }();
322 
323  ROS_DEBUG(
324  "[onNewSensor_PointCloud] robot pose %s",
325  robotPose.asString().c_str());
326  }
327  catch (const tf2::TransformException& ex)
328  {
329  ROS_ERROR("%s", ex.what());
330  return;
331  }
332 
333  // Insert into the observation history:
334  TInfoPerTimeStep ipt;
335  ipt.observation = obsPts;
336  ipt.robot_pose = robotPose;
337 
338  m_hist_obs_mtx.lock();
339  m_hist_obs.insert(
340  m_hist_obs.end(), TListObservations::value_type(timestamp, ipt));
341  m_hist_obs_mtx.unlock();
342 
343  } // end onNewSensor_Laser2D
344 
346  void onDoPublish(const ros::TimerEvent&)
347  {
348  CTimeLoggerEntry tle(m_profiler, "onDoPublish");
349 
350  // Purge old observations & latch a local copy:
351  TListObservations obs;
352  {
353  CTimeLoggerEntry tle(m_profiler, "onDoPublish.removingOld");
354  m_hist_obs_mtx.lock();
355 
356  // Purge old obs:
357  if (!m_hist_obs.empty())
358  {
359  const double last_time = m_hist_obs.rbegin()->first;
360  TListObservations::iterator it_first_valid =
361  m_hist_obs.lower_bound(last_time - m_time_window);
362  const size_t nToRemove =
363  std::distance(m_hist_obs.begin(), it_first_valid);
364  ROS_DEBUG(
365  "[onDoPublish] Removing %u old entries, last_time=%lf",
366  static_cast<unsigned int>(nToRemove), last_time);
367  m_hist_obs.erase(m_hist_obs.begin(), it_first_valid);
368  }
369  // Local copy in this thread:
370  obs = m_hist_obs;
371  m_hist_obs_mtx.unlock();
372  }
373 
374  ROS_DEBUG(
375  "Building local map with %u observations.",
376  static_cast<unsigned int>(obs.size()));
377  if (obs.empty()) return;
378 
379  // Build local map(s):
380  // -----------------------------------------------
381  m_localmap_pts->clear();
382  mrpt::poses::CPose3D curRobotPose;
383  {
384  CTimeLoggerEntry tle2(m_profiler, "onDoPublish.buildLocalMap");
385 
386  // Get the latest robot pose in the reference frame (typ: /odom ->
387  // /base_link)
388  // so we can build the local map RELATIVE to it:
389  ros::Duration timeout(1.0);
390 
391  try
392  {
393  geometry_msgs::TransformStamped tx;
394  tx = m_tf_buffer.lookupTransform(
395  m_frameid_reference, m_frameid_robot, ros::Time(0),
396  timeout);
397 
398  tf2::Transform tfx;
399  tf2::fromMsg(tx.transform, tfx);
400  curRobotPose = mrpt::ros1bridge::fromROS(tfx);
401  }
402  catch (const tf2::ExtrapolationException& ex)
403  {
404  ROS_ERROR("%s", ex.what());
405  return;
406  }
407 
408  ROS_DEBUG(
409  "[onDoPublish] Building local map relative to latest robot "
410  "pose: %s",
411  curRobotPose.asString().c_str());
412 
413  // For each observation: compute relative robot pose & insert obs
414  // into map:
415  for (TListObservations::const_iterator it = obs.begin();
416  it != obs.end(); ++it)
417  {
418  const TInfoPerTimeStep& ipt = it->second;
419 
420  // Relative pose in the past:
421  mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE);
422  relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose);
423 
424  // Insert obs:
425  m_localmap_pts->insertObservationPtr(ipt.observation, relPose);
426 
427  } // end for
428  }
429 
430  // Filtering:
431  mrpt::maps::CPointsMap::Ptr filteredPts;
432 
433  if (!m_filter_pipeline.empty())
434  {
435  mp2p_icp::metric_map_t mm;
436  mm.layers[mp2p_icp::metric_map_t::PT_LAYER_RAW] = m_localmap_pts;
437  mp2p_icp_filters::apply_filter_pipeline(m_filter_pipeline, mm);
438 
439  filteredPts = mm.point_layer(m_filter_output_layer_name);
440  }
441  else
442  {
443  filteredPts = m_localmap_pts;
444  }
445 
446  // Publish them:
447  if (m_pub_local_map_pointcloud.getNumSubscribers() > 0)
448  {
449  sensor_msgs::PointCloud2 msg_pts;
450  msg_pts.header.frame_id = m_frameid_robot;
451  msg_pts.header.stamp = ros::Time(obs.rbegin()->first);
452 
453  auto simplPts =
454  std::dynamic_pointer_cast<mrpt::maps::CSimplePointsMap>(
455  filteredPts);
456  ASSERT_(simplPts);
457 
458  mrpt::ros1bridge::toROS(*simplPts, msg_pts.header, msg_pts);
459  m_pub_local_map_pointcloud.publish(msg_pts);
460  }
461 
462  // Show gui:
463  if (m_show_gui)
464  {
465  if (!m_gui_win)
466  {
467  m_gui_win = mrpt::gui::CDisplayWindow3D::Create(
468  "LocalObstaclesNode", 800, 600);
469  mrpt::opengl::COpenGLScene::Ptr& scene =
470  m_gui_win->get3DSceneAndLock();
471  scene->insert(mrpt::opengl::CGridPlaneXY::Create());
472  scene->insert(
473  mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 4.0));
474 
475  auto gl_obs = mrpt::opengl::CSetOfObjects::Create();
476  gl_obs->setName("obstacles");
477  scene->insert(gl_obs);
478 
479  auto gl_rawpts = mrpt::opengl::CPointCloud::Create();
480  gl_rawpts->setName("raw_points");
481  gl_rawpts->setPointSize(1.0);
482  gl_rawpts->setColor_u8(TColor(0x00ff00));
483  scene->insert(gl_rawpts);
484 
485  auto gl_pts = mrpt::opengl::CPointCloud::Create();
486  gl_pts->setName("final_points");
487  gl_pts->setPointSize(3.0);
488  gl_pts->setColor_u8(TColor(0x0000ff));
489  scene->insert(gl_pts);
490 
491  m_gui_win->unlockAccess3DScene();
492  }
493 
494  auto& scene = m_gui_win->get3DSceneAndLock();
495  auto gl_obs = mrpt::ptr_cast<mrpt::opengl::CSetOfObjects>::from(
496  scene->getByName("obstacles"));
497  ROS_ASSERT(!!gl_obs);
498  gl_obs->clear();
499 
500  auto glRawPts = mrpt::ptr_cast<mrpt::opengl::CPointCloud>::from(
501  scene->getByName("raw_points"));
502 
503  auto glFinalPts = mrpt::ptr_cast<mrpt::opengl::CPointCloud>::from(
504  scene->getByName("final_points"));
505 
506  for (const auto& o : obs)
507  {
508  const TInfoPerTimeStep& ipt = o.second;
509  // Relative pose in the past:
510  mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE);
511  relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose);
512 
513  mrpt::opengl::CSetOfObjects::Ptr gl_axis =
514  mrpt::opengl::stock_objects::CornerXYZSimple(0.9, 2.0);
515  gl_axis->setPose(relPose);
516  gl_obs->insert(gl_axis);
517  } // end for
518 
519  glRawPts->loadFromPointsMap(m_localmap_pts.get());
520  glFinalPts->loadFromPointsMap(filteredPts.get());
521 
522  m_gui_win->unlockAccess3DScene();
523  m_gui_win->repaint();
524  }
525 
526  } // onDoPublish
527 
528  public:
530  LocalObstaclesNode(int argc, char** argv)
531  : m_auxinit(argc, argv), m_nh(), m_localn("~")
532  {
533  // Load params:
534  m_localn.param("show_gui", m_show_gui, m_show_gui);
535 
536  m_localn.param(
537  "frameid_reference", m_frameid_reference, m_frameid_reference);
538  m_localn.param("frameid_robot", m_frameid_robot, m_frameid_robot);
539 
540  m_localn.param(
541  "topic_local_map_pointcloud", m_topic_local_map_pointcloud,
542  m_topic_local_map_pointcloud);
543 
544  m_localn.param(
545  "source_topics_2dscan", m_source_topics_2dscan,
546  m_source_topics_2dscan);
547  m_localn.param(
548  "source_topics_pointclouds", m_source_topics_pointclouds,
549  m_source_topics_pointclouds);
550 
551  m_localn.param("time_window", m_time_window, m_time_window);
552  m_localn.param("publish_period", m_publish_period, m_publish_period);
553 
554  ROS_ASSERT(m_time_window > m_publish_period);
555  ROS_ASSERT(m_publish_period > 0);
556 
557  // Optional filter pipeline:
558  if (const auto fil =
559  m_localn.param<std::string>("filter_yaml_file", {});
560  !fil.empty())
561  {
562  m_filter_pipeline =
563  mp2p_icp_filters::filter_pipeline_from_yaml_file(fil);
564 
565  m_filter_output_layer_name =
566  m_localn.param<std::string>("filter_output_layer_name", {});
567  ASSERTMSG_(
568  !m_filter_output_layer_name.empty(),
569  "'filter_yaml_file' param also requires "
570  "'filter_output_layer_name'");
571  }
572 
573  // Init ROS publishers:
574  m_pub_local_map_pointcloud = m_nh.advertise<sensor_msgs::PointCloud2>(
575  m_topic_local_map_pointcloud, 10);
576 
577  // Init ROS subs:
578  // Subscribe to one or more laser sources:
579  size_t nSubsTotal = 0;
580  nSubsTotal += this->subscribeToMultipleTopics(
581  m_source_topics_2dscan, m_subs_2dlaser,
582  &LocalObstaclesNode::onNewSensor_Laser2D);
583 
584  nSubsTotal += this->subscribeToMultipleTopics(
585  m_source_topics_pointclouds, m_subs_pointclouds,
586  &LocalObstaclesNode::onNewSensor_PointCloud);
587 
588  ROS_INFO(
589  "Total number of sensor subscriptions: %u\n",
590  static_cast<unsigned int>(nSubsTotal));
591  ROS_ASSERT_MSG(
592  nSubsTotal > 0,
593  "*Error* It is mandatory to set at least one source topic for "
594  "sensory information!");
595 
596  // Local map params:
597  m_localmap_pts->insertionOptions.minDistBetweenLaserPoints = 0;
598  m_localmap_pts->insertionOptions.also_interpolate = false;
599 
600  // Init timers:
601  m_timer_publish = m_nh.createTimer(
602  ros::Duration(m_publish_period), &LocalObstaclesNode::onDoPublish,
603  this);
604 
605  } // end ctor
606 }; // end class
607 
608 int main(int argc, char** argv)
609 {
610  LocalObstaclesNode the_node(argc, argv);
611  ros::spin();
612  return 0;
613 }
LocalObstaclesNode::TInfoPerTimeStep
Definition: mrpt_local_obstacles_node.cpp:99
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:143
ros::Publisher
LocalObstaclesNode::TListObservations
std::multimap< double, TInfoPerTimeStep > TListObservations
Definition: mrpt_local_obstacles_node.cpp:104
mp2p_icp_filters::apply_filter_pipeline
void apply_filter_pipeline(const FilterPipeline &filters, mp2p_icp::metric_map_t &inOut)
Definition: FilterBase.cpp:24
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
LocalObstaclesNode::TInfoPerTimeStep::observation
CObservation::Ptr observation
Definition: mrpt_local_obstacles_node.cpp:101
tf2::fromMsg
void fromMsg(const A &, B &b)
LocalObstaclesNode::m_localn
ros::NodeHandle m_localn
"~"
Definition: mrpt_local_obstacles_node.cpp:81
ros.h
main
int main(int argc, char **argv)
Definition: mrpt_local_obstacles_node.cpp:608
LocalObstaclesNode::TInfoPerTimeStep::robot_pose
mrpt::poses::CPose3D robot_pose
Definition: mrpt_local_obstacles_node.cpp:102
LocalObstaclesNode::m_profiler
CTimeLogger m_profiler
Definition: mrpt_local_obstacles_node.cpp:77
mp2p_icp_filters::filter_pipeline_from_yaml_file
FilterPipeline filter_pipeline_from_yaml_file(const std::string &filename, const mrpt::system::VerbosityLevel &vLevel=mrpt::system::LVL_INFO)
Definition: FilterBase.cpp:66
LocalObstaclesNode::m_auxinit
TAuxInitializer m_auxinit
Just to make sure ROS is init first.
Definition: mrpt_local_obstacles_node.cpp:79
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
LocalObstaclesNode::m_filter_pipeline
mp2p_icp_filters::FilterPipeline m_filter_pipeline
Definition: mrpt_local_obstacles_node.cpp:115
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
LocalObstaclesNode::TAuxInitializer
Definition: mrpt_local_obstacles_node.cpp:69
LocalObstaclesNode::m_pub_local_map_pointcloud
ros::Publisher m_pub_local_map_pointcloud
Subscriber to 2D laser scans.
Definition: mrpt_local_obstacles_node.cpp:123
mp2p_icp::metric_map_t::PT_LAYER_RAW
constexpr static const char * PT_LAYER_RAW
Definition: metricmap.h:56
testing::internal::string
::std::string string
Definition: gtest.h:1979
tf2_ros::TransformListener
LocalObstaclesNode::m_timer_publish
ros::Timer m_timer_publish
Definition: mrpt_local_obstacles_node.cpp:96
LocalObstaclesNode::m_gui_win
mrpt::gui::CDisplayWindow3D::Ptr m_gui_win
Definition: mrpt_local_obstacles_node.cpp:119
ROS_DEBUG
#define ROS_DEBUG(...)
LocalObstaclesNode::LocalObstaclesNode
LocalObstaclesNode(int argc, char **argv)
Definition: mrpt_local_obstacles_node.cpp:530
tf2::ExtrapolationException
LocalObstaclesNode::onDoPublish
void onDoPublish(const ros::TimerEvent &)
Definition: mrpt_local_obstacles_node.cpp:346
LocalObstaclesNode::m_filter_output_layer_name
std::string m_filter_output_layer_name
mp2p_icp output layer name
Definition: mrpt_local_obstacles_node.cpp:117
tf2::Transform
ROS_ASSERT_MSG
#define ROS_ASSERT_MSG(cond,...)
mp2p_icp::metric_map_t::point_layer
mrpt::maps::CPointsMap::Ptr point_layer(const layer_name_t &name) const
Definition: metricmap.cpp:483
tf2_ros::Buffer
mp2p_icp_filters::FilterPipeline
std::vector< FilterBase::Ptr > FilterPipeline
Definition: FilterBase.h:61
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
ros::TimerEvent
LocalObstaclesNode::onNewSensor_Laser2D
void onNewSensor_Laser2D(const sensor_msgs::LaserScanConstPtr &scan)
Definition: mrpt_local_obstacles_node.cpp:157
LocalObstaclesNode::onNewSensor_PointCloud
void onNewSensor_PointCloud(const sensor_msgs::PointCloud2::ConstPtr &pts)
Definition: mrpt_local_obstacles_node.cpp:251
transform_listener.h
LocalObstaclesNode::m_hist_obs
TListObservations m_hist_obs
Definition: mrpt_local_obstacles_node.cpp:105
ros::Time
ROS_ERROR
#define ROS_ERROR(...)
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
mp2p_icp::metric_map_t
Generic container of pointcloud(s), extracted features and other maps.
Definition: metricmap.h:47
tf2_geometry_msgs.h
LocalObstaclesNode::m_subs_pointclouds
std::vector< ros::Subscriber > m_subs_pointclouds
Definition: mrpt_local_obstacles_node.cpp:129
ros::spin
ROSCPP_DECL void spin()
LocalObstaclesNode::m_subs_2dlaser
std::vector< ros::Subscriber > m_subs_2dlaser
Subscriber to point cloud sensors.
Definition: mrpt_local_obstacles_node.cpp:126
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
tf2::TransformException
ROS_INFO
#define ROS_INFO(...)
LocalObstaclesNode::m_tf_buffer
tf2_ros::Buffer m_tf_buffer
Definition: mrpt_local_obstacles_node.cpp:131
ROS_ASSERT
#define ROS_ASSERT(cond)
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
mp2p_icp::metric_map_t::layers
std::map< layer_name_t, mrpt::maps::CMetricMap::Ptr > layers
Definition: metricmap.h:74
ros::Duration
ros::Timer
FilterDecimateVoxels.h
Builds a new layer with a decimated version of an input layer.
LocalObstaclesNode
Definition: mrpt_local_obstacles_node.cpp:66
LocalObstaclesNode::TAuxInitializer::TAuxInitializer
TAuxInitializer(int argc, char **argv)
Definition: mrpt_local_obstacles_node.cpp:71
tf2_ros::Buffer::lookupTransform
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout) const
LocalObstaclesNode::m_nh
ros::NodeHandle m_nh
The node handle.
Definition: mrpt_local_obstacles_node.cpp:80
ros::NodeHandle

mrpt_local_obstacles
Author(s): Jose-Luis Blanco-Claraco
autogenerated on Wed Mar 20 2024 02:10:23