This is an old revision of the document!


Learning about the transport function with an example

The transport function is a high order function which will use most of the functions in cram_fetch_deliver_plans. Therefore we'll explain this function with an example and show the below function calls. Our goal with the PR2 is to put the bowl from the sink area on the table. To explain the other functions we're gonna follow the execution of the other high level functions in the deliver function. We do this by loading the packages cram_pr2_pick_and_place and cram_bullet_world_tutorial

(ros-load:load-system "cram_pr2_pick_and_place" :cram_pr2_pick_and_place)
(ros-load:load-system "cram_bullet_world_tutorial" :cram_bullet_world_tutorial)

… and call the following functions: </code> (in-package :cram-bullet-world-tutorial) (roslisp-utilities:startup-ros) (urdf-proj:with-projected-robot

(cram-pr2-pick-place-demo::demo-random nil '(:bowl)))

</code> After that the robot gets correctly positioned and kitchen objects spawned before starting.

Since we passed (demo-random) the object of type bowl the actions searching and transporting will be executed:

(when (eq ?object-type :bowl)
(cpl:with-failure-handling
((common-fail:high-level-failure (e)
(roslisp:ros-warn (pp-plans demo) "Failure happened: ~a~%Skipping the search" e)
(return)))
(let ((?loc (cdr (assoc :breakfast-cereal object-fetching-locations))))
(exe:perform
(desig:an action
(type searching)
(object (desig:an object (type breakfast-cereal)))
(location ?loc))))))
 
(cpl:with-failure-handling
((common-fail:high-level-failure (e)
(roslisp:ros-warn (pp-plans demo) "Failure happened: ~a~%Skipping..." e)
(return)))
(if (eq ?object-type :bowl)
(exe:perform
(desig:an action
(type transporting)
(object ?object-to-fetch)
;; (arm right)
(location ?fetching-location)
(target ?delivering-location)))

If any of the actions fails, we just skip them. Since in the code the searching actions is executed first, let's take a look at it.

search-for-object

(defun search-for-object (&key
((:object ?object-designator))
((:location ?search-location))
((:robot-location ?robot-location))
(retries 3)
&allow-other-keys)
(declare (type desig:object-designator ?object-designator)
;; location desigs can turn NILL in the course of execution
;; but should not be passed as NILL to start with.
(type (or desig:location-designator null) ?search-location ?robot-location))
"Searches for `?object-designator' in its likely location `?search-location'.
If the object is not there or navigation location is unreachable,
retries with different search location or robot base location."

The input parameters are the object which should be searched from the location ?robot-location at the location ?search-location. retries says how often we should try again to find a object at a specific robot location.

(cpl:with-failure-handling
((desig:designator-error (e)
(roslisp:ros-warn (fd-plans search-for-object)
"Desig ~a could not be resolved: ~a~%Propagating up."
?search-location e)
(cpl:fail 'common-fail:object-nowhere-to-be-found
:description "Search location designator could not be resolved.")))

If the referencing of below designators fail the error object-nowhere-to-be-found will be thrown.

The basic idea is the following: First the robot tries to navigate to the location ?robot-location. If this is succeeded within a number of retries the robot tries now to turn towards the location ?search-location and detect the object within in the number of specific retries too. Now if the robot cannot detect the object or cannot navigate to the ?robot-location, we jump above the navigation part, set a new ?search-location and try all above again. This is done maximal outer-search-location-retries times.

;; take new `?search-location' sample if a failure happens and retry
(cpl:with-retry-counters ((outer-search-location-retries 2))
(cpl:with-failure-handling
((common-fail:object-nowhere-to-be-found (e)
(common-fail:retry-with-loc-designator-solutions
?search-location
outer-search-location-retries
(:error-object-or-string e
:warning-namespace (fd-plans search-for-object)
:reset-designators (list ?robot-location)
:rethrow-failure 'common-fail:object-nowhere-to-be-found)
(roslisp:ros-warn (fd-plans search-for-object)
"Search is about to give up. Retrying~%"))))

So here we set a new ?search-location since we could not move to robot or/and find detect the object.

;; if the going action fails, pick another `?robot-location' sample and retry
(cpl:with-retry-counters ((robot-location-retries 10))
(cpl:with-failure-handling
(((or common-fail:navigation-goal-in-collision
common-fail:looking-high-level-failure
common-fail:perception-low-level-failure) (e)
(common-fail:retry-with-loc-designator-solutions
?robot-location
robot-location-retries
(:error-object-or-string e
:warning-namespace (fd-plans search-for-object)
:reset-designators (list ?search-location)
:rethrow-failure 'common-fail:object-nowhere-to-be-found))))
 
;; navigate
(exe:perform (desig:an action
(type navigating)
(location ?robot-location)))

Here we try to navigate the robot to ?robot-location and if it fails we try robot-location-retries times again before throwing object-nowhere-to-be-found and jump in the above code snippet. To understand how this action will be resolved we take a look at an example action:

#<A ACTION
	(TYPE NAVIGATING)
	(LOCATION #<A LOCATION
		(VISIBLE-FOR PR2)
		(LOCATION #<A LOCATION
			(ON #<A OBJECT
				(TYPE COUNTER-TOP)
				(URDF-NAME SINK-AREA-SURFACE)
				(OWL-NAME kitchen_sink_block_counter_top)
				(PART-OF KITCHEN)>)
			(SIDE LEFT)
			(SIDE FRONT)
			(RANGE 0.5)>)>)>

To resolve the nested location designator ?robot-location in this action the function cram_manipulation_interfaces::get-location-poses will be called twice with the following location designators:

#<A LOCATION
(VISIBLE-FOR PR2)
(LOCATION #<A LOCATION
	(ON #<A OBJECT
		(TYPE COUNTER-TOP)
		(URDF-NAME SINK-AREA-SURFACE)
		(OWL-NAME "kitchen_sink_block_counter_top")
		(PART-OF KITCHEN)>)
	(SIDE LEFT)
	(SIDE FRONT)
	(RANGE 0.5)>)> 

This designator is a subset of the action designator above and …

#<A LOCATION
(ON #<A OBJECT
	(TYPE COUNTER-TOP)
	(URDF-NAME SINK-AREA-SURFACE)
	(OWL-NAME "kitchen_sink_block_counter_top")
	(PART-OF KITCHEN)>)
(SIDE LEFT)
(SIDE FRONT)
(RANGE 0.5)>

this location designator is the subset of the above location designator. Since we have two location designators to resolve the function cram_manipulation_interfaces::get-location-poses will be called twice returning both times a lazy list of poses:

(#<CL-TRANSFORMS:POSE 
#<3D-VECTOR (1.5d0 0.9000000953674316d0 0.8500000258286794d0)>
#<QUATERNION (0.0d0 0.0d0 0.9084313017742992d0 0.41803417319239833d0)>>
. #S(CRAM-UTILITIES::LAZY-CONS-ELEM
:GENERATOR #<CLOSURE (LAMBDA () :IN CRAM-UTILITIES:LAZY-MAPCAN)
{1018B51EBB}>)) 

Since the type of the action is navigating the function cram_fetch_deliver_plans::go-without-collisions can be evaluated with the above pose. The procedure can be visualized with the btr too:

This is the costmap after we called get-location-poses the first time only with the object designator.

This is the costmap after we called the nested location designator with get-location-poses.

This is after the execution of the navigation action designator.

;; if perception action fails, try another `?search-location' and retry
(cpl:with-retry-counters ((search-location-retries retries))
(cpl:with-failure-handling
(((or common-fail:perception-low-level-failure
common-fail:looking-high-level-failure) (e)
(common-fail:retry-with-loc-designator-solutions
?search-location
search-location-retries
(:error-object-or-string e
:warning-namespace (fd-plans search-for-object)
:reset-designators (list ?robot-location)))))
 
(exe:perform (desig:an action
(type turning-towards)
(target ?search-location)))
(exe:perform (desig:an action
(type detecting)
(object ?object-designator)))))))))))
ablauf:

Here we try to turn towards the location to search for and try to detect the object. If on of these action fails, we try like the parameter retries says times again. If it stills fails we throw the error object-nowhere-to-be-found and try therefore then first to navigate the robot before detecting again the object. The detection action calls the perceive function in cram_mobile_pick_place_plans/src/atomic-action-plans to detect the object and returns a motion designator. The function turn-towards will create a looking action designator which calls the look-at function in the above file too. At the end the function search-for-object returns an object designator with a pose like this e.g.:

#<A OBJECT
(LOCATION #<A LOCATION
(ON #<A OBJECT
(TYPE COUNTER-TOP)
(URDF-NAME SINK-AREA-SURFACE)
(OWL-NAME "kitchen_sink_block_counter_top")
(PART-OF KITCHEN)>)
(SIDE LEFT)
(SIDE FRONT)
(RANGE-INVERT 0.5)>)
(TYPE BOWL)
(NAME BOWL-1)
(POSE ((:POSE
#<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
FRAME-ID: "base_footprint", STAMP: 1.560865249078827d9
#<3D-VECTOR (0.8618418535601576d0 0.028947115308607385d0 0.8886420529683431d0)>
#<QUATERNION (-0.004970445237876797d0 -0.0035147137040045353d0 0.10407392205337866d0 0.994550963155759d0)>>)
(:TRANSFORM
#<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED 
FRAME-ID: "base_footprint", CHILD-FRAME-ID: "bowl_1", STAMP: 1.560865249078827d9
#<3D-VECTOR (0.8618418535601576d0 0.028947115308607385d0 0.8886420529683431d0)>
#<QUATERNION (-0.004970445237876797d0 -0.0035147137040045353d0 0.10407392205337866d0 0.994550963155759d0)>>)
(:POSE-IN-MAP
#<CL-TRANSFORMS-STAMPED:POSE-STAMPED 
FRAME-ID: "map", STAMP: 1.560865249078827d9
#<3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>
#<QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>)
(:TRANSFORM-IN-MAP
#<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED 
FRAME-ID: "map", CHILD-FRAME-ID: "bowl_1", STAMP: 1.560865249078827d9
#<3D-VECTOR (1.3993186950683594d0 0.8007776260375976d0 0.8886420567830403d0)>
#<QUATERNION (-0.005307710729539394d0 -0.0029810641426593065d0 5.163228488527238d-4 0.9999813437461853d0)>>)))>