class mrpt::kinematics::CVehicleSimul_DiffDriven

Overview

Simulates the kinematics of a differential-driven planar mobile robot/vehicle, including odometry errors and dynamics limitations.

#include <mrpt/kinematics/CVehicleSimul_DiffDriven.h>

class CVehicleSimul_DiffDriven: public mrpt::kinematics::CVehicleSimulVirtualBase
{
public:
    // typedefs

    typedef CVehicleVelCmd_DiffDriven kinematic_cmd_t;

    // construction

    CVehicleSimul_DiffDriven();

    // methods

    void simulateOneTimeStep(const double dt);
    const mrpt::math::TPose2D& getCurrentGTPose() const;
    void setCurrentGTPose(const mrpt::math::TPose2D& pose);
    const mrpt::math::TPose2D& getCurrentOdometricPose() const;

    template <typename T>
    void setCurrentOdometricPose(const T& pose);

    const mrpt::math::TTwist2D& getCurrentGTVel() const;
    mrpt::math::TTwist2D getCurrentGTVelLocal() const;
    const mrpt::math::TTwist2D& getCurrentOdometricVel() const;
    mrpt::math::TTwist2D getCurrentOdometricVelLocal() const;
    double getTime() const;

    void setOdometryErrors(
        bool enabled,
        double Ax_err_bias = 1e-3,
        double Ax_err_std = 10e-3,
        double Ay_err_bias = 1e-3,
        double Ay_err_std = 10e-3,
        double Aphi_err_bias = mrpt::DEG2RAD(1e-3),
        double Aphi_err_std = mrpt::DEG2RAD(10e-3)
        );

    void resetTime();
    void setDelayModelParams(double TAU_delay_sec = 1.8, double CMD_delay_sec = 0.);
    void setV(double v);
    void setW(double w);
    double getV();
    double getW();
    void movementCommand(double lin_vel, double ang_vel);
    virtual void sendVelCmd(const CVehicleVelCmd& cmd_vel);
    virtual CVehicleVelCmd::Ptr getVelCmdType() const;
};

Inherited Members

public:
    // methods

    virtual void sendVelCmd(const CVehicleVelCmd& cmd_vel) = 0;
    virtual CVehicleVelCmd::Ptr getVelCmdType() const = 0;
    void resetStatus();

Methods

void simulateOneTimeStep(const double dt)

Runs the simulator during “dt” seconds.

It will be split into periods of “m_firmware_control_period”.

const mrpt::math::TPose2D& getCurrentGTPose() const

Returns the instantaneous, ground truth pose in world coordinates.

void setCurrentGTPose(const mrpt::math::TPose2D& pose)

Brute-force move robot to target coordinates (“teleport”)

const mrpt::math::TPose2D& getCurrentOdometricPose() const

Returns the current pose according to (noisy) odometry.

See also:

setOdometryErrors

template <typename T>
void setCurrentOdometricPose(const T& pose)

Brute-force overwrite robot odometry

const mrpt::math::TTwist2D& getCurrentGTVel() const

Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates.

mrpt::math::TTwist2D getCurrentGTVelLocal() const

Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in the robot local frame.

const mrpt::math::TTwist2D& getCurrentOdometricVel() const

Returns the instantaneous, odometric velocity vector (vx,vy,omega) in world coordinates.

mrpt::math::TTwist2D getCurrentOdometricVelLocal() const

Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame.

double getTime() const

Get the current simulation time.

void setOdometryErrors(
    bool enabled,
    double Ax_err_bias = 1e-3,
    double Ax_err_std = 10e-3,
    double Ay_err_bias = 1e-3,
    double Ay_err_std = 10e-3,
    double Aphi_err_bias = mrpt::DEG2RAD(1e-3),
    double Aphi_err_std = mrpt::DEG2RAD(10e-3)
    )

Enable/Disable odometry errors.

Errors in odometry are 1 sigma Gaussian values per second

void resetTime()

Reset all simulator variables to 0 (except the.

simulation time).

See also:

resetTime Reset time counter

resetStatus

void setDelayModelParams(double TAU_delay_sec = 1.8, double CMD_delay_sec = 0.)

Change the model of delays used for the orders sent to the robot.

See also:

movementCommand

void movementCommand(double lin_vel, double ang_vel)

Used to command the robot a desired movement:

Parameters:

lin_vel

Linar velocity (m/s)

ang_vel

Angular velocity (rad/s)

virtual void sendVelCmd(const CVehicleVelCmd& cmd_vel)

Sends a velocity command to the robot.

The number of components and their meaning depends on the vehicle-kinematics derived class

virtual CVehicleVelCmd::Ptr getVelCmdType() const

Gets an empty velocity command object that can be queried to find out the number of velcmd components,…