Table of Contents
Semantic environment map in CRAM
Description: This tutorial demonstrates how to use the semantic environment map published by Knowrob within CRAM. Such map has semantically annotated objects of robot's environment, mostly the furniture in indoor scenarios or landmarks outdoors. This tutorial shows how to load the map, how to query for specific parts thereof and how to build and resolve location designators using the semantic map.
Prerequisites: To be able to follow this tutorial you need the following packages in your ROS workspace, compiled without errors:
- KnowRob (the core): https://github.com/knowrob/knowrob.git, this contains the
rosprolog
andjson_prolog
packages, which allow for sending Prolog queries to a knowledge base, where the semantic map is stored, over ROS in JSON format (make sure you haverosjava
installed, either from source or the ROS Debian package) - A sample semantic environment map in OWL format: https://github.com/code-iai/iai_maps.git, this contains the map of the kitchen lab of Institute for AI at Uni Bremen
- Core packages of CRAM: https://github.com/cram2/{roslisp_common,cram_3rdparty,cram_core,cram_plans}.git, where
{}
syntax corresponds to similar syntax in a Linux shell - Lisp implementation of JSON Prolog protocol: https://github.com/cram2/cram_json_prolog.git
- Semantic environment map functionality for CRAM: https://github.com/cram2/cram_semantic_maps.git
If you installed CRAM using the official installation instructions you should have everything already. Otherwise, clone the listed repos, install the missing system dependencies with rosdep install
and catkin_make
your workspace.
Setting up
Step 1: start publishing the semantic environment map through JSON Prolog and TF etc.
In a fresh terminal:
$ roslaunch iai_maps iai_maps.launch
Step 2: visualization setup
In a separate terminal:
$ rosrun rviz rviz
The recommended RViz setup is shown below:
Pay attention especially at the tf_prefix
of the RobotModel
.
Depending on the version of your map you might also have an additional table there but it really doesn't matter.
Step 3: Initializing the Lisp / CRAM environment
Fire up your REPL! (If you don't know how, check the Get ready for development part of the installation guide, you will need the roslisp_repl
ROS package installed ($ sudo apt-get install ros-DISTRO-roslisp-repl
).
We will be working with the cram_semantic_map_costmap
package.
First, let's load the ASDF system:
CL-USER> , r-l-s RET cram_semantic_map_costmap RET RET
You should get the “Compilation finished. (No warnings)” message in the Emacs minibuffer.
Now let's change the namespace:
CL-USER> , !p RET sem-map-costmap RET SEMANTIC-MAP-COSTMAP>
To use semantic maps from KnowRob in CRAM we first need to set our *fixed-frame*
, configure the location costmap and start a new ROS node:
SEMANTIC-MAP-COSTMAP> (setf cram-tf:*fixed-frame* "map") SEMANTIC-MAP-COSTMAP> (prolog:def-fact-group costmap-metadata () (<- (location-costmap:costmap-size 12 12)) (<- (location-costmap:costmap-origin -6 -6)) (<- (location-costmap:costmap-resolution 0.05)) (<- (location-costmap:costmap-padding 0.2)) (<- (location-costmap:costmap-manipulation-padding 0.2)) (<- (location-costmap:costmap-in-reach-distance 0.9)) (<- (location-costmap:costmap-reach-minimal-distance 0.1))) SEMANTIC-MAP-COSTMAP> (roslisp-utilities:startup-ros)
Semantic map objects
Let's ask KnowRob for the semantic map (it might take a while):
SEMANTIC-MAP-COSTMAP> (cram-semantic-map:get-semantic-map) #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP {1005F1CDD3}>
If you right-click on the result object and click Inspect
, you can see how semantic maps are stored in CRAM.
There is a hash table stored in the parts
slot, which maps name strings to semantic-map-geom
-s, i.e. a hash table of the constituent parts of the map.
Here are a bunch of semantic-map-geom
-s from the example semantic map:
"drawer_fridge_lower" = #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {1005EB9363}> "kitchen_island" = #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {1005495AD3}> "kitchen_island_counter_top" = #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {100548C003}> "kitchen_sink_block" = #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {1004C71763}> "kitchen_sink_block_counter_top" = #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {1004C4F603}>
If you inspect a semantic-map-geom
you can see the slots of that object, e.g. the “kitchen_island”
looks like this:
Most of the classes and utility functions are defined in the cram_semantic_map_utils
package, mostly in the semantic-map.lisp
file: to get a list of all the parts use semantic-map-parts
, for a specific part use semantic-map-part
, you can get the pose and dimensions of the part using pose
and dimensions
functions, if your semantic map contains joints there are functions to access them as well, etc., see the examples below:
SEMANTIC-MAP-COSTMAP> (cram-semantic-map-utils:semantic-map-parts (get-semantic-map)) SEMANTIC-MAP-COSTMAP> (cram-semantic-map-utils:semantic-map-part (get-semantic-map) "kitchen_island") #<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {1005495AD3}> SEMANTIC-MAP-COSTMAP> (cram-semantic-map-utils:pose *) #<CL-TRANSFORMS:POSE #<3D-VECTOR (-1.0528899431228638d0 1.6562440395355225d0 0.42500001192092896d0)> #<QUATERNION (0.0d0 0.0d0 1.0d0 0.0d0)>> SEMANTIC-MAP-COSTMAP> (cram-semantic-map-utils:dimensions **) #<CL-TRANSFORMS:3D-VECTOR (0.800000011920929d0 2.450000047683716d0 0.8500000238418579d0)>
Designators
Now let's create a symbolic object description to correspond to a part of the semantic map.
We cannot resolve object designators using the reference
function currently, as the object designator resolving mechanism is a big flimsy right now - only perception can resolve object designators - so we will use an explicit function designator→semantic-map-objects
to do that. Note, that you can have a location designator with the same property and it will work just as well:
SEMANTIC-MAP-COSTMAP> (cram-semantic-map-designators:designator->semantic-map-objects (cram-designators:make-designator :object '((:name "kitchen_sink_block")))) (#<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {100FB08C23}>) SEMANTIC-MAP-COSTMAP> (cram-semantic-map-designators:designator->semantic-map-objects (cram-designators:make-designator :location '((:name "kitchen_island")))) (#<CRAM-SEMANTIC-MAP-UTILS:SEMANTIC-MAP-GEOM {100FE7E4A3}>)
We can also ask for a location of an semantic map part:
SEMANTIC-MAP-COSTMAP> (make-designator :location `((:of ,(make-designator :object '((:name "kitchen_island")))))) #<LOCATION-DESIGNATOR ((:OF #<OBJECT-DESIGNATOR ((:NAME "kitchen_island")) {100E276F63}>)) {100E37ABA3}> SEMANTIC-MAP-COSTMAP> (reference *) #<CL-TRANSFORMS-STAMPED:POSE-STAMPED FRAME-ID: "map", STAMP: 0.0 #<3D-VECTOR (-1.0528899431228638d0 1.6562440395355225d0 0.42500001192092896d0)> #<QUATERNION (0.0d0 0.0d0 1.0d0 0.0d0)>>
A location description can be ambiguous, e.g., if we would like to have a location of an object of a certain semantic map part type:
SEMANTIC-MAP-COSTMAP> (let* ((object-of-type-cupboard (make-designator :object '((:type "Cupboard")))) (location-of-cupboard (make-designator :location `((:of ,object-of-type-cupboard))))) (format t "one solution: ~a~%" (reference location-of-cupboard)) (format t "another solution: ~a~%" (reference (next-solution location-of-cupboard)))) one solution: #<POSE-STAMPED FRAME-ID: "map", STAMP: 0.0 #<3D-VECTOR (-1.0528899431228638d0 -0.44999998807907104d0 0.36000001430511475d0)> #<QUATERNION (0.0d0 0.0d0 1.0d0 0.0d0)>> another solution: #<POSE-STAMPED FRAME-ID: "map", STAMP: 0.0 #<3D-VECTOR (1.5159399509429932d0 0.30313000082969666d0 0.42500001192092896d0)> #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
There are multiple objects of type cupboard, so we get multiple solutions.
In the example semantic map :type
can be “Cupboard”
, “Drawer”
, “Refrigerator”
, etc., see the semantic map object for more detail.
Now, let's use the location costmap mechanism to ground locations on or in objects:
SEMANTIC-MAP-COSTMAP> (reference (make-designator :location '((:in "Refrigerator")))) #<CL-TRANSFORMS-STAMPED:POSE-STAMPED FRAME-ID: "map", STAMP: 0.0 #<3D-VECTOR (1.3000001907348633d0 -0.9499998092651367d0 0.53999999538064d0)> #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
The big red dot is the location sampled for this designator.
If we want a location on a specific object, not on all objects of a certain type, we can specify that using the :name
key:
SEMANTIC-MAP-COSTMAP> (reference (make-designator :location '((:on "Cupboard") (:name "kitchen_island")))) #<CL-TRANSFORMS-STAMPED:POSE-STAMPED FRAME-ID: "map", STAMP: 0.0 #<3D-VECTOR (-0.6999998092651367d0 1.0500001907348633d0 0.8500000238418579d0)> #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>>
The marker array in RViz show a uniform distribution of all possible locations that satisfy the symbolic constraint in the designator.