Multi-Robot Systems with ROS Lesson 5

Multi-Robot Systems with ROS Lesson 5

Teaching Assistant: Roi [email protected]

Summer 2015

mailto:[email protected]

(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


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

http://gazebosim.org/wiki


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


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

http://u.cs.biu.ac.il/~yehoshr1/89-685/demos/lesson11/r2d2_description.zip




r2d2 URDF Structure


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>


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


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


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"/> </launch>

• There are two arguments to pass for each robot’s initialization: name and position


Robots Launch File<launch>  <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>

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


• gazebo_multi.launch

Simulation Launch File

<launch> <param name="/use_sim_time" value="true" />

 <include file="$(find gazebo_ros)/launch/willowgarage_world.launch"/>

 <include file="$(find gazebo_multi)/launch/robots.launch"/></launch>


Launch Simulation• To launch the simulation run:

• This will open an empty world with the two r2d2 robots

$ roslaunch gazebo_multi gazebo_multi.launch


Launch Simulation


Launch Simulation


Robots Topics• You can see that both robots publish the base_scan

and cmd_vel topics in the correct namespaces


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


Moving the Robot with Teleop


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


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);


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);


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;}


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; }}


Launch Random Walk Node• To launch the random walk node type:

$ rosrun gazebo_multi random_walk [robot name]


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"/>  <node name="random_walk" pkg="gazebo_multi" type="random_walk" args="$(arg robot_name)" output="screen"/></launch>


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


gazebo_navigation_multi package


gazebo_navigation_multi launch files


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


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>  <include file="$(find gazebo_navigation_multi)/launch/willowgarage_world.launch"/>

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


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

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

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


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


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


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"/>

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


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>


TF Tree• The TF tree should now look like this:


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


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 .


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>


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


willowgarage_world.launch

<launch>

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


Running the Navigation Stack• Now we are ready to run the navigation stack

$ roslaunch gazebo_navigation_multi navigation_multi.launch


Gazebo


rviz


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:


Set Navigation Goal


Following the Global Plan


Final Pose


Final Pose in Gazebo


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}}}'


Sending Goals From Terminal


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


Sending Goals From Code

Date post:	23-Feb-2016
Category:	Documents
Upload:	ginny
View:	112 times
Download:	11 times

Multi-Robot Systems with ROS Lesson 5

Documents