ArUco marker detection

Aruco markers are squares with bit patterns which can easily be detected and their orientation can always be determined due to their non-symmetric pattern. These markers can be generated here.

The repository in the following link implements detection of aruco markers in opencv with a realsense camera, the authors are Pontus Rosqvist, Josefin Gustafsson, Marcus Nagy and Martin Lyrå. This script can take the following command line arguments:

The aruco marker ids and their size should be specified in a text file of the following form:

id: size, length
10:    7, 10
23:    7, 20
34:    5, 12.2

The first line is ignored so it should only contain the header “id: size, length” which tells you how the data is parsed. All lines after that are parsed where the number to the left of the colon is an aruco marker id which is associated with the length in millimeters which is the second number to the right of the colon. The first number to the right of the colon specifies the width of the aruco marker in pixels, the possible sizes are 4, 5, 6 and 7.

The code expects a realsense camera to be connected to the computer. If the pipeline does not receive a frame from the camera within 5 seconds the following error will be thrown

RuntimeError: Frame didn't arrive within 5000

In which case one should check that the camera works and sends frames to the computer with realsense-viewer.

It is very important to check if the camera is recognized as a USB2 device or USB3 device since USB2 supports a smaller sets of resolution than USB3. If a resolution that is not possible is requested (for example if the camera is detected as a USB2 device and the expected resolution is too big) the pipeline throws the following error:

Runtime error: Couldn't resolve requests

In which case one should check what resolution the frames from the camera have with realsense-viewer to verify that the expected resolution is correct.

First the aruco markers of some size are detected in the current frame with cv2.aruco.detectMarkers. For this function to work the python package opencv-contrib-python needs to be installed. From this we get the pixel locations of the corners in the image, since the size of each marker is known we can recover the 3d position of all of these corners solvePnP. This function returns the translation of the aruco marker and the angle-axis representation of the orientation.

In order to make use of the translation of the aruco markers and the angle-axis one could use openCV cv2.Rodrigues on the angle-axis to get the rotation matrix and sequentially use SciPy scipy.spatial.transform.Rotation on the rotation matrix to get a quaternion. With this it is easy to construct a rospy geometry_msgs/Pose.msg.

This script only draws the determined pose of each detected aruco marker in each frame and displays it but it can easily be modified to instead return the pose of each aruco marker and their associated id.

To detect aruco makers with a skill and implement a skill in SkiROS one could use the wiki to get a better understanding of how todo or use the example skill “camera_skill” as an example which uses the implmented aruco marker deteciton in the above section for picking up an object.

There are a couple of steps to create a skill in SkiROS.

1. Create a skill. Checkout the section right above part 2 as well all of part 2.
   1. Create a SkillDescription
      1. addParam needed for the skill
   2. Create a PrimitiveBase
      1. Handle the different situation like, [onPreempt, onInit, onStart, execute].
   3. Create a SkillBase [OPTIONAL] Required if wanted to create skills executed in order.
      1. See here.
2. Add skill to simulation in e.g. launch file for Heron.
3. Add skill code to into one of the skill repositories:
   1. General skill.
   2. Vision skill.
4. Add possible new ontologies to the ontology for Heron.