moesenle / cram_highlevel Goto Github PK

When including the semantic-map-costmap package, the generated location costmaps result in no free space for the robot to drive to. Even though the semantic map is working (semantically annotated objects are placed correctly, resolution of positions on those items works like a charm), the predicate

  (<- (desig-costmap ?desig ?cm)
    (or (desig-prop ?desig (to see))
        (desig-prop ?desig (to reach)))
    (costmap ?cm)
    (semantic-map-objects ?objects)
    (costmap-padding ?padding)
    (costmap-add-function semantic-map-free-space
                          (make-semantic-map-costmap
                           ?objects :invert t :padding ?padding)
                          ?cm)
    ;; Locations to see and to reach are on the floor, so we can use a                                                                                                                                          
    ;; constant height of 0                                                                                                                                                                                     
    (costmap-add-cached-height-generator
     (make-constant-height-function 0.0)
     ?cm))

seems to be buggy. The offending call seems to be make-semantic-map-costmap as it does not mark free space when called. Commenting out the predicates removes the feature of free space determination, resulting in normal behavior for resolution of locations on objects.

The at-location macro is prone to get stuck when the target location and the current robot location are ambiguous, even though the robot is at the correct location. A reason for this can be invalid input from a loaded semantic map. The macro then tried to reach the 'correct' location by specifying a new goal, driving away from the real target and then falsifying the result again.

This results is a loop, repeatedly calling PERCEIVE-STATE (LOC ROBOT ...). With a very small chance, the loop concludes in the correct solution and returns. Though, this happens rarely for me and only between 500-1000 iterations of the loop.

An error message, checking for 10 or so of such oscillations (which should be more than enough for one at-location run) should be implemented, stating such a faulty situation.