Visual Servoing Platform  version 3.6.1 under development (2024-10-01)
vpRobotMavsdk Class Reference

#include <visp3/robot/vpRobotMavsdk.h>

Public Member Functions

 vpRobotMavsdk ()
 
 vpRobotMavsdk (const std::string &connection_info)
 
virtual ~vpRobotMavsdk ()
 
Robot connection
void connect (const std::string &connection_info)
 
General robot information
float getBatteryLevel () const
 
void getPosition (float &ned_north, float &ned_east, float &ned_down, float &ned_yaw) const
 
void getPosition (vpHomogeneousMatrix &ned_M_frd) const
 
std::tuple< float, float > getHome () const
 
std::string getAddress () const
 
bool isRunning () const
 

Robot commands

bool arm ()
 
bool disarm ()
 
void doFlatTrim ()
 
bool hasFlyingCapability ()
 
bool holdPosition ()
 
bool kill ()
 
bool land ()
 
bool releaseControl ()
 
bool sendMocapData (const vpHomogeneousMatrix &enu_M_flu, int display_fps=1)
 
void setAutoLand (bool auto_land)
 
bool setForwardSpeed (double body_frd_vx)
 
bool setLateralSpeed (double body_frd_vy)
 
bool setGPSGlobalOrigin (double latitude, double longitude, double altitude)
 
void setPositioningIncertitude (float position_incertitude, float yaw_incertitude)
 
bool setPosition (float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking=true, int timeout_sec=10)
 
bool setPosition (const vpHomogeneousMatrix &ned_M_frd, bool blocking=true, int timeout_sec=10)
 
bool setPositionRelative (float ned_delta_north, float ned_delta_east, float ned_delta_down, float ned_delta_yaw, bool blocking=true, int timeout_sec=10)
 
bool setPositionRelative (const vpHomogeneousMatrix &delta_frd_M_frd, bool blocking=true, int timeout_sec=10)
 
bool setVelocity (const vpColVector &frd_vel_cmd)
 
bool setVerticalSpeed (double body_frd_vz)
 
bool setYawSpeed (double body_frd_wz)
 
void setTakeOffAlt (double altitude)
 
void setVerbose (bool verbose)
 
bool stopMoving ()
 
bool takeControl ()
 
bool takeOff (bool interactive=true, int timeout_sec=10, bool use_gps=false)
 
bool takeOff (bool interactive, double takeoff_altitude, int timeout_sec=10, bool use_gps=false)
 

Detailed Description

Interface for Mavlink allowing to control drones or rovers using a MavLink compatible controller such a Pixhawk running PX4 or Ardupilot.

This class needs cxx17 or more recent standard enabled during ViSP cmake configuration.

This class is enabled when MavSDK C++ is installed and detected by ViSP during cmake configuration step.

Note
The body frame associated to the vehicle controlled through MavLink is supposed to be Front-Right-Down (FRD) respectively for X-Y-Z.
  1. This class was tested to control a quadcopter equipped with a Pixhawk running PX4 firmware connected to a Jetson TX2.

    We provide a set of tests if you want to have a try on your flying vehicle:

    We provide also this Tutorial: Image-based visual-servoing on a drone equipped with a Pixhawk.

  2. This class was also tested to control an AION ROBOTICS rover equipped with a Pixhawk running Ardupilot firmware directly connected by serial to a laptop running Ubuntu 22.04.

    If you want to have a try you may see:

See also
Tutorial: Image-based visual-servoing on a drone equipped with a Pixhawk
Examples
sendMocapToPixhawk.cpp, servoPixhawkDroneIBVS.cpp, testPixhawkDroneKeyboard.cpp, testPixhawkDronePositionAbsoluteControl.cpp, testPixhawkDronePositionRelativeControl.cpp, testPixhawkDroneTakeoff.cpp, testPixhawkDroneVelocityControl.cpp, and testPixhawkRoverVelocityControl.cpp.

Definition at line 93 of file vpRobotMavsdk.h.

Constructor & Destructor Documentation

◆ vpRobotMavsdk() [1/2]

vpRobotMavsdk::vpRobotMavsdk ( )

Default constructor without parameters. You need to use the connect() function afterwards.

Set default positioning incertitude to 0.05 meter in translation, and 5 degrees along yaw orientation. These default values are used to determine when a position is reached and could be changed using setPositioningIncertitude(). When the vehicle has flying capabilities, call by default land() in the destructor. This behavior could be changed using setAutoLand().

To control the vehicle using this class, you need to call takeControl() to start the off-board mode with PX4 or the guided mode with Ardupilot. After this call you can call setPosition() to move the vehicle to a desired position and yaw orientation or call setVelocity() to move the vehicle in velocity.

See also
connect(), setPositioningIncertitude()

Definition at line 1168 of file vpRobotMavsdk.cpp.

References vpMath::rad().

◆ vpRobotMavsdk() [2/2]

BEGIN_VISP_NAMESPACE vpRobotMavsdk::vpRobotMavsdk ( const std::string &  connection_info)

Constructor.

Initializes vehicle controller, by discovering vehicles connected either with an Ethernet TCP or UDP link, or with a serial link the computer is currently connected to.

Warning
This constructor should be called after the vehicle is turned on, and after the computer is connected to the vehicle Ethernet network or with a serial link.
If the connection to the vehicle failed, the program will throw an exception.

After having called this constructor, it is recommended to check if the vehicle is running with isRunning() before sending commands to the vehicle.

Set default positioning incertitude to 0.05 meter in translation, and 5 degrees along yaw orientation. These default values are used to determine when a position is reached and could be changed using setPositioningIncertitude(). When the vehicle has flying capabilities, call by default land() in the destructor. This behavior could be changed using setAutoLand().

To control the vehicle using this class, you need to call takeControl() to start the off-board mode with PX4 or the guided mode with Ardupilot. After this call you can call setPosition() to move the vehicle to a desired position and yaw orientation or call setVelocity() to move the vehicle in velocity.

Parameters
[in]connection_info: Specify connection information. This parameter must be written following these conventions:
  • for TCP link: tcp://[server_host][:server_port]
  • for UDP link: udp://[bind_host][:bind_port]
  • for Serial link: serial:///path/to/serial/dev[:baudrate]
    Examples: udp://192.168.30.111:14550 or serial:///dev/ttyACMO

For more information see here.

Exceptions
vpException::fatalError: If the program failed to connect to the vehicle.
See also
setPositioningIncertitude(), setAutoLand(), takeControl(), releaseControl()

Definition at line 1149 of file vpRobotMavsdk.cpp.

References vpMath::rad().

◆ ~vpRobotMavsdk()

vpRobotMavsdk::~vpRobotMavsdk ( )
virtual

Destructor. When the vehicle has flying capabilities and when auto land mode is enabled, lands the vehicle if not landed and safely disconnects everything.

See also
setAutoLand()

Definition at line 1180 of file vpRobotMavsdk.cpp.

Member Function Documentation

◆ arm()

bool vpRobotMavsdk::arm ( )

Arms the vehicle.

Returns
true if arming is successful, false otherwise.

Definition at line 1287 of file vpRobotMavsdk.cpp.

◆ connect()

void vpRobotMavsdk::connect ( const std::string &  connection_info)

Connects to the vehicle and setups the different controllers.

Parameters
[in]connection_info: The connection information given to connect to the vehicle. You may use:
  • for TCP link: tcp://[server_host][:server_port]
  • for UDP link: udp://[bind_host][:bind_port]
  • for Serial link: serial:///path/to/serial/dev[:baudrate]
    Examples: udp://192.168.30.111:14550 or serial:///dev/ttyACMO

For more information see here.

See also
getAddress()

Definition at line 1194 of file vpRobotMavsdk.cpp.

◆ disarm()

bool vpRobotMavsdk::disarm ( )

Disarms the vehicle.

Returns
true if disarming is successful, false otherwise.

Definition at line 1293 of file vpRobotMavsdk.cpp.

◆ doFlatTrim()

void vpRobotMavsdk::doFlatTrim ( )

Sends a flat trim command to the vehicle, to calibrate accelerometer and gyro.

Warning
Should be executed only when the vehicle is on a flat surface.

Definition at line 1272 of file vpRobotMavsdk.cpp.

◆ getAddress()

std::string vpRobotMavsdk::getAddress ( ) const

Gives the address given to connect to the vehicle.

Returns
: A string corresponding to the Ethernet or serial address used for the connection to the vehicle.
See also
connect()

Definition at line 1232 of file vpRobotMavsdk.cpp.

◆ getBatteryLevel()

float vpRobotMavsdk::getBatteryLevel ( ) const

Gets current battery level in volts.

Warning
When the vehicle battery gets below a certain threshold (around 14.8 for a 4S battery), you should recharge it.

Definition at line 1239 of file vpRobotMavsdk.cpp.

◆ getHome()

std::tuple< float, float > vpRobotMavsdk::getHome ( ) const

Gets the robot home position in GPS coord.

Warning
Only available if the GPS is initialized, for example in simulation.

Definition at line 1265 of file vpRobotMavsdk.cpp.

◆ getPosition() [1/2]

void vpRobotMavsdk::getPosition ( float &  ned_north,
float &  ned_east,
float &  ned_down,
float &  ned_yaw 
) const

Gets the current vehicle FRD position in its local NED frame.

Parameters
[in]ned_north: Position of the vehicle along NED north axis in [m].
[in]ned_east: Position of the vehicle along NED east axis in [m].
[in]ned_down: Position of the vehicle along NED down axis in [m].
[in]ned_yaw: Yaw angle in [rad] of the vehicle along NED down axis.

Definition at line 1254 of file vpRobotMavsdk.cpp.

◆ getPosition() [2/2]

void vpRobotMavsdk::getPosition ( vpHomogeneousMatrix ned_M_frd) const

Gets the current vehicle FRD position in its local NED frame.

Parameters
[in]ned_M_frd: Homogeneous matrix describing the position and attitude of the vehicle returned by telemetry.

Definition at line 1245 of file vpRobotMavsdk.cpp.

◆ hasFlyingCapability()

bool vpRobotMavsdk::hasFlyingCapability ( )

Return true if the vehicle has flying capabilities. Ground rover, surface boat and submarine vehicles are considered with non flying capabilities, while all the other vehicles are considered with flying capabilities.

Definition at line 1581 of file vpRobotMavsdk.cpp.

◆ holdPosition()

bool vpRobotMavsdk::holdPosition ( )

Makes the vehicle hold its position.

Warning
When the vehicle is equipped with a GPS, switches to hold mode. It means that takeControl() needs to be called after.
Returns
true when success, false otherwise.

Definition at line 1341 of file vpRobotMavsdk.cpp.

◆ isRunning()

bool vpRobotMavsdk::isRunning ( ) const

Checks if the vehicle is running, ie if the vehicle is connected and ready to receive commands.

Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1199 of file vpRobotMavsdk.cpp.

◆ kill()

bool vpRobotMavsdk::kill ( )

Cuts the motors like a kill switch. Should only be used in emergency cases.

Returns
true if the cut motors command was successful, false otherwise.
Warning
If your vehicle has flying capabilities, it will fall.
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1457 of file vpRobotMavsdk.cpp.

◆ land()

bool vpRobotMavsdk::land ( )

Sends landing command if the vehicle has flying capabilities.

Returns
  • If the vehicle has flying capabilities, returns true if the landing is successful, false otherwise.
  • If the vehicle doesn't have flying capabilities, returns true.
See also
takeOff(), hasFlyingCapability()
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1357 of file vpRobotMavsdk.cpp.

◆ releaseControl()

bool vpRobotMavsdk::releaseControl ( )

Release control allowing running software outside of the autopilot:

  • When using the PX4 flight stack stop the off-board mode,
  • When using Ardupilot stack stop the guided mode.
Returns
true when off-board or guided mode are successfully stopped, false otherwise.
See also
takeControl()

Definition at line 1533 of file vpRobotMavsdk.cpp.

◆ sendMocapData()

bool vpRobotMavsdk::sendMocapData ( const vpHomogeneousMatrix enu_M_flu,
int  display_fps = 1 
)

Sends MoCap position data to the vehicle.

We consider here that the MoCap global reference frame is ENU (East-North-Up). The vehicle body frame if FLU (Front-Left-Up) where X axis is aligned with the vehicle front axis and Z axis going upward.

Internally, this pose called enu_M_flu is transformed to match the requirements of the Pixhawk into ned_M_frd corresponding to the FRD (Front-Right-Down) body frame position in the NED (North-East-Down) local reference frame.

Returns
true if the MoCap data was successfully sent to the vehicle, false otherwise.
Parameters
[in]enu_M_flu: Homogeneous matrix containing the pose of the vehicle given by the MoCap system. To be more precise, this matrix gives the pose of the vehicle FLU body frame returned by the MoCap where MoCap global reference frame is defined as ENU.
[in]display_fps: Display ned_M_frd pose internally sent through mavlink at the given framerate. A value of 0 can be used to disable this display.

Internally we transform this FRD pose in a NED global reference frame as expected by Pixhawk convention.

Definition at line 1221 of file vpRobotMavsdk.cpp.

◆ setAutoLand()

void vpRobotMavsdk::setAutoLand ( bool  auto_land)

Enable/disable auto land mode in the destructor.

Parameters
[in]auto_land: When true auto land mode is enabled and the destructor calls land() when the vehicle has flying capabilities. When false the destructor doesn't call land().
See also
land()

Definition at line 1542 of file vpRobotMavsdk.cpp.

◆ setForwardSpeed()

bool vpRobotMavsdk::setForwardSpeed ( double  body_frd_vx)

Sets the forward speed, expressed in m/s, in the Front-Right-Down body frame.

Warning
The vehicle will not stop moving in that direction until you send another motion command.
Parameters
[in]body_frd_vx: Desired FRD body frame forward speed in m/s.
  • Positive values will make the vehicle go forward
  • Negative values will make the vehicle go backwards
Returns
true when success, false otherwise.
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1483 of file vpRobotMavsdk.cpp.

◆ setGPSGlobalOrigin()

bool vpRobotMavsdk::setGPSGlobalOrigin ( double  latitude,
double  longitude,
double  altitude 
)

Allows to set GPS global origin to initialize the Kalman filter when the vehicle is not equipped with a GPS.

Parameters
latitude: Latitude in deg (WGS84).
longitude: Longitude in deg (WGS84).
altitude: Altitude in meter. Positive for up.

Definition at line 1506 of file vpRobotMavsdk.cpp.

◆ setLateralSpeed()

bool vpRobotMavsdk::setLateralSpeed ( double  body_frd_vy)

Sets the lateral speed, expressed in m/s, in the Front-Right-Down body frame.

Warning
The vehicle will not stop moving in that direction until you send another motion command.
Parameters
[in]body_frd_vy: Desired FRD body frame lateral speed in m/s.
  • Positive values will make the vehicle go right
  • Negative values will make the vehicle go left
Returns
true when success, false otherwise.
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1496 of file vpRobotMavsdk.cpp.

◆ setPosition() [1/2]

bool vpRobotMavsdk::setPosition ( const vpHomogeneousMatrix ned_M_frd,
bool  blocking = true,
int  timeout_sec = 10 
)

Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame.

Parameters
[in]ned_M_frd: Homogeneous matrix that express the FRD absolute position to reach by the vehicle expressed in the NED global reference frame.
[in]blocking: When true this function is blocking until the position is reached.
[in]timeout_sec: Timeout value in seconds applied when blocking is set to true.
Returns
true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position.
Warning
The rotation around the FRD X and Y axes should be equal to 0, as the vehicle (drone or rover) cannot rotate around these axes.
This function is blocking.
See also
setPosition(float, float, float, float, bool, float)
setPositionRelative(const vpHomogeneousMatrix &, bool, float)

Definition at line 1395 of file vpRobotMavsdk.cpp.

◆ setPosition() [2/2]

bool vpRobotMavsdk::setPosition ( float  ned_north,
float  ned_east,
float  ned_down,
float  ned_yaw,
bool  blocking = true,
int  timeout_sec = 10 
)

Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame.

Parameters
[in]ned_north: Absolute position to reach along north axis (meters).
[in]ned_east: Absolute position to reach along east axis (meters).
[in]ned_down: Absolute position to reach along down axis (meters).
[in]ned_yaw: Absolute position to reach of the heading (radians).
[in]blocking: When true this function is blocking until the position is reached.
[in]timeout_sec: Timeout value in seconds applied when blocking is set to true.
Returns
true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position.
See also
setPosition(const vpHomogeneousMatrix &, bool, float)
setPositionRelative(float, float, float, float, bool, float)

Definition at line 1373 of file vpRobotMavsdk.cpp.

◆ setPositioningIncertitude()

void vpRobotMavsdk::setPositioningIncertitude ( float  position_incertitude,
float  yaw_incertitude 
)

Incertitude used to decided if a position is reached when using setPosition() and setPositionRelative().

Parameters
[in]position_incertitude: Position incertitude in [m].
[in]yaw_incertitude: Yaw angle incertitude in [rad].
See also
setPosition(), setPositionRelative()

Definition at line 1551 of file vpRobotMavsdk.cpp.

◆ setPositionRelative() [1/2]

bool vpRobotMavsdk::setPositionRelative ( const vpHomogeneousMatrix delta_frd_M_frd,
bool  blocking = true,
int  timeout_sec = 10 
)

Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame.

Parameters
[in]delta_frd_M_frd: Homogeneous matrix that express the FRD absolute position to reach by the vehicle expressed in the NED global reference frame.
[in]blocking: When true this function is blocking until the position is reached.
[in]timeout_sec: Timeout value in seconds applied when blocking is set to true.
Returns
true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position.
Warning
The rotation around the FRD X and Y axes should be equal to 0, as the vehicle (drone or rover) cannot rotate around these axes.
This function is blocking.
See also
setPositionRelative(float, float, float, float, bool, float)
setPosition(const vpHomogeneousMatrix &, bool, float)

Definition at line 1436 of file vpRobotMavsdk.cpp.

◆ setPositionRelative() [2/2]

bool vpRobotMavsdk::setPositionRelative ( float  ned_delta_north,
float  ned_delta_east,
float  ned_delta_down,
float  ned_delta_yaw,
bool  blocking = true,
int  timeout_sec = 10 
)

Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame.

Parameters
[in]ned_delta_north: Relative displacement along north (meters).
[in]ned_delta_east: Relative displacement along east (meters).
[in]ned_delta_down: Relative displacement along down axis (meters).
[in]ned_delta_yaw: Relative rotation of the heading (radians).
[in]blocking: When true this function is blocking until the position is reached.
[in]timeout_sec: Timeout value in seconds applied when blocking is set to true.
Returns
true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position.
See also
setPositionRelative(const vpHomogeneousMatrix &, bool, float)
setPosition(float, float, float, float, bool, float)

Definition at line 1414 of file vpRobotMavsdk.cpp.

◆ setTakeOffAlt()

void vpRobotMavsdk::setTakeOffAlt ( double  altitude)

Sets the take off altitude.

Parameters
[in]altitude: Desired altitude for take off in meters, equal to 1.0 m by default.
Warning
The altitude must be positive.
See also
takeOff()

Definition at line 1281 of file vpRobotMavsdk.cpp.

◆ setVelocity()

bool vpRobotMavsdk::setVelocity ( const vpColVector frd_vel_cmd)

Sets the vehicle velocity in its own Front-Right-Down (FRD) body frame.

Parameters
[in]frd_vel_cmd: 4-dim vehicle velocity commands, vx, vy, vz, wz. Translation velocities (vx, vy, vz) should be expressed in m/s and rotation velocity (wz) in rad/s.
Warning
The dimension of the velocity vector should be equal to 4, as the vehicle cannot rotate around X and Y axes.
The vehicle applies this command until given another one.

Definition at line 1450 of file vpRobotMavsdk.cpp.

◆ setVerbose()

void vpRobotMavsdk::setVerbose ( bool  verbose)

Enable/disable verbose mode.

Parameters
[in]verbose: When true enable verbose mode.

Definition at line 1574 of file vpRobotMavsdk.cpp.

◆ setVerticalSpeed()

bool vpRobotMavsdk::setVerticalSpeed ( double  body_frd_vz)

Sets the vertical speed, expressed in m/s, in the Front-Right-Down body frame.

Warning
The vehicle will not stop moving in that direction until you send another motion command.
Parameters
[in]body_frd_vz: Desired FRD body frame vertical speed in m/s.
  • Positive values will make the vehicle go down.
  • Negative values will make the vehicle go up.
Returns
true when success, false otherwise.
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1567 of file vpRobotMavsdk.cpp.

◆ setYawSpeed()

bool vpRobotMavsdk::setYawSpeed ( double  body_frd_wz)

Sets the yaw speed, expressed in rad/s, in the Front-Right-Down body frame.

Warning
The vehicle will not stop moving in that direction until you send another motion command.
Parameters
[in]body_frd_wz: Desired FRD body frame yaw speed in rad/s.
  • Positive values will make the vehicle turn to its right (clockwise)
  • Negative values will make the vehicle turn to its left (counterclockwise)
Returns
true when success, false otherwise.
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1470 of file vpRobotMavsdk.cpp.

◆ stopMoving()

bool vpRobotMavsdk::stopMoving ( )

Stops any vehicle movement.

Warning
Depending on the speed of the vehicle when the function is called, it may still move a bit until it stabilizes.
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1348 of file vpRobotMavsdk.cpp.

◆ takeControl()

bool vpRobotMavsdk::takeControl ( )

Take control using software running outside of the autopilot:

  • When using the PX4 flight stack start the off-board mode,
  • When using Ardupilot stack start the guided mode.
Returns
true when off-board or guided mode are successfully started, false otherwise.

This method should be called before using setPosition(), setVelocity()

See also
releaseControl()
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1522 of file vpRobotMavsdk.cpp.

◆ takeOff() [1/2]

bool vpRobotMavsdk::takeOff ( bool  interactive,
double  takeoff_altitude,
int  timeout_sec = 10,
bool  use_gps = false 
)

Sends take off command when the vehicle has flying capabilities.

Parameters
[in]interactive: If true asks the user if the offboard mode is to be forced through the terminal. If false offboard mode is automatically set.
[in]takeoff_altitude: Take off altitude in [m]. Should be a positive value.
[in]timeout_sec: Time out in seconds to achieve takeoff.
[in]use_gps: When GPS is to use, set this flag to true. Set to false otherwise.
Returns
  • If the vehicle has flying capabilities, returns true if the take off is successful, false otherwise, typically when a timeout occurs.
  • If the vehicle doesn't have flying capabilities, returns true.
Warning
This function is blocking.
See also
setTakeOffAlt(), land(), hasFlyingCapability()

Definition at line 1328 of file vpRobotMavsdk.cpp.

◆ takeOff() [2/2]

bool vpRobotMavsdk::takeOff ( bool  interactive = true,
int  timeout_sec = 10,
bool  use_gps = false 
)

Sends take off command when the vehicle has flying capabilities.

Parameters
[in]interactive: If true asks the user if the offboard mode is to be forced through the terminal. If false offboard mode is automatically set.
[in]timeout_sec: Time out in seconds to achieve takeoff.
[in]use_gps: When GPS is to use, set this flag to true. Set to false otherwise.
Returns
  • If the vehicle has flying capabilities, returns true if the take off is successful, false otherwise, typically when a timeout occurs. If the vehicle has flying capabilities and is already flying, return true.
  • If the vehicle doesn't have flying capabilities, returns true.
Warning
This function is blocking.
See also
setTakeOffAlt(), land(), hasFlyingCapability()
Examples
testPixhawkDroneKeyboard.cpp.

Definition at line 1309 of file vpRobotMavsdk.cpp.