#include <World.h>
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 GeoreferenceOptions & | georeferenceOptions () const |
std::set< float > | getElevationsAt (const mrpt::math::TPoint2D &worldXY) const |
float | getHighestElevationUnder (const mrpt::math::TPoint3Df &queryPt) const |
std::optional< mvsim::TJoyStickEvent > | getJoystickState () 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 |
b2Body * | getBox2DGroundBody () |
const VehicleList & | getListOfVehicles () const |
VehicleList & | getListOfVehicles () |
const BlockList & | getListOfBlocks () const |
BlockList & | getListOfBlocks () |
const WorldElementList & | getListOfWorldElements () const |
SimulableList & | getListOfSimulableObjects () |
Always lock/unlock getListOfSimulableObjectsMtx() before using this: More... | |
const SimulableList & | getListOfSimulableObjects () 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 &)> |
Static Private Member Functions | |
static lut_2d_coordinates_t | xy_to_lut_coords (const mrpt::math::TPoint2Df &p) |
Private Attributes | |
b2Body * | b2_ground_body_ = nullptr |
int | b2dPosIters_ = 3 |
int | b2dVelIters_ = 8 |
std::string | basePath_ {"."} |
BlockList | blocks_ |
std::unique_ptr< b2World > | box2d_world_ |
std::vector< on_observation_callback_t > | callbacksOnObservation_ |
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< Joystick > | joystick_ |
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_t > | xmlParsers_ |
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) |
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
using mvsim::World::block_visitor_t = std::function<void(Block&)> |
using mvsim::World::BlockList = std::multimap<std::string, Block::Ptr> |
Map 'block-name' => block object. See getListOfBlocks()
|
private |
using mvsim::World::on_observation_callback_t = std::function<void(const Simulable& , const mrpt::obs::CObservation::Ptr& )> |
using mvsim::World::SimulableList = std::multimap<std::string, Simulable::Ptr> |
using mvsim::World::vehicle_visitor_t = std::function<void(VehicleBase&)> |
using mvsim::World::VehicleList = std::multimap<std::string, VehicleBase::Ptr> |
Map 'vehicle-name' => vehicle object. See getListOfVehicles()
using mvsim::World::world_element_visitor_t = std::function<void(WorldElementBase&)> |
using mvsim::World::WorldElementList = std::list<WorldElementBase::Ptr> |
|
private |
|
inline |
|
inline |
void World::clear_all | ( | ) |
|
inline |
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.
void World::connectToServer | ( | ) |
bool World::evaluate_tag_if | ( | const rapidxml::xml_node< char > & | node | ) | const |
Definition at line 357 of file World_load_xml.cpp.
|
inline |
|
inline |
|
inline |
|
inline |
Seconds since start of simulation.
|
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.
double World::get_simul_timestep | ( | ) | const |
Simulation fixed-time interval for numerical integration.
Definition at line 171 of file World_simul.cpp.
|
inline |
|
inline |
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.
float World::getHighestElevationUnder | ( | const mrpt::math::TPoint3Df & | queryPt | ) | const |
std::optional< mvsim::TJoyStickEvent > World::getJoystickState | ( | ) | const |
|
inline |
|
inline |
Always lock/unlock getListOfSimulableObjectsMtx() before using this:
|
inline |
|
inline |
|
inline |
|
inline |
|
private |
Ensure the cache is built and up-to-date, then return it:
Definition at line 582 of file World_simul.cpp.
|
inline |
|
inline |
|
inline |
|
private |
|
private |
Definition at line 182 of file World_services.cpp.
|
private |
Definition at line 1127 of file World_gui.cpp.
|
private |
Definition at line 1071 of file World_gui.cpp.
|
private |
Definition at line 1086 of file World_gui.cpp.
|
private |
Definition at line 1114 of file World_gui.cpp.
void World::internal_GUI_thread | ( | ) |
Definition at line 603 of file World_gui.cpp.
|
private |
Runs one individual time step
Definition at line 66 of file World_simul.cpp.
void World::internal_process_pending_gui_user_tasks | ( | ) |
Definition at line 901 of file World_gui.cpp.
|
private |
This will parse a main XML file, or its included.
Definition at line 126 of file World_load_xml.cpp.
void World::internal_simul_pre_step_terrain_elevation | ( | ) |
Definition at line 308 of file World_simul.cpp.
|
private |
Definition at line 598 of file World_simul.cpp.
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.
|
private |
Definition at line 226 of file World_simul.cpp.
|
private |
Definition at line 259 of file World_simul.cpp.
void World::internalRunSensorsOn3DScene | ( | mrpt::opengl::COpenGLScene & | physicalObjects | ) |
Definition at line 914 of file World_gui.cpp.
void World::internalUpdate3DSceneObjects | ( | mrpt::opengl::COpenGLScene & | viz, |
mrpt::opengl::COpenGLScene & | physical | ||
) |
Definition at line 926 of file World_gui.cpp.
bool mvsim::World::is_GUI_open | ( | ) | const |
Return true if the GUI window is open, after
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.
[in] | fileNameForPath | Optionally, provide the full path to an XML file from which to take relative paths. |
std::exception | On any error, with what() giving a descriptive error message |
Definition at line 37 of file World_load_xml.cpp.
void World::load_from_XML_file | ( | const std::string & | xmlFileNamePath | ) |
Load an entire world description into this object from a specification in XML format.
[in] | xmlFileNamePath | The relative or full path to the XML file. |
std::exception | On any error, with what() giving a descriptive error message |
Definition at line 31 of file World_load_xml.cpp.
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.
Replace macros, prefix the base_path if input filename is relative, etc.
|
inline |
|
private |
Definition at line 208 of file World_load_xml.cpp.
|
private |
Definition at line 216 of file World_load_xml.cpp.
|
private |
<element>
Definition at line 162 of file World_load_xml.cpp.
|
private |
Definition at line 311 of file World_load_xml.cpp.
|
private |
Definition at line 233 of file World_load_xml.cpp.
|
private |
Definition at line 222 of file World_load_xml.cpp.
|
private |
Definition at line 346 of file World_load_xml.cpp.
|
private |
Definition at line 259 of file World_load_xml.cpp.
|
private |
Definition at line 228 of file World_load_xml.cpp.
|
private |
Definition at line 376 of file World_load_xml.cpp.
|
private |
<sensor>
Definition at line 195 of file World_load_xml.cpp.
|
private |
Definition at line 287 of file World_load_xml.cpp.
|
private |
<vehicle>
Definition at line 172 of file World_load_xml.cpp.
|
private |
<vehicle:class>
Definition at line 189 of file World_load_xml.cpp.
|
private |
Definition at line 257 of file World_load_xml.cpp.
|
inline |
|
private |
Definition at line 120 of file World_walls.cpp.
|
private |
Definition at line 103 of file World_load_xml.cpp.
|
inlineprivate |
|
inlineprivate |
|
inline |
void World::run_simulation | ( | double | dt | ) |
Runs the simulation for a given time interval (in seconds)
Runs the simulation for a given time interval (in seconds)
Definition at line 25 of file World_simul.cpp.
void World::runVisitorOnBlocks | ( | const block_visitor_t & | v | ) |
void World::runVisitorOnVehicles | ( | const vehicle_visitor_t & | v | ) |
void World::runVisitorOnWorldElements | ( | const world_element_visitor_t & | v | ) |
|
inline |
|
inline |
|
private |
Changes the light source direction from azimuth and elevation angles (in radians)
Definition at line 1208 of file World_gui.cpp.
|
inline |
void World::update_GUI | ( | TUpdateGUIParams * | params = nullptr | ) |
Updates (or sets-up upon first call) the GUI visualization of the scene.
[in,out] | params | Optional inputs/outputs to the GUI update process. See struct for details. |
Definition at line 1006 of file World_gui.cpp.
|
inline |
|
inline |
|
inline |
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().
|
staticprivate |
Definition at line 589 of file World_simul.cpp.
|
friend |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
mrpt::opengl::CSetOfObjects::Ptr mvsim::World::guiUserObjectsPhysical_ |
mrpt::opengl::CSetOfObjects::Ptr mvsim::World::guiUserObjectsViz_ |
|
mutableprivate |
TGUIKeyEvent mvsim::World::lastKeyEvent_ |
|
private |
|
mutableprivate |
|
private |
|
private |
|
private |
|
private |
|
private |
|
mutableprivate |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
std::atomic_bool mvsim::World::simulator_must_close_ = false |
|
private |
|
mutableprivate |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
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()
|
private |
3D scene with all visual objects (vehicles, obstacles, markers, etc.)
|
private |