VehicleBase.h
Go to the documentation of this file.
00001 /*+-------------------------------------------------------------------------+
00002   |                       MultiVehicle simulator (libmvsim)                 |
00003   |                                                                         |
00004   | Copyright (C) 2014  Jose Luis Blanco Claraco (University of Almeria)    |
00005   | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
00006   | Distributed under GNU General Public License version 3                  |
00007   |   See <http://www.gnu.org/licenses/>                                    |
00008   +-------------------------------------------------------------------------+ */
00009 
00010 #pragma once
00011 
00012 #include <mutex>
00013 
00014 #include <mvsim/basic_types.h>
00015 #include <mvsim/VisualObject.h>
00016 #include <mvsim/Simulable.h>
00017 #include <mvsim/Wheel.h>
00018 #include <mvsim/ClassFactory.h>
00019 #include <mvsim/FrictionModels/FrictionBase.h>
00020 #include <mvsim/Sensors/SensorBase.h>
00021 #include <mvsim/ControllerBase.h>
00022 
00023 #include <Box2D/Dynamics/b2World.h>
00024 #include <Box2D/Dynamics/b2Body.h>
00025 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
00026 #include <Box2D/Dynamics/b2Fixture.h>
00027 
00028 #include <mrpt/poses/CPose2D.h>
00029 
00030 #include <mrpt/opengl/CSetOfObjects.h>
00031 #include <mrpt/opengl/CSetOfLines.h>
00032 #if MRPT_VERSION<0x199
00033 #include <mrpt/utils/TColor.h>
00034 using mrpt::utils::TColor;
00035 #else
00036 #include <mrpt/img/TColor.h>
00037 using mrpt::img::TColor;
00038 #endif
00039 
00040 #include <string>
00041 #include <map>
00042 #include "CsvLogger.h"
00043 
00044 namespace mvsim
00045 {
00050 class VehicleBase : public VisualObject, public Simulable
00051 {
00052    public:
00055         static VehicleBase* factory(
00056                 World* parent, const rapidxml::xml_node<char>* xml_node);
00058         static VehicleBase* factory(World* parent, const std::string& xml_text);
00059 
00062         static void register_vehicle_class(
00063                 const rapidxml::xml_node<char>* xml_node);
00064 
00065         // ------- Interface with "World" ------
00066         virtual void simul_pre_timestep(
00067                 const TSimulContext& context);  // See derived class docs
00068         virtual void simul_post_timestep(
00069                 const TSimulContext& context);  // See derived class docs
00070         virtual void apply_force(
00071                 double fx, double fy, double local_ptx = 0.0,
00072                 double local_pty = 0.0);  // See derived class docs
00073 
00075         void simul_post_timestep_common(const TSimulContext& context);
00076 
00079         virtual void create_multibody_system(b2World* world);
00080 
00083         virtual float getMaxVehicleRadius() const { return m_max_radius; }
00085         virtual double getChassisMass() const { return m_chassis_mass; }
00086         b2Body* getBox2DChassisBody() { return m_b2d_vehicle_body; }
00087         mrpt::math::TPoint2D getChassisCenterOfMass() const
00088         {
00089                 return m_chassis_com;
00090         }  
00091 
00092         size_t getNumWheels() const { return m_wheels_info.size(); }
00093         const Wheel& getWheelInfo(const size_t idx) const
00094         {
00095                 return m_wheels_info[idx];
00096         }
00097         Wheel& getWheelInfo(const size_t idx) { return m_wheels_info[idx]; }
00098         const mrpt::math::TPose3D& getPose() const
00099         {
00100                 return m_q;
00101         }  
00102 
00103         void setPose(const mrpt::math::TPose3D& p) const
00104         {
00105                 const_cast<mrpt::math::TPose3D&>(m_q) = p;
00106         }  
00107 
00108 
00109         mrpt::poses::CPose2D getCPose2D() const;  
00110 
00112         const vec3& getVelocity() const { return m_dq; }
00116         vec3 getVelocityLocal() const;
00119         void getWheelsVelocityLocal(
00120                 std::vector<mrpt::math::TPoint2D>& vels,
00121                 const vec3& veh_vel_local) const;
00122 
00127         virtual vec3 getVelocityLocalOdoEstimate() const = 0;
00128 
00129         typedef std::vector<SensorBase::Ptr> TListSensors;
00130 
00131         const TListSensors& getSensors() const { return m_sensors; }
00132         TListSensors& getSensors() { return m_sensors; }
00133         std::shared_ptr<CSVLogger> getLoggerPtr(std::string logger_name)
00134         {
00135                 return m_loggers[logger_name];
00136         }
00137 
00139         const std::string& getName() const { return m_name; }
00142         const mrpt::math::TPolygon2D& getChassisShape() const
00143         {
00144                 return m_chassis_poly;
00145         }
00146 
00148         void setVehicleIndex(size_t idx) { m_vehicle_index = idx; }
00150         size_t getVehicleIndex() const { return m_vehicle_index; }
00151         void setRecording(bool record)
00152         {
00153                 for (auto& logger : m_loggers) logger.second->setRecording(record);
00154         }
00155         void clearLogs()
00156         {
00157                 for (auto& logger : m_loggers) logger.second->clear();
00158         }
00159         void newLogSession()
00160         {
00161                 for (auto& logger : m_loggers) logger.second->newSession();
00162         }
00163 
00170         virtual void gui_update(mrpt::opengl::COpenGLScene& scene);
00171 
00172         virtual ControllerBaseInterface* getControllerInterface() = 0;
00173 
00174    protected:
00175         std::map<std::string, std::shared_ptr<CSVLogger>> m_loggers;
00176         std::string m_log_path;
00177 
00178         virtual void initLoggers();
00179         virtual void writeLogStrings();
00180 
00181    protected:
00182         // Protected ctor for class factory
00183         VehicleBase(World* parent, size_t nWheels);
00184 
00188         virtual void dynamics_load_params_from_xml(
00189                 const rapidxml::xml_node<char>* xml_node) = 0;
00190 
00191         virtual void invoke_motor_controllers(
00192                 const TSimulContext& context,
00193                 std::vector<double>& out_force_per_wheel) = 0;
00194 
00201         void gui_update_common(
00202                 mrpt::opengl::COpenGLScene& scene, bool defaultVehicleBody = true);
00203 
00204         std::string
00205                 m_name;  
00206         size_t m_vehicle_index;  
00207 
00208 
00209 
00215         b2Body* m_b2d_vehicle_body;
00216 
00217         FrictionBasePtr m_friction;  
00218 
00219 
00220         TListSensors m_sensors;  
00221 
00222         mrpt::math::TPose3D
00223                 m_q;  
00224         vec3 m_dq;  
00225 
00226 
00227         std::vector<double>
00228                 m_torque_per_wheel;  
00229 
00230         // Chassis info:
00231         double m_chassis_mass;
00232         mrpt::math::TPolygon2D m_chassis_poly;
00233         double m_max_radius;  
00234 
00235         double m_chassis_z_min, m_chassis_z_max;
00236         TColor m_chassis_color;
00237 
00238         mrpt::math::TPoint2D m_chassis_com;  
00239 
00240 
00241         void updateMaxRadiusFromPoly();
00242 
00243         // Wheels info:
00244         std::vector<Wheel> m_wheels_info;  
00245 
00246 
00247 
00248 
00249         // Box2D elements:
00250         b2Fixture* m_fixture_chassis;  
00251         std::vector<b2Fixture*> m_fixture_wheels;  
00252 
00253 
00254 
00255    private:
00256         void internal_gui_update_sensors(
00257                 mrpt::opengl::COpenGLScene&
00258                         scene);  
00259         void internal_gui_update_forces(
00260                 mrpt::opengl::COpenGLScene&
00261                         scene);  
00262 
00263         mrpt::opengl::CSetOfObjects::Ptr m_gl_chassis;
00264         std::vector<mrpt::opengl::CSetOfObjects::Ptr> m_gl_wheels;
00265         mrpt::opengl::CSetOfLines::Ptr m_gl_forces;
00266         std::mutex m_force_segments_for_rendering_cs;
00267         std::vector<mrpt::math::TSegment3D> m_force_segments_for_rendering;
00268 
00269    public:  // data logger header entries
00270         static constexpr char DL_TIMESTAMP[] = "timestamp";
00271         static constexpr char LOGGER_POSE[] = "logger_pose";
00272         static constexpr char LOGGER_WHEEL[] = "logger_wheel";
00273 
00274         static constexpr char PL_Q_X[] = "Qx";
00275         static constexpr char PL_Q_Y[] = "Qy";
00276         static constexpr char PL_Q_Z[] = "Qz";
00277         static constexpr char PL_Q_YAW[] = "Qyaw";
00278         static constexpr char PL_Q_PITCH[] = "Qpitch";
00279         static constexpr char PL_Q_ROLL[] = "Qroll";
00280         static constexpr char PL_DQ_X[] = "dQx";
00281         static constexpr char PL_DQ_Y[] = "dQy";
00282         static constexpr char PL_DQ_Z[] = "dQz";
00283 
00284         static constexpr char WL_TORQUE[] = "torque";
00285         static constexpr char WL_WEIGHT[] = "weight";
00286         static constexpr char WL_VEL_X[] = "velocity_x";
00287         static constexpr char WL_VEL_Y[] = "velocity_y";
00288         static constexpr char WL_FRIC_X[] = "friction_x";
00289         static constexpr char WL_FRIC_Y[] = "friction_y";
00290 };  // end VehicleBase
00291 
00292 // Class factory:
00293 typedef ClassFactory<VehicleBase, World*> TClassFactory_vehicleDynamics;
00294 extern TClassFactory_vehicleDynamics classFactory_vehicleDynamics;
00295 
00296 #define DECLARES_REGISTER_VEHICLE_DYNAMICS(CLASS_NAME) \
00297         DECLARES_REGISTER_CLASS1(CLASS_NAME, VehicleBase, World*)
00298 
00299 #define REGISTER_VEHICLE_DYNAMICS(TEXTUAL_NAME, CLASS_NAME)          \
00300         REGISTER_CLASS1(                                                 \
00301                 TClassFactory_vehicleDynamics, classFactory_vehicleDynamics, \
00302                 TEXTUAL_NAME, CLASS_NAME)
00303 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35