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 fly

  • Hover 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 with chmod +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:

  1. 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.

  2. 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.

  3. 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

  4. After a certain length of time, return to the default height

    • Use rospy.sleep(dur), NOT time.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() - use rospy.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/