AR Obstacle Avoidance¶
In this document, you will learn how to write obstacle avoidance code for your Intel Aero RTF drone, utilizing tools we have already seen in the course of the program. You can find our presentation on this here: https://docs.google.com/presentation/d/1TayWiAUiq3rBzKxJigS7_iZ-nUdeq2IVuoayHSu3zeA/edit?usp=sharing
Closed loop velocity control
Open loop velocity control
rqt_plotto plot error signals
Start >1 m away from an AR tag and fly to the default height of 0.75 m
Fly forward until the AR tag is <= 1 m away from the drone
Based on AR tag height, fly over/under the AR tag ‘obstacle’ and return to the default height on the other side
Resume going forward (but pilot-in-command needs to take over before the drone hits the net).
Demonstrate the drone automatically controlling its height.
Graph the commanded z velocity with
rqt_plotand show a TA before you fly
Hover the drone low over the ground and show it rises/lowers to 0.75 m
Show the drone an AR tag and based on the AR tag’s height the drone will decide whether to go over or under the AR tag
Make sure you are holding the drone over the ground so it knows its height off the ground
This checkpoint doesn’t require you to fly
Do after completing Over/Under logic checkpoint. Demonstrate that the drone changes its height setpoint after detecting the AR tag to avoid the obstacle and, after a period of time, reverts to the default height.
This does not require you to actually fly the drone, so you can complete this before you finish Height Control
Launching Obstacle Avoidance¶
You will be using a launch file to execute obstacle avoidance code called
obstacle_avoid.launch. This launch file will be provided to you in
aero_control upstream for you to use with your obstacle avoidance code.
obstacle_avoid.launch does a few things:
obstacle_avoid.py, your avoidance code. Don’t forget to make it executable with
Starts the Intel RealSense Camera
You will also need
aero-teraranger.service running outside of the launch file to be able to fly autonomously.
We recommend you start by copying code from your open loop controller, as we are not providing skeleton code. Here is an overview of what your controller needs to do:
Cruise forward until you are close enough to an AR tag, using P control to control your height
Send horizontal velocities and a height setpoint to the streaming thread
In the streaming thread, calculate vertical velocities using current height and height setpoint using a proportional controller.
Calculate the AR tag’s height using the drone’s pose and the pose of the AR tag relative to the drone. Look at the AR tag localization lab for ideas.
Decide based on the height of the drone whether to go up or down.
Change the height setpoint
Have a check in the streaming thread that prevents the drone from moving forward if it isn’t close enough to its target altitude yet - this prevents the drone from plowing forward into an obstacle while changing altitude
After a certain length of time, return to the default height
Reset the height setpoint to the default height
Side topic: Finite-state Machine¶
To do obstacle avoidance, we are creating a simple finite-state machine. This means that there are a limited number of tasks, or “states” that the drone could be occupying, such as
normal flight or
going under. At each time step, you will need to update this state with new information from AR tag detections, pose information, and time from when you last saw an AR tag.
Mistakes to avoid¶
Do not forget to read the directions
Do not avoid the obstacle by hard-coding to go up, over, and across
Do not hard-code any constants (ex. threshold height, PID constants)
Check how far away the AR tag you saw was
Don’t begin your obstacle avoidance routine until the obstacle is close enough
Alvar occasionally sees tags with a position of (nan nan nan).
Comparing nan to anything returns false!
Do not use
This plays better with ROS
By now, you have gained experience with
rostopic echo. THere is one last tool you need to know:
rqt_plot. This tool will allow us to graph numerical ros topics, such as velocity commands, which can be essential for tuning PID loops.
Accessing parts of ROS topics¶
Whenever you want to view the commanded velocity, you run
rostopic echo /mavros/setpoint_velocity/cmd_vel_unstamped. This prints out the whole ROS message. However, you may want to print only parts of the message, such as the angular component of the message. Do this by extending the name of the ROS topic: ex.
rostopic echo /mavros/setpoint_velocity/cmd_vel_unstamped/angular
rqt_image_view, this is a graphical tool (and can be very slow). Accordingly, you must have run
ssh with the
-Y flags to be able to use it. You can either add the correct ROS topics in the GUI (not recommended) or add them via the command line: ex.
rqt_plot /topic_name. Note that this only plots vectors and scalars: if you try to do
rqt_plot /mavros/setpoint_velocity/cmd_vel_unstamped it will plot nothing. You need to tell
rqt_plot to plot only a vector or
a scalar component of the message (such as the
linear components of
You can pass multiple topics too: simply add them one after another:
rqt_plot /mavros/setpoint_velocity/cmd_vel_unstamped /some_fake_topic
Be careful: topics with
/ at the end are NOT the same as topics without the
/mavros/setpoint_velocity/cmd_vel_unstamped is not the same as