|
|
|
@@ -2,10 +2,11 @@
|
|
|
|
|
|
|
|
|
|
Visual Inertial Odometry (VIO) and Motion Capture (MoCap) systems allow vehicles to navigate when a global position source is unavailable or unreliable (e.g. indoors, or when flying under a bridge. etc.).
|
|
|
|
|
|
|
|
|
|
Both VIO and MoCap determine a vehicle's *pose* (position and attitude) from "visual" information.
|
|
|
|
|
Both VIO and MoCap determine a vehicle's _pose_ (position and attitude) from "visual" information.
|
|
|
|
|
The main difference between them is the frame perspective:
|
|
|
|
|
- VIO uses *onboard sensors* to get pose data from the vehicle's perspective (see [egomotion](https://en.wikipedia.org/wiki/Visual_odometry#Egomotion)).
|
|
|
|
|
- MoCap uses a system of *off-board cameras* to get vehicle pose data in a 3D space (i.e. it is an external system that tells the vehicle its pose).
|
|
|
|
|
|
|
|
|
|
- VIO uses _onboard sensors_ to get pose data from the vehicle's perspective (see [egomotion](https://en.wikipedia.org/wiki/Visual_odometry#Egomotion)).
|
|
|
|
|
- MoCap uses a system of _off-board cameras_ to get vehicle pose data in a 3D space (i.e. it is an external system that tells the vehicle its pose).
|
|
|
|
|
|
|
|
|
|
Pose data from either type of system can be used to update a PX4-based autopilot's local position estimate (relative to the local origin) and also can optionally also be fused into the vehicle attitude estimation. Additionally, if the external pose system also provides linear velocity measurements, it can be used to improve the state estimate (fusion of linear velocity measurements is only supported by the EKF2).
|
|
|
|
|
|
|
|
|
@@ -19,12 +20,12 @@ The instructions differ depending on whether you are using the EKF2 or LPE estim
|
|
|
|
|
|
|
|
|
|
PX4 uses the following MAVLink messages for getting external position information, and maps them to [uORB topics](../middleware/uorb.md):
|
|
|
|
|
|
|
|
|
|
MAVLink | uORB
|
|
|
|
|
--- | ---
|
|
|
|
|
[VISION_POSITION_ESTIMATE](https://mavlink.io/en/messages/common.html#VISION_POSITION_ESTIMATE) | `vehicle_visual_odometry`
|
|
|
|
|
[ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) (`frame_id =` [MAV_FRAME_LOCAL_FRD](https://mavlink.io/en/messages/common.html#MAV_FRAME_LOCAL_FRD)) | `vehicle_visual_odometry`
|
|
|
|
|
[ATT_POS_MOCAP](https://mavlink.io/en/messages/common.html#ATT_POS_MOCAP) | `vehicle_mocap_odometry`
|
|
|
|
|
[ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) (`frame_id =` [MAV_FRAME_MOCAP_NED](https://mavlink.io/en/messages/common.html#MAV_FRAME_MOCAP_NED)) | `vehicle_mocap_odometry`
|
|
|
|
|
| MAVLink | uORB |
|
|
|
|
|
| -------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------- |
|
|
|
|
|
| [VISION_POSITION_ESTIMATE](https://mavlink.io/en/messages/common.html#VISION_POSITION_ESTIMATE) | `vehicle_visual_odometry` |
|
|
|
|
|
| [ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) (`frame_id =` [MAV_FRAME_LOCAL_FRD](https://mavlink.io/en/messages/common.html#MAV_FRAME_LOCAL_FRD)) | `vehicle_visual_odometry` |
|
|
|
|
|
| [ATT_POS_MOCAP](https://mavlink.io/en/messages/common.html#ATT_POS_MOCAP) | `vehicle_mocap_odometry` |
|
|
|
|
|
| [ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) (`frame_id =` [MAV_FRAME_MOCAP_NED](https://mavlink.io/en/messages/common.html#MAV_FRAME_MOCAP_NED)) | `vehicle_mocap_odometry` |
|
|
|
|
|
|
|
|
|
|
EKF2 only subscribes to `vehicle_visual_odometry` topics and can hence only process the first two messages
|
|
|
|
|
(a MoCap system must generate these messages to work with EKF2). The odometry message is the only message that can send also linear velocities to PX4.
|
|
|
|
@@ -43,46 +44,45 @@ The following MAVLink "vision" messages are not currently supported by PX4:
|
|
|
|
|
[VISION_SPEED_ESTIMATE](https://mavlink.io/en/messages/common.html#VISION_SPEED_ESTIMATE),
|
|
|
|
|
[VICON_POSITION_ESTIMATE](https://mavlink.io/en/messages/common.html#VICON_POSITION_ESTIMATE)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
## Reference Frames
|
|
|
|
|
|
|
|
|
|
PX4 uses FRD (X **F**orward, Y **R**ight and Z **D**own) for the local body frame as well for the reference frame. When using the heading of the magnetometer, the PX4 reference frame x axis will be aligned with north, so therefore it is called NED (X **N**orth, Y **E**ast, Z **D**own). The heading of the reference frame of the PX4 estimator and the one of the external pose estimate will not match in most cases.
|
|
|
|
|
Therefore the reference frame of the external pose estimate is named differently, it is called [MAV_FRAME_LOCAL_FRD](https://mavlink.io/en/messages/common.html#MAV_FRAME_LOCAL_FRD).
|
|
|
|
|
|
|
|
|
|
Depending on the source of your reference frame, you will need to apply a custom transformation to the pose estimate before sending the MAVLink Vision/MoCap message.
|
|
|
|
|
This is necessary to change the orientation of the parent and child frame of the pose estimate, such that it fits the PX4 convention. Have a look at the MAVROS [*odom* plugin](https://github.com/mavlink/mavros/blob/master/mavros_extras/src/plugins/odom.cpp) for the necessary transformations.
|
|
|
|
|
This is necessary to change the orientation of the parent and child frame of the pose estimate, such that it fits the PX4 convention. Have a look at the MAVROS [_odom_ plugin](https://github.com/mavlink/mavros/blob/master/mavros_extras/src/plugins/odom.cpp) for the necessary transformations.
|
|
|
|
|
|
|
|
|
|
:::tip
|
|
|
|
|
ROS users can find more detailed instructions below in [Reference Frames and ROS](#reference-frames-and-ros).
|
|
|
|
|
:::
|
|
|
|
|
|
|
|
|
|
For example, if using the Optitrack framework the local frame has $x{}$ and $z{}$ on the horizontal plane (*x* front and *z* right) while *y* axis is vertical and pointing up.
|
|
|
|
|
For example, if using the Optitrack framework the local frame has $x{}$ and $z{}$ on the horizontal plane (_x_ front and _z_ right) while _y_ axis is vertical and pointing up.
|
|
|
|
|
A simple trick is swapping axis in order to obtained NED convention.
|
|
|
|
|
|
|
|
|
|
If `x_{mav}`, `y_{mav}` and `z_{mav}` are the coordinates that are sent through MAVLink as position feedback, then we obtain:
|
|
|
|
|
|
|
|
|
|
```
|
|
|
|
|
x_{mav} = x_{mocap}
|
|
|
|
|
y_{mav} = z_{mocap}
|
|
|
|
|
z_{mav} = - y_{mocap}
|
|
|
|
|
```
|
|
|
|
|
|
|
|
|
|
Regarding the orientation, keep the scalar part *w* of the quaternion the same and swap the vector part *x*, *y* and *z* in the same way.
|
|
|
|
|
Regarding the orientation, keep the scalar part _w_ of the quaternion the same and swap the vector part _x_, _y_ and _z_ in the same way.
|
|
|
|
|
You can apply this trick with every system - if you need to obtain a NED frame, look at your MoCap output and swap axis accordingly.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
## EKF2 Tuning/Configuration
|
|
|
|
|
|
|
|
|
|
Note: this is a quick overview.
|
|
|
|
|
For more detailed information, check the [Using PX4's Navigation Filter (EKF2)](../advanced_config/tuning_the_ecl_ekf.md)
|
|
|
|
|
|
|
|
|
|
The following parameters must be set to use external position information with EKF2 (these can be set in *QGroundControl* > **Vehicle Setup > Parameters > EKF2**).
|
|
|
|
|
The following parameters must be set to use external position information with EKF2 (these can be set in _QGroundControl_ > **Vehicle Setup > Parameters > EKF2**).
|
|
|
|
|
|
|
|
|
|
Parameter | Setting for External Position Estimation
|
|
|
|
|
--- | ---
|
|
|
|
|
[EKF2_EV_CTRL](../advanced_config/parameter_reference.md#EKF2_EV_CTRL) | Set *horizontal position fusion*, *vertical vision fusion*, *velocity fusion*, and *yaw fusion*, according to your desired fusion model.
|
|
|
|
|
[EKF2_HGT_REF](../advanced_config/parameter_reference.md#EKF2_HGT_REF) | Set to *Vision* to use the vision as the reference source for altitude estimation.
|
|
|
|
|
[EKF2_EV_DELAY](../advanced_config/parameter_reference.md#EKF2_EV_DELAY) | Set to the difference between the timestamp of the measurement and the "actual" capture time. For more information see [below](#tuning-EKF2_EV_DELAY).
|
|
|
|
|
[EKF2_EV_POS_X](../advanced_config/parameter_reference.md#EKF2_EV_POS_X), [EKF2_EV_POS_Y](../advanced_config/parameter_reference.md#EKF2_EV_POS_Y), [EKF2_EV_POS_Z](../advanced_config/parameter_reference.md#EKF2_EV_POS_Z) | Set the position of the vision sensor (or MoCap markers) with respect to the robot's body frame.
|
|
|
|
|
| Parameter | Setting for External Position Estimation |
|
|
|
|
|
| ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
|
|
|
|
|
| [EKF2_EV_CTRL](../advanced_config/parameter_reference.md#EKF2_EV_CTRL) | Set _horizontal position fusion_, _vertical vision fusion_, _velocity fusion_, and _yaw fusion_, according to your desired fusion model. |
|
|
|
|
|
| [EKF2_HGT_REF](../advanced_config/parameter_reference.md#EKF2_HGT_REF) | Set to _Vision_ to use the vision as the reference source for altitude estimation. |
|
|
|
|
|
| [EKF2_EV_DELAY](../advanced_config/parameter_reference.md#EKF2_EV_DELAY) | Set to the difference between the timestamp of the measurement and the "actual" capture time. For more information see [below](#tuning-EKF2_EV_DELAY). |
|
|
|
|
|
| [EKF2_EV_POS_X](../advanced_config/parameter_reference.md#EKF2_EV_POS_X), [EKF2_EV_POS_Y](../advanced_config/parameter_reference.md#EKF2_EV_POS_Y), [EKF2_EV_POS_Z](../advanced_config/parameter_reference.md#EKF2_EV_POS_Z) | Set the position of the vision sensor (or MoCap markers) with respect to the robot's body frame. |
|
|
|
|
|
|
|
|
|
|
You can also disable GNSS, baro and range finder fusion using [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL), [EKF2_BARO_CTRL](../advanced_config/parameter_reference.md#EKF2_BARO_CTRL) and [EKF2_RNG_CTRL](../advanced_config/parameter_reference.md#EKF2_RNG_CTRL), respectively.
|
|
|
|
|
|
|
|
|
@@ -91,9 +91,10 @@ Reboot the flight controller in order for parameter changes to take effect.
|
|
|
|
|
:::
|
|
|
|
|
|
|
|
|
|
<a id="tuning-EKF2_EV_DELAY"></a>
|
|
|
|
|
|
|
|
|
|
#### Tuning EKF2_EV_DELAY
|
|
|
|
|
|
|
|
|
|
[EKF2_EV_DELAY](../advanced_config/parameter_reference.md#EKF2_EV_DELAY) is the *Vision Position Estimator delay relative to IMU measurements*.
|
|
|
|
|
[EKF2_EV_DELAY](../advanced_config/parameter_reference.md#EKF2_EV_DELAY) is the _Vision Position Estimator delay relative to IMU measurements_.
|
|
|
|
|
|
|
|
|
|
Or in other words, it is the difference between the vision system timestamp and the "actual" capture time that would have been recorded by the IMU clock (the "base clock" for EKF2).
|
|
|
|
|
|
|
|
|
@@ -125,19 +126,18 @@ See [Building the Code](../dev_setup/building_px4.md) for more details.
|
|
|
|
|
|
|
|
|
|
### Enabling External Pose Input
|
|
|
|
|
|
|
|
|
|
The following parameters must be set to use external position information with LPE (these can be set in *QGroundControl* > **Vehicle Setup > Parameters > Local Position Estimator**).
|
|
|
|
|
|
|
|
|
|
Parameter | Setting for External Position Estimation
|
|
|
|
|
--- | ---
|
|
|
|
|
[LPE_FUSION](../advanced_config/parameter_reference.md#LPE_FUSION) | Vision integration is enabled if *fuse vision position* is checked (it is enabled by default).
|
|
|
|
|
[ATT_EXT_HDG_M](../advanced_config/parameter_reference.md#ATT_EXT_HDG_M) | Set to 1 or 2 to enable external heading integration. Setting it to 1 will cause vision to be used, while 2 enables MoCap heading use.
|
|
|
|
|
The following parameters must be set to use external position information with LPE (these can be set in _QGroundControl_ > **Vehicle Setup > Parameters > Local Position Estimator**).
|
|
|
|
|
|
|
|
|
|
| Parameter | Setting for External Position Estimation |
|
|
|
|
|
| ------------------------------------------------------------------------ | -------------------------------------------------------------------------------------------------------------------------------------- |
|
|
|
|
|
| [LPE_FUSION](../advanced_config/parameter_reference.md#LPE_FUSION) | Vision integration is enabled if _fuse vision position_ is checked (it is enabled by default). |
|
|
|
|
|
| [ATT_EXT_HDG_M](../advanced_config/parameter_reference.md#ATT_EXT_HDG_M) | Set to 1 or 2 to enable external heading integration. Setting it to 1 will cause vision to be used, while 2 enables MoCap heading use. |
|
|
|
|
|
|
|
|
|
|
### Disabling Barometer Fusion
|
|
|
|
|
|
|
|
|
|
If a highly accurate altitude is already available from VIO or MoCap information, it may be useful to disable the baro correction in LPE to reduce drift on the Z axis.
|
|
|
|
|
|
|
|
|
|
This can be done by in *QGroundControl* by unchecking the *fuse baro* option in the [LPE_FUSION](../advanced_config/parameter_reference.md#LPE_FUSION) parameter.
|
|
|
|
|
This can be done by in _QGroundControl_ by unchecking the _fuse baro_ option in the [LPE_FUSION](../advanced_config/parameter_reference.md#LPE_FUSION) parameter.
|
|
|
|
|
|
|
|
|
|
### Tuning Noise Parameters
|
|
|
|
|
|
|
|
|
@@ -161,7 +161,7 @@ This can then be used when planning and executing indoor missions, or to set a l
|
|
|
|
|
|
|
|
|
|
## Working with ROS
|
|
|
|
|
|
|
|
|
|
ROS is not *required* for supplying external pose information, but is highly recommended as it already comes with good integrations with VIO and MoCap systems.
|
|
|
|
|
ROS is not _required_ for supplying external pose information, but is highly recommended as it already comes with good integrations with VIO and MoCap systems.
|
|
|
|
|
PX4 must already have been set up as above.
|
|
|
|
|
|
|
|
|
|
### Getting Pose Data Into ROS
|
|
|
|
@@ -171,51 +171,51 @@ VIO and MoCap systems have different ways of obtaining pose data, and have their
|
|
|
|
|
The setup for specific systems is covered [below](#setup_specific_systems).
|
|
|
|
|
For other systems consult the vendor setup documentation.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
<a id="relaying_pose_data_to_px4"></a>
|
|
|
|
|
|
|
|
|
|
### Relaying Pose Data to PX4
|
|
|
|
|
|
|
|
|
|
MAVROS has plugins to relay a visual estimation from a VIO or MoCap system using the following pipelines:
|
|
|
|
|
|
|
|
|
|
ROS | MAVLink | uORB
|
|
|
|
|
--- | --- | ---
|
|
|
|
|
/mavros/vision_pose/pose | [VISION_POSITION_ESTIMATE](https://mavlink.io/en/messages/common.html#VISION_POSITION_ESTIMATE) | `vehicle_visual_odometry`
|
|
|
|
|
/mavros/odometry/out (`frame_id = odom`, `child_frame_id = base_link`) | [ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) (`frame_id =` [MAV_FRAME_LOCAL_FRD](https://mavlink.io/en/messages/common.html#MAV_FRAME_LOCAL_FRD)) | `vehicle_visual_odometry`
|
|
|
|
|
/mavros/mocap/pose | [ATT_POS_MOCAP](https://mavlink.io/en/messages/common.html#ATT_POS_MOCAP) | `vehicle_mocap_odometry`
|
|
|
|
|
/mavros/odometry/out (`frame_id = odom`, `child_frame_id = base_link`) | [ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) (`frame_id =` [MAV_FRAME_LOCAL_FRD](https://mavlink.io/en/messages/common.html#MAV_FRAME_LOCAL_FRD)) | `vehicle_mocap_odometry`
|
|
|
|
|
| ROS | MAVLink | uORB |
|
|
|
|
|
| ---------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------- |
|
|
|
|
|
| /mavros/vision_pose/pose | [VISION_POSITION_ESTIMATE](https://mavlink.io/en/messages/common.html#VISION_POSITION_ESTIMATE) | `vehicle_visual_odometry` |
|
|
|
|
|
| /mavros/odometry/out (`frame_id = odom`, `child_frame_id = base_link`) | [ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) (`frame_id =` [MAV_FRAME_LOCAL_FRD](https://mavlink.io/en/messages/common.html#MAV_FRAME_LOCAL_FRD)) | `vehicle_visual_odometry` |
|
|
|
|
|
| /mavros/mocap/pose | [ATT_POS_MOCAP](https://mavlink.io/en/messages/common.html#ATT_POS_MOCAP) | `vehicle_mocap_odometry` |
|
|
|
|
|
| /mavros/odometry/out (`frame_id = odom`, `child_frame_id = base_link`) | [ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) (`frame_id =` [MAV_FRAME_LOCAL_FRD](https://mavlink.io/en/messages/common.html#MAV_FRAME_LOCAL_FRD)) | `vehicle_mocap_odometry` |
|
|
|
|
|
|
|
|
|
|
You can use any of the above pipelines with LPE.
|
|
|
|
|
|
|
|
|
|
If you're working with EKF2, only the "vision" pipelines are supported.
|
|
|
|
|
To use MoCap data with EKF2 you will have to [remap](http://wiki.ros.org/roslaunch/XML/remap) the pose topic that you get from MoCap:
|
|
|
|
|
To use MoCap data with EKF2 you will have to [remap](https://wiki.ros.org/roslaunch/XML/remap) the pose topic that you get from MoCap:
|
|
|
|
|
|
|
|
|
|
- MoCap ROS topics of type `geometry_msgs/PoseStamped` or `geometry_msgs/PoseWithCovarianceStamped` must be remapped to `/mavros/vision_pose/pose`.
|
|
|
|
|
The `geometry_msgs/PoseStamped` topic is most common as MoCap doesn't usually have associated covariances to the data.
|
|
|
|
|
- If you get data through a `nav_msgs/Odometry` ROS message then you will need to remap it to `/mavros/odometry/out`, making sure to update the `frame_id` and `child_frame_id` accordingly.
|
|
|
|
|
- The odometry frames `frame_id = odom`, `child_frame_id = base_link` can be changed by updating the file in `mavros/launch/px4_config.yaml`. However, the current version of mavros (`1.3.0`) needs to be able to use the tf tree to find a transform from `frame_id` to the hardcoded frame `odom_ned`. The same applies to the `child_frame_id`, which needs to be connected in the tf tree to the hardcoded frame `base_link_frd`. If you are using mavros `1.2.0` and you didn't update the file `mavros/launch/px4_config.yaml`, then you can safely use the odometry frames `frame_id = odom`, `child_frame_id = base_link` without much worry.
|
|
|
|
|
- Note that if you are sending odometry data to px4 using `child_frame_id = base_link`, then you need to make sure that the `twist` portion of the `nav_msgs/Odometry` message is **expressed in body frame**, **not in inertial frame!!!!!**.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
### Reference Frames and ROS
|
|
|
|
|
|
|
|
|
|
The local/world and world frames used by ROS and PX4 are different.
|
|
|
|
|
|
|
|
|
|
Frame | PX4 | ROS
|
|
|
|
|
--- | --- | ---
|
|
|
|
|
Body | FRD (X **F**orward, Y **R**ight, Z **D**own) | FLU (X **F**orward, Y **L**eft, Z **U**p), usually named `base_link`
|
|
|
|
|
World | FRD or NED (X **N**orth, Y **E**ast, Z **D**own) | FLU or ENU (X **E**ast, Y **N**orth, Z **U**p), with the naming being `odom` or `map`
|
|
|
|
|
| Frame | PX4 | ROS |
|
|
|
|
|
| ----- | ------------------------------------------------ | ------------------------------------------------------------------------------------- |
|
|
|
|
|
| Body | FRD (X **F**orward, Y **R**ight, Z **D**own) | FLU (X **F**orward, Y **L**eft, Z **U**p), usually named `base_link` |
|
|
|
|
|
| World | FRD or NED (X **N**orth, Y **E**ast, Z **D**own) | FLU or ENU (X **E**ast, Y **N**orth, Z **U**p), with the naming being `odom` or `map` |
|
|
|
|
|
|
|
|
|
|
:::tip
|
|
|
|
|
See [REP105: Coordinate Frames for Mobile Platforms](http://www.ros.org/reps/rep-0105.html) for more information about ROS frames.
|
|
|
|
|
See [REP105: Coordinate Frames for Mobile Platforms](https://www.ros.org/reps/rep-0105.html) for more information about ROS frames.
|
|
|
|
|
:::
|
|
|
|
|
|
|
|
|
|
Both frames are shown in the image below (FRD on the left/FLU on the right).
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
With EKF2 when using external heading estimation, magnetic north can either be ignored and or the heading offset to magnetic north can be calculated and compensated. Depending on your choice the yaw angle is given with respect to either magnetic north or local *x*.
|
|
|
|
|
With EKF2 when using external heading estimation, magnetic north can either be ignored and or the heading offset to magnetic north can be calculated and compensated. Depending on your choice the yaw angle is given with respect to either magnetic north or local _x_.
|
|
|
|
|
|
|
|
|
|
::: info
|
|
|
|
|
When creating the rigid body in the MoCap software, remember to first align the robot's local *x* axis with the world *x* axis otherwise the yaw estimate will have an offset. This can stop the external pose estimate fusion from working properly.
|
|
|
|
|
When creating the rigid body in the MoCap software, remember to first align the robot's local _x_ axis with the world _x_ axis otherwise the yaw estimate will have an offset. This can stop the external pose estimate fusion from working properly.
|
|
|
|
|
Yaw angle should be zero when body and reference frame align.
|
|
|
|
|
:::
|
|
|
|
|
|
|
|
|
@@ -234,23 +234,27 @@ You therefore have to add the external pose's body frame to the tf tree. This ca
|
|
|
|
|
<node pkg="tf" type="static_transform_publisher" name="tf_baseLink_externalPoseChildFrame"
|
|
|
|
|
args="0 0 0 <yaw> <pitch> <roll> base_link <external_pose_child_frame> 1000"/>
|
|
|
|
|
```
|
|
|
|
|
|
|
|
|
|
Make sure that you change the values of yaw, pitch and roll such that it properly attaches the external pose's body frame to the `base_link` or `base_link_frd`.
|
|
|
|
|
Have a look at the [tf package](http://wiki.ros.org/tf#static_transform_publisher) for further help on how to specify the transformation between the frames.
|
|
|
|
|
Have a look at the [tf package](https://wiki.ros.org/tf#static_transform_publisher) for further help on how to specify the transformation between the frames.
|
|
|
|
|
You can use rviz to check if you attached the frame right. The name of the `external_pose_child_frame` has to match the child_frame_id of your `nav_msgs/Odometry` message.
|
|
|
|
|
The same also applies for the reference frame of the external pose. You have to attach the reference frame of the external pose as child to either the `odom` or `odom_frd` frame. Adapt therefore the following code line accordingly.
|
|
|
|
|
|
|
|
|
|
```
|
|
|
|
|
<node pkg="tf" type="static_transform_publisher" name="tf_odom_externalPoseParentFrame"
|
|
|
|
|
args="0 0 0 <yaw> <pitch> <roll> odom <external_pose_parent_frame> 1000"/>
|
|
|
|
|
```
|
|
|
|
|
|
|
|
|
|
If the reference frame has the z axis pointing upwards you can attached it without any rotation (yaw=0, pitch=0, roll=0) to the `odom` frame.
|
|
|
|
|
The name of `external_pose_parent_frame` has to match the frame_id of the odometry message.
|
|
|
|
|
|
|
|
|
|
::: info
|
|
|
|
|
When using the MAVROS *odom* plugin, it is important that no other node is publishing a transform between the external pose's reference and child frame.
|
|
|
|
|
This might break the *tf* tree.
|
|
|
|
|
When using the MAVROS _odom_ plugin, it is important that no other node is publishing a transform between the external pose's reference and child frame.
|
|
|
|
|
This might break the _tf_ tree.
|
|
|
|
|
:::
|
|
|
|
|
|
|
|
|
|
<a id="setup_specific_systems"></a>
|
|
|
|
|
|
|
|
|
|
## Specific System Setups
|
|
|
|
|
|
|
|
|
|
### OptiTrack MoCap
|
|
|
|
@@ -259,17 +263,17 @@ The following steps explain how to feed position estimates from an [OptiTrack](h
|
|
|
|
|
It is assumed that the MoCap system is calibrated.
|
|
|
|
|
See [this video](https://www.youtube.com/watch?v=cNZaFEghTBU) for a tutorial on the calibration process.
|
|
|
|
|
|
|
|
|
|
#### Steps on the *Motive* MoCap software
|
|
|
|
|
#### Steps on the _Motive_ MoCap software
|
|
|
|
|
|
|
|
|
|
* Align your robot's forward direction with the [system +x-axis](https://v20.wiki.optitrack.com/index.php?title=Template:Coordinate_System)
|
|
|
|
|
* [Define a rigid body in the Motive software](https://www.youtube.com/watch?v=1e6Qqxqe-k0). Give the robot a name that does not contain spaces, e.g. `robot1` instead of `Rigidbody 1`
|
|
|
|
|
* [Enable Frame Broadacst and VRPN streaming](https://www.youtube.com/watch?v=yYRNG58zPFo)
|
|
|
|
|
* Set the Up axis to be the Z axis (the default is Y)
|
|
|
|
|
- Align your robot's forward direction with the [system +x-axis](https://v20.wiki.optitrack.com/index.php?title=Template:Coordinate_System)
|
|
|
|
|
- [Define a rigid body in the Motive software](https://www.youtube.com/watch?v=1e6Qqxqe-k0). Give the robot a name that does not contain spaces, e.g. `robot1` instead of `Rigidbody 1`
|
|
|
|
|
- [Enable Frame Broadacst and VRPN streaming](https://www.youtube.com/watch?v=yYRNG58zPFo)
|
|
|
|
|
- Set the Up axis to be the Z axis (the default is Y)
|
|
|
|
|
|
|
|
|
|
#### Getting pose data into ROS
|
|
|
|
|
|
|
|
|
|
* Install the `vrpn_client_ros` package
|
|
|
|
|
* You can get each rigid body pose on an individual topic by running
|
|
|
|
|
- Install the `vrpn_client_ros` package
|
|
|
|
|
- You can get each rigid body pose on an individual topic by running
|
|
|
|
|
```sh
|
|
|
|
|
roslaunch vrpn_client_ros sample.launch server:=<mocap machine ip>
|
|
|
|
|
```
|
|
|
|
@@ -291,7 +295,6 @@ Assuming that you have configured EKF2 parameters as described above, PX4 now is
|
|
|
|
|
|
|
|
|
|
You are now set to proceed to the first flight.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
## First Flight
|
|
|
|
|
|
|
|
|
|
After setting up one of the (specific) systems described above you should now be ready to test.
|
|
|
|
@@ -301,19 +304,19 @@ The instructions below show how to do so for MoCap and VIO systems
|
|
|
|
|
|
|
|
|
|
Be sure to perform the following checks before your first flight:
|
|
|
|
|
|
|
|
|
|
* Set the PX4 parameter `MAV_ODOM_LP` to 1.
|
|
|
|
|
- Set the PX4 parameter `MAV_ODOM_LP` to 1.
|
|
|
|
|
PX4 will then stream back the received external pose as MAVLink [ODOMETRY](https://mavlink.io/en/messages/common.html#ODOMETRY) messages.
|
|
|
|
|
* You can check these MAVLink messages with the *QGroundControl* [MAVLink Inspector](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/analyze_view/mavlink_inspector.html)
|
|
|
|
|
- You can check these MAVLink messages with the _QGroundControl_ [MAVLink Inspector](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/analyze_view/mavlink_inspector.html)
|
|
|
|
|
In order to do this, yaw the vehicle until the quaternion of the `ODOMETRY` message is very close to a unit quaternion. (w=1, x=y=z=0)
|
|
|
|
|
* At this point the body frame is aligned with the reference frame of the external pose system.
|
|
|
|
|
- At this point the body frame is aligned with the reference frame of the external pose system.
|
|
|
|
|
If you do not manage to get a quaternion close to the unit quaternion without rolling or pitching your vehicle, your frame probably still have a pitch or roll offset.
|
|
|
|
|
Do not proceed if this is the case and check your coordinate frames again.
|
|
|
|
|
* Once aligned you can pick the vehicle up from the ground and you should see the position's z coordinate decrease.
|
|
|
|
|
- Once aligned you can pick the vehicle up from the ground and you should see the position's z coordinate decrease.
|
|
|
|
|
Moving the vehicle in forward direction, should increase the position's x coordinate.
|
|
|
|
|
While moving the vehicle to the right should increase the y coordinate.
|
|
|
|
|
In the case you send also linear velocities from the external pose system, you should also check the linear velocities.
|
|
|
|
|
Check that the linear velocities are in expressed in the *FRD* body frame reference frame.
|
|
|
|
|
* Set the PX4 parameter `MAV_ODOM_LP` back to 0. PX4 will stop streaming this message back.
|
|
|
|
|
Check that the linear velocities are in expressed in the _FRD_ body frame reference frame.
|
|
|
|
|
- Set the PX4 parameter `MAV_ODOM_LP` back to 0. PX4 will stop streaming this message back.
|
|
|
|
|
|
|
|
|
|
If those steps are consistent, you can try your first flight.
|
|
|
|
|
|
|
|
|
|
Hamish Willee