Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision
Previous revision
tutorials:beginner:controlling_turtlesim_2 [2016/01/25 17:11] gkazhoyatutorials:beginner:controlling_turtlesim_2 [2022/02/21 10:08] (current) – [Fluents] schimpf
Line 10: Line 10:
 ==== ROS dependencies ==== ==== ROS dependencies ====
  
-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. Add these dependencies to the ''package.xml'' as described in [[http://wiki.ros.org/catkin/Tutorials/CreatingPackage|Package creation]]. You can find the resulting code in the corresponding [[https://github.com/cram-code/cram_tutorials/tree/master/cram_beginner_tutorial|Github repo]].+In this tutorial we will re-use the package ''cram_my_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''. Add these dependencies to the ''package.xml'' as described in [[http://wiki.ros.org/catkin/Tutorials/CreatingPackage|Package creation]]. You can find the resulting code in the corresponding [[https://github.com/cram2/cram/tree/master/cram_tutorials/cram_beginner_tutorial|Github repo]].
  
 ==== ASDF dependencies ==== ==== ASDF dependencies ====
  
-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:+Now open ''cram-my-beginner-tutorial.asd'' and update the system dependencies to include the system  ''roslisp'', ''turtlesim-msg'', ''turtlesim-srv'', ''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> <code lisp>
-(defsystem cram-beginner-tutorial +(defsystem cram-my-beginner-tutorial 
-  :depends-on (roslisp cram-language turtlesim-msg cl-transforms geometry_msgs-msg)+  :depends-on (roslisp cram-language turtlesim-msg turtlesim-srv cl-transforms geometry_msgs-msg)
   :components   :components
   ((:module "src"   ((:module "src"
Line 30: Line 30:
 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. 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> <code lisp>
-(defpackage :cram-beginner-tutorial+(defpackage :cram-my-beginner-tutorial
   (:nicknames :tut)   (:nicknames :tut)
   (:use :cpl :roslisp :cl-transforms))   (:use :cpl :roslisp :cl-transforms))
Line 37: Line 37:
 ===== Writing the communication glue 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.+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 waiting for commands to move the turtle. This is the topic through which we will be controlling our turtles from Lisp.
  
 ==== The code ==== ==== The code ====
  
 Open the file ''control-turtlesim.lisp'' and add the following code. Open the file ''control-turtlesim.lisp'' and add the following code.
- 
-=== hydro === 
  
 <code lisp> <code lisp>
Line 54: Line 52:
 (defvar *pose-sub* nil "pose ROS subscriber") (defvar *pose-sub* nil "pose ROS subscriber")
 (defvar *cmd-vel-pub* nil "velocity commands ROS publisher") (defvar *cmd-vel-pub* nil "velocity commands ROS publisher")
 +
 +(defvar *pen-srv* nil "name of ROS service for controlling the pen")
 +
  
 (defun init-ros-turtle (name) (defun init-ros-turtle (name)
Line 65: Line 66:
                               #'pose-cb))                               #'pose-cb))
   (setf *cmd-vel-pub* (advertise (format nil "~a/cmd_vel" name)   (setf *cmd-vel-pub* (advertise (format nil "~a/cmd_vel" name)
-                                 "geometry_msgs/Twist")))+                                 "geometry_msgs/Twist")) 
 +  (setf *pen-srv* (concatenate 'string "/" name "/set_pen")))
  
 (defun color-cb (msg) (defun color-cb (msg)
Line 84: Line 86:
                          :linear (make-msg "geometry_msgs/Vector3" :x lin)                          :linear (make-msg "geometry_msgs/Vector3" :x lin)
                          :angular (make-msg "geometry_msgs/Vector3" :z ang))))                          :angular (make-msg "geometry_msgs/Vector3" :z ang))))
-</code> +                          
- +(defun call-set-pen (r g b width off
-=== groovy and older === +  "Function to call the SetPen service." 
- +  (call-service *pen-srv* 'turtlesim-srv:SetPen 
-<code lisp> +                :r r 
-(in-package :tut) +                :g g 
- +                :b b 
-(defvar *color-value* (make-fluent :name :color-value) "current color of turtle"+                :width width 
-(defvar *turtle-pose* (make-fluent :name :turtle-pose) "current pose of turtle"+                :off off)) 
- +</code>                           
-(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 === === 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.+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'' topic. The topic clients are stored in global variables we defined before. A string containing the name of the SetPen service of the turtle is also stored in a global variable.
  
 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. 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.
Line 134: Line 107:
 ===== Experimenting in the REPL ===== ===== 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:+Now let's try it out. Open your Lisp REPL and make sure that you loaded the system ''cram-my-beginner-tutorial'' and switched to the package ''tut'', hint:
  
 <code lisp> <code lisp>
-CL-USER> (ros-load:load-system "cram_beginner_tutorial" :cram-beginner-tutorial)+CL-USER> (ros-load:load-system "cram_my_beginner_tutorial" :cram-my-beginner-tutorial)
 ... ...
 CL-USER> (in-package :tut) CL-USER> (in-package :tut)
Line 161: Line 134:
  
 <code lisp> <code lisp>
-TUT> (init-ros-turtle "/turtle1")+TUT> (init-ros-turtle "turtle1")
 </code> </code>
  
Line 258: Line 231:
 Let's see if we can also move the turtle from Lisp. Try the following: Let's see if we can also move the turtle from Lisp. Try the following:
 <code lisp> <code lisp>
-TUT> (dotimes (i 10) (send-vel-cmd 1 1) (sleep 1))+TUT> (dotimes (i 10) (send-vel-cmd 1 1) (wait-duration 1))
 </code> </code>
 We use the ''send-vel-cmd'' function that we defined in ''control-turtlesim.lisp''. We send the same command 10 times, once every second. The turtle should now move along a circle. We use the ''send-vel-cmd'' function that we defined in ''control-turtlesim.lisp''. We send the same command 10 times, once every second. The turtle should now move along a circle.
  
-== Next == 
  
 +=== Setting the pen ===
 +
 +We also can change how the turtle writes on the background. Try:
 +<code lisp>
 +TUT> (call-set-pen 255 0 0 10 0)
 +</code>
 +
 +We use the ''call-set-pen'' function, also defined in ''control-turtlesim.lisp''. The line the turtle leaves on the ground should be red. Try moving the turtle, either with the teleop or the ''send-vel-cmd'' function.
 +
 +
 +== Next ==
  
 Now that we have functions and fluents to connect to the turtlesim, let's implement some simple plans. Now that we have functions and fluents to connect to the turtlesim, let's implement some simple plans.
  
 [[tutorials:beginner:simple_plans|Implementing simple plans to move a turtle]] [[tutorials:beginner:simple_plans|Implementing simple plans to move a turtle]]