Builds kinbodies from MarkerArray
This page describes the process for using apriltags to detect objects.
What you will need:
- A physical object.
- A corresponding kinbody.
- An apriltag sticker or card.
We use the .kinbody.xml format to represent objects in OpenRAVE. Apriltags detects rigid bodies, so the kinbodies created by apriltags typically consist of one link and no joints.
The following .kinbody.xml file creates a (1 m x 2 m x 3 m) box centered at the origin:
<KinBody name="Body1"> <!-- Set "name" to the default name of the KinBody. -->
<Body name="base_link" type="dynamic">
<Geom type="box">
<AmbientColor>0.5 0.5 0.5 1.0</AmbientColor> <!-- RGBA color -->
<DiffuseColor>0.5 0.5 0.5 1.0</DiffuseColor> <!-- RGBA color -->
<Extents>0.5 1.0 1.5</Extents> <!-- x, y, and z half-extents, centered at the origin -->
<Translation>0.0 0.0 1.5</Translation> <!-- translation of the origin -->
</Geom>
</Body>
</KinBody>We follow some conventions when creating kinbodies:
- lengths are in meters
- x is forward, y is left, z is up
- the origin is located at the bottom-center of the object, so the object appears to be resting on the center of the xy-plane when placed at the origin
-
Choose an unused tag ID in the "36h11" family. You can find a list of used tags in the
data/objects/tag_data.jsonfile in thepr-ordatarepository. -
Find the corresponding
tag36_11_00xxx.pngfile in thedata/tags/textures/tag36h11directory in this repository. -
Use ImageMagick to convert the
.pngfile to a.pdfthat is suitable for printing:
$ convert tag36_11_00XXX.png -density 300 -interpolate Nearest -filter point -resize 708x708 tag36_11_00XXX.pdfThe output is a 4.8 cm tag surrounded by 0.6 cm of padding (6 cm square total).
- Print this tag at 100% scale.
- Run
tag_kinbodyscript on your.kinbody.xmlfile and specify the tag index that you chose above:
$ rosrun kinbody_detector tag_kinbody.py --body PATH/TO/MY.kinbody.xml --index XXX --offsetpath PATH/TO/tag_data.jsonThis script assumes that you are using the default tag size (4.8 cm on HERB) by default. If your tag has a different size, use the --width option to specify the size.
On HERB, you should set --offsetpath to $(rospack find pr_ordata)/data/objects/tag_data.json. For example, to tag the glass used in the table_clearing demo, you would run:
$ rosrun kinbody_detector tag_kinbody.py -b pr_ordata/data/objects/glass.kinbody.xml -i 124 -o pr_ordata/data/objects/tag_data.jsonThis will launch an or_rviz window with two objects present: (1) the kinbody specified by --body and (2) the apriltag specified by --index. It also opens an IPython terminal that you can use to interact with the environment.
If you can't see the objects, make sure InteractiveMarker or OpenRAVEMarkers is being displayed in rviz.
-
If you are updating an existing model, run
read()in the IPython terminal to load the pose of the tag from the JSON database. -
Right click on the apriltag and choose
Body > Pose Controls. This will create translation and rotation handles that allow you to move the pose of the apriltag in the environment.
Note that the tag may start out under the object. You may need to move the camera around to find it. If the object is much larger than the tag, the view will start zoomed out and the tag may be very small, so zoom in to find it.
-
Use the handles to position the apriltag correctly relative to the model. Be careful to match the orientation of the tag correctly - it's easy to accidentally rotate the tag by 90 degrees.
-
When you are done, type
write()in theIPythonterminal to save your changes to the JSON database file. -
Commit the changes to the JSON database file.
- Now that you have your offset file created, you convert apriltag detections to kinbodies in OpenRAVE. In Python, you can now run:
import kinbody_detector.kinbody_detector as kd
detector = kd.KinBodyDetector(env, '/PATH/TO/tag_data.json', '/PATH/TO/KINBODY/FOLDER', '/apriltags/marker_array')This creates a detector object that you can use to update the environment with detected kinbodies.
- To update the environment with the most recent apriltag detections run:
detector.Update()- This system does not update live, so you must continue to run
detector.Update()whenever you want to get a new snapshot of the physical world.