ROS Multi Lesson6

Multi-Robot Systems with ROS
Lesson 6
Teaching Assistant: Roi Yehoshua

roiyeho@gmail.com
Agenda
• ROS navigation stack with multiple robots
• Using rviz
• Sending goals to robots
(C)2014 Roi Yehoshua

ROS Navigation Stack
• http://wiki.ros.org/navigation
• The navigation stack handles moving a robot
from one position to another position safely
(without crashing or getting lost)
• It takes in information from odometry and the
sensors, and a goal pose and outputs safe
velocity commands

ROS Navigation Stack

Navigation Stack Main Components
• map_server - offers map data as a ROS Service
• gmapping - provides laser-based SLAM (Simultaneous
Localization and Mapping)
• amcl - a probabilistic localization system
• global_planner - implementation of a fast global
planner for navigation
• local_planner - implementations of the Trajectory
Rollout and Dynamic Window approaches to local
robot navigation
• move_base - links together the global and local
planner to accomplish the navigation task
Navigation Stack Requirements
• There are three main hardware requirements:
– The navigation stack can only handle a differential
drive and holonomic wheeled robots.
– A planar laser must be mounted on the mobile base
of the robot to create the map and localization
– Its performance will be best on robots that are nearly
square or circular

Navigation Stack with Multiple Robots
• Download the navigation tutorials from git
– https://github.com/ros-planning/navigation_tutorials
$$ cd
cd ~/ros/stacks
~/ros/stacks
$$ git
git clone
clone https://github.com/ros-planning/navigation_tutorials.git
https://github.com/ros-planning/navigation_tutorials.git
• This will create a navigation_stack package

• In the launch directory of the package you will
find move_base_multi_robot.launch
• This is an example launch file for running
the navigation stack with multiple robots in stage
move_base_multi_robot.launch (1)
• Nodes that are common to all robots:
<launch>
<launch>
<master
<master auto="start"/>
auto="start"/>
<param
<param name="/use_sim_time"
name="/use_sim_time" value="true"/>
value="true"/>
<node
<node pkg="map_server"
pkg="map_server" type="map_server"
type="map_server" name="map_server"
name="map_server" args="$(find
args="$(find
navigation_stage)/stage_config/maps/willow-full.pgm
navigation_stage)/stage_config/maps/willow-full.pgm 0.1"
0.1" respawn="false"
respawn="false" >>
<param
<param name="frame_id"
name="frame_id" value="/map"
value="/map" />
/>
</node>
</node>
<node
<node pkg="stage_ros"
pkg="stage_ros" type="stageros"
type="stageros" name="stageros"
name="stageros" args="$(optenv
args="$(optenv
ROS_STAGE_GRAPHICS
ROS_STAGE_GRAPHICS -g)-g) $(find
$(find navigation_stage)/stage_config/worlds/willow-pr2-
navigation_stage)/stage_config/worlds/willow-pr2-
multi.world"
multi.world" respawn="false">
respawn="false">
<param
<param name="base_watchdog_timeout"
name="base_watchdog_timeout" value="0.2"/>
value="0.2"/>
</node>
</node>
– Remove the first argument in the stage_ros node to make

stage’s window visible

• Nodes for controlling robot 0:

-->
<group
<group ns="robot_0">
ns="robot_0">
<param
<param name="tf_prefix"
name="tf_prefix" value="robot_0"
value="robot_0" />
/>
<node
<node pkg="move_base"
pkg="move_base" type="move_base"
type="move_base" respawn="false"
respawn="false" name="move_base_node"
name="move_base_node"
output="screen">
output="screen">
<remap
<remap from="map"
from="map" to="/map"
to="/map" />
/>
<param
<param name="controller_frequency"
name="controller_frequency" value="10.0"
value="10.0" />
/>
<rosparam
<rosparam file="$(find
file="$(find
navigation_stage)/move_base_config/costmap_common_params.yaml"
navigation_stage)/move_base_config/costmap_common_params.yaml" command="load"
command="load"
ns="global_costmap"
ns="global_costmap" /> />
<rosparam
file="$(find
command="load"
ns="local_costmap"
ns="local_costmap" /> />
<rosparam
file="$(find navigation_stage)/move_base_config/local_costmap_params.yaml"
navigation_stage)/move_base_config/local_costmap_params.yaml"
command="load"
command="load" /> />
<rosparam
file="$(find navigation_stage)/move_base_config/global_costmap_params.yaml"
navigation_stage)/move_base_config/global_costmap_params.yaml"
command="load"
<rosparam
file="$(find
navigation_stage)/move_base_config/base_local_planner_params.yaml"
navigation_stage)/move_base_config/base_local_planner_params.yaml" command="load"
command="load" />
/>
</node>
</node>

<node
<node pkg="fake_localization"
pkg="fake_localization" type="fake_localization"
type="fake_localization" name="fake_localization"
name="fake_localization"
respawn="false"
respawn="false" output="screen">
output="screen">
<param
<param name="odom_frame_id"
name="odom_frame_id" value="robot_0/odom"
value="robot_0/odom" /> />
<param
<param name="base_frame_id"
name="base_frame_id" value="robot_0/base_link"
value="robot_0/base_link" />/>
</node>
</node>
</group>
</group>

-->

• Nodes for controlling robot 1:

-->
<group
ns="robot_1">
<param
<param name="tf_prefix"
name="tf_prefix" value="robot_1"
value="robot_1" />
/>
<node
<node pkg="move_base"
pkg="move_base" type="move_base"
type="move_base" respawn="false"
respawn="false" name="move_base_node"
name="move_base_node"
output="screen">
output="screen">
<remap
<remap from="map"
from="map" to="/map"
to="/map" />
/>
<param
<param name="controller_frequency"
name="controller_frequency" value="10.0"
value="10.0" />
/>
<rosparam
file="$(find
command="load"
ns="global_costmap"
ns="global_costmap" /> />
<rosparam
file="$(find
command="load"
ns="local_costmap"
ns="local_costmap" /> />
<rosparam
file="$(find navigation_stage)/move_base_config/local_costmap_params.yaml"
navigation_stage)/move_base_config/local_costmap_params.yaml"
command="load"
<rosparam
file="$(find navigation_stage)/move_base_config/global_costmap_params.yaml"
navigation_stage)/move_base_config/global_costmap_params.yaml"
command="load"
<rosparam
file="$(find
navigation_stage)/move_base_config/base_local_planner_params.yaml"
navigation_stage)/move_base_config/base_local_planner_params.yaml" command="load"
command="load" />
/>
</node>
</node>

<node
<node pkg="fake_localization"
pkg="fake_localization" type="fake_localization"
type="fake_localization" name="fake_localization"
name="fake_localization"
respawn="false">
respawn="false">
<param
<param name="odom_frame_id"
name="odom_frame_id" value="robot_1/odom"
value="robot_1/odom" /> />
<param
<param name="base_frame_id"
name="base_frame_id" value="robot_1/base_link"
value="robot_1/base_link" />/>
</node>
</node>
</group>
</group>

-->
<node
<node name="rviz"
name="rviz" pkg="rviz"
pkg="rviz" type="rviz"
type="rviz" args="-d
args="-d $(find
$(find navigation_stage)/multi_robot.rviz"
navigation_stage)/multi_robot.rviz" />
/>
</launch>
</launch>

Running the Navigation Stack
• To run this launch file type:
$$ roslaunch
roslaunch navigation_stage
navigation_stage move_base_multi_robot.launch
move_base_multi_robot.launch

Running the Navigation Stack

rviz

Using rviz with Navigation Stack
• rviz is a ROS 3D visualization tool that lets
you see the world from a robot's perspective
• rviz can help you work with the navigation stack,
including:
– Displaying all the visualization information that the
navigation stack provides, such as the global and local
plans and the costmaps
– Sending goals to the navigation stack
– Setting the initial pose of the robot for a localization
system like amcl

Map in Stage and rviz

• By default the origin of the map is different in
Stage and rviz
• In Stage the origin is by default at the center of
the map while in rviz it is at the lower-left corner
• The map’s origin in Stage can be changed by
adjusting the floorplan pose in its world file
• rviz reads the map from the /map topic that is
published by map_server
• Its origin can be changed in the map’s yaml file

• Change the map’s pose in Stage world file so the
map’s origin will be adjusted to its origin in rviz
• Also change the robots’ positions accordingly
## load
load an
an environment
environment bitmap bitmap
floorplan
floorplan
((
name
name "willow"
"willow"
bitmap
bitmap "../maps/willow-full.pgm"
"../maps/willow-full.pgm"
size
size [58.4
[58.4 52.6
52.6 0.5]
0.5]
#pose
#pose [[ -26.300
-26.300 29.200
29.200 00 90.000
90.000 ]]
pose
pose [[ 29.2
29.2 26.2
26.2 00 00 ]]
))
## throw
throw inin aa robot
robot
#pr2(
#pr2( pose
pose [[ -21.670
-21.670 47.120
47.120 00 28.166
28.166 ]] name
name "pr2_0"
"pr2_0" color
color "blue")
"blue")
#pr2(
#pr2( pose
pose [[ -21.670
-21.670 48.120
48.120 00 28.166
28.166 ]] name
name "pr2_1"
"pr2_1" color
color "green")
"green")
#block( pose [ -24.269 48.001 0 180.000 ]
#block( pose [ -24.269 48.001 0 180.000 ] color "red")color "red")
pr2(
pr2( pose
pose [[ 9.5
9.5 14.5
14.5 00 28.166
28.166 ]] name
name "pr2_0"
"pr2_0" color
color "blue")
"blue")
pr2(
pr2( pose
pose [[ 9.5
9.5 15.5
15.5 00 28.166
28.166 ]] name
name "pr2_1"
"pr2_1" color
color "green")
"green")
block(
block( pose
pose [[ 12.5
12.5 15.5
15.5 00 180.000
180.000 ]] color
color "red")
"red")


Robot Footprint
• To see the robot’s footprint in rviz change the
robot footprint topic to:
/robot_N/move_base_node/local_costmap/footprint_
layer/footprint_stamped
• In our case, the robots have a pentagon-shape
– Defined in
move_base_config/costmap_common_params.yaml

Robot Footprint

TF
• Add the TF display to watch the TF tree

Sending Goals
• The 2D nav goal button allows you to send a goal
to the navigation by setting a desired pose for
the robot to achieve
• By default the goal is published on the topic
/move_base_simple/goal
• However, when having multiple robots, the topic
is /robot_N/move_base_simple/goal
• To change the topic name, first enable the Tool
Properties panel via the Panels menu

Sending Goals

Sending Goals
• Change the 2D Nav Goal topic to
/robot_0/move_base_simple/goal
• Click on the 2D Nav Goal button (or press G) and
select the map and the goal for the first robot
• You can select the x and y position and the end
orientation for the robot

Sending Goals

Sending Goals

Collision Avoidance
• To avoid the robots from colliding into each
other, change the following definition in willow-
pr2-multi.world:
define
define pr2
pr2 position
position
((
size
size [0.65
[0.65 0.65
0.65 0.25]
0.25]
origin
origin [-0.05
[-0.05 00 00 0]
0]
gui_nose
gui_nose 11
drive
drive "omni”
"omni”
topurg(pose
topurg(pose [[ 0.275
0.275 0.000
0.000 -0.1
-0.1 0.000
0.000 ])])
))

Using the Navigation Stack
• We will now create a node that will make a given
robot to move to a specific location on the map
• First create a package called navigation_multi that
depends on roscpp, rospy, tf, action_lib and
move_base_msgs
$$ cd
cd ~/catkin_ws/src
~/catkin_ws/src
$$ catkin_create_pkg
catkin_create_pkg navigation_multi
navigation_multi roscpp
roscpp rospy
rospy tf
tf actionlib
actionlib
move_base_msgs
move_base_msgs
• Build the package by calling catkin_make

• Open the package in Eclipse and add a new
source file called print_location.cpp
Integrating with move_base
• Copy the following directories and files from the
navigation_stage package to your package:
– Copy the entire directory move_base_config
– From the launch directory copy
move_base_multi_robot.launch
– From stage_config/maps copy willow-full.pgm
– From stage_config/worlds copy willow-pr2-
multi.world
– From the root directory copy multi_robot.rviz

Package Directory Structure

• move_base_config files:

• Fix move_base.xml to use the correct package:

• Fix the package name also in the launch file:

Check Package Configuration
• Test that all the configuration is correct by
running the launch file:
$$ roslaunch
roslaunch navigation_multi
navigation_multi navigation_multi.launch
navigation_multi.launch

Sending Goals From Code
• Open the project file in Eclipse
• Under the src subdirectory, create a new file
called send_goal.cpp

Using the Navigation Stack
• Open the package in Eclipse and add a new
source file called send_goal.cpp
• Copy the following code into it

send_goal.cpp (1)
#include
#include <ros/ros.h>
<ros/ros.h>
#include
#include <move_base_msgs/MoveBaseAction.h>
<move_base_msgs/MoveBaseAction.h>
#include
#include <actionlib/client/simple_action_client.h>
<actionlib/client/simple_action_client.h>
#include
#include <tf/transform_datatypes.h>
<tf/transform_datatypes.h>
typedef
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
MoveBaseClient;
MoveBaseClient;
using
using namespace
namespace std;
std;
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 leader
leader robot
robot id.");
id.");
return -1;
return -1;
}}
char
char *robot_id
*robot_id == argv[1];
argv[1];
ros::init(argc,
ros::init(argc, argv,
argv, "send_goals");
"send_goals");
ros::NodeHandle nh;
ros::NodeHandle nh;
//
// Define
Define the
the goal
goal
double
double goal_x
goal_x == 7.45;
7.45;
double
double goal_y
goal_y == 18.5;
18.5;
double
double goal_theta == 0;
goal_theta 0;

send_goal.cpp (2)
//
// Create
Create the
the string
string "robot_X/move_base"
"robot_X/move_base"
string
string move_base_str
move_base_str == "/robot_";
"/robot_";
move_base_str += robot_id;
move_base_str += robot_id;
move_base_str
move_base_str +=
+= "/move_base";
"/move_base";
//
// create
create the
the action
action client
client
MoveBaseClient
MoveBaseClient ac(move_base_str,
ac(move_base_str, true);
true);
//
// Wait
Wait for
for the
the action
action server
server to
to become
become available
available
ROS_INFO("Waiting
ROS_INFO("Waiting for
for the
the move_base
move_base action
action server");
server");
ac.waitForServer(ros::Duration(5));
ac.waitForServer(ros::Duration(5));
ROS_INFO("Connected
ROS_INFO("Connected to
to move
move base
base server");
server");
//
// Send
Send aa goal
goal to
to move_base
move_base
move_base_msgs::MoveBaseGoal
move_base_msgs::MoveBaseGoal goal;
goal;
goal.target_pose.header.frame_id
goal.target_pose.header.frame_id == "map";
"map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x
goal.target_pose.pose.position.x == goal_x;
goal_x;
goal.target_pose.pose.position.y
goal.target_pose.pose.position.y == goal_y;
goal_y;


send_goal.cpp (3)
//
// Convert
Convert the
the Euler
Euler angle
angle to
to quaternion
quaternion
double
double radians
radians == goal_theta
goal_theta ** (M_PI/180);
(M_PI/180);
tf::Quaternion
tf::Quaternion quaternion;
quaternion;
quaternion
quaternion == tf::createQuaternionFromYaw(radians);
tf::createQuaternionFromYaw(radians);
geometry_msgs::Quaternion
geometry_msgs::Quaternion qMsg;
qMsg;
tf::quaternionTFToMsg(quaternion,
tf::quaternionTFToMsg(quaternion, qMsg);
qMsg);
goal.target_pose.pose.orientation
goal.target_pose.pose.orientation == qMsg;
qMsg;
ROS_INFO("Sending
ROS_INFO("Sending goal
goal to
to robot
robot no.
no. %s:
%s: xx == %f,
%f, yy == %f,
%f, theta
theta == %f",
%f",
robot_id, goal_x, goal_y, goal_theta);
robot_id, goal_x, goal_y, goal_theta);
ac.sendGoal(goal);
ac.sendGoal(goal);
//
// Wait
Wait for
for the
the action
action to
to return
return
ac.waitForResult();
ac.waitForResult();
if
if (ac.getState()
(ac.getState() ==
== actionlib::SimpleClientGoalState::SUCCEEDED)
actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("You
ROS_INFO("You have
have reached
reached the
the goal!");
goal!");
else
else
ROS_INFO("The
ROS_INFO("The base
base failed
failed for
for some
some reason");
reason");
return
return 0;
0;
}}

Compiling the Node
• Change the following lines in CMakeLists.txt:
cmake_minimum_required(VERSION
cmake_minimum_required(VERSION 2.8.3) 2.8.3)
project(tf_multi)
project(tf_multi)
…
…
##
## Declare
Declare aa cpp
cpp executable
executable
add_executable(send_goal
add_executable(send_goal src/send_goal.cpp)
src/send_goal.cpp)
…
…
##
## Specify
Specify libraries
libraries to
to link
link aa library
library or
or executable
executable target
target against
against
target_link_libraries(send_goal
target_link_libraries(send_goal ${catkin_LIBRARIES})
${catkin_LIBRARIES})
• Then call catkin_make

• For example, to send a goal to robot no.1 type:
$$ rosrun
rosrun navigation_multi
navigation_multi send_goal
send_goal 11

Running send_goal node

• Initial position:

• In the middle of the path:

• Final position:

send_goal Parameters
• Now let us make the desired pose of the robot
configurable in a launch file, so we can send
different goals to the robots from the terminal
• You can define parameters for a node by using
the <param> tag in the ROS launch file
• Create the following send_goals.launch file

<launch>
<launch>

-->
<param
<param name="goal_x"
name="goal_x" value="6.32"
value="6.32" />
/>
<param
<param name="goal_y"
name="goal_y" value="17.67"
value="17.67" />
/>
<param
<param name="goal_theta"
name="goal_theta" value="0"
value="0" />
/>
<node
<node pkg="navigation_multi"
pkg="navigation_multi" type="send_goal"
type="send_goal" respawn="false"
respawn="false"
name="send_goal" output="screen" args="0"/>
</group>
</group>

-->

-->
<param
<param name="goal_x"
name="goal_x" value="10.12"
value="10.12" />
/>
<param
<param name="goal_y"
name="goal_y" value="12.97"
value="12.97" />
/>
<param
<param name="goal_theta"
name="goal_theta" value="45"
value="45" />
/>
<node
<node pkg="navigation_multi"
pkg="navigation_multi" type="send_goal"
type="send_goal" respawn="false"
respawn="false"
</group>
</group>

-->
</launch>
</launch>

• Now roslaunch send_goal.launch:


ROS Graph
• Graph of nodes running in the system:

Homework – Assignment 1
• Implement a simple line formation control for a
team of robots
• More details can be found at:
http://u.cs.biu.ac.il/~yehoshr1/89-689/assignment
1/Assignment1.html

ROS Multi Lesson6

Uploaded by

Document Information

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 Lesson6

Uploaded by

Copyright:

Available Formats

Multi-Robot Systems with ROS

Teaching Assistant: Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

• This will create a navigation_stack package

– Remove the first argument in the stage_ros node to make

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

• Build the package by calling catkin_make

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

• Then call catkin_make

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

(C)2014 Roi Yehoshua

You might also like