Map-based navigation with ArUco markers

Marker detection requires the camera module to be correctly plugged in and configured.

We recommend using our custom PX4 firmware.

aruco_map module detects whole ArUco-based maps. Map-based navigation is possible using vision position estimate (VPE).

Configuration

Set the aruco argument in ~/catkin_ws/src/clever/clever/launch/aruco.launch to true:

<arg name="aruco" default="true"/>

In order to enable map detection set aruco_map and aruco_detect arguments to true in ~/catkin_ws/src/clever/clever/launch/aruco.launch:

<arg name="aruco_detect" default="true"/>
<arg name="aruco_map" default="true"/>

Set aruco_vpe to true to publish detected camera position to the flight controller as VPE data:

<arg name="aruco_vpe" default="true"/>

Marker map definition

Map is defined in a text file; each line has the following format:

marker_id marker_size x y z z_angle y_angle x_angle

N_angle is the angle of rotation along the N axis in radians.

Map path is defined in the map parameter:

<param name="map" value="$(find aruco_pose)/map/map.txt"/>

Some map examples are provided in ~/catkin_ws/src/clever/aruco_pose/map.

Grid maps may be generated using the genmap.py script:

rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt

length is the size of each marker, x is the marker count along the x axis, y is the marker count along the y axis, dist_x is the distance between the centers of adjacent markers along the x axis, dist_y is the distance between the centers of the y axis, first is the ID of the first marker (bottom left marker, unless --top-left is specified), test_map.txt is the name of the generated map file. The optional --top-left parameter changes the numbering of markers, making the top left marker the first one.

Usage example:

rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt

Checking the map

The currently active map is posted in the /aruco_map/image ROS topic. It can be viewed using web_video_server by opening the following link: http://192.168.11.1:8080/snapshot?topic=/aruco_map/image

Current estimated pose (relative to the detected map) is published in the aruco_map/pose ROS topic. If the VPE is disabled, the aruco_map TF frame is created; otherwise, the aruco_map_detected frame is created instead. Visualization for the current map is also posted in the aruco_map/visualization ROS topic; it may be visualized in rviz.

An easy to understand detected map visualization is posted in the aruco_map/debug ROS topic (live view is available on http://192.168.11.1:8080/stream_viewer?topic=/aruco_map/debug):

Coordinate system

The marker map adheres to the ROS coordinate system convention, using the ENU coordinate system:

  • the x axis points to the right side of the map;
  • the y axis points to the top side of the map;
  • the z axis points outwards from the plane of the marker

VPE setup

In order to enable vision position estimation you should use the following PX4 parameters.

If you're using EKF2 estimator (SYS_MC_EST_GROUP parameter is set to ekf2), make sure the following is set:

  • EKF2_AID_MASK should have vision position fusion and vision yaw fusion flags set.
  • Vision angle observations noise: EKF2_EVA_NOISE = 0.1 rad.
  • Vision position observations noise: EKF2_EVP_NOISE = 0.1 m.
  • EKF2_EV_DELAY = 0.

If you're using LPE (SYS_MC_EST_GROUP parameter is set to local_position_estimator,attitude_estimator_q):

  • LPE_FUSION should have vision position and land detector flags set. We suggest unsetting the baro flag for indoor flights.
  • External heading (yaw) weight: ATT_W_EXT_HDG = 0.5.
  • External heading (yaw) mode: ATT_EXT_HDG_M = 1 (Vision).
  • Vision position standard deviations: LPE_VIS_XY = 0.1 m, LPE_VIS_Z = 0.1 m.
  • LPE_VIS_DELAY = 0 sec.

We recommend using LPE for marker-based navigation.

You may use the selfcheck.py utility to check your settings.

In order to use LPE with the Pixhawk v1 hardware you should download the px4fmu-v2_lpe.px4 firmware

Flight

If the setup is done correctly, the drone will hold its position in POSCTL and OFFBOARD flight modes automatically.

You will also be able to use navigate, set_position and set_velocity ROS services for autonomous flights. In order to fly to a specific coordinate within the ArUco map you should use the aruco_map frame:

# Takeoff should be performed in the "body" frame; "aruco_map" frame will appear as soon as the drone detects the marker field
navigate(x=0, y=0, z=2, frame_id='body', speed=0.5, auto_arm=True) # Takeoff and hover 2 metres above the ground

time.sleep(5)

# Fly to the (2, 2) point on the marker field while being 2 metres above it
navigate(x=2, y=2, z=2, speed=1, frame_id='aruco_map')

Using a specific marker frame

Starting with the image version 0.18, the drone also can fly relative to a marker in the map, even if it is not currently visible. Like with single-marker navigation, this works by setting the frame_id parameter to aruco_ID, where ID is the desired marker number.

The folloding code will move the drone to the point 1 meter above the center of marker 5:

navigate(frame_id='aruco_5', x=0, y=0, z=1)

Additional settings

If the drone's position is not stable when VPE is used, try increasing the P term in the velocity PID regulator: increase the MPC_XY_VEL_P and MPC_Z_VEL_P parameters.

If the drone's altitude is not stable, try increasing the MPC_Z_VEL_P parameter and adjusting hover thrust via MPC_THR_HOVER.

Placing markers on the ceiling

Ceiling markers

In order to navigate using markers on the ceiling, mount the onboard camera so that it points up and adjust the camera frame accordingly.

You should also set the known_tilt parameter to map_flipped in both aruco_detect and aruco_map sections of ~/catkin_ws/src/clever/clever/launch/aruco.launch:

<param name="known_tilt" value="map_flipped"/>

This will flip the aruco_map frame (making its z axis point downward). Thus, in order to fly 2 metres below ceiling, the z argument for the navigate service should be set to 2:

navigate(x=1, y=1.1, z=2, speed=0.5, frame_id='aruco_map')

results matching ""

    No results matching ""