Differences

This shows you the differences between two versions of the page.

--- tutorials:intermediate:costmaps [2019/04/02 12:03] – amar
+++ tutorials:intermediate:costmaps [2022/04/27 14:02] (current) – [Creating your own cost function] schimpf
@@ Line 16: / Line 16: @@
 ===== Setup =====
+    *set up the environment in our terminal by calling the launch file:
-  * launch the map-server in a terminal with <code> roslaunch iai_maps map.launch </code>
+        roslaunch cram_bullet_world_tutorial world.launch
-  * load cram_occupancy_grid_costmap in the REPL
+    *load cram_bullet_world_tutorial in the REPL
+    *start a node in REPL with (roslisp-utilities:startup-ros)
-  * start a node in REPL with (roslisp-utilities:startup-ros)
   * define costmap parameters:
@@ Line 27: / Line 29: @@
 <code lisp>
 (prolog:def-fact-group costmap-metadata ()
-  (prolog:<- (location-costmap:costmap-size 10 10)) ; in meters
+  (prolog:<- (:costmap-size 10 10)) ; in meters
-  (prolog:<- (location-costmap:costmap-origin -5 -5))
+  (prolog:<- (:costmap-origin -5 -5))
-  (prolog:<- (location-costmap:costmap-resolution 0.05))
+  (prolog:<- (:costmap-resolution 0.05))
-  (prolog:<- (location-costmap:costmap-padding 0.01))) ; padding to occupancy map obstacles
+  (prolog:<- (:costmap-padding 0.01))) ; padding to occupancy map obstacles
 </code>
@@ Line 101: / Line 103: @@
                  (if (and (< (cl-transforms:x point) 0.0d0)
                           (> (abs (/ (cl-transforms:x point) vector-length)) 0))
-                     (abs (/ (cl-transforms:x point) vector-length))
-.0d0)))))
+)))))
-</code> To summarize this, we can see that the individual value in the costmap generator is set to the normalized x value of the sampling point if it lies behind the reference point (when x is negative compared to the reference). Else it's set to 0.
+</code>
+To summerize, we can see that the individual value in the costmap generator is set to 1 if it is behind the reference point and else it's set to 0.
   * define order for your costmap function and give it a name:
@@ Line 143: / Line 146: @@
   (car (prolog:prolog `(and (location-costmap:desig-costmap ,*behind-designator* ?cm))))))
 </code>
+The code can also be used to visualize the other costmap functions in this tutorial.
-{{:tutorials:intermediate:location-costmap-tutorial-behind.png?nolink&300|}}
+{{:tutorials:intermediate:location-costmap-tutorial-behind-2.png?400|}}
 ===== Using other (pre-defined) cost-functions =====
 In the location-costmap package there are other cost function already available for generating costmaps. Here are two examples:
@@ Line 164: / Line 167: @@
 </code>
+The visualized costmap looks like this:
+{{:tutorials:intermediate:location-costmap-gaussian-costmap.png?400|}}
   * using range costmap function:
@@ Line 175: / Line 181: @@
      (location-costmap:make-range-cost-function ?pose 1.0)
      ?costmap)))
+</code>
+The visualized costmap looks like this:
+{{:tutorials:intermediate:location-costmap-tutorial-range-costmap.png?400|}}
+  * using range costmap function inverted:
+<code lisp>
 (prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
   (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
@@ Line 185: / Line 198: @@
 </code>
+The visualized costmap looks like this:
+{{:tutorials:intermediate:location-costmap-tutorial-range-costmap-invert.png?400|}}
+===== Using your own Z coordinate function =====
+So far the costmaps that you have generated/used is set on the 2D XY plane and the Z is taken to be 0 by default. Height generators can also be defined along with the cost functions which determines the z coordinate of the sampled pose from the costmap. There are a couple of ways you can define a height generator for your costmap:
+==== Consistent Height ====
+With this, you can fix the z coordinate to one constant value for all the poses generated. CRAM provides you the method to do this easily. Let's take the example we used above and modify it a little bit.
+<code lisp>
+(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
+  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
+    (desig:desig-prop ?designator (:behind ?pose))
+    (location-costmap:costmap ?costmap)
+    (location-costmap:costmap-add-function
+     behind-cost-function
+     (location-costmap:make-range-cost-function ?pose 1.0)
+     ?costmap)
+    (costmap:costmap-add-cached-height-generator
+     (costmap:make-constant-height-function 1.0)
+     ?costmap)))
+</code>
+Notice that the only thing that we added is this particular line:
+<code lisp>
+(costmap:costmap-add-height-generator
+  (costmap:make-constant-height-function 1.0)
+  ?costmap)
+</code>
+The end result is that all the poses generated will now have a z-coordinate of 1.0 units.
+The visualized costmap using the bullet world looks like this:
+{{:tutorials:intermediate:location-costmap-tutorial-consistent-height.png?400|}}
+==== Configuring height according to conditions ====
+In a real scenario, not all the z coordinate would be constant, due to terrain differences, obstacles, etc. CRAM allows you to model this similar to how you created your own custom costmap. For this, let's define our own height-generator, as shown below:
+<code lisp>
+(defun positive-y-height-generator ()
+  (lambda (x y)
+    (if (> y 0.0)
+        '(1.0) '(0.0))))
+</code>
+As before, the lambda function will get both x and y as inputs but this time we are expected to return a list of possible heights and a random value out of the satisfied condition list is taken.
+In this example, if y is greater than 0, then the height generator returns 1, else 0 otherwise.
+To use it in our designator, we redefine our fact group again:
+<code lisp>
+(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
+  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
+    (desig:desig-prop ?designator (:behind ?pose))
+    (location-costmap:costmap ?costmap)
+    (location-costmap:costmap-add-function
+     behind-cost-function
+     (location-costmap:make-range-cost-function ?pose 1.0)
+     ?costmap)
+    (costmap:costmap-add-height-generator
+     (positive-y-height-generator)
+     ?costmap)))
+</code>
+Note that we only replaced the constant height generator with our own function.
+===== Using your own Orientation Generator =====
+As mentioned in the introduction, the resolution of the designator can also involve resolving an orientation in addition to resolving height and the pose from the costmap. In this section, we will see how we can employ orientation generators to give various poses when resolving a location costmap. This works similar to how we defined and used the height generator, with some minor changes. By default, if no custom orientation generators are used, the system will use it's own identity orientation generator where all the poses will have identity rotation aligned to the reference axis.
+Let's jump into defining and using our own version of an orientation generator:
+<code lisp>
+(defun make-4-orientations-generator ()
+  (lambda (x y previous-orientations)
+    (declare (ignore x y previous-orientations))
+    (mapcar
+     (lambda (angle)
+       (cl-transforms:axis-angle->quaternion
+        (cl-transforms:make-3d-vector 0 0 1)
+        angle))
+     `(0.0 ,(/ pi 2) ,pi ,(- (/ pi 2))))))
+</code>
+Note: Here we will receive a parameter called previous-orientations in addition to x and y, and this parameter can be used to incrementally modify the orientation that are generated. In this particular example, we are not using these parameters, but just creating 4 possible orientations (0, pi/2, pi and -pi/2) regardless of the previous orientation or the current position. The user can make use of these parameters according to their own use-case.
+And finally adding it to the fact-group
+<code lisp>
+(prolog:def-fact-group tutorial-rules (location-costmap:desig-costmap)
+  (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
+    (desig:desig-prop ?designator (:behind ?pose))
+    (location-costmap:costmap ?costmap)
+    (location-costmap:costmap-add-function
+     behind-cost-function
+     (location-costmap:make-range-cost-function ?pose 1.0)
+     ?costmap)
+    (costmap:costmap-add-height-generator
+     (positive-y-height-generator)
+     ?costmap)
+    (costmap:costmap-add-orientation-generator
+     (make-4-orientations-generator)
+     ?costmap)))
+</code>
+The visualized costmap using the bullet world looks like this:
+{{:tutorials:intermediate:location-costmap-tutorial-orientation-generator.png?400|}}