mavros_apriltag_tracking

This package allows a PX4-powered drone to track a moving target (an AprilTag in this case) which is detected by a camera. Tag detection is done using apriltag_ros package. The target's position and velocity estimation is done using an implementaion of a Kalman filter based on a constant velocity model. The drone is controllerd using an implementation of a position controller which sends velocity commands to PX4. The detection and control are done with respect to the drone frame. So, errors caused by drifts in the local fixed frame can be avoided.

Kindly, give this repo a STAR if it helps you in your work

Video:

Installation

This package is tested with Ubuntu 18 + ROS Melodic + PX4 10.1

Docker image

The easiest way to test this package is through a ready Docker image prepared for this project which is available here

Use docker_px4_vehicle_tracking.sh script to automatically pull and run the docker image. Don't forget to make it executable using chmod +x docker_px4_vehicle_tracking.sh

./docker_px4_vehicle_tracking.sh <optional_container_name>

Once logged into the container terminal, proceed to the Simulation seciton.

Normal installation

Clone this package into your ~/catkin_ws/src folder

cd ~/catkin_ws/src
git clone https://github.com/mzahana/mavros_apriltag_tracking.git

Use the install/setup.sh script to do the installation

cd ~/catkin_ws/src/mavros_apriltag_tracking/install
./setup.sh

Copy the modified typhoon_h480 model to /home/arrow/Firmware/Tools/sitl_gazebo/models

cp -R ~/catkin_ws/src/mavros_apriltag_tracking/models/typhoon_h480 ~/Firmware/Tools/sitl_gazebo/models/

Copy the modified Husky simulation model. You need sudo for this one

sudo cp ~/catkin_ws/src/mavros_apriltag_tracking/models/custom_husky/husky.urdf.xacro $(catkin_find husky_description/urdf)/

Simulation

Use the launch/tracker.launch file to run the simulation
```
roslaunch mavros_apriltag_tracking tracker.launch
```
All steps required to set a takeoff position, bringing camera down, arming and setting OFFBOARD mode, and moving the Husky are available in a shell script which can be run using rosrun mavros_apriltag_tracking start_tracking.sh

THAT IS IT!

The steps in the start_tracking.sh script are explained in details below:

Bring the drone above and close to the tag by publishing one message to the /setpoint/local_pos
```
rostopic pub --once /setpoint/local_pos geometry_msgs/Point "x: 0.0
y: 0.0
z: 3.0"
```

Make the camera face downward (pitch rotation of -90 degrees)

rostopic pub --once /mavros/mount_control/command mavros_msgs/MountControl "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
mode: 2
pitch: -90.0
roll: 0.0
yaw: 0.0
altitude: 0.0
latitude: 0.0
longitude: 0.0"

NOTE: The camera won't move until the drone is armed, but the command will be saved.

Arm the drone and activate the OFFBOARD mode for the setpoint to take effect
```
rosservice call /arm_and_offboard "{}"
```

You should see the drone taking off. Then, once the tag on the Husky UGV is detected, the drone will be automatically controlled to hover above the tag. The dron will keep tracking the tag on top of the Husky while it's moving.

To move the Husky around, publish a linear velocity (along forward direction of the Husky's x-axis), and a rotational velocity about the Husky's z-axis as follows.

rostopic pub -r 10 /husky_velocity_controller/cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.4"

Look at the launch/tracker.launch file and change parameters as needed.

Nodes

The following are the ROS nodes in this package.

mavros_offboard_controller.py: Mainly implements a position controller and sends velocity commands to PX4. This controller accepts two types of position setpoints.
- Position setpoints in local fixed frame. Corresponding topic is /setpoint/local_pos
- Position setpoints relative to the drone horizontal body frame. This one is used for the tag tracking as the tag is detected relative to the camera. Corresponding topic is /setpoint/relative_pos
apriltag_setpoint_publisher.py: This node takes the pose of the detected tag and computes relative positino setpoints which are published to /setpoint/relative_pos topic.
kf_tracker_node: Performs Kalman filtering based on a constant velocity model for object tracking. The input to this node is a PoseStamped topic of the object pose measurements (only the 3D position is consumed). The output of the filter (kf/estimate topic) is an estimate of the 3D position of the tracked object in the same frame as the input measrements.

Tuning

The position controller may require tuning for the PI gains. There are two services to help you tune those gains in runtime, for both horizontal and verical controllers.

To tune the PI gains for the horizontal controller,

rosservice call /horizontal_controller/pid_gains "p: 1.0
i: 0.01
d: 0.0"

for the verical controller,

rosservice call /vertical_controller/pid_gains "p: 1.0
i: 0.01
d: 0.0"

Error signals are also published to the following topics. This helps you to plot the error response while tuning the controllers.

# Velocity errors help you to tune the velocity controller on PX4 side
/analysis/body_vel_err
/analysis/local_vel_err

# Position errors help you to tune the position controllers implemented in this package
/analysis/local_pos_err
/analysis/relative_pos_err

You can use rqt_plot or plotjuggler to plot signals in realtime.

Citation

If you use this package in an academic context, please cite it as follows

@misc{mohamed_abdelkader_2020_07,
  author       = {Mohamed Abdelkader},
  title        = {{mavros_apriltag_tracking - Vision-based target tracking for PX4-powered drones}},
  month        = july,
  year         = 2020,
  doi          = {10.5281/zenodo.3959150 },
  url          = {https://zenodo.org/badge/latestdoi/279731687}
}

mzahana / mavros_apriltag_tracking