Lesson 5 Teaching Assistant Roi Yehoshua roiyehogmailcom Summer 2015 Agenda Spanning multiple robots in Gazebo Controlling multiple robots in Gazebo Running navigation stack in Gazebo with multiple ID: 480586
Download Presentation The PPT/PDF document "Multi-Robot Systems with ROS" is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.
Slide1
Multi-Robot Systems with ROS Lesson 5
Teaching Assistant:
Roi Yehoshuaroiyeho@gmail.com
Summer 2015Slide2
AgendaSpanning multiple robots in GazeboControlling multiple robots in
GazeboRunning navigation stack in Gazebo with multiple robotsSending goals to robots in Gazebo
(C)2015 Roi Yehoshua
2Slide3
GazeboGazebo is a 3D multi-robot simulatorROS
Indigo comes with Gazebo V2Composed of two processes:Server:
Runs the physics loop and generates sensor dataClient: Provides user interaction and visualization of a simulation(C)2015 Roi Yehoshua
3Slide4
Loading World FilesYou can use roslaunch to load world models
For example, to open willowgarage_world type:There are other 5 world files examples in the /launch subdirectory of
gazebo_ros package(C)2015 Roi Yehoshua
$
roslaunch
gazebo_ros
willowgarage_world.launch
4Slide5
Robot Description PackageTypically, each robot comes with a robot description package that contains its URDF and mesh files for simulation and visualizationWe will use r2d2_description package that we have built in the Introduction course
This package contains r2d2 URDF file and the mesh file for the Hokuyo laserYou can download this package from the link:http://u.cs.biu.ac.il/~
yehoshr1/89-685/demos/lesson11/r2d2_description.zip(C)2015 Roi Yehoshua5Slide6
r2d2 URDF Structure(C)2015 Roi Yehoshua
6Slide7
URDF File(C)2015 Roi Yehoshua
Make sure that all the topics and frames specified in the URDF file are without the /, otherwise
they will be shared, e.g.:<
plugin
name="
gazebo_ros_head_hokuyo_controller
" filename="libgazebo_ros_laser.so">
<
topicName
>
base_scan
</
topicName
>
<
frameName>
hokuyo_link
</
frameName
>
</
plugin
>
7Slide8
gazebo_multi packageLet’s create a new package called gazebo_multi
Create a launch subdirectory within the package and add the following launch file called one_robot.launch
(C)2015 Roi Yehoshua
$
cd
~/
catkin_ws
/
src
$
catkin_create_pkg
gazebo_multi
std_msgs
rospy
roscpp
8Slide9
Add Joint and State PublishersTo work with the robot model in ROS, we need to publish its joint states and TF treeFor that purpose we need to start two nodes:
a joint_state_publisher node that reads the robot’s model from its URDF file and publishes /joint_states
messagesa robot_state_publisher node that listens to /joint_states messages and publishes the transforms to /tf(C)2015 Roi Yehoshua
9Slide10
Launch File For One Robot(C)2015 Roi Yehoshua
<launch>
<arg name="robot_name"/> <arg name="init_pose"/>
<
param
name="
robot_description
"
textfile
="$(find r2d2_description)/
urdf
/r2d2.urdf"/>
<node name="
spawn_urdf
"
pkg
="gazebo_ros" type="spawn_model" args
="$(
arg
init_pose
) -
urdf
-
param
robot_description
-model $(
arg
robot_name
)" output="screen"/>
<node name="
joint_state_publisher
"
pkg
="
joint_state_publisher
" type="
joint_state_publisher
"/>
<node
pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/> <!-- All the stuff as from usual robot launch file goes here --></launch>
There are two arguments to pass for each robot’s initialization: name and position
10Slide11
Robots Launch File(C)2015 Roi Yehoshua
<launch>
<!-- ROBOT 1--> <group ns="robot1"> <param name="tf_prefix" value="robot1" /> <include file="$(find
gazebo_multi
)/launch/
one_robot.launch
" >
<
arg
name="
init_pose
" value="-x 1 -y 1 -z 1" />
<
arg
name="
robot_name" value="robot1" />
</include> </group> <!-- ROBOT 2-->
<group ns="robot2">
<
param
name="
tf_prefix
" value="robot2" />
<include file="$(find
gazebo_multi
)/launch/
one_robot.launch
" >
<
arg
name="
init_pose
" value="-x -1 -y 1 -z 1" />
<
arg
name="
robot_name
" value="robot2" />
</include>
</group>
</launch>
11Slide12
gazebo_multi.launchSimulation Launch File
(C)2015 Roi Yehoshua
<launch> <param name="/use_sim_time
" value="true" />
<!-- start world -->
<include file="$(find
gazebo_ros
)/launch/
willowgarage_world.launch
"/>
<!-- include our robots -->
<include file="$(find
gazebo_multi
)/launch/
robots.launch
"/>
</launch>
12Slide13
Launch SimulationTo launch the simulation run:This will open an empty world with the two r2d2 robots
(C)2015 Roi Yehoshua
$ roslaunch gazebo_multi
gazebo_multi.launch
13Slide14
Launch Simulation(C)2015 Roi Yehoshua
14Slide15
Launch Simulation(C)2015 Roi Yehoshua
15Slide16
Robots TopicsYou can see that both robots publish the base_scan and
cmd_vel topics in the correct namespaces
(C)2015 Roi Yehoshua
16Slide17
Moving the Robot with TeleopNow we are going to move one of the robots using the teleop_twist_keyboard
node Run the following command:You should see console output that gives you the key-to-control mapping
(C)2015 Roi Yehoshua$
rosrun
teleop_twist_keyboard
teleop_twist_keyboard.py
cmd_vel
:=/robot2/
cmd_vel
17Slide18
Moving the Robot with Teleop(C)2015 Roi Yehoshua
18Slide19
Node controlling the robot(C)2015 Roi Yehoshua
We will now add a node that will make one of the robots start random walking in the environmentThe code of the node is the same as the one we used to control the robot in Stage simulator
Since the robots are using the same topicsAdd random_walk.cpp to your package19Slide20
random_walk.cpp (1)(C)2015 Roi Yehoshua
#include <
ros/ros.h>
#include <
geometry_msgs
/
Twist.h
>
#include <
sensor_msgs
/
LaserScan.h
>
#include <string>
using namespace std;
#define MIN_SCAN_ANGLE_RAD -45.0/180*M_PI
#define MAX_SCAN_ANGLE_RAD +45.0/180*M_PI
bool
obstacleFound
=
false
;
void
readSensorCallback
(
const
sensor_msgs
::
LaserScan
::
ConstPtr
&
sensor_msg
);
20Slide21
random_walk.cpp (2)(C)2015 Roi Yehoshua
int
main(int
argc
,
char
**
argv
) {
if
(
argc
< 2) {
ROS_ERROR(
"You must specify robot id."
);
return
-1;
}
char
*
robot_name
=
argv
[1];
ROS_INFO(
"Moving robot %s"
,
robot_name
);
ros
::init(
argc
,
argv
,
"
random_walk
"
);
ros
::
NodeHandle
nh
;
string
cmd_vel_topic_name
=
robot_name
;
cmd_vel_topic_name
+=
"/
cmd_vel
"
;
ros
::Publisher
cmd_vel_pub
=
nh.advertise
<
geometry_msgs
::Twist>(
cmd_vel_topic_name
, 10);
string
laser_topic_name
=
robot_name
;
laser_topic_name
+=
"/
base_scan
"
;
ros::Subscriber base_scan_sub = nh.subscribe<sensor_msgs::LaserScan>(laser_topic_name, 1, &readSensorCallback);
21Slide22
random_walk.cpp (3)(C)2015 Roi Yehoshua
geometry_msgs
::Twist moveForwardCommand;
moveForwardCommand.linear.x
= 1.0;
geometry_msgs
::Twist
turnCommand
;
turnCommand.angular.z
= 1.0;
ros
::Rate
loop_rate
(10);
while
(
ros
::ok()) {
if
(
obstacleFound
) {
ROS_INFO(
"Turning around"
);
cmd_vel_pub.publish
(
turnCommand
);
}
else
{
ROS_INFO(
"Moving forward"
);
cmd_vel_pub.publish
(
moveForwardCommand
);
}
ros
::
spinOnce
();
// let ROS process incoming messages
loop_rate.sleep
();
}
return
0;
}
22Slide23
random_walk.cpp (4)(C)2015 Roi Yehoshua
void
readSensorCallback(
const
sensor_msgs
::
LaserScan
::
ConstPtr
&scan)
{
bool
isObstacle
=
false
;
int
minIndex
= ceil((MIN_SCAN_ANGLE_RAD - scan->
angle_min
) / scan->
angle_increment
);
int
maxIndex
= floor((MAX_SCAN_ANGLE_RAD - scan->
angle_min
) / scan->
angle_increment
);
for
(
int
i
=
minIndex
;
i
<=
maxIndex
;
i
++) {
if
(scan->ranges[
i
] < 0.5) {
isObstacle
=
true
;
}
}
if
(
isObstacle
)
{
ROS_INFO(
"Obstacle in front of you!"
);
obstacleFound
=
true
;
}
else
{
obstacleFound
=
false
;
}
}
23Slide24
Launch Random Walk Node(C)2015 Roi Yehoshua
To launch the random walk node type:
$ rosrun gazebo_multi random_walk [robot name]
24Slide25
All Robots Random Walk(C)2015 Roi Yehoshua
To make all robots random walk, you can add the random_walk node to one_robot.launch
file<launch> <arg
name="
robot_name
"/>
<
arg
name="
init_pose
"/>
<node name="
spawn_urdf
"
pkg
="gazebo_ros
" type="spawn_model" args="$(
arg
init_pose
) -
urdf
-
param
/
robot_description
-model $(
arg
robot_name
)" output="screen"/>
<node name="
joint_state_publisher
"
pkg
="
joint_state_publisher
" type="
joint_state_publisher
"/>
<node
pkg
="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/> <!-- All the stuff as from usual robot launch file go here --> <node name="random_walk
" pkg="gazebo_multi" type="
random_walk
"
args
="$(
arg
robot_name
)" output="screen"/>
</launch>
25Slide26
Running Navigation Stack in GazeboWe will now see how to run the navigation stack in Gazebo with multiple robotsCreate a new package
gazebo_navigation_multiCopy launch files (gazebo_multi.launch,
robots.launch, one_robot.launch) from the launch directory in gazebo_multi packageRename gazebo_multi.launch to navigation_multi.launchCopy the folder
move_base_config
from
navigation_multi
package
26
(C)2015 Roi YehoshuaSlide27
gazebo_navigation_multi package27
(C)2015 Roi YehoshuaSlide28
gazebo_navigation_multi launch files28
(C)2015 Roi YehoshuaSlide29
Use Static MapWe will first run navigation stack with a static known mapCreate a maps directory in your package and copy willow-full.png file into it
In navigation_multi.launch file make sure map_server points to this map file
29(C)2015 Roi YehoshuaSlide30
navigation_multi.launch30
(C)2015 Roi Yehoshua
<launch> <master auto="start"/> <param name="/use_sim_time" value="true"/>
<node
pkg
="
map_server
" type="
map_server
" name="
map_server
"
args
="$(find
gazebo_navigation_multi
)/maps/willow-full.png 0.1" respawn
="false" > <param name="frame_id
" value="/map" />
</node>
<!-- start gazebo -->
<include file="$(find
gazebo_navigation_multi
)/launch/
willowgarage_world.launch
"/>
<!-- include our robots -->
<include file="$(find
gazebo_navigation_multi
)/launch/
robots.launch
"/>
<node name="
rviz
"
pkg
="
rviz
" type="
rviz
"
args="-d $(find gazebo_navigation_multi)/multi_robot.rviz" /></launch>Slide31
robots.launch31
(C)2015 Roi Yehoshua
<launch> <!-- ROBOT 1 --> <group ns="lizi1"> <param name="tf_prefix
" value="lizi1" />
<include file="$(find
gazebo_navigation_multi
)/launch/
one_robot.launch
" >
<
arg
name="
init_pose
" value="-x 20 -y 15 -z 0" />
<
arg name="
robot_name" value="lizi1" /> </include> </group>
<!-- ROBOT 2 -->
<group ns="lizi2">
<
param
name="
tf_prefix
" value="lizi2" />
<include file="$(find
gazebo_navigation_multi
)/launch/
one_robot.launch
" >
<
arg
name="
init_pose
" value="-x 22 -y 15 -z 0" />
<
arg
name="
robot_name
" value="lizi2" />
</include>
</group>
<!-- ROBOT 3 -->
<group ns="lizi3"> <param name="tf_prefix" value="lizi3" /> <include file="$(find gazebo_navigation_multi)/launch/one_robot.launch" > <arg name="
init_pose" value="-x 24 -y 15 -z 0" /> <arg name="robot_name" value="lizi3" /> </include>
</group>
</launch>Slide32
one_robot.launchCopy navigation stack nodes (move_base,
fake_localization or amcl) to one_robot.launch
Add a static transform publisher between base_link and laser_link frames32
(C)2015 Roi YehoshuaSlide33
TF TreeFor the navigation stack to work properly, the robot needs to publish the following transformations:
33
(C)2015 Roi Yehoshuamap
robotX
/
odom
robotX
/
base_link
robotX
/
laser_link
published by the localization system
published by Gazebo’s driver controller
published by static
tf
defined in your launch fileSlide34
one_robot.launch (1)34
(C)2015 Roi Yehoshua
<launch> <arg name="robot_name"/>
<
arg
name="
init_pose
"/>
<
param
name="
robot_description
" command="$(find
xacro
)/xacro.py $(find lizi_description)/
urdf/lizi.urdf"/> <node name="spawn_urdf
"
pkg
="
gazebo_ros
" type="
spawn_model
"
args
="$(
arg
init_pose
) -
urdf
-
param
robot_description
-model $(
arg
robot_name
)" output="screen"/>
<node name="
joint_state_publisher
" pkg="joint_state_publisher" type="joint_state_publisher"/> <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/>
<!-- Run navigation stack --> <node pkg="move_base" type="move_base"
respawn
="false" name="
move_base_node
" output="screen">
<remap from="map" to="/map" />
<
param
name="
controller_frequency
" value="10.0" />
<
rosparam
file="$(find
gazebo_navigation_multi
)/
move_base_config
/
costmap_common_params.yaml
" command="load" ns="
global_costmap
" />
<
rosparam
file="$(find
gazebo_navigation_multi
)/
move_base_config
/
costmap_common_params.yaml
" command="load" ns="
local_costmap
" />
<
rosparam
file="$(find
gazebo_navigation_multi
)/
move_base_config
/
local_costmap_params.yaml
" command="load" />
<
rosparam
file="$(find
gazebo_navigation_multi
)/
move_base_config
/global_costmap_params.yaml" command="load" /> <rosparam file="$(find gazebo_navigation_multi)/move_base_config/base_local_planner_params.yaml" command="load" /> </node> Slide35
one_robot.launch (2)35
(C)2015 Roi Yehoshua
<node pkg="fake_localization" type="fake_localization
" name="
fake_localization
"
respawn
="false" output="screen">
<
param
name="
odom_frame_id
" value="$(
arg
robot_name)/odom
" /> <param name="base_frame_id" value="$(
arg
robot_name
)/
base_link
" />
</node>
<node
pkg
="
tf
" type="
static_transform_publisher
" name="
laser_link_broadcaster
"
args
="0 0 1 0 0 0 $(
arg
robot_name
)/
base_link
$(
arg
robot_name)/laser_link 100" /></launch>Slide36
TF Tree36
(C)2015 Roi Yehoshua
The TF tree should now look like this:Slide37
Map in Gazebo and rvizBy default the origin of the map is different in Gazebo and rviz
In Gazebo the origin is by default at the center of the map while in rviz it is at the lower-left cornerThe map’s pose in Gazebo can be changed by adjusting its corresponding model in Gazebo’s world file
For that purpose, we first need to copy the world’s file into our package37(C)2015 Roi YehoshuaSlide38
Change Map’s Pose in GazeboCreate worlds directory in your packageCopy
willowgarage.world file from gazebo’s worlds directory to worlds directory of your packageNow edit the world’s file to adjust the model’s pose
The pose parameter consists of 6 values: <x y z r p y>Angles are specified in degrees38(C)2015 Roi Yehoshua
$
roscd
gazebo_navigation_multi
/worlds
$ cp /
usr
/share/gazebo-1.9/worlds/
willowgarage.world
.Slide39
willowgarage.world39
(C)2015 Roi Yehoshua
<?xml version="1.0" ?><sdf version="1.4"> <world name="default"> <include>
<
uri
>model://ground_plane</uri>
</include>
<include>
<
uri
>model://sun</uri>
</include>
<include>
<
uri
>model://willowgarage</uri>
<pose>-3 54 0 0 0 30</pose>
</include>
</world>
</
sdf
>Slide40
Change Map’s Pose in GazeboWe also need to update the launch file of Gazebo’s world to launch our own version of the world fileFirst copy
willowgarage_world.launch from gazebo_ros package to the launch directory of your package
Now edit this file40(C)2015 Roi Yehoshua
$
roscd
gazebo_ros
/launch
$ cp
willowgarage_world.launch
~/
catkin_ws
/
src
/
gazebo_navigation_multi
/launchSlide41
willowgarage_world.launch41
(C)2015 Roi Yehoshua
<launch> <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find
gazebo_ros
)/launch/
empty_world.launch
">
<
arg
name="
world_name
" value="$(find
gazebo_navigation_multi
)/worlds/
willowgarage.world
"/>
<
arg
name="paused" value="false"/>
<
arg
name="
use_sim_time
" value="true"/>
<
arg
name="
gui
" value="true"/>
<
arg
name="headless" value="false"/>
<
arg
name="debug" value="false"/>
</include>
</launch>Slide42
Running the Navigation StackNow we are ready to run the navigation stack
(C)2015 Roi Yehoshua
$ roslaunch gazebo_navigation_multi navigation_multi.launch
42Slide43
Gazebo43
(C)2015 Roi YehoshuaSlide44
rviz44
(C)2015 Roi YehoshuaSlide45
Set Navigation GoalOpen rviz menu – Tool Properties
Change the topic name for the 2D Nav Goal according to the robot that you want to activate:
45(C)2015 Roi YehoshuaSlide46
Set Navigation Goal46
(C)2015 Roi YehoshuaSlide47
Following the Global Plan47
(C)2015 Roi YehoshuaSlide48
Final Pose48
(C)2015 Roi YehoshuaSlide49
Final Pose in Gazebo49
(C)2015 Roi YehoshuaSlide50
Sending Goals From TerminalTo send a goal to a robot from terminal, you can publish a message to the topic [robot_name
]/move_base_simple/goalFor example to send a goal command to lizi2:
50(C)2015 Roi Yehoshua
$
rostopic
pub /lizi2/
move_base_simple
/goal
geometry_msgs
/
PoseStamped
'{header: {
frame_id
: "map"}, pose: {position: {x: 22, y: 17, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 1}}}'Slide51
Sending Goals From Terminal51
(C)2015 Roi YehoshuaSlide52
Sending Goals From CodeCopy send_goals.cpp from the package navigation_multi
Add send_goals.cpp to CMakeLists.txtChange the name of the executable from send_goal to
gazebo_send_goalYou cannot have two executables in the same workspaceCompile the packageNow you can send goals to robots by running the gazebo_send_goal node52
(C)2015 Roi YehoshuaSlide53
Sending Goals From Code53
(C)2015 Roi Yehoshua