mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 07:53:34 +08:00
Move PX4 Guide source into /docs (#24490)
* Add vitepress tree * Update existing workflows so they dont trigger on changes in the docs path * Add nojekyll, package.json, LICENCE etc * Add crowdin docs upload/download scripts * Add docs flaw checker workflows * Used docs prefix for docs workflows * Crowdin obvious fixes * ci: docs move to self hosted runner runs on a beefy server for faster builds Signed-off-by: Ramon Roche <mrpollo@gmail.com> * ci: don't run build action for docs or ci changes Signed-off-by: Ramon Roche <mrpollo@gmail.com> * ci: update runners Signed-off-by: Ramon Roche <mrpollo@gmail.com> * Add docs/en * Add docs assets and scripts * Fix up editlinks to point to PX4 sources * Download just the translations that are supported * Add translation sources for zh, uk, ko * Update latest tranlsation and uorb graphs * update vitepress to latest --------- Signed-off-by: Ramon Roche <mrpollo@gmail.com> Co-authored-by: Ramon Roche <mrpollo@gmail.com>
This commit is contained in:
co-authored by
Ramon Roche
parent
8e6d2ebe4a
commit
88d623bedb
@@ -0,0 +1,335 @@
|
||||
# Using Vision or Motion Capture Systems for Position Estimation
|
||||
|
||||
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.
|
||||
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).
|
||||
|
||||
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).
|
||||
|
||||
This topic explains how to configure a PX4-based system to get data from MoCap/VIO systems (either via ROS or some other MAVLink system) and more specifically how to set up MoCap systems like VICON and Optitrack, and vision-based estimation systems like [ROVIO](https://github.com/ethz-asl/rovio), [SVO](https://github.com/uzh-rpg/rpg_svo) and [PTAM](https://github.com/ethz-asl/ethzasl_ptam)).
|
||||
|
||||
::: info
|
||||
The instructions differ depending on whether you are using the EKF2 or LPE estimator.
|
||||
:::
|
||||
|
||||
## PX4 MAVLink Integration
|
||||
|
||||
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`
|
||||
|
||||
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.
|
||||
The LPE estimator subscribes to both topics, and can hence process all the above messages.
|
||||
|
||||
:::tip
|
||||
EKF2 is the default estimator used by PX4.
|
||||
It is better tested and supported than LPE, and should be used by preference.
|
||||
:::
|
||||
|
||||
The messages should be streamed at between 30Hz (if containing covariances) and 50 Hz.
|
||||
If the message rate is too low, EKF2 will not fuse the external vision messages.
|
||||
|
||||
The following MAVLink "vision" messages are not currently supported by PX4:
|
||||
[GLOBAL_VISION_POSITION_ESTIMATE](https://mavlink.io/en/messages/common.html#GLOBAL_VISION_POSITION_ESTIMATE),
|
||||
[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.
|
||||
|
||||
:::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.
|
||||
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.
|
||||
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**).
|
||||
|
||||
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.
|
||||
|
||||
:::tip
|
||||
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*.
|
||||
|
||||
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).
|
||||
|
||||
Technically this can be set to 0 if there is correct timestamping (not just arrival time) and timesync (e.g NTP) between MoCap and (for example) ROS computers.
|
||||
In reality, this needs some empirical tuning since delays in the entire MoCap->PX4 chain are very setup-specific.
|
||||
It is rare that a system is setup with an entirely synchronised chain!
|
||||
|
||||
A rough estimate of the delay can be obtained from logs by checking the offset between IMU rates and the EV rates.
|
||||
To enable logging of EV rates set bit 7 (Computer Vision and Avoidance) of [SDLOG_PROFILE](../advanced_config/parameter_reference.md#SDLOG_PROFILE).
|
||||
|
||||

|
||||
|
||||
::: info
|
||||
A plot of external data vs. onboard estimate (as above) can be generated using [FlightPlot](../log/flight_log_analysis.md#flightplot) or similar flight analysis tools.
|
||||
At time of writing (July 2021) neither [Flight Review](../log/flight_log_analysis.md#flight-review-online-tool) nor [MAVGCL](../log/flight_log_analysis.md#mavgcl) support this functionality.
|
||||
:::
|
||||
|
||||
The value can further be tuned by varying the parameter to find the value that yields the lowest EKF innovations during dynamic maneuvers.
|
||||
|
||||
## LPE Tuning/Configuration
|
||||
|
||||
You will first need to [switch to the LPE estimator](../advanced/switching_state_estimators.md) by setting the following parameters: [LPE_EN](../advanced_config/parameter_reference.md#LPE_EN) (1), [EKF2_EN](../advanced_config/parameter_reference.md#EKF2_EN) (0), [ATT_EN](../advanced_config/parameter_reference.md#ATT_EN) (0).
|
||||
|
||||
::: info
|
||||
If targeting `px4_fmu-v2` hardware you will also need to use a firmware version that includes the LPE module (firmware for other FMU-series hardware includes both LPE and EKF).
|
||||
The LPE version can be found in the zip file for each PX4 release or it can be built from source using the build command `make px4_fmu-v2_lpe`.
|
||||
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.
|
||||
|
||||
|
||||
### 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.
|
||||
|
||||
### Tuning Noise Parameters
|
||||
|
||||
If your vision or MoCap data is highly accurate, and you just want the estimator to track it tightly, you should reduce the standard deviation parameters: [LPE_VIS_XY](../advanced_config/parameter_reference.md#LPE_VIS_XY) and [LPE_VIS_Z](../advanced_config/parameter_reference.md#LPE_VIS_Z) (for VIO) or [LPE_VIC_P](../advanced_config/parameter_reference.md#LPE_VIC_P) (for MoCap).
|
||||
Reducing them will cause the estimator to trust the incoming pose estimate more.
|
||||
You may need to set them lower than the allowed minimum and force-save.
|
||||
|
||||
:::tip
|
||||
If performance is still poor, try increasing the [LPE_PN_V](../advanced_config/parameter_reference.md#LPE_PN_V) parameter.
|
||||
This will cause the estimator to trust measurements more during velocity estimation.
|
||||
:::
|
||||
|
||||
## Enabling Auto Modes with a Local Position
|
||||
|
||||
All PX4 automatic flight modes (such as [Mission](../flight_modes_mc/mission.md), [Return](../flight_modes_mc/return.md), [Land](../flight_modes_mc/land.md), [Hold](../flight_modes_mc/land.md), [Orbit](../flight_modes_mc/orbit.md))) require a _global_ position estimate, which would normally come from a GPS/GNSS system.
|
||||
|
||||
Systems that only have a _local_ position estimate (from MOCAP, VIO, or similar) can use the [SET_GPS_GLOBAL_ORIGIN](https://mavlink.io/en/messages/common.html#SET_GPS_GLOBAL_ORIGIN) MAVLink message to set the origin of the EKF to a particular global location.
|
||||
EKF will then provide a global position estimate based on origin and local frame position.
|
||||
|
||||
This can then be used when planning and executing indoor missions, or to set a local return point, and so on.
|
||||
|
||||
## 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.
|
||||
PX4 must already have been set up as above.
|
||||
|
||||
### Getting Pose Data Into ROS
|
||||
|
||||
VIO and MoCap systems have different ways of obtaining pose data, and have their own setup and topics.
|
||||
|
||||
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`
|
||||
|
||||
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:
|
||||
- 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`
|
||||
|
||||
:::tip
|
||||
See [REP105: Coordinate Frames for Mobile Platforms](http://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*.
|
||||
|
||||
::: 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.
|
||||
Yaw angle should be zero when body and reference frame align.
|
||||
:::
|
||||
|
||||
Using MAVROS, this operation is straightforward.
|
||||
ROS uses ENU frames as convention, therefore position feedback must be provided in ENU.
|
||||
If you have an Optitrack system you can use [mocap_optitrack](https://github.com/ros-drivers/mocap_optitrack) node which streams the object pose on a ROS topic already in ENU.
|
||||
With a remapping you can directly publish it on `mocap_pose_estimate` as it is without any transformation and MAVROS will take care of NED conversions.
|
||||
|
||||
The MAVROS odometry plugin makes it easy to handle the coordinate frames.
|
||||
It uses ROS's tf package. Your external pose system might have a completely different frame convention that does not match the one of PX4.
|
||||
The body frame of the external pose estimate can depend on how you set the body frame in the MOCAP software or on how you mount the VIO sensor on the drone.
|
||||
The MAVROS odometry plugin needs to know how the external pose's child frame is oriented with respect to either the airframe's FRD or FLU body frame known by MAVROS.
|
||||
You therefore have to add the external pose's body frame to the tf tree. This can be done by including an adapted version of the following line into your ROS launch file.
|
||||
|
||||
```
|
||||
<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.
|
||||
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.
|
||||
:::
|
||||
|
||||
<a id="setup_specific_systems"></a>
|
||||
## Specific System Setups
|
||||
|
||||
### OptiTrack MoCap
|
||||
|
||||
The following steps explain how to feed position estimates from an [OptiTrack](https://optitrack.com/motion-capture-robotics/) system to PX4.
|
||||
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
|
||||
|
||||
* 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
|
||||
```sh
|
||||
roslaunch vrpn_client_ros sample.launch server:=<mocap machine ip>
|
||||
```
|
||||
|
||||
If you named the rigidbody as `robot1`, you will get a topic like `/vrpn_client_node/robot1/pose`
|
||||
|
||||
#### Relaying/remapping Pose Data
|
||||
|
||||
MAVROS provides a plugin to relay pose data published on `/mavros/vision_pose/pose` to PX4.
|
||||
Assuming that MAVROS is running, you just need to **remap** the pose topic that you get from MoCap `/vrpn_client_node/<rigid_body_name>/pose` directly to `/mavros/vision_pose/pose`.
|
||||
Note that there is also a `mocap` topic that MAVROS provides to feed `ATT_POS_MOCAP` to PX4, but it is not applicable for EKF2.
|
||||
However, it is applicable with LPE.
|
||||
|
||||
::: info
|
||||
Remapping pose topics is covered above [Relaying pose data to PX4](#relaying_pose_data_to_px4) (`/vrpn_client_node/<rigid_body_name>/pose` is of type `geometry_msgs/PoseStamped`).
|
||||
:::
|
||||
|
||||
Assuming that you have configured EKF2 parameters as described above, PX4 now is set and fusing MoCap data.
|
||||
|
||||
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.
|
||||
The instructions below show how to do so for MoCap and VIO systems
|
||||
|
||||
### Check external estimate
|
||||
|
||||
Be sure to perform the following checks before your first flight:
|
||||
|
||||
* 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)
|
||||
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.
|
||||
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.
|
||||
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.
|
||||
|
||||
If those steps are consistent, you can try your first flight.
|
||||
|
||||
Put the robot on the ground and start streaming MoCap feedback.
|
||||
Lower your left (throttle) stick and arm the motors.
|
||||
|
||||
At this point, with the left stick at the lowest position, switch to position control.
|
||||
You should have a green light.
|
||||
The green light tells you that position feedback is available and position control is now activated.
|
||||
|
||||
Put your left stick at the middle, this is the dead zone.
|
||||
With this stick value, the robot maintains its altitude;
|
||||
raising the stick will increase the reference altitude while lowering the value will decrease it.
|
||||
Same for right stick on x and y.
|
||||
|
||||
Increase the value of the left stick and the robot will take off,
|
||||
put it back to the middle right after. Check if it is able to keep its position.
|
||||
|
||||
If it works, you may want to set up an [offboard](offboard_control.md) experiment by sending position-setpoint from a remote ground station.
|
||||
@@ -0,0 +1 @@
|
||||
<Redirect to="../ros2/" />
|
||||
@@ -0,0 +1,357 @@
|
||||
# Sending a Custom Message from MAVROS to PX4
|
||||
|
||||
:::warning
|
||||
This article has been tested against:
|
||||
- **Ubuntu:** 20.04
|
||||
- **ROS:** Noetic
|
||||
- **PX4 Firmware:** v1.13
|
||||
|
||||
However these steps are fairly general and so it should work with other distros/versions with little to no modifications.
|
||||
:::
|
||||
|
||||
<!-- Content reproduced with permission from @JoonmoAhn in https://github.com/JoonmoAhn/Sending-Custom-Message-from-MAVROS-to-PX4/issues/1 -->
|
||||
|
||||
## MAVROS Installation
|
||||
|
||||
Follow *Source Installation* instructions from [mavlink/mavros](https://github.com/mavlink/mavros/blob/master/mavros/index.md) to install "ROS Kinetic".
|
||||
|
||||
## MAVROS
|
||||
|
||||
1. We start by creating a new MAVROS plugin, in this example named **keyboard_command.cpp** (in **workspace/src/mavros/mavros_extras/src/plugins**) by using the code below:
|
||||
|
||||
The code subscribes a 'char' message from ROS topic `/mavros/keyboard_command/keyboard_sub` and sends it as a MAVLink message.
|
||||
```c
|
||||
#include <mavros/mavros_plugin.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <iostream>
|
||||
#include <std_msgs/Char.h>
|
||||
|
||||
namespace mavros {
|
||||
namespace extra_plugins{
|
||||
|
||||
class KeyboardCommandPlugin : public plugin::PluginBase {
|
||||
public:
|
||||
KeyboardCommandPlugin() : PluginBase(),
|
||||
nh("~keyboard_command")
|
||||
|
||||
{ };
|
||||
|
||||
void initialize(UAS &uas_)
|
||||
{
|
||||
PluginBase::initialize(uas_);
|
||||
keyboard_sub = nh.subscribe("keyboard_sub", 10, &KeyboardCommandPlugin::keyboard_cb, this);
|
||||
};
|
||||
|
||||
Subscriptions get_subscriptions()
|
||||
{
|
||||
return {/* RX disabled */ };
|
||||
}
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh;
|
||||
ros::Subscriber keyboard_sub;
|
||||
|
||||
void keyboard_cb(const std_msgs::Char::ConstPtr &req)
|
||||
{
|
||||
std::cout << "Got Char : " << req->data << std::endl;
|
||||
mavlink::common::msg::KEY_COMMAND kc {};
|
||||
kc.command = req->data;
|
||||
UAS_FCU(m_uas)->send_message_ignore_drop(kc);
|
||||
}
|
||||
};
|
||||
} // namespace extra_plugins
|
||||
} // namespace mavros
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::KeyboardCommandPlugin, mavros::plugin::PluginBase)
|
||||
```
|
||||
|
||||
1. Edit **mavros_plugins.xml** (in **workspace/src/mavros/mavros_extras**) and add the following lines:
|
||||
```xml
|
||||
<class name="keyboard_command" type="mavros::extra_plugins::KeyboardCommandPlugin" base_class_type="mavros::plugin::PluginBase">
|
||||
<description>Accepts keyboard command.</description>
|
||||
</class>
|
||||
```
|
||||
|
||||
1. Edit **CMakeLists.txt** (in **workspace/src/mavros/mavros_extras**) and add the following line in `add_library`.
|
||||
```cmake
|
||||
add_library(
|
||||
...
|
||||
src/plugins/keyboard_command.cpp
|
||||
)
|
||||
```
|
||||
|
||||
1. Inside **common.xml** in (**workspace/src/mavlink/message_definitions/v1.0**), copy the following lines to add your MAVLink message:
|
||||
```xml
|
||||
...
|
||||
<message id="229" name="KEY_COMMAND">
|
||||
<description>Keyboard char command.</description>
|
||||
<field type="char" name="command"> </field>
|
||||
</message>
|
||||
...
|
||||
```
|
||||
|
||||
## PX4 Changes
|
||||
|
||||
1. Inside **common.xml** (in **PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0**), add your MAVLink message as following (same procedure as for MAVROS section above):
|
||||
```xml
|
||||
...
|
||||
<message id="229" name="KEY_COMMAND">
|
||||
<description>Keyboard char command.</description>
|
||||
<field type="char" name="command"> </field>
|
||||
</message>
|
||||
...
|
||||
```
|
||||
|
||||
:::warning
|
||||
Make sure that the **common.xml** files in the following directories are exactly the same:
|
||||
- `PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0`
|
||||
- `workspace/src/mavlink/message_definitions/v1.0`
|
||||
are exactly the same.
|
||||
:::
|
||||
|
||||
1. Make your own uORB message file **key_command.msg** in (PX4-Autopilot/msg). For this example the "key_command.msg" has only the code:
|
||||
```
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
char cmd
|
||||
```
|
||||
|
||||
Then, in **CMakeLists.txt** (in **PX4-Autopilot/msg**), include:
|
||||
|
||||
```cmake
|
||||
set(
|
||||
...
|
||||
key_command.msg
|
||||
)
|
||||
```
|
||||
|
||||
1. Edit **mavlink_receiver.h** (in **PX4-Autopilot/src/modules/mavlink**)
|
||||
|
||||
```cpp
|
||||
...
|
||||
#include <uORB/topics/key_command.h>
|
||||
...
|
||||
class MavlinkReceiver
|
||||
{
|
||||
...
|
||||
private:
|
||||
void handle_message_key_command(mavlink_message_t *msg);
|
||||
...
|
||||
orb_advert_t _key_command_pub{nullptr};
|
||||
}
|
||||
```
|
||||
|
||||
1. Edit **mavlink_receiver.cpp** (in **PX4-Autopilot/src/modules/mavlink**).
|
||||
This is where PX4 receives the MAVLink message sent from ROS, and publishes it as a uORB topic.
|
||||
```cpp
|
||||
...
|
||||
void MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
{
|
||||
...
|
||||
case MAVLINK_MSG_ID_KEY_COMMAND:
|
||||
handle_message_key_command(msg);
|
||||
break;
|
||||
...
|
||||
}
|
||||
...
|
||||
void
|
||||
MavlinkReceiver::handle_message_key_command(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_key_command_t man;
|
||||
mavlink_msg_key_command_decode(msg, &man);
|
||||
|
||||
struct key_command_s key = {};
|
||||
|
||||
key.timestamp = hrt_absolute_time();
|
||||
key.cmd = man.command;
|
||||
|
||||
if (_key_command_pub == nullptr) {
|
||||
_key_command_pub = orb_advertise(ORB_ID(key_command), &key);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(key_command), _key_command_pub, &key);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
1. Make your own uORB topic subscriber just like any example subscriber module.
|
||||
For this example lets create the model in (/PX4-Autopilot/src/modules/key_receiver).
|
||||
In this directory, create three files **CMakeLists.txt**, **key_receiver.cpp**, **Kconfig**
|
||||
Each one looks like the following.
|
||||
|
||||
-CMakeLists.txt
|
||||
|
||||
```cmake
|
||||
px4_add_module(
|
||||
MODULE modules__key_receiver
|
||||
MAIN key_receiver
|
||||
STACK_MAIN 2500
|
||||
STACK_MAX 4000
|
||||
SRCS
|
||||
key_receiver.cpp
|
||||
DEPENDS
|
||||
|
||||
)
|
||||
```
|
||||
|
||||
-key_receiver.cpp
|
||||
|
||||
```
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/key_command.h>
|
||||
|
||||
extern "C" __EXPORT int key_receiver_main(int argc, char **argv);
|
||||
|
||||
int key_receiver_main(int argc, char **argv)
|
||||
{
|
||||
int key_sub_fd = orb_subscribe(ORB_ID(key_command));
|
||||
orb_set_interval(key_sub_fd, 200); // limit the update rate to 200ms
|
||||
|
||||
px4_pollfd_struct_t fds[] = {
|
||||
{ .fd = key_sub_fd, .events = POLLIN },
|
||||
};
|
||||
|
||||
int error_counter = 0;
|
||||
|
||||
for (int i = 0; i < 10; i++)
|
||||
{
|
||||
int poll_ret = px4_poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret == 0)
|
||||
{
|
||||
PX4_ERR("Got no data within a second");
|
||||
}
|
||||
|
||||
else if (poll_ret < 0)
|
||||
{
|
||||
if (error_counter < 10 || error_counter % 50 == 0)
|
||||
{
|
||||
PX4_ERR("ERROR return value from poll(): %d", poll_ret);
|
||||
}
|
||||
|
||||
error_counter++;
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
if (fds[0].revents & POLLIN)
|
||||
{
|
||||
struct key_command_s input;
|
||||
orb_copy(ORB_ID(key_command), key_sub_fd, &input);
|
||||
PX4_INFO("Received Char : %c", input.cmd);
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
```
|
||||
|
||||
-Kconfig
|
||||
|
||||
```
|
||||
menuconfig MODULES_KEY_RECEIVER
|
||||
bool "key_receiver"
|
||||
default n
|
||||
---help---
|
||||
Enable support for key_receiver
|
||||
|
||||
```
|
||||
|
||||
For a more detailed explanation see the topic [Writing your first application](../modules/hello_sky.md).
|
||||
|
||||
1. Lastly, add your module in the **default.px4board** file correspondent to your board in **PX4-Autopilot/boards/**.
|
||||
For example:
|
||||
-for the Pixhawk 4, add the following code in **PX4-Autopilot/boards/px4/fmu-v5/default.px4board**:
|
||||
-for the SITL, add the following code in **PX4-Autopilot/boards/px4/sitl/default.px4board**
|
||||
|
||||
```
|
||||
CONFIG_MODULES_KEY_RECEIVER=y
|
||||
```
|
||||
|
||||
Now you are ready to build all your work!
|
||||
|
||||
## Building
|
||||
|
||||
### Build for ROS
|
||||
|
||||
1. In your workspace enter: `catkin build`.
|
||||
1. Beforehand, you have to set your "px4.launch" in (/workspace/src/mavros/mavros/launch).
|
||||
Edit "px4.launch" as below.
|
||||
If you are using USB to connect your computer with Pixhawk, you have to set "fcu_url" as shown below.
|
||||
But, if you are using CP2102 to connect your computer with Pixhawk, you have to replace "ttyACM0" with "ttyUSB0".
|
||||
And if you are using the SITL to connect to your terminal, you have to replace "/dev/ttyACM0:57600" with "udp://:14540@127.0.0.1:14557".
|
||||
Modifying "gcs_url" is to connect your Pixhawk with UDP, because serial communication cannot accept MAVROS, and your nutshell connection simultaneously.
|
||||
|
||||
1. Write your IP address at "xxx.xx.xxx.xxx"
|
||||
```xml
|
||||
...
|
||||
<arg name="fcu_url" default="/dev/ttyACM0:57600" />
|
||||
<arg name="gcs_url" default="udp://:14550@xxx.xx.xxx.xxx:14557" />
|
||||
...
|
||||
```
|
||||
|
||||
### Build for PX4
|
||||
|
||||
1. Clean the previously built PX4-Autopilot directory. In the root of **PX4-Autopilot** directory:
|
||||
```sh
|
||||
make clean
|
||||
```
|
||||
|
||||
1. Build PX4-Autopilot and upload [in the normal way](../dev_setup/building_px4.md#nuttx-pixhawk-based-boards).
|
||||
|
||||
For example:
|
||||
|
||||
- to build for Pixhawk 4/FMUv5 execute the following command in the root of the PX4-Autopilot directory:
|
||||
```sh
|
||||
make px4_fmu-v5_default upload
|
||||
```
|
||||
- to build for SITL execute the following command in the root of the PX4-Autopilot directory (using jmavsim simulation):
|
||||
```sh
|
||||
make px4_sitl jmavsim
|
||||
```
|
||||
|
||||
## Running the Code
|
||||
|
||||
Next test if the MAVROS message is sent to PX4.
|
||||
|
||||
### Running ROS
|
||||
|
||||
1. In a terminal enter
|
||||
```sh
|
||||
roslaunch mavros px4.launch
|
||||
```
|
||||
1. In a second terminal run:
|
||||
```sh
|
||||
rostopic pub -r 10 /mavros/keyboard_command/keyboard_sub std_msgs/Char 97
|
||||
```
|
||||
This means, publish 97 ('a' in ASCII) to ROS topic "/mavros/keyboard_command/keyboard_sub" in message type "std_msgs/Char". "-r 10" means to publish continuously in "10Hz".
|
||||
|
||||
### Running PX4
|
||||
|
||||
1. Enter the Pixhawk nutshell through UDP.
|
||||
Replace xxx.xx.xxx.xxx with your IP.
|
||||
```sh
|
||||
cd PX4-Autopilot/Tools
|
||||
./mavlink_shell.py xxx.xx.xxx.xxx:14557 --baudrate 57600
|
||||
```
|
||||
|
||||
1. After few seconds, press **Enter** a couple of times.
|
||||
You should see a prompt in the terminal as below:
|
||||
```sh
|
||||
nsh>
|
||||
nsh>
|
||||
```
|
||||
Type "key_receiver", to run your subscriber module.
|
||||
```
|
||||
nsh> key_receiver
|
||||
```
|
||||
|
||||
Check if it successfully receives `a` from your ROS topic.
|
||||
@@ -0,0 +1,208 @@
|
||||
# ROS (1) with MAVROS Installation Guide
|
||||
|
||||
:::warning
|
||||
The PX4 development team recommend that all users [upgrade to ROS 2](../ros2/index.md).
|
||||
This documentation reflects the "old approach".
|
||||
:::
|
||||
|
||||
This documentation explains how to set up communication between the PX4 Autopilot and a ROS 1 enabled companion computer using MAVROS.
|
||||
|
||||
[MAVROS](http://wiki.ros.org/mavros#mavros.2BAC8-Plugins.sys_status) is a ROS 1 package that enables MAVLink extendable communication between computers running ROS 1 for any MAVLink enabled autopilot, ground station, or peripheral.
|
||||
_MAVROS_ is the "official" supported bridge between ROS 1 and the MAVLink protocol.
|
||||
|
||||
First we install PX4 and ROS, and then MAVROS.
|
||||
|
||||
## Install ROS and PX4
|
||||
|
||||
This section explains how to install [ROS 1](../ros/index.md) with PX4.
|
||||
ROS 1 full desktop builds come with Gazebo Classic, so normally you will not install the simulator dependencies yourself!
|
||||
|
||||
:::tip
|
||||
These instructions are a simplified version of the [official installation guide](https://github.com/mavlink/mavros/tree/master/mavros#installation).
|
||||
They cover the _ROS Melodic and Noetic_ releases.
|
||||
:::
|
||||
|
||||
:::: tabs
|
||||
|
||||
::: tab ROS Noetic (Ubuntu 20.04)
|
||||
|
||||
If you're working with [ROS Noetic](http://wiki.ros.org/noetic) on Ubuntu 20.04:
|
||||
|
||||
1. Install PX4 without the simulator toolchain:
|
||||
|
||||
1. [Download PX4 Source Code](../dev_setup/building_px4.md):
|
||||
|
||||
```sh
|
||||
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
|
||||
```
|
||||
|
||||
1. Run the **ubuntu.sh** the `--no-sim-tools` (and optionally `--no-nuttx`):
|
||||
|
||||
```sh
|
||||
bash ./PX4-Autopilot/Tools/setup/ubuntu.sh --no-sim-tools --no-nuttx
|
||||
```
|
||||
|
||||
- Acknowledge any prompts as the script progress.
|
||||
|
||||
1. Restart the computer on completion.
|
||||
|
||||
1. You _may_ need to install the following additional dependencies:
|
||||
|
||||
```sh
|
||||
sudo apt-get install protobuf-compiler libeigen3-dev libopencv-dev -y
|
||||
```
|
||||
|
||||
1. Follow the [Noetic Installation instructions](http://wiki.ros.org/noetic/Installation/Ubuntu#Installation) (ros-noetic-desktop-full is recommended).
|
||||
|
||||
:::
|
||||
|
||||
::: tab ROS Melodic (Ubuntu 18.04)
|
||||
|
||||
If you're working with ROS "Melodic on Ubuntu 18.04:
|
||||
|
||||
1. Download the [ubuntu_sim_ros_melodic.sh](https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh) script in a bash shell:
|
||||
|
||||
```sh
|
||||
wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
|
||||
```
|
||||
|
||||
1. Run the script:
|
||||
|
||||
```sh
|
||||
bash ubuntu_sim_ros_melodic.sh
|
||||
```
|
||||
|
||||
You may need to acknowledge some prompts as the script progresses.
|
||||
|
||||
::: tip
|
||||
You don't need to install MAVROS (as shown below), as this is included by the script
|
||||
|
||||
Also note:
|
||||
- ROS Melodic is installed with Gazebo (Classic) 9 by default.
|
||||
- Your catkin (ROS build system) workspace is created at **~/catkin_ws/**.
|
||||
- The script uses instructions from the ROS Wiki "Melodic" [Ubuntu page](http://wiki.ros.org/melodic/Installation/Ubuntu).
|
||||
:::
|
||||
|
||||
::::
|
||||
|
||||
## Install MAVROS
|
||||
|
||||
Then MAVROS can be installed either from source or binary.
|
||||
We recommend that developers use the source installation.
|
||||
|
||||
#### Binary Installation (Debian / Ubuntu)
|
||||
|
||||
The ROS repository has binary packages for Ubuntu x86, amd64 (x86_64) and armhf (ARMv7).
|
||||
Kinetic also supports Debian Jessie amd64 and arm64 (ARMv8).
|
||||
|
||||
Use `apt-get` for installation, where `${ROS_DISTRO}` below should resolve to `kinetic` or `noetic`, depending on your version of ROS:
|
||||
|
||||
```sh
|
||||
sudo apt-get install ros-${ROS_DISTRO}-mavros ros-${ROS_DISTRO}-mavros-extras ros-${ROS_DISTRO}-mavros-msgs
|
||||
```
|
||||
|
||||
Then install [GeographicLib](https://geographiclib.sourceforge.io/) datasets by running the `install_geographiclib_datasets.sh` script:
|
||||
|
||||
```sh
|
||||
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
|
||||
sudo bash ./install_geographiclib_datasets.sh
|
||||
```
|
||||
|
||||
#### Source Installation
|
||||
|
||||
This installation assumes you have a catkin workspace located at `~/catkin_ws` If you don't create one with:
|
||||
|
||||
```sh
|
||||
mkdir -p ~/catkin_ws/src
|
||||
cd ~/catkin_ws
|
||||
catkin init
|
||||
wstool init src
|
||||
```
|
||||
|
||||
You will be using the ROS Python tools: _wstool_ (for retrieving sources), _rosinstall_, and _catkin_tools_ (building) for this installation. While they may have been installed during your installation of ROS you can also install them with:
|
||||
|
||||
```sh
|
||||
sudo apt-get install python-catkin-tools python-rosinstall-generator -y
|
||||
```
|
||||
|
||||
:::tip
|
||||
While the package can be built using **catkin_make** the preferred method is using **catkin_tools** as it is a more versatile and "friendly" build tool.
|
||||
:::
|
||||
|
||||
If this is your first time using wstool you will need to initialize your source space with:
|
||||
|
||||
```sh
|
||||
$ wstool init ~/catkin_ws/src
|
||||
```
|
||||
|
||||
Now you are ready to do the build:
|
||||
|
||||
1. Install MAVLink:
|
||||
|
||||
```sh
|
||||
# We use the Kinetic reference for all ROS distros as it's not distro-specific and up to date
|
||||
rosinstall_generator --rosdistro kinetic mavlink | tee /tmp/mavros.rosinstall
|
||||
```
|
||||
|
||||
1. Install MAVROS from source using either released or latest version:
|
||||
|
||||
- Released/stable
|
||||
|
||||
```sh
|
||||
rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
|
||||
```
|
||||
|
||||
- Latest source
|
||||
|
||||
```sh
|
||||
rosinstall_generator --upstream-development mavros | tee -a /tmp/mavros.rosinstall
|
||||
```
|
||||
|
||||
```sh
|
||||
# For fetching all the dependencies into your catkin_ws,
|
||||
# just add '--deps' to the above scripts, E.g.:
|
||||
# rosinstall_generator --upstream mavros --deps | tee -a /tmp/mavros.rosinstall
|
||||
```
|
||||
|
||||
1. Create workspace & deps
|
||||
|
||||
```sh
|
||||
wstool merge -t src /tmp/mavros.rosinstall
|
||||
wstool update -t src -j4
|
||||
rosdep install --from-paths src --ignore-src -y
|
||||
```
|
||||
|
||||
1. Install [GeographicLib](https://geographiclib.sourceforge.io/) datasets:
|
||||
|
||||
```sh
|
||||
./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
|
||||
```
|
||||
|
||||
1. Build source
|
||||
|
||||
```sh
|
||||
catkin build
|
||||
```
|
||||
|
||||
1. Make sure that you use setup.bash or setup.zsh from workspace.
|
||||
|
||||
```sh
|
||||
#Needed or rosrun can't find nodes from this workspace.
|
||||
source devel/setup.bash
|
||||
```
|
||||
|
||||
In the case of error, there are addition installation and troubleshooting notes in the [mavros repo](https://github.com/mavlink/mavros/tree/master/mavros#installation).
|
||||
|
||||
## MAVROS Examples
|
||||
|
||||
The [MAVROS Offboard Example (C++)](../ros/mavros_offboard_cpp.md), will show you the basics of MAVROS, from reading telemetry, checking the drone state, changing flight modes and controlling the drone.
|
||||
|
||||
::: info
|
||||
If you have an example app using the PX4 Autopilot and MAVROS, we can help you get it on our docs.
|
||||
:::
|
||||
|
||||
## See Also
|
||||
|
||||
- [mavros ROS Package Summary](http://wiki.ros.org/mavros#mavros.2BAC8-Plugins.sys_status)
|
||||
- [mavros source](https://github.com/mavlink/mavros/)
|
||||
- [ROS Melodic installation instructions](http://wiki.ros.org/melodic/Installation)
|
||||
@@ -0,0 +1,236 @@
|
||||
# MAVROS _Offboard_ control example (C++)
|
||||
|
||||
This tutorial shows the basics of _Offboard_ control with MAVROS, using an Iris quadcopter simulated in Gazebo Classic/SITL.
|
||||
At the end of the tutorial, you should see the same behaviour as in the video below, i.e. a slow takeoff to an altitude of 2 meters.
|
||||
|
||||
:::warning
|
||||
_Offboard_ control is dangerous.
|
||||
If you are operating on a real vehicle be sure to have a way of gaining back manual control in case something goes wrong.
|
||||
:::
|
||||
|
||||
:::tip
|
||||
This example uses C++.
|
||||
A very similar example for Python can be found in [ROS/MAVROS Offboard Example (Python)](../ros/mavros_offboard_python.md) (also see the examples in [integrationtests/python_src/px4_it/mavros](https://github.com/PX4/PX4-Autopilot/tree/main/integrationtests/python_src/px4_it/mavros)).
|
||||
:::
|
||||
|
||||
<video width="100%" autoplay="true" controls="true">
|
||||
<source src="../../assets/simulation/gazebo_classic/gazebo_offboard.webm" type="video/webm">
|
||||
</video>
|
||||
|
||||
## Code
|
||||
|
||||
Create the `offb_node.cpp` file in your ROS package (by also adding it to your `CMakeList.txt` so it is compiled), and paste the following inside it:
|
||||
|
||||
```cpp
|
||||
/**
|
||||
* @file offb_node.cpp
|
||||
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
|
||||
* Stack and tested in Gazebo Classic SITL
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
|
||||
mavros_msgs::State current_state;
|
||||
void state_cb(const mavros_msgs::State::ConstPtr& msg){
|
||||
current_state = *msg;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "offb_node");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
|
||||
("mavros/state", 10, state_cb);
|
||||
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
|
||||
("mavros/setpoint_position/local", 10);
|
||||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
|
||||
("mavros/cmd/arming");
|
||||
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
|
||||
("mavros/set_mode");
|
||||
|
||||
//the setpoint publishing rate MUST be faster than 2Hz
|
||||
ros::Rate rate(20.0);
|
||||
|
||||
// wait for FCU connection
|
||||
while(ros::ok() && !current_state.connected){
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
geometry_msgs::PoseStamped pose;
|
||||
pose.pose.position.x = 0;
|
||||
pose.pose.position.y = 0;
|
||||
pose.pose.position.z = 2;
|
||||
|
||||
//send a few setpoints before starting
|
||||
for(int i = 100; ros::ok() && i > 0; --i){
|
||||
local_pos_pub.publish(pose);
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
mavros_msgs::SetMode offb_set_mode;
|
||||
offb_set_mode.request.custom_mode = "OFFBOARD";
|
||||
|
||||
mavros_msgs::CommandBool arm_cmd;
|
||||
arm_cmd.request.value = true;
|
||||
|
||||
ros::Time last_request = ros::Time::now();
|
||||
|
||||
while(ros::ok()){
|
||||
if( current_state.mode != "OFFBOARD" &&
|
||||
(ros::Time::now() - last_request > ros::Duration(5.0))){
|
||||
if( set_mode_client.call(offb_set_mode) &&
|
||||
offb_set_mode.response.mode_sent){
|
||||
ROS_INFO("Offboard enabled");
|
||||
}
|
||||
last_request = ros::Time::now();
|
||||
} else {
|
||||
if( !current_state.armed &&
|
||||
(ros::Time::now() - last_request > ros::Duration(5.0))){
|
||||
if( arming_client.call(arm_cmd) &&
|
||||
arm_cmd.response.success){
|
||||
ROS_INFO("Vehicle armed");
|
||||
}
|
||||
last_request = ros::Time::now();
|
||||
}
|
||||
}
|
||||
|
||||
local_pos_pub.publish(pose);
|
||||
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
## Code explanation
|
||||
|
||||
```cpp
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
```
|
||||
|
||||
The `mavros_msgs` package contains all of the custom messages required to operate services and topics provided by the MAVROS package.
|
||||
All services and topics as well as their corresponding message types are documented in the [mavros wiki](http://wiki.ros.org/mavros).
|
||||
|
||||
```cpp
|
||||
mavros_msgs::State current_state;
|
||||
void state_cb(const mavros_msgs::State::ConstPtr& msg){
|
||||
current_state = *msg;
|
||||
}
|
||||
```
|
||||
|
||||
We create a simple callback which will save the current state of the autopilot.
|
||||
This will allow us to check connection, arming and _Offboard_ flags.
|
||||
|
||||
```cpp
|
||||
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
|
||||
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
|
||||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
|
||||
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
|
||||
```
|
||||
|
||||
We instantiate a publisher to publish the commanded local position and the appropriate clients to request arming and mode change.
|
||||
Note that for your own system, the "mavros" prefix might be different as it will depend on the name given to the node in it's launch file.
|
||||
|
||||
```cpp
|
||||
//the setpoint publishing rate MUST be faster than 2Hz
|
||||
ros::Rate rate(20.0);
|
||||
```
|
||||
|
||||
PX4 has a timeout of 500ms between two _Offboard_ commands.
|
||||
If this timeout is exceeded, the commander will fall back to the last mode the vehicle was in before entering _Offboard_ mode.
|
||||
This is why the publishing rate **must** be faster than 2 Hz to also account for possible latencies.
|
||||
This is also the same reason why it is recommended to enter _Offboard_ mode from _Position_ mode, this way if the vehicle drops out of _Offboard_ mode it will stop in its tracks and hover.
|
||||
|
||||
```cpp
|
||||
// wait for FCU connection
|
||||
while(ros::ok() && !current_state.connected){
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
```
|
||||
|
||||
Before publishing anything, we wait for the connection to be established between MAVROS and the autopilot.
|
||||
This loop should exit as soon as a heartbeat message is received.
|
||||
|
||||
```cpp
|
||||
geometry_msgs::PoseStamped pose;
|
||||
pose.pose.position.x = 0;
|
||||
pose.pose.position.y = 0;
|
||||
pose.pose.position.z = 2;
|
||||
```
|
||||
|
||||
Even though the PX4 Pro Flight Stack operates in the aerospace NED coordinate frame, MAVROS translates these coordinates to the standard ENU frame and vice-versa.
|
||||
This is why we set `z` to positive 2.
|
||||
|
||||
```cpp
|
||||
//send a few setpoints before starting
|
||||
for(int i = 100; ros::ok() && i > 0; --i){
|
||||
local_pos_pub.publish(pose);
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
```
|
||||
|
||||
Before entering _Offboard_ mode, you must have already started streaming setpoints.
|
||||
Otherwise the mode switch will be rejected. Here, `100` was chosen as an arbitrary amount.
|
||||
|
||||
```cpp
|
||||
mavros_msgs::SetMode offb_set_mode;
|
||||
offb_set_mode.request.custom_mode = "OFFBOARD";
|
||||
```
|
||||
|
||||
We set the custom mode to `OFFBOARD`.
|
||||
A list of [supported modes](http://wiki.ros.org/mavros/CustomModes#PX4_native_flight_stack) is available for reference.
|
||||
|
||||
```cpp
|
||||
mavros_msgs::CommandBool arm_cmd;
|
||||
arm_cmd.request.value = true;
|
||||
|
||||
ros::Time last_request = ros::Time::now();
|
||||
|
||||
while(ros::ok()){
|
||||
if( current_state.mode != "OFFBOARD" &&
|
||||
(ros::Time::now() - last_request > ros::Duration(5.0))){
|
||||
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
|
||||
ROS_INFO("Offboard enabled");
|
||||
}
|
||||
last_request = ros::Time::now();
|
||||
} else {
|
||||
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))) {
|
||||
if( arming_client.call(arm_cmd) && arm_cmd.response.success) {
|
||||
ROS_INFO("Vehicle armed");
|
||||
}
|
||||
last_request = ros::Time::now();
|
||||
}
|
||||
}
|
||||
|
||||
local_pos_pub.publish(pose);
|
||||
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
```
|
||||
|
||||
The rest of the code is pretty self explanatory.
|
||||
We attempt to switch to _Offboard_ mode, after which we arm the quad to allow it to fly.
|
||||
We space out the service calls by 5 seconds so to not flood the autopilot with the requests.
|
||||
In the same loop, we continue sending the requested pose at the appropriate rate.
|
||||
|
||||
:::tip
|
||||
This code has been simplified to the bare minimum for illustration purposes.
|
||||
In larger systems, it is often useful to create a new thread which will be in charge of periodically publishing the setpoints.
|
||||
:::
|
||||
@@ -0,0 +1,365 @@
|
||||
# MAVROS _Offboard_ control example (Python)
|
||||
|
||||
This tutorial shows the basics of _OFFBOARD_ control with MAVROS Python, using an Iris quadcopter simulated in [Gazebo Classic](../sim_gazebo_classic/index.md).
|
||||
It provides step-by-step instructions demonstrating how to start developing programs to control a vehicle and running the code in simulation.
|
||||
|
||||
At the end of the tutorial, you should see the same behaviour as in the video below, i.e. a slow takeoff to an altitude of 2 meters.
|
||||
|
||||
:::warning
|
||||
_OFFBOARD_ control is dangerous.
|
||||
If you are operating on a real vehicle be sure to have a way of gaining back manual control in case something goes wrong.
|
||||
:::
|
||||
|
||||
:::tip
|
||||
This example uses Python.
|
||||
Other examples in Python can be found here: [integrationtests/python_src/px4_it/mavros](https://github.com/PX4/PX4-Autopilot/tree/main/integrationtests/python_src/px4_it/mavros).
|
||||
:::
|
||||
|
||||
<a id="offb_video"></a>
|
||||
|
||||
<video width="100%" autoplay="true" controls="true">
|
||||
<source src="../../assets/simulation/gazebo_classic/gazebo_offboard.webm" type="video/webm">
|
||||
</video>
|
||||
|
||||
## Creating the ROS Package
|
||||
|
||||
1. Open the terminal and go to `~/catkin_ws/src` directory
|
||||
|
||||
```sh
|
||||
roscd # Should cd into ~/catkin_ws/devel
|
||||
cd ..
|
||||
cd src
|
||||
```
|
||||
|
||||
2. In the `~/catkin_ws/src` directory create a new package named `offboard_py` (in this case) with the `rospy` dependency:
|
||||
|
||||
```sh
|
||||
catkin_create_pkg offboard_py rospy
|
||||
```
|
||||
|
||||
3. Build the new package in the `~/catkin_ws/` directory:
|
||||
|
||||
```sh
|
||||
cd .. # Assuming previous directory to be ~/catkin_ws/src
|
||||
catkin build
|
||||
source devel/setup.bash
|
||||
```
|
||||
|
||||
4. You should now be able to cd into the package by using:
|
||||
|
||||
```sh
|
||||
roscd offboard_py
|
||||
```
|
||||
|
||||
5. To store your Python files, create a new folder called `/scripts` on the package:
|
||||
|
||||
```sh
|
||||
mkdir scripts
|
||||
cd scripts
|
||||
```
|
||||
|
||||
## Code
|
||||
|
||||
After creating the ROS package and scripts folder you are ready to start your Python script.
|
||||
Inside the scripts folder create the `offb_node.py` file and give it executable permissions:
|
||||
|
||||
```sh
|
||||
touch offb_node.py
|
||||
chmod +x offb_node.py
|
||||
```
|
||||
|
||||
After that, open `offb_node.py` file and paste the following code:
|
||||
|
||||
```py
|
||||
"""
|
||||
* File: offb_node.py
|
||||
* Stack and tested in Gazebo Classic 9 SITL
|
||||
"""
|
||||
|
||||
#! /usr/bin/env python
|
||||
|
||||
import rospy
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from mavros_msgs.msg import State
|
||||
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest
|
||||
|
||||
current_state = State()
|
||||
|
||||
def state_cb(msg):
|
||||
global current_state
|
||||
current_state = msg
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
rospy.init_node("offb_node_py")
|
||||
|
||||
state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
|
||||
|
||||
local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
|
||||
|
||||
rospy.wait_for_service("/mavros/cmd/arming")
|
||||
arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
|
||||
|
||||
rospy.wait_for_service("/mavros/set_mode")
|
||||
set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
|
||||
|
||||
|
||||
# Setpoint publishing MUST be faster than 2Hz
|
||||
rate = rospy.Rate(20)
|
||||
|
||||
# Wait for Flight Controller connection
|
||||
while(not rospy.is_shutdown() and not current_state.connected):
|
||||
rate.sleep()
|
||||
|
||||
pose = PoseStamped()
|
||||
|
||||
pose.pose.position.x = 0
|
||||
pose.pose.position.y = 0
|
||||
pose.pose.position.z = 2
|
||||
|
||||
# Send a few setpoints before starting
|
||||
for i in range(100):
|
||||
if(rospy.is_shutdown()):
|
||||
break
|
||||
|
||||
local_pos_pub.publish(pose)
|
||||
rate.sleep()
|
||||
|
||||
offb_set_mode = SetModeRequest()
|
||||
offb_set_mode.custom_mode = 'OFFBOARD'
|
||||
|
||||
arm_cmd = CommandBoolRequest()
|
||||
arm_cmd.value = True
|
||||
|
||||
last_req = rospy.Time.now()
|
||||
|
||||
while(not rospy.is_shutdown()):
|
||||
if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
|
||||
if(set_mode_client.call(offb_set_mode).mode_sent == True):
|
||||
rospy.loginfo("OFFBOARD enabled")
|
||||
|
||||
last_req = rospy.Time.now()
|
||||
else:
|
||||
if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
|
||||
if(arming_client.call(arm_cmd).success == True):
|
||||
rospy.loginfo("Vehicle armed")
|
||||
|
||||
last_req = rospy.Time.now()
|
||||
|
||||
local_pos_pub.publish(pose)
|
||||
|
||||
rate.sleep()
|
||||
|
||||
```
|
||||
|
||||
## Code explanation
|
||||
|
||||
The `mavros_msgs` package contains all of the custom messages required to operate services and topics provided by the MAVROS package.
|
||||
All services and topics as well as their corresponding message types are documented in the [mavros wiki](http://wiki.ros.org/mavros).
|
||||
|
||||
```py
|
||||
import rospy
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from mavros_msgs.msg import State
|
||||
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest
|
||||
```
|
||||
|
||||
We create a simple callback which will save the current state of the autopilot.
|
||||
This will allow us to check connection, arming and _OFFBOARD_ flags.:
|
||||
|
||||
```py
|
||||
current_state = State()
|
||||
|
||||
def state_cb(msg):
|
||||
global current_state
|
||||
current_state = msg
|
||||
```
|
||||
|
||||
We instantiate a publisher to publish the commanded local position and the appropriate clients to request arming and mode change.
|
||||
Note that for your own system, the "mavros" prefix might be different as it will depend on the name given to the node in it's launch file.
|
||||
|
||||
```py
|
||||
state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
|
||||
|
||||
local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
|
||||
|
||||
rospy.wait_for_service("/mavros/cmd/arming")
|
||||
arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
|
||||
|
||||
rospy.wait_for_service("/mavros/set_mode")
|
||||
set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
|
||||
```
|
||||
|
||||
PX4 has a timeout of 500ms between two _OFFBOARD_ commands.
|
||||
If this timeout is exceeded, the commander will fall back to the last mode the vehicle was in before entering _OFFBOARD_ mode.
|
||||
This is why the publishing rate **must** be faster than 2 Hz to also account for possible latencies.
|
||||
This is also the same reason why it is **recommended to enter _OFFBOARD_ mode from _Position_ mode**, this way if the vehicle drops out of _OFFBOARD_ mode it will stop in its tracks and hover.
|
||||
|
||||
Here we set the publishing rate appropriately:
|
||||
|
||||
```py
|
||||
# Setpoint publishing MUST be faster than 2Hz
|
||||
rate = rospy.Rate(20)
|
||||
```
|
||||
|
||||
Before publishing anything, we wait for the connection to be established between MAVROS and the autopilot.
|
||||
This loop should exit as soon as a heartbeat message is received.
|
||||
|
||||
```py
|
||||
# Wait for Flight Controller connection
|
||||
while(not rospy.is_shutdown() and not current_state.connected):
|
||||
rate.sleep()
|
||||
```
|
||||
|
||||
Even though PX4 operates in the aerospace NED coordinate frame, MAVROS translates these coordinates to the standard ENU frame and vice-versa.
|
||||
This is why we set `z` to positive 2:
|
||||
|
||||
```py
|
||||
pose = PoseStamped()
|
||||
|
||||
pose.pose.position.x = 0
|
||||
pose.pose.position.y = 0
|
||||
pose.pose.position.z = 2
|
||||
```
|
||||
|
||||
Before entering _OFFBOARD_ mode, you must have already started streaming setpoints.
|
||||
Otherwise the mode switch will be rejected.
|
||||
Below, `100` was chosen as an arbitrary amount.
|
||||
|
||||
```py
|
||||
# Send a few setpoints before starting
|
||||
for i in range(100):
|
||||
if(rospy.is_shutdown()):
|
||||
break
|
||||
|
||||
local_pos_pub.publish(pose)
|
||||
rate.sleep()
|
||||
```
|
||||
|
||||
We prepare the message request used to set the custom mode to `OFFBOARD`.
|
||||
A list of [supported modes](http://wiki.ros.org/mavros/CustomModes#PX4_native_flight_stack) is available for reference.
|
||||
|
||||
```py
|
||||
offb_set_mode = SetModeRequest()
|
||||
offb_set_mode.custom_mode = 'OFFBOARD'
|
||||
```
|
||||
|
||||
The rest of the code is largely self explanatory.
|
||||
We attempt to switch to _Offboard_ mode, after which we arm the quad to allow it to fly.
|
||||
We space out the service calls by 5 seconds so to not flood the autopilot with the requests.
|
||||
In the same loop, we continue sending the requested pose at the rate previously defined.
|
||||
|
||||
```py
|
||||
arm_cmd = CommandBoolRequest()
|
||||
arm_cmd.value = True
|
||||
|
||||
last_req = rospy.Time.now()
|
||||
|
||||
while(not rospy.is_shutdown()):
|
||||
if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
|
||||
if(set_mode_client.call(offb_set_mode).mode_sent == True):
|
||||
rospy.loginfo("OFFBOARD enabled")
|
||||
|
||||
last_req = rospy.Time.now()
|
||||
else:
|
||||
if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
|
||||
if(arming_client.call(arm_cmd).success == True):
|
||||
rospy.loginfo("Vehicle armed")
|
||||
|
||||
last_req = rospy.Time.now()
|
||||
|
||||
local_pos_pub.publish(pose)
|
||||
|
||||
rate.sleep()
|
||||
```
|
||||
|
||||
:::tip
|
||||
This code has been simplified to the bare minimum for illustration purposes.
|
||||
In larger systems, it is often useful to create a new thread which will be in charge of periodically publishing the setpoints.
|
||||
:::
|
||||
|
||||
## Creating the ROS launch file
|
||||
|
||||
In your `offboard_py` package, create another folder inside the `~/catkin_ws/src/offboard_py/src` directory named `launch`.
|
||||
This is where your launch files for the package will be stored.
|
||||
After that, create your first launch file, in this case we will call it `start_offb.launch`.
|
||||
|
||||
```sh
|
||||
roscd offboard_py
|
||||
mkdir launch
|
||||
cd launch
|
||||
touch start_offb.launch
|
||||
```
|
||||
|
||||
For the `start_offb.launch` copy the following code:
|
||||
|
||||
```xml
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Include the MAVROS node with SITL and Gazebo -->
|
||||
<include file="$(find px4)/launch/mavros_posix_sitl.launch">
|
||||
</include>
|
||||
|
||||
<!-- Our node to control the drone -->
|
||||
<node pkg="offboard_py" type="offb_node.py" name="offb_node_py" required="true" output="screen" />
|
||||
</launch>
|
||||
```
|
||||
|
||||
As you can see, the `mavros_posix_sitl.launch` file is included.
|
||||
This file is responsible for launching MAVROS, the PX4 SITL, the Gazebo Classic Environment and for spawning a vehicle in a given world (for further information see the file [here](https://github.com/PX4/PX4-Autopilot/blob/main/launch/mavros_posix_sitl.launch)).
|
||||
|
||||
:::tip
|
||||
The `mavros_posix_sitl.launch` file takes several arguments that can be set according to your preferences such as the vehicle to spawn or the Gazebo Classic world (refer to [here](https://github.com/PX4/PX4-Autopilot/blob/main/launch/mavros_posix_sitl.launch)) for a complete list).
|
||||
|
||||
You can override the default value of these arguments defined in `mavros_posix_sitl.launch` by declaring them inside the _include_ tags.
|
||||
As an example, if you wanted to spawn the vehicle in the `warehouse.world`, you would write the following:
|
||||
|
||||
```xml
|
||||
<!-- Include the MAVROS node with SITL and Gazebo -->
|
||||
<include file="$(find px4)/launch/mavros_posix_sitl.launch">
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/warehouse.world"/>
|
||||
</include>
|
||||
```
|
||||
|
||||
:::
|
||||
|
||||
## Launching your script
|
||||
|
||||
If everything is done, you should now be able to launch and test your script.
|
||||
|
||||
In the terminal write:
|
||||
|
||||
```sh
|
||||
roslaunch offboard_py start_offb.launch
|
||||
```
|
||||
|
||||
You should now see the PX4 firmware initiating and the Gazebo Classic application running.
|
||||
After the _OFFBOARD_ mode is set and the vehicle is armed, the behavior shown in the [video](#offb_video) should be observed.
|
||||
|
||||
:::warning
|
||||
It is possible that when running the script an error appears saying:
|
||||
|
||||
> Resource not found: px4
|
||||
> ROS path [0] = ...
|
||||
> ...
|
||||
|
||||
This means that PX4 SITL was not included in the path.
|
||||
To solve this add these lines at the end of the `.bashrc` file:
|
||||
|
||||
```sh
|
||||
source ~/PX4-Autopilot/Tools/simulation/gazebo/setup_gazebo.bash ~/PX4-Autopilot ~/PX4-Autopilot/build/px4_sitl_default
|
||||
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot
|
||||
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot/Tools/simulation/gazebo-classic/sitl_gazebo-classic
|
||||
export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/usr/lib/x86_64-linux-gnu/gazebo-9/plugins
|
||||
```
|
||||
|
||||
Now in the terminal, go to the home directory and run the following command to apply the changes above to the current terminal:
|
||||
|
||||
```sh
|
||||
source .bashrc
|
||||
```
|
||||
|
||||
After this step, every time you open a new terminal window you should not have to worry about this error anymore.
|
||||
If it appears again, a simple `source .bashrc` should fix it.
|
||||
This solution was obtained from this [issue](https://github.com/mzahana/px4_fast_planner/issues/4) thread, where you can get more information about the problem.
|
||||
:::
|
||||
@@ -0,0 +1,88 @@
|
||||
# Offboard Control
|
||||
|
||||
::: warning
|
||||
[Offboard control](../flight_modes/offboard.md) is dangerous.
|
||||
It is the responsibility of the developer to ensure adequate preparation, testing and safety precautions are taken before offboard flights.
|
||||
:::
|
||||
|
||||
The idea behind off-board control is to be able to control the PX4 flight stack using software running outside of the autopilot.
|
||||
This is done through the MAVLink protocol, specifically the [SET_POSITION_TARGET_LOCAL_NED](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED) and the [SET_ATTITUDE_TARGET](https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET) messages.
|
||||
|
||||
## Offboard Control Firmware Setup
|
||||
|
||||
There are two things you want to setup on the firmware side before starting offboard development.
|
||||
|
||||
### Enable RC Override
|
||||
|
||||
In _QGroundControl_ you can set the [COM_RC_OVERRIDE](../advanced_config/parameter_reference.md#COM_RC_OVERRIDE) parameter to automatically switch from offboard mode (or any mode) to Position mode if the RC sticks are moved.
|
||||
This is the best way to ensure that an operator can easily take control of the vehicle and switch to the safest flight mode.
|
||||
|
||||
### Map an RC switch to offboard mode activation
|
||||
|
||||
In _QGroundControl_ you can set the [RC_MAP_OFFB_SW](../advanced_config/parameter_reference.md#RC_MAP_OFFB_SW) parameter to the RC channel that will be used to activate offboard mode.
|
||||
This can be used to switch between offboard mode and the mode set by the mode switch ([RC_MAP_MODE_SW](../advanced_config/parameter_reference.md#RC_MAP_MODE_SW)).
|
||||
You can also switch into offboard mode using a GCS/MAVLink so this is not "mandatory".
|
||||
|
||||
Note also that this mechanism is not as "safe" as using [RC Override](#enable-rc-override) to switch out of offboard mode, because the mode you switch to is unpredictable.
|
||||
|
||||
### Enable the companion computer interface
|
||||
|
||||
Enable MAVLink on the serial port that you connect to the companion computer (see [Companion Computers](../companion_computer/index.md)).
|
||||
|
||||
## Hardware setup
|
||||
|
||||
Usually, there are three ways of setting up offboard communication.
|
||||
|
||||
### Serial radios
|
||||
|
||||
1. One connected to a UART port of the autopilot
|
||||
2. One connected to a ground station computer
|
||||
|
||||
Example radios include:
|
||||
|
||||
- [Lairdtech RM024](http://www.lairdtech.com/products/rm024)
|
||||
- [Digi International XBee Pro](http://www.digi.com/products/xbee-rf-solutions/modules)
|
||||
|
||||
[](https://mermaid-js.github.io/mermaid-live-editor/#/edit/eyJjb2RlIjoiZ3JhcGggVEQ7XG4gIGduZFtHcm91bmQgU3RhdGlvbl0gLS1NQVZMaW5rLS0-IHJhZDFbR3JvdW5kIFJhZGlvXTtcbiAgcmFkMSAtLVJhZGlvUHJvdG9jb2wtLT4gcmFkMltWZWhpY2xlIFJhZGlvXTtcbiAgcmFkMiAtLU1BVkxpbmstLT4gYVtBdXRvcGlsb3RdOyIsIm1lcm1haWQiOnsidGhlbWUiOiJkZWZhdWx0In0sInVwZGF0ZUVkaXRvciI6ZmFsc2V9)
|
||||
|
||||
<!-- original mermaid graph
|
||||
graph TD;
|
||||
gnd[Ground Station] --MAVLink-- > rad1[Ground Radio];
|
||||
rad1 --RadioProtocol-- > rad2[Vehicle Radio];
|
||||
rad2 --MAVLink-- > a[Autopilot];
|
||||
-->
|
||||
|
||||
### On-board processor
|
||||
|
||||
A small computer mounted onto the vehicle, connected to the autopilot through a serial port or Ehthernet port.
|
||||
There are many possibilities here and it will depend on what kind of additional on-board processing you want to do in addition to sending commands to the autopilot.
|
||||
Some examples are provided in [Companion Computers](../companion_computer/index.md#companion-computer-options).
|
||||
|
||||
[](https://mermaid-js.github.io/mermaid-live-editor/#/edit/eyJjb2RlIjoiZ3JhcGggVEQ7XG4gIGNvbXBbQ29tcGFuaW9uIENvbXB1dGVyXSAtLU1BVkxpbmstLT4gdWFydFtVQVJUIEFkYXB0ZXJdO1xuICB1YXJ0IC0tTUFWTGluay0tPiBBdXRvcGlsb3Q7IiwibWVybWFpZCI6eyJ0aGVtZSI6ImRlZmF1bHQifSwidXBkYXRlRWRpdG9yIjpmYWxzZX0)
|
||||
|
||||
<!-- original mermaid graph
|
||||
graph TD;
|
||||
comp[Companion Computer] --MAVLink-- > uart[UART Adapter];
|
||||
uart --MAVLink-- > Autopilot;
|
||||
-->
|
||||
|
||||
### On-board processor and wifi link to ROS (**_Recommended_**)
|
||||
|
||||
A small computer mounted onto the vehicle connected to the autopilot through a UART to USB adapter while also having a WiFi link to a ground station running ROS.
|
||||
This can be any of the computers from the above section coupled with a WiFi adapter.
|
||||
|
||||
[](https://mermaid-js.github.io/mermaid-live-editor/#/edit/eyJjb2RlIjoiZ3JhcGggVERcbiAgc3ViZ3JhcGggR3JvdW5kICBTdGF0aW9uXG4gIGduZFtST1MgRW5hYmxlZCBDb21wdXRlcl0gLS0tIHFnY1txR3JvdW5kQ29udHJvbF1cbiAgZW5kXG4gIGduZCAtLU1BVkxpbmsvVURQLS0-IHdbV2lGaV07XG4gIHFnYyAtLU1BVkxpbmstLT4gdztcbiAgc3ViZ3JhcGggVmVoaWNsZVxuICBjb21wW0NvbXBhbmlvbiBDb21wdXRlcl0gLS1NQVZMaW5rLS0-IHVhcnRbVUFSVCBBZGFwdGVyXVxuICB1YXJ0IC0tLSBBdXRvcGlsb3RcbiAgZW5kXG4gIHcgLS0tIGNvbXAiLCJtZXJtYWlkIjp7InRoZW1lIjoiZGVmYXVsdCJ9LCJ1cGRhdGVFZGl0b3IiOmZhbHNlfQ)
|
||||
|
||||
<!-- original mermaid graph
|
||||
graph TD
|
||||
subgraph Ground Station
|
||||
gnd[ROS Enabled Computer] --- qgc[qGroundControl]
|
||||
end
|
||||
gnd --MAVLink/UDP-- > w[WiFi];
|
||||
qgc --MAVLink-- > w;
|
||||
subgraph Vehicle
|
||||
comp[Companion Computer] --MAVLink-- > uart[UART Adapter]
|
||||
uart --- Autopilot
|
||||
end
|
||||
w --- comp
|
||||
-->
|
||||
@@ -0,0 +1,40 @@
|
||||
# Raspberry Pi - ROS installation
|
||||
|
||||
This is a guide on how to install ROS-indigo on a Raspberry Pi 2 serving as a companion computer for Pixhawk.
|
||||
|
||||
## Prerequisites
|
||||
* A working Raspberry Pi with monitor, keyboard, or configured SSH connection
|
||||
* This guide assumes that you have Raspbian "JESSIE" installed on your RPi. If not: [install it](https://www.raspberrypi.org/downloads/raspbian/) or [upgrade](http://raspberrypi.stackexchange.com/questions/27858/upgrade-to-raspbian-jessie) your Raspbian Wheezy to Jessie.
|
||||
|
||||
## Installation
|
||||
Follow [this guide](http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Indigo%20on%20Raspberry%20Pi) for the actual installation of ROS Indigo. Note: Install the "ROS-Comm" variant. The Desktop variant is too heavyweight.
|
||||
|
||||
### Errors when installing packages
|
||||
If you want to download packages (e.g. `sudo apt-get install ros-indigo-ros-tutorials`), you might get an error saying: "unable to locate package ros-indigo-ros-tutorials".
|
||||
|
||||
If so, proceed as follows:
|
||||
Go to your catkin workspace (e.g. ~/ros_catkin_ws) and change the name of the packages.
|
||||
|
||||
```sh
|
||||
$ cd ~/ros_catkin_ws
|
||||
|
||||
$ rosinstall_generator ros_tutorials --rosdistro indigo --deps --wet-only --exclude roslisp --tar > indigo-custom_ros.rosinstall
|
||||
```
|
||||
|
||||
Next, update your workspace with wstool.
|
||||
|
||||
```sh
|
||||
$ wstool merge -t src indigo-custom_ros.rosinstall
|
||||
|
||||
$ wstool update -t src
|
||||
```
|
||||
|
||||
Next (still in your workspace folder), source and make your files.
|
||||
|
||||
```sh
|
||||
$ source /opt/ros/indigo/setup.bash
|
||||
|
||||
$ source devel/setup.bash
|
||||
|
||||
$ catkin_make
|
||||
```
|
||||
@@ -0,0 +1,28 @@
|
||||
# ROS 1 (Deprecated)
|
||||
|
||||
::: warning
|
||||
The PX4 development team recommend that users migrate to [ROS 2](../ros2/index.md) (i.e. skip this section)!
|
||||
|
||||
ROS 1 is now "discommended" as the last LTS version is approaching end of life.
|
||||
ROS 2 has much deeper integration with PX4, enabling lower-latency communication with access to PX4 internal messaging.
|
||||
:::
|
||||
|
||||
[ROS (1)](http://www.ros.org/) is a general purpose robotics library that can be used with PX4 for drone application development.
|
||||
|
||||
This version of ROS uses the [MAVROS](../ros/mavros_installation.md) package to communicate with PX4 over [MAVLink](../middleware/mavlink.md) (MAVROS bridges ROS topics to MAVLink and PX4 conventions).
|
||||
|
||||
## Topics
|
||||
|
||||
- [ROS/MAVROS Installation Guide](../ros/mavros_installation.md): Setup a PX4 development environment with ROS 1 and MAVROS.
|
||||
- [ROS/MAVROS Offboard Example (C++)](../ros/mavros_offboard_cpp.md): Tutorial showing the main concepts related to writing a C++ MAVROS/ROS node.
|
||||
- [ROS MAVROS Sending Custom Messages](../ros/mavros_custom_messages.md)
|
||||
- [ROS with Gazebo Classic Simulation](../simulation/ros_interface.md)
|
||||
- [Gazebo Classic OctoMap Models with ROS](../sim_gazebo_classic/octomap.md)
|
||||
- [ROS Installation on RPi](../ros/raspberrypi_installation.md)
|
||||
- [External Position Estimation (Vision/Motion based)](../ros/external_position_estimation.md)
|
||||
|
||||
## Further Infomration
|
||||
|
||||
- [XTDrone](https://github.com/robin-shaun/XTDrone/blob/master/README.en.md) - ROS + PX4 simulation environment for computer vision.
|
||||
The [XTDrone Manual](https://www.yuque.com/xtdrone/manual_en) has everything you need to get started!
|
||||
- [Prometheus Autonomous Drone Project](https://github.com/amov-lab/Prometheus/blob/master/README_EN.md) - Prometheus is a ROS 1 based, BSD-3 licensed collection of autonomous drone software packages from [AMOVLab](https://github.com/amov-lab), which provides a full set of solutions for the intelligent and autonomous flight of drones, such as mapping, localization, planning, control, and target detection, fully integrated with the [Gazebo Classic](../sim_gazebo_classic/index.md) Simulator.
|
||||
@@ -0,0 +1 @@
|
||||
<Redirect to="../ros2/" />
|
||||
@@ -0,0 +1 @@
|
||||
<Redirect to="../ros2/user_guide" />
|
||||
@@ -0,0 +1 @@
|
||||
<Redirect to="../ros2/multi_vehicle" />
|
||||
@@ -0,0 +1 @@
|
||||
<Redirect to="../ros2/offboard_control" />
|
||||
Reference in New Issue
Block a user