Package Delivery

Goal

The goal of this exercise is to implement the logic that allows a quadrotor to deliver a package box from the warehouse to a target house. The task of the drone is to:

  1. Pick the package box from the warehouse and reach the target house location
  2. Locate the target beacon and drop the package box
  3. Return to the warehouse
Package Delivery.
Gallery.

Instructions

Installing and Launching

  1. Download Docker. Windows users should choose WSL 2 backend Docker installation if possible, as it has better performance than Hyper-V.

  2. Pull the current distribution of RoboticsBackend:

      docker pull jderobot/robotics-backend:latest

  • In order to obtain optimal performance, Docker should be using multiple CPU cores. In case of Docker for Mac or Docker for Windows, the VM should be assigned a greater number of cores.

  • It is recommended to use the latest image. However, older distributions of RARoboticsBackendDI can be found here.

How to perform the exercises?

  • Start a new docker container of the image and keep it running in the background:

    docker run --rm -it -p 7164:7164 -p 2303:2303 -p 1905:1905 -p 8765:8765 -p 6080:6080 -p 1108:1108 -p 7163:7163 jderobot/robotics-backend

  • On the local machine navigate to 127.0.0.1:7164/ in the browser and choose the desired exercise.

  • Wait for the Connect button to turn green and display “Connected”. Click on the “Launch” button and wait for some time until an alert appears with the message Connection Established and button displays “Ready”.

  • The exercise can be used after the alert.

Enable GPU Acceleration

  • Follow the advanced launching instructions from here.

Optional: Store terminal output

  • To store the terminal output of manager.py and launch.py to a file execute the following docker run command and keep it running in the background: 
    docker run -it --rm -v $HOME/.roboticsacademy/log/:/root/.roboticsacademy/log/ --device /dev/dri -p 7164:7164 -p 2303:2303 -p 1905:1905 -p 8765:8765 -p 6080:6080 -p 1108:1108 -p 2304:2304 -p 1904:1904 jderobot/robotics-backend --logs
  • The log files will be stored inside $HOME/.roboticsacademy/{year-month-date-hours-mins}/. After the session, use more to view the logs, for example: 
    more $HOME/.roboticsacademy/log/2021-11-06-14-45/manager.log

Where to insert the code?

In the launched webpage, type your code in the text editor,

from GUI import GUI
from HAL import HAL
# Enter sequential code!

while True:
    # Enter iterative code!

Using the Interface

  • Control Buttons: The control buttons enable the control of the interface. Play button sends the code written by User to the Robot. Stop button stops the code that is currently running on the Robot. Save button saves the code on the local machine. Load button loads the code from the local machine. Reset button resets the simulation(primarily, the position of the robot).

  • Brain and GUI Frequency: This input shows the running frequency of the iterative part of the code (under the while True:). A smaller value implies the code runs less number of times. A higher value implies the code runs a large number of times. The numerator is the one set as the Measured Frequency who is the one measured by the computer (a frequency of execution the computer is able to maintain despite the commanded one) and the input (denominator) is the Target Frequency which is the desired frequency by the student. The student should adjust the Target Frequency according to the Measured Frequency.

  • RTF (Real Time Factor): The RTF defines how much real time passes with each step of simulation time. A RTF of 1 implies that simulation time is passing at the same speed as real time. The lower the value the slower the simulation will run, which will vary depending on the computer.

  • Pseudo Console: This shows the error messages related to the student’s code that is sent. In order to print certain debugging information on this console. The student can use the print() command in the Editor.

Robot API

  • from HAL import HAL - to import the HAL(Hardware Abstraction Layer) library class. This class contains the functions that sends and receives information to and from the Hardware(Gazebo).
  • from GUI import GUI - to import the GUI(Graphical User Interface) library class. This class contains the functions used to view the debugging information, like image widgets.

Sensors and drone state

  • HAL.get_position() - Returns the actual position of the drone as a numpy array [x, y, z], in m.
  • HAL.get_velocity() - Returns the actual velocities of the drone as a numpy array [vx, vy, vz], in m/s
  • HAL.get_yaw_rate() - Returns the actual yaw rate of the drone, in rad/s.
  • HAL.get_orientation() - Returns the actual roll, pitch and yaw of the drone as a numpy array [roll, pitch, yaw], in rad.
  • HAL.get_roll() - Returns the roll angle of the drone, in rad
  • HAL.get_pitch() - Returns the pitch angle of the drone, in rad.
  • HAL.get_yaw() - Returns the yaw angle of the drone, in rad.
  • HAL.get_landed_state() - Returns 1 if the drone is on the ground (landed), 2 if the drone is in the air and 4 if the drone is landing. 0 could be also returned if the drone landed state is unknown.
  • HAL.get_pkg_state() - Returns True if the drone has picked up the package box else returns False.

Actuators and drone control

The three following drone control functions are non-blocking, i.e. each time you send a new command to the aircraft it immediately discards the previous control command.

1. Position control

  • HAL.set_cmd_pos(x, y, z, az) - Commands the position (x,y,z) of the drone, in m and the yaw angle (az) (in rad) taking as reference the first takeoff point (map frame)

2. Velocity control

  • HAL.set_cmd_vel(vx, vy, vz, az) - Commands the linear velocity of the drone in the x, y and z directions (in m/s) and the yaw rate (az) (rad/s) in its body fixed frame

3. Mixed control

  • HAL.set_cmd_mix(vx, vy, z, az) - Commands the linear velocity of the drone in the x, y directions (in m/s), the height (z) related to the takeoff point and the yaw rate (az) (in rad/s)

Drone takeoff and land

Besides using the buttons at the drone teleoperator GUI, taking off and landing can also be controlled from the following commands in your code:

  • HAL.takeoff(height) - Takeoff at the current location, to the given height (in m)
  • HAL.land() - Land at the current location.

Drone cameras

  • HAL.get_frontal_image() - Returns the latest image from the frontal camera as a OpenCV cv2_image
  • HAL.get_ventral_image() - Returns the latest image from the ventral camera as a OpenCV cv2_image

Package box control

  • HAL.set_cmd_pick() - Drone picks up the package box from a minimum distance of 0.5 m
  • HAL.set_cmd_drop() - Drone drops the package box

GUI

  • GUI.showImage(cv2_image) - Shows a image of the camera in the GUI
  • GUI.showLeftImage(cv2_image) - Shows another image of the camera in the GUI

Hints

Simple hints provided to help you solve the package_delivery exercise. Please note that the full solution has not been provided.

Position Detection of the Target

Notice that the target house has a visual signal near it. You can filter this signal in order to get the position of the target location.

How do I get the waypoint coordinates?

  1. Green Pad: Located at (x, y) = (-1, -4) m
  2. Red Pad: Located at (x, y) = (-1, -1) m
  3. Target House: Located at (x, y) = (20, 20) m

Directional control. How should drone yaw be handled?

If you don’t take care of the drone yaw angle or yaw_rate in your code (keeping them always equal to zero), you will fly in what’s generally called Heads Free Mode. The drone will always face towards its initial orientation, and it will fly sideways or even backwards when commanded towards a target destination. Multi-rotors can easily do that, but what’s not the best way of flying a drone.

Another possibility is to use Nose Forward Mode, where the drone follows the path similar to a fixed-wing aircraft. Then, to accomplish it, you’ll have to implement by yourself some kind of directional control, to rotate the nose of your drone left or right using yaw angle, or yaw_rate.

In this exercise, you should use the Nose Forward Mode.

Do I need to know when the drone is in the air?

No, you can solve this exercise without taking care of the land state of the drone. However, it could be a great enhancement to your blocking position control function if you make it only work when the drone is actually flying, not on the ground.

Videos

Contributors