World.h

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2024  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */
 
#pragma once
 
#include <box2d/b2_body.h>
#include <box2d/b2_world.h>
#include <mrpt/core/bits_math.h>
#include <mrpt/core/format.h>
#include <mrpt/gui/CDisplayWindowGUI.h>
#include <mrpt/img/CImage.h>
#include <mrpt/img/TColor.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/topography/data_types.h>
#include <mvsim/Block.h>
#include <mvsim/Joystick.h>
#include <mvsim/RemoteResourcesManager.h>
#include <mvsim/TParameterDefinitions.h>
#include <mvsim/VehicleBase.h>
#include <mvsim/WorldElements/WorldElementBase.h>
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Client.h>
#endif
 
#include <functional>
#include <list>
#include <map>
#include <set>
#include <unordered_map>
 
#if MVSIM_HAS_ZMQ && MVSIM_HAS_PROTOBUF
// forward declarations:
namespace mvsim_msgs
{
class SrvGetPose;
class SrvGetPoseAnswer;
class SrvSetPose;
class SrvSetPoseAnswer;
class SrvSetControllerTwist;
class SrvSetControllerTwistAnswer;
class SrvShutdown;
class SrvShutdownAnswer;
}  // namespace mvsim_msgs
#endif
 
namespace mvsim
{
class World : public mrpt::system::COutputLogger
{
   public:
        World();  
        ~World();  
 
        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("."));
        double get_simul_time() const
        {
                auto lck = mrpt::lockHelper(simul_time_mtx_);
                return simulTime_;
        }
 
        void force_set_simul_time(double newSimulatedTime)
        {
                auto lck = mrpt::lockHelper(simul_time_mtx_);
                simulTime_ = newSimulatedTime;
        }
 
        mrpt::Clock::time_point get_simul_timestamp() const
        {
                auto lck = mrpt::lockHelper(simul_time_mtx_);
                ASSERT_(simul_start_wallclock_time_.has_value());
                return mrpt::Clock::fromDouble(simulTime_ + simul_start_wallclock_time_.value());
        }
 
        double get_simul_timestep() const;
 
        void set_simul_timestep(double timestep) { simulTimestep_ = timestep; }
 
        double get_gravity() const { return gravity_; }
 
        void set_gravity(double accel) { gravity_ = accel; }
 
        void run_simulation(double dt);
 
        struct TGUIKeyEvent
        {
                int keycode = 0;  
                bool modifierShift = false;
                bool modifierCtrl = false;
                bool modifierAlt = false;
                bool modifierSuper = false;
 
                TGUIKeyEvent() = default;
        };
 
        struct TUpdateGUIParams
        {
                TGUIKeyEvent keyevent;  
                std::string msg_lines;  
 
                TUpdateGUIParams() = default;
        };
 
        void update_GUI(TUpdateGUIParams* params = nullptr);
 
        const mrpt::gui::CDisplayWindowGUI::Ptr& gui_window() const { return gui_.gui_win; }
 
        const mrpt::math::TPoint3D& gui_mouse_point() const { return gui_.clickedPt; }
 
        mrpt::opengl::CSetOfObjects::Ptr guiUserObjectsPhysical_, guiUserObjectsViz_;
        std::mutex guiUserObjectsMtx_;
 
        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();
 
        std::mutex pendingRunSensorsOn3DSceneMtx_;
        bool pendingRunSensorsOn3DScene_ = false;
 
        void mark_as_pending_running_sensors_on_3D_scene()
        {
                pendingRunSensorsOn3DSceneMtx_.lock();
                pendingRunSensorsOn3DScene_ = true;
                pendingRunSensorsOn3DSceneMtx_.unlock();
        }
        void clear_pending_running_sensors_on_3D_scene()
        {
                pendingRunSensorsOn3DSceneMtx_.lock();
                pendingRunSensorsOn3DScene_ = false;
                pendingRunSensorsOn3DSceneMtx_.unlock();
        }
        bool pending_running_sensors_on_3D_scene()
        {
                pendingRunSensorsOn3DSceneMtx_.lock();
                bool ret = pendingRunSensorsOn3DScene_;
                pendingRunSensorsOn3DSceneMtx_.unlock();
                return ret;
        }
 
        std::string guiMsgLines_;
        std::mutex guiMsgLinesMtx_;
 
        std::thread gui_thread_;
 
        std::atomic_bool gui_thread_running_ = false;
        std::atomic_bool simulator_must_close_ = false;
        mutable std::mutex gui_thread_start_mtx_;
 
        bool simulator_must_close() const
        {
                gui_thread_start_mtx_.lock();
                const bool v = simulator_must_close_;
                gui_thread_start_mtx_.unlock();
                return v;
        }
        void simulator_must_close(bool value)
        {
                gui_thread_start_mtx_.lock();
                simulator_must_close_ = value;
                gui_thread_start_mtx_.unlock();
        }
 
        void enqueue_task_to_run_in_gui_thread(const std::function<void(void)>& f)
        {
                guiUserPendingTasksMtx_.lock();
                guiUserPendingTasks_.emplace_back(f);
                guiUserPendingTasksMtx_.unlock();
        }
 
        std::vector<std::function<void(void)>> guiUserPendingTasks_;
        std::mutex guiUserPendingTasksMtx_;
 
        TGUIKeyEvent lastKeyEvent_;
        std::atomic_bool lastKeyEventValid_ = false;
        std::mutex lastKeyEventMtx_;
 
        bool is_GUI_open() const;  
 
        void close_GUI();  
 
        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>;
 
        std::unique_ptr<b2World>& getBox2DWorld() { return box2d_world_; }
        const std::unique_ptr<b2World>& getBox2DWorld() const { return box2d_world_; }
        b2Body* getBox2DGroundBody() { return b2_ground_body_; }
        const VehicleList& getListOfVehicles() const { return vehicles_; }
        VehicleList& getListOfVehicles() { return vehicles_; }
        const BlockList& getListOfBlocks() const { return blocks_; }
        BlockList& getListOfBlocks() { return blocks_; }
        const WorldElementList& getListOfWorldElements() const { return worldElements_; }
 
        SimulableList& getListOfSimulableObjects() { return simulableObjects_; }
        const SimulableList& getListOfSimulableObjects() const { return simulableObjects_; }
        auto& getListOfSimulableObjectsMtx() { return simulableObjectsMtx_; }
 
        mrpt::system::CTimeLogger& getTimeLogger() { return timlogger_; }
 
        std::string local_to_abs_path(const std::string& in_path) const;
 
        std::string xmlPathToActualPath(const std::string& modelURI) const;
 
        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);
 
        using on_observation_callback_t =
                std::function<void(const Simulable& /*veh*/, const mrpt::obs::CObservation::Ptr& /*obs*/)>;
 
        void registerCallbackOnObservation(const on_observation_callback_t& f)
        {
                callbacksOnObservation_.emplace_back(f);
        }
 
        void dispatchOnObservation(const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs);
 
        void connectToServer();
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mvsim::Client& commsClient() { return client_; }
        const mvsim::Client& commsClient() const { return client_; }
#endif
 
        void free_opengl_resources();
 
        auto& physical_objects_mtx() { return worldPhysicalMtx_; }
 
        bool headless() const { return guiOptions_.headless; }
        void headless(bool setHeadless) { guiOptions_.headless = setHeadless; }
 
        bool sensor_has_to_create_egl_context();
 
        const std::map<std::string, std::string>& user_defined_variables() const
        {
                return userDefinedVariables_;
        }
 
        std::optional<mvsim::TJoyStickEvent> getJoystickState() const;
 
        bool evaluate_tag_if(const rapidxml::xml_node<char>& node) const;
 
        float collisionThreshold() const { return collisionThreshold_; }
 
        std::set<float> getElevationsAt(const mrpt::math::TPoint2D& worldXY) const;
 
        float getHighestElevationUnder(const mrpt::math::TPoint3Df& queryPt) const;
 
        void internal_simul_pre_step_terrain_elevation();
 
   private:
        friend class VehicleBase;
        friend class Block;
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        mvsim::Client client_{"World"};
#endif
 
        std::vector<on_observation_callback_t> callbacksOnObservation_;
 
        // -------- World Params ----------
        double gravity_ = 9.81;
 
        mutable double simulTimestep_ = 0;
 
        mutable bool joystickEnabled_ = false;
        mutable std::optional<Joystick> joystick_;
 
        int b2dVelIters_ = 8, b2dPosIters_ = 3;
 
        float collisionThreshold_ = 0.03f;
 
        std::string serverAddress_ = "localhost";
 
        std::string save_to_rawlog_;
 
        double rawlog_odometry_rate_ = 10.0;  
 
        std::string save_ground_truth_trajectory_;
 
        double ground_truth_rate_ = 50.0;  
 
        double max_slope_to_collide_ = 0.30;
        double min_slope_to_collide_ = -0.50;
 
        const TParameterDefinitions otherWorldParams_ = {
                {"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_}},
        };
 
        std::map<std::string, std::string> userDefinedVariables_;
 
        double simulTime_ = 0;
        std::optional<double> simul_start_wallclock_time_;
        std::mutex simul_time_mtx_;
 
        std::string basePath_{"."};
 
        mrpt::opengl::CSetOfObjects::Ptr glUserObjsPhysical_ = mrpt::opengl::CSetOfObjects::Create();
        mrpt::opengl::CSetOfObjects::Ptr glUserObjsViz_ = mrpt::opengl::CSetOfObjects::Create();
 
        // ------- GUI options -----
        struct TGUI_Options
        {
                unsigned int win_w = 800, win_h = 600;
                bool start_maximized = true;
                int refresh_fps = 20;
                bool ortho = false;
                bool show_forces = false;
                bool show_sensor_points = true;
                double force_scale = 0.01;      
                double camera_distance = 80.0;
                double camera_azimuth_deg = 45.0;
                double camera_elevation_deg = 40.0;
                double fov_deg = 60.0;
                float clip_plane_min = 0.05f;
                float clip_plane_max = 10e3f;
                mrpt::math::TPoint3D camera_point_to{0, 0, 0};
                std::string follow_vehicle;      
                bool headless = false;
 
                const TParameterDefinitions params = {
                        {"win_w", {"%u", &win_w}},
                        {"win_h", {"%u", &win_h}},
                        {"ortho", {"%bool", &ortho}},
                        {"show_forces", {"%bool", &show_forces}},
                        {"show_sensor_points", {"%bool", &show_sensor_points}},
                        {"force_scale", {"%lf", &force_scale}},
                        {"fov_deg", {"%lf", &fov_deg}},
                        {"follow_vehicle", {"%s", &follow_vehicle}},
                        {"start_maximized", {"%bool", &start_maximized}},
                        {"refresh_fps", {"%i", &refresh_fps}},
                        {"headless", {"%bool", &headless}},
                        {"clip_plane_min", {"%f", &clip_plane_min}},
                        {"clip_plane_max", {"%f", &clip_plane_max}},
                        {"cam_distance", {"%lf", &camera_distance}},
                        {"cam_azimuth", {"%lf", &camera_azimuth_deg}},
                        {"cam_elevation", {"%lf", &camera_elevation_deg}},
                        {"cam_point_to", {"%point3d", &camera_point_to}},
                };
 
                TGUI_Options() = default;
                void parse_from(const rapidxml::xml_node<char>& node, COutputLogger& logger);
        };
 
        TGUI_Options guiOptions_;
 
        struct LightOptions
        {
                LightOptions() = default;
 
                void parse_from(const rapidxml::xml_node<char>& node, COutputLogger& logger);
 
                bool enable_shadows = true;
                int shadow_map_size = 2048;
 
                double light_azimuth = mrpt::DEG2RAD(45.0);
                double light_elevation = mrpt::DEG2RAD(70.0);
 
                float light_clip_plane_min = 0.1f;
                float light_clip_plane_max = 900.0f;
 
                float shadow_bias = 1e-5;
                float shadow_bias_cam2frag = 1e-5;
                float shadow_bias_normal = 1e-4;
 
                mrpt::img::TColor light_color = {0xff, 0xff, 0xff, 0xff};
                float light_ambient = 0.5f;
 
                float eye_distance_to_shadow_map_extension = 2.0f;      
                float minimum_shadow_map_extension_ratio = 0.005f;      
 
                const TParameterDefinitions params = {
                        {"enable_shadows", {"%bool", &enable_shadows}},
                        {"shadow_map_size", {"%i", &shadow_map_size}},
                        {"light_azimuth_deg", {"%lf_deg", &light_azimuth}},
                        {"light_elevation_deg", {"%lf_deg", &light_elevation}},
                        {"light_clip_plane_min", {"%f", &light_clip_plane_min}},
                        {"light_clip_plane_max", {"%f", &light_clip_plane_max}},
                        {"light_color", {"%color", &light_color}},
                        {"shadow_bias", {"%f", &shadow_bias}},
                        {"shadow_bias_cam2frag", {"%f", &shadow_bias_cam2frag}},
                        {"shadow_bias_normal", {"%f", &shadow_bias_normal}},
                        {"light_ambient", {"%f", &light_ambient}},
                        {"eye_distance_to_shadow_map_extension", {"%f", &eye_distance_to_shadow_map_extension}},
                        {"minimum_shadow_map_extension_ratio", {"%f", &minimum_shadow_map_extension_ratio}},
                };
        };
 
        LightOptions lightOptions_;
 
   public:
        // Options for simulating GNSS (GPS) sensors.
        struct GeoreferenceOptions
        {
                GeoreferenceOptions() = default;
 
                void parse_from(const rapidxml::xml_node<char>& node, COutputLogger& logger);
 
                mrpt::topography::TGeodeticCoords georefCoord;
 
                double world_to_enu_rotation = .0;
 
                bool world_is_utm = false;
 
                mrpt::topography::TUTMCoords utmRef;
                int utm_zone = 0;  // auto calculated
                char utm_band = 'X';  // auto calculated
 
                const TParameterDefinitions params = {
                        {"latitude", {"%lf", &georefCoord.lat.decimal_value}},
                        {"longitude", {"%lf", &georefCoord.lon.decimal_value}},
                        {"height", {"%lf", &georefCoord.height}},
                        {"world_to_enu_rotation_deg", {"%lf_deg", &world_to_enu_rotation}},
                        {"world_is_utm", {"%bool", &world_is_utm}},
                };
        };
 
        const GeoreferenceOptions& georeferenceOptions() const { return georeferenceOptions_; }
 
        mrpt::math::TVector3D worldRenderOffset() const
        {
                return worldRenderOffset_ ? *worldRenderOffset_ : mrpt::math::TVector3D(0, 0, 0);
        }
        mrpt::math::TPose3D applyWorldRenderOffset(mrpt::math::TPose3D p) const
        {
                const auto t = worldRenderOffset();
                p.x += t.x;
                p.y += t.y;
                p.z += t.z;
                return p;
        }
        mrpt::poses::CPose3D applyWorldRenderOffset(mrpt::poses::CPose3D p) const
        {
                const auto t = worldRenderOffset();
                p.x_incr(t.x);
                p.y_incr(t.y);
                p.z_incr(t.z);
                return p;
        }
        void worldRenderOffsetPropose(const mrpt::math::TVector3D& v)
        {
                if (!worldRenderOffset_) worldRenderOffset_ = v;
        }
 
   private:
        GeoreferenceOptions georeferenceOptions_;
 
        // -------- World contents ----------
        std::recursive_mutex world_cs_;
 
        std::unique_ptr<b2World> box2d_world_;
 
        b2Body* b2_ground_body_ = nullptr;
 
        VehicleList vehicles_;
        WorldElementList worldElements_;
        BlockList blocks_;
 
        bool initialized_ = false;
 
        // List of all objects above (vehicles, world_elements, blocks), but as
        // shared_ptr to their Simulable interfaces, so we can easily iterate on
        // this list only for common tasks:
        SimulableList simulableObjects_;
        std::mutex simulableObjectsMtx_;
 
        void internal_one_timestep(double dt);
 
        std::mutex simulationStepRunningMtx_;
 
        // A 2D-hash table of objects
        struct lut_2d_coordinates_t
        {
                int32_t x, y;
 
                bool operator==(const lut_2d_coordinates_t& o) const noexcept
                {
                        return (x == o.x && y == o.y);
                }
        };
 
        static lut_2d_coordinates_t xy_to_lut_coords(const mrpt::math::TPoint2Df& p);
 
        struct LutIndexHash
        {
                std::size_t operator()(const lut_2d_coordinates_t& p) const noexcept
                {
                        // These are the implicit assumptions of the reinterpret cast below:
                        static_assert(sizeof(int32_t) == sizeof(uint32_t));
                        static_assert(offsetof(lut_2d_coordinates_t, x) == 0 * sizeof(uint32_t));
                        static_assert(offsetof(lut_2d_coordinates_t, y) == 1 * sizeof(uint32_t));
 
                        const uint32_t* vec = reinterpret_cast<const uint32_t*>(&p);
                        return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663);
                }
                bool operator()(
                        const lut_2d_coordinates_t& k1, const lut_2d_coordinates_t& k2) const noexcept
                {
                        if (k1.x != k2.x) return k1.x < k2.x;
                        return k1.y < k2.y;
                }
        };
 
        using LUTCache =
                std::unordered_map<lut_2d_coordinates_t, std::vector<Simulable::Ptr>, LutIndexHash>;
 
        const LUTCache& getLUTCacheOfObjects() const;
 
        mutable LUTCache lut2d_objects_;
        mutable bool lut2d_objects_is_up_to_date_ = false;
 
        void internal_update_lut_cache() const;
 
        struct GUI
        {
                GUI(World& parent) : parent_(parent) {}
 
                mrpt::gui::CDisplayWindowGUI::Ptr gui_win;
                nanogui::Label* lbCpuUsage = nullptr;
                std::vector<nanogui::Label*> lbStatuses;
                nanogui::Button* btnReplaceObject = nullptr;
 
                struct InfoPerObject
                {
                        nanogui::CheckBox* cb = nullptr;
                        Simulable::Ptr simulable;
                        VisualObject* visual = nullptr;
                };
 
                // Buttons that must be {dis,en}abled when there is a selected object:
                std::vector<nanogui::Widget*> btns_selectedOps;
                std::vector<InfoPerObject> gui_cbObjects;
                InfoPerObject gui_selectedObject;
 
                mrpt::math::TPoint3D clickedPt{0, 0, 0};
 
                void prepare_control_window();
                void prepare_status_window();
                void prepare_editor_window();
 
                void handle_mouse_operations();
 
           private:
                World& parent_;
        };
        GUI gui_{*this};  
 
        mrpt::opengl::COpenGLScene::Ptr worldVisual_ = mrpt::opengl::COpenGLScene::Create();
 
        mrpt::opengl::COpenGLScene worldPhysical_;
        std::recursive_mutex worldPhysicalMtx_;
 
        std::optional<mrpt::math::TVector3D> worldRenderOffset_;
 
        std::map<std::string, mrpt::math::TPose3D> copy_of_objects_dynstate_pose_;
        std::map<std::string, mrpt::math::TTwist2D> copy_of_objects_dynstate_twist_;
        std::set<std::string> copy_of_objects_had_collision_;
        std::recursive_mutex copy_of_objects_dynstate_mtx_;
 
        std::set<std::string> reset_collision_flags_;
        std::mutex reset_collision_flags_mtx_;
 
        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);
 
        mrpt::math::TPoint2D internal_gui_on_image(
                const std::string& label, const mrpt::img::CImage& im, int winPosX);
 
        std::map<std::string, nanogui::Window*> guiObsViz_;      
 
        void setLightDirectionFromAzimuthElevation(const float azimuth, const float elevation);
  // end GUI stuff
 
        mrpt::system::CTimeLogger timlogger_{true /*enabled*/, "mvsim::World"};
        mrpt::system::CTicTac timer_iteration_;
 
        void process_load_walls(const rapidxml::xml_node<char>& node);
        void insertBlock(const Block::Ptr& block);
 
        struct XmlParserContext
        {
                XmlParserContext(const rapidxml::xml_node<char>* n, const std::string& basePath)
                        : node(n), currentBasePath(basePath)
                {
                }
 
                const rapidxml::xml_node<char>* node = nullptr;
                const std::string currentBasePath;
        };
 
        void internal_recursive_parse_XML(const XmlParserContext& ctx);
 
        using xml_tag_parser_function_t = std::function<void(const XmlParserContext&)>;
 
        std::map<std::string, xml_tag_parser_function_t> xmlParsers_;
 
        void register_standard_xml_tag_parsers();
 
        void register_tag_parser(const std::string& xmlTagName, const xml_tag_parser_function_t& f)
        {
                xmlParsers_.emplace(xmlTagName, f);
        }
        void register_tag_parser(
                const std::string& xmlTagName, void (World::*f)(const XmlParserContext& ctx))
        {
                xmlParsers_.emplace(
                        xmlTagName, [this, f](const XmlParserContext& ctx) { (this->*f)(ctx); });
        }
 
        // ======== XML parser tags ========
        void parse_tag_element(const XmlParserContext& ctx);  
        void parse_tag_vehicle(const XmlParserContext& ctx);  
 
        void parse_tag_vehicle_class(const XmlParserContext& ctx);
        void parse_tag_sensor(const XmlParserContext& ctx);      
        void parse_tag_block(const XmlParserContext& ctx);
        void parse_tag_block_class(const XmlParserContext& ctx);
        void parse_tag_gui(const XmlParserContext& ctx);
        void parse_tag_lights(const XmlParserContext& ctx);
        void parse_tag_georeference(const XmlParserContext& ctx);
        void parse_tag_walls(const XmlParserContext& ctx);
        void parse_tag_include(const XmlParserContext& ctx);
        void parse_tag_variable(const XmlParserContext& ctx);
        void parse_tag_for(const XmlParserContext& ctx);
        void parse_tag_if(const XmlParserContext& ctx);
        void parse_tag_marker(const XmlParserContext& ctx);
 
        // ======== end of XML parser tags ========
 
        mutable RemoteResourcesManager remoteResources_;
 
        void internalOnObservation(const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs);
 
        void internalPostSimulStepForRawlog();
        void internalPostSimulStepForTrajectory();
 
        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_;
 
        std::mutex gt_io_mtx_;
        std::map<std::string, std::fstream> gt_io_per_veh_;
        std::optional<double> gt_last_time_;
 
        // ============ Elevation Field Collision artifacts ==============
        struct TFixturePtr
        {
                TFixturePtr() = default;
                b2Fixture* fixture = nullptr;
        };
        struct TInfoPerCollidableobj
        {
                TInfoPerCollidableobj() = default;
 
                mrpt::poses::CPose3D pose;
                b2Body* collide_body = nullptr;
                double representativeHeight = 0.01;
                double maxWorkableStepHeight = 0.10;
                double speed = .0;
                mrpt::math::TPolygon2D contour;
                const std::vector<float>* wheel_heights = nullptr;
                std::vector<float> contour_heights;
                std::vector<TFixturePtr> collide_fixtures;
        };
        std::vector<std::optional<TInfoPerCollidableobj>> obstacles_for_each_obj_;
        // ============ end of elevation field collision =================
 
        // Services:
        void internal_advertiseServices();      // called from connectToServer()
 
#if MVSIM_HAS_ZMQ && MVSIM_HAS_PROTOBUF
 
        mvsim_msgs::SrvSetPoseAnswer srv_set_pose(const mvsim_msgs::SrvSetPose& req);
        mvsim_msgs::SrvGetPoseAnswer srv_get_pose(const mvsim_msgs::SrvGetPose& req);
        mvsim_msgs::SrvSetControllerTwistAnswer srv_set_controller_twist(
                const mvsim_msgs::SrvSetControllerTwist& req);
        mvsim_msgs::SrvShutdownAnswer srv_shutdown(const mvsim_msgs::SrvShutdown& req);
#endif
};
}  // namespace mvsim