multi-robot systems with ros lesson 5
DESCRIPTION
Multi-Robot Systems with ROS Lesson 5. Teaching Assistant: Roi Yehoshua [email protected]. Agenda. Coordinate frames tf prefix Get a robot position Leader-Followers formation. Coordinate Frames. In ROS various types of data are published in different coordinate frames - PowerPoint PPT PresentationTRANSCRIPT
(C)2015 Roi Yehoshua 2
Agenda• Spanning multiple robots in Gazebo• Controlling multiple robots in Gazebo• Running navigation stack in Gazebo with multiple
robots• Sending goals to robots in Gazebo
(C)2015 Roi Yehoshua 3
Gazebo• Gazebo is a 3D multi-robot simulator• ROS Indigo comes with Gazebo V2• Composed of two processes:– Server: Runs the physics loop and generates sensor
data– Client: Provides user interaction and visualization of a
simulation
(C)2015 Roi Yehoshua 4
Loading World Files• You 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
$ roslaunch gazebo_ros willowgarage_world.launch
(C)2015 Roi Yehoshua 5
Robot Description Package• Typically, each robot comes with a robot
description package that contains its URDF and mesh files for simulation and visualization
• We 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 laser
• You can download this package from the link:– http://u.cs.biu.ac.il/~
yehoshr1/89-685/demos/lesson11/r2d2_description.zip
(C)2015 Roi Yehoshua 6
r2d2 URDF Structure
(C)2015 Roi Yehoshua 7
URDF File• 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>
(C)2015 Roi Yehoshua 8
gazebo_multi package• Let’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
$ cd ~/catkin_ws/src$ catkin_create_pkg gazebo_multi std_msgs rospy roscpp
(C)2015 Roi Yehoshua 9
Add Joint and State Publishers• To work with the robot model in ROS, we need to
publish its joint states and TF tree• For 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 messages
– a robot_state_publisher node that listens to /joint_states messages and publishes the transforms to /tf
(C)2015 Roi Yehoshua 10
Launch File For One Robot<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
(C)2015 Roi Yehoshua 11
Robots Launch File<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>
(C)2015 Roi Yehoshua 12
• gazebo_multi.launch
Simulation Launch File
<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>
(C)2015 Roi Yehoshua 13
Launch Simulation• To launch the simulation run:
• This will open an empty world with the two r2d2 robots
$ roslaunch gazebo_multi gazebo_multi.launch
(C)2015 Roi Yehoshua 14
Launch Simulation
(C)2015 Roi Yehoshua 15
Launch Simulation
(C)2015 Roi Yehoshua 16
Robots Topics• You can see that both robots publish the base_scan
and cmd_vel topics in the correct namespaces
(C)2015 Roi Yehoshua 17
Moving the Robot with Teleop• Now 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
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/robot2/cmd_vel
(C)2015 Roi Yehoshua 18
Moving the Robot with Teleop
(C)2015 Roi Yehoshua 19
Node controlling the robot• We will now add a node that will make one of the
robots start random walking in the environment• The 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 topics
• Add random_walk.cpp to your package
(C)2015 Roi Yehoshua 20
random_walk.cpp (1)#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);
(C)2015 Roi Yehoshua 21
random_walk.cpp (2)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);
(C)2015 Roi Yehoshua 22
random_walk.cpp (3)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;}
(C)2015 Roi Yehoshua 23
random_walk.cpp (4)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; }}
(C)2015 Roi Yehoshua 24
Launch Random Walk Node• To launch the random walk node type:
$ rosrun gazebo_multi random_walk [robot name]
(C)2015 Roi Yehoshua 25
All Robots Random Walk• 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>
(C)2015 Roi Yehoshua 26
Running Navigation Stack in Gazebo• We will now see how to run the navigation stack in
Gazebo with multiple robots• Create a new package gazebo_navigation_multi• Copy launch files (gazebo_multi.launch, robots.launch,
one_robot.launch) from the launch directory in gazebo_multi package
• Rename gazebo_multi.launch to navigation_multi.launch
• Copy the folder move_base_config from navigation_multi package
(C)2015 Roi Yehoshua 27
gazebo_navigation_multi package
(C)2015 Roi Yehoshua 28
gazebo_navigation_multi launch files
(C)2015 Roi Yehoshua 29
Use Static Map• We will first run navigation stack with a static
known map• Create 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
(C)2015 Roi Yehoshua 30
navigation_multi.launch
<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>
(C)2015 Roi Yehoshua 31
robots.launch<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>
(C)2015 Roi Yehoshua 32
one_robot.launch• Copy navigation stack nodes (move_base,
fake_localization or amcl) to one_robot.launch• Add a static transform publisher between
base_link and laser_link frames
(C)2015 Roi Yehoshua 33
TF Tree• For the navigation stack to work properly, the robot
needs to publish the following transformations:map
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 file
(C)2015 Roi Yehoshua 34
one_robot.launch (1)<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>
(C)2015 Roi Yehoshua 35
one_robot.launch (2) <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>
(C)2015 Roi Yehoshua 36
TF Tree• The TF tree should now look like this:
(C)2015 Roi Yehoshua 37
Map in Gazebo and rviz• By 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 corner• The 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 package
(C)2015 Roi Yehoshua 38
Change Map’s Pose in Gazebo• Create worlds directory in your package• Copy willowgarage.world file from gazebo’s worlds
directory to worlds directory of your package
• Now 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 degrees
$ roscd gazebo_navigation_multi/worlds$ cp /usr/share/gazebo-1.9/worlds/willowgarage.world .
(C)2015 Roi Yehoshua 39
willowgarage.world
<?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>
(C)2015 Roi Yehoshua 40
Change Map’s Pose in Gazebo• We also need to update the launch file of Gazebo’s
world to launch our own version of the world file• First copy willowgarage_world.launch from
gazebo_ros package to the launch directory of your package
• Now edit this file
$ roscd gazebo_ros/launch$ cp willowgarage_world.launch ~/catkin_ws/src/gazebo_navigation_multi/launch
(C)2015 Roi Yehoshua 41
willowgarage_world.launch
<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>
(C)2015 Roi Yehoshua 42
Running the Navigation Stack• Now we are ready to run the navigation stack
$ roslaunch gazebo_navigation_multi navigation_multi.launch
(C)2015 Roi Yehoshua 43
Gazebo
(C)2015 Roi Yehoshua 44
rviz
(C)2015 Roi Yehoshua 45
Set Navigation Goal• Open rviz menu – Tool Properties• Change the topic name for the 2D Nav Goal
according to the robot that you want to activate:
(C)2015 Roi Yehoshua 46
Set Navigation Goal
(C)2015 Roi Yehoshua 47
Following the Global Plan
(C)2015 Roi Yehoshua 48
Final Pose
(C)2015 Roi Yehoshua 49
Final Pose in Gazebo
(C)2015 Roi Yehoshua 50
Sending Goals From Terminal• To send a goal to a robot from terminal, you can
publish a message to the topic [robot_name]/move_base_simple/goal
• For example to send a goal command to lizi2:$ 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}}}'
(C)2015 Roi Yehoshua 51
Sending Goals From Terminal
(C)2015 Roi Yehoshua 52
Sending Goals From Code• Copy send_goals.cpp from the package
navigation_multi• Add send_goals.cpp to CMakeLists.txt• Change the name of the executable from send_goal
to gazebo_send_goal– You cannot have two executables in the same workspace
• Compile the package• Now you can send goals to robots by running the
gazebo_send_goal node
(C)2015 Roi Yehoshua 53
Sending Goals From Code