World.h

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2020  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */

#pragma once

#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2World.h>
#include <mrpt/core/bits_math.h>
#include <mrpt/core/format.h>
#include <mrpt/gui/CDisplayWindowGUI.h>
#include <mrpt/img/TColor.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mvsim/Block.h>
#include <mvsim/Comms/Client.h>
#include <mvsim/TParameterDefinitions.h>
#include <mvsim/VehicleBase.h>
#include <mvsim/WorldElements/WorldElementBase.h>

#include <list>

namespace mvsim
{
class World : public mrpt::system::COutputLogger
{
   public:
        World();  
        ~World();  

        void clear_all();

        void load_from_XML(
                const std::string& xml_text,
                const std::string& fileNameForPath = std::string("."));
        double get_simul_time() const
        {
                return m_simul_time;
        }  

        double get_simul_timestep() const
        {
                return m_simul_timestep;
        }  
        void set_simul_timestep(double timestep)
        {
                m_simul_timestep = timestep;
        }  

        double get_gravity() const
        {
                return m_gravity;
        }  
        void set_gravity(double accel)
        {
                m_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 m_gui.gui_win;
        }

        void internalUpdate3DSceneObjects(
                mrpt::opengl::COpenGLScene::Ptr& gl_scene);
        void internal_GUI_thread();
        void internal_process_pending_gui_user_tasks();

        std::string m_gui_msg_lines;
        std::mutex m_gui_msg_lines_mtx;

        std::thread m_gui_thread;

        std::atomic_bool m_gui_thread_running = false;
        std::atomic_bool m_gui_thread_must_close = false;
        std::mutex m_gui_thread_start_mtx;

        void enqueue_task_to_run_in_gui_thread(const std::function<void(void)>& f)
        {
                m_gui_user_pending_tasks_mtx.lock();
                m_gui_user_pending_tasks.emplace_back(f);
                m_gui_user_pending_tasks_mtx.unlock();
        }

        std::vector<std::function<void(void)>> m_gui_user_pending_tasks;
        std::mutex m_gui_user_pending_tasks_mtx;

        TGUIKeyEvent m_lastKeyEvent;
        std::atomic_bool m_lastKeyEventValid = false;
        std::mutex m_lastKeyEvent_mtx;

        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 m_box2d_world; }
        const std::unique_ptr<b2World>& getBox2DWorld() const
        {
                return m_box2d_world;
        }
        b2Body* getBox2DGroundBody() { return m_b2_ground_body; }
        const VehicleList& getListOfVehicles() const { return m_vehicles; }
        VehicleList& getListOfVehicles() { return m_vehicles; }
        const BlockList& getListOfBlocks() const { return m_blocks; }
        BlockList& getListOfBlocks() { return m_blocks; }
        const WorldElementList& getListOfWorldElements() const
        {
                return m_world_elements;
        }

        SimulableList& getListOfSimulableObjects() { return m_simulableObjects; }
        const SimulableList& getListOfSimulableObjects() const
        {
                return m_simulableObjects;
        }

        mrpt::system::CTimeLogger& getTimeLogger() { return m_timlogger; }
        std::string resolvePath(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&)>;

        void runVisitorOnVehicles(const vehicle_visitor_t& v);

        void runVisitorOnWorldElements(const world_element_visitor_t& v);

        virtual void onNewObservation(
                [[maybe_unused]] const VehicleBase& veh,
                [[maybe_unused]] const mrpt::obs::CObservation* obs)
        {
                /* default: do nothing */
        }
        void connectToServer();

        mvsim::Client& commsClient() { return m_client; }
        const mvsim::Client& commsClient() const { return m_client; }

   private:
        friend class VehicleBase;
        friend class Block;

        mvsim::Client m_client{"World"};

        // -------- World Params ----------
        double m_gravity = 9.81;

        double m_simul_timestep = 10e-3;

        int m_b2d_vel_iters = 6, m_b2d_pos_iters = 3;

        const TParameterDefinitions m_other_world_params = {
                {"gravity", {"%lf", &m_gravity}},
                {"simul_timestep", {"%lf", &m_simul_timestep}},
                {"b2d_vel_iters", {"%i", &m_b2d_vel_iters}},
                {"b2d_pos_iters", {"%i", &m_b2d_pos_iters}},
        };

        double m_simul_time = 0;

        std::string m_base_path{"."};

        // ------- 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;
                double force_scale = 0.01;      
                double camera_distance = 80.0;
                double fov_deg = 60.0;
                std::string follow_vehicle;

                const TParameterDefinitions params = {
                        {"win_w", {"%u", &win_w}},
                        {"win_h", {"%u", &win_h}},
                        {"ortho", {"%bool", &ortho}},
                        {"show_forces", {"%bool", &show_forces}},
                        {"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}},
                };

                TGUI_Options() = default;
                void parse_from(const rapidxml::xml_node<char>& node);
        };

        TGUI_Options m_gui_options;      

        // -------- World contents ----------
        std::recursive_mutex m_world_cs;

        std::unique_ptr<b2World> m_box2d_world;

        b2Body* m_b2_ground_body = nullptr;

        VehicleList m_vehicles;
        WorldElementList m_world_elements;
        BlockList m_blocks;

        // 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 m_simulableObjects;

        void internal_one_timestep(double dt);

        std::mutex m_simulationStepRunningMtx;

        struct GUI
        {
                GUI(World& parent) : m_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_top_menu();
                void prepare_status_window();
                void prepare_editor_window();

                void handle_mouse_operations();

           private:
                World& m_parent;
        };
        GUI m_gui{*this};  
  // end GUI stuff

        mrpt::system::CTimeLogger m_timlogger;
        mrpt::system::CTicTac m_timer_iteration;

        void process_load_walls(const rapidxml::xml_node<char>& node);
        void insertBlock(const Block::Ptr& block);
};
}  // namespace mvsim