intro to ros

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

Introduction

This document is an attempt to give an overview of ROS to robotic hobbyist. While we cant

hope to provide you with a full coverage of ROS capabilities, we present some of the most

important key concepts and in the second part, go through a practical usage with the NXT

Robot platform. You are in the meantime invited to visit the official documentation

maintained by the developer of ROS, where you will find number of tutorials, at

http://www.ros.org

1. ROS Basics

What is ROS?

ROS (Robot Operating System) is an open-source, meta-operating system for your robot. It

provides libraries and tools to help software developers create robot application. It also

provide hardware abstraction, device drivers, visualizers, message-passing, package

management, and more. 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.

ROS Distributions

ROS releases might not be compatible with other. They are often referred to by code name

rather than version number. The major releases so far are listed in the table below:

ROS Distributions Release Date Wiki Page

ROS Groovy December, 2012 http://ros.org/wiki/groovy

ROS Fuerte April, 2012 http://ros.org/wiki/fuerte

ROS Electric August, 2011 http://ros.org/wiki/electric

ROS Diamondback March, 2011 http://ros.org/wiki/diamondback

ROS C Turtle August, 2010 http://ros.org/wiki/cturtle

ROS Box Turtle March, 2010 http://ros.org/wiki/boxturtle

ROS currently runs on Unix-based platform. Software for ROS is primarily tested on Ubuntu

and Mac OS X systems. The ROS community has been contributing support for Fedora, Arch

Linux, and other Linux platforms. A Port to Microsoft Windows is currently under

development and still at an experimental stage.

It is highly recommended to install ROS on Ubuntu. Go to the following link for installation

guide.

http://www.ros.org/wiki/ROS/Installation

Where is it used?

From small differential-drive robots to mobile manipulators to autonomous cars, robots of

every size and shape are using ROS to do interesting research and applications

development. Groups around the world are also releasing free, open-source software to get

you started on your own robot. Find a list of robots using ROS at:

http://www.ros.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. The robot is connected to a PC running ROS, and is connected via

USB. On the ROS software layout, we have a Node for our Robot, robot-node that is set to

receive velocities on cmd_vel Topic. The robot Node also creates a Topic for all the sensors

available, on which it publishes their respective values. On the other hand we have a control

node which in this case receives distance values from the Ultrasonic sensor on Topic

ultrasonic. Control node then publishes velocities on Topic cmd_vel.

The ROS Master here plays a very essential role in the system. When systems starts every

Nodes needs to present themselves to the Master and specify the topic they will be

subscribing or publishing to. 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.

Assume we want to add a camera to our system, in order to capture image and move the

robot based on what we receive. 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

Key concepts and Terminology

Before we dive into using ROS it is important for use to understand its terminology and

structure. ROS has three levels of concepts: the Filesystem level, the Computational Graph

level, and the Community level. A brief description of each concept and will give more details

in later sections of this document.

ROS Filesystem

Packages

Packages are the main unit for organizing software in ROS.

A package contain ROS runtime processes (Nodes),

configurations files, build files, launch files, datasets or

anything that is usefully organised together. A Package can

contain any number of nodes, and all the code in a package

should be related.

Manifests

Manifests (manifest.xml) provide metadata about the

package, including license information and dependencies as

well as language-specific information such as compiler flags.

Stacks

Stacks are a collection of packages that provide aggregate functionality. As an example the

Navigation Stack contains all the necessary packages for finding where the robot is and how

it can get somewhere else. ROS software are also released in a form of Stacks and have

associated version numbers.

Stack Manifests

Stack manifest (stack.xml) provide data about a stack, including its license information and

its dependencies on other stacks.

Message Types

Messages descriptions, store in my_package/msg/MyMessageType.msg, define the data

structures for messages sent in ROS.

Services types

Services descriptions, stored in my_package/srv/MyServicesType.srv, define the request and

response data structures for services in ROS.

ROS Computation Graph Level

This is peer-to-peer network of ROS processes that are processing data together. The basic

Computation Graph concepts of ROS are nodes, Master, Parameter Server, messages,

services, topics, and bags all of which provide data to the Graph in different ways.

Nodes

A Node is a process that performs computation. A robot will control system is typically

composed of many nodes. For example, one node will be used to control a specific sensor,

one node to control the wheel motors, one node to perform teleoperation, one node to

perform path planning, and so on. Nodes may reside in different systems.

Master

The ROS Master provides naming, registration services and lookup to the rest of the nodes

in the ROS system. The Master is the one responsible for making sure that every ROS Nodes

find each other, exchange messages, or invoke Services.

Messages

Nodes make use of Messages to communicate with one another. A Message is a simple data

structure, containing typed fields. Standard primitive types such as integer, floating point,

Boolean, etc. are supported, as well as arrays of primitive types. Messages can include

arbitrarily nested structures and arrays (much like C structs).

Topics

Messages are routed via a transport system using publish / subscribe semantics. A node sends

out a message by publishing it to a given Topic. The Topic is a Name that is used to identify

the content of a message. Whenever a Node is interested in acquiring a certain kind of data,

it just need to subscribe to the appropriate Topic. A Topic can be seen as a Bus by which

information travel in a ROS system. Each Bus has a name, and anyone can connect to the

bus to send and receive messages as long as they are of the right type.

Services

The publish / subscribe model is a very flexible communication paradigm, but its many-to-

many, one-way transport is not appropriate for request / reply interactions, which are often

required in distributed system. Request / Reply is done via Services, which are defined by pair

of message structures, which are defined by a pair of message structures: one for the request

and one for the reply.

ROS Community Level

The ROS Community Level concepts are ROS resources that enable separate communities

to exchange software and knowledge. These resources include Distributions, Repositories,

the ROS Wiki, etc. Find out more at

http://www.ros.org/wiki/ROS/Concepts#ROS_Community_Level

Installing and Configuring your ROS Environment

Go to the following link for installation guide for Linux Ubuntu, and instructions on how to

setup your environment variables.

http://www.ros.org/wiki/groovy/Installation/Ubuntu

After successfully installing and configuring ROS, we can now create our workspace. When

working with ROS source code, it is often useful to do so in a "workspace". We are going to

see how to do so in the next section.

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/ (replace

with the name of the distribution installed on your system):

$ rosws init ~/ros_workspace /opt/ros/

Next we create a sandbox directory where we will store packages created during the tutorial.

New packages need to be put in a path that is in the variable ROS_PACKAGE_PATH. All

directory managed by rosws are automatically added to the ROS_PACKAGE_PATH.

$ mkdir ~/ros_workspace/sandbox

$ rosws set ~/ros_workspace/sandbox

This will create a sandbox folder in our workspace. Next we need to re-source

~/ros_workspace/setup.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++.

Create a ROS Package

To do so open a new terminal windows. Move to the sandbox directory.

$ cd ~/ros_workspace/sandbox

Then create the package by entering the following command

$ 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.

For our tutorial we will type in the following. 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.

Writing our first ROS node in C++

The code we are going to use for our first example are from ROS tutorials

http://www.ros.org/wiki/ROS/Tutorials

Take time to go through these tutorials on your own.

In this first example we will create two nodes, talker and listener. They will both use the topic

chatter to talk to each other. Talker will publish messages on chatter, 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, talker and listener. They will both use the topic

chatter to talk to each other. Talker will publish messages on chatter, and listener on the

other hand will subscribe to chatter in other to listen to what talker has to say.

The code

First create the src/talker.cpp file within the ros_intro package and write the code provided

below.

$ roscd ros_intro

$ gedit src/talker.cpp

C++ Code - talker.cpp

1. #include "ros/ros.h"

2. #include "std_msgs/String.h"

3. #include

4.

5. int main(int argc, char **argv){

6. ros::init(argc, argv, "talker");

7. ros::NodeHandle n;

8. ros::Publisher chatter_pub = n.advertise("chatter", 1000);

9. ros::Rate loop_rate(10);

10. int count = 0;

11.

12. while (ros::ok()){

13. std_msgs::String msg;

14. std::stringstream ss;

15. ss

1. ros::Publisher chatter_pub = n.advertise("chatter", 1000);

2. ros::Rate loop_rate(10);

NodeHandle::advertise() returns a ros::Publisher object, which serves two purposes: 1) it

contains a publish() method that lets you publish messages onto the topic it was created

with, and 2) when it goes out of scope, it will automatically unadvertise. 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. It will keep

track of how long it has been since the last call to Rate::sleep(), and sleep for the correct

amount of time. In this case it maintain the frequency of publishing at 10 Hz.

10. int count = 0;

11.

12. while (ros::ok()){

count here will just be used to keep track of the number of messages transmitted. Its value

will be attached to the string message that is published.

By default roscpp will install a SIGINT handler which provides Ctrl-C handling which will

cause ros::ok() to return false if that happens.

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.

all Ros::NodeHandle have been destroyed

Once ros::ok() returns false, all ROS calls will fail.

13. std_msgs::String msg;

14. std::stringstream ss;

15. ss

20. loop_rate.sleep();

21. ++count;

Calling the ros::spinOnce() in this example is note really necessary, because we are not

subscribing to any topic. The callbacks for receiving messages on those topics are not called

immediately, instead they are placed in a queue which is processed when a call to

ros::spinOnce() is made. But it is good practice to always have it in a program.

ros::Rate::sleep() allows us here to keep a particular publishing frequency

count gets incremented to keep track of messages.

To sum up, here is a condensed version of whats 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.

The Listener

The code

First create the src/listener.cpp file within the ros_intro package and write the code provided

below.

$ roscd ros_intro

$ gedit src/listener.cpp

C++ Code - listener.cpp



3.

4. void chatterCallback(const std_msgs::String::ConstPtr& msg){

5. ROS_INFO("I heard: [%s]", msg->data.c_str());

6. }

7.


9. ros::init(argc, argv, "listener");


11. ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

12. ros::spin();

13.

14. return 0;

15. }

Code Breakdown



3.

4. void chatterCallback(const std_msgs::String::ConstPtr& msg){

5. ROS_INFO("I heard: [%s]", msg->data.c_str());

6. }

We include the same headers as before.

chatterCallback() is the function we defined that gets called whenever we receive a message

on the subscribed topic. This is where we retrieve the content of the message for display as

we did in this case, or we could store it in a variable for later processing.


9. ros::init(argc, argv, "listener");


11. ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

12. ros::spin();

13.

14. return 0;

15. }

ros::NodeHandle::subscribe makes an XML/RPC call to the ROS Master. It subscribes to the

topic chatter. The second argument is the queue size, in case we are not able to process

messages fast enough. In this case, if the queue reaches 1000 messages, we will start

throwing away old messages as new ones arrive. ROS will call the chatterCallback() function

whenever a new message arrives.

NodeHandle::subscribe() returns a ros::Subscriber object, that should be hold until you want

to unsubscribe. When the Subscriber object is destructed, it will automatically unsubscribe

from the chatter topic.

ros::spin() loops around calling message callbacks as fast as possible. It will exit once ros::ok()

returns false, meaning ros::shutdown() has been called, either by the default Ctrl-C handler,

or it being called manually.

In sum, here is a condensed version of whats happening:

Initialize the ROS system

Subscribe to the chatter topic

Spin, waiting for messages to arrive

When a message arrives, the chatterCallback() function is called.

Building and executing the nodes

First we need to edit CMakeLists.txt from our package. Open the file with the following

command from the terminal.

$ gedit ros_intro/CMakeLists.txt

Then add the following 2 lines to the file

Rosbuild_add_executable(talker src/talker.cpp)

Rosbuild_add_executable(listener src/listener.cpp)

This will create two executables, talker and listener, which by default go into the bin

directory. Save and exit.

For more information on using CMake with ROS, check CMakeLists

Now lets build our package.

$ rosmake ros_intro

In separate terminal windows, run the following programs

$ roscore

$ rosrun ros_intro talker

$ rosrun ros_intro listener

You should see the following results

Talker Node

Listener Node

2. NXT Robot and ROS

There is quite a range of robots being used with ROS at this day. ROS has quite become a

great tools for hobbyist and researchers. For this second part we will apply some of the

concept we learned to a physical robot. We will be using the NXT Lego Robot. A ROS stack

is available for the NXT. It has basic interfaces for interacting with ROS and NXT.

The bridge between NXT and ROS is the nxt_ros package. This package creates a ROS topic

for each NXT sensor, and publish the sensors data on this topic. It also creates topic for each

NXT motor, which allows for the command of motors from ROS. The nxt_ros bindings talks

directly to the NXT brick, either over USB or Bluetooth.

Installation and Configurations

The following will require you to have a PC running Ubuntu 10.04 or newer. I will be using

Ubuntu 11.10.

Configure the computer

In order for your computer to talk to the NXT brick you need to set your udev1 rules. 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

Replace in the previous command with yours. Next create a udev rules file for

the lego group that you just created.

$ echo "BUS==\"usb\", ATTRS{idVendor}==\"0694\", GROUP=\"lego\", MODE=\"0660\"" > /tmp/70-

lego.rules && sudo mv /tmp/70-lego.rules /etc/udev/rules.d/70-lego.rules

Now restart the udev:

$ sudo restart udev

To complete the configuration, log out and log back in.

1 For more information on udev visit http://en.wikipedia.org/wiki/Udev

Installing ROS and NXT ROS

Before you can install ROS on your computer, you have to configure the Ubuntu repositories

to allow restricted, universe, and multiverse

Once that is done open a terminal window and setup up your computer to accept software

from ROS.org

$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu oneiric main" >

/etc/apt/sources.list.d/ros-latest.list'

This is for Ubuntu 11.10. If you are running a different version please refers to this page1 to

get the correct command.

Next set up your Keys

$ wget http://packages.ros.org/ros.key -O - | sudo apt-key add -

Installation of ROS Electric. We will get the Desktop-Full Install which includes ROS packages,

robot-generic libraries, 2D/3D simulators, navigation and 2D/3D perception.

$ sudo apt-get update

$ sudo apt-get install ros-electric-desktop-full

The installation will take quite a while. You need to make sure your internet connection is

always on. Once ROS installation is complete. Set up your environment variables:

$ echo source /opt/ros/electric/setup.bash >> ~/.bashrc

. ~/.bashrc

Now that the installation of ROS is complete, you can install the NXT-ROS bindings:

1 http://www.ros.org/wiki/Robots/NXT/electric#Installation_Instructions

$ 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.

Lets Try It!

IMPORTANT: For NXT-ROS work with your Mindstorms, you need to have the firmware 1.28 or higher. If you do not know

how to upgrade, you can follow the following tutorial: Updating NXT Firmware.

Make sure you have your NXT ready and connected to your PC. We are now going to run a

program to test if the NXT can indeed talk to ROS. For that connect a touch sensor to Port

1 of you NXT. Then in a terminal window, type the following

$ roscore

Then in another terminal, run the following

$ rosrun nxt_python touch_sensor_test.py

You should get the following results in your terminal. When press the touch sensor, it will

print True and False when released.

Testing the touch sensor

If instead you see the following, then you probably didnt set something right or the Brick

is not connected or powered on.

NXT Brick not found

If you got the touch sensor test right, we can now write a small application to run our NXT

using the robot car sensor1 model provided with the ROS package. If you like you can go

through this tutorial2 to create your own robot for ROS.

Teleoperate the NXT using Keyboard

We will now walk through the steps to control the NXT using your keyboard. The following

diagram, will attempt to present some of the important nodes used to achieve that, and

how they inter connect

1 Robot car Sensor - http://www.ros.org/wiki/nxt_robot_sensor_car 2 Create your own robot model - http://www.ros.org/wiki/nxt/Tutorials

Inside the nxt_robot_car_sensor package, there a file robot.yaml which is a description file

for the robot, where we need to specify the parameters for the sensors, motors, and define

on which port they will be physically connected.

On my PC the file is located at the following location.

/opt/ros/electric/stacks/nxt_robots/nxt_robot_sensor_car

You will have to edit this file in order to match your robot configuration. The initial file

looks like this.

robot.yaml

1. nxt_robot:

2. - type: motor

3. name: r_wheel_joint

4. port: PORT_A

5. desired_frequency: 20.0

6.

7. - type: motor

8. name: l_wheel_joint

9. port: PORT_B


11.

12. - type: motor

13. name: m_wheel_joint

14. port: PORT_C


16.

17. - type: gyro

18. name: gyro

19. frame_id: gyro_link

20. port: PORT_3

21. offset: 0


23.

24. - type: ultrasonic

25. frame_id: ultrasonic_link

26. name: ultrasonic_sensor

27. port: PORT_2

28. spread_angle: 0.2

29. min_range: 0.01

30. max_range: 2.5


32.

33. - type: color

34. frame_id: color_link

35. name: color_sensor

36. port: PORT_1


This file defines a robot with a gyroscope, ultrasonic sensor, color sensor, and 3 motors. The

one I will be using only make use of 2 motors (PORT A and B) and an ultrasonic sensor

(PORT 4). I will then have to edit this file so it looks like this.

robot.yaml

1. nxt_robot:

2. - type: motor

3. name: r_wheel_joint

4. port: PORT_A


6.

7. - type: motor

8. name: l_wheel_joint

9. port: PORT_B


11.

12. - type: ultrasonic

13. frame_id: ultrasonic_link

14. name: ultrasonic_sensor

15. port: PORT_4

16. spread_angle: 0.2

17. min_range: 0.01

18. max_range: 2.5


20.

Feel free to add any sensors you might have to the robot.yaml file. After editing the file we

now have to build the package again from the terminal for our system to consider the

changes.

$ rosmake nxt_robot_sensor_car

Once thats done, you can now run the robot with the following command.

$ roscore

First start the ROS Master with roscore. Always make sure its running before you try to

execute a ROS process. Now execute the following from another terminal

$ roslaunch nxt_robot_sensor_car robot.launch

Keep this program running, then open a new terminal window and run the control program.

$ roslaunch nxt_teleop teleop_keyboard.launch

You should have a prompt on your screen instructing you to use the direction keys to move

the robot. If you will like to see a graph showing all the running nodes and their connections

run the following command

$ rxgraph

And you should get this

You can also use a joystick to control your robot. Instead of the teleop_keyboard.launch, you

will just need to launch the teleop_joy.launch

$ roslaunch nxt_teleop teleop_joy.launch

Follow Objects with your NXT and ROS

Now that we can control our NXT with keyboard/joystick lets have our robot follow an object

place in front of it. Basically what we want to achieve here is read the distance provided by

the ultrasonic sensor. When the object is within a certain range of the robot and moving

away, the robot should move forward, following the object. If the object is too close of the

robot, it should back up (move backward). When the object stops moving the robot should

also stop within a given range.

In order to achieve that we will have to write a node that will subscribe to the topic

ultrasonic_sensor, in order to read distance. Then based on the distance it receives will publish

velocities on topic cmd_vel, to control the movements of the robot.

We will create a new package in the sandbox folder of our workspace (see section 1.5).

$ roscreate-pkg nxt_follow_obj nxt_ros geometry_msgs nxt_msgs

This package will depend on the nxt_ros package, the geometry_msgs, and nxt_msgs.

Next we need to write the code for the node.

The code

Create the src/follow_obj.cpp file within the nxt_follow_obj package and write the code

provided below.

C++ Code follow_obj.cpp

1. #include

2. #include

3. #include

4. #include

5. #include

6. #include

7. #include "boost/thread/mutex.hpp"

8. #include "boost/thread/thread.hpp"

9.

10. class NxtFollow {

11. public:

12. NxtFollow();

13. void controlLoop();

14. void watchdog();

15.

16. private:

17. ros::NodeHandle nh_, ph_;

18.

19. double linear_, angular_;

20. double l_scale_, a_scale_;

21. double dist_from_obj_;

22.

23. ros::Publisher vel_pub_;

24. ros::Subscriber range_sub_;

25. ros::Time last_publish_;

26.

27. boost::mutex publish_mutex_;

28.

29. void publish(double, double);

30. void callback(const nxt_msgs::Range::ConstPtr& Range);

31. };

32.

33. NxtFollow::NxtFollow() : linear_(0),

34. angular_(0),

35. l_scale_(1.0),

36. a_scale_(1.0)

37. {

38. ph_.param("scale_angular", a_scale_, a_scale_);

39. ph_.param("scale_linear", l_scale_, l_scale_);

40.

41. vel_pub_ = nh_.advertise("cmd_vel", 1);

42.

43. range_sub_ = nh_.subscribe("ultrasonic_sensor", 1, &NxtFollow::callback, this);

44. }

45.

46. int kfd = 0;

47. struct termios cooked, raw;

48.

49. void NxtFollow::watchdog() {

50. boost::mutex::scoped_lock lock(publish_mutex_);

51. if (ros::Time::now() > last_publish_ + ros::Duration(0.15))

52. publish(0, 0);

53. }

54.

55. void NxtFollow::controlLoop() {

56. char c;

57. tcgetattr(kfd, &cooked);

58. memcpy(&raw, &cooked, sizeof(struct termios));

59. raw.c_lflag &=~ (ICANON | ECHO);

60.

61. raw.c_cc[VEOL] = 1;

62. raw.c_cc[VEOF] = 2;

63. tcsetattr(kfd, TCSANOW, &raw);

64.

65. puts("Let me follow a box");

66. puts("---------------------------");

67. puts("Place an object in front of the nxt to start");

68.

69. while(ros::ok()) {

70. linear_= angular_= 0;

71. ROS_DEBUG("value: 0x%02X\n", c);

72. ROS_INFO("controlLoop");

73.

74. if (dist_from_obj_ > 0.3 && dist_from_obj_

79. } else if (dist_from_obj_ < 0.1) {

80. ROS_INFO("Moving away");

81. ROS_DEBUG("Move away from the Object");

82. linear_ = -1.0;

83. angular_ = 0.0;

84. } else {

85. ROS_INFO("Not Moving");

86. ROS_DEBUG("Don't Move");

87. linear_ = 0.0;

88. angular_ = 0.0;

89. }

90.

91. boost::mutex::scoped_lock lock(publish_mutex_);

92. last_publish_ = ros::Time::now();

93. publish(angular_, linear_);

94. }

95.

96. }

97.

98. void NxtFollow::callback(const nxt_msgs::Range::ConstPtr& Range) {

99. ROS_INFO("Range: [%f]", Range->range);

100. dist_from_obj_ = Range->range;

101. }

102.

103. void NxtFollow::publish(double angular, double linear) {

104. geometry_msgs::Twist vel;

105. vel.angular.z = a_scale_*angular;

106. vel.linear.x = l_scale_*linear;

107.

108. vel_pub_.publish(vel);

109.

110. return;

111. }

112.

113. void quit(int sig) {

114. tcsetattr(kfd, TCSANOW, &cooked);

115. ros::shutdown();

116. exit(0);

117. }

118.

119. int main(int argc, char** argv) {

120. ros::init(argc, argv, "nxt_follow");

121. NxtFollow nxt_follow;

122.


124.

125. signal(SIGINT, quit);

126.

127. boost::thread my_thread(boost::bind(&NxtFollow::controlLoop, &nxt_follow));

128.

129. ros::Timer timer = n.createTimer(ros::Duration(0.1), boost::bind(&NxtFollow::watchdog, &nxt

_follow));

130.

131. ros::spin();

132.

133. my_thread.interrupt();

134. my_thread.join();

135.

136. return(0);

137. }

Code Breakdown

Now lets go through the code and see whats happening

The header files

1. #include

2. #include

3. #include

4. #include

5. #include

6. #include

7. #include "boost/thread/mutex.hpp"

8. #include "boost/thread/thread.hpp"

ros/ros.h is a convenience include that includes all the headers necessary to use the most

common public pieces of the ROS system.

geometry_msgs provides messages from common geometric primitives. Twist.h expresses

the velocity in free space broken into its linear and angular parts.

nxt_msgs/Range.h is the message type published by the ultrasonic sensor. We will need this

to extract the range (distance) returned by our ultrasonic sensor.

signal.h required to handle signals during the execution of the program. A signal is a limited

form of inter-process communication used in Unix. It is an asynchronous notification sent to

a process within a process in order to notify it of an event that occurred. They can be viewed

as Interrupts Events.

termios.h functions describe a general terminal interface that is provided to control

asynchronous communications ports.

stdio.h C++ standart I/O Library

mutex.hpp and thread.hpp both from the boost C++ Libraries are used for multithreading.

NxtFollow Class definition

The following block code is the definition of the class NxtFollow. All the methods and data

members needed to make our robot achieve its task will be declared here. In the chatter

example we did before, we wrote used simple function to perform our task. For this example

we will see how to write a Node using a class

10. class NxtFollow {

11. public:

12. NxtFollow(); //Class Constructor

13. void controlLoop(); //ControlLoop this where we will check distance and decide to

//move the robot or not

14. void watchdog();

15.

16. private:

17. ros::NodeHandle nh_, ph_; //Node Handles

18.

19. double linear_, angular_; //Linear and angular composite of the velocity

20. double l_scale_, a_scale_; // The amount to scale the keyboard input for the command //velocity output.

21. double dist_from_obj_; //Will hold the distance from the object

22.

23. ros::Publisher vel_pub_; //Publisher Will publish velocities

24. ros::Subscriber range_sub_; //Subscriber Will subscribe to ultrasonic sensor

25. ros::Time last_publish_;

26.

27. boost::mutex publish_mutex_;

28.

29. void publish(double, double); //Method will be used to publish velocity

30. void callback(const nxt_msgs::Range::ConstPtr& Range); //Callback

31. };

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.

Here we set the linear and angular composite of the velocity to 0

33. NxtFollow::NxtFollow() : linear_(0),

34. angular_(0),

35. l_scale_(1.0),

36. a_scale_(1.0)

37. {

38. ph_.param("scale_angular", a_scale_, a_scale_);

39. ph_.param("scale_linear", l_scale_, l_scale_);

40.

41. vel_pub_ = nh_.advertise("cmd_vel", 1);

42.

43. range_sub_ = nh_.subscribe("ultrasonic_sensor", 1, &NxtFollow::callback, this);

44. }

nh_.advertise() will return a ros::Publisher object, which contains a publish() method that will

let us publish messages onto the cmd_vel topic, and when it goes out of scope, it will

automatically unadvertise. 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_.subscribe() makes an XML/RPC call to the ROS Master. It subscribes to the topic

ultrasonic_sensor. The second argument is the queue size, in case we are not able to

process messages fast enough. In this case, if the queue reaches 1 message, we will start

throwing away old messages as new ones arrive. ROS will call the callback() function

whenever a new message arrives. Since we are using a class, we need to pass in a 4th

parameter which is the address of the Node.

The control loop

55. void NxtFollow::controlLoop() {

56. char c;

57. tcgetattr(kfd, &cooked);

58. memcpy(&raw, &cooked, sizeof(struct termios));

59. raw.c_lflag &=~ (ICANON | ECHO);

60.

61. raw.c_cc[VEOL] = 1;

62. raw.c_cc[VEOF] = 2;

63. tcsetattr(kfd, TCSANOW, &raw);

64.

65. puts("Let me follow a box");

66. puts("---------------------------");

67. puts("Place an object in front of the nxt to start");

68.

69. while(ros::ok()) {

70. linear_= angular_= 0;

71. ROS_DEBUG("value: 0x%02X\n", c);

72. ROS_INFO("controlLoop");

73.

74. if (dist_from_obj_ > 0.3 && dist_from_obj_

The callback method

98. void NxtFollow::callback(const nxt_msgs::Range::ConstPtr& Range) {

99. ROS_INFO("Range: [%f]", Range->range);

100. dist_from_obj_ = Range->range;

101. }

The callback() method gets called everytime we receive a message on the subscribed topic.

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 robots move.

The publish method

103. void NxtFollow::publish(double angular, double linear) {

104. geometry_msgs::Twist vel;

105. vel.angular.z = a_scale_*angular;

106. vel.linear.x = l_scale_*linear;

107.

108. vel_pub_.publish(vel);

109.

110. return;

111. }

The publish() gets called in the control loop to publish the velocity to the cmd_vel.

The main

119. int main(int argc, char** argv) {

120. ros::init(argc, argv, "nxt_follow"); //Initialize ROS

121. NxtFollow nxt_follow; //Instanciate an NxtFollow object

122.

123. ros::NodeHandle n; //NodeHandle

124.

125. signal(SIGINT, quit); //When Ctrl-C signal is sent from keyboard, quit() is called

126.

127. boost::thread my_thread(boost::bind(&NxtFollow::controlLoop, &nxt_follow));

128.

129. ros::Timer timer = n.createTimer(ros::Duration(0.1), boost::bind(&NxtFollow::watchdog, &nxt_fo

llow));

130.

131. ros::spin();

132.

133. my_thread.interrupt();

134. my_thread.join();

135.

136. return(0);

137. }

In the main we create instance of the NxtFollow class that will subscribe to ultrasonic_sensor

topic and publish cmd_vel topic

Build and Run the Execute the Node

First we need to edit CMakeLists.txt from our package. Open the file with the following

command from the terminal.

$ gedit nxt_follow_obj/CMakeLists.txt

Then add the following line to the file

Rosbuild_add_executable(follow_obj src/follow_obj.cpp)

This will create the executable for follow_obj, which by default go into the bin directory. Save

and exit.

Now lets build our package.

$ rosmake nxt_follow_obj

In separate terminal windows, run the following programs

$ roscore

$ rosrun nxt_robot_sensor_car robot.launch

$ rosrun nxt_follow_obj follow_obj

You should see the following results. And if you place an object in front of you robot it

should start following it.

You should see this after starting robot.launch

follow_obj program running

In the previous windows, we have our follow_obj running and continuously printing the state

of the robot.

3. Useful Links

Learn C++

http://www.learncpp.com/

http://www.cplusplus.com/doc/tutorial/

ROS Wiki

http://www.ros.org

http://www.ros.org/wiki/roscpp

http://www.ros.org/wiki/roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks

http://www.ros.org/wiki/roscpp/Overview/NodeHandles

NXT ROS

http://www.ros.org/wiki/Robots/NXT/

http://www.ros.org/wiki/nxt_robot_sensor_car?distro=electric

intro to ros

Documents

ros robot

ros basics

ros distributions

ros filesystem

ros environment

ros workspace

ros community level

ros computation graph