Classes | Public Member Functions | Private Types | Private Member Functions | Static Private Member Functions | Private Attributes | Friends | List of all members
mvsim::World Class Reference

#include <World.h>

Inheritance diagram for mvsim::World:
Inheritance graph
[legend]

Classes

struct  GeoreferenceOptions
 
struct  GUI
 
struct  LightOptions
 
struct  lut_2d_coordinates_t
 
struct  LutIndexHash
 
struct  TFixturePtr
 
struct  TGUI_Options
 
struct  TGUIKeyEvent
 
struct  TInfoPerCollidableobj
 
struct  TUpdateGUIParams
 
struct  XmlParserContext
 

Public Types

Public types
using VehicleList = std::multimap< std::string, VehicleBase::Ptr >
 
using WorldElementList = std::list< WorldElementBase::Ptr >
 
using BlockList = std::multimap< std::string, Block::Ptr >
 
using SimulableList = std::multimap< std::string, Simulable::Ptr >
 

Public Member Functions

mrpt::math::TPose3D applyWorldRenderOffset (mrpt::math::TPose3D p) const
 
mrpt::poses::CPose3D applyWorldRenderOffset (mrpt::poses::CPose3D p) const
 
float collisionThreshold () const
 
void connectToServer ()
 
bool evaluate_tag_if (const rapidxml::xml_node< char > &node) const
 
void free_opengl_resources ()
 
const GeoreferenceOptionsgeoreferenceOptions () const
 
std::set< float > getElevationsAt (const mrpt::math::TPoint2D &worldXY) const
 
float getHighestElevationUnder (const mrpt::math::TPoint3Df &queryPt) const
 
std::optional< mvsim::TJoyStickEventgetJoystickState () const
 
bool headless () const
 
void headless (bool setHeadless)
 
void internal_simul_pre_step_terrain_elevation ()
 
auto & physical_objects_mtx ()
 
bool sensor_has_to_create_egl_context ()
 
const std::map< std::string, std::string > & user_defined_variables () const
 
mrpt::math::TVector3D worldRenderOffset () const
 (See docs for worldRenderOffset_) More...
 
void worldRenderOffsetPropose (const mrpt::math::TVector3D &v)
 (See docs for worldRenderOffset_) More...
 
Initialization, simulation set-up
 World ()
 Default ctor: inits an empty world. More...
 
 ~World ()
 Dtor. More...
 
void clear_all ()
 
void load_from_XML_file (const std::string &xmlFileNamePath)
 
void internal_initialize ()
 
void load_from_XML (const std::string &xml_text, const std::string &fileNameForPath=std::string("."))
 
Access inner working objects
std::unique_ptr< b2World > & getBox2DWorld ()
 
const std::unique_ptr< b2World > & getBox2DWorld () const
 
b2BodygetBox2DGroundBody ()
 
const VehicleListgetListOfVehicles () const
 
VehicleListgetListOfVehicles ()
 
const BlockListgetListOfBlocks () const
 
BlockListgetListOfBlocks ()
 
const WorldElementListgetListOfWorldElements () const
 
SimulableListgetListOfSimulableObjects ()
 Always lock/unlock getListOfSimulableObjectsMtx() before using this: More...
 
const SimulableListgetListOfSimulableObjects () const
 
auto & getListOfSimulableObjectsMtx ()
 
mrpt::system::CTimeLogger & getTimeLogger ()
 
std::string local_to_abs_path (const std::string &in_path) const
 
std::string xmlPathToActualPath (const std::string &modelURI) const
 

Private Types

using LUTCache = std::unordered_map< lut_2d_coordinates_t, std::vector< Simulable::Ptr >, LutIndexHash >
 
using xml_tag_parser_function_t = std::function< void(const XmlParserContext &)>
 

Private Member Functions

const LUTCachegetLUTCacheOfObjects () const
 Ensure the cache is built and up-to-date, then return it: More...
 
void insertBlock (const Block::Ptr &block)
 
void internal_advertiseServices ()
 
mrpt::math::TPoint2D internal_gui_on_image (const std::string &label, const mrpt::img::CImage &im, int winPosX)
 
void internal_gui_on_observation (const Simulable &veh, const mrpt::obs::CObservation::Ptr &obs)
 
void internal_gui_on_observation_3Dscan (const Simulable &veh, const std::shared_ptr< mrpt::obs::CObservation3DRangeScan > &obs)
 
void internal_gui_on_observation_image (const Simulable &veh, const std::shared_ptr< mrpt::obs::CObservationImage > &obs)
 
void internal_one_timestep (double dt)
 
void internal_recursive_parse_XML (const XmlParserContext &ctx)
 This will parse a main XML file, or its included. More...
 
void internal_update_lut_cache () const
 
void internalOnObservation (const Simulable &veh, const mrpt::obs::CObservation::Ptr &obs)
 
void internalPostSimulStepForRawlog ()
 
void internalPostSimulStepForTrajectory ()
 
void parse_tag_block (const XmlParserContext &ctx)
 
void parse_tag_block_class (const XmlParserContext &ctx)
 
void parse_tag_element (const XmlParserContext &ctx)
 <element> More...
 
void parse_tag_for (const XmlParserContext &ctx)
 
void parse_tag_georeference (const XmlParserContext &ctx)
 
void parse_tag_gui (const XmlParserContext &ctx)
 
void parse_tag_if (const XmlParserContext &ctx)
 
void parse_tag_include (const XmlParserContext &ctx)
 
void parse_tag_lights (const XmlParserContext &ctx)
 
void parse_tag_marker (const XmlParserContext &ctx)
 
void parse_tag_sensor (const XmlParserContext &ctx)
 <sensor> More...
 
void parse_tag_variable (const XmlParserContext &ctx)
 
void parse_tag_vehicle (const XmlParserContext &ctx)
 <vehicle> More...
 
void parse_tag_vehicle_class (const XmlParserContext &ctx)
 
void parse_tag_walls (const XmlParserContext &ctx)
 
void process_load_walls (const rapidxml::xml_node< char > &node)
 
void register_standard_xml_tag_parsers ()
 
void register_tag_parser (const std::string &xmlTagName, const xml_tag_parser_function_t &f)
 
void register_tag_parser (const std::string &xmlTagName, void(World::*f)(const XmlParserContext &ctx))
 
void setLightDirectionFromAzimuthElevation (const float azimuth, const float elevation)
 

Static Private Member Functions

static lut_2d_coordinates_t xy_to_lut_coords (const mrpt::math::TPoint2Df &p)
 

Private Attributes

b2Bodyb2_ground_body_ = nullptr
 
int b2dPosIters_ = 3
 
int b2dVelIters_ = 8
 
std::string basePath_ {"."}
 
BlockList blocks_
 
std::unique_ptr< b2Worldbox2d_world_
 
std::vector< on_observation_callback_tcallbacksOnObservation_
 
float collisionThreshold_ = 0.03f
 
std::recursive_mutex copy_of_objects_dynstate_mtx_
 
std::map< std::string, mrpt::math::TPose3D > copy_of_objects_dynstate_pose_
 Updated in internal_one_step() More...
 
std::map< std::string, mrpt::math::TTwist2D > copy_of_objects_dynstate_twist_
 
std::set< std::string > copy_of_objects_had_collision_
 
GeoreferenceOptions georeferenceOptions_
 
mrpt::opengl::CSetOfObjects::Ptr glUserObjsPhysical_ = mrpt::opengl::CSetOfObjects::Create()
 
mrpt::opengl::CSetOfObjects::Ptr glUserObjsViz_ = mrpt::opengl::CSetOfObjects::Create()
 
double gravity_ = 9.81
 
double ground_truth_rate_ = 50.0
 In Hz. More...
 
std::mutex gt_io_mtx_
 
std::map< std::string, std::fstream > gt_io_per_veh_
 
std::optional< double > gt_last_time_
 
GUI gui_ {*this}
 gui state More...
 
std::map< std::string, nanogui::Window * > guiObsViz_
 by sensorLabel More...
 
TGUI_Options guiOptions_
 
bool initialized_ = false
 
std::optional< Joystickjoystick_
 
bool joystickEnabled_ = false
 
LightOptions lightOptions_
 
LUTCache lut2d_objects_
 
bool lut2d_objects_is_up_to_date_ = false
 
double max_slope_to_collide_ = 0.30
 
double min_slope_to_collide_ = -0.50
 
std::vector< std::optional< TInfoPerCollidableobj > > obstacles_for_each_obj_
 
const TParameterDefinitions otherWorldParams_
 
std::mutex rawlog_io_mtx_
 
std::map< std::string, std::shared_ptr< mrpt::io::CFileGZOutputStream > > rawlog_io_per_veh_
 
std::optional< double > rawlog_last_odom_time_
 
double rawlog_odometry_rate_ = 10.0
 In Hz. More...
 
RemoteResourcesManager remoteResources_
 
std::set< std::string > reset_collision_flags_
 
std::mutex reset_collision_flags_mtx_
 
std::string save_ground_truth_trajectory_
 
std::string save_to_rawlog_
 
std::string serverAddress_ = "localhost"
 
std::optional< double > simul_start_wallclock_time_
 
std::mutex simul_time_mtx_
 
SimulableList simulableObjects_
 
std::mutex simulableObjectsMtx_
 
std::mutex simulationStepRunningMtx_
 
double simulTime_ = 0
 
double simulTimestep_ = 0
 
mrpt::system::CTicTac timer_iteration_
 
mrpt::system::CTimeLogger timlogger_ {true , "mvsim::World"}
 
std::map< std::string, std::string > userDefinedVariables_
 
VehicleList vehicles_
 
std::recursive_mutex world_cs_
 
WorldElementList worldElements_
 
mrpt::opengl::COpenGLScene worldPhysical_
 
std::recursive_mutex worldPhysicalMtx_
 
std::optional< mrpt::math::TVector3D > worldRenderOffset_
 
mrpt::opengl::COpenGLScene::Ptr worldVisual_ = mrpt::opengl::COpenGLScene::Create()
 
std::map< std::string, xml_tag_parser_function_txmlParsers_
 

Friends

class Block
 
class VehicleBase
 

Simulation execution

mrpt::opengl::CSetOfObjects::Ptr guiUserObjectsPhysical_
 
mrpt::opengl::CSetOfObjects::Ptr guiUserObjectsViz_
 
std::mutex guiUserObjectsMtx_
 
std::mutex pendingRunSensorsOn3DSceneMtx_
 
bool pendingRunSensorsOn3DScene_ = false
 
std::string guiMsgLines_
 
std::mutex guiMsgLinesMtx_
 
std::thread gui_thread_
 
std::atomic_bool gui_thread_running_ = false
 
std::atomic_bool simulator_must_close_ = false
 
std::mutex gui_thread_start_mtx_
 
std::vector< std::function< void(void)> > guiUserPendingTasks_
 
std::mutex guiUserPendingTasksMtx_
 
TGUIKeyEvent lastKeyEvent_
 
std::atomic_bool lastKeyEventValid_ = false
 
std::mutex lastKeyEventMtx_
 
double get_simul_time () const
 
void force_set_simul_time (double newSimulatedTime)
 Normally should not be called by users, for internal use only. More...
 
mrpt::Clock::time_point get_simul_timestamp () const
 
double get_simul_timestep () const
 Simulation fixed-time interval for numerical integration. More...
 
void set_simul_timestep (double timestep)
 
double get_gravity () const
 
void set_gravity (double accel)
 
void run_simulation (double dt)
 
void update_GUI (TUpdateGUIParams *params=nullptr)
 
const mrpt::gui::CDisplayWindowGUI::Ptr & gui_window () const
 
const mrpt::math::TPoint3D & gui_mouse_point () const
 
void internalGraphicsLoopTasksForSimulation ()
 
void internalRunSensorsOn3DScene (mrpt::opengl::COpenGLScene &physicalObjects)
 
void internalUpdate3DSceneObjects (mrpt::opengl::COpenGLScene &viz, mrpt::opengl::COpenGLScene &physical)
 
void internal_GUI_thread ()
 
void internal_process_pending_gui_user_tasks ()
 
void mark_as_pending_running_sensors_on_3D_scene ()
 
void clear_pending_running_sensors_on_3D_scene ()
 
bool pending_running_sensors_on_3D_scene ()
 
bool simulator_must_close () const
 
void simulator_must_close (bool value)
 
void enqueue_task_to_run_in_gui_thread (const std::function< void(void)> &f)
 
bool is_GUI_open () const
 
void close_GUI ()
 a previous call to update_GUI() More...
 

Visitors API

using vehicle_visitor_t = std::function< void(VehicleBase &)>
 
using world_element_visitor_t = std::function< void(WorldElementBase &)>
 
using block_visitor_t = std::function< void(Block &)>
 
void runVisitorOnVehicles (const vehicle_visitor_t &v)
 
void runVisitorOnWorldElements (const world_element_visitor_t &v)
 
void runVisitorOnBlocks (const block_visitor_t &v)
 

Optional user hooks

using on_observation_callback_t = std::function< void(const Simulable &, const mrpt::obs::CObservation::Ptr &)>
 
void registerCallbackOnObservation (const on_observation_callback_t &f)
 
void dispatchOnObservation (const Simulable &veh, const mrpt::obs::CObservation::Ptr &obs)
 

Detailed Description

Simulation happens inside a World object. This is the central class for usage from user code, running the simulation, loading XML models, managing GUI visualization, etc. The ROS node acts as a bridge between this class and the ROS subsystem.

See: https://mvsimulator.readthedocs.io/en/latest/world.html

Definition at line 82 of file World.h.

Member Typedef Documentation

◆ block_visitor_t

using mvsim::World::block_visitor_t = std::function<void(Block&)>

Definition at line 343 of file World.h.

◆ BlockList

using mvsim::World::BlockList = std::multimap<std::string, Block::Ptr>

Map 'block-name' => block object. See getListOfBlocks()

Definition at line 297 of file World.h.

◆ LUTCache

using mvsim::World::LUTCache = std::unordered_map<lut_2d_coordinates_t, std::vector<Simulable::Ptr>, LutIndexHash>
private

Definition at line 719 of file World.h.

◆ on_observation_callback_t

using mvsim::World::on_observation_callback_t = std::function<void(const Simulable& , const mrpt::obs::CObservation::Ptr& )>

Definition at line 360 of file World.h.

◆ SimulableList

using mvsim::World::SimulableList = std::multimap<std::string, Simulable::Ptr>

For convenience, all elements (vehicles, world elements, blocks) are also stored here for each look-up by name

Definition at line 301 of file World.h.

◆ vehicle_visitor_t

Definition at line 341 of file World.h.

◆ VehicleList

using mvsim::World::VehicleList = std::multimap<std::string, VehicleBase::Ptr>

Map 'vehicle-name' => vehicle object. See getListOfVehicles()

Definition at line 291 of file World.h.

◆ world_element_visitor_t

Definition at line 342 of file World.h.

◆ WorldElementList

See getListOfWorldElements()

Definition at line 294 of file World.h.

◆ xml_tag_parser_function_t

using mvsim::World::xml_tag_parser_function_t = std::function<void(const XmlParserContext&)>
private

Definition at line 830 of file World.h.

Constructor & Destructor Documentation

◆ World()

World::World ( )

Default ctor: inits an empty world.

Definition at line 24 of file World.cpp.

◆ ~World()

World::~World ( )

Dtor.

Definition at line 27 of file World.cpp.

Member Function Documentation

◆ applyWorldRenderOffset() [1/2]

mrpt::math::TPose3D mvsim::World::applyWorldRenderOffset ( mrpt::math::TPose3D  p) const
inline

Definition at line 631 of file World.h.

◆ applyWorldRenderOffset() [2/2]

mrpt::poses::CPose3D mvsim::World::applyWorldRenderOffset ( mrpt::poses::CPose3D  p) const
inline

Definition at line 639 of file World.h.

◆ clear_all()

void World::clear_all ( )

Resets the entire simulation environment to an empty world.

Definition at line 46 of file World.cpp.

◆ clear_pending_running_sensors_on_3D_scene()

void mvsim::World::clear_pending_running_sensors_on_3D_scene ( )
inline

Definition at line 229 of file World.h.

◆ close_GUI()

void World::close_GUI ( )

a previous call to update_GUI()

Forces closing the GUI window, if any.

Definition at line 50 of file World_gui.cpp.

◆ collisionThreshold()

float mvsim::World::collisionThreshold ( ) const
inline

Definition at line 403 of file World.h.

◆ connectToServer()

void World::connectToServer ( )

Connect to server, advertise topics and services, etc. per the world description loaded from XML file.

Definition at line 143 of file World.cpp.

◆ dispatchOnObservation()

void World::dispatchOnObservation ( const Simulable veh,
const mrpt::obs::CObservation::Ptr &  obs 
)

Calls all registered callbacks:

Definition at line 243 of file World.cpp.

◆ enqueue_task_to_run_in_gui_thread()

void mvsim::World::enqueue_task_to_run_in_gui_thread ( const std::function< void(void)> &  f)
inline

Definition at line 266 of file World.h.

◆ evaluate_tag_if()

bool World::evaluate_tag_if ( const rapidxml::xml_node< char > &  node) const

Definition at line 357 of file World_load_xml.cpp.

◆ force_set_simul_time()

void mvsim::World::force_set_simul_time ( double  newSimulatedTime)
inline

Normally should not be called by users, for internal use only.

Definition at line 126 of file World.h.

◆ free_opengl_resources()

void World::free_opengl_resources ( )

Definition at line 181 of file World.cpp.

◆ georeferenceOptions()

const GeoreferenceOptions& mvsim::World::georeferenceOptions ( ) const
inline

Definition at line 624 of file World.h.

◆ get_gravity()

double mvsim::World::get_gravity ( ) const
inline

Gravity acceleration (Default=9.8 m/s^2). Used to evaluate weights for friction, etc.

Definition at line 154 of file World.h.

◆ get_simul_time()

double mvsim::World::get_simul_time ( ) const
inline

Seconds since start of simulation.

See also
get_simul_timestamp()

Definition at line 119 of file World.h.

◆ get_simul_timestamp()

mrpt::Clock::time_point mvsim::World::get_simul_timestamp ( ) const
inline

Get the current simulation full timestamp, computed as the real wall clock timestamp at the beginning of the simulation, plus the number of seconds simulation has run.

See also
get_simul_time()

Definition at line 137 of file World.h.

◆ get_simul_timestep()

double World::get_simul_timestep ( ) const

Simulation fixed-time interval for numerical integration.

Definition at line 171 of file World_simul.cpp.

◆ getBox2DGroundBody()

b2Body* mvsim::World::getBox2DGroundBody ( )
inline

Definition at line 309 of file World.h.

◆ getBox2DWorld() [1/2]

std::unique_ptr<b2World>& mvsim::World::getBox2DWorld ( )
inline

Definition at line 307 of file World.h.

◆ getBox2DWorld() [2/2]

const std::unique_ptr<b2World>& mvsim::World::getBox2DWorld ( ) const
inline

Definition at line 308 of file World.h.

◆ getElevationsAt()

std::set< float > World::getElevationsAt ( const mrpt::math::TPoint2D &  worldXY) const

Returns the list of "z" coordinate or "elevations" for all simulable objects at a given world-frame 2D coordinates (x,y). If no object reports any height, the value "0.0" will be always reported by default. In multistorey worlds, for example, this will return the height of each floor for the queried point.

Definition at line 275 of file World.cpp.

◆ getHighestElevationUnder()

float World::getHighestElevationUnder ( const mrpt::math::TPoint3Df &  queryPt) const

with query points the center of a wheel, this returns the highest "ground" under it, or .0 if nothing found.

Definition at line 307 of file World.cpp.

◆ getJoystickState()

std::optional< mvsim::TJoyStickEvent > World::getJoystickState ( ) const

If joystick usage is enabled (via XML file option, for example), this will read the joystick state and return it. Otherwise (or on device error or disconnection), a null optional variable is returned.

Definition at line 203 of file World.cpp.

◆ getListOfBlocks() [1/2]

BlockList& mvsim::World::getListOfBlocks ( )
inline

Definition at line 313 of file World.h.

◆ getListOfBlocks() [2/2]

const BlockList& mvsim::World::getListOfBlocks ( ) const
inline

Definition at line 312 of file World.h.

◆ getListOfSimulableObjects() [1/2]

SimulableList& mvsim::World::getListOfSimulableObjects ( )
inline

Always lock/unlock getListOfSimulableObjectsMtx() before using this:

Definition at line 317 of file World.h.

◆ getListOfSimulableObjects() [2/2]

const SimulableList& mvsim::World::getListOfSimulableObjects ( ) const
inline

Definition at line 318 of file World.h.

◆ getListOfSimulableObjectsMtx()

auto& mvsim::World::getListOfSimulableObjectsMtx ( )
inline

Definition at line 319 of file World.h.

◆ getListOfVehicles() [1/2]

VehicleList& mvsim::World::getListOfVehicles ( )
inline

Definition at line 311 of file World.h.

◆ getListOfVehicles() [2/2]

const VehicleList& mvsim::World::getListOfVehicles ( ) const
inline

Definition at line 310 of file World.h.

◆ getListOfWorldElements()

const WorldElementList& mvsim::World::getListOfWorldElements ( ) const
inline

Definition at line 314 of file World.h.

◆ getLUTCacheOfObjects()

const World::LUTCache & World::getLUTCacheOfObjects ( ) const
private

Ensure the cache is built and up-to-date, then return it:

Definition at line 582 of file World_simul.cpp.

◆ getTimeLogger()

mrpt::system::CTimeLogger& mvsim::World::getTimeLogger ( )
inline

Definition at line 321 of file World.h.

◆ gui_mouse_point()

const mrpt::math::TPoint3D& mvsim::World::gui_mouse_point ( ) const
inline

Definition at line 198 of file World.h.

◆ gui_window()

const mrpt::gui::CDisplayWindowGUI::Ptr& mvsim::World::gui_window ( ) const
inline

Definition at line 196 of file World.h.

◆ headless() [1/2]

bool mvsim::World::headless ( ) const
inline

Definition at line 385 of file World.h.

◆ headless() [2/2]

void mvsim::World::headless ( bool  setHeadless)
inline

Definition at line 386 of file World.h.

◆ insertBlock()

void World::insertBlock ( const Block::Ptr block)
private

Definition at line 166 of file World.cpp.

◆ internal_advertiseServices()

void World::internal_advertiseServices ( )
private

Definition at line 182 of file World_services.cpp.

◆ internal_gui_on_image()

mrpt::math::TPoint2D World::internal_gui_on_image ( const std::string &  label,
const mrpt::img::CImage &  im,
int  winPosX 
)
private

Definition at line 1127 of file World_gui.cpp.

◆ internal_gui_on_observation()

void World::internal_gui_on_observation ( const Simulable veh,
const mrpt::obs::CObservation::Ptr &  obs 
)
private

Definition at line 1071 of file World_gui.cpp.

◆ internal_gui_on_observation_3Dscan()

void World::internal_gui_on_observation_3Dscan ( const Simulable veh,
const std::shared_ptr< mrpt::obs::CObservation3DRangeScan > &  obs 
)
private

Definition at line 1086 of file World_gui.cpp.

◆ internal_gui_on_observation_image()

void World::internal_gui_on_observation_image ( const Simulable veh,
const std::shared_ptr< mrpt::obs::CObservationImage > &  obs 
)
private

Definition at line 1114 of file World_gui.cpp.

◆ internal_GUI_thread()

void World::internal_GUI_thread ( )

Definition at line 603 of file World_gui.cpp.

◆ internal_initialize()

void World::internal_initialize ( )

Definition at line 68 of file World.cpp.

◆ internal_one_timestep()

void World::internal_one_timestep ( double  dt)
private

Runs one individual time step

Definition at line 66 of file World_simul.cpp.

◆ internal_process_pending_gui_user_tasks()

void World::internal_process_pending_gui_user_tasks ( )

Definition at line 901 of file World_gui.cpp.

◆ internal_recursive_parse_XML()

void World::internal_recursive_parse_XML ( const XmlParserContext ctx)
private

This will parse a main XML file, or its included.

Definition at line 126 of file World_load_xml.cpp.

◆ internal_simul_pre_step_terrain_elevation()

void World::internal_simul_pre_step_terrain_elevation ( )

Definition at line 308 of file World_simul.cpp.

◆ internal_update_lut_cache()

void World::internal_update_lut_cache ( ) const
private

Definition at line 598 of file World_simul.cpp.

◆ internalGraphicsLoopTasksForSimulation()

void World::internalGraphicsLoopTasksForSimulation ( )

Update 3D vehicles, sensors, run render-based sensors, etc: Called from World_gui thread in normal mode, or mvsim-cli in headless mode.

Definition at line 1175 of file World_gui.cpp.

◆ internalOnObservation()

void World::internalOnObservation ( const Simulable veh,
const mrpt::obs::CObservation::Ptr &  obs 
)
private

Definition at line 249 of file World.cpp.

◆ internalPostSimulStepForRawlog()

void World::internalPostSimulStepForRawlog ( )
private

Definition at line 226 of file World_simul.cpp.

◆ internalPostSimulStepForTrajectory()

void World::internalPostSimulStepForTrajectory ( )
private

Definition at line 259 of file World_simul.cpp.

◆ internalRunSensorsOn3DScene()

void World::internalRunSensorsOn3DScene ( mrpt::opengl::COpenGLScene &  physicalObjects)

Definition at line 914 of file World_gui.cpp.

◆ internalUpdate3DSceneObjects()

void World::internalUpdate3DSceneObjects ( mrpt::opengl::COpenGLScene &  viz,
mrpt::opengl::COpenGLScene &  physical 
)

Definition at line 926 of file World_gui.cpp.

◆ is_GUI_open()

bool mvsim::World::is_GUI_open ( ) const

Return true if the GUI window is open, after

◆ load_from_XML()

void World::load_from_XML ( const std::string &  xml_text,
const std::string &  fileNameForPath = std::string(".") 
)

Load an entire world description into this object from a specification in XML format.

Parameters
[in]fileNameForPathOptionally, provide the full path to an XML file from which to take relative paths.
Exceptions
std::exceptionOn any error, with what() giving a descriptive error message

Definition at line 37 of file World_load_xml.cpp.

◆ load_from_XML_file()

void World::load_from_XML_file ( const std::string &  xmlFileNamePath)

Load an entire world description into this object from a specification in XML format.

Parameters
[in]xmlFileNamePathThe relative or full path to the XML file.
Exceptions
std::exceptionOn any error, with what() giving a descriptive error message

Definition at line 31 of file World_load_xml.cpp.

◆ local_to_abs_path()

std::string World::local_to_abs_path ( const std::string &  s_in) const

Replace macros, prefix the base_path if input filename is relative, etc.

See also
xmlPathToActualPath

Replace macros, prefix the base_path if input filename is relative, etc.

Definition at line 106 of file World.cpp.

◆ mark_as_pending_running_sensors_on_3D_scene()

void mvsim::World::mark_as_pending_running_sensors_on_3D_scene ( )
inline

Definition at line 223 of file World.h.

◆ parse_tag_block()

void World::parse_tag_block ( const XmlParserContext ctx)
private

Definition at line 208 of file World_load_xml.cpp.

◆ parse_tag_block_class()

void World::parse_tag_block_class ( const XmlParserContext ctx)
private

Definition at line 216 of file World_load_xml.cpp.

◆ parse_tag_element()

void World::parse_tag_element ( const XmlParserContext ctx)
private

<element>

Definition at line 162 of file World_load_xml.cpp.

◆ parse_tag_for()

void World::parse_tag_for ( const XmlParserContext ctx)
private

Definition at line 311 of file World_load_xml.cpp.

◆ parse_tag_georeference()

void World::parse_tag_georeference ( const XmlParserContext ctx)
private

Definition at line 233 of file World_load_xml.cpp.

◆ parse_tag_gui()

void World::parse_tag_gui ( const XmlParserContext ctx)
private

Definition at line 222 of file World_load_xml.cpp.

◆ parse_tag_if()

void World::parse_tag_if ( const XmlParserContext ctx)
private

Definition at line 346 of file World_load_xml.cpp.

◆ parse_tag_include()

void World::parse_tag_include ( const XmlParserContext ctx)
private

Definition at line 259 of file World_load_xml.cpp.

◆ parse_tag_lights()

void World::parse_tag_lights ( const XmlParserContext ctx)
private

Definition at line 228 of file World_load_xml.cpp.

◆ parse_tag_marker()

void World::parse_tag_marker ( const XmlParserContext ctx)
private

Definition at line 376 of file World_load_xml.cpp.

◆ parse_tag_sensor()

void World::parse_tag_sensor ( const XmlParserContext ctx)
private

<sensor>

Definition at line 195 of file World_load_xml.cpp.

◆ parse_tag_variable()

void World::parse_tag_variable ( const XmlParserContext ctx)
private

Definition at line 287 of file World_load_xml.cpp.

◆ parse_tag_vehicle()

void World::parse_tag_vehicle ( const XmlParserContext ctx)
private

<vehicle>

Definition at line 172 of file World_load_xml.cpp.

◆ parse_tag_vehicle_class()

void World::parse_tag_vehicle_class ( const XmlParserContext ctx)
private

<vehicle:class>

Definition at line 189 of file World_load_xml.cpp.

◆ parse_tag_walls()

void World::parse_tag_walls ( const XmlParserContext ctx)
private

Definition at line 257 of file World_load_xml.cpp.

◆ pending_running_sensors_on_3D_scene()

bool mvsim::World::pending_running_sensors_on_3D_scene ( )
inline

Definition at line 235 of file World.h.

◆ physical_objects_mtx()

auto& mvsim::World::physical_objects_mtx ( )
inline

Definition at line 383 of file World.h.

◆ process_load_walls()

void World::process_load_walls ( const rapidxml::xml_node< char > &  node)
private

Definition at line 120 of file World_walls.cpp.

◆ register_standard_xml_tag_parsers()

void World::register_standard_xml_tag_parsers ( )
private

Definition at line 103 of file World_load_xml.cpp.

◆ register_tag_parser() [1/2]

void mvsim::World::register_tag_parser ( const std::string &  xmlTagName,
const xml_tag_parser_function_t f 
)
inlineprivate

Definition at line 836 of file World.h.

◆ register_tag_parser() [2/2]

void mvsim::World::register_tag_parser ( const std::string &  xmlTagName,
void(World::*)(const XmlParserContext &ctx)  f 
)
inlineprivate

Definition at line 840 of file World.h.

◆ registerCallbackOnObservation()

void mvsim::World::registerCallbackOnObservation ( const on_observation_callback_t f)
inline

Definition at line 362 of file World.h.

◆ run_simulation()

void World::run_simulation ( double  dt)

Runs the simulation for a given time interval (in seconds)

Note
The minimum simulation time is the timestep set (e.g. via set_simul_timestep()), even if time advanced further than the provided "dt".

Runs the simulation for a given time interval (in seconds)

Definition at line 25 of file World_simul.cpp.

◆ runVisitorOnBlocks()

void World::runVisitorOnBlocks ( const block_visitor_t v)

Run the user-provided visitor on each world block

Definition at line 137 of file World.cpp.

◆ runVisitorOnVehicles()

void World::runVisitorOnVehicles ( const vehicle_visitor_t v)

Run the user-provided visitor on each vehicle

Definition at line 125 of file World.cpp.

◆ runVisitorOnWorldElements()

void World::runVisitorOnWorldElements ( const world_element_visitor_t v)

Run the user-provided visitor on each world element

Definition at line 131 of file World.cpp.

◆ sensor_has_to_create_egl_context()

bool World::sensor_has_to_create_egl_context ( )

Definition at line 191 of file World.cpp.

◆ set_gravity()

void mvsim::World::set_gravity ( double  accel)
inline

Gravity acceleration (Default=9.8 m/s^2). Used to evaluate weights for friction, etc.

Definition at line 158 of file World.h.

◆ set_simul_timestep()

void mvsim::World::set_simul_timestep ( double  timestep)
inline

Simulation fixed-time interval for numerical integration 0 means auto-determine as the minimum of 50 ms and the shortest sensor sample period.

Definition at line 150 of file World.h.

◆ setLightDirectionFromAzimuthElevation()

void World::setLightDirectionFromAzimuthElevation ( const float  azimuth,
const float  elevation 
)
private

Changes the light source direction from azimuth and elevation angles (in radians)

Definition at line 1208 of file World_gui.cpp.

◆ simulator_must_close() [1/2]

bool mvsim::World::simulator_must_close ( ) const
inline

Definition at line 252 of file World.h.

◆ simulator_must_close() [2/2]

void mvsim::World::simulator_must_close ( bool  value)
inline

Definition at line 259 of file World.h.

◆ update_GUI()

void World::update_GUI ( TUpdateGUIParams params = nullptr)

Updates (or sets-up upon first call) the GUI visualization of the scene.

Parameters
[in,out]paramsOptional inputs/outputs to the GUI update process. See struct for details.
Note
This method is prepared to be called concurrently with the simulation, and doing so is recommended to assure a smooth multi-threading simulation.

Definition at line 1006 of file World_gui.cpp.

◆ user_defined_variables()

const std::map<std::string, std::string>& mvsim::World::user_defined_variables ( ) const
inline

Definition at line 390 of file World.h.

◆ worldRenderOffset()

mrpt::math::TVector3D mvsim::World::worldRenderOffset ( ) const
inline

(See docs for worldRenderOffset_)

Definition at line 627 of file World.h.

◆ worldRenderOffsetPropose()

void mvsim::World::worldRenderOffsetPropose ( const mrpt::math::TVector3D &  v)
inline

(See docs for worldRenderOffset_)

Definition at line 648 of file World.h.

◆ xmlPathToActualPath()

std::string World::xmlPathToActualPath ( const std::string &  modelURI) const

Parses URIs in all the forms explained in RemoteResourcesManager::resolve_path(), then passes it through local_to_abs_path().

See also
local_to_abs_path

Definition at line 98 of file World.cpp.

◆ xy_to_lut_coords()

World::lut_2d_coordinates_t World::xy_to_lut_coords ( const mrpt::math::TPoint2Df &  p)
staticprivate

Definition at line 589 of file World_simul.cpp.

Friends And Related Function Documentation

◆ Block

friend class Block
friend

Definition at line 420 of file World.h.

◆ VehicleBase

friend class VehicleBase
friend

Definition at line 419 of file World.h.

Member Data Documentation

◆ b2_ground_body_

b2Body* mvsim::World::b2_ground_body_ = nullptr
private

Used to declare friction between vehicles-ground

Definition at line 665 of file World.h.

◆ b2dPosIters_

int mvsim::World::b2dPosIters_ = 3
private

Definition at line 443 of file World.h.

◆ b2dVelIters_

int mvsim::World::b2dVelIters_ = 8
private

Velocity and position iteration count (refer to libbox2d docs)

Definition at line 443 of file World.h.

◆ basePath_

std::string mvsim::World::basePath_ {"."}
private

Path from which to take relative directories.

Definition at line 491 of file World.h.

◆ blocks_

BlockList mvsim::World::blocks_
private

Definition at line 669 of file World.h.

◆ box2d_world_

std::unique_ptr<b2World> mvsim::World::box2d_world_
private

Box2D dynamic simulator instance

Definition at line 662 of file World.h.

◆ callbacksOnObservation_

std::vector<on_observation_callback_t> mvsim::World::callbacksOnObservation_
private

Definition at line 426 of file World.h.

◆ collisionThreshold_

float mvsim::World::collisionThreshold_ = 0.03f
private

Distance between two body edges to be considered a collision.

Definition at line 446 of file World.h.

◆ copy_of_objects_dynstate_mtx_

std::recursive_mutex mvsim::World::copy_of_objects_dynstate_mtx_
private

Definition at line 788 of file World.h.

◆ copy_of_objects_dynstate_pose_

std::map<std::string, mrpt::math::TPose3D> mvsim::World::copy_of_objects_dynstate_pose_
private

Updated in internal_one_step()

Definition at line 785 of file World.h.

◆ copy_of_objects_dynstate_twist_

std::map<std::string, mrpt::math::TTwist2D> mvsim::World::copy_of_objects_dynstate_twist_
private

Definition at line 786 of file World.h.

◆ copy_of_objects_had_collision_

std::set<std::string> mvsim::World::copy_of_objects_had_collision_
private

Definition at line 787 of file World.h.

◆ georeferenceOptions_

GeoreferenceOptions mvsim::World::georeferenceOptions_
private

Options for lights

Definition at line 655 of file World.h.

◆ glUserObjsPhysical_

mrpt::opengl::CSetOfObjects::Ptr mvsim::World::glUserObjsPhysical_ = mrpt::opengl::CSetOfObjects::Create()
private

This private container will be filled with objects in the public gui_user_objects_

Definition at line 495 of file World.h.

◆ glUserObjsViz_

mrpt::opengl::CSetOfObjects::Ptr mvsim::World::glUserObjsViz_ = mrpt::opengl::CSetOfObjects::Create()
private

Definition at line 496 of file World.h.

◆ gravity_

double mvsim::World::gravity_ = 9.81
private

Gravity acceleration (Default=9.81 m/s^2). Used to evaluate weights for friction, etc.

Definition at line 431 of file World.h.

◆ ground_truth_rate_

double mvsim::World::ground_truth_rate_ = 50.0
private

In Hz.

Definition at line 459 of file World.h.

◆ gt_io_mtx_

std::mutex mvsim::World::gt_io_mtx_
private

Definition at line 878 of file World.h.

◆ gt_io_per_veh_

std::map<std::string, std::fstream> mvsim::World::gt_io_per_veh_
private

Definition at line 879 of file World.h.

◆ gt_last_time_

std::optional<double> mvsim::World::gt_last_time_
private

Definition at line 880 of file World.h.

◆ gui_

GUI mvsim::World::gui_ {*this}
private

gui state

Definition at line 762 of file World.h.

◆ gui_thread_

std::thread mvsim::World::gui_thread_

Definition at line 246 of file World.h.

◆ gui_thread_running_

std::atomic_bool mvsim::World::gui_thread_running_ = false

Definition at line 248 of file World.h.

◆ gui_thread_start_mtx_

std::mutex mvsim::World::gui_thread_start_mtx_
mutable

Definition at line 250 of file World.h.

◆ guiMsgLines_

std::string mvsim::World::guiMsgLines_

Definition at line 243 of file World.h.

◆ guiMsgLinesMtx_

std::mutex mvsim::World::guiMsgLinesMtx_

Definition at line 244 of file World.h.

◆ guiObsViz_

std::map<std::string, nanogui::Window*> mvsim::World::guiObsViz_
private

by sensorLabel

Definition at line 802 of file World.h.

◆ guiOptions_

TGUI_Options mvsim::World::guiOptions_
private

Some of these options are only used the first time the GUI window is created.

Definition at line 544 of file World.h.

◆ guiUserObjectsMtx_

std::mutex mvsim::World::guiUserObjectsMtx_

Definition at line 206 of file World.h.

◆ guiUserObjectsPhysical_

mrpt::opengl::CSetOfObjects::Ptr mvsim::World::guiUserObjectsPhysical_

If !=null, a set of objects to be rendered merged with the default visualization. Lock the mutex gui_user_objects_mtx_ while writing. There are two sets of objects: "viz" for visualization only, "physical" for objects which should be detected by sensors.

Definition at line 205 of file World.h.

◆ guiUserObjectsViz_

mrpt::opengl::CSetOfObjects::Ptr mvsim::World::guiUserObjectsViz_

Definition at line 205 of file World.h.

◆ guiUserPendingTasks_

std::vector<std::function<void(void)> > mvsim::World::guiUserPendingTasks_

Definition at line 273 of file World.h.

◆ guiUserPendingTasksMtx_

std::mutex mvsim::World::guiUserPendingTasksMtx_

Definition at line 274 of file World.h.

◆ initialized_

bool mvsim::World::initialized_ = false
private

Definition at line 671 of file World.h.

◆ joystick_

std::optional<Joystick> mvsim::World::joystick_
mutableprivate

Definition at line 440 of file World.h.

◆ joystickEnabled_

bool mvsim::World::joystickEnabled_ = false
mutableprivate

Definition at line 439 of file World.h.

◆ lastKeyEvent_

TGUIKeyEvent mvsim::World::lastKeyEvent_

Definition at line 276 of file World.h.

◆ lastKeyEventMtx_

std::mutex mvsim::World::lastKeyEventMtx_

Definition at line 278 of file World.h.

◆ lastKeyEventValid_

std::atomic_bool mvsim::World::lastKeyEventValid_ = false

Definition at line 277 of file World.h.

◆ lightOptions_

LightOptions mvsim::World::lightOptions_
private

Options for lights

Definition at line 589 of file World.h.

◆ lut2d_objects_

LUTCache mvsim::World::lut2d_objects_
mutableprivate

Definition at line 724 of file World.h.

◆ lut2d_objects_is_up_to_date_

bool mvsim::World::lut2d_objects_is_up_to_date_ = false
mutableprivate

Definition at line 725 of file World.h.

◆ max_slope_to_collide_

double mvsim::World::max_slope_to_collide_ = 0.30
private

Definition at line 461 of file World.h.

◆ min_slope_to_collide_

double mvsim::World::min_slope_to_collide_ = -0.50
private

Definition at line 462 of file World.h.

◆ obstacles_for_each_obj_

std::vector<std::optional<TInfoPerCollidableobj> > mvsim::World::obstacles_for_each_obj_
private

Definition at line 902 of file World.h.

◆ otherWorldParams_

const TParameterDefinitions mvsim::World::otherWorldParams_
private
Initial value:
= {
{"server_address", {"%s", &serverAddress_}},
{"gravity", {"%lf", &gravity_}},
{"simul_timestep", {"%lf", &simulTimestep_}},
{"b2d_vel_iters", {"%i", &b2dVelIters_}},
{"b2d_pos_iters", {"%i", &b2dPosIters_}},
{"collision_threshold", {"%f", &collisionThreshold_}},
{"joystick_enabled", {"%bool", &joystickEnabled_}},
{"save_to_rawlog", {"%s", &save_to_rawlog_}},
{"rawlog_odometry_rate", {"%lf", &rawlog_odometry_rate_}},
{"save_ground_truth_trajectory", {"%s", &save_ground_truth_trajectory_}},
{"ground_truth_rate", {"%lf", &ground_truth_rate_}},
{"max_slope_to_collide", {"%lf", &max_slope_to_collide_}},
{"min_slope_to_collide", {"%lf", &min_slope_to_collide_}},
}

Definition at line 464 of file World.h.

◆ pendingRunSensorsOn3DScene_

bool mvsim::World::pendingRunSensorsOn3DScene_ = false

Definition at line 221 of file World.h.

◆ pendingRunSensorsOn3DSceneMtx_

std::mutex mvsim::World::pendingRunSensorsOn3DSceneMtx_

Definition at line 220 of file World.h.

◆ rawlog_io_mtx_

std::mutex mvsim::World::rawlog_io_mtx_
private

Definition at line 874 of file World.h.

◆ rawlog_io_per_veh_

std::map<std::string, std::shared_ptr<mrpt::io::CFileGZOutputStream> > mvsim::World::rawlog_io_per_veh_
private

Definition at line 875 of file World.h.

◆ rawlog_last_odom_time_

std::optional<double> mvsim::World::rawlog_last_odom_time_
private

Definition at line 876 of file World.h.

◆ rawlog_odometry_rate_

double mvsim::World::rawlog_odometry_rate_ = 10.0
private

In Hz.

Definition at line 453 of file World.h.

◆ remoteResources_

RemoteResourcesManager mvsim::World::remoteResources_
mutableprivate

Definition at line 867 of file World.h.

◆ reset_collision_flags_

std::set<std::string> mvsim::World::reset_collision_flags_
private

Definition at line 790 of file World.h.

◆ reset_collision_flags_mtx_

std::mutex mvsim::World::reset_collision_flags_mtx_
private

Definition at line 791 of file World.h.

◆ save_ground_truth_trajectory_

std::string mvsim::World::save_ground_truth_trajectory_
private

If non-empty, the ground truth trajectory of all vehicles will be saved to a text file in the TUM trajectory format

Definition at line 457 of file World.h.

◆ save_to_rawlog_

std::string mvsim::World::save_to_rawlog_
private

If non-empty, all observations will be saved to a .rawlog

Definition at line 451 of file World.h.

◆ serverAddress_

std::string mvsim::World::serverAddress_ = "localhost"
private

Definition at line 448 of file World.h.

◆ simul_start_wallclock_time_

std::optional<double> mvsim::World::simul_start_wallclock_time_
private

Definition at line 487 of file World.h.

◆ simul_time_mtx_

std::mutex mvsim::World::simul_time_mtx_
private

Definition at line 488 of file World.h.

◆ simulableObjects_

SimulableList mvsim::World::simulableObjects_
private

Definition at line 676 of file World.h.

◆ simulableObjectsMtx_

std::mutex mvsim::World::simulableObjectsMtx_
private

Definition at line 677 of file World.h.

◆ simulationStepRunningMtx_

std::mutex mvsim::World::simulationStepRunningMtx_
private

Definition at line 682 of file World.h.

◆ simulator_must_close_

std::atomic_bool mvsim::World::simulator_must_close_ = false

Definition at line 249 of file World.h.

◆ simulTime_

double mvsim::World::simulTime_ = 0
private

In seconds, real simulation time since beginning (may be different than wall-clock time because of time warp, etc.)

Definition at line 486 of file World.h.

◆ simulTimestep_

double mvsim::World::simulTimestep_ = 0
mutableprivate

Simulation fixed-time interval for numerical integration. 0 means auto-determine as the minimum of 50 ms and the shortest sensor sample period.

Definition at line 437 of file World.h.

◆ timer_iteration_

mrpt::system::CTicTac mvsim::World::timer_iteration_
private

Definition at line 811 of file World.h.

◆ timlogger_

mrpt::system::CTimeLogger mvsim::World::timlogger_ {true , "mvsim::World"}
private

Definition at line 810 of file World.h.

◆ userDefinedVariables_

std::map<std::string, std::string> mvsim::World::userDefinedVariables_
private

User-defined variables as defined via ‘<variable name=’' value='' /> tags in the World xml file, for use within$f{}` expressions

Definition at line 482 of file World.h.

◆ vehicles_

VehicleList mvsim::World::vehicles_
private

Definition at line 667 of file World.h.

◆ world_cs_

std::recursive_mutex mvsim::World::world_cs_
private

Mutex protecting simulation objects from multi-thread access

Definition at line 659 of file World.h.

◆ worldElements_

WorldElementList mvsim::World::worldElements_
private

Definition at line 668 of file World.h.

◆ worldPhysical_

mrpt::opengl::COpenGLScene mvsim::World::worldPhysical_
private

3D scene with all physically observable objects: we will use this scene as input to simulated sensors like cameras, where we don't wont to see visualization marks, etc.

See also
world_visual_

Definition at line 774 of file World.h.

◆ worldPhysicalMtx_

std::recursive_mutex mvsim::World::worldPhysicalMtx_
private

Definition at line 775 of file World.h.

◆ worldRenderOffset_

std::optional<mrpt::math::TVector3D> mvsim::World::worldRenderOffset_
private

World coordinates offset for rendering. Useful mainly to keep numerical accuracy in the OpenGL pipeline (using "floats") when using UTM world coordinates. All coordinates to be send to OpenGL must add this number. It is automatically set via calling worldRenderOffsetPropose() and must be retrieved via worldRenderOffset()

Definition at line 782 of file World.h.

◆ worldVisual_

mrpt::opengl::COpenGLScene::Ptr mvsim::World::worldVisual_ = mrpt::opengl::COpenGLScene::Create()
private

3D scene with all visual objects (vehicles, obstacles, markers, etc.)

See also
worldPhysical_

Definition at line 767 of file World.h.

◆ xmlParsers_

std::map<std::string, xml_tag_parser_function_t> mvsim::World::xmlParsers_
private

Definition at line 832 of file World.h.


The documentation for this class was generated from the following files:
mvsim::World::simulTimestep_
double simulTimestep_
Definition: World.h:437
mvsim::World::max_slope_to_collide_
double max_slope_to_collide_
Definition: World.h:461
mvsim::World::joystickEnabled_
bool joystickEnabled_
Definition: World.h:439
mvsim::World::ground_truth_rate_
double ground_truth_rate_
In Hz.
Definition: World.h:459
mvsim::World::save_to_rawlog_
std::string save_to_rawlog_
Definition: World.h:451
mvsim::World::save_ground_truth_trajectory_
std::string save_ground_truth_trajectory_
Definition: World.h:457
mvsim::World::serverAddress_
std::string serverAddress_
Definition: World.h:448
mvsim::World::b2dPosIters_
int b2dPosIters_
Definition: World.h:443
mvsim::World::min_slope_to_collide_
double min_slope_to_collide_
Definition: World.h:462
mvsim::World::gravity_
double gravity_
Definition: World.h:431
mvsim::World::collisionThreshold_
float collisionThreshold_
Definition: World.h:446
mvsim::World::rawlog_odometry_rate_
double rawlog_odometry_rate_
In Hz.
Definition: World.h:453
mvsim::World::b2dVelIters_
int b2dVelIters_
Definition: World.h:443


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