Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
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
00066 virtual void simul_pre_timestep(
00067 const TSimulContext& context);
00068 virtual void simul_post_timestep(
00069 const TSimulContext& context);
00070 virtual void apply_force(
00071 double fx, double fy, double local_ptx = 0.0,
00072 double local_pty = 0.0);
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
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
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
00244 std::vector<Wheel> m_wheels_info;
00245
00246
00247
00248
00249
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:
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 };
00291
00292
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 }