Classes | Public Member Functions | Public Attributes | Protected Member Functions | Static Protected Member Functions | Protected Attributes | List of all members
MVSimNode Class Reference

#include <mvsim_node_core.h>

Classes

struct  TPubSubPerVehicle
 
struct  TThreadParams
 
struct  WorldPubs
 

Public Member Functions

void launch_mvsim_server ()
 
void loadWorldModel (const std::string &world_xml_file)
 
 MVSimNode (rclcpp::Node::SharedPtr &n)
 
void onNewObservation (const mvsim::Simulable &veh, const mrpt::obs::CObservation::Ptr &obs)
 
void spin ()
 Process pending msgs, run real-time simulation, etc. More...
 
void terminateSimulation ()
 
 ~MVSimNode ()
 

Public Attributes

bool do_fake_localization_ = true
 
int gui_refresh_period_ms_ = 50
 
bool headless_ = false
 
mvsim_node::shared_ptr< mvsim::Worldmvsim_world_ = mvsim_node::make_shared<mvsim::World>()
 
bool publish_tf_odom2baselink_ = true
 
int publisher_history_len_ = 50
 
double realtime_factor_ = 1.0
 (Defaul=1.0) >1: speed-up, <1: slow-down More...
 
mrpt::WorkerThreadsPool ros_publisher_workers_
 

Protected Member Functions

void initPubSubs (TPubSubPerVehicle &out_pubsubs, mvsim::VehicleBase *veh)
 
void internalOn (const mvsim::VehicleBase &veh, const mrpt::obs::CObservation2DRangeScan &obs)
 
void internalOn (const mvsim::VehicleBase &veh, const mrpt::obs::CObservation3DRangeScan &obs)
 
void internalOn (const mvsim::VehicleBase &veh, const mrpt::obs::CObservationGPS &obs)
 
void internalOn (const mvsim::VehicleBase &veh, const mrpt::obs::CObservationImage &obs)
 
void internalOn (const mvsim::VehicleBase &veh, const mrpt::obs::CObservationIMU &obs)
 
void internalOn (const mvsim::VehicleBase &veh, const mrpt::obs::CObservationPointCloud &obs)
 
ros_Time myNow () const
 
double myNowSec () const
 
void notifyROSWorldIsUpdated ()
 
void onROSMsgCmdVel (Msg_Twist_CSPtr cmd, mvsim::VehicleBase *veh)
 
void publishVehicles (mvsim::VehicleBase &veh)
 
void publishWorldElements (mvsim::WorldElementBase &obj)
 
void spinNotifyROS ()
 
std::string vehVarName (const std::string &sVarName, const mvsim::VehicleBase &veh) const
 

Static Protected Member Functions

static void thread_update_GUI (TThreadParams &thread_params)
 

Protected Attributes

ros_Time base_last_cmd_
 received a vel_cmd (for watchdog) More...
 
ros_Duration base_watchdog_timeout_ = ros_Duration(1, 0)
 
rclcpp::Clock::SharedPtr clock_
 
bool force_publish_vehicle_namespace_ = false
 If true, vehicle namespaces will be used even if there is only one vehicle: More...
 
mvsim::World::TGUIKeyEvent gui_key_events_
 
std::map< mvsim::VehicleBase *, double > lastCmdVelTimestamp_
 
std::string msg2gui_
 
rclcpp::Node::SharedPtr n_
 
double period_ms_publish_tf_ = 20
 
double period_ms_teleop_refresh_ = 100
 
mrpt::system::CTimeLogger profiler_ {true , "mvsim_node"}
 
std::vector< TPubSubPerVehiclepubsub_vehicles_
 
std::mutex pubsub_vehicles_mtx_
 
mrpt::system::CTicTac realtime_tictac_
 
double t_old_ = -1
 
size_t teleop_idx_veh_ = 0
 for teleoperation from the GUI (selects the focused" vehicle) More...
 
const tf2::Transform tfIdentity_ = tf2::Transform::getIdentity()
 Unit transform (const, once) More...
 
std::thread thGUI_
 
TThreadParams thread_params_
 
mrpt::system::CTicTac tim_publish_tf_
 
mrpt::system::CTicTac tim_teleop_refresh_
 
rclcpp::TimeSource ts_ {n_}
 
bool world_init_ok_ = false
 will be true after a success call to loadWorldModel() More...
 
WorldPubs worldPubs_
 

Detailed Description

A class to wrap libmvsim as a ROS node

Definition at line 125 of file mvsim_node_core.h.

Constructor & Destructor Documentation

◆ MVSimNode()

MVSimNode::MVSimNode ( rclcpp::Node::SharedPtr &  n)

Constructor.

Definition at line 130 of file mvsim_node.cpp.

◆ ~MVSimNode()

MVSimNode::~MVSimNode ( )

Destructor.

Definition at line 281 of file mvsim_node.cpp.

Member Function Documentation

◆ initPubSubs()

void MVSimNode::initPubSubs ( TPubSubPerVehicle pubsubs,
mvsim::VehicleBase veh 
)
protected

Initialize all pub/subs required for each vehicle, for the specific vehicle veh

Definition at line 564 of file mvsim_node.cpp.

◆ internalOn() [1/6]

void MVSimNode::internalOn ( const mvsim::VehicleBase veh,
const mrpt::obs::CObservation2DRangeScan &  obs 
)
protected

Definition at line 1025 of file mvsim_node.cpp.

◆ internalOn() [2/6]

void MVSimNode::internalOn ( const mvsim::VehicleBase veh,
const mrpt::obs::CObservation3DRangeScan &  obs 
)
protected

Definition at line 1324 of file mvsim_node.cpp.

◆ internalOn() [3/6]

void MVSimNode::internalOn ( const mvsim::VehicleBase veh,
const mrpt::obs::CObservationGPS &  obs 
)
protected

Definition at line 1122 of file mvsim_node.cpp.

◆ internalOn() [4/6]

void MVSimNode::internalOn ( const mvsim::VehicleBase veh,
const mrpt::obs::CObservationImage &  obs 
)
protected

Definition at line 1259 of file mvsim_node.cpp.

◆ internalOn() [5/6]

void MVSimNode::internalOn ( const mvsim::VehicleBase veh,
const mrpt::obs::CObservationIMU &  obs 
)
protected

Definition at line 1074 of file mvsim_node.cpp.

◆ internalOn() [6/6]

void MVSimNode::internalOn ( const mvsim::VehicleBase veh,
const mrpt::obs::CObservationPointCloud &  obs 
)
protected

Definition at line 1426 of file mvsim_node.cpp.

◆ launch_mvsim_server()

void MVSimNode::launch_mvsim_server ( )

Definition at line 246 of file mvsim_node.cpp.

◆ loadWorldModel()

void MVSimNode::loadWorldModel ( const std::string &  world_xml_file)

Definition at line 260 of file mvsim_node.cpp.

◆ myNow()

ros_Time MVSimNode::myNow ( ) const
protected

Definition at line 544 of file mvsim_node.cpp.

◆ myNowSec()

double MVSimNode::myNowSec ( ) const
protected

Definition at line 553 of file mvsim_node.cpp.

◆ notifyROSWorldIsUpdated()

void MVSimNode::notifyROSWorldIsUpdated ( )
protected

Publish relevant stuff whenever a new world model is loaded (grid maps, etc.)

Definition at line 502 of file mvsim_node.cpp.

◆ onNewObservation()

void MVSimNode::onNewObservation ( const mvsim::Simulable veh,
const mrpt::obs::CObservation::Ptr &  obs 
)

Definition at line 955 of file mvsim_node.cpp.

◆ onROSMsgCmdVel()

void MVSimNode::onROSMsgCmdVel ( Msg_Twist_CSPtr  cmd,
mvsim::VehicleBase veh 
)
protected

Definition at line 768 of file mvsim_node.cpp.

◆ publishVehicles()

void MVSimNode::publishVehicles ( mvsim::VehicleBase veh)
protected

Definition at line 471 of file mvsim_node.cpp.

◆ publishWorldElements()

void MVSimNode::publishWorldElements ( mvsim::WorldElementBase obj)
protected

Definition at line 478 of file mvsim_node.cpp.

◆ spin()

void MVSimNode::spin ( )

Process pending msgs, run real-time simulation, etc.

Definition at line 317 of file mvsim_node.cpp.

◆ spinNotifyROS()

void MVSimNode::spinNotifyROS ( )
protected

Publish everything to be published at each simulation iteration

Definition at line 787 of file mvsim_node.cpp.

◆ terminateSimulation()

void MVSimNode::terminateSimulation ( )

Definition at line 283 of file mvsim_node.cpp.

◆ thread_update_GUI()

void MVSimNode::thread_update_GUI ( TThreadParams thread_params)
staticprotected

Definition at line 430 of file mvsim_node.cpp.

◆ vehVarName()

std::string MVSimNode::vehVarName ( const std::string &  sVarName,
const mvsim::VehicleBase veh 
) const
protected

Creates the string "/<VEH_NAME>/<VAR_NAME>" if there're more than one vehicle in the World, or "/<VAR_NAME>" otherwise.

Definition at line 1013 of file mvsim_node.cpp.

Member Data Documentation

◆ base_last_cmd_

ros_Time MVSimNode::base_last_cmd_
protected

received a vel_cmd (for watchdog)

Definition at line 285 of file mvsim_node_core.h.

◆ base_watchdog_timeout_

ros_Duration MVSimNode::base_watchdog_timeout_ = ros_Duration(1, 0)
protected

Definition at line 286 of file mvsim_node_core.h.

◆ clock_

rclcpp::Clock::SharedPtr MVSimNode::clock_
protected

Definition at line 190 of file mvsim_node_core.h.

◆ do_fake_localization_

bool MVSimNode::do_fake_localization_ = true

Default=true. Behaves as navigation/fake_localization for each vehicle without the need to launch additional nodes.

Definition at line 167 of file mvsim_node_core.h.

◆ force_publish_vehicle_namespace_

bool MVSimNode::force_publish_vehicle_namespace_ = false
protected

If true, vehicle namespaces will be used even if there is only one vehicle:

Definition at line 313 of file mvsim_node_core.h.

◆ gui_key_events_

mvsim::World::TGUIKeyEvent MVSimNode::gui_key_events_
protected

Definition at line 324 of file mvsim_node_core.h.

◆ gui_refresh_period_ms_

int MVSimNode::gui_refresh_period_ms_ = 50

Definition at line 162 of file mvsim_node_core.h.

◆ headless_

bool MVSimNode::headless_ = false

Definition at line 163 of file mvsim_node_core.h.

◆ lastCmdVelTimestamp_

std::map<mvsim::VehicleBase*, double> MVSimNode::lastCmdVelTimestamp_
protected

Definition at line 320 of file mvsim_node_core.h.

◆ msg2gui_

std::string MVSimNode::msg2gui_
protected

Definition at line 325 of file mvsim_node_core.h.

◆ mvsim_world_

The mvsim library simulated world (includes everything: vehicles, obstacles, etc.)

Definition at line 155 of file mvsim_node_core.h.

◆ n_

rclcpp::Node::SharedPtr MVSimNode::n_
protected

Definition at line 182 of file mvsim_node_core.h.

◆ period_ms_publish_tf_

double MVSimNode::period_ms_publish_tf_ = 20
protected

Minimum period between publication of TF transforms & /*‍/odom topics (In ms)

Definition at line 308 of file mvsim_node_core.h.

◆ period_ms_teleop_refresh_

double MVSimNode::period_ms_teleop_refresh_ = 100
protected

Minimum period between update of live info & read of teleop key strokes in GUI (In ms)

Definition at line 317 of file mvsim_node_core.h.

◆ profiler_

mrpt::system::CTimeLogger MVSimNode::profiler_ {true , "mvsim_node"}
protected

Definition at line 344 of file mvsim_node_core.h.

◆ publish_tf_odom2baselink_

bool MVSimNode::publish_tf_odom2baselink_ = true

Definition at line 169 of file mvsim_node_core.h.

◆ publisher_history_len_

int MVSimNode::publisher_history_len_ = 50

Definition at line 171 of file mvsim_node_core.h.

◆ pubsub_vehicles_

std::vector<TPubSubPerVehicle> MVSimNode::pubsub_vehicles_
protected

Pubs/Subs for each vehicle. Initialized by initPubSubs(), called from notifyROSWorldIsUpdated()

Definition at line 268 of file mvsim_node_core.h.

◆ pubsub_vehicles_mtx_

std::mutex MVSimNode::pubsub_vehicles_mtx_
protected

Definition at line 269 of file mvsim_node_core.h.

◆ realtime_factor_

double MVSimNode::realtime_factor_ = 1.0

(Defaul=1.0) >1: speed-up, <1: slow-down

Definition at line 161 of file mvsim_node_core.h.

◆ realtime_tictac_

mrpt::system::CTicTac MVSimNode::realtime_tictac_
protected

Definition at line 299 of file mvsim_node_core.h.

◆ ros_publisher_workers_

mrpt::WorkerThreadsPool MVSimNode::ros_publisher_workers_
Initial value:
{
4 , mrpt::WorkerThreadsPool::POLICY_FIFO}

Definition at line 157 of file mvsim_node_core.h.

◆ t_old_

double MVSimNode::t_old_ = -1
protected

Definition at line 301 of file mvsim_node_core.h.

◆ teleop_idx_veh_

size_t MVSimNode::teleop_idx_veh_ = 0
protected

for teleoperation from the GUI (selects the focused" vehicle)

Definition at line 323 of file mvsim_node_core.h.

◆ tfIdentity_

const tf2::Transform MVSimNode::tfIdentity_ = tf2::Transform::getIdentity()
protected

Unit transform (const, once)

Definition at line 289 of file mvsim_node_core.h.

◆ thGUI_

std::thread MVSimNode::thGUI_
protected

Definition at line 327 of file mvsim_node_core.h.

◆ thread_params_

TThreadParams MVSimNode::thread_params_
protected

Definition at line 298 of file mvsim_node_core.h.

◆ tim_publish_tf_

mrpt::system::CTicTac MVSimNode::tim_publish_tf_
protected

Definition at line 310 of file mvsim_node_core.h.

◆ tim_teleop_refresh_

mrpt::system::CTicTac MVSimNode::tim_teleop_refresh_
protected

Definition at line 318 of file mvsim_node_core.h.

◆ ts_

rclcpp::TimeSource MVSimNode::ts_ {n_}
protected

Definition at line 189 of file mvsim_node_core.h.

◆ world_init_ok_

bool MVSimNode::world_init_ok_ = false
protected

will be true after a success call to loadWorldModel()

Definition at line 304 of file mvsim_node_core.h.

◆ worldPubs_

WorldPubs MVSimNode::worldPubs_
protected

Definition at line 207 of file mvsim_node_core.h.


The documentation for this class was generated from the following files:


mvsim
Author(s):
autogenerated on Wed May 28 2025 02:13:09