Introduction to ROS(Robot Operating System) Raoul DIFFOUO Tshwane University of Technology Department of Computer Engineering April 2013 Table of Contents Introduction .................................................................................................................................... 3 1. ROS Basics ............................................................................................................................... 3 What is ROS? .............................................................................................................................. 3 ROS Distributions ................................................................................................................... 3 Where is it used? ........................................................................................................................ 4 How does ROS work? ................................................................................................................. 4 Key concepts and Terminology ................................................................................................. 6 ROS Filesystem ....................................................................................................................... 6 Packages .............................................................................................................................. 6 Manifests.............................................................................................................................. 6 Stacks ................................................................................................................................... 6 Stack Manifests.................................................................................................................... 6 Message Types .................................................................................................................... 6 Services types....................................................................................................................... 6 ROS Computation Graph Level ............................................................................................. 7 Nodes ................................................................................................................................... 7 Master .................................................................................................................................. 7 Messages ............................................................................................................................. 7 Topics ................................................................................................................................... 7 Services ................................................................................................................................ 7 ROS Community Level ........................................................................................................... 8 Installing and Configuring your ROS Environment ................................................................... 8 Create a ROS Workspace ...................................................................................................... 8 Create a ROS Package ........................................................................................................... 8 Writing our first ROS node in C++ ............................................................................................ 9 The Publisher .......................................................................................................................... 9 The code .............................................................................................................................. 9 Code Breakdown ............................................................................................................... 10 The Listener .......................................................................................................................... 12 The code ............................................................................................................................ 12 Code Breakdown ............................................................................................................... 13 Building and executing the nodes ...................................................................................... 14 2. NXT Robot and ROS ............................................................................................................. 15 Installation and Configurations ................................................................................................ 15 Configure the computer ...................................................................................................... 15 Installing ROS and NXT ROS ............................................................................................... 16 Lets Try It! ............................................................................................................................. 17 Teleoperate the NXT using Keyboard ..................................................................................... 18 Follow Objects with your NXT and ROS .................................................................................. 21 The code ............................................................................................................................... 22 Code Breakdown .................................................................................................................. 25 The header files ................................................................................................................. 25 NxtFollow Class definition ................................................................................................ 25 The constructor ................................................................................................................. 26 The control loop ................................................................................................................ 27 The callback method ......................................................................................................... 28 The publish method .......................................................................................................... 28 The main ............................................................................................................................ 28 Build and Run the Execute the Node ................................................................................. 29 3. Useful Links ............................................................................................................................ 31 Learn C++ ............................................................................................................................. 31 ROS Wiki ............................................................................................................................... 31 NXT ROS ............................................................................................................................... 31 Software for ROS is primarily tested on Ubuntu and Mac OS X systems. device drivers.org/wiki/ROS/Installation . Arch Linux.org/wiki/diamondback http://ros. A Port to Microsoft Windows is currently under development and still at an experimental stage. visualizers. ROS Distributions ROS releases might not be compatible with other. ROS Basics What is ROS? ROS (Robot Operating System) is an open-source. It also provide hardware abstraction.org 1. meta-operating system for your robot. It provides libraries and tools to help software developers create robot application. http://www. 2010 March. 2012 August. Go to the following link for installation guide. go through a practical usage with the NXT Robot platform. where you will find number of tutorials. While we can’t hope to provide you with a full coverage of ROS capabilities. 2011 March. 2010 Wiki Page http://ros. and other Linux platforms. The major releases so far are listed in the table below: ROS Distributions ROS Groovy ROS Fuerte ROS Electric ROS Diamondback ROS C Turtle ROS Box Turtle Release Date December. The ROS community has been contributing support for Fedora. package management.org/wiki/boxturtle ROS currently runs on Unix-based platform. we present some of the most important key concepts and in the second part. It is highly recommended to install ROS on Ubuntu. ROS was originally developed in 2007 under the name switchyard by the Stanford AI Laboratory and was further developed at Willow Garage with contributions all around the world. at http://www.ros.org/wiki/fuerte http://ros.org/wiki/cturtle http://ros. They are often referred to by code name rather than version number.org/wiki/electric http://ros. and more.Introduction This document is an attempt to give an overview of ROS to robotic hobbyist. message-passing. 2011 August. 2012 April. You are in the meantime invited to visit the official documentation maintained by the developer of ROS.ros.org/wiki/groovy http://ros. on which it publishes their respective values. robots of every size and shape are using ROS to do interesting research and applications development. The robot is connected to a PC running ROS. robot-node that is set to receive velocities on cmd_vel Topic. The ROS Master here plays a very essential role in the system. .Where is it used? From small differential-drive robots to mobile manipulators to autonomous cars. we have a Node for our Robot. and is connected via USB. Groups around the world are also releasing free.org/wiki/Robots How does ROS work? The previous pictures depict a situation where we would like to control a simple differential drive robot using ROS. Find a list of robots using ROS at: http://www. open-source software to get you started on your own robot. Control node then publishes velocities on Topic cmd_vel. On the ROS software layout. On the other hand we have a control node which in this case receives distance values from the Ultrasonic sensor on Topic ultrasonic. The Master is responsible for setting the communication between Nodes and making sure that every Node finds each other and exchange messages through the proper channels. The robot Node also creates a Topic for all the sensors available. When systems starts every Nodes needs to present themselves to the Master and specify the topic they will be subscribing or publishing to.ros. in order to capture image and move the robot based on what we receive.Assume we want to add a camera to our system. All we would have to do is create a Node to handle camera that will publish a message images on Topic image. Then a Node camvision that receives images on topic image then publish camdata on topic image_data. Lastly our control node would be modified to subscribe to topic image-data instead and still publish velocities on cmd_vel. It would at the end look something like this . configurations files. ROS Filesystem Packages Packages are the main unit for organizing software in ROS. A package contain ROS runtime processes (Nodes). launch files. build files. including license information and dependencies as well as language-specific information such as compiler flags. Stack Manifests Stack manifest (stack. As an example the ‘Navigation Stack’ contains all the necessary packages for finding where the robot is and how it can get somewhere else. store in my_package/msg/MyMessageType. datasets or anything that is usefully organised together. ROS software are also released in a form of Stacks and have associated version numbers. including its license information and its dependencies on other stacks. and all the code in a package should be related. A brief description of each concept and will give more details in later sections of this document. define the data structures for messages sent in ROS. Manifests Manifests (manifest.xml) provide metadata about the package. . Stacks Stacks are a collection of packages that provide aggregate functionality.msg.Key concepts and Terminology Before we dive into using ROS it is important for use to understand its terminology and structure.srv. ROS has three levels of concepts: the Filesystem level. define the request and response data structures for services in ROS. the Computational Graph level. Message Types Messages descriptions. and the Community level. Services types Services descriptions. stored in my_package/srv/MyServicesType.xml) provide data about a stack. A Package can contain any number of nodes. Master The ROS Master provides naming. one node to control the wheel motors. The basic Computation Graph concepts of ROS are nodes. messages. A node sends out a message by publishing it to a given Topic. Request / Reply is done via Services. Master. one-way transport is not appropriate for request / reply interactions. registration services and lookup to the rest of the nodes in the ROS system. For example. and so on. one node will be used to control a specific sensor. services. exchange messages. and bags all of which provide data to the Graph in different ways. one node to perform teleoperation. are supported. etc. The Topic is a Name that is used to identify the content of a message. Each Bus has a name. but its many-tomany. as well as arrays of primitive types.ROS Computation Graph Level This is peer-to-peer network of ROS processes that are processing data together. Standard primitive types such as integer. A robot will control system is typically composed of many nodes. topics. . Messages can include arbitrarily nested structures and arrays (much like C structs). Messages Nodes make use of Messages to communicate with one another. which are defined by a pair of message structures: one for the request and one for the reply. A Message is a simple data structure. Topics Messages are routed via a transport system using publish / subscribe semantics. and anyone can connect to the bus to send and receive messages as long as they are of the right type. The Master is the one responsible for making sure that every ROS Nodes find each other. it just need to subscribe to the appropriate Topic. floating point. which are defined by pair of message structures. Boolean. Parameter Server. Whenever a Node is interested in acquiring a certain kind of data. Services The publish / subscribe model is a very flexible communication paradigm. A Topic can be seen as a Bus by which information travel in a ROS system. Nodes A Node is a process that performs computation. one node to perform path planning. Nodes may reside in different systems. or invoke Services. containing typed fields. which are often required in distributed system. ROS Community Level The ROS Community Level concepts are ROS resources that enable separate communities to exchange software and knowledge. the ROS Wiki. Find out more at http://www. $ mkdir ~/ros_workspace/sandbox $ rosws set ~/ros_workspace/sandbox This will create a sandbox folder in our workspace. we can now create our workspace. Create a ROS Package To do so open a new terminal windows. We are going to see how to do so in the next section. $ cd ~/ros_workspace/sandbox Then create the package by entering the following command . New packages need to be put in a path that is in the variable ROS_PACKAGE_PATH.ros. Move to the sandbox directory. it is often useful to do so in a "workspace".ros.bash to make sure that the updated ROS_PACKAGE_PATH is used. Next we are going to create a package for our next section where we will be writing our first ROS node in C++. http://www. Create a ROS Workspace The following command (execute from the Terminal) creates a new workspace in ~/ros_workspace which extends the set of packages installed in /opt/ros/<distro> (replace <distro> with the name of the distribution installed on your system): $ rosws init ~/ros_workspace /opt/ros/<distro> Next we create a sandbox directory where we will store packages created during the tutorial.org/wiki/groovy/Installation/Ubuntu After successfully installing and configuring ROS. All directory managed by rosws are automatically added to the ROS_PACKAGE_PATH. These resources include Distributions. and instructions on how to setup your environment variables.org/wiki/ROS/Concepts#ROS_Community_Level Installing and Configuring your ROS Environment Go to the following link for installation guide for Linux Ubuntu. When working with ROS source code. Repositories. Next we need to re-source ~/ros_workspace/setup. etc. and listener on the other hand will subscribe to chatter in other to listen to what talker has to say. The Publisher In this first example we will create two nodes. For our tutorial we will type in the following.$ roscreate-pkg [package_name] [depend1] [depend2] [depend3] where [package_name] will be the name of your package followed by the dependencies of that package [depend1] [depend2] etc.org/wiki/ROS/Tutorials Take time to go through these tutorials on your own. They will both use the topic chatter to talk to each other. The code First create the src/talker. talker and listener. Writing our first ROS node in C++ The code we are going to use for our first example are from ROS tutorials http://www.cpp . talker and listener. They will both use the topic chatter to talk to each other. and listener on the other hand will subscribe to chatter in other to listen to what talker has to say.talker. Talker will publish messages on chatter. In this first example we will create two nodes.cpp C++ Code . $ roscd ros_intro $ gedit src/talker. Talker will publish messages on chatter. Our package will depend on the roscpp and std_msgs packages $ roscreate-pkg ros_intro std_msgs roscpp To move into the directory of the package $ roscd ros_intro $ pwd The following should be printed if the package was correctly created YOUR_PACKAGE_PATH/ros_intro Now that we created the package we can start writing some code.ros.cpp file within the ros_intro package and write the code provided below. 7. 26. } Code Breakdown 1. 10.h" #include <sstream> int main(int argc. 2.h is includes the description of the message type that will be used for this example. #include "ros/ros. 21. 23. stream is responsible for some string manipulation in C++. . You need to include a different header if you have to use a different message type. 7. ros::init(argc. 4.advertise<std_msgs::String>("chatter".publish(msg). 15. The first NodeHandle created will actually do the initialization of the node. ros::init Initializes ROS. 5. ros::spinOnce().h is a convenience include that includes all the headers necessary to use the most common public pieces of the ROS system. ros::NodeHandle n. #include "ros/ros. ++count.sleep(). 6. Node names must be unique in a running system. int count = 0. 3. 11. 14. ros::Rate loop_rate(10). ros::NodeHandle n. msg. argv.h" #include <sstream> ros/ros.str().h" #include "std_msgs/String. chatter_pub. 2. Std_msgs/String. "talker"). 9. msg. 1000). 12.1. } return 0. It is responsible for collecting ROS specific information from arguments passed at the comment line. 20.h" #include "std_msgs/String. 22. argv.c_str()). ss << "hello world " << count. This is also where we specify the name of our node. std::stringstream ss. 24. 8. "talker"). and the last one destructed will clean up any resources the node was using. 17. 6.data = ss. ROS_INFO("%s". It allows you to interact with the node associated with this process. ros::Publisher chatter_pub = n. 13.data. 18. 3. 19. 25. 16. while (ros::ok()){ std_msgs::String msg. char **argv){ ros::init(argc. ros::NodeHandle Create a handle to this process' node. loop_rate. 12. ROS_INFO is the ROS replacement for printf or cout. Its value will be attached to the string message that is published. all Ros::NodeHandle have been destroyed Once ros::ok() returns false.publish(msg). which serves two purposes: 1) it contains a publish() method that lets you publish messages onto the topic it was created with. 14. It will keep track of how long it has been since the last call to Rate::sleep(). ros::Publisher::publish() sends the message to all the subscribers. ros::Rate loop_rate(10).data. 11. std_msgs::String msg. msg. ROS_INFO("%s".data = ss. it will automatically unadvertise.advertise<std_msgs::String>("chatter". 1000). For more details. while (ros::ok()){ count here will just be used to keep track of the number of messages transmitted.c_str()). and sleep for the correct amount of time. 15. 18.1. . ss << "hello world " << count. These 4 lines perform some strings manipulation too add count to the message the will be broadcasted.str(). msg. NodeHandle::advertise() returns a ros::Publisher object. ros::Publisher chatter_pub = n. ros::spinOnce(). In this case it maintain the frequency of publishing at 10 Hz. 17. and 2) when it goes out of scope. int count = 0. std::stringstream ss. 16. chatter_pub. 19. This is the function in charge of making the XML/RPC call to the ROS Master advertising std_msgs::String on the topic named “chatter” A ros::Rate object allows you to specify a frequency that you would like to loop at. all ROS calls will fail. ros::ok() will return false if: a SIGINT is received (Ctrl-C) we have been kicked off the network by another node with the same name ros::shutdown() has been called by another part of the application. 13. 10. see the rosconsole documentation. 2. By default roscpp will install a SIGINT handler which provides Ctrl-C handling which will cause ros::ok() to return false if that happens. Calling the ros::spinOnce() in this example is note really necessary.h" void chatterCallback(const std_msgs::String::ConstPtr& msg){ ROS_INFO("I heard: [%s]". argv. 9. } int main(int argc. "listener"). char **argv){ ros::init(argc. loop_rate.cpp file within the ros_intro package and write the code provided below. 12. ++count. 5. 2. instead they are placed in a queue which is processed when a call to ros::spinOnce() is made. ros::Subscriber sub = n.h" #include "std_msgs/String. $ roscd ros_intro $ gedit src/listener. The Listener The code First create the src/listener. 13. 10. To sum up. 11. 15. #include "ros/ros. return 0. here is a condensed version of what’s going on with the ‘talker’ program: Initialize the ROS system Advertise that we are going to be publishing std_msgs/String messages on the chatter topic to the master Loop while publishing messages to chatter 10 times a second We will now look into our receiving node. 7. 1000.cpp C++ Code . chatterCallback). msg->data. The callbacks for receiving messages on those topics are not called immediately.cpp 1. 8. } . But it is good practice to always have it in a program.listener. 14. 3. because we are not subscribing to any topic.subscribe("chatter". 6. ros::Rate::sleep() allows us here to keep a particular publishing frequency count gets incremented to keep track of messages.sleep(). ros::NodeHandle n.c_str()). 4. ros::spin(). 21.20. return 0. chatterCallback() is the function we defined that gets called whenever we receive a message on the subscribed topic. that should be hold until you want to unsubscribe. 1000. It subscribes to the topic chatter. waiting for messages to arrive When a message arrives. or it being called manually. In sum. "listener"). ros::init(argc. } We include the same headers as before. ros::Subscriber sub = n. NodeHandle::subscribe() returns a ros::Subscriber object.subscribe("chatter". 2. 15. #include "ros/ros. ROS will call the chatterCallback() function whenever a new message arrives. ros::spin(). argv. 6.h" #include "std_msgs/String. 10. msg->data. 4. in case we are not able to process messages fast enough. we will start throwing away old messages as new ones arrive.c_str()). here is a condensed version of what’s happening: Initialize the ROS system Subscribe to the chatter topic Spin. The second argument is the queue size. 13. ros::NodeHandle n.Code Breakdown 1. In this case. int main(int argc. if the queue reaches 1000 messages. either by the default Ctrl-C handler. the chatterCallback() function is called. 8. It will exit once ros::ok() returns false. 5. meaning ros::shutdown() has been called. 12. it will automatically unsubscribe from the chatter topic. chatterCallback).h" void chatterCallback(const std_msgs::String::ConstPtr& msg){ ROS_INFO("I heard: [%s]". This is where we retrieve the content of the message for display as we did in this case. } ros::NodeHandle::subscribe makes an XML/RPC call to the ROS Master. 3. 14. When the Subscriber object is destructed. ros::spin() loops around calling message callbacks as fast as possible. or we could store it in a variable for later processing. . 11. char **argv){ 9. $ rosmake ros_intro In separate terminal windows. $ gedit ros_intro/CMakeLists. which by default go into the ‘bin’ directory.Building and executing the nodes First we need to edit CMakeLists. run the following programs $ roscore $ rosrun ros_intro talker $ rosrun ros_intro listener You should see the following results Talker Node Listener Node . For more information on using CMake with ROS. talker and listener.cpp) This will create two executables. check CMakeLists Now let’s build our package. Save and exit. Open the file with the following command from the terminal.cpp) Rosbuild_add_executable(listener src/listener.txt Then add the following 2 lines to the file Rosbuild_add_executable(talker src/talker.txt from our package. Configure the computer In order for your computer to talk to the NXT brick you need to set your udev1 rules. It has basic interfaces for interacting with ROS and NXT. log out and log back in. A ROS stack is available for the NXT. This package creates a ROS topic for each NXT sensor. and publish the sensor’s data on this topic. I will be using Ubuntu 11.2.rules && sudo mv /tmp/70-lego.d/70-lego. MODE=\"0660\"" > /tmp/70lego. $ echo "BUS==\"usb\". It also creates topic for each NXT motor. For this second part we will apply some of the concept we learned to a physical robot. ROS has quite become a great tools for hobbyist and researchers. The nxt_ros bindings talks directly to the NXT brick. We will be using the NXT Lego Robot. Installation and Configurations The following will require you to have a PC running Ubuntu 10. NXT Robot and ROS There is quite a range of robots being used with ROS at this day.org/wiki/Udev . which allows for the command of motors from ROS. ATTRS{idVendor}==\"0694\". GROUP=\"lego\".04 or newer.rules Now restart the udev: $ sudo restart udev To complete the configuration. The bridge between NXT and ROS is the nxt_ros package.rules /etc/udev/rules. either over USB or Bluetooth.wikipedia. 1 For more information on udev visit http://en. To do so you are going to first add a lego group using the following command: $ sudo groupadd lego Then add yourself to that group: $ sudo usermod –a –G lego <username> Replace <username> in the previous command with yours.10. Next create a udev rules file for the lego group that you just created. 10. We will get the Desktop-Full Install which includes ROS packages. and ‘multiverse’ Once that is done open a terminal window and setup up your computer to accept software from ROS.Installing ROS and NXT ROS Before you can install ROS on your computer. robot-generic libraries. you can install the NXT-ROS bindings: 1 http://www.bash” >> ~/.ros.org/wiki/Robots/NXT/electric#Installation_Instructions . You need to make sure your internet connection is always on. you have to configure the Ubuntu repositories to allow ‘restricted’.key -O . Set up your environment variables: $ echo “source /opt/ros/electric/setup.ros.bashrc . navigation and 2D/3D perception. $ sudo apt-get update $ sudo apt-get install ros-electric-desktop-full The installation will take quite a while. Once ROS installation is complete. If you are running a different version please refers to this page1 to get the correct command.ros.org/ros/ubuntu oneiric main" > /etc/apt/sources.bashrc Now that the installation of ROS is complete.| sudo apt-key add - Installation of ROS Electric.list' This is for Ubuntu 11. ‘universe’. ~/.org/ros.org $ sudo sh -c 'echo "deb http://packages.d/ros-latest.list. 2D/3D simulators. Next set up your Keys $ wget http://packages. For that connect a touch sensor to Port 1 of you NXT.$ sudo apt-get update $ sudo apt-get install ros-electric-nxtall When installation is complete you should be ready to test it with your robot. you can follow the following tutorial: Updating NXT Firmware. . Then in a terminal window. Make sure you have your NXT ready and connected to your PC. Testing the touch sensor If instead you see the following. you need to have the firmware 1. Lets Try It! IMPORTANT: For NXT-ROS work with your Mindstorms. it will print ‘True’ and ‘False’ when released. then you probably didn’t set something right or the Brick is not connected or powered on. We are now going to run a program to test if the NXT can indeed talk to ROS.28 or higher. If you do not know how to upgrade.py You should get the following results in your terminal. When press the touch sensor. type the following $ roscore Then in another terminal. run the following $ rosrun nxt_python touch_sensor_test. org/wiki/nxt_robot_sensor_car Create your own robot model . and how they inter connect 1 2 Robot car Sensor . we can now write a small application to run our NXT using the robot car sensor1 model provided with the ROS package. The following diagram. Teleoperate the NXT using Keyboard We will now walk through the steps to control the NXT using your keyboard.org/wiki/nxt/Tutorials .ros.NXT Brick not found If you got the touch sensor test right.http://www. If you like you can go through this tutorial2 to create your own robot for ROS.ros. will attempt to present some of the important nodes used to achieve that.http://www. name: color_sensor 36. port: PORT_3 21. 24. name: gyro 19.01 30.0 11. . . name: ultrasonic_sensor 27.2 29. desired_frequency: 20. motors. name: l_wheel_joint 9. port: PORT_2 28. port: PORT_B 10.0 . frame_id: gyro_link 20. nxt_robot: 2.yaml which is a description file for the robot. port: PORT_C 15. desired_frequency: 20.0 23. where we need to specify the parameters for the sensors.type: gyro 18. 7. desired_frequency: 5. 33. frame_id: color_link 35. /opt/ros/electric/stacks/nxt_robots/nxt_robot_sensor_car You will have to edit this file in order to match your robot configuration.type: ultrasonic 25. 17. min_range: 0. port: PORT_A 5. max_range: 2.yaml 1. . there a file robot. robot. . desired_frequency: 10. offset: 0 22. name: r_wheel_joint 4.5 31. and define on which port they will be physically connected. . On my PC the file is located at the following location.type: motor 13. .type: color 34. The initial file looks like this. frame_id: ultrasonic_link 26. spread_angle: 0.0 32. 12.0 6.type: motor 8. port: PORT_1 37.Inside the nxt_robot_car_sensor package. desired_frequency: 1. name: m_wheel_joint 14.type: motor 3.0 16. desired_frequency: 20. 7.launch Keep this program running. $ rosmake nxt_robot_sensor_car Once that’s done. max_range: 2. you can now run the robot with the following command.5 19. spread_angle: 0. $ roscore First start the ROS Master with roscore. The one I will be using only make use of 2 motors (PORT A and B) and an ultrasonic sensor (PORT 4). desired_frequency: 20. 12.type: motor 8.launch You should have a prompt on your screen instructing you to use the direction keys to move the robot. .0 20. name: r_wheel_joint 4. Always make sure its running before you try to execute a ROS process. . I will then have to edit this file so it looks like this.yaml file.0 11. nxt_robot: 2.01 18. desired_frequency: 20. name: ultrasonic_sensor 15.yaml 1.0 6. port: PORT_A 5. port: PORT_B 10. $ roslaunch nxt_teleop teleop_keyboard.2 17. desired_frequency: 5. color sensor. If you will like to see a graph showing all the running nodes and their connections run the following command .This file defines a robot with a gyroscope. frame_id: ultrasonic_link 14. ultrasonic sensor. name: l_wheel_joint 9. . then open a new terminal window and run the control program. Now execute the following from another terminal $ roslaunch nxt_robot_sensor_car robot. After editing the file we now have to build the package again from the terminal for our system to consider the changes. port: PORT_4 16. min_range: 0. and 3 motors.type: motor 3. robot.type: ultrasonic 13. Feel free to add any sensors you might have to the robot. launch $ roslaunch nxt_teleop teleop_joy.launch. In order to achieve that we will have to write a node that will subscribe to the topic ultrasonic_sensor. Instead of the teleop_keyboard. it should back up (move backward). you will just need to launch the teleop_joy. Basically what we want to achieve here is read the distance provided by the ultrasonic sensor. the robot should move forward. to control the movements of the robot. When the object stops moving the robot should also stop within a given range.launch Follow Objects with your NXT and ROS Now that we can control our NXT with keyboard/joystick let’s have our robot follow an object place in front of it. Then based on the distance it receives will publish velocities on topic cmd_vel. following the object.$ rxgraph And you should get this You can also use a joystick to control your robot. . in order to read distance. If the object is too close of the robot. When the object is within a certain range of the robot and moving away. NxtFollow(). 14. class NxtFollow { 11. 15.hpp" 9. . #include "boost/thread/mutex. #include <termios.h> 7.h> 3.h> 4. #include <ros/ros. void controlLoop(). private: 17. #include <geometry_msgs/Twist.hpp" 8.cpp file within the nxt_follow_obj package and write the code provided below. public: 12.h> 2. ros::NodeHandle nh_. 10. #include "boost/thread/thread. #include <stdio. ph_. and nxt_msgs.cpp 1.h> 6.We will create a new package in the sandbox folder of our workspace (see section 1. 16.h> 5. #include <signal. the geometry_msgs. Next we need to write the code for the node. $ roscreate-pkg nxt_follow_obj nxt_ros geometry_msgs nxt_msgs This package will depend on the nxt_ros package. #include <nxt_msgs/Range. The code Create the src/follow_obj.5). void watchdog(). 13. C++ Code – follow_obj. 75. 43. 52. 68. l_scale_(1. void callback(const nxt_msgs::Range::ConstPtr& Range). puts("---------------------------"). 19. 35. 37. }. 56. raw. 61.param("scale_angular".c_cc[VEOL] = 1. } void NxtFollow::controlLoop() { char c. 58. NxtFollow::NxtFollow() : linear_(0). 33. struct termios cooked. 47. 67. 70. a_scale_. angular_.3 && dist_from_obj_ <= 1. 51.0). 57. 49. 26. 77. 41. puts("Let me follow a box"). linear_ = 1. . ROS_DEBUG("value: 0x%02X\n". 36. 66. double). 44. ros::Time last_publish_. 73. 48. raw. 39.18. tcsetattr(kfd. void NxtFollow::watchdog() { boost::mutex::scoped_lock lock(publish_mutex_). tcgetattr(kfd. 46. } int kfd = 0. c). raw. 23. double dist_from_obj_. 74. ROS_INFO("controlLoop").0) { ph_. &cooked. &NxtFollow::callback. 62. angular_(0). 29. 78. 34. l_scale_). sizeof(struct termios)). double l_scale_. this). ros::Subscriber range_sub_.0) { ROS_INFO("Following").15)) publish(0. ROS_DEBUG("Follow the Object"). 28. 53. TCSANOW. if (dist_from_obj_ > 0. a_scale_(1. 71. 40. 25. void publish(double. 31. 32. ros::Publisher vel_pub_. 30. 54. raw. memcpy(&raw. 38. 0).c_lflag &=~ (ICANON | ECHO). 59. a_scale_). 50.param("scale_linear".advertise<geometry_msgs::Twist>("cmd_vel". 27.c_cc[VEOF] = 2. puts("Place an object in front of the nxt to start"). angular_ = 0. 22. double linear_. boost::mutex publish_mutex_. a_scale_. 21.0. 24. &cooked). 63. 69. range_sub_ = nh_. 65. 45. vel_pub_ = nh_. 60. 55. 76. ph_.subscribe("ultrasonic_sensor". 72. if (ros::Time::now() > last_publish_ + ros::Duration(0. &raw). 1).0. while(ros::ok()) { linear_= angular_= 0. 42. 1. 64. l_scale_. 20. 84. signal(SIGINT.x = l_scale_*linear. 123. 131. ROS_INFO("Range: [%f]". 83. linear_). double linear) { 104. 86. } 90. ros::Timer timer = n. 89. return(0). 135. linear_ = 0. my_thread. ROS_DEBUG("Don't Move"). vel. 122. 103.79.1) { 80. 88. void NxtFollow::callback(const nxt_msgs::Range::ConstPtr& Range) { 99. void quit(int sig) { 114. boost::bind(&NxtFollow::watchdog. my_thread. boost::thread my_thread(boost::bind(&NxtFollow::controlLoop. char** argv) { 120. 126. 134. 81. 137. 98. 128. 130. 119.z = a_scale_*angular. } . NxtFollow nxt_follow. geometry_msgs::Twist vel.0. 94. angular_ = 0. } 112. 92. ros::init(argc. 106. int main(int argc. 82. ros::NodeHandle n. 110.linear. 109. 125. return.join(). } 102. Range->range).0. exit(0). tcsetattr(kfd. &nxt _follow)).createTimer(ros::Duration(0. TCSANOW. } 118. ros::spin(). 96. 115.publish(vel). 100. ROS_INFO("Not Moving"). &nxt_follow)). void NxtFollow::publish(double angular. 124.angular. ROS_DEBUG("Move away from the Object"). } 95. 116. 129. 132. "nxt_follow"). 111. boost::mutex::scoped_lock lock(publish_mutex_). vel_pub_. publish(angular_. 105. dist_from_obj_ = Range->range.interrupt(). 127. angular_ = 0. 91. ros::shutdown(). 101. &cooked). last_publish_ = ros::Time::now(). linear_ = -1. 87. 117. 93. ROS_INFO("Moving away").0. 107. 113.0. 136. 121. } 97. argv.1). 108. } else { 85. } else if (dist_from_obj_ < 0. quit). vel. 133. Code Breakdown Now let’s go through the code and see what’s happening The header files 1. public: .hpp both from the boost C++ Libraries are used for multithreading. A signal is a limited form of inter-process communication used in Unix. we wrote used simple function to perform our task. 6. geometry_msgs provides messages from common geometric primitives. We will need this to extract the range (distance) returned by our ultrasonic sensor.h required to handle signals during the execution of the program. termios.h functions describe a general terminal interface that is provided to control asynchronous communications ports. 8.h> <geometry_msgs/Twist. stdio. It is an asynchronous notification sent to a process within a process in order to notify it of an event that occurred. class NxtFollow { 11.h> "boost/thread/mutex. NxtFollow Class definition The following block code is the definition of the class NxtFollow. 7.h> <nxt_msgs/Range.h> <signal. #include #include #include #include #include #include #include #include <ros/ros. signal. They can be viewed as Interrupts Events. 5.h> <termios. 2.hpp" "boost/thread/thread. Twist.h> <stdio. 4. In the chatter example we did before.h is a convenience include that includes all the headers necessary to use the most common public pieces of the ROS system.h is the message type published by the ultrasonic sensor. For this example we will see how to write a Node using a class 10. nxt_msgs/Range.hpp and thread. All the methods and data members needed to make our robot achieve its task will be declared here.h C++ standart I/O Library mutex.hpp" ros/ros. 3.h expresses the velocity in free space broken into its linear and angular parts. param("scale_linear". //Method will be used to publish velocity 30. and when it goes out of scope. double linear_. { 38. ROS will call the callback() function whenever a new message arrives. . }.12. it will automatically unadvertise. ros::Time last_publish_. 19. It subscribes to the topic “ultrasonic_sensor”. Here we set the linear and angular composite of the velocity to 0 33. 35. //Publisher – Will publish velocities 24. 1. 27. 41. double dist_from_obj_. we will start throwing away old messages as new ones arrive. boost::mutex publish_mutex_. //Will hold the distance from the object 22.0) 37. This is the function in charge of making the XML/RPC call to the ROS Master advertising geometry_msgs::Twist on the topic named “cmd_vel” nh_. if the queue reaches 1 message. 1). 44.advertise() will return a ros::Publisher object. vel_pub_ = nh_. ros::NodeHandle nh_. ros::Subscriber range_sub_. l_scale_. 13. l_scale_).0).subscribe() makes an XML/RPC call to the ROS Master. NxtFollow::NxtFollow() : linear_(0). range_sub_ = nh_. // The amount to scale the keyboard input for the command //velocity output. void callback(const nxt_msgs::Range::ConstPtr& Range). The constructor The constructor will be call when a new NxtFollow object will be created to initialize the data members and parameters to default values. this). 28. l_scale_(1.subscribe("ultrasonic_sensor". 21. //Node Handles 18.param("scale_angular". which contains a publish() method that will let us publish messages onto the cmd_vel topic. we need to pass in a 4th parameter which is the address of the Node. 16. &NxtFollow::callback. 40. private: 17. a_scale_). //Subscriber – Will subscribe to ultrasonic sensor 25. //Class Constructor void controlLoop(). ph_. double). //ControlLoop this where we will check distance and decide to //move the robot or not 14. 29. in case we are not able to process messages fast enough. NxtFollow(). void publish(double. //Callback 31. void watchdog().advertise<geometry_msgs::Twist>("cmd_vel". 42. //Linear and angular composite of the velocity 20. } nh_. ros::Publisher vel_pub_. angular_(0). 23. ph_. 39. a_scale_(1. angular_. a_scale_. ph_. 26. The second argument is the queue size. a_scale_. Since we are using a class. 43. 15. In this case. double l_scale_. 34. 36. sizeof(struct termios)). last_publish_ = ros::Time::now(). ROS_INFO("controlLoop"). } 90. ROS_INFO("Not Moving"). 67. linear_). 81. linear_ = -1. puts("Let me follow a box"). linear_ = 1.0. 72.c_cc[VEOL] = 1. We assign the values to linear_ and angular_ variable then at the end of the loop we make a call to the publish() to publish to the topic cmd_vel. boost::mutex::scoped_lock lock(publish_mutex_). memcpy(&raw. } The while loop will execute until Ctrl-C is pressed. } else if (dist_from_obj_ < 0. tcgetattr(kfd. 73.The control loop 55. 82. 88. puts("---------------------------"). 76.1) { 80.c_lflag &=~ (ICANON | ECHO). c). angular_ = 0. 96. ROS_DEBUG("Move away from the Object"). 74. tcsetattr(kfd. TCSANOW. 69. ROS_INFO("Moving away"). 59.0. ROS_DEBUG("Follow the Object").0.0) { 75. then decide whether we want the robot to move towards the object. raw. void NxtFollow::controlLoop() { 56. angular_ = 0. linear_= angular_= 0. 64. 77. angular_ = 0. This is where we check for the distance from the object. 91. 65. 84. } 95. 66. 57. 89. while(ros::ok()) { 70. ROS_INFO("Following"). 86. raw. move away from it or not move at all. 71. char c. 83.3 && dist_from_obj_ <= 1.0. 62. publish(angular_. 78. ROS_DEBUG("Don't Move"). 61. 68. ROS_DEBUG("value: 0x%02X\n".c_cc[VEOF] = 2. 94.0. 79. 92. puts("Place an object in front of the nxt to start"). which will cause the program to stop. if (dist_from_obj_ > 0. &raw). &cooked.0. . &cooked). 60. raw. 63. 58. linear_ = 0. 87. 93. } else { 85. ros::NodeHandle n.z = a_scale_*angular. geometry_msgs::Twist vel.angular. ros::init(argc. dist_from_obj_ = Range->range. &nxt_fo llow)). 101. ROS_INFO("Range: [%f]". 123. Then we extract the range and assign it to the variable dist_from_obj that is used in the control loop to decide on the robot’s move. 134.linear. void NxtFollow::publish(double angular. The publish method 103. 111. //Initialize ROS 121. 109.interrupt(). //NodeHandle 124. return(0). 137.The callback method 98. 133. char** argv) { 120. 135. //Instanciate an NxtFollow object 122.x = l_scale_*linear. ros::spin(). 128. void NxtFollow::callback(const nxt_msgs::Range::ConstPtr& Range) { 99. 130. vel_pub_. 125. 108. int main(int argc. my_thread. signal(SIGINT. vel. 132. quit() is called 126. quit). boost::thread my_thread(boost::bind(&NxtFollow::controlLoop. 110. 106. Range->range). 131.publish(vel). return.createTimer(ros::Duration(0. } The publish() gets called in the control loop to publish the velocity to the cmd_vel. //When Ctrl-C signal is sent from keyboard. 129. 100. "nxt_follow"). The main 119. &nxt_follow)). NxtFollow nxt_follow. boost::bind(&NxtFollow::watchdog. ros::Timer timer = n. vel. double linear) { 104. } In the main we create instance of the NxtFollow class that will subscribe to ultrasonic_sensor topic and publish cmd_vel topic . argv. } The callback() method gets called everytime we receive a message on the subscribed topic. my_thread. 105.1). 127. 107.join(). 136. And if you place an object in front of you robot it should start following it.Build and Run the Execute the Node First we need to edit CMakeLists. run the following programs $ roscore $ rosrun nxt_robot_sensor_car robot.launch $ rosrun nxt_follow_obj follow_obj You should see the following results. $ rosmake nxt_follow_obj In separate terminal windows. which by default go into the ‘bin’ directory.txt from our package.launch . $ gedit nxt_follow_obj/CMakeLists. Save and exit.cpp) This will create the executable for follow_obj. Now let’s build our package.txt Then add the following line to the file Rosbuild_add_executable(follow_obj src/follow_obj. Open the file with the following command from the terminal. You should see this after starting robot. . we have our follow_obj running and continuously printing the state of the robot.follow_obj program running In the previous windows. ros.org/wiki/nxt_robot_sensor_car?distro=electric .com/ http://www.org/wiki/roscpp/Overview/NodeHandles NXT ROS http://www.ros.3.learncpp.org/wiki/roscpp http://www.org http://www.com/doc/tutorial/ ROS Wiki http://www.ros. Useful Links Learn C++ http://www.cplusplus.ros.org/wiki/roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks http://www.ros.ros.org/wiki/Robots/NXT/ http://www.