#include <VehicleBase.h>
Public Types | |
using | Ptr = std::shared_ptr< VehicleBase > |
![]() | |
using | Ptr = std::shared_ptr< Simulable > |
Public Member Functions | |
virtual void | apply_force (const mrpt::math::TVector2D &force, const mrpt::math::TPoint2D &applyPoint=mrpt::math::TPoint2D(0, 0)) override |
void | chassisAndWheelsVisible (bool visible) |
double | chassisZMax () const |
double | chassisZMin () const |
void | clearLogs () |
virtual void | create_multibody_system (b2World &world) |
void | freeOpenGLResources () override |
b2Fixture * | get_fixture_chassis () |
const b2Fixture * | get_fixture_chassis () const |
std::vector< b2Fixture * > & | get_fixture_wheels () |
const std::vector< b2Fixture * > & | get_fixture_wheels () const |
b2Body * | getBox2DChassisBody () |
virtual double | getChassisMass () const |
const mrpt::math::TPolygon2D & | getChassisShape () const |
virtual ControllerBaseInterface * | getControllerInterface ()=0 |
std::shared_ptr< CSVLogger > | getLoggerPtr (std::string logger_name) |
virtual float | getMaxVehicleRadius () const |
size_t | getNumWheels () const |
TListSensors & | getSensors () |
const TListSensors & | getSensors () const |
size_t | getVehicleIndex () const |
virtual mrpt::math::TTwist2D | getVelocityLocalOdoEstimate () const =0 |
Wheel & | getWheelInfo (const size_t idx) |
const Wheel & | getWheelInfo (const size_t idx) const |
std::vector< mrpt::math::TVector2D > | getWheelsVelocityLocal (const mrpt::math::TTwist2D &veh_vel_local) const |
bool | isLogging () const |
void | newLogSession () |
void | registerOnServer (mvsim::Client &c) override |
void | setRecording (bool record) |
void | setVehicleIndex (size_t idx) |
virtual void | simul_post_timestep (const TSimulContext &context) override |
virtual void | simul_pre_timestep (const TSimulContext &context) override |
![]() | |
const std::optional< Shape2p5 > & | collisionShape () const |
bool | customVisualVisible () const |
void | customVisualVisible (const bool visible) |
virtual void | guiUpdate (const mrpt::optional_ref< mrpt::opengl::COpenGLScene > &viz, const mrpt::optional_ref< mrpt::opengl::COpenGLScene > &physical) |
World * | parent () |
const World * | parent () const |
void | showCollisionShape (bool show) |
VisualObject (World *parent, bool insertCustomVizIntoViz=true, bool insertCustomVizIntoPhysical=true) | |
virtual | ~VisualObject () |
![]() | |
b2Body * | b2d_body () |
const b2Body * | b2d_body () const |
mrpt::poses::CPose2D | getCPose2D () const |
Alternative to getPose() More... | |
mrpt::poses::CPose3D | getCPose3D () const |
Alternative to getPose() More... | |
virtual std::optional< float > | getElevationAt ([[maybe_unused]] const mrpt::math::TPoint2D &worldXY) const |
mrpt::math::TVector3D | getLinearAcceleration () const |
const std::string & | getName () const |
mrpt::math::TPose3D | getPose () const |
mrpt::math::TPose3D | getPoseNoLock () const |
No thread-safe version. Used internally only. More... | |
virtual mrpt::math::TPose3D | getRelativePose () const |
World * | getSimulableWorldObject () |
const World * | getSimulableWorldObject () const |
mrpt::math::TTwist2D | getTwist () const |
mrpt::math::TTwist2D | getVelocityLocal () const |
bool | hadCollision () const |
bool | isInCollision () const |
void | resetCollisionFlag () |
void | setName (const std::string &s) |
void | setPose (const mrpt::math::TPose3D &p, bool notifyChange=true) const |
virtual void | setRelativePose (const mrpt::math::TPose3D &p) |
void | setTwist (const mrpt::math::TTwist2D &dq) const |
Simulable (World *parent) | |
Static Public Member Functions | |
static Ptr | factory (World *parent, const rapidxml::xml_node< char > *xml_node) |
static Ptr | factory (World *parent, const std::string &xml_text) |
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More... | |
static void | register_vehicle_class (const World &parent, const rapidxml::xml_node< char > *xml_node) |
![]() | |
static void | FreeOpenGLResources () |
Static Public Attributes | |
static constexpr char | DL_TIMESTAMP [] = "Timestamp" |
static constexpr char | LOGGER_POSE [] = "logger_pose" |
static constexpr char | LOGGER_WHEEL [] = "logger_wheel" |
static constexpr char | PL_DQ_X [] = "dqx" |
static constexpr char | PL_DQ_Y [] = "dqy" |
static constexpr char | PL_DQ_Z [] = "dqz" |
static constexpr char | PL_Q_PITCH [] = "q4pitch" |
static constexpr char | PL_Q_ROLL [] = "q5roll" |
static constexpr char | PL_Q_X [] = "q0x" |
static constexpr char | PL_Q_Y [] = "q1y" |
static constexpr char | PL_Q_YAW [] = "q3yaw" |
static constexpr char | PL_Q_Z [] = "q2z" |
static constexpr char | WL_FRIC_X [] = "friction_x" |
static constexpr char | WL_FRIC_Y [] = "friction_y" |
static constexpr char | WL_TORQUE [] = "torque" |
static constexpr char | WL_VEL_X [] = "velocity_x" |
static constexpr char | WL_VEL_Y [] = "velocity_y" |
static constexpr char | WL_WEIGHT [] = "weight" |
![]() | |
static double | GeometryEpsilon = 1e-3 |
Protected Member Functions | |
virtual void | dynamics_load_params_from_xml (const rapidxml::xml_node< char > *xml_node)=0 |
virtual void | initLoggers () |
virtual void | internalGuiUpdate (const mrpt::optional_ref< mrpt::opengl::COpenGLScene > &viz, const mrpt::optional_ref< mrpt::opengl::COpenGLScene > &physical, bool childrenOnly) override |
virtual std::vector< double > | invoke_motor_controllers (const TSimulContext &context)=0 |
virtual void | invoke_motor_controllers_post_step ([[maybe_unused]] const TSimulContext &context) |
VisualObject * | meAsVisualObject () override |
void | updateMaxRadiusFromPoly () |
VehicleBase (World *parent, size_t nWheels) | |
virtual void | writeLogStrings () |
![]() | |
void | addCustomVisualization (const mrpt::opengl::CRenderizable::Ptr &glModel, const mrpt::poses::CPose3D &modelPose={}, const float modelScale=1.0f, const std::string &modelName="group", const std::optional< std::string > &modelURI=std::nullopt, const bool initialShowBoundingBox=false) |
bool | parseVisual (const JointXMLnode<> &rootNode) |
bool | parseVisual (const rapidxml::xml_node< char > &rootNode) |
Returns true if there is at least one <visual>...</visual> entry. More... | |
void | setCollisionShape (const Shape2p5 &cs) |
Protected Attributes | |
mrpt::img::TColor | chassis_color_ {0xff, 0x00, 0x00} |
mrpt::math::TPoint2D | chassis_com_ {0, 0} |
double | chassis_mass_ = 15.0 |
mrpt::math::TPolygon2D | chassis_poly_ |
double | chassis_z_max_ = 0.6 |
double | chassis_z_min_ = 0.05 |
b2Fixture * | fixture_chassis_ |
Created at. More... | |
std::vector< b2Fixture * > | fixture_wheels_ |
FrictionBasePtr | friction_ |
std::string | log_path_ |
std::map< std::string, std::shared_ptr< CSVLogger > > | loggers_ |
double | maxRadius_ = 0.1 |
TListSensors | sensors_ |
Sensors aboard. More... | |
size_t | vehicle_index_ = 0 |
std::deque< Wheel > | wheels_info_ |
![]() | |
std::shared_ptr< mrpt::opengl::CSetOfObjects > | glCollision_ |
std::shared_ptr< mrpt::opengl::CSetOfObjects > | glCustomVisual_ |
int32_t | glCustomVisualId_ = -1 |
const bool | insertCustomVizIntoPhysical_ = true |
const bool | insertCustomVizIntoViz_ = true |
World * | world_ |
![]() | |
std::string | name_ |
Private Member Functions | |
void | internal_internalGuiUpdate_forces (mrpt::opengl::COpenGLScene &scene) |
void | internal_internalGuiUpdate_sensors (const mrpt::optional_ref< mrpt::opengl::COpenGLScene > &viz, const mrpt::optional_ref< mrpt::opengl::COpenGLScene > &physical) |
Private Attributes | |
std::vector< mrpt::math::TSegment3D > | forceSegmentsForRendering_ |
std::mutex | forceSegmentsForRenderingMtx_ |
mrpt::opengl::CSetOfObjects::Ptr | glChassisPhysical_ |
mrpt::opengl::CSetOfObjects::Ptr | glChassisViz_ |
mrpt::opengl::CSetOfLines::Ptr | glForces_ |
std::atomic_bool | glInit_ = false |
mrpt::opengl::CSetOfLines::Ptr | glMotorTorques_ |
std::vector< mrpt::opengl::CSetOfObjects::Ptr > | glWheelsPhysical_ |
std::vector< mrpt::opengl::CSetOfObjects::Ptr > | glWheelsViz_ |
std::vector< mrpt::math::TSegment3D > | torqueSegmentsForRendering_ |
Virtual base class for each vehicle "actor" in the simulation. Derived classes implements different dynamical models (Differential, Ackermann,...)
Definition at line 44 of file VehicleBase.h.
using mvsim::VehicleBase::Ptr = std::shared_ptr<VehicleBase> |
Definition at line 47 of file VehicleBase.h.
|
protected |
Definition at line 78 of file VehicleBase.cpp.
|
overridevirtual |
Override to register external forces exerted by other WorldElements. Force is (fx,fy) in global coordinates. Application point is (local_ptx,local_pty) in the body local frame
Reimplemented from mvsim::Simulable.
Definition at line 802 of file VehicleBase.cpp.
void VehicleBase::chassisAndWheelsVisible | ( | bool | visible | ) |
Definition at line 818 of file VehicleBase.cpp.
|
inline |
Definition at line 141 of file VehicleBase.h.
|
inline |
Definition at line 140 of file VehicleBase.h.
|
inline |
Definition at line 116 of file VehicleBase.h.
Create bodies, fixtures, etc. for the dynamical simulation. May be overrided by derived classes
Create bodies, fixtures, etc. for the dynamical simulation
Definition at line 590 of file VehicleBase.cpp.
|
protectedpure virtual |
Parse node <dynamics>: The derived-class part of load_params_from_xml(), also called in factory(). Includes parsing the <controller></controller> block.
Implemented in mvsim::DynamicsDifferential, mvsim::DynamicsAckermannDrivetrain, and mvsim::DynamicsAckermann.
|
static |
Class factory: Creates a vehicle from XML description of type "<vehicle>...</vehicle>".
Definition at line 118 of file VehicleBase.cpp.
|
static |
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
Definition at line 338 of file VehicleBase.cpp.
|
inlineoverridevirtual |
Reimplemented from mvsim::Simulable.
Definition at line 134 of file VehicleBase.h.
|
inline |
Definition at line 129 of file VehicleBase.h.
|
inline |
Definition at line 131 of file VehicleBase.h.
|
inline |
Definition at line 130 of file VehicleBase.h.
|
inline |
Definition at line 132 of file VehicleBase.h.
|
inline |
Definition at line 76 of file VehicleBase.h.
|
inlinevirtual |
Get the overall vehicle mass, excluding wheels.
Definition at line 75 of file VehicleBase.h.
|
inline |
Get the 2D shape of the vehicle chassis, as set from the config file (only used for collision detection)
Definition at line 106 of file VehicleBase.h.
|
pure virtual |
Implemented in mvsim::DynamicsDifferential, mvsim::DynamicsAckermannDrivetrain, and mvsim::DynamicsAckermann.
|
inline |
Definition at line 99 of file VehicleBase.h.
|
inlinevirtual |
Get (an approximation of) the max radius of the vehicle, from its point of reference (in meters)
Definition at line 73 of file VehicleBase.h.
|
inline |
Definition at line 82 of file VehicleBase.h.
|
inline |
Definition at line 98 of file VehicleBase.h.
|
inline |
Definition at line 97 of file VehicleBase.h.
|
inline |
Get the vehicle index in the World
Definition at line 111 of file VehicleBase.h.
|
pure virtual |
Gets the current estimation of odometry-based velocity as reconstructed solely from wheels spinning velocities and geometry. This is the input of any realistic low-level controller onboard.
Implemented in mvsim::DynamicsDifferential, mvsim::DynamicsAckermannDrivetrain, and mvsim::DynamicsAckermann.
|
inline |
Definition at line 84 of file VehicleBase.h.
|
inline |
Definition at line 83 of file VehicleBase.h.
std::vector< mrpt::math::TVector2D > VehicleBase::getWheelsVelocityLocal | ( | const mrpt::math::TTwist2D & | veh_vel_local | ) | const |
Current velocity of each wheel's center point (in local coords). Call with veh_vel_local=getVelocityLocal() for ground-truth.
Last time-step velocity of each wheel's center point (in local coords)
Definition at line 518 of file VehicleBase.cpp.
|
protectedvirtual |
Definition at line 780 of file VehicleBase.cpp.
|
private |
Definition at line 548 of file VehicleBase.cpp.
|
private |
Definition at line 541 of file VehicleBase.cpp.
|
overrideprotectedvirtual |
Implements mvsim::VisualObject.
Definition at line 666 of file VehicleBase.cpp.
|
protectedpure virtual |
Implemented in mvsim::DynamicsDifferential, mvsim::DynamicsAckermannDrivetrain, and mvsim::DynamicsAckermann.
|
inlineprotectedvirtual |
Definition at line 164 of file VehicleBase.h.
bool mvsim::VehicleBase::isLogging | ( | ) | const |
Definition at line 569 of file VehicleBase.cpp.
|
inlineoverrideprotectedvirtual |
Reimplemented from mvsim::Simulable.
Definition at line 168 of file VehicleBase.h.
|
inline |
Definition at line 120 of file VehicleBase.h.
|
static |
Register a new class of vehicles from XML description of type "<vehicle:class name='name'>...</vehicle:class>".
Definition at line 96 of file VehicleBase.cpp.
|
overridevirtual |
Reimplemented from mvsim::Simulable.
Definition at line 811 of file VehicleBase.cpp.
|
inline |
Definition at line 112 of file VehicleBase.h.
|
inline |
Set the vehicle index in the World
Definition at line 109 of file VehicleBase.h.
|
overridevirtual |
Override to do any required process right after the integration of dynamic equations for each timestep
Reimplemented from mvsim::Simulable.
Definition at line 466 of file VehicleBase.cpp.
|
overridevirtual |
Process right before the integration of dynamic equations for each timestep: set action forces from motors, update friction models, etc.
Reimplemented from mvsim::Simulable.
Definition at line 360 of file VehicleBase.cpp.
|
protected |
Definition at line 577 of file VehicleBase.cpp.
|
protectedvirtual |
Definition at line 793 of file VehicleBase.cpp.
|
protected |
Definition at line 189 of file VehicleBase.h.
|
protected |
center of mass. in local coordinates (this excludes the mass of wheels)
Definition at line 193 of file VehicleBase.h.
|
protected |
Definition at line 180 of file VehicleBase.h.
|
protected |
Definition at line 181 of file VehicleBase.h.
|
protected |
Definition at line 187 of file VehicleBase.h.
|
protected |
Definition at line 187 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 229 of file VehicleBase.h.
|
protected |
Created at.
Definition at line 204 of file VehicleBase.h.
|
protected |
[0]:rear-left, etc. (depending on derived class). Size set at constructor.
Definition at line 208 of file VehicleBase.h.
|
private |
Definition at line 224 of file VehicleBase.h.
|
private |
Definition at line 226 of file VehicleBase.h.
|
protected |
Instance of friction model for the vehicle-to-ground interaction.
Definition at line 175 of file VehicleBase.h.
|
private |
Definition at line 218 of file VehicleBase.h.
|
private |
Definition at line 218 of file VehicleBase.h.
|
private |
Definition at line 220 of file VehicleBase.h.
|
private |
Definition at line 222 of file VehicleBase.h.
|
private |
Definition at line 221 of file VehicleBase.h.
|
private |
Definition at line 219 of file VehicleBase.h.
|
private |
Definition at line 219 of file VehicleBase.h.
|
protected |
Definition at line 145 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 230 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 231 of file VehicleBase.h.
|
protected |
Definition at line 144 of file VehicleBase.h.
|
protected |
Automatically computed from chassis_poly_ upon each change via updateMaxRadiusFromPoly()
Definition at line 185 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 239 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 240 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 241 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 237 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 238 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 233 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 234 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 236 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 235 of file VehicleBase.h.
|
protected |
Sensors aboard.
Definition at line 177 of file VehicleBase.h.
|
private |
Definition at line 225 of file VehicleBase.h.
|
protected |
user-supplied index number: must be set/get'ed with setVehicleIndex() getVehicleIndex() (default=0)
Definition at line 172 of file VehicleBase.h.
|
protected |
Wheels info. The fixed size of this vector is set upon construction. Derived classes must define the order of the wheels, e.g. [0]=rear left, etc.
Definition at line 201 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 247 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 248 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 243 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 245 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 246 of file VehicleBase.h.
|
staticconstexpr |
Definition at line 244 of file VehicleBase.h.