mvsim_node.cpp
Go to the documentation of this file.
1 /*+-------------------------------------------------------------------------+
2  | MultiVehicle simulator (libmvsim) |
3  | |
4  | Copyright (C) 2014-2023 Jose Luis Blanco Claraco |
5  | Copyright (C) 2017 Borys Tymchenko (Odessa Polytechnic University) |
6  | Distributed under 3-clause BSD License |
7  | See COPYING |
8  +-------------------------------------------------------------------------+ */
9 
10 #include <mrpt/core/lock_helper.h>
11 #include <mrpt/obs/CObservation3DRangeScan.h>
12 #include <mrpt/obs/CObservationIMU.h>
13 #include <mrpt/obs/CObservationPointCloud.h>
14 #include <mrpt/system/CTicTac.h>
15 #include <mrpt/system/filesystem.h>
16 #include <mrpt/system/os.h> // kbhit()
17 #include <mvsim/WorldElements/OccupancyGridMap.h>
18 
19 #if PACKAGE_ROS_VERSION == 1
20 // ===========================================
21 // ROS 1
22 // ===========================================
23 #include <geometry_msgs/Polygon.h>
24 #include <geometry_msgs/PoseArray.h>
25 #include <geometry_msgs/PoseWithCovarianceStamped.h>
26 #include <mrpt/ros1bridge/image.h>
27 #include <mrpt/ros1bridge/imu.h>
28 #include <mrpt/ros1bridge/laser_scan.h>
29 #include <mrpt/ros1bridge/map.h>
30 #include <mrpt/ros1bridge/point_cloud2.h>
31 #include <mrpt/ros1bridge/pose.h>
32 #include <nav_msgs/GetMap.h>
33 #include <nav_msgs/MapMetaData.h>
34 #include <nav_msgs/Odometry.h>
35 #include <sensor_msgs/Image.h>
36 #include <sensor_msgs/Imu.h>
37 #include <sensor_msgs/LaserScan.h>
38 #include <sensor_msgs/PointCloud2.h>
39 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
40 // usings:
41 using Msg_OccupancyGrid = nav_msgs::OccupancyGrid;
42 using Msg_MapMetaData = nav_msgs::MapMetaData;
43 using Msg_TransformStamped = geometry_msgs::TransformStamped;
44 #else
45 // ===========================================
46 // ROS 2
47 // ===========================================
48 #include <mrpt/ros2bridge/image.h>
49 #include <mrpt/ros2bridge/imu.h>
50 #include <mrpt/ros2bridge/laser_scan.h>
51 #include <mrpt/ros2bridge/map.h>
52 #include <mrpt/ros2bridge/point_cloud2.h>
53 #include <mrpt/ros2bridge/pose.h>
54 
55 #include <nav_msgs/msg/map_meta_data.hpp>
56 #include <nav_msgs/msg/odometry.hpp>
57 #include <nav_msgs/srv/get_map.hpp>
58 #include <sensor_msgs/msg/image.hpp>
59 #include <sensor_msgs/msg/imu.hpp>
60 #include <sensor_msgs/msg/laser_scan.hpp>
61 #include <sensor_msgs/msg/point_cloud2.hpp>
62 
63 // see: https://github.com/ros2/geometry2/pull/416
64 #if defined(MVSIM_HAS_TF2_GEOMETRY_MSGS_HPP)
65 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
66 #else
67 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
68 #endif
69 
70 // usings:
71 using Msg_OccupancyGrid = nav_msgs::msg::OccupancyGrid;
72 using Msg_MapMetaData = nav_msgs::msg::MapMetaData;
73 using Msg_TransformStamped = geometry_msgs::msg::TransformStamped;
74 #endif
75 
76 #include <iostream>
77 #include <rapidxml_utils.hpp>
78 
79 #include "mvsim/mvsim_node_core.h"
80 
81 #if PACKAGE_ROS_VERSION == 1
82 namespace mrpt2ros = mrpt::ros1bridge;
83 #else
84 namespace mrpt2ros = mrpt::ros2bridge;
85 #endif
86 
87 #if PACKAGE_ROS_VERSION == 1
88 #define ROS12_INFO(...) ROS_INFO(__VA_ARGS__)
89 #define ROS12_ERROR(...) ROS_ERROR(__VA_ARGS__)
90 #else
91 #define ROS12_INFO(...) RCLCPP_INFO(n_->get_logger(), __VA_ARGS__)
92 #define ROS12_ERROR(...) RCLCPP_ERROR(n_->get_logger(), __VA_ARGS__)
93 #endif
94 
95 /*------------------------------------------------------------------------------
96  * MVSimNode()
97  * Constructor.
98  *----------------------------------------------------------------------------*/
99 #if PACKAGE_ROS_VERSION == 1
100 MVSimNode::MVSimNode(ros::NodeHandle& n)
101 #else
102 MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n)
103 #endif
104  : n_(n)
105 {
106 #if PACKAGE_ROS_VERSION == 2
107  clock_ = n_->get_clock();
108  ts_.attachClock(clock_);
109 
110  // ROS2: needs to declare parameters:
111  n_->declare_parameter<std::string>("world_file", "default.world.xml");
112  n_->declare_parameter<double>("simul_rate", 100);
113  n_->declare_parameter<double>("base_watchdog_timeout", 0.2);
114  {
115  double t;
116  base_watchdog_timeout_ = std::chrono::milliseconds(
117  1000 * n_->get_parameter_or("base_watchdog_timeout", t, 0.2));
118  }
119 
120  realtime_factor_ =
121  n_->declare_parameter<double>("realtime_factor", realtime_factor_);
122 
123  gui_refresh_period_ms_ = n_->declare_parameter<double>(
124  "gui_refresh_period", gui_refresh_period_ms_);
125 
126  headless_ = n_->declare_parameter<bool>("headless", headless_);
127 
128  period_ms_publish_tf_ = n_->declare_parameter<double>(
129  "period_ms_publish_tf", period_ms_publish_tf_);
130 
131  do_fake_localization_ = n_->declare_parameter<bool>(
132  "do_fake_localization", do_fake_localization_);
133 
134  publisher_history_len_ = n_->declare_parameter<int>(
135  "publisher_history_len", publisher_history_len_);
136 
137  // n_->declare_parameter("use_sim_time"); // already declared error?
138  if (true == n_->get_parameter_or("use_sim_time", false))
139  {
140  THROW_EXCEPTION(
141  "At present, MVSIM can only work with use_sim_time=false");
142  }
143 #endif
144 
145  // Launch GUI thread:
146  thread_params_.obj = this;
147  thGUI_ =
148  std::thread(&MVSimNode::thread_update_GUI, std::ref(thread_params_));
149 
150  // Init ROS publishers:
151 #if PACKAGE_ROS_VERSION == 1
152  // pub_clock_ = n_.advertise<rosgraph_msgs::Clock>("/clock", 10);
153 
154  pub_map_ros_ = n_.advertise<nav_msgs::OccupancyGrid>(
155  "simul_map", 1 /*queue len*/, true /*latch*/);
156  pub_map_metadata_ = n_.advertise<nav_msgs::MapMetaData>(
157  "simul_map_metadata", 1 /*queue len*/, true /*latch*/);
158 #else
159  rclcpp::QoS qosLatched(rclcpp::KeepLast(10));
160  qosLatched.durability(
161  rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
162 
163  pub_map_ros_ = n_->create_publisher<nav_msgs::msg::OccupancyGrid>(
164  "simul_map", qosLatched);
165  pub_map_metadata_ = n_->create_publisher<nav_msgs::msg::MapMetaData>(
166  "simul_map_metadata", qosLatched);
167 #endif
168 
169 #if PACKAGE_ROS_VERSION == 1
170  // sim_time_.fromSec(0.0);
171  base_last_cmd_.fromSec(0.0);
172 #else
173  // sim_time_ = rclcpp::Time(0);
174  base_last_cmd_ = rclcpp::Time(0);
175 #endif
176 
177  // Node parameters:
178 #if PACKAGE_ROS_VERSION == 1
179  double t;
180  if (!localn_.getParam("base_watchdog_timeout", t)) t = 0.2;
181  base_watchdog_timeout_.fromSec(t);
182  localn_.param("realtime_factor", realtime_factor_, 1.0);
183  localn_.param(
184  "gui_refresh_period", gui_refresh_period_ms_, gui_refresh_period_ms_);
185  localn_.param("headless", headless_, headless_);
186  localn_.param(
187  "period_ms_publish_tf", period_ms_publish_tf_, period_ms_publish_tf_);
188  localn_.param(
189  "do_fake_localization", do_fake_localization_, do_fake_localization_);
190 
191  // JLBC: At present, mvsim does not use sim_time for neither ROS 1 nor
192  // ROS 2.
193  // n_.setParam("use_sim_time", false);
194  if (true == localn_.param("use_sim_time", false))
195  {
196  THROW_EXCEPTION(
197  "At present, MVSIM can only work with use_sim_time=false");
198  }
199 #endif
200 
201  mvsim_world_->registerCallbackOnObservation(
202  [this](
203  const mvsim::Simulable& veh,
204  const mrpt::obs::CObservation::Ptr& obs) {
205  if (!obs) return;
206 
207  mrpt::system::CTimeLoggerEntry tle(
208  profiler_, "lambda_onNewObservation");
209 
210  const mvsim::Simulable* vehPtr = &veh;
211  const mrpt::obs::CObservation::Ptr obsCopy = obs;
212  auto fut = ros_publisher_workers_.enqueue([this, vehPtr,
213  obsCopy]() {
214  try
215  {
216  onNewObservation(*vehPtr, obsCopy);
217  }
218  catch (const std::exception& e)
219  {
220  ROS12_ERROR(
221  "[MVSimNode] Error processing observation with label "
222  "'%s':\n%s",
223  obsCopy ? obsCopy->sensorLabel.c_str() : "(nullptr)",
224  e.what());
225  }
226  });
227  });
228 }
229 
230 void MVSimNode::launch_mvsim_server()
231 {
232  ROS12_INFO("[MVSimNode] launch_mvsim_server()");
233 
234  ASSERT_(!mvsim_server_);
235 
236  // Start network server:
237  mvsim_server_ = std::make_shared<mvsim::Server>();
238 
239 #if 0
240  if (argPort.isSet()) server->listenningPort(argPort.getValue());
241  mvsim_server_->setMinLoggingLevel(
242  mrpt::typemeta::TEnumType<mrpt::system::VerbosityLevel>::name2value(
243  argVerbosity.getValue()));
244 #endif
245 
246  mvsim_server_->start();
247 }
248 
249 void MVSimNode::loadWorldModel(const std::string& world_xml_file)
250 {
251  ROS12_INFO("[MVSimNode] Loading world file: %s", world_xml_file.c_str());
252 
253  ASSERT_FILE_EXISTS_(world_xml_file);
254 
255  // Load from XML:
256  rapidxml::file<> fil_xml(world_xml_file.c_str());
257  mvsim_world_->load_from_XML(fil_xml.data(), world_xml_file);
258 
259  ROS12_INFO("[MVSimNode] World file load done.");
260  world_init_ok_ = true;
261 
262  // Notify the ROS system about the good news:
263  notifyROSWorldIsUpdated();
264 }
265 
266 /*------------------------------------------------------------------------------
267  * ~MVSimNode()
268  * Destructor.
269  *----------------------------------------------------------------------------*/
270 MVSimNode::~MVSimNode() { terminateSimulation(); }
271 
272 void MVSimNode::terminateSimulation()
273 {
274  if (!mvsim_world_) return;
275 
276  mvsim_world_->simulator_must_close(true);
277 
278  thread_params_.closing = true;
279  if (thGUI_.joinable()) thGUI_.join();
280 
281  mvsim_world_->free_opengl_resources();
282 
283  ros_publisher_workers_.clear();
284  // Don't destroy mvsim_server_ yet, since "world" needs to unregister.
285  mvsim_world_.reset();
286  std::cout << "[MVSimNode::terminateSimulation] All done." << std::endl;
287 }
288 
289 #if PACKAGE_ROS_VERSION == 1
290 /*------------------------------------------------------------------------------
291  * configCallback()
292  * Callback function for dynamic reconfigure server.
293  *----------------------------------------------------------------------------*/
294 void MVSimNode::configCallback(
295  mvsim::mvsimNodeConfig& config, [[maybe_unused]] uint32_t level)
296 {
297  // Set class variables to new values. They should match what is input at the
298  // dynamic reconfigure GUI.
299  // message = config.message.c_str();
300  ROS_INFO("MVSimNode::configCallback() called.");
301 
302  if (mvsim_world_->is_GUI_open() && !config.show_gui)
303  mvsim_world_->close_GUI();
304 }
305 #endif
306 
307 // Process pending msgs, run real-time simulation, etc.
308 void MVSimNode::spin()
309 {
310  using namespace mvsim;
311  using namespace std::string_literals;
312  using mrpt::DEG2RAD;
313  using mrpt::RAD2DEG;
314 
315  if (!mvsim_world_) return;
316 
317  // Do simulation itself:
318  // ========================================================================
319  // Handle 1st iter:
320  if (t_old_ < 0) t_old_ = realtime_tictac_.Tac();
321  // Compute how much time has passed to simulate in real-time:
322  double t_new = realtime_tictac_.Tac();
323  double incr_time = realtime_factor_ * (t_new - t_old_);
324 
325  // Just in case the computer is *really fast*...
326  if (incr_time < mvsim_world_->get_simul_timestep()) return;
327 
328  // Simulate:
329  mvsim_world_->run_simulation(incr_time);
330 
331  // t_old_simul = world.get_simul_time();
332  t_old_ = t_new;
333 
334  const auto& vehs = mvsim_world_->getListOfVehicles();
335 
336  // Publish new state to ROS
337  // ========================================================================
338  this->spinNotifyROS();
339 
340  // GUI msgs, teleop, etc.
341  // ========================================================================
342  if (tim_teleop_refresh_.Tac() > period_ms_teleop_refresh_ * 1e-3)
343  {
344  tim_teleop_refresh_.Tic();
345 
346  std::string txt2gui_tmp;
347  World::TGUIKeyEvent keyevent = gui_key_events_;
348 
349  // Global keys:
350  switch (keyevent.keycode)
351  {
352  // case 27: do_exit=true; break;
353  case '1':
354  case '2':
355  case '3':
356  case '4':
357  case '5':
358  case '6':
359  teleop_idx_veh_ = keyevent.keycode - '1';
360  break;
361  };
362 
363  { // Test: Differential drive: Control raw forces
364  txt2gui_tmp += mrpt::format(
365  "Selected vehicle: %u/%u\n",
366  static_cast<unsigned>(teleop_idx_veh_ + 1),
367  static_cast<unsigned>(vehs.size()));
368  if (vehs.size() > teleop_idx_veh_)
369  {
370  // Get iterator to selected vehicle:
371  auto it_veh = vehs.begin();
372  std::advance(it_veh, teleop_idx_veh_);
373 
374  // Get speed: ground truth
375  txt2gui_tmp += "gt. vel: "s +
376  it_veh->second->getVelocityLocal().asString();
377 
378  // Get speed: ground truth
379  txt2gui_tmp +=
380  "\nodo vel: "s +
381  it_veh->second->getVelocityLocalOdoEstimate().asString();
382 
383  // Generic teleoperation interface for any controller that
384  // supports it:
385  {
386  ControllerBaseInterface* controller =
387  it_veh->second->getControllerInterface();
388  ControllerBaseInterface::TeleopInput teleop_in;
389  ControllerBaseInterface::TeleopOutput teleop_out;
390  teleop_in.keycode = keyevent.keycode;
391  controller->teleop_interface(teleop_in, teleop_out);
392  txt2gui_tmp += teleop_out.append_gui_lines;
393  }
394  }
395  }
396 
397  msg2gui_ = txt2gui_tmp; // send txt msgs to show in the GUI
398 
399  // Clear the keystroke buffer
400  if (keyevent.keycode != 0) gui_key_events_ = World::TGUIKeyEvent();
401 
402  } // end refresh teleop stuff
403 }
404 
405 /*------------------------------------------------------------------------------
406  * thread_update_GUI()
407  *----------------------------------------------------------------------------*/
408 void MVSimNode::thread_update_GUI(TThreadParams& thread_params)
409 {
410  try
411  {
412  using namespace mvsim;
413 
414  MVSimNode* obj = thread_params.obj;
415 
416  while (!thread_params.closing)
417  {
418  if (obj->world_init_ok_ && !obj->headless_)
419  {
420  World::TUpdateGUIParams guiparams;
421  guiparams.msg_lines = obj->msg2gui_;
422 
423  obj->mvsim_world_->update_GUI(&guiparams);
424 
425  // Send key-strokes to the main thread:
426  if (guiparams.keyevent.keycode != 0)
427  obj->gui_key_events_ = guiparams.keyevent;
428 
429  std::this_thread::sleep_for(
430  std::chrono::milliseconds(obj->gui_refresh_period_ms_));
431  }
432  else if (obj->world_init_ok_ && obj->headless_)
433  {
434  obj->mvsim_world_->internalGraphicsLoopTasksForSimulation();
435 
436  std::this_thread::sleep_for(
437  std::chrono::microseconds(static_cast<size_t>(
438  obj->mvsim_world_->get_simul_timestep() * 1000000)));
439  }
440  else
441  {
442  std::this_thread::sleep_for(
443  std::chrono::milliseconds(obj->gui_refresh_period_ms_));
444  }
445  }
446  }
447  catch (const std::exception& e)
448  {
449  std::cerr << "[MVSimNode::thread_update_GUI] Exception:\n" << e.what();
450  }
451 }
452 
453 // Visitor: Vehicles
454 // ----------------------------------------
455 void MVSimNode::publishVehicles([[maybe_unused]] mvsim::VehicleBase& veh)
456 {
457  //
458 }
459 
460 // Visitor: World elements
461 // ----------------------------------------
462 void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj)
463 {
464  // GridMaps --------------
465  if (mvsim::OccupancyGridMap* grid =
466  dynamic_cast<mvsim::OccupancyGridMap*>(&obj);
467  grid)
468  {
469  static Msg_OccupancyGrid ros_map;
470  static mvsim::OccupancyGridMap* cachedGrid = nullptr;
471 
472  if (cachedGrid != grid)
473  {
474  cachedGrid = grid;
475  mrpt2ros::toROS(grid->getOccGrid(), ros_map);
476  }
477 
478 #if PACKAGE_ROS_VERSION == 1
479  static size_t loop_count = 0;
480  ros_map.header.stamp = ros::Time::now();
481  ros_map.header.seq = loop_count++;
482 #else
483  ros_map.header.stamp = clock_->now();
484  ros_map.header.frame_id = "map";
485 #endif
486 
487 #if PACKAGE_ROS_VERSION == 1
488  auto& pubMap = pub_map_ros_;
489  auto& pubMapMeta = pub_map_metadata_;
490 #else
491  auto& pubMap = *pub_map_ros_;
492  auto& pubMapMeta = *pub_map_metadata_;
493 #endif
494  pubMap.publish(ros_map);
495  pubMapMeta.publish(ros_map.info);
496 
497  } // end gridmap
498 
499 } // end visit(World Elements)
500 
501 // ROS: Publish grid map for visualization purposes:
502 void MVSimNode::notifyROSWorldIsUpdated()
503 {
504 #if PACKAGE_ROS_VERSION == 2
505  // In ROS1 latching works so we only need to do this once, here.
506  // In ROS2,latching doesn't work, we must re-publish on a regular basis...
507  static mrpt::system::CTicTac lastMapPublished;
508  if (lastMapPublished.Tac() > 2.0)
509  {
510  mvsim_world_->runVisitorOnWorldElements(
511  [this](mvsim::WorldElementBase& obj) {
512  publishWorldElements(obj);
513  });
514  lastMapPublished.Tic();
515  }
516 #endif
517 
518  mvsim_world_->runVisitorOnVehicles(
519  [this](mvsim::VehicleBase& v) { publishVehicles(v); });
520 
521  // Create subscribers & publishers for each vehicle's stuff:
522  // ----------------------------------------------------
523  auto& vehs = mvsim_world_->getListOfVehicles();
524  pubsub_vehicles_.clear();
525  pubsub_vehicles_.resize(vehs.size());
526  size_t idx = 0;
527  for (auto it = vehs.begin(); it != vehs.end(); ++it, ++idx)
528  {
529  mvsim::VehicleBase* veh =
530  dynamic_cast<mvsim::VehicleBase*>(it->second.get());
531  if (!veh) continue;
532 
533  initPubSubs(pubsub_vehicles_[idx], veh);
534  }
535 
536  // Publish the static transform /world -> /map
537  sendStaticTF("world", "map", tfIdentity_, myNow());
538 }
539 
540 #if PACKAGE_ROS_VERSION == 1
541 ros::Time MVSimNode::myNow() const { return ros::Time::now(); }
542 #else
543 rclcpp::Time MVSimNode::myNow() const { return n_->get_clock()->now(); }
544 #endif
545 
546 void MVSimNode::sendStaticTF(
547  const std::string& frame_id, const std::string& child_frame_id,
548  const tf2::Transform& txf,
549 #if PACKAGE_ROS_VERSION == 1
550  const ros::Time& stamp
551 #else
552  const rclcpp::Time& stamp
553 #endif
554 )
555 {
556  Msg_TransformStamped tx;
557  tx.header.frame_id = frame_id;
558  tx.child_frame_id = child_frame_id;
559  tx.header.stamp = stamp;
560  tx.transform = tf2::toMsg(txf);
561  static_tf_br_.sendTransform(tx);
562 }
563 
566 void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh)
567 {
568  // sub: <VEH>/cmd_vel
569 #if PACKAGE_ROS_VERSION == 1
570  pubsubs.sub_cmd_vel = n_.subscribe<geometry_msgs::Twist>(
571  vehVarName("cmd_vel", *veh), 10,
572  boost::bind(&MVSimNode::onROSMsgCmdVel, this, _1, veh));
573 #else
574  using std::placeholders::_1;
575 
576  pubsubs.sub_cmd_vel = n_->create_subscription<geometry_msgs::msg::Twist>(
577  vehVarName("cmd_vel", *veh), 10,
578  [this, veh](const geometry_msgs::msg::Twist::ConstSharedPtr& msg) {
579  return this->onROSMsgCmdVel(msg, veh);
580  });
581 #endif
582 
583 #if PACKAGE_ROS_VERSION == 1
584  // pub: <VEH>/odom
585  pubsubs.pub_odom = n_.advertise<nav_msgs::Odometry>(
586  vehVarName("odom", *veh), publisher_history_len_);
587 
588  // pub: <VEH>/base_pose_ground_truth
589  pubsubs.pub_ground_truth = n_.advertise<nav_msgs::Odometry>(
590  vehVarName("base_pose_ground_truth", *veh), publisher_history_len_);
591 #else
592  // pub: <VEH>/odom
593  pubsubs.pub_odom = n_->create_publisher<nav_msgs::msg::Odometry>(
594  vehVarName("odom", *veh), publisher_history_len_);
595  // pub: <VEH>/base_pose_ground_truth
596  pubsubs.pub_ground_truth = n_->create_publisher<nav_msgs::msg::Odometry>(
597  vehVarName("base_pose_ground_truth", *veh), publisher_history_len_);
598 #endif
599 
600  // pub: <VEH>/chassis_markers
601  {
602 #if PACKAGE_ROS_VERSION == 1
603  pubsubs.pub_chassis_markers =
604  n_.advertise<visualization_msgs::MarkerArray>(
605  vehVarName("chassis_markers", *veh), 5, true /*latch*/);
606 #else
607  rclcpp::QoS qosLatched(rclcpp::KeepLast(5));
608  qosLatched.durability(rmw_qos_durability_policy_t::
609  RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
610 
611  pubsubs.pub_chassis_markers =
612  n_->create_publisher<visualization_msgs::msg::MarkerArray>(
613  vehVarName("chassis_markers", *veh), qosLatched);
614 #endif
615  const mrpt::math::TPolygon2D& poly = veh->getChassisShape();
616 
617  // Create one "ROS marker" for each wheel + 1 for the chassis:
618  auto& msg_shapes = pubsubs.chassis_shape_msg;
619  msg_shapes.markers.resize(1 + veh->getNumWheels());
620 
621  // [0] Chassis shape:
622  auto& chassis_shape_msg = msg_shapes.markers[0];
623 
624  chassis_shape_msg.pose =
625  mrpt2ros::toROS_Pose(mrpt::poses::CPose3D::Identity());
626 
627 #if PACKAGE_ROS_VERSION == 1
628  chassis_shape_msg.action = visualization_msgs::Marker::MODIFY;
629  chassis_shape_msg.type = visualization_msgs::Marker::LINE_STRIP;
630 #else
631  chassis_shape_msg.action = visualization_msgs::msg::Marker::MODIFY;
632  chassis_shape_msg.type = visualization_msgs::msg::Marker::LINE_STRIP;
633 #endif
634 
635  chassis_shape_msg.header.frame_id = vehVarName("base_link", *veh);
636  chassis_shape_msg.ns = "mvsim.chassis_shape";
637  chassis_shape_msg.id = veh->getVehicleIndex();
638  chassis_shape_msg.scale.x = 0.05;
639  chassis_shape_msg.scale.y = 0.05;
640  chassis_shape_msg.scale.z = 0.02;
641  chassis_shape_msg.points.resize(poly.size() + 1);
642  for (size_t i = 0; i <= poly.size(); i++)
643  {
644  size_t k = i % poly.size();
645  chassis_shape_msg.points[i].x = poly[k].x;
646  chassis_shape_msg.points[i].y = poly[k].y;
647  chassis_shape_msg.points[i].z = 0;
648  }
649  chassis_shape_msg.color.a = 0.9;
650  chassis_shape_msg.color.r = 0.9;
651  chassis_shape_msg.color.g = 0.9;
652  chassis_shape_msg.color.b = 0.9;
653  chassis_shape_msg.frame_locked = true;
654 
655  // [1:N] Wheel shapes
656  for (size_t i = 0; i < veh->getNumWheels(); i++)
657  {
658  auto& wheel_shape_msg = msg_shapes.markers[1 + i];
659  const mvsim::Wheel& w = veh->getWheelInfo(i);
660 
661  const double lx = w.diameter * 0.5, ly = w.width * 0.5;
662 
663  // Init values. Copy the contents from the chassis msg
664  wheel_shape_msg = msg_shapes.markers[0];
665 
666  chassis_shape_msg.ns = mrpt::format(
667  "mvsim.chassis_shape.wheel%u", static_cast<unsigned int>(i));
668  wheel_shape_msg.points.resize(5);
669  wheel_shape_msg.points[0].x = lx;
670  wheel_shape_msg.points[0].y = ly;
671  wheel_shape_msg.points[0].z = 0;
672  wheel_shape_msg.points[1].x = lx;
673  wheel_shape_msg.points[1].y = -ly;
674  wheel_shape_msg.points[1].z = 0;
675  wheel_shape_msg.points[2].x = -lx;
676  wheel_shape_msg.points[2].y = -ly;
677  wheel_shape_msg.points[2].z = 0;
678  wheel_shape_msg.points[3].x = -lx;
679  wheel_shape_msg.points[3].y = ly;
680  wheel_shape_msg.points[3].z = 0;
681  wheel_shape_msg.points[4] = wheel_shape_msg.points[0];
682 
683  wheel_shape_msg.color.r = w.color.R / 255.0f;
684  wheel_shape_msg.color.g = w.color.G / 255.0f;
685  wheel_shape_msg.color.b = w.color.B / 255.0f;
686  wheel_shape_msg.color.a = 1.0f;
687 
688  // Set local pose of the wheel wrt the vehicle:
689  wheel_shape_msg.pose = mrpt2ros::toROS_Pose(w.pose());
690  } // end for each wheel
691 
692  // Publish Initial pose
693 #if PACKAGE_ROS_VERSION == 1
694  pubsubs.pub_chassis_markers.publish(msg_shapes);
695 #else
696  pubsubs.pub_chassis_markers->publish(msg_shapes);
697 #endif
698  }
699 
700  // pub: <VEH>/chassis_polygon
701  {
702 #if PACKAGE_ROS_VERSION == 1
703  pubsubs.pub_chassis_shape = n_.advertise<geometry_msgs::Polygon>(
704  vehVarName("chassis_polygon", *veh), 1, true /*latch*/);
705 
706  geometry_msgs::Polygon poly_msg;
707 #else
708  rclcpp::QoS qosLatched(rclcpp::KeepLast(1));
709  qosLatched.durability(rmw_qos_durability_policy_t::
710  RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
711 
712  pubsubs.pub_chassis_shape =
713  n_->create_publisher<geometry_msgs::msg::Polygon>(
714  vehVarName("chassis_polygon", *veh), qosLatched);
715 
716  geometry_msgs::msg::Polygon poly_msg;
717 #endif
718  // Do the first (and unique) publish:
719  const mrpt::math::TPolygon2D& poly = veh->getChassisShape();
720  poly_msg.points.resize(poly.size());
721  for (size_t i = 0; i < poly.size(); i++)
722  {
723  poly_msg.points[i].x = poly[i].x;
724  poly_msg.points[i].y = poly[i].y;
725  poly_msg.points[i].z = 0;
726  }
727 #if PACKAGE_ROS_VERSION == 1
728  pubsubs.pub_chassis_shape.publish(poly_msg);
729 #else
730  pubsubs.pub_chassis_shape->publish(poly_msg);
731 #endif
732  }
733 
734  if (do_fake_localization_)
735  {
736 #if PACKAGE_ROS_VERSION == 1
737  // pub: <VEH>/amcl_pose
738  pubsubs.pub_amcl_pose =
739  n_.advertise<geometry_msgs::PoseWithCovarianceStamped>(
740  vehVarName("amcl_pose", *veh), 1);
741  // pub: <VEH>/particlecloud
742  pubsubs.pub_particlecloud = n_.advertise<geometry_msgs::PoseArray>(
743  vehVarName("particlecloud", *veh), 1);
744 #else
745  // pub: <VEH>/amcl_pose
746  pubsubs.pub_amcl_pose =
747  n_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
748  vehVarName("amcl_pose", *veh), 1);
749  // pub: <VEH>/particlecloud
750  pubsubs.pub_particlecloud =
751  n_->create_publisher<geometry_msgs::msg::PoseArray>(
752  vehVarName("particlecloud", *veh), 1);
753 #endif
754  }
755 
756  // STATIC Identity transform <VEH>/base_link -> <VEH>/base_footprint
757  sendStaticTF(
758  vehVarName("base_link", *veh), vehVarName("base_footprint", *veh),
759  tfIdentity_, myNow());
760 }
761 
762 void MVSimNode::onROSMsgCmdVel(
763 #if PACKAGE_ROS_VERSION == 1
764  const geometry_msgs::Twist::ConstPtr& cmd,
765 #else
766  const geometry_msgs::msg::Twist::ConstSharedPtr& cmd,
767 #endif
768  mvsim::VehicleBase* veh)
769 {
770  mvsim::ControllerBaseInterface* controller = veh->getControllerInterface();
771 
772  const bool ctrlAcceptTwist = controller->setTwistCommand(
773  {cmd->linear.x, cmd->linear.y, cmd->angular.z});
774 
775  if (!ctrlAcceptTwist)
776  {
777 #if PACKAGE_ROS_VERSION == 1
778  ROS_WARN_THROTTLE(
779  1.0,
780  "*Warning* Vehicle's controller ['%s'] refuses Twist commands!",
781  veh->getName().c_str());
782 #else
783  RCLCPP_WARN_THROTTLE(
784  n_->get_logger(), *n_->get_clock(), 1.0,
785  "*Warning* Vehicle's controller ['%s'] refuses Twist commands!",
786  veh->getName().c_str());
787 #endif
788  }
789 }
790 
792 void MVSimNode::spinNotifyROS()
793 {
794  if (!mvsim_world_) return;
795 
796  using namespace mvsim;
797  const auto& vehs = mvsim_world_->getListOfVehicles();
798 
799  // skip if the node is already shutting down:
800 #if PACKAGE_ROS_VERSION == 1
801  if (!ros::ok()) return;
802 #else
803  if (!rclcpp::ok()) return;
804 #endif
805 
806  // Get current simulation time (for messages) and publish "/clock"
807  // ----------------------------------------------------------------
808 #if PACKAGE_ROS_VERSION == 1
809  // sim_time_.fromSec(mvsim_world_->get_simul_time());
810  // clockMsg_.clock = sim_time_;
811  // pub_clock_.publish(clockMsg_);
812 #else
813  // sim_time_ = n_->get_clock()->now();
814  // MRPT_TODO("Publish /clock for ROS2 too?");
815 #endif
816 
817 #if PACKAGE_ROS_VERSION == 2
818  // In ROS2,latching doesn't work, we must re-publish on a regular basis...
819  mvsim_world_->runVisitorOnWorldElements(
820  [this](mvsim::WorldElementBase& obj) { publishWorldElements(obj); });
821 #endif
822 
823  // Publish all TFs for each vehicle:
824  // ---------------------------------------------------------------------
825  if (tim_publish_tf_.Tac() > period_ms_publish_tf_ * 1e-3)
826  {
827  tim_publish_tf_.Tic();
828 
829  size_t i = 0;
830  ASSERT_EQUAL_(pubsub_vehicles_.size(), vehs.size());
831 
832  for (auto it = vehs.begin(); it != vehs.end(); ++it, ++i)
833  {
834  const VehicleBase::Ptr& veh = it->second;
835 
836  const std::string sOdomName = vehVarName("odom", *veh);
837  const std::string sBaseLinkFrame = vehVarName("base_link", *veh);
838 
839  // 1) Ground-truth pose and velocity
840  // --------------------------------------------
841  const mrpt::math::TPose3D& gh_veh_pose = veh->getPose();
842  // [vx,vy,w] in global frame
843  const auto& gh_veh_vel = veh->getTwist();
844 
845  {
846 #if PACKAGE_ROS_VERSION == 1
847  nav_msgs::Odometry gtOdoMsg;
848 #else
849  nav_msgs::msg::Odometry gtOdoMsg;
850 #endif
851 
852  gtOdoMsg.pose.pose = mrpt2ros::toROS_Pose(gh_veh_pose);
853 
854  gtOdoMsg.twist.twist.linear.x = gh_veh_vel.vx;
855  gtOdoMsg.twist.twist.linear.y = gh_veh_vel.vy;
856  gtOdoMsg.twist.twist.linear.z = 0;
857  gtOdoMsg.twist.twist.angular.z = gh_veh_vel.omega;
858 
859  gtOdoMsg.header.stamp = myNow();
860  gtOdoMsg.header.frame_id = sOdomName;
861  gtOdoMsg.child_frame_id = sBaseLinkFrame;
862 
863 #if PACKAGE_ROS_VERSION == 1
864  pubsub_vehicles_[i].pub_ground_truth.publish(gtOdoMsg);
865 #else
866  pubsub_vehicles_[i].pub_ground_truth->publish(gtOdoMsg);
867 #endif
868  if (do_fake_localization_)
869  {
870 #if PACKAGE_ROS_VERSION == 1
871  geometry_msgs::PoseWithCovarianceStamped currentPos;
872  geometry_msgs::PoseArray particleCloud;
873 #else
874  geometry_msgs::msg::PoseWithCovarianceStamped currentPos;
875  geometry_msgs::msg::PoseArray particleCloud;
876 #endif
877 
878  // topic: <Ri>/particlecloud
879  {
880  particleCloud.header.stamp = myNow();
881  particleCloud.header.frame_id = "map";
882  particleCloud.poses.resize(1);
883  particleCloud.poses[0] = gtOdoMsg.pose.pose;
884 #if PACKAGE_ROS_VERSION == 1
885  pubsub_vehicles_[i].pub_particlecloud.publish(
886  particleCloud);
887 #else
888  pubsub_vehicles_[i].pub_particlecloud->publish(
889  particleCloud);
890 #endif
891  }
892 
893  // topic: <Ri>/amcl_pose
894  {
895  currentPos.header = gtOdoMsg.header;
896  currentPos.pose.pose = gtOdoMsg.pose.pose;
897 #if PACKAGE_ROS_VERSION == 1
898  pubsub_vehicles_[i].pub_amcl_pose.publish(currentPos);
899 #else
900  pubsub_vehicles_[i].pub_amcl_pose->publish(currentPos);
901 #endif
902  }
903 
904  // TF: /map -> <Ri>/odom
905  {
906  Msg_TransformStamped tx;
907  tx.header.frame_id = "map";
908  tx.child_frame_id = sOdomName;
909  tx.header.stamp = myNow();
910  tx.transform =
911  tf2::toMsg(tf2::Transform::getIdentity());
912 
913  tf_br_.sendTransform(tx);
914  }
915  }
916  }
917 
918  // 2) Chassis markers (for rviz visualization)
919  // --------------------------------------------
920  // pub: <VEH>/chassis_markers
921  {
922  // visualization_msgs::MarkerArray
923  auto& msg_shapes = pubsub_vehicles_[i].chassis_shape_msg;
924  ASSERT_EQUAL_(
925  msg_shapes.markers.size(), (1 + veh->getNumWheels()));
926 
927  // [0] Chassis shape: static no need to update.
928  // [1:N] Wheel shapes: may move
929  for (size_t j = 0; j < veh->getNumWheels(); j++)
930  {
931  // visualization_msgs::Marker
932  auto& wheel_shape_msg = msg_shapes.markers[1 + j];
933  const mvsim::Wheel& w = veh->getWheelInfo(j);
934 
935  // Set local pose of the wheel wrt the vehicle:
936  wheel_shape_msg.pose = mrpt2ros::toROS_Pose(w.pose());
937 
938  } // end for each wheel
939 
940  // Publish Initial pose
941 #if PACKAGE_ROS_VERSION == 1
942  pubsub_vehicles_[i].pub_chassis_markers.publish(msg_shapes);
943 #else
944  pubsub_vehicles_[i].pub_chassis_markers->publish(msg_shapes);
945 #endif
946  }
947 
948  // 3) odometry transform
949  // --------------------------------------------
950  {
951  const mrpt::math::TPose3D odo_pose = gh_veh_pose;
952 
953  {
954  Msg_TransformStamped tx;
955  tx.header.frame_id = sOdomName;
956  tx.child_frame_id = sBaseLinkFrame;
957  tx.header.stamp = myNow();
958  tx.transform =
959  tf2::toMsg(mrpt2ros::toROS_tfTransform(odo_pose));
960  tf_br_.sendTransform(tx);
961  }
962 
963  // Apart from TF, publish to the "odom" topic as well
964  {
965 #if PACKAGE_ROS_VERSION == 1
966  nav_msgs::Odometry odoMsg;
967 #else
968  nav_msgs::msg::Odometry odoMsg;
969 #endif
970  odoMsg.pose.pose = mrpt2ros::toROS_Pose(odo_pose);
971 
972  // first, we'll populate the header for the odometry msg
973  odoMsg.header.stamp = myNow();
974  odoMsg.header.frame_id = sOdomName;
975  odoMsg.child_frame_id = sBaseLinkFrame;
976 
977  // publish:
978 #if PACKAGE_ROS_VERSION == 1
979  pubsub_vehicles_[i].pub_odom.publish(odoMsg);
980 #else
981  pubsub_vehicles_[i].pub_odom->publish(odoMsg);
982 #endif
983  }
984  }
985 
986  } // end for each vehicle
987 
988  } // end publish tf
989 
990 } // end spinNotifyROS()
991 
992 void MVSimNode::onNewObservation(
993  const mvsim::Simulable& sim, const mrpt::obs::CObservation::Ptr& obs)
994 {
995  mrpt::system::CTimeLoggerEntry tle(profiler_, "onNewObservation");
996 
997  using mrpt::obs::CObservation2DRangeScan;
998 
999  // skip if the node is already shutting down:
1000 #if PACKAGE_ROS_VERSION == 1
1001  if (!ros::ok()) return;
1002 #else
1003  if (!rclcpp::ok()) return;
1004 #endif
1005 
1006  ASSERT_(obs);
1007  ASSERT_(!obs->sensorLabel.empty());
1008 
1009  const auto& vehPtr = dynamic_cast<const mvsim::VehicleBase*>(&sim);
1010  if (!vehPtr) return; // for example, if obs from invisible aux block.
1011  const auto& veh = *vehPtr;
1012 
1013  // -----------------------------
1014  // Observation: 2d laser scans
1015  // -----------------------------
1016  if (const auto* o2DLidar =
1017  dynamic_cast<const CObservation2DRangeScan*>(obs.get());
1018  o2DLidar)
1019  {
1020  internalOn(veh, *o2DLidar);
1021  }
1022  else if (const auto* oImage =
1023  dynamic_cast<const mrpt::obs::CObservationImage*>(obs.get());
1024  oImage)
1025  {
1026  internalOn(veh, *oImage);
1027  }
1028  else if (const auto* oRGBD =
1029  dynamic_cast<const mrpt::obs::CObservation3DRangeScan*>(
1030  obs.get());
1031  oRGBD)
1032  {
1033  internalOn(veh, *oRGBD);
1034  }
1035  else if (const auto* oPC =
1036  dynamic_cast<const mrpt::obs::CObservationPointCloud*>(
1037  obs.get());
1038  oPC)
1039  {
1040  internalOn(veh, *oPC);
1041  }
1042  else if (const auto* oIMU =
1043  dynamic_cast<const mrpt::obs::CObservationIMU*>(obs.get());
1044  oIMU)
1045  {
1046  internalOn(veh, *oIMU);
1047  }
1048  else
1049  {
1050  // Don't know how to emit this observation to ROS!
1051 #if PACKAGE_ROS_VERSION == 1
1052  ROS_WARN_STREAM_THROTTLE(
1053  1.0, "Do not know how to publish this observation to ROS: '"
1054  << obs->sensorLabel
1055  << "', class: " << obs->GetRuntimeClass()->className);
1056 #else
1057  RCLCPP_WARN_STREAM_THROTTLE(
1058  n_->get_logger(), *n_->get_clock(), 1.0,
1059  "Do not know how to publish this observation to ROS: '"
1060  << obs->sensorLabel
1061  << "', class: " << obs->GetRuntimeClass()->className);
1062 #endif
1063  }
1064 
1065 } // end of onNewObservation()
1066 
1069 std::string MVSimNode::vehVarName(
1070  const std::string& sVarName, const mvsim::VehicleBase& veh) const
1071 {
1072  using namespace std::string_literals;
1073 
1074  if (mvsim_world_->getListOfVehicles().size() == 1)
1075  {
1076  return sVarName;
1077  }
1078  else
1079  {
1080  return veh.getName() + std::string("/") + sVarName;
1081  }
1082 }
1083 
1084 void MVSimNode::internalOn(
1085  const mvsim::VehicleBase& veh,
1086  const mrpt::obs::CObservation2DRangeScan& obs)
1087 {
1088  auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_);
1089  TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()];
1090 
1091  // Create the publisher the first time an observation arrives:
1092  const bool is_1st_pub =
1093  pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end();
1094  auto& pub = pubs.pub_sensors[obs.sensorLabel];
1095 
1096  if (is_1st_pub)
1097  {
1098 #if PACKAGE_ROS_VERSION == 1
1099  pub = n_.advertise<sensor_msgs::LaserScan>(
1100  vehVarName(obs.sensorLabel, veh), publisher_history_len_);
1101 #else
1102  pub = n_->create_publisher<sensor_msgs::msg::LaserScan>(
1103  vehVarName(obs.sensorLabel, veh), publisher_history_len_);
1104 #endif
1105  }
1106  lck.unlock();
1107 
1108 #if PACKAGE_ROS_VERSION == 2
1109  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pubLidar =
1110  std::dynamic_pointer_cast<
1111  rclcpp::Publisher<sensor_msgs::msg::LaserScan>>(pub);
1112  ASSERT_(pubLidar);
1113 #endif
1114 
1115  const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh);
1116 
1117  // Send TF:
1118  mrpt::poses::CPose3D sensorPose;
1119  obs.getSensorPose(sensorPose);
1120 
1121  tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose);
1122 
1123  Msg_TransformStamped tfStmp;
1124  tfStmp.transform = tf2::toMsg(transform);
1125  tfStmp.child_frame_id = sSensorFrameId;
1126  tfStmp.header.frame_id = vehVarName("base_link", veh);
1127  tfStmp.header.stamp = myNow();
1128  tf_br_.sendTransform(tfStmp);
1129 
1130  // Send observation:
1131  {
1132  // Convert observation MRPT -> ROS
1133 #if PACKAGE_ROS_VERSION == 1
1134  geometry_msgs::Pose msg_pose_laser;
1135  sensor_msgs::LaserScan msg_laser;
1136 #else
1137  geometry_msgs::msg::Pose msg_pose_laser;
1138  sensor_msgs::msg::LaserScan msg_laser;
1139 #endif
1140  mrpt2ros::toROS(obs, msg_laser, msg_pose_laser);
1141 
1142  // Force usage of simulation time:
1143  msg_laser.header.stamp = myNow();
1144  msg_laser.header.frame_id = sSensorFrameId;
1145 
1146 #if PACKAGE_ROS_VERSION == 1
1147  pub.publish(msg_laser);
1148 #else
1149  pubLidar->publish(msg_laser);
1150 #endif
1151  }
1152 }
1153 
1154 void MVSimNode::internalOn(
1155  const mvsim::VehicleBase& veh, const mrpt::obs::CObservationIMU& obs)
1156 {
1157  auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_);
1158  TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()];
1159 
1160  // Create the publisher the first time an observation arrives:
1161  const bool is_1st_pub =
1162  pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end();
1163  auto& pub = pubs.pub_sensors[obs.sensorLabel];
1164 
1165  if (is_1st_pub)
1166  {
1167 #if PACKAGE_ROS_VERSION == 1
1168  pub = n_.advertise<sensor_msgs::Imu>(
1169  vehVarName(obs.sensorLabel, veh), publisher_history_len_);
1170 #else
1171  pub = n_->create_publisher<sensor_msgs::msg::Imu>(
1172  vehVarName(obs.sensorLabel, veh), publisher_history_len_);
1173  ASSERT_(pub);
1174 #endif
1175  }
1176  lck.unlock();
1177 
1178 #if PACKAGE_ROS_VERSION == 2
1179  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pubImu =
1180  std::dynamic_pointer_cast<rclcpp::Publisher<sensor_msgs::msg::Imu>>(
1181  pub);
1182  ASSERT_(pubImu);
1183 #endif
1184 
1185  const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh);
1186 
1187  // Send TF:
1188  mrpt::poses::CPose3D sensorPose;
1189  obs.getSensorPose(sensorPose);
1190 
1191  tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose);
1192 
1193  Msg_TransformStamped tfStmp;
1194  tfStmp.transform = tf2::toMsg(transform);
1195  tfStmp.child_frame_id = sSensorFrameId;
1196  tfStmp.header.frame_id = vehVarName("base_link", veh);
1197  tfStmp.header.stamp = myNow();
1198  tf_br_.sendTransform(tfStmp);
1199 
1200  // Send observation:
1201  {
1202  // Convert observation MRPT -> ROS
1203 #if PACKAGE_ROS_VERSION == 1
1204  sensor_msgs::Imu msg_imu;
1205  geometry_msgs::Pose msg_pose_imu;
1206  std_msgs::Header msg_header;
1207 #else
1208  sensor_msgs::msg::Imu msg_imu;
1209  std_msgs::msg::Header msg_header;
1210 #endif
1211  // Force usage of simulation time:
1212  msg_header.stamp = myNow();
1213  msg_header.frame_id = sSensorFrameId;
1214 
1215  mrpt2ros::toROS(obs, msg_header, msg_imu);
1216 
1217 #if PACKAGE_ROS_VERSION == 1
1218  pub.publish(msg_imu);
1219 #else
1220  pubImu->publish(msg_imu);
1221 #endif
1222  }
1223 }
1224 
1225 void MVSimNode::internalOn(
1226  const mvsim::VehicleBase& veh, const mrpt::obs::CObservationImage& obs)
1227 {
1228  auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_);
1229  TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()];
1230 
1231  // Create the publisher the first time an observation arrives:
1232  const bool is_1st_pub =
1233  pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end();
1234  auto& pub = pubs.pub_sensors[obs.sensorLabel];
1235 
1236  if (is_1st_pub)
1237  {
1238 #if PACKAGE_ROS_VERSION == 1
1239  pub = n_.advertise<sensor_msgs::Image>(
1240  vehVarName(obs.sensorLabel, veh), publisher_history_len_);
1241 #else
1242  pub = n_->create_publisher<sensor_msgs::msg::Image>(
1243  vehVarName(obs.sensorLabel, veh), publisher_history_len_);
1244 #endif
1245  }
1246  lck.unlock();
1247 
1248 #if PACKAGE_ROS_VERSION == 2
1249  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pubImg =
1250  std::dynamic_pointer_cast<rclcpp::Publisher<sensor_msgs::msg::Image>>(
1251  pub);
1252  ASSERT_(pubImg);
1253 #endif
1254 
1255  const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh);
1256 
1257  // Send TF:
1258  mrpt::poses::CPose3D sensorPose;
1259  obs.getSensorPose(sensorPose);
1260 
1261  tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose);
1262 
1263  Msg_TransformStamped tfStmp;
1264  tfStmp.transform = tf2::toMsg(transform);
1265  tfStmp.child_frame_id = sSensorFrameId;
1266  tfStmp.header.frame_id = vehVarName("base_link", veh);
1267  tfStmp.header.stamp = myNow();
1268  tf_br_.sendTransform(tfStmp);
1269 
1270  // Send observation:
1271  {
1272  // Convert observation MRPT -> ROS
1273 #if PACKAGE_ROS_VERSION == 1
1274  sensor_msgs::Image msg_img;
1275  std_msgs::Header msg_header;
1276 #else
1277  sensor_msgs::msg::Image msg_img;
1278  std_msgs::msg::Header msg_header;
1279 #endif
1280  msg_header.stamp = myNow();
1281  msg_header.frame_id = sSensorFrameId;
1282 
1283  msg_img = mrpt2ros::toROS(obs.image, msg_header);
1284 
1285 #if PACKAGE_ROS_VERSION == 1
1286  pub.publish(msg_img);
1287 #else
1288  pubImg->publish(msg_img);
1289 #endif
1290  }
1291 }
1292 
1293 void MVSimNode::internalOn(
1294  const mvsim::VehicleBase& veh,
1295  const mrpt::obs::CObservation3DRangeScan& obs)
1296 {
1297  using namespace std::string_literals;
1298 
1299  auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_);
1300  TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()];
1301 
1302  const auto lbPoints = obs.sensorLabel + "_points"s;
1303  const auto lbImage = obs.sensorLabel + "_image"s;
1304 
1305  // Create the publisher the first time an observation arrives:
1306  const bool is_1st_pub =
1307  pubs.pub_sensors.find(lbPoints) == pubs.pub_sensors.end();
1308 
1309  auto& pubPts = pubs.pub_sensors[lbPoints];
1310  auto& pubImg = pubs.pub_sensors[lbImage];
1311 
1312  if (is_1st_pub)
1313  {
1314 #if PACKAGE_ROS_VERSION == 1
1315  pubPts = n_.advertise<sensor_msgs::PointCloud2>(
1316  vehVarName(lbPoints, veh), publisher_history_len_);
1317  pubImg = n_.advertise<sensor_msgs::Image>(
1318  vehVarName(lbImage, veh), publisher_history_len_);
1319 #else
1320  pubImg = n_->create_publisher<sensor_msgs::msg::Image>(
1321  vehVarName(lbImage, veh), publisher_history_len_);
1322  pubPts = n_->create_publisher<sensor_msgs::msg::PointCloud2>(
1323  vehVarName(lbPoints, veh), publisher_history_len_);
1324 #endif
1325  }
1326  lck.unlock();
1327 
1328 #if PACKAGE_ROS_VERSION == 2
1329  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubPoints =
1330  std::dynamic_pointer_cast<
1331  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>>(pubPts);
1332  ASSERT_(pubPoints);
1333 
1334  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pubImage =
1335  std::dynamic_pointer_cast<rclcpp::Publisher<sensor_msgs::msg::Image>>(
1336  pubImg);
1337  ASSERT_(pubImage);
1338 #endif
1339 
1340  const std::string sSensorFrameId_image = vehVarName(lbImage, veh);
1341  const std::string sSensorFrameId_points = vehVarName(lbPoints, veh);
1342 
1343  const auto now = myNow();
1344 
1345  // IMAGE
1346  // --------
1347  if (obs.hasIntensityImage)
1348  {
1349  // Send TF:
1350  mrpt::poses::CPose3D sensorPose =
1351  obs.sensorPose + obs.relativePoseIntensityWRTDepth;
1352 
1353  tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose);
1354 
1355  Msg_TransformStamped tfStmp;
1356  tfStmp.transform = tf2::toMsg(transform);
1357  tfStmp.child_frame_id = sSensorFrameId_image;
1358  tfStmp.header.frame_id = vehVarName("base_link", veh);
1359  tfStmp.header.stamp = now;
1360  tf_br_.sendTransform(tfStmp);
1361 
1362  // Send observation:
1363  {
1364  // Convert observation MRPT -> ROS
1365 #if PACKAGE_ROS_VERSION == 1
1366  sensor_msgs::Image msg_img;
1367  std_msgs::Header msg_header;
1368 #else
1369  sensor_msgs::msg::Image msg_img;
1370  std_msgs::msg::Header msg_header;
1371 #endif
1372  msg_header.stamp = now;
1373  msg_header.frame_id = sSensorFrameId_image;
1374 
1375  msg_img = mrpt2ros::toROS(obs.intensityImage, msg_header);
1376 
1377 #if PACKAGE_ROS_VERSION == 1
1378  pubImg.publish(msg_img);
1379 #else
1380  pubImage->publish(msg_img);
1381 #endif
1382  }
1383  }
1384 
1385  // POINTS
1386  // --------
1387  if (obs.hasRangeImage)
1388  {
1389  // Send TF:
1390  mrpt::poses::CPose3D sensorPose = obs.sensorPose;
1391 
1392  tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose);
1393 
1394  Msg_TransformStamped tfStmp;
1395  tfStmp.transform = tf2::toMsg(transform);
1396  tfStmp.child_frame_id = sSensorFrameId_points;
1397  tfStmp.header.frame_id = vehVarName("base_link", veh);
1398  tfStmp.header.stamp = now;
1399  tf_br_.sendTransform(tfStmp);
1400 
1401  // Send observation:
1402  {
1403  // Convert observation MRPT -> ROS
1404 #if PACKAGE_ROS_VERSION == 1
1405  sensor_msgs::PointCloud2 msg_pts;
1406  std_msgs::Header msg_header;
1407 #else
1408  sensor_msgs::msg::PointCloud2 msg_pts;
1409  std_msgs::msg::Header msg_header;
1410 #endif
1411  msg_header.stamp = now;
1412  msg_header.frame_id = sSensorFrameId_points;
1413 
1414  mrpt::obs::T3DPointsProjectionParams pp;
1415  pp.takeIntoAccountSensorPoseOnRobot = false;
1416 
1417  mrpt::maps::CSimplePointsMap pts;
1418  const_cast<mrpt::obs::CObservation3DRangeScan&>(obs).unprojectInto(
1419  pts, pp);
1420 
1421  mrpt2ros::toROS(pts, msg_header, msg_pts);
1422 
1423 #if PACKAGE_ROS_VERSION == 1
1424  pubPts.publish(msg_pts);
1425 #else
1426  pubPoints->publish(msg_pts);
1427 #endif
1428  }
1429  }
1430 }
1431 
1432 void MVSimNode::internalOn(
1433  const mvsim::VehicleBase& veh, const mrpt::obs::CObservationPointCloud& obs)
1434 {
1435  using namespace std::string_literals;
1436 
1437  auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_);
1438  TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()];
1439 
1440  const auto lbPoints = obs.sensorLabel + "_points"s;
1441 
1442  // Create the publisher the first time an observation arrives:
1443  const bool is_1st_pub =
1444  pubs.pub_sensors.find(lbPoints) == pubs.pub_sensors.end();
1445 
1446  auto& pubPts = pubs.pub_sensors[lbPoints];
1447 
1448  if (is_1st_pub)
1449  {
1450 #if PACKAGE_ROS_VERSION == 1
1451  pubPts = n_.advertise<sensor_msgs::PointCloud2>(
1452  vehVarName(lbPoints, veh), publisher_history_len_);
1453 #else
1454  pubPts = n_->create_publisher<sensor_msgs::msg::PointCloud2>(
1455  vehVarName(lbPoints, veh), publisher_history_len_);
1456 #endif
1457  }
1458  lck.unlock();
1459 
1460 #if PACKAGE_ROS_VERSION == 2
1461  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubPoints =
1462  std::dynamic_pointer_cast<
1463  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>>(pubPts);
1464  ASSERT_(pubPoints);
1465 #endif
1466 
1467  const std::string sSensorFrameId_points = vehVarName(lbPoints, veh);
1468 
1469  const auto now = myNow();
1470 
1471  // POINTS
1472  // --------
1473 
1474  // Send TF:
1475  mrpt::poses::CPose3D sensorPose = obs.sensorPose;
1476 
1477  tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose);
1478 
1479  Msg_TransformStamped tfStmp;
1480  tfStmp.transform = tf2::toMsg(transform);
1481  tfStmp.child_frame_id = sSensorFrameId_points;
1482  tfStmp.header.frame_id = vehVarName("base_link", veh);
1483  tfStmp.header.stamp = now;
1484  tf_br_.sendTransform(tfStmp);
1485 
1486  // Send observation:
1487  {
1488  // Convert observation MRPT -> ROS
1489 #if PACKAGE_ROS_VERSION == 1
1490  sensor_msgs::PointCloud2 msg_pts;
1491  std_msgs::Header msg_header;
1492 #else
1493  sensor_msgs::msg::PointCloud2 msg_pts;
1494  std_msgs::msg::Header msg_header;
1495 #endif
1496  msg_header.stamp = now;
1497  msg_header.frame_id = sSensorFrameId_points;
1498 
1499  mrpt::obs::T3DPointsProjectionParams pp;
1500  pp.takeIntoAccountSensorPoseOnRobot = false;
1501 
1502  if (auto* sPts = dynamic_cast<const mrpt::maps::CSimplePointsMap*>(
1503  obs.pointcloud.get());
1504  sPts)
1505  {
1506  mrpt2ros::toROS(*sPts, msg_header, msg_pts);
1507  }
1508  else if (auto* xyzi = dynamic_cast<const mrpt::maps::CPointsMapXYZI*>(
1509  obs.pointcloud.get());
1510  xyzi)
1511  {
1512  mrpt2ros::toROS(*xyzi, msg_header, msg_pts);
1513  }
1514  else
1515  {
1516  THROW_EXCEPTION(
1517  "Do not know how to handle this variant of CPointsMap");
1518  }
1519 
1520 #if PACKAGE_ROS_VERSION == 1
1521  pubPts.publish(msg_pts);
1522 #else
1523  pubPoints->publish(msg_pts);
1524 #endif
1525  }
1526 }
mvsim::VehicleBase::getVehicleIndex
size_t getVehicleIndex() const
Definition: VehicleBase.h:115
ImGui::Image
IMGUI_API void Image(ImTextureID user_texture_id, const ImVec2 &size, const ImVec2 &uv0=ImVec2(0, 0), const ImVec2 &uv1=ImVec2(1, 1), const ImVec4 &tint_col=ImVec4(1, 1, 1, 1), const ImVec4 &border_col=ImVec4(0, 0, 0, 0))
Definition: imgui_widgets.cpp:812
MVSimNode::gui_key_events_
mvsim::World::TGUIKeyEvent gui_key_events_
Definition: mvsim_node_core.h:250
MVSimNode::thread_update_GUI
static void thread_update_GUI(TThreadParams &thread_params)
Definition: mvsim_node.cpp:408
MVSimNode::TPubSubPerVehicle::sub_cmd_vel
rclcpp::Subscription< geometry_msgs::msg::Twist >::SharedPtr sub_cmd_vel
Subscribers vehicle&#39;s "cmd_vel" topic.
Definition: mvsim_node_core.h:161
MVSimNode::static_tf_br_
tf2_ros::StaticTransformBroadcaster static_tf_br_
Definition: mvsim_node_core.h:139
MVSimNode::TThreadParams::closing
std::atomic_bool closing
Definition: mvsim_node_core.h:227
MVSimNode::TPubSubPerVehicle::pub_odom
rclcpp::Publisher< nav_msgs::msg::Odometry >::SharedPtr pub_odom
Publisher of "odom" topic.
Definition: mvsim_node_core.h:163
ros::NodeHandle
mvsim::Wheel::width
double width
Definition: Wheel.h:44
ROS_WARN_STREAM_THROTTLE
#define ROS_WARN_STREAM_THROTTLE(period, args)
mvsim::VehicleBase::getWheelInfo
const Wheel & getWheelInfo(const size_t idx) const
Definition: VehicleBase.h:81
rapidxml::file
Represents data loaded from a file.
Definition: rapidxml_utils.hpp:21
MVSimNode::TPubSubPerVehicle::chassis_shape_msg
visualization_msgs::msg::MarkerArray chassis_shape_msg
Definition: mvsim_node_core.h:183
MVSimNode::realtime_tictac_
mrpt::system::CTicTac realtime_tictac_
Definition: mvsim_node_core.h:230
MVSimNode::mvsim_world_
std::shared_ptr< mvsim::World > mvsim_world_
Definition: mvsim_node_core.h:92
mvsim::ControllerBaseInterface::setTwistCommand
virtual bool setTwistCommand([[maybe_unused]] const mrpt::math::TTwist2D &t)
Definition: ControllerBase.h:46
MVSimNode::n_
rclcpp::Node::SharedPtr n_
Definition: mvsim_node_core.h:119
s
XmlRpcServer s
MVSimNode::TPubSubPerVehicle::pub_chassis_markers
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr pub_chassis_markers
"<VEH>/chassis_markers"
Definition: mvsim_node_core.h:178
MVSimNode::launch_mvsim_server
void launch_mvsim_server()
Definition: mvsim_node.cpp:230
mvsim::Wheel::color
mrpt::img::TColor color
Definition: Wheel.h:52
Msg_MapMetaData
nav_msgs::msg::MapMetaData Msg_MapMetaData
Definition: mvsim_node.cpp:72
t
geometry_msgs::TransformStamped t
mvsim::Wheel::pose
mrpt::math::TPose3D pose() const
Definition: Wheel.h:41
MVSimNode::profiler_
mrpt::system::CTimeLogger profiler_
Definition: mvsim_node_core.h:284
MVSimNode::publishVehicles
void publishVehicles(mvsim::VehicleBase &veh)
Definition: mvsim_node.cpp:455
MVSimNode::pub_map_ros_
rclcpp::Publisher< nav_msgs::msg::OccupancyGrid >::SharedPtr pub_map_ros_
used for simul_map publication
Definition: mvsim_node_core.h:128
Msg_OccupancyGrid
nav_msgs::msg::OccupancyGrid Msg_OccupancyGrid
Definition: mvsim_node.cpp:71
mvsim::World::TUpdateGUIParams::keyevent
TGUIKeyEvent keyevent
Keystrokes in the window are returned here.
Definition: World.h:163
mvsim::VehicleBase::getChassisShape
const mrpt::math::TPolygon2D & getChassisShape() const
Definition: VehicleBase.h:107
MVSimNode::TPubSubPerVehicle::pub_particlecloud
rclcpp::Publisher< geometry_msgs::msg::PoseArray >::SharedPtr pub_particlecloud
Definition: mvsim_node_core.h:171
MVSimNode::world_init_ok_
bool world_init_ok_
will be true after a success call to loadWorldModel()
Definition: mvsim_node_core.h:235
MVSimNode::loadWorldModel
void loadWorldModel(const std::string &world_xml_file)
Definition: mvsim_node.cpp:249
MVSimNode::onROSMsgCmdVel
void onROSMsgCmdVel(const geometry_msgs::msg::Twist::ConstSharedPtr &cmd, mvsim::VehicleBase *veh)
Definition: mvsim_node.cpp:762
MVSimNode::spin
void spin()
Process pending msgs, run real-time simulation, etc.
Definition: mvsim_node.cpp:308
MVSimNode::period_ms_publish_tf_
double period_ms_publish_tf_
Definition: mvsim_node_core.h:239
MVSimNode::onNewObservation
void onNewObservation(const mvsim::Simulable &veh, const mrpt::obs::CObservation::Ptr &obs)
Definition: mvsim_node.cpp:992
MVSimNode::gui_refresh_period_ms_
int gui_refresh_period_ms_
Definition: mvsim_node_core.h:100
MVSimNode::clock_
rclcpp::Clock::SharedPtr clock_
Definition: mvsim_node_core.h:131
MVSimNode::MVSimNode
MVSimNode(rclcpp::Node::SharedPtr &n)
Definition: mvsim_node.cpp:102
MVSimNode::publisher_history_len_
int publisher_history_len_
(Default=0.1) Time tolerance for published TFs
Definition: mvsim_node_core.h:107
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(period,...)
MVSimNode::sendStaticTF
void sendStaticTF(const std::string &frame_id, const std::string &child_frame_id, const tf2::Transform &tx, const rclcpp::Time &stamp)
Definition: mvsim_node.cpp:546
ROS_INFO
#define ROS_INFO(...)
MVSimNode::spinNotifyROS
void spinNotifyROS()
Definition: mvsim_node.cpp:792
mvsim::VehicleBase::Ptr
std::shared_ptr< VehicleBase > Ptr
Definition: VehicleBase.h:44
MVSimNode::msg2gui_
std::string msg2gui_
Definition: mvsim_node_core.h:251
rapidxml_utils.hpp
mvsim::World::TUpdateGUIParams::msg_lines
std::string msg_lines
Messages to show.
Definition: World.h:164
mvsim::Wheel::diameter
double diameter
Definition: Wheel.h:44
MVSimNode::vehVarName
std::string vehVarName(const std::string &sVarName, const mvsim::VehicleBase &veh) const
Definition: mvsim_node.cpp:1069
ros::ok
ROSCPP_DECL bool ok()
tf2::Transform::getIdentity
static const Transform & getIdentity()
MVSimNode::pubsub_vehicles_mtx_
std::mutex pubsub_vehicles_mtx_
Definition: mvsim_node_core.h:190
tf2_geometry_msgs.h
MVSimNode::TPubSubPerVehicle::pub_chassis_shape
rclcpp::Publisher< geometry_msgs::msg::Polygon >::SharedPtr pub_chassis_shape
"<VEH>/chassis_shape"
Definition: mvsim_node_core.h:181
MVSimNode::tim_teleop_refresh_
mrpt::system::CTicTac tim_teleop_refresh_
Definition: mvsim_node_core.h:246
mvsim::Simulable::getName
const std::string & getName() const
Definition: Simulable.h:92
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
MVSimNode::period_ms_teleop_refresh_
double period_ms_teleop_refresh_
Definition: mvsim_node_core.h:245
MVSimNode::publishWorldElements
void publishWorldElements(mvsim::WorldElementBase &obj)
Definition: mvsim_node.cpp:462
MVSimNode::thread_params_
TThreadParams thread_params_
Definition: mvsim_node_core.h:229
tf2::toMsg
B toMsg(const A &a)
MVSimNode::terminateSimulation
void terminateSimulation()
Definition: mvsim_node.cpp:272
MVSimNode::do_fake_localization_
bool do_fake_localization_
Definition: mvsim_node_core.h:105
MVSimNode::tim_publish_tf_
mrpt::system::CTicTac tim_publish_tf_
Definition: mvsim_node_core.h:241
MVSimNode::TPubSubPerVehicle::pub_sensors
std::map< std::string, rclcpp::PublisherBase::SharedPtr > pub_sensors
Map <sensor_label> => publisher.
Definition: mvsim_node_core.h:174
MVSimNode::TPubSubPerVehicle::pub_ground_truth
rclcpp::Publisher< nav_msgs::msg::Odometry >::SharedPtr pub_ground_truth
"base_pose_ground_truth" topic
Definition: mvsim_node_core.h:165
MVSimNode::thGUI_
std::thread thGUI_
Definition: mvsim_node_core.h:253
MVSimNode::initPubSubs
void initPubSubs(TPubSubPerVehicle &out_pubsubs, mvsim::VehicleBase *veh)
Definition: mvsim_node.cpp:566
MVSimNode::pub_map_metadata_
rclcpp::Publisher< nav_msgs::msg::MapMetaData >::SharedPtr pub_map_metadata_
Definition: mvsim_node_core.h:129
Msg_TransformStamped
geometry_msgs::msg::TransformStamped Msg_TransformStamped
Definition: mvsim_node.cpp:73
MVSimNode::myNow
rclcpp::Time myNow() const
Definition: mvsim_node.cpp:543
MVSimNode::ros_publisher_workers_
mrpt::WorkerThreadsPool ros_publisher_workers_
Definition: mvsim_node_core.h:95
ros::Time::now
static Time now()
ROS12_ERROR
#define ROS12_ERROR(...)
Definition: mvsim_node.cpp:92
server
std::shared_ptr< mvsim::Server > server
Definition: mvsim-cli-server.cpp:16
MVSimNode::notifyROSWorldIsUpdated
void notifyROSWorldIsUpdated()
Definition: mvsim_node.cpp:502
MVSimNode::mvsim_server_
std::shared_ptr< mvsim::Server > mvsim_server_
Definition: mvsim_node_core.h:113
tf2_ros::StaticTransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
MVSimNode::TPubSubPerVehicle::pub_amcl_pose
rclcpp::Publisher< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr pub_amcl_pose
"fake_localization" pubs:
Definition: mvsim_node_core.h:169
mvsim::ControllerBaseInterface::teleop_interface
virtual void teleop_interface([[maybe_unused]] const TeleopInput &in, [[maybe_unused]] TeleopOutput &out)
Definition: ControllerBase.h:32
MVSimNode::internalOn
void internalOn(const mvsim::VehicleBase &veh, const mrpt::obs::CObservation2DRangeScan &obs)
Definition: mvsim_node.cpp:1084
ROS12_INFO
#define ROS12_INFO(...)
Definition: mvsim_node.cpp:91
MVSimNode::tfIdentity_
const tf2::Transform tfIdentity_
Unit transform (const, once)
Definition: mvsim_node_core.h:220
mvsim::VehicleBase::getNumWheels
size_t getNumWheels() const
Definition: VehicleBase.h:80
MVSimNode::teleop_idx_veh_
size_t teleop_idx_veh_
for teleoperation from the GUI (selects the focused" vehicle)
Definition: mvsim_node_core.h:249
MVSimNode::tf_br_
tf2_ros::TransformBroadcaster tf_br_
Use to send data to TF.
Definition: mvsim_node_core.h:138
mvsim::World::TGUIKeyEvent::keycode
int keycode
0=no Key. Otherwise, ASCII code.
Definition: World.h:152
MVSimNode::pubsub_vehicles_
std::vector< TPubSubPerVehicle > pubsub_vehicles_
Definition: mvsim_node_core.h:189
mvsim_node_core.h
MVSimNode::realtime_factor_
double realtime_factor_
(Defaul=1.0) >1: speed-up, <1: slow-down
Definition: mvsim_node_core.h:99

mvsim
Author(s):
autogenerated on Tue Jul 4 2023 03:08:21