====== Controlling turtlesim from Lisp ======

**Description:** In this tutorial you will learn how to control a turtle in turtlesim with CRAM Fluents

**Next Tutorial:** [[doc:beginner:simple_plans|Implementing simple plans to move a turtle]]

In this tutorial we will re-use the package ''cram_beginner_tutorial'' that you have created in the previous tutorial. For controlling the turtle, we need to depend on the ''turtlesim'' package, since it contains the message definitions we will use. Further, we will want to use the communication functionality of ROS to talk to the ''turtlesim'' from inside Lisp, so we depend on ''roslisp''. Finally, we will have to deal with poses. For that, we want to use the package ''cl_transforms'' and ''geometry_msgs'', the latter one is only needed if you're working with Hydro or a newer ROS version. 


===== Updating the dependencies =====

==== Updating ROS dependecies ====

==== catkin ====

Open ''package.xml'' from the root directory of your ''cram_beginner_tutorial'' package and add the lines
<code xml>
  <build_depend>roslisp</build_depend>
  <build_depend>turtlesim</build_depend>
  <build_depend>cl_transforms</build_depend>
  <build_depend>geometry_msgs</build_depend>
</code>
under ''<build_depend>cram_language</build_depend>'' and
<code xml>
  <run_depend>roslisp</run_depend>
  <run_depend>turtlesim</run_depend>
  <run_depend>cl_transforms</run_depend>
  <run_depend>geometry_msgs</run_depend>  
</code>
under ''<run_depend>cram_language</run_depend>''.

==== rosbuild ====
Open ''manifest.xml'' and add the lines
<code xml>
  <depend package="roslisp"/>
  <depend package="turtlesim"/>
  <depend package="cl_transforms"/>
  <depend package="geometry_msgs"/>
</code>
under ''<depend package="cram_language">''.

==== Updating cram dependecies ====

Now open ''cram-beginner-tutorial.asd'' and update the system dependencies to include the system  ''roslisp'', ''turtlesim-msg'', ''geometry_msgs-msg'' and ''cl-transforms''. The systems that correspond to the messages of a package are always named like the package name with a ''-msg'' suffix. Your system should now look like this:

<code lisp>
(defsystem cram-beginner-tutorial
  :depends-on (roslisp cram-language turtlesim-msg cl-transforms geometry_msgs-msg)
  :components
  ((:module "src"
            :components
            ((:file "package")
             (:file "tutorial" :depends-on ("package"))))))
</code>

===== Updating the Lisp Package =====

We also want to add ''roslisp'' and ''cl-transforms'' to our namespace in the file ''package.lisp'' so that we don't have to specify the namespace each time we use a function from that package in our code.
<code lisp>
(defpackage cram-beginner-tutorial
  (:nicknames :tut)
  (:use #:cpl #:roslisp #:cl-transforms))
</code>

===== Writing the communication glue code =====

Now it's time to use roslisp to connect to turtlesim. Turtlesim creates one ROS topic namespace per turtle. For each turtle name (turtle1, turtle2, etc.) it publishes the pose of the turtle on a ''~/pose'' topic, publishes the color value under the turtle (the background color) on a ''~/color_sensor'' topic. For each turtle turtlesim subscribes to a ''~/cmd_vel'' topic (or ''~/command_velocity'' before hydro) waiting for commands to move the turtle. This is the topic through which we will be controlling our turtles from Lisp.

==== The code ====

Open the file ''tutorial.lisp'' and add the following code.

=== hydro ===

<code lisp>
(in-package :tut)

(defvar *color-value* (make-fluent :name :color-value) "current color under the turtle")
(defvar *turtle-pose* (make-fluent :name :turtle-pose) "current pose of turtle")

(defvar *color-sub* nil "color ROS subscriber")
(defvar *pose-sub* nil "pose ROS subscriber")
(defvar *cmd-vel-pub* nil "velocity commands ROS publisher")

(defun init-ros-turtle (name)
  "subscribes to topics for a turtle and binds callbacks. `name' specifies the name of the turtle."
  (setf *color-sub* (subscribe (format nil "~a/color_sensor" name)
                               "turtlesim/Color"
                               #'color-cb))
  (setf *pose-sub* (subscribe (format nil "~a/pose" name)
                               "turtlesim/Pose"
                               #'pose-cb))
  (setf *cmd-vel-pub* (advertise (format nil "~a/cmd_vel" name)
                                 "geometry_msgs/Twist")))

(defun color-cb (msg)
  "Callback for color values. Called by the color topic subscriber."
  (setf (value *color-value*) msg))

(defun pose-cb (msg)
  "Callback for pose values. Called by the pose topic subscriber."
  (setf (value *turtle-pose*) msg))

(defun send-vel-cmd (lin ang)
  "Function to send velocity commands"
  (publish *cmd-vel-pub* (make-message "geometry_msgs/Twist"
                                       :linear (make-msg "geometry_msgs/Vector3"
                                                         :x lin)
                                       :angular (make-msg "geometry_msgs/Vector3"
                                                          :z ang))))
</code>

=== groovy and older ===

<code lisp>
(in-package :tut)

(defvar *color-value* (make-fluent :name :color-value) "current color of turtle")
(defvar *turtle-pose* (make-fluent :name :turtle-pose) "current pose of turtle")

(defvar *color-sub* nil "color subscription client")
(defvar *pose-sub* nil "pose subscription client")
(defvar *cmd-vel-pub* nil "command velocity subscription client")

(defun init-ros-turtle (name)
  "subscribes to topics for a turtle and binds callbacks. `name' specifies the name of the turtle."
  (setf *color-sub* (subscribe (format nil "~a/color_sensor" name)
                               "turtlesim/Color"
                               #'color-cb))
  (setf *pose-sub* (subscribe (format nil "~a/pose" name)
                               "turtlesim/Pose"
                               #'pose-cb))
  (setf *cmd-vel-pub* (advertise (format nil "~a/command_velocity" name)
                                 "turtlesim/Velocity")))

(defun color-cb (msg)
  "Callback for color values"
  (setf (value *color-value*) msg))

(defun pose-cb (msg)
  "Callback for pose values"
  (setf (value *turtle-pose*) msg))

(defun send-vel-cmd (lin ang)
  "function to send velocity commands"
  (publish *cmd-vel-pub* (make-message "turtlesim/Velocity"
                                       linear lin
                                       angular ang)))
</code>                                       
                                       
=== The code explained ===

In ''(defun init-ros-turtle ...)'' we subscribe to the pose and the color sensor topic and create a function that can publish to the ''cmd_vel'' (or ''command_velocity'' in old ROS) topic. The topic clients are stored in global variables we defined before.

We use callback functions ''*-cb'' for all updates on the pose of the turtle and the color sensor value to a fluent in order to be able to use those values in a reactive control program.

A fluent is a proxy around a value with support for notification on change. Further, fluents can be combined to so-called fluent networks. There will be some more explanations about fluents later in this tutorial.

===== Experimenting in the REPL =====

Now let's try it out. Open your Lisp REPL and make sure that you loaded the system ''cram-beginner-tutorial'' and switched to the package ''tut'', hint:

<code lisp>
CL-USER> (ros-load:load-system "cram_beginner_tutorial" :cram-beginner-tutorial)
...
CL-USER> (in-package :tut)
</code>

For experiments in the REPL, we want to initialize a Lisp ROS node and then call ''init-ros-turtle''.

Make sure ''roscore'' is running in a terminal. Else we cannot create ROS nodes.

Enter the following commands:

<code lisp>
TUT> (start-ros-node "cram_tutorial_client")
[(ROSLISP TOP) INFO] 1292688669.674: Node name is cram_tutorial_client
[(ROSLISP TOP) INFO] 1292688669.687: Namespace is /
[(ROSLISP TOP) INFO] 1292688669.688: Params are NIL
[(ROSLISP TOP) INFO] 1292688669.689: Remappings are:
[(ROSLISP TOP) INFO] 1292688669.691: master URI is 127.0.0.1:11311
[(ROSLISP TOP) INFO] 1292688670.875: Node startup complete
</code>

The name of the node is arbitrary. We need to call ''start-ros-node'' before we can use the functions subscribe and advertise.

<code lisp>
TUT> (init-ros-turtle "/turtle1")
</code>

This calls our function ''init-ros-turtle'', with the name that the turtlesim uses for the first turtle spawned.

Now we should start up turtlesim in a new terminal:

<code bash>
$ rosrun turtlesim turtlesim_node
</code>

(Note: if you're using turtlesim for the first time, you might need to compile the package first: ''$ rosmake turtlesim''.)

You will see the turtlesim window:

{{:doc:beginner:turtlesim.png?400|}}

Notice that thanks to ROS, we could subscribe to the topics even before they had been advertised by the turtlesim.

We are ready for experiments now. Let's see if the fluents are containing the right values:

<code lisp>
TUT>  (value *turtle-pose*)
[TURTLESIM-MSG:<POSE>
   X:
     5.55555534362793d0
   Y:
     5.55555534362793d0
   THETA:
     0.0d0
   LINEAR_VELOCITY:
     0.0d0
   ANGULAR_VELOCITY:
     0.0d0]
TUT>  (value *color-value*)
[TURTLESIM-MSG:<COLOR>
   R:
     179
   G:
     184
   B:
     255]
</code>

You see that the value of the fluents contain structured data, which contains the pose and color information as provided by the turtlesim simulator.

Let's see if the pose changes if we move the turtle. Start up ''turtle_teleop_key'' and move the turtle a little bit using the arrows on your keyboard:

<code bash>$ rosrun turtlesim turtle_teleop_key</code>

Try evaluating ''*turtle-pose*'' in the REPL again:
<code lisp>
TUT>  (value *turtle-pose*)
[TURTLESIM-MSG:<POSE>
   X:
     8.339859008789063d0
   Y:
     7.269973278045654d0
   THETA:
     0.9920001029968262d0
   LINEAR_VELOCITY:
     0.0d0
   ANGULAR_VELOCITY:
     0.0d0]
</code>
As the pose fluent has been updated, the subscribers seem to work.


==== Fluents ====

Now let's play around with fluents a little bit more.

Notice how in the checks before, we used the function ''(value *turtle-pose*)'', instead of accessing the variable directly. This is because fluents are proxies, and to get the values we need to use the value function.

Fluents allow us to wait for specific events. For instance, we can wait for the ''x'' coordinate of the turtle being smaller than ''5'':
<code lisp>
TUT> (wait-for (< (fl-funcall #'turtlesim-msg:x *turtle-pose*)
                  5.0))
</code>
The code above constructs a fluent network. The expression
<code lisp>
(fl-funcall #'turtlesim-msg:x *turtle-pose*)
</code>
returns a new fluent that contains the ''x''-value of the turtle pose. So from a proxy that wrapped the whole pose of a turtle, we got a new proxy that wraps just the ''x'' coordinate. The expression
<code lisp>
(< (fl-funcall #'turtlesim-msg:x *turtle-pose*)
   5.0)
</code>
returns a fluent that is either ''T'' or ''NIL'' indicating if the x-coordinate is either smaller or bigger than 5.0. Finally, ''wait-for'' waits for the fluent to become ''T''.

Execute the above expression. If the turtle's ''x'' position is already smaller than 5, the expression should return ''T'' immediately. If not, the expression should block until you move the turtle far enough to the left using the teleop.


=== Moving the turtle ===

Let's see if we can also move the turtle from LISP. Try the following:
<code lisp>
TUT> (dotimes (i 10) (send-vel-cmd 1 1) (sleep 1))
</code>
We use the ''send-vel-cmd'' function that we defined in ''tutorial.lisp''. We send the same command 10 times, once every second. The turtle should now move along a circle.

== Next ==


Now that we have functions and fluents to connect to the turtlesim, let's implement some simple plans.

[[doc:beginner:simple_plans|Implementing simple plans to move a turtle]]