#include <VehicleBase.h>
Public Types | |
typedef std::vector < SensorBase::Ptr > | TListSensors |
Public Member Functions | |
virtual void | apply_force (double fx, double fy, double local_ptx=0.0, double local_pty=0.0) |
void | clearLogs () |
virtual void | create_multibody_system (b2World *world) |
b2Body * | getBox2DChassisBody () |
mrpt::math::TPoint2D | getChassisCenterOfMass () const |
In local coordinates (this excludes the mass of wheels) | |
virtual double | getChassisMass () const |
const mrpt::math::TPolygon2D & | getChassisShape () const |
virtual ControllerBaseInterface * | getControllerInterface ()=0 |
mrpt::poses::CPose2D | getCPose2D () const |
std::shared_ptr< CSVLogger > | getLoggerPtr (std::string logger_name) |
virtual float | getMaxVehicleRadius () const |
const std::string & | getName () const |
size_t | getNumWheels () const |
const mrpt::math::TPose3D & | getPose () const |
const TListSensors & | getSensors () const |
TListSensors & | getSensors () |
size_t | getVehicleIndex () const |
const vec3 & | getVelocity () const |
vec3 | getVelocityLocal () const |
virtual vec3 | getVelocityLocalOdoEstimate () const =0 |
const Wheel & | getWheelInfo (const size_t idx) const |
Wheel & | getWheelInfo (const size_t idx) |
void | getWheelsVelocityLocal (std::vector< mrpt::math::TPoint2D > &vels, const vec3 &veh_vel_local) const |
virtual void | gui_update (mrpt::opengl::COpenGLScene &scene) |
void | newLogSession () |
void | setPose (const mrpt::math::TPose3D &p) const |
void | setRecording (bool record) |
void | setVehicleIndex (size_t idx) |
virtual void | simul_post_timestep (const TSimulContext &context) |
void | simul_post_timestep_common (const TSimulContext &context) |
virtual void | simul_pre_timestep (const TSimulContext &context) |
Static Public Member Functions | |
static VehicleBase * | factory (World *parent, const rapidxml::xml_node< char > *xml_node) |
static VehicleBase * | factory (World *parent, const std::string &xml_text) |
static void | register_vehicle_class (const rapidxml::xml_node< char > *xml_node) |
Protected Member Functions | |
virtual void | dynamics_load_params_from_xml (const rapidxml::xml_node< char > *xml_node)=0 |
void | gui_update_common (mrpt::opengl::COpenGLScene &scene, bool defaultVehicleBody=true) |
virtual void | initLoggers () |
virtual void | invoke_motor_controllers (const TSimulContext &context, std::vector< double > &out_force_per_wheel)=0 |
VehicleBase (World *parent, size_t nWheels) | |
virtual void | writeLogStrings () |
Protected Attributes | |
std::string | m_log_path |
std::map< std::string, std::shared_ptr< CSVLogger > > | m_loggers |
std::string | m_name |
User-supplied name of the vehicle (e.g. "r1", "veh1") | |
size_t | m_vehicle_index |
Virtual base class for each vehicle "actor" in the simulation. Derived classes implements different dynamical models (Differential, Ackermann,...)
Definition at line 50 of file VehicleBase.h.
typedef std::vector<SensorBase::Ptr> mvsim::VehicleBase::TListSensors |
Definition at line 129 of file VehicleBase.h.
VehicleBase::VehicleBase | ( | World * | parent, |
size_t | nWheels | ||
) | [protected] |
Definition at line 77 of file VehicleBase.cpp.
void VehicleBase::apply_force | ( | double | fx, |
double | fy, | ||
double | local_ptx = 0.0 , |
||
double | local_pty = 0.0 |
||
) | [virtual] |
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 766 of file VehicleBase.cpp.
void mvsim::VehicleBase::clearLogs | ( | ) | [inline] |
Definition at line 155 of file VehicleBase.h.
void VehicleBase::create_multibody_system | ( | b2World * | world | ) | [virtual] |
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 635 of file VehicleBase.cpp.
virtual void mvsim::VehicleBase::dynamics_load_params_from_xml | ( | const rapidxml::xml_node< char > * | xml_node | ) | [protected, pure 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::DynamicsAckermannDrivetrain, mvsim::DynamicsAckermann, and mvsim::DynamicsDifferential.
VehicleBase * VehicleBase::factory | ( | World * | parent, |
const rapidxml::xml_node< char > * | root | ||
) | [static] |
Class factory: Creates a vehicle from XML description of type "<vehicle>...</vehicle>".
Definition at line 150 of file VehicleBase.cpp.
VehicleBase * VehicleBase::factory | ( | World * | parent, |
const std::string & | xml_text | ||
) | [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 336 of file VehicleBase.cpp.
b2Body* mvsim::VehicleBase::getBox2DChassisBody | ( | ) | [inline] |
Definition at line 86 of file VehicleBase.h.
mrpt::math::TPoint2D mvsim::VehicleBase::getChassisCenterOfMass | ( | ) | const [inline] |
In local coordinates (this excludes the mass of wheels)
Definition at line 87 of file VehicleBase.h.
virtual double mvsim::VehicleBase::getChassisMass | ( | ) | const [inline, virtual] |
Get the overall vehicle mass, excluding wheels.
Definition at line 85 of file VehicleBase.h.
const mrpt::math::TPolygon2D& mvsim::VehicleBase::getChassisShape | ( | ) | const [inline] |
Get the 2D shape of the vehicle chassis, as set from the config file (only used for collision detection)
Definition at line 142 of file VehicleBase.h.
virtual ControllerBaseInterface* mvsim::VehicleBase::getControllerInterface | ( | ) | [pure virtual] |
Implemented in mvsim::DynamicsAckermannDrivetrain, mvsim::DynamicsAckermann, and mvsim::DynamicsDifferential.
mrpt::poses::CPose2D VehicleBase::getCPose2D | ( | ) | const |
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 536 of file VehicleBase.cpp.
std::shared_ptr<CSVLogger> mvsim::VehicleBase::getLoggerPtr | ( | std::string | logger_name | ) | [inline] |
Definition at line 133 of file VehicleBase.h.
virtual float mvsim::VehicleBase::getMaxVehicleRadius | ( | ) | const [inline, virtual] |
Get (an approximation of) the max radius of the vehicle, from its point of reference (in meters)
Definition at line 83 of file VehicleBase.h.
const std::string& mvsim::VehicleBase::getName | ( | ) | const [inline] |
User-supplied name of the vehicle (e.g. "r1", "veh1")
Definition at line 139 of file VehicleBase.h.
size_t mvsim::VehicleBase::getNumWheels | ( | ) | const [inline] |
Definition at line 92 of file VehicleBase.h.
const mrpt::math::TPose3D& mvsim::VehicleBase::getPose | ( | ) | const [inline] |
Last time-step pose (of the ref. point, in global coords) (ground-truth)
Definition at line 98 of file VehicleBase.h.
const TListSensors& mvsim::VehicleBase::getSensors | ( | ) | const [inline] |
Definition at line 131 of file VehicleBase.h.
TListSensors& mvsim::VehicleBase::getSensors | ( | ) | [inline] |
Definition at line 132 of file VehicleBase.h.
size_t mvsim::VehicleBase::getVehicleIndex | ( | ) | const [inline] |
Get the vehicle index in the World
Definition at line 150 of file VehicleBase.h.
const vec3& mvsim::VehicleBase::getVelocity | ( | ) | const [inline] |
Last time-step velocity (of the ref. point, in global coords) (ground-truth)
Definition at line 112 of file VehicleBase.h.
vec3 VehicleBase::getVelocityLocal | ( | ) | const |
Last time-step velocity (of the ref. point, in local coords) (ground-truth)
Last time-step velocity (of the ref. point, in local coords)
Definition at line 525 of file VehicleBase.cpp.
virtual vec3 mvsim::VehicleBase::getVelocityLocalOdoEstimate | ( | ) | const [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::DynamicsAckermannDrivetrain, mvsim::DynamicsAckermann, and mvsim::DynamicsDifferential.
const Wheel& mvsim::VehicleBase::getWheelInfo | ( | const size_t | idx | ) | const [inline] |
Definition at line 93 of file VehicleBase.h.
Wheel& mvsim::VehicleBase::getWheelInfo | ( | const size_t | idx | ) | [inline] |
Definition at line 97 of file VehicleBase.h.
void VehicleBase::getWheelsVelocityLocal | ( | std::vector< mrpt::math::TPoint2D > & | vels, |
const vec3 & | 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 503 of file VehicleBase.cpp.
void VehicleBase::gui_update | ( | mrpt::opengl::COpenGLScene & | scene | ) | [virtual] |
Must create a new object in the scene and/or update it according to the current state. If overrided in derived classes, it may be time-saving to call gui_update_common() and associated methods for 3D elements common to any vehicle.
Implements mvsim::VisualObject.
Definition at line 712 of file VehicleBase.cpp.
void VehicleBase::gui_update_common | ( | mrpt::opengl::COpenGLScene & | scene, |
bool | defaultVehicleBody = true |
||
) | [protected] |
To be called at derived classes' gui_update(), updates all stuff common to any vehicle type. Calls: internal_gui_update_sensors(), internal_gui_update_forces()
[in] | defaultVehicleBody | If true, will draw default wheels & vehicle chassis. |
To be called at derived classes' gui_update()
Definition at line 542 of file VehicleBase.cpp.
void VehicleBase::initLoggers | ( | ) | [protected, virtual] |
Definition at line 717 of file VehicleBase.cpp.
virtual void mvsim::VehicleBase::invoke_motor_controllers | ( | const TSimulContext & | context, |
std::vector< double > & | out_force_per_wheel | ||
) | [protected, pure virtual] |
Implemented in mvsim::DynamicsAckermannDrivetrain, mvsim::DynamicsAckermann, and mvsim::DynamicsDifferential.
void mvsim::VehicleBase::newLogSession | ( | ) | [inline] |
Definition at line 159 of file VehicleBase.h.
void VehicleBase::register_vehicle_class | ( | const rapidxml::xml_node< char > * | xml_node | ) | [static] |
Register a new class of vehicles from XML description of type "<vehicle:class name='name'>...</vehicle:class>".
Definition at line 126 of file VehicleBase.cpp.
void mvsim::VehicleBase::setPose | ( | const mrpt::math::TPose3D & | p | ) | const [inline] |
Manually override vehicle pose (Use with caution!) (purposely set as "const")
Definition at line 103 of file VehicleBase.h.
void mvsim::VehicleBase::setRecording | ( | bool | record | ) | [inline] |
Definition at line 151 of file VehicleBase.h.
void mvsim::VehicleBase::setVehicleIndex | ( | size_t | idx | ) | [inline] |
Set the vehicle index in the World
Definition at line 148 of file VehicleBase.h.
void VehicleBase::simul_post_timestep | ( | const TSimulContext & | context | ) | [virtual] |
Override to do any required process right after the integration of dynamic equations for each timestep
Reimplemented from mvsim::Simulable.
Definition at line 463 of file VehicleBase.cpp.
void VehicleBase::simul_post_timestep_common | ( | const TSimulContext & | context | ) |
Gets the body dynamical state into q, dot{q}
Definition at line 102 of file VehicleBase.cpp.
void VehicleBase::simul_pre_timestep | ( | const TSimulContext & | context | ) | [virtual] |
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.
void VehicleBase::writeLogStrings | ( | ) | [protected, virtual] |
Definition at line 757 of file VehicleBase.cpp.
std::string mvsim::VehicleBase::m_log_path [protected] |
Definition at line 176 of file VehicleBase.h.
std::map<std::string, std::shared_ptr<CSVLogger> > mvsim::VehicleBase::m_loggers [protected] |
Definition at line 175 of file VehicleBase.h.
std::string mvsim::VehicleBase::m_name [protected] |
User-supplied name of the vehicle (e.g. "r1", "veh1")
Definition at line 205 of file VehicleBase.h.
size_t mvsim::VehicleBase::m_vehicle_index [protected] |
user-supplied index number: must be set/get'ed
Definition at line 206 of file VehicleBase.h.