World.h

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2026  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_distance_joint.h>
#include <box2d/b2_revolute_joint.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/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/HumanActor.h>
#include <mvsim/Joystick.h>
#include <mvsim/RemoteResourcesManager.h>
#include <mvsim/TParameterDefinitions.h>
#include <mvsim/VehicleBase.h>
#include <mvsim/WorldElements/WorldElementBase.h>
 
//
#include <mrpt/version.h>
#if MRPT_VERSION >= 0x020f07
#include <mrpt/io/CCompressedOutputStream.h>
#else
#include <mrpt/io/CFileGZOutputStream.h>
#endif
 
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/Comms/Client.h>
#endif
 
#include <any>
#include <atomic>
#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
{
struct WorldJoint
{
    enum class Type : uint8_t
    {
        Distance = 0,  
        Revolute  
    };
 
    Type type = Type::Distance;
 
    std::string bodyA_name;
    std::string bodyB_name;
 
    mrpt::math::TPoint2D anchorA{0, 0};
    mrpt::math::TPoint2D anchorB{0, 0};
 
    float maxLength = 5.0f;
    float minLength = 0.0f;
    float stiffness = 0.0f;
    float damping = 0.0f;
 
    bool enableLimit = false;
    float lowerAngle_deg = 0.0f;  
    float upperAngle_deg = 0.0f;
 
    b2Joint* b2joint = nullptr;
};
 
class World : public mrpt::system::COutputLogger
{
   public:
    World();  
    ~World();  
 
    // Rule of Five: explicitly delete copy operations, allow move operations
    World(const World&) = delete;
    World& operator=(const World&) = delete;
    World(World&&) = delete;
    World& operator=(World&&) = delete;
 
    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());
    }
 
    bool has_simul_timestamp() const
    {
        auto lck = mrpt::lockHelper(simul_time_mtx_);
        return simul_start_wallclock_time_.has_value();
    }
 
    double get_realtime_factor_achieved() const { return achievedRealtimeFactor_.load(); }
 
    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);
 
    void insert_vehicle(const VehicleBase::Ptr& veh);
 
    struct GUIKeyEvent
    {
        int keycode = 0;  
        bool modifierShift = false;
        bool modifierCtrl = false;
        bool modifierAlt = false;
        bool modifierSuper = false;
 
        GUIKeyEvent() = default;
    };
 
    struct TUpdateGUIParams
    {
        GUIKeyEvent 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_;
 
    GUIKeyEvent 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>;
 
    using ActorList = std::multimap<std::string, HumanActor::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_; }
 
    const std::vector<WorldJoint>& getListOfJoints() const { return joints_; }
 
    const ActorList& getListOfActors() const { return actors_; }
    ActorList& getListOfActors() { return actors_; }
 
    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();
 
    std::optional<std::any> getPropertyAt(
        const std::string& propertyName, const mrpt::math::TPoint3D& worldXYZ) const;
 
   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::atomic<double> achievedRealtimeFactor_{1.0};
    std::optional<double> lastRunSimulWallclock_;
 
    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;
        bool show_trajectories = false;
        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}},
            {"show_trajectories", {"%bool", &show_trajectories}},
            {"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_;
    ActorList actors_;
 
    std::vector<WorldJoint> joints_;
 
    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, bool startVisible);
 
    static bool internal_gui_sensor_preview_visible(
        const Simulable& veh, const std::string& sensorLabel);
 
    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);
    void parse_tag_joint(const XmlParserContext& ctx);  
    void parse_tag_actor(const XmlParserContext& ctx);
    void parse_tag_actor_class(const XmlParserContext& ctx);
    void parse_tag_remote_resources(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_;
#if MRPT_VERSION >= 0x020f07
    std::map<std::string, std::shared_ptr<mrpt::io::CCompressedOutputStream>> rawlog_io_per_veh_;
#else
    std::map<std::string, std::shared_ptr<mrpt::io::CFileGZOutputStream>> rawlog_io_per_veh_;
#endif
    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