This is a slightly modified version of the original two tutorials on organizing roslisp files and basic roslisp usage that has an easier to understand structure for ROS and Lisp beginners and omits some narrow problem-specific details.
Part 1: roslisp Basics
- Setting up a new roslisp ROS package
- Example for publishing and subscribing
- Example for server and client
- Interactive use
- Working with ROS messages
- Accessing the parameter server
- Publishing and subscribing to non-standard topics
- Part 2: Controlling the TurtleSim with roslisp
Part 1: roslisp Basics
Setting up a new roslisp ROS package
The following gives a detailed example to follow for Lisp beginners.
Create a ROS package as usual including a dependency to roslisp. For catkin it will be the src of your catkin workspace. For that call:
$ roscd && cd ../src && catkin_create_pkg tutorial_ros_package roslisp std_msgs
It is a good practice to compile your package after creating it and updating the rospack profile:
$ cd .. && catkin_make && rospack profile
We include the dependency to std_msgs to be able to access the message types defined in that ROS package. We include roslisp to be able to use the ROS API in our code.
Inside the new directory tutorial_ros_package, create a directory named src where the Lisp files will go. The ASDF files should either stay in the root, or be placed in a subfolder called asdf.
ASDF is a build system for Lisp files similar to what CMake is to C++. ASDF systems are the equivalent of CMake projects.
roslisp allows to reference any ASDF system that is in any ROS package that you can roscd to, given a few conventions are met. In a nutshell, all you need to do is to place your .asd file (or a softlink to it) into the root of the ROS package.
We will be using a non-standard package setup in this tutorial, with distinctive names for the ROS package and ASDF system in order to make finding errors easier. Normally, if there is only one system in a particular ROS package the system should be called as the ROS package with underscores replaced by dashes. We will also create a Lisp package, which is the equivalent of namespaces of C++ and Java.
We will use the following names:
ROS Package : tutorial_ros_package
ASDF System : tutorial-asdf-system
Lisp Package : tutorial-lisp-package
Note that one ROS package may contain several ASDF systems, e.g. -main and -test, and that each ASDF system may contain several Lisp packages (namespaces), e.g. -core, -util, -internal.
Moving on to the ASDF system: in the root of your package, create a file named tutorial-asdf-system.asd with this content:
$ roscd tutorial_ros_package
(defsystem tutorial-asdf-system :depends-on (roslisp std_msgs-msg) :components ((:module "src" :components ((:file "package") (:file "talker" :depends-on ("package")) (:file "listener" :depends-on ("package"))))))
This declares the system components: a subdirectory named src, and within it 3 files, package.lisp, talker.lisp and listener.lisp. The first file will define the Lisp namespace, and the last two files will contain the code for a simple topic publisher and subscriber that will be discussed in the next section.
Note: ALWAYS name the .asd file like the system in defsystem. Otherwise ASDF will fail to load it.
One thing to notice is the way the messages from the std_msgs package are referenced. This is a concatenation of the ROS package name and "-msg", so to use the messages in the ROS package std_msgs we write :depends-on (std_msgs-msg). For more information on message and service files read here.
We conclude creating the initial structure of our roslisp package by creating the Lisp package definition file:
$ roscd tutorial_ros_package/src
(defpackage :tutorial-lisp-package (:nicknames :tut) (:use :cl :roslisp))
This defines our main package, a short nickname, and adds common-lisp and roslisp to the namespace of our package to allow using the functions within common-lisp and roslisp without qualifying them (e.g., roslisp:make-msg turns into simply make-msg).
Example for publishing and subscribing
First, we will create a talker node publishing regularly on a topic, and a subscriber node listening to the messages and printing them. Here's the code for the talker. It shows how to use topics, and the code follows the examples given in the basic ROS tutorials for Python or c++.
(in-package :tutorial-lisp-package) (defun talker () "Periodically print a string message on the /chatter topic" (with-ros-node ("talker") (let ((i 0) (pub (advertise "chatter" "std_msgs/String"))) (loop-at-most-every .1 (publish-msg pub :data (format nil "foo ~a" (incf i)))))))
ROS node code will usually be wrapped in a with-ros-node call as shown here, which handles command line arguments, makes sure to close subscriptions after shutdown, etc. The node name "talker" is arbitrary.
To publish on a topic, we need to first advertise the topic with a name and a topic type. The (advertise) function does that and returns a topic client that we can use for publishing.
The talker node then loops forever publishing to the topic. The publish-msg command creates a message (inferring the type from the publication object) and publishes it. Here's the corresponding listener:
(in-package :tutorial-lisp-package) (defun listener () (with-ros-node ("listener" :spin t) (subscribe "chatter" "std_msgs/String" #'print)))
This node subscribes to the chatter topic, then spins. The callback to print just prints the data field of any received message. Note that the Lisp pretty printer works for ROS messages.
The testing process is described in interactive use chapter below.
Example for server and client
The following examples use the service definitions from the ROS message and service tutorial. We will briefly summarize the above-mentioned next.
A .srv is a simple text file that describes a service. It is composed of two parts: a definition of a request message type and a response type. As all the .srv files of a ROS package are stored in a srv directory of that package, we will need to create one for our tutorial_ros_package:
$ roscd tutorial_ros_package $ mkdir srv && cd srv
Inside that directory we define our own service type. We name our service AddTwoInts, so the file will have the same name too:
# request int64 a int64 b --- # response int64 sum
In order to be able to use our service definition from the ROS API we need to generate representations of this service type in the programming language we'll be using, e.g. Lisp. ROS takes care of this automatically if we add a couple of lines to our package.xml and CMakeLists.txt: we add a ROS package dependency on the following two packages: message_generation and message_runtime. We also add a couple of CMake rules for creating targets for our auto-generated message definitions:
<?xml version="1.0"?> <package> <name ... <version ... <description ... <maintainer ... <license ... <buildtool_depend>catkin</buildtool_depend> <build_depend>roslisp</build_depend> <build_depend>std_msgs</build_depend> <build_depend>message_generation</build_depend> <run_depend>roslisp</run_depend> <run_depend>std_msgs</run_depend> <run_depend>message_runtime</run_depend> <export ... </package>
cmake_minimum_required(... project(... find_package(catkin REQUIRED COMPONENTS roslisp std_msgs message_generation ) add_service_files( FILES AddTwoInts.srv ) generate_messages( DEPENDENCIES std_msgs ) catkin_package( # INCLUDE_DIRS include # LIBRARIES assignment_6_1 CATKIN_DEPENDS roslisp std_msgs message_runtime # DEPENDS system_lib ) include_directories( ...
We also need to update the dependency on the auto-generated Lisp representation of our service type in our ASDF system definition and (optionally) Lisp namespace definition. We will simultaneously also add the new Lisp files for service server and client to our ASDF system definition. We will call the files add-two-ints-server and add-two-ints-client.
(defsystem tutorial-asdf-system :depends-on (roslisp std_msgs-msg tutorial_ros_package-srv) :components ((:module "src" :components ((:file "package") (:file "talker" :depends-on ("package")) (:file "listener" :depends-on ("package")) (:file "add-two-ints-server" :depends-on ("package")) (:file "add-two-ints-client" :depends-on ("package"))))))
(defpackage :tutorial-lisp-package (:nicknames :tut) (:use :cl :roslisp :tutorial_ros_package-srv))
To test that everything is correct, we compile our package and search for the auto-generated files:
$ roscd && cd .. && catkin_make $ sudo updatedb && locate tutorial_ros_package-srv.asd
Now we can finally take a look at the code. Based on the service definitions, a service provider can be created like this:
(in-package :tutorial-lisp-package) (def-service-callback AddTwoInts (a b) (ros-info (basics-system) "Returning [~a + ~a = ~a]" a b (+ a b)) (make-response :sum (+ a b))) (defun add-two-ints-server () (with-ros-node ("two_ints_server" :spin t) (register-service "add_two_ints" 'AddTwoInts) (ros-info (basics-system) "Ready to add two ints.")))
AddTwoInts is the service type, declared in a .srv file. The callback should be named with that type. "add_two_ints" is the service name the clients will use and that will appear when rosservice list is called.
As long as the node stays alive, requests to the service will be handled by calling the callback function. Its return value must be of the message response type of the service.
The code for a client looks like this:
(in-package :tutorial-lisp-package) (defun add-two-ints-client (a b) "adds by calling ros service" (with-ros-node ("two_ints_client") (if (not (wait-for-service "add_two_ints" 10)) (ros-warn nil "Timed out waiting for service add_two_ints") (format t "~a + ~a = ~a~&" a b (sum (call-service "add_two_ints" 'AddTwoInts :a a :b b)))))) (defun add-two-ints-client-main () ;; parse command line args (let ((args (cdr sb-ext:*posix-argv*))) (if (not (= 2 (length args))) (ros-info (basics-system) "Error ~a~%usage: add_two_ints_client X Y" args) ;; else (add-two-ints-client (parse-integer (first args)) (parse-integer (second args))))))
In case the service is provided by a different ROS package, you would need to load its auto-generated Lisp bindings for the services, and specify the service type with the according Lisp package namespace. E.g.:
;; in .asd file :depends-on (:external_service_package-srv) ;; in REPL (ros-load:load-system "external_service_package" "external_service_package-srv") ;; in code 'external_service_package-srv:AddTwoInts ; instead of just AddTwoInts
There will be an example in part 2 of this tutorial.
Lisp is most fun when used interactively. You have two alternatives here: either use a pre-setup roslisp_repl package (recommended for beginners) or directly use Common Lisp command line.
REPL through roslisp_repl
It is recommended to use Emacs with Slime and rosemacs Slime support.
If you do not want to setup your own Emacs copy with Slime, there is a ROS package roslisp_repl provided for that. Start a configured Emacs instance with an interactive Lisp session by calling
$ rosrun roslisp_repl roslisp_repl
Otherwise, after starting Emacs, run M-x slime to start a Lisp REPL. (In Emacs, "M-x" means pressing the Meta Key usually labelled "Alt" and while keeping it pressed pressing the "x" key. Then enter "slime" in the minibuffer at the bottom of the emacs window.) This will only work if you have slime installed as a Debian on your system.
REPL through shell
You can run roslisp nodes interactively from within the raw Lisp shell. To start sbcl:
This starts a Lisp REPL. This has a prompt looking like this: "*", whereas the slime REPL in Emacs has a prompt like this "CL-USER>". The latter will be used in tutorials.
Note that if you have rlwrap installed (sudo aptitude install rlwrap), using this primitive shell will be more tolerable, e.g.
$ rlwrap sbcl
Outside roslisp_repl, the first step for using roslisp is to load the roslisp init file:
* (load #P"/PATH/TO/ROSLISP/scripts/roslisp-sbcl-init")
You can find out your path to roslip using rospack find roslisp. By default this is:
* (load #P"/opt/ros/YOUR_ROS_DISTRO/share/roslisp/scripts/roslisp-sbcl-init")
Note that the SBCL REPL can be quit by pressing Ctrl+d at the prompt.
See the installation instructions for details.
Interactive publishing and subscribing (from either terminal or roslisp_repl)
Once Lisp is started you can use specific ROS packages by loading the ASDF system found in that package's root directory. For example, to load assignment_6_1,
CL-USER> (ros-load:load-system "tutorial_ros_package" "tutorial-asdf-system")
This will take care of locating the tutorial_ros_package package and all its dependencies as well as loading the corresponding .asd files.
Alternatively, you can also use the ros-load-system buffer command from roslisp_repl for loading packages, that gives a better compilation debugging interface:
CL-USER> , ros-load-system RET tutorial_ros_package RET tutorial-asdf-system RET
Now, before starting the publisher, make sure you have roscore running in some terminal. Note that you can also start the roscore using the functionality of rosemacs: simply press <Ctrl>+x+r+c, where x stands for "execute", r for "ROS" and c for "core".
The (talker) and (listener) functions shown above can be invoked using
CL-USER> (in-package :tutorial-lisp-package) #<PACKAGE "TUTORIAL-LISP-PACKAGE"> TUT> (talker)
CL-USER> (in-package :tutorial-lisp-package) #<PACKAGE "TUTORIAL-LISP-PACKAGE"> TUT> (listener)
in two separate Lisp REPLs. You will need two terminals or two running roslisp REPL instances (to start a new roslisp REPL press "<Alt>+x slime") to run both the talker and the listener interactively. Make sure you load the ASDF systems and change the Lisp packages in both Lisp shells. To stop a process in a Lisp shell press <Ctrl>+c+c.
Calling a service interactively
First we need to start our server. Make sure a roscore is running in a terminal.
In the REPL, do
;; load ros-load-manifest unless already done CL-USER> (ros-load:load-system "tutorial_ros_package" "tutorial-asdf-system") CL-USER> (in-package :tutorial-lisp-package) TUT> (add-two-ints-server)
Now let's start a ROS node in a different Lisp shell:
;; load the tutorial_ros_package and change the namespace TUT> (start-ros-node "my_node")
Calling (start-ros-node) sets up a ROS node in the context, this is good for interactive use, but else (with-ros-node) as used above in the code is more comfortable, as it closes the node when not used anymore.
You can now call the various ROS commands, set up callbacks (which will run in the background), etc. For example, type in the Lisp REPL:
;; start a node in case it's not running yet TUT> (start-ros-node "server_client_node") ;; create client node and call service TUT> (roslisp:call-service "add_two_ints" ; name of service 'tutorial_ros_package-srv:AddTwoInts ; message type :a 42 :b 24)
If all goes well, this should print out a message containing the answer 66.
Shutting down a running ROS node
Remember we started a ROS node earlier with (start-ros-node)? When done, remember to shut the ROS node down using
Working with ROS messages
For the examples, we shall use geometry messages.
In order for roslisp to have access to the messages, first load the ROS package:
CL-USER> (ros-load:load-system "geometry_msgs" "geometry_msgs-msg")
To create a ROS message for publishing on a topic, use (roslisp:make-message ...) or its alias (make-msg ...) The first argument is the name of the message type, then an even list of message slots and message contents are expected.
CL-USER> (roslisp:make-msg "geometry_msgs/Point" (y) 2 (x) 1) [GEOMETRY_MSGS-MSG:<POINT> X: 1 Y: 2 Z: 0.0]
Note how the order of arguments is arbitrary, and how arguments not given are still filled according to the message structure. The '(x)' notation uses a list because that's how we can set values deeper in a hierarchy.
It's also easy to create and access nested message types, e.g. Pose contains a Point and a Quaternion, the hard way looks like this:
(roslisp:make-message "geometry_msgs/Pose" (position) (roslisp:make-msg "geometry_msgs/Point" (x) 3) (orientation) (roslisp:make-msg "geometry_msgs/Quaternion" (w) 1))
However, we can also directly create the hierarchy. Try, for example,
(defvar pose-message (roslisp:make-msg "geometry_msgs/Pose" (x position) 3 (w orientation) 1))
This creates a Pose message.
You can get at the fields of a message you received (or the one we just created above) using calls like
(roslisp:with-fields ((x (x position)) orientation) pose-message (format t "~&X is ~a and orientation is ~a" x orientation)) X is 3 and orientation is [<QUATERNION> X: 0.0 Y: 0.0 Z: 0.0 W: 1]
Modified objects can be created using
CL-USER> (defvar pose2 (roslisp:modify-message-copy pose-message (z position) 1.0))
Messages are actually CLOS objects, and to maximize efficiency, you can also access that representation directly: see the API for details.
Accessing the parameter server
(roslisp:get-param ...) (roslisp:set-param ...) (roslisp:has-param ...)
For example, in the REPL execute the following:
CL-USER> (if (roslisp:has-param "~foo") (roslisp:ros-info params "Param foo exists with value ~a" (roslisp:get-param "~foo")) (roslisp:ros-info params "Param foo does not exist"))
foo should not exist at the moment. Now let's set foo:
CL-USER> (roslisp:set-param "~foo" (if (roslisp:has-param "~foo") (1+ (roslisp:get-param "~foo")) 42))
and execute the same REPL code for checking the param again. You can use <Ctrl>-<Up> to scroll through history.
Now we should get a result 42.
Publishing and subscribing to non-standard topics
Now we want to publish and subscribe on a topic of custom-specified type.
Using custom topics is very similar to using custom services. More information on creating custom messages can be found here.
To briefly summarize, we do the following.
We create a directory in our package called msg:
$ roscd tutorial_ros_package $ mkdir msg && cd msg
In that directory we specify two message files, Point.msg and Test.msg, where Point is a component of Test:
float32 x float32 y float32 z
Point location int32 orientation
We add a rule to our CMakeLists.txt to trigger auto-generation of the message Lisp binding during compilation:
cmake_minimum_required(... project(... find_package(... add_message_files( FILES Point.msg Test.msg ) add_service_files(... generate_messages(... catkin_package(... include_directories(...
We add a dependency on the auto-generated Lisp binding into our ASDF system definition, and also define two new files, array-talker.lisp and array-listener.lisp, where the code will go:
(defsystem tutorial-asdf-system :depends-on (roslisp std_msgs-msg tutorial_ros_package-srv tutorial_ros_package-msg) :components ((:module "src" :components ((:file "package") (:file "talker" :depends-on ("package")) (:file "listener" :depends-on ("package")) (:file "add-two-ints-server" :depends-on ("package")) (:file "add-two-ints-client" :depends-on ("package")) (:file "array-talker" :depends-on ("package")) (:file "array-listener" :depends-on ("package"))))))
And not to forget the Lisp package definition: src/package.lisp:
(defpackage :tutorial-lisp-package (:nicknames :tut) (:use :cl :roslisp :tutorial_ros_package-srv :tutorial_ros_package-msg))
The code looks like the following: array-talker.lisp:
(in-package :tutorial-lisp-package) (defun array-talker () "Illustrates array and compound messages, and rosout functions." (with-ros-node ("talker") (let ((pub (advertise "array_chatter" 'Test))) (loop-at-most-every 1 (let ((s (random 10)) (p (make-instance 'Point :x 1.0 :y 2.0 :z 42.0))) (publish pub (make-instance 'Test :location (vector p p) :orientation (vector (* 2 s) s s))))))))
(in-package :tutorial-lisp-package) (defun array-listener () "Like listener, except illustrates an array message." (with-ros-node ("listener" :spin t) (subscribe "array_chatter" 'Test #'(lambda (x) (ros-info nil "~&Location is ~a and orientation is ~a" (location x) (orientation x))))))
Compile the package:
$ roscd tutorial_ros_package/../.. $ catkin_make
We will test it in the REPL. Each time we compile a package and the ASDF system changed or there were new ROS packages added, we need to restart our REPL. For that either restart your Emacs or use ,restart-inferior-lisp buffer command:
CL-USER> , restart-inferior-lisp CL-USER>, ros-load-system RET tutorial_rosTAB RET tutoTAB RET CL-USER>, !p tutorial-lisp-package RET TUT> (array-talker)
And then in a new Lisp shell:
Part 2: Controlling the TurtleSim with roslisp
In this part of the tutorial we will set up a roslisp package to control the turtles in the ROS turtlesim.
Creating the package
This time we will use the same name for the ROS package, ASDF system and the Lisp package, making sure to stick to the naming conventions.
Create a ROS package as usual including a dependency to roslisp, turtlesim and geometry_msgs in the src of your catkin workspace:
$ roscd && cd ../src && catkin_create_pkg lisp_turtles roslisp turtlesim geometry_msgs
It is a good practice to compile your package after creating it and updating the rospack profile:
$ cd .. && catkin_make && rospack profile
We include the dependency on turtlesim to be able to access the service and message types. Certain commands of turtlesim need a message of type that is specified in geometry_msgs, so we include that package as a dependency as well.
Inside the new folder lisp_turtles create a src directory. To demonstrate how to organize files when a ROS package has multiple ASDF systems, we will collect our Lisp files in a subdirectory turtles, which will correspond to one ASDF system. We will put our *.asd files in the src directory and put a symbolic link to them into the root of our package.
Let's do so. Create a turtles subdirectory (inside src). Now, in src create lisp-turtles.asd with this content:
(asdf:defsystem lisp-turtles :depends-on (roslisp turtlesim-msg geometry_msgs-msg turtlesim-srv) :components ((:module "turtles" :components ((:file "package") (:file "turtles-core" :depends-on ("package"))))))
This declares the system components: a subdirectory turtles and in there 2 files, package.lisp and turtle-core.lisp.
Create a softlink inside the root folder (or the asdf folder if you choose to use it) to this file:
$ roscd lisp_turtles $ ln -s src/lisp-turtles.asd
Now, we can imagine having a different ASDF system in the same ROS package, say, lisp-turtles-test.asd, which would have its own subdirectory within src, such that the two are nicely separated.
The two files in src/turtles/ look like the following:
(defpackage lisp-turtles (:nicknames :lturtle) (:use :roslisp :cl))
This defines our main package, a short nickname, and adds roslisp to the namespace of our package.
turtlesim roslisp code
(in-package :lturtle) (defun start-node () (roslisp:start-ros-node "lispturtles")) (defun stop-node () (roslisp:shutdown-ros-node)) (defun reset-turtlesim () (roslisp:call-service "/reset" 'std_srvs-srv:empty)) (defun clear-turtlesim () (roslisp:call-service "/clear" 'std_srvs-srv:empty)) (defun spawn-turtle (&key (x 0) (y 0) (theta 0)) (turtlesim-srv:name (roslisp:call-service "/spawn" 'turtlesim-srv:spawn :x x :y y :theta theta))) (defun unspawn-turtle (name) (roslisp:call-service "/kill" 'turtlesim-srv:kill :name name))
We defined some functions to manipulate the turtlesim, if you've gone through the first part of this tutorial you should be able to understand the functions by now.
We still use the roslisp: prefix for calling roslisp functions for the sake of clarity, although it is not obligatory now that we have (:use :roslisp) in our package.lisp.
Your src directory should now look like this:
src/turtles/package.lisp src/turtles/turtles-core.lisp src/lisp-turtles.asd
In Emacs, we can now load this system using roslisp_repl. In a fresh REPL, press ",", type in ros-load-system, enter, then type lisp_turtles for the ROS package, then lisp-turtles for the ASDF system. If roslisp_repl does not find those, check that the package can be found by ROS (roscd into it from a terminal), that the link to the .asd file is not broken, and that the name of the .asd file and the name of the ASDF system within are exactly the same. You might need to restart your REPL or manually reinitialize the source registry of ASDF if you started the REPL before creating the ROS package.
Now you should be able to call the Lisp functions in the REPL, after having started the turtlesim. In 2 terminals, call
$ rosrun turtlesim turtlesim_node
respectively. In the REPL, call (after loading the lisp_turtles package):
CL-USER> (in-package :LTURTLE) #<PACKAGE "LISP-TURTLES"> LTURTLE> (start-node) [(ROSLISP TOP) INFO] 1287314273.963: Node name is /lispturtles [(ROSLISP TOP) INFO] 1287314273.964: Namespace is / [(ROSLISP TOP) INFO] 1287314273.964: Params are NIL [(ROSLISP TOP) INFO] 1287314273.964: Remappings are: [(ROSLISP TOP) INFO] 1287314273.964: master URI is 127.0.0.1:11311 [(ROSLISP TOP) INFO] 1287314275.014: Node startup complete LTURTLE> (spawn-turtle :x 1 :y 1) "turtle2"
First, we switch into the LTURTLE package in which our functions are defined. Then we start a node in roslisp which will serve for all the following calls to topics and services. Finally, we spawn a second turtle at (1, 1) using the corresponding turtlesim service. Now you should see a second turtle in the turtlesim window.
To do more useful things with the turtlesim, add these functions to the file turtles-core.lisp and recompile it (in Slime that would be <Ctrl>+C <Ctrl>+K when editing the .lisp file, or in the worst case restart your Lisp or reload the ASDF system):
(defun set-turtle-velocity (name &key (lin 0) (ang 0)) "Publishes a velocity command once." (let ((pub (advertise (concatenate 'string "/" name "/cmd_vel") "geometry_msgs/Twist"))) (publish pub (make-msg "geometry_msgs/Twist" (x linear) lin (z angular) ang)))) (defun set-pen (name &key (r 0) (g 0) (b 0) (width 1) (off 0)) "Changes the color of the turtle trajectory." (roslisp:call-service (concatenate 'string "/" name "/set_pen") 'turtlesim-srv:setpen :r r :g g :b b :width width :off off))
We use the fact that the topics created by turtlesim include the name of the turtle.
To test this execute the following in the REPL:
LTURTLE> (loop-at-most-every 1 (set-pen "turtle2" :r (random 255) :g (random 255) :b (random 255) :width (random 5)) (set-turtle-velocity "turtle2" :lin (- (random 10) 5) :ang (- (random 10) 5)))