Differences

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

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
tutorials:intermediate:costmaps [2019/06/13 11:01] – Added section about height and orientation generators amartutorials:intermediate:costmaps [2022/04/27 14:02] (current) – [Creating your own cost function] schimpf
Line 16: Line 16:
  
 ===== Setup ===== ===== 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:   * define costmap parameters:
Line 27: Line 29:
 <code lisp> <code lisp>
 (prolog:def-fact-group costmap-metadata () (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> </code>
  
Line 101: Line 103:
                  (if (and (< (cl-transforms:x point) 0.0d0)                  (if (and (< (cl-transforms:x point) 0.0d0)
                           (> (abs (/ (cl-transforms:x point) vector-length)) 0))                           (> (abs (/ (cl-transforms:x point) vector-length)) 0))
-                     (abs (/ (cl-transforms:x point) vector-length)) +                     1 
-                     0.0d0))))) +                     0))))) 
-</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:   * 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))))))   (car (prolog:prolog `(and (location-costmap:desig-costmap ,*behind-designator* ?cm))))))
 </code> </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 ===== ===== 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: In the location-costmap package there are other cost function already available for generating costmaps. Here are two examples:
Line 164: Line 167:
 </code> </code>
  
 +The visualized costmap looks like this:
 +
 +{{:tutorials:intermediate:location-costmap-gaussian-costmap.png?400|}}
   * using range costmap function:   * using range costmap function:
  
Line 175: Line 181:
      (location-costmap:make-range-cost-function ?pose 1.0)      (location-costmap:make-range-cost-function ?pose 1.0)
      ?costmap)))      ?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:def-fact-group tutorial-rules (location-costmap:desig-costmap)
   (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)   (prolog:<- (location-costmap:desig-costmap ?designator ?costmap)
Line 185: Line 198:
 </code> </code>
  
 +The visualized costmap looks like this:
 +
 +{{:tutorials:intermediate:location-costmap-tutorial-range-costmap-invert.png?400|}}
 ===== Using your own Z coordinate function ===== ===== 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: 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:
Line 210: Line 226:
 </code> </code>
 The end result is that all the poses generated will now have a z-coordinate of 1.0 units. 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 ==== ==== Configuring height according to conditions ====
Line 269: Line 289:
      ?costmap)))      ?costmap)))
 </code> </code>
 +
 +The visualized costmap using the bullet world looks like this:
 + 
 +{{:tutorials:intermediate:location-costmap-tutorial-orientation-generator.png?400|}}