Description: in this tutorial you will learn about CRAM process modules and write a simple one to move the turtlesim.
Previous Tutorial: Creating action designators for the turtlesim
Next Tutorial: Using location designators with the turtlesim
A process module is a program that controls a robot actuator. Different robots will have different kinds of actuators, requiring different kinds of controllers, and we would like to abstract away such specifics at the high level of task specification. When we ask a robot to clean a kitchen, we don't care that it has two arms or one; rather, as long as it has some capacity to manipulate objects, and some ability to move around, we can issue the cleaning task to it. Process modules allow us this flexibility by providing a well-defined, robot-independent interface to high level planning which works as an abstraction layer over the robot controllers. You can read some more about process modules in the documentation of the package.
For better organization, process modules can also be referenced by aliases that describe what kind of function they can perform. For example, currently in the cram_pr2 stack for the PR2 mobile manipulation platform the following aliases are defined:
A plan will typically refer to a process module by the alias. The process module would be defined for the particular robot in use and associated with the proper alias when the system is initialized.
Here's an example of invoking a process module:
(with-designators ((my-designator :location '((:close-to :fridge)))) (pm-execute :navigation my-designator))
This will run a process module associated with :navigation and use my-designator as an input parameter. In this case, the designator is a semantic description of a target location and it's up to the process module to ask that this designator be resolved (by some appropriate other module in the system) so as to get an actual location and then control the robot to reach the target. The specifics of the controller may vary enormously; the robot might be differential drive, or legged. But these details are not important for this high level plan. All we want is for the robot to approach the fridge, in whatever way it can carry itself there.
For this tutorial we will use the ActionLib turtlesim server for having a turtle draw a shape.
Once again, some new dependencies must be declared in the tutorial files you've been working on.
In your package.xml file you need to add build and runtime dependencies on actionlib_lisp, actionlib_msgs, turtle_actionlib and cram_process_modules:
<build_depend>actionlib_lisp</build_depend> <build_depend>actionlib_msgs</build_depend> <build_depend>turtle_actionlib</build_depend> <build_depend>cram_process_modules</build_depend> <run_depend>actionlib_lisp</run_depend> <run_depend>actionlib_msgs</run_depend> <run_depend>turtle_actionlib</run_depend> <run_depend>cram_process_modules</run_depend>
Similarly, in your .asd file you should add actionlib, actionlib_msgs-msg, turtle_actionlib-msg, cram-process-modules and cram-language-designator-support to the :depends-on list. Let's also create two new source files in the src directory of your tutorial, call them turtle-action-client.lisp and process-modules.lisp, and add them to your *.asd file, which should now look like this:
(defsystem cram-beginner-tutorial :depends-on (cram-language roslisp turtlesim-msg geometry_msgs-msg cl-transforms cram-designators cram-prolog actionlib actionlib_msgs-msg turtle_actionlib-msg cram-process-modules cram-language-designator-support) :components ((:module "src" :components ((:file "package") (:file "control-turtlesim" :depends-on ("package")) (:file "simple-plans" :depends-on ("package" "control-turtlesim")) (:file "action-designators" :depends-on ("package")) (:file "turtle-action-client" :depends-on ("package")) (:file "process-modules" :depends-on ("package" "control-turtlesim" "simple-plans" "action-designators" "turtle-action-client"))))))
Finally, let's also add :cram-process-modules and cram-language-designator-support to the use list of our Lisp package for convenience:
(:use :cpl :roslisp :cl-transforms :cram-designators :cram-process-modules :cram-language-designator-support)
We first need to connect to the turtlesim action server with an ActionLib client. To do this, append the following to your turtle-action-client.lisp:
(in-package :tut) (defvar *navp-client* nil) (defun init-action-client () (setf *navp-client* (actionlib:make-action-client "turtle_shape" "turtle_actionlib/ShapeAction")) (roslisp:ros-info (turtle-shape-action-client) "Waiting for turtle shape action server...") ;; workaround for race condition in actionlib wait-for server (loop until (actionlib:wait-for-server *navp-client*)) (roslisp:ros-info (turtle-shape-action-client) "Turtle shape action client created.")) (defun get-action-client () (when (null *navp-client*) (init-action-client)) *navp-client*)
The above code simply declares a variable *navp-client* which we can then initialize with a pointer to our ROS action client and retrieve this pointer later with the init-action-client and get-action-client functions, respectively.
We will need an action goal for our action client that should represent a shape (which is specified by a number of edges and a radius), so append this to the turtle-action-client.lisp:
(defun make-shape-action-goal (in-edges in-radius) (actionlib:make-action-goal (get-action-client) edges in-edges radius in-radius)) (defun call-shape-action (&key edges radius) (multiple-value-bind (result status) (with-failure-handling ((simple-error (e) (format t "An error occured!~%~a~%Reinitializing...~%~%" e) (setf *navp-client* nil) (retry))) (let ((actionlib:*action-server-timeout* 10.0)) (actionlib:call-goal (get-action-client) (make-shape-action-goal edges radius)))) (roslisp:ros-info (turtle-shape-action-client) "Nav action finished.") (values result status)))
In the above code we define a simple function to convert an edge and radius pair of values into a goal for the action client, and a function that will call said action client, with some error handling built in (for example, if the function is called without an action client being defined, it will (re)initialize one and retry).
Now that the lower level of controlling the TurtleSim is taken care of, it's finally time to look at process modules and their interface to the higher levels. Append the following to your process-modules.lisp file:
(in-package :tut) (def-process-module actionlib-navigation (action-designator) (roslisp:ros-info (turtle-process-modules) "Turtle shape navigation invoked with action designator `~a'." action-designator) (destructuring-bind (command action-goal) (reference action-designator) (ecase command (draw-shape (call-shape-action :edges (turtle-shape-edges action-goal) :radius (turtle-shape-radius action-goal)))))) (defmacro with-turtle-process-modules (&body body) `(with-process-modules-running (actionlib-navigation) ,@body))
First, we use the cram-process-modules:def-process-module macro to define actionlib-navigation as a process module taking one parameter (action-designator). The process module then chooses which action to perform depending on the command specified in the designator: destructuring-bind maps the results from (reference action-designator) to the variables command and action-goal respectively. Note that the inference rules we defined previously provide a name for the kind of action goal we have (currently, all are draw-shape), and a turtle-shape object. We run an ecase on the kind of goal (currently, we only have the shape case) and use call-shape-action to tell the lower level to move the turtle around, given these parameters we infer from designator resolution.
The with-turtle-process-modules macro is a macro we define for convenience. It allows us to set up a context in which to run commands, knowing that the turtle process modules are all running concurrently. Right now we only have one defined, actionlib-navigation. When we will have several, we will add them to the list we pass to cram-process-modules:with-process-modules-running. Note that the with-process-modules-running macro (and therefore with-turtle-process-modules too) needs to be run inside a top-level form. We will see this below.
Let's try this out. Make sure you have roscore, turtlesim, and turtle_actionlib running. In a terminal tab for each,
$ roscore $ rosrun turtlesim turtlesim_node $ rosrun turtle_actionlib shape_server
For convenience, let's append one more function to process-modules.lisp that will do all our calls for us:
(defun draw-hexagon (radius) (let ((turtle-name "turtle1")) (start-ros-node turtle-name) (init-ros-turtle turtle-name) (top-level (with-turtle-process-modules (process-module-alias :navigation 'actionlib-navigation) (with-designators ((trajectory :action `((:type :shape) (:shape :hexagon) (:radius ,radius)))) (pm-execute :navigation trajectory))))))
What the function does is simply start a ROS node for our turtle and sets up the action client, then activates the turtle process modules (in our case, the sole existing one), creates a designator to describe how the turtle should move, and calls the cram-process-modules:pm-execute macro to have the process module follow the trajectory specified by the designator.
Two other things to observe here are the use of the top-level macro, which sets up a CRAM running context and is needed for with-turtle-process-modules. Also, we declare that :navigation is the alias for our actionlib-navigation process module. That way, our plan can invoke a generic name like :navigation, rather than a robot-specific one like actionlib-navigation.
Reload the tutorial in REPL, and let's try to start the tutorial
TUT> (draw-hexagon 2) [(ROSLISP TOP) INFO] 1453721727.445: Node name is /turtle1 [(ROSLISP TOP) INFO] 1453721727.446: Namespace is / [(ROSLISP TOP) INFO] 1453721727.446: Params are NIL [(ROSLISP TOP) INFO] 1453721727.446: Remappings are: [(ROSLISP TOP) INFO] 1453721727.446: master URI is 127.0.0.1:11311 [(ROSLISP TOP) INFO] 1453721728.452: Node startup complete [(TURTLE-PROCESS-MODULES) INFO] 1453721728.467: Turtle shape navigation invoked with action designator `#<ACTION-DESIGNATOR ((TYPE SHAPE) (SHAPE HEXAGON) (RADIUS 2)) {10056DB513}>'. An error occured! Client lost connection to server. Reinitializing... [(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1453721736.385: Waiting for turtle shape action server... [(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1453721738.386: Turtle shape action client created. [(TURTLE-SHAPE-ACTION-CLIENT) INFO] 1453721761.206: Nav action finished. [TURTLE_ACTIONLIB-MSG:SHAPERESULT INTERIOR_ANGLE: 2.094395160675049d0 APOTHEM: 1.7320507764816284d0] :SUCCEEDED
You should also see the turtle move in the TurtleSim window and trace the required trajectory.
Let's have a look at location designators and other ways to move the turtle, as well as have some more practice with designator resolution and process modules …