ROS Multi Lesson2

Summer 2015
Multi-Robot Systems with ROS

Lesson 2
Teaching Assistant: Roi Yehoshua

roiyeho@gmail.com
Agenda
Simulating multiple robots in Stage
Collision avoidance
Robots synchronization
Creating custom messages
(C)2015 Roi Yehoshua

ROS Stage Simulator
Stage simulates a population of mobile robots,
sensors and objects in a 2D bitmapped environment
Stage is designed to support research of multi-agent
autonomous systems, so it provides fairly simple,
computationally cheap models of lots of devices
rather than attempting to emulate any device with
great fidelity
Multi-Robot Patrolling Task - Simulation in Stage/ROS

Stage World Files
The world file is a description of the world that
Stage must simulate
It describes robots, sensors, actuators, moveable
and immovable objects
Sample world files can be found at the /world
subdirectory in ros_stage package

Simulating Multiple Robots in Stage
First, you need to build a model of every robot
you need in the world file
For example, lets add 3 more robots to willow-
erratic.world
First we will copy it to the home directory and
change its name to willow_multi_erratic.world
Notice that you also need to copy willow-full.pgm
$$ cd
cd ~~
$$ cp
cp /opt/ros/indigo/share/stage_ros/world/willow-erratic.world
/opt/ros/indigo/share/stage_ros/world/willow-erratic.world willow-multi-erratic.world
willow-multi-erratic.world
$$ cp
cp /opt/ros/indigo/share/stage_ros/world/willow-full.pgm
/opt/ros/indigo/share/stage_ros/world/willow-full.pgm ..

Now add the following lines at the end of the file
## robots
robots
erratic(
erratic( pose
pose [[ -11.5
-11.5 23.5
23.5 00 00 ]] name
name "robot0"
"robot0" color
color "blue")
"blue")
erratic(
erratic( pose
pose [[ -13.5
-13.5 23.5
23.5 00 00 ]] name
name "robot1"
"robot1" color
color "red")
"red")
erratic(
erratic( pose
pose [[ -11.5
-11.5 21.5
21.5 00 00 ]] name
name "robot2"
"robot2" color
color "green")
"green")
erratic(
erratic( pose
pose [[ -13.5
-13.5 21.5
21.5 00 00 ]] name
name "robot3"
"robot3" color
color "magenta")
"magenta")
## block(
block( pose
pose [[ -13.924
-13.924 25.020
25.020 00 180.000
180.000 ]] color
color "red")
"red")
Then run again the stage simulator

rosrun
rosrun stage_ros
stage_ros stageros
stageros willow-multi-erratic.world


Each robot subscribes and publishes its own
topics

You can move one of the robots by publishing
to /robot_X/cmd_vel topic
For example, to make robot 2 move forward
type:
rostopic
rostopic pub
pub /robot_2/cmd_vel
/robot_2/cmd_vel -r
-r 10
10 geometry_msgs/Twist
geometry_msgs/Twist '{linear:
'{linear: {x:
{x: 0.2}}'
0.2}}'


Creating a ROS Package
Now we will create a ROS node that will make
one of the robots move forward
The ID of the robot will be given as a command-
line argument to the node

catkin Workspace
Make sure you have a catkin workspace ready
If you dont have, run the following commands:
$$ mkdir
mkdir catkin_ws
catkin_ws catkin_ws/src
catkin_ws/src
$$ cd
cd catkin_ws/src
catkin_ws/src
$$ catkin_init_workspace
catkin_init_workspace
$$ cd
cd ....
$$ catkin_make
catkin_make
And add the following line to your bashrc
source
source ~/catkin_ws/devel/setup.bash
~/catkin_ws/devel/setup.bash

Create a new ROS package called stage_multi
$$ cd
cd ~/catkin_ws/src
~/catkin_ws/src
$$ catkin_create_pkg
catkin_create_pkg stage_multi
stage_multi std_msgs
std_msgs rospy
rospy roscpp
roscpp
Create a world subdirectory in the package and

copy the world files into it
$$ mkdir
mkdir ~/catkin_ws/src/stage_multi/world
~/catkin_ws/src/stage_multi/world
$$ cp
cp ~/willow-multi-erratic.world
~/willow-multi-erratic.world ~/catkin_ws/src/stage_multi/world
$$ cp
cp ~/willow-full.pgm
~/willow-full.pgm ~/catkin_ws/src/stage_multi/world

Now compile the package and create an Eclipse
project file for it:
$$ cd
cd ~/catkin_ws
~/catkin_ws
$$ catkin_make
catkin_make --force-cmake
--force-cmake -G"Eclipse
-G"Eclipse CDT4
CDT4 -- Unix
Unix Makefiles"
Makefiles"

Directory Structure

Import the Project into Eclipse
Now start Eclipse
Choose catkin_ws folder as the workspace folder

Choose File --> Import --> General --> Existing
Projects into Workspace

Now choose the ~/catkin_ws/build folder

Project Structure
Open the "Source directory" within the project
so that you can edit the source code

Add New Source File
Right click on src and select New > Source File,
and create a file named move_robot.cpp

ROS Init
A version of ros::init() must be called before using
any of the rest of the ROS system
Typical call in the main() function:
ros::init(argc,
ros::init(argc, argv,
argv, "Node
"Node name");
name");
Node names must be unique in a running system

We will make the node names unique by
concatenating the robots number to the end of
the node name
For that purpose we will need to supply the
robots number as a command-line argument
move_robot.cpp (1)
#include
#include "ros/ros.h"
"ros/ros.h"
#include
#include "geometry_msgs/Twist.h"
"geometry_msgs/Twist.h"
#include
#include <string>
<string>
using
using namespace std;
namespace std;
#define
#define MAX_ROBOTS_NUM
MAX_ROBOTS_NUM 20
20
#define
#define FORWARD_SPEED
FORWARD_SPEED 0.2
0.2
int
int robot_id;
robot_id;
ros::Publisher
ros::Publisher cmd_vel_pub;
cmd_vel_pub; //
// publisher
publisher for
for movement
movement commands
commands
void
void move_forward();
move_forward();

move_robot.cpp (2)
int
int main(int
main(int argc,
argc, char
char **argv)
**argv)
{{
if
if (argc
(argc << 2)
2) {{
ROS_ERROR("You
ROS_ERROR("You must
must specify
specify robot
robot id.");
id.");
return
return -1;
-1;
}}
char
char *id
*id == argv[1];
argv[1];
robot_id
robot_id == atoi(id);
atoi(id);
//
// Check
Check that
that robot
robot id
id is
is between
between 00 and
and MAX_ROBOTS_NUM
MAX_ROBOTS_NUM
if
if (robot_id
(robot_id > MAX_ROBOTS_NUM || robot_id << 00 )) {{
> MAX_ROBOTS_NUM || robot_id
ROS_ERROR("The
ROS_ERROR("The robot's
robot's ID
ID must
must be
be an
an integer
integer number
number between
between 00 an
an 19");
19");
return
return -1;
-1;
}}
ROS_INFO("moving
ROS_INFO("moving robot
robot no.
no. %d",
%d", robot_id);
robot_id);
//
// Create
Create aa unique
unique node
node name
name
string
string node_name
node_name == "move_robot_";
"move_robot_";
node_name
node_name +=
+= id;
id;
cout
cout <<
<< node_name;
node_name;
ros::init(argc,
ros::init(argc, argv,
argv, node_name);
node_name);
ros::NodeHandle
ros::NodeHandle nh;
nh;

move_robot.cpp (3)
//
// cmd_vel_topic
cmd_vel_topic == "robot_X/cmd_vel"
"robot_X/cmd_vel"
string
string cmd_vel_topic_name
cmd_vel_topic_name == "robot_";
"robot_";
cmd_vel_topic_name
cmd_vel_topic_name +=
+= id;
id;
cmd_vel_topic_name
cmd_vel_topic_name +=
+= "/cmd_vel";
"/cmd_vel";
cmd_vel_pub
cmd_vel_pub == nh.advertise<geometry_msgs::Twist>(cmd_vel_topic_name,
nh.advertise<geometry_msgs::Twist>(cmd_vel_topic_name, 10);
10);
move_forward();
move_forward();
return
return 0;
0;
}}

move_forward function
void
void move_forward()
move_forward()
{{
//
// Drive
Drive forward
forward at
at aa given
given speed.
speed.
geometry_msgs::Twist
geometry_msgs::Twist cmd_vel;
cmd_vel;
cmd_vel.linear.x = FORWARD_SPEED;
cmd_vel.angular.z
cmd_vel.angular.z == 0.0;
0.0;
//
// Loop
Loop at
at 10Hz,
10Hz, publishing
publishing movement
movement commands
commands until
until we
we shut
shut down
down
ros::Rate
ros::Rate rate(10);
rate(10);
while
while (ros::ok())
(ros::ok()) //
// Keep
Keep spinning
spinning loop
loop until
until user
user presses
presses Ctrl+C
Ctrl+C
{{
cmd_vel_pub.publish(cmd_vel);
rate.sleep();
rate.sleep();
}}
//
// Stop
Stop the
the robot
robot
geometry_msgs::Twist stop_cmd_vel;
stop_cmd_vel;
stop_cmd_vel.linear.x
stop_cmd_vel.linear.x == 0.0;
0.0;
stop_cmd_vel.angular.z
stop_cmd_vel.angular.z == 0.0;
0.0;
cmd_vel_pub.publish(stop_cmd_vel);
cmd_vel_pub.publish(stop_cmd_vel);
ROS_INFO("robot
ROS_INFO("robot no.
no. %d
%d stopped",
stopped", robot_id);
robot_id);
}}

Compiling the Node
Before building your node, you should modify
the generated CMakeLists.txt in the package
Change the lines marked in red:
cmake_minimum_required(VERSION
cmake_minimum_required(VERSION 2.8.3) 2.8.3)
project(stage_multi)
project(stage_multi)

##
## Declare
Declare aa cpp
cpp executable
executable
add_executable(multi_sync
add_executable(multi_sync src/SyncRobots.cpp)
src/SyncRobots.cpp)
##
## Add
Add cmake
cmake target
target dependencies
dependencies of
of the
the executable/library
executable/library
##
## as
as an
an example,
example, message
message headers
headers may
may need
need to
to be
be generated
generated before
before nodes
nodes
## add_dependencies(stage_multi_node
add_dependencies(stage_multi_node stage_multi_generate_messages_cpp)
stage_multi_generate_messages_cpp)
##
## Specify
Specify libraries
libraries to
to link
link aa library
library or
or executable
executable target
target against
against
target_link_libraries(move_robot
target_link_libraries(move_robot ${catkin_LIBRARIES})
${catkin_LIBRARIES})
After changing the file call catkin_make
Running the Node Inside Eclipse
Create a new launch configuration, by clicking on
Run --> Run configurations... --> C/C++
Application (double click or click on New).
Select the correct binary on the main tab (use
the Browse button)
~/catkin_ws/devel/lib/stage_multi/move_robot
Make sure roscore and stage are running
Click Run


Setting Command Line Arguments




Launch File
Now we will create a launch file that will make all
the 4 robots move forward
We will use the args attribute in the <node> tag
to specify the command line arguments
Create a launch subdirectory in stage_multi
package
Add stage_multi.launch file to this directory and
copy the following code

Launch File
<launch>
<launch>
<node
<node name="stage"
name="stage" pkg="stage_ros"
pkg="stage_ros" type="stageros"
type="stageros" args="$(find
args="$(find
stage_multi)/world/willow-multi-erratic.world"/>
stage_multi)/world/willow-multi-erratic.world"/>
<node
<node name="move_robot_0"
name="move_robot_0" pkg="stage_multi"
pkg="stage_multi" type="move_robot"
type="move_robot" args="0"
args="0"
output="screen"/>
output="screen"/>
<node
args="1"
output="screen"/>
output="screen"/>
<node
args="2"
output="screen"/>
output="screen"/>
<node
args="3"
output="screen"/>
output="screen"/>
</launch>
</launch>
Note that the node names must be unique

Launch File
Run the launch file using the following command:
roslaunch
roslaunch stage_multi
stage_multi stage_multi.launch
stage_multi.launch

Launch File

Launch File
The robots will eventually bump into the obstacles:

Reading Sensors
Now we will make the robots move until they
sense an obstacle
Will they be able to sense each other as obstacles?
For that purpose we will use the laser data
published on the topic /base_scan
The message type used to send information of
the laser is sensor_msgs/LaserScan

move_robot.cpp (1)
#include
#include "ros/ros.h"
"ros/ros.h"
#include
#include "geometry_msgs/Twist.h"
"geometry_msgs/Twist.h"
#include
#include "sensor_msgs/LaserScan.h"
"sensor_msgs/LaserScan.h"
#include
#include <string>
<string>
using
using namespace std;
namespace std;
#define
#define MAX_ROBOTS_NUM
MAX_ROBOTS_NUM 20
20
#define
#define FORWARD_SPEED
FORWARD_SPEED 0.2
0.2
#define
#define MIN_SCAN_ANGLE
MIN_SCAN_ANGLE -60.0/180*M_PI
-60.0/180*M_PI
#define
#define MAX_SCAN_ANGLE +60.0/180*M_PI
MAX_SCAN_ANGLE +60.0/180*M_PI
#define
#define MIN_PROXIMITY_RANGE
MIN_PROXIMITY_RANGE 0.5
0.5
int
int robot_id;
robot_id;
ros::Publisher
ros::Publisher cmd_vel_pub;
cmd_vel_pub; //
// publisher
publisher for
for movement
movement commands
commands
ros::Subscriber
ros::Subscriber laser_scan_sub; // subscriber to the robot's laser
laser_scan_sub; // subscriber to the robot's laser scan
scan topic
topic
bool
bool keepMoving
keepMoving == true;
true;
void
void move_forward();
move_forward();
void
void scanCallback(const
scanCallback(const sensor_msgs::LaserScan::ConstPtr&
sensor_msgs::LaserScan::ConstPtr& scan);
scan);

move_robot.cpp (2)
int
int main(int
main(int argc,
argc, char
char **argv)
**argv)
{{
if
if (argc
(argc << 2)
2) {{
ROS_ERROR("You
ROS_ERROR("You must
must specify
specify robot
robot id.");
id.");
return -1;
return -1;
}}
char
char *id
*id == argv[1];
argv[1];
robot_id
robot_id == atoi(id);
atoi(id);
...
...
//
// subscribe
subscribe to
to robot's
robot's laser
laser scan
scan topic
topic "robot_X/base_scan"
"robot_X/base_scan"
string
string laser_scan_topic_name
laser_scan_topic_name == "robot_";
"robot_";
laser_scan_topic_name
laser_scan_topic_name +=
+= id;
id;
laser_scan_topic_name += "/base_scan";
laser_scan_topic_name += "/base_scan";
laser_scan_sub
laser_scan_sub == nh.subscribe(laser_scan_topic_name,
nh.subscribe(laser_scan_topic_name, 1,
1, &scanCallback);
&scanCallback);
move_forward();
move_forward();
return
return 0;
0;
}}

move_robot.cpp (2)
void
void move_forward()
move_forward()
{{
//
// Drive
Drive forward
forward at
at aa given
given speed.
speed.
geometry_msgs::Twist cmd_vel;
cmd_vel;
cmd_vel.angular.z
cmd_vel.angular.z == 0.0;
0.0;
//
// Loop
Loop at
at 10Hz,
10Hz, publishing
publishing movement
movement commands
commands until
until we
we shut
shut down
down
ros::Rate
ros::Rate rate(10);
rate(10);
while
while (ros::ok()
(ros::ok() &&
&& keepMoving)
keepMoving) //
// Keep
Keep spinning
spinning loop
loop until
until user
user presses
presses
Ctrl+C
Ctrl+C
{{
ros::spinOnce();
ros::spinOnce(); //
// Need
Need to
to call
call this
this function
function often
often to
to allow
allow ROS
ROS to
to
process incoming messages
process incoming messages
rate.sleep();
rate.sleep();
}}
...
...
}}

scanCallback
//
// Process
Process the
the incoming
incoming laser
laser scan
scan message
message
void
void scanCallback(const
scanCallback(const sensor_msgs::LaserScan::ConstPtr&
sensor_msgs::LaserScan::ConstPtr& scan)
scan)
{{
//
// Find
Find the
the closest
closest range
range between
between the
the defined
defined minimum
minimum and
and maximum
maximum angles
angles
int
int minIndex
minIndex == ceil((MIN_SCAN_ANGLE
ceil((MIN_SCAN_ANGLE -- scan->angle_min)
scan->angle_min) // scan-
scan-
>angle_increment);
>angle_increment);
int
int maxIndex
maxIndex == floor((MAX_SCAN_ANGLE
floor((MAX_SCAN_ANGLE -- scan->angle_min)
scan->angle_min) // scan-
scan-
>angle_increment);
>angle_increment);
float
float closestRange
closestRange == scan->ranges[minIndex];
scan->ranges[minIndex];
for
for (int
(int currIndex
currIndex == minIndex
minIndex ++ 1;
1; currIndex
currIndex <=
<= maxIndex;
maxIndex; currIndex++)
currIndex++) {{
if
if (scan->ranges[currIndex]
(scan->ranges[currIndex] << closestRange)
closestRange) {{
closestRange
closestRange == scan->ranges[currIndex];
scan->ranges[currIndex];
}}
}}
//ROS_INFO_STREAM("Closest
//ROS_INFO_STREAM("Closest range:
range: "" <<
<< closestRange);
closestRange);
if
if (closestRange < MIN_PROXIMITY_RANGE) {{
(closestRange < MIN_PROXIMITY_RANGE)
keepMoving
keepMoving == false;
false;
}}
}}

Reading Sensors

Collision Avoidance
All models in Stage (including lasers and robot bases)
have heights
A sensor will only "see" other models at the same
height
In Stage .world examples, the robots have lasers mounted on
their tops in such a way that a robot's laser only sees other
robots' lasers, not their bases
This is the correct result for such a robot configuration, since in
the real world, the laser planes don't intersect the robot bases.
If you want to simulate a different (perhaps less realistic) kind
of robot, you can adjust the laser heights

Collision Avoidance
define
define erratic
erratic position
position
((
#size
#size [0.415
[0.415 0.392
0.392 0.25]
0.25]
size
size [0.35
[0.35 0.35
0.35 0.25]
0.25]
origin
origin [-0.05
[-0.05 00 00 0]
0]
gui_nose
gui_nose 11
drive
drive "diff"
"diff"
topurg(pose
topurg(pose [[ 0.050
0.050 0.000
0.000 -0.1
-0.1 0.000
0.000 ])
])
))

Collision Avoidance

Collision Avoidance
A more difficult problem related to collision
avoidance is that when two lasers see each other
directly, they often get "dazzled", and the data
are discarded.
Laser-based obstacle avoidance among a group
of homogeneous robots is difficult
Can you think of a solution to this problem?

Robots Synchronization
One important aspect of multi-robot systems is
the need of coordination and synchronization of
behavior between robots in the team
We will now make our robots wait for each other
before they start moving

Robots Synchronization
First we will create a shared topic called team_status
Each robot when ready will publish a ready status
message to the shared topic
In addition, we will create a monitor node that will
listen for the ready messages
When all ready messages arrive, the monitor node
will publish a broadcast message that will let the
robots know that the team is ready and they can
start moving

RobotStatus Message
We will start by creating a new ROS message
type for the ready messages called RobotStatus
The structure of the message will be:
Header header
int32 robot_id
bool is_ready
The header contains a timestamp and coordinate
frame information that are commonly used in
ROS

Message Header
stamp specifies the publishing time

frame_id specifies the point of reference for data
contained in that message

Message Field Types
Field types can be:
a built-in type, such as "float32 pan" or "string
name"
names of Message descriptions defined on their
own, such as "geometry_msgs/PoseStamped"
fixed- or variable-length arrays (lists) of the above,
such as "float32[] ranges" or "Point32[10] points
the special Header type, which maps to
std_msgs/Header

Built-In Types

msg Files
msg files are simple text files that describe the
fields of a ROS message
They are used to generate source code for
messages in different languages
Stored in the msg directory of your package

Create a Message
First create a new package called multi_sync
$$ cd
cd ~/catkin_ws/src
~/catkin_ws/src
$$ catkin_create_pkg
catkin_create_pkg multi_sync
multi_sync std_msgs
std_msgs rospy
rospy roscpp
roscpp
Copy the world subdirectory from the stage_multi

package we created in the last lesson to the new
package
Now create a subdirectory msg and the file
RobotStatus.msg within it
$$ cd
cd ~/catkin_ws/src/multi_sync
~/catkin_ws/src/multi_sync
$$ mkdir
mkdir msg
msg
$$ gedit
gedit msg/RobotStatus.msg
msg/RobotStatus.msg

Create a Message Type
Add the following lines to RobotStatus.msg:
Header
Header header
header
int32
int32 robot_id
robot_id
bool
bool is_ready
is_ready
Now we need to make sure that the msg files are

turned into source code for C++, Python, and
other languages

package.xml
Open package.xml, and add the following two lines to it
<build_depend>roscpp</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>message_runtime</run_depend>
Note that at build time, we need "message_generation",

while at runtime, we need "message_runtime"

CMakeLists
Add the message_generation dependency to the find
package call so that you can generate messages:
find_package(catkin
find_package(catkin REQUIRED
REQUIRED COMPONENTS
COMPONENTS
roscpp
roscpp
rospy
rospy
std_msgs
std_msgs
message_generation
message_generation
))
Also make sure you export the message runtime

dependency:
catkin_package(
catkin_package(
## INCLUDE_DIRS
INCLUDE_DIRS include
include
## LIBRARIES
LIBRARIES multi_sync
multi_sync
CATKIN_DEPENDS
CATKIN_DEPENDS roscpp
roscpp rospy
rospy std_msgs
std_msgs message_runtime
message_runtime
## DEPENDS
DEPENDS system_lib
system_lib
))

CMakeLists
Find the following block of code:
##
## Generate
Generate messages
messages in
in the
the 'msg'
'msg' folder
folder
## add_message_files(
add_message_files(
## FILES
FILES
## Message1.msg
Message1.msg
## Message2.msg
Message2.msg
## ))
Uncomment it by removing the # symbols and then

replace the stand in Message*.msg files with
your .msg file, such that it looks like this:
add_message_files(
add_message_files(
FILES
FILES
RobotStatus.msg
RobotStatus.msg
))

CMakeLists
Now we must ensure the generate_messages() function is
called
Uncomment these lines:
## generate_messages(
generate_messages(
## DEPENDENCIES
DEPENDENCIES
## std_msgs
std_msgs
## ))
So it looks like:
generate_messages(
generate_messages(
DEPENDENCIES
DEPENDENCIES
std_msgs
std_msgs
))

Compiling a package with custom messages
If you need to compile nodes that depend on custom
messages in the same package, you need to run
catkin_make with the following argument:
catkin_make
catkin_make -j4
-j4
Otherwise catkin_make tries to compile the library

without waiting for all the message headers to be
generated

Using rosmsg
That's all you need to do to create a msg
Let's make sure that ROS can see it using
the rosmsg show command:
$$ rosmsg
rosmsg show
show [message
[message type]
type]

Create a Message
Now we need to make our package:
$$ cd
cd ~/catkin_ws
~/catkin_ws
$$ catkin_make
catkin_make
During the build, source files will be created from

the .msg file:

Create a Message
Any .msg file in the msg directory will generate
code for use in all supported languages.
The C++ message header file will be generated
in ~/catkin_ws/devel/include/multi_sync/

RobotStatus.h
#include
#include <std_msgs/Header.h>
<std_msgs/Header.h>
namespace
namespace multi_sync
multi_sync
{{
template
template <class
<class ContainerAllocator>
ContainerAllocator>
struct
struct RobotStatus_
RobotStatus_
{{
typedef
typedef RobotStatus_<ContainerAllocator>
RobotStatus_<ContainerAllocator> Type;
Type;
RobotStatus_()
RobotStatus_()
:: header()
header()
,, robot_id(0)
robot_id(0)
,, is_ready(false)
is_ready(false) {{
}}
typedef
typedef ::std_msgs::Header_<ContainerAllocator>
::std_msgs::Header_<ContainerAllocator> _header_type;
_header_type;
_header_type
_header_type header;
header;
typedef
typedef uint32_t
uint32_t _robot_id_type;
_robot_id_type;
_robot_id_type robot_id;
_robot_id_type robot_id;
typedef
typedef uint8_t
uint8_t _is_ready_type;
_is_ready_type;
_is_ready_type
_is_ready_type is_ready;
is_ready;
typedef
typedef boost::shared_ptr<
boost::shared_ptr< ::multi_sync::RobotStatus_<ContainerAllocator>
::multi_sync::RobotStatus_<ContainerAllocator> >> Ptr;
Ptr;
typedef
typedef boost::shared_ptr<
boost::shared_ptr< ::multi_sync::RobotStatus_<ContainerAllocator>
::multi_sync::RobotStatus_<ContainerAllocator> const>
const> ConstPtr;
ConstPtr;
boost::shared_ptr<std::map<std::string,
boost::shared_ptr<std::map<std::string, std::string>
std::string> >> __connection_header;
__connection_header;
};
}; //
// struct
struct RobotStatus_
RobotStatus_
typedef
typedef ::multi_sync::RobotStatus_<std::allocator<void> >> RobotStatus;
::multi_sync::RobotStatus_<std::allocator<void> RobotStatus;

Importing Messages
Copy move_robot.cpp from the last lesson to the
package
Now import the RobotStatus header file by
adding the following line:
#include
#include <multi_sync/RobotStatus.h>
<multi_sync/RobotStatus.h>
Note that messages are put into a namespace

that matches the name of the package

Team Status Messages
We will use a shared topic called team_status to
receive status messages from other team
members
Each robot will have both a publisher and a
listener object to this topic
Add the following global (class) variables:
ros::Subscriber
ros::Subscriber team_status_sub;
team_status_sub;
ros::Publisher
ros::Publisher team_status_pub;
team_status_pub;

Team Status Messages
In the main() function initialize the publisher and
the listener:
team_status_pub
team_status_pub == nh.advertise<multi_sync::RobotStatus>("team_status",
nh.advertise<multi_sync::RobotStatus>("team_status",
10);
10);
team_status_sub
team_status_sub == nh.subscribe("team_status",
nh.subscribe("team_status", 1,
1, &teamStatusCallback);
&teamStatusCallback);
Then call a function that will publish a ready

status message (before calling move_forward):
publishReadyStatus();
publishReadyStatus();
After that call a function that will wait for all the
other team members to become ready:
waitForTeam();
waitForTeam();
Publishing Ready Status Message
Add the following function that will publish a status message
when the robot is ready:
void
void publishReadyStatus()
publishReadyStatus() {{
multi_sync::RobotStatus
multi_sync::RobotStatus status_msg;
status_msg;
status_msg.header.stamp
status_msg.header.stamp == ros::Time::now();
ros::Time::now();
status_msg.robot_id
status_msg.robot_id == robot_id;
robot_id;
status_msg.is_ready
status_msg.is_ready == true;
true;
//
// Wait
Wait for
for the
the publisher
publisher to
to connect
connect to
to subscribers
subscribers
sleep(1.0);
sleep(1.0);
team_status_pub.publish(status_msg);
team_status_pub.publish(status_msg);
ROS_INFO("Robot
ROS_INFO("Robot %d
%d published
published ready
ready status",
status", robot_id);
robot_id);
}}
Note that the publisher needs some time to connect to the

subscribers, thus you need to wait between creating the
publisher and sending the first message
Waiting for Team
First add a global (class) boolean variable that will
indicate that all robots are ready:
bool
bool teamReady
teamReady == false;
false;
Now add the following function:

void
void waitForTeam()
waitForTeam() {{
ros::Rate
ros::Rate loopRate(1);
loopRate(1);
//
// Wait
Wait until
until all
all robots
robots are
are ready...
ready...
while
while (!teamReady)
(!teamReady) {{
ROS_INFO("Robot
ROS_INFO("Robot %d:
%d: waiting
waiting for
for team",
team", robot_id);
robot_id);
ros::spinOnce();
ros::spinOnce();
loopRate.sleep();
loopRate.sleep();
}}
}}
ros::spinOnce() will call all the team status callbacks waiting

to be called at that point in time.
Receiving Team Ready Status
Add the following callback function to
move_robot.cpp that will receive a team ready
message from the monitor node:
void
void teamStatusCallback(const
teamStatusCallback(const multi_sync::RobotStatus::ConstPtr&
multi_sync::RobotStatus::ConstPtr& status_msg)
status_msg)
{{
//
// Check
Check if
if message
message came
came from
from monitor
monitor
if
if (status_msg->header.frame_id
(status_msg->header.frame_id ==
== "monitor")
"monitor") {{
ROS_INFO("Robot
ROS_INFO("Robot %d:
%d: Team
Team is
is ready.
ready. Let's
Let's move!",
move!", robot_id);
robot_id);
teamReady = true;
teamReady = true;
}}
}}

Compiling the Node
Uncomment the following lines in CMakeLists.txt:
cmake_minimum_required(VERSION
cmake_minimum_required(VERSION 2.8.3)
2.8.3)
project(multi_sync)
project(multi_sync)

##
## Declare
Declare aa cpp
cpp executable
executable
add_executable(move_robot_sync
add_executable(move_robot_sync src/move_robot.cpp)
src/move_robot.cpp)
##
## Specify
Specify libraries
libraries to
to link
link aa library
library or
or executable
executable target
target against
against
target_link_libraries(move_robot_sync
target_link_libraries(move_robot_sync
${catkin_LIBRARIES}
${catkin_LIBRARIES}
))
Then call catkin_make

Launch File
Create a launch file named multi_sync.launch in
/launch subdirectory and add the following lines:
<launch>
<launch>
<node
<node name="stage"
name="stage" pkg="stage_ros"
pkg="stage_ros" type="stageros"
type="stageros" args="$(find
args="$(find
multi_sync)/world/willow-multi-erratic.world"/>
multi_sync)/world/willow-multi-erratic.world"/>
<node
name="move_robot_0" pkg="multi_sync"
pkg="multi_sync" type="move_robot"
args="0"
output="screen"/>
output="screen"/>
<node
args="1"
output="screen"/>
output="screen"/>
<node
args="2"
output="screen"/>
output="screen"/>
<node
args="3"
output="screen"/>
output="screen"/>
</launch>
</launch>

Running the move_robot nodes

Ready status messages
Type rostopic echo /team_status to watch the
ready messages:

Homework (not for submission)
Create a monitor node that will listen for the ready
messages and announce when all robots are ready

ROS Multi Lesson2

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

ROS Multi Lesson2

Uploaded by

Copyright:

Available Formats

Summer 2015

Multi-Robot Systems with ROS

Teaching Assistant: Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

Then run again the stage simulator

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

Create a world subdirectory in the package and

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

Node names must be unique in a running system

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

Note that the node names must be unique

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

stamp specifies the publishing time

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

(C)2015 Roi Yehoshua

Copy the world subdirectory from the stage_multi

(C)2015 Roi Yehoshua

Now we need to make sure that the msg files are

(C)2015 Roi Yehoshua