World.h

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014  Jose Luis Blanco Claraco (University of Almeria)    |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under GNU General Public License version 3                  |
  |   See <http://www.gnu.org/licenses/>                                    |
  +-------------------------------------------------------------------------+ */

#pragma once

#include <mvsim/VehicleBase.h>
#include <mvsim/WorldElements/WorldElementBase.h>
#include <mvsim/Block.h>

#include <mrpt/gui/CDisplayWindow3D.h>

#include <mrpt/version.h>
#if MRPT_VERSION<0x199
#include <mrpt/utils/utils_defs.h>  // mrpt::format()
#include <mrpt/utils/CTimeLogger.h>
#include <mrpt/utils/CTicTac.h>
#include <mrpt/utils/TColor.h>
using mrpt::utils::CTicTac;
using mrpt::utils::CTimeLogger;
using mrpt::utils::CTimeLoggerEntry;
using mrpt::utils::TColor;
using mrpt::utils::TColorf;
using mrpt::utils::keep_max;
using mrpt::utils::DEG2RAD;
using mrpt::utils::RAD2DEG;
#else
#include <mrpt/core/format.h>
#include <mrpt/core/bits_math.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/img/TColor.h>
using mrpt::system::CTicTac;
using mrpt::system::CTimeLogger;
using mrpt::system::CTimeLoggerEntry;
using mrpt::img::TColor;
using mrpt::img::TColorf;
using mrpt::keep_max;
using mrpt::DEG2RAD;
using mrpt::RAD2DEG;
#endif

#include <mrpt/version.h>
#if MRPT_VERSION >= 0x130
#include <mrpt/obs/CObservation.h>
#else
#include <mrpt/slam/CObservation.h>
#endif

#include <Box2D/Dynamics/b2World.h>
#include <Box2D/Dynamics/b2Body.h>

#include <list>

namespace mvsim
{
class World
{
   public:
        World();  
        ~World();  

        void clear_all(bool acquire_mt_lock = true);  








        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;  
                mrpt::gui::mrptKeyModifier key_modifier;

                TGUIKeyEvent() : keycode(0) {}
        };

        struct TUpdateGUIParams
        {
                TGUIKeyEvent keyevent;  
                std::string msg_lines;  

                TUpdateGUIParams();
        };

        void update_GUI(TUpdateGUIParams* params = NULL);

        bool is_GUI_open() const;  


        void close_GUI();  

        typedef std::multimap<std::string, VehicleBase*>
                TListVehicles;  

        typedef std::list<WorldElementBase*>
                TListWorldElements;  
        typedef std::multimap<std::string, Block*>
                TListBlocks;  


        b2World* getBox2DWorld() { return m_box2d_world; }
        const b2World* getBox2DWorld() const { return m_box2d_world; }
        b2Body* getBox2DGroundBody() { return m_b2_ground_body; }
        const TListVehicles& getListOfVehicles() const { return m_vehicles; }
        TListVehicles& getListOfVehicles() { return m_vehicles; }
        const TListBlocks& getListOfBlocks() const { return m_blocks; }
        TListBlocks& getListOfBlocks() { return m_blocks; }
        const TListWorldElements& getListOfWorldElements() const
        {
                return m_world_elements;
        }

        CTimeLogger& getTimeLogger() { return m_timlogger; }
        std::string resolvePath(const std::string& in_path) const;

        struct VehicleVisitorBase
        {
                virtual void visit(VehicleBase* obj) = 0;
        };
        struct WorldElementVisitorBase
        {
                virtual void visit(WorldElementBase* obj) = 0;
        };

        void runVisitorOnVehicles(VehicleVisitorBase& v);

        void runVisitorOnWorldElements(WorldElementVisitorBase& v);

        virtual void onNewObservation(
                const VehicleBase& veh,
#if MRPT_VERSION >= 0x130
                const mrpt::obs::CObservation* obs
#else
                const mrpt::slam::CObservation* obs
#endif
                )
        {
                /* default: do nothing */
        }
   private:
        friend class VehicleBase;
        friend class Block;

        // -------- World Params ----------
        double m_gravity;  

        double m_simul_time;  


        double m_simul_timestep;  

        int m_b2d_vel_iters,
                m_b2d_pos_iters;  
        std::string m_base_path;  

        // ------- GUI options -----
        struct TGUI_Options
        {
                unsigned int win_w, win_h;
                bool ortho;
                bool show_forces;
                double force_scale;  
                double camera_distance;
                double fov_deg;
                std::string
                        follow_vehicle;  

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

        TGUI_Options m_gui_options;  


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


        b2World* m_box2d_world;  
        b2Body*
                m_b2_ground_body;  

        TListVehicles m_vehicles;
        TListWorldElements m_world_elements;
        TListBlocks m_blocks;

        void internal_one_timestep(double dt);

        // -------- GUI stuff ----------
        mrpt::gui::CDisplayWindow3D::Ptr m_gui_win;

        CTimeLogger m_timlogger;
        CTicTac m_timer_iteration;
};
}