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
Key tools:
ar_track_alvar
Closed loop velocity control
Open loop velocity control
rqt_plot
to plot error signals
Challenge
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).
Checkpoints
Height Control
Demonstrate the drone automatically controlling its height.
Graph the commanded z velocity with
rqt_plot
and show a TA before you flyHover the drone low over the ground and show it rises/lowers to 0.75 m
Over/Under logic
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
State transitions
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:
Starts
obstacle_avoid.py
, your avoidance code. Don’t forget to make it executable withchmod +x
.Starts MAVROS
Starts the Intel RealSense Camera
Starts
ar_track_alvar
You will also need aero-optical-flow
and aero-teraranger.service
running outside of the launch file to be able to fly autonomously.
Code overview
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
Use
rospy.sleep(dur)
, NOTtime.sleep(dur)
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
time.sleep()
- userospy.sleep()
This plays better with ROS
Debugging
By now, you have gained experience with rqt_image_view
and 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_plot
Like rqt_image_view
, this is a graphical tool (and can be very slow). Accordingly, you must have run ssh
with the -X
or -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 angular
or linear
components of /mavros/setpoint_velocity/cmd_vel_unstamped
.
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 /mavros/setpoint_velocity/cmd_vel_unstamped/