Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
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 ===== | ===== Setup ===== | ||
+ | *set up the environment in our terminal by calling the launch file: | ||
- | * launch the map-server in a terminal with < | + | |
- | | + | |
+ | |||
+ | *start a node in REPL with (roslisp-utilities: | ||
- | * start a node in REPL with (roslisp-utilities: | ||
* define costmap parameters: | * define costmap parameters: | ||
Line 27: | Line 29: | ||
<code lisp> | <code lisp> | ||
(prolog: | (prolog: | ||
- | (prolog:< | + | (prolog:< |
- | (prolog:< | + | (prolog:< |
- | (prolog:< | + | (prolog:< |
- | (prolog:< | + | (prolog:< |
</ | </ | ||
Line 101: | Line 103: | ||
(if (and (< (cl-transforms: | (if (and (< (cl-transforms: | ||
(> (abs (/ (cl-transforms: | (> (abs (/ (cl-transforms: | ||
- | (abs (/ (cl-transforms: | + | 1 |
- | 0.0d0))))) | + | |
- | </ | + | </ |
+ | 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: | (car (prolog: | ||
</ | </ | ||
+ | The code can also be used to visualize the other costmap functions in this tutorial. | ||
- | {{: | + | {{: |
===== 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: | ||
</ | </ | ||
+ | The visualized costmap looks like this: | ||
+ | |||
+ | {{: | ||
* using range costmap function: | * using range costmap function: | ||
Line 175: | Line 181: | ||
| | ||
? | ? | ||
+ | </ | ||
+ | |||
+ | The visualized costmap looks like this: | ||
+ | |||
+ | {{: | ||
+ | * using range costmap function inverted: | ||
+ | <code lisp> | ||
(prolog: | (prolog: | ||
(prolog:< | (prolog:< | ||
Line 185: | Line 198: | ||
</ | </ | ||
+ | The visualized costmap looks like this: | ||
+ | |||
+ | {{: | ||
+ | ===== Using your own Z coordinate function ===== | ||
+ | So far the costmaps that you have generated/ | ||
+ | |||
+ | ==== 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: | ||
+ | (prolog:< | ||
+ | (desig: | ||
+ | (location-costmap: | ||
+ | (location-costmap: | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | (costmap: | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | Notice that the only thing that we added is this particular line: | ||
+ | <code lisp> | ||
+ | (costmap: | ||
+ | (costmap: | ||
+ | ?costmap) | ||
+ | </ | ||
+ | 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: | ||
+ | |||
+ | {{: | ||
+ | |||
+ | ==== Configuring height according to conditions ==== | ||
+ | In a real scenario, not all the z coordinate would be constant, due to terrain differences, | ||
+ | <code lisp> | ||
+ | (defun positive-y-height-generator () | ||
+ | (lambda (x y) | ||
+ | (if (> y 0.0) | ||
+ | '(1.0) ' | ||
+ | </ | ||
+ | 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: | ||
+ | (prolog:< | ||
+ | (desig: | ||
+ | (location-costmap: | ||
+ | (location-costmap: | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | (costmap: | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | Note that we only replaced the constant height generator with our own function. | ||
+ | |||
+ | ===== Using your own Orientation Generator ===== | ||
+ | As mentioned in the introduction, | ||
+ | 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 | ||
+ | | ||
+ | | ||
+ | (cl-transforms: | ||
+ | angle)) | ||
+ | `(0.0 ,(/ pi 2) ,pi ,(- (/ pi 2)))))) | ||
+ | </ | ||
+ | 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: | ||
+ | (prolog:< | ||
+ | (desig: | ||
+ | (location-costmap: | ||
+ | (location-costmap: | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | (costmap: | ||
+ | | ||
+ | ? | ||
+ | (costmap: | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | |||
+ | The visualized costmap using the bullet world looks like this: | ||
+ | |||
+ | {{: |