MultiRobot Systems with ROS Lesson 8 Teaching Assistant

Multi-Robot Systems with ROS Lesson 8 Teaching Assistant: Roi Yehoshua roiyeho@gmail. com

Agenda • Running navigation stack in Gazebo with multiple robots • Sending goals to

lizi_description package (C)2014 Roi Yehoshua

Setting Up Navigation Stack • Copy the package navigation_multi into a new package called gazebo_navigation_multi • Change the name of the package in package. xml, CMake. Lists. txt and move_base. xml • Remove stage_config subdirectory • Copy launch files (gazebo_multi. launch, robots. launch, one_robot. launch) from the launch directory in gazebo_multi package • Change package name in launch files to gazebo_navigation_multi • Rename gazebo_multi. launch to navigation_multi. launch (C)2014 Roi Yehoshua 5

gazebo_navigation_multi package (C)2014 Roi Yehoshua 6

gazebo_navigation_multi launch files (C)2014 Roi Yehoshua 7

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)2014 Roi Yehoshua 8

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> (C)2014 Roi Yehoshua 9

robots. launch <launch>  <group ns="lizi 1"> <param name="tf_prefix" value="lizi 1" /> <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="lizi 1" /> </include> </group>  <group ns="lizi 2"> <param name="tf_prefix" value="lizi 2" /> <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="lizi 2" /> </include> </group>  <group ns="lizi 3"> <param name="tf_prefix" value="lizi 3" /> <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="lizi 3" /> </include> </group> </launch> (C)2014 Roi Yehoshua 10

one_robot. launch • Copy navigation stack nodes (move_base, fake_localization) to one_robot. launch • Add a static transform publisher between base_link and laser_link frames (will be explained later) (C)2014 Roi Yehoshua 11

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> (C)2014 Roi Yehoshua 12

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)2014 Roi Yehoshua 13

TF Tree • For the navigation stack to work properly, the robot needs to publish the following transformations: map published by the localization system robot. X/odom published by Gazebo’s driver controller robot. X/base_link robot. X/laser_link (C)2014 Roi Yehoshua published by static tf defined in your launch file 14

Gazebo’s Driver Controllers • http: //gazebosim. org/wiki/Tutorials/1. 9/ROS_Motor _and_Sensor_Plugins • The driver controller is specified in the robot’s URDF file • There are different Gazebo built-in driver plugins that you can use, including: – differential_drive_controller – a basic controller for differential drive robots (useful for robots with 2 wheels) – skid_steer_drive_controller – a basic controller for skid steering drive robots in Gazebo (useful for robots with 4 wheels, such as Pioneer 3 AT) • The driver controller is automatically publishing the transformation from odom to base_link (C)2014 Roi Yehoshua 15

Driver Plugin Definition <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive. so"> <update. Rate>100. 0</update. Rate> <left. Front. Joint>front_left_joint</left. Front. Joint> <right. Front. Joint>front_right_joint</right. Front. Joint> <left. Rear. Joint>rear_left_joint</left. Rear. Joint> <right. Rear. Joint>rear_right_joint</right. Rear. Joint> <wheel. Separation>0. 255</wheel. Separation> <wheel. Diameter>0. 154</wheel. Diameter> <odometry. Topic>odom</odometry. Topic> <odometry. Frame>odom</odometry. Frame> <robot. Base. Frame>base_link</robot. Base. Frame> <torque>500</torque> <command. Topic>cmd_vel</command. Topic> <broadcast. TF>1</broadcast. TF> </plugin> (C)2014 Roi Yehoshua 16

Gazebo position_3 d Controller • In addition, you need to make Gazebo publish the robot’s position to ROS • The easiest way to do this is with the position_3 d controller • Add the following to your URDF file: <plugin name="p 3 d_base_controller" filename="libgazebo_ros_p 3 d. so"> <always. On>true</always. On> <update. Rate>100. 0</update. Rate> <body. Name>base_link</body. Name> <topic. Name>base_pose_ground_truth</topic. Name> <gaussian. Noise>0</gaussian. Noise> <frame. Name>/map</frame. Name> <xyz. Offsets>0 0 0</xyz. Offsets> <rpy. Offsets>0 0 0</rpy. Offsets> </plugin> (C)2014 Roi Yehoshua 17

Laser Scanner Controller • The gazebo model, written in URDF, makes use of an Hokuyo laser scanner through the libgazebo_ros_laser. so plugin • The frame. Name property of the plugin specifies the TF frame that corresponds to the laser link <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_laser. so"> <always. On>true</always. On> <update. Rate>40</update. Rate> <topic. Name>base_scan</topic. Name> <frame. Name>laser_link</frame. Name> </plugin> (C)2014 Roi Yehoshua 18

Laser Scanner Controller • However, the laser plugin doesn't automatically publishes a transform from base_link to the frame that you specified in the file (laser_link) • Thus we need to manually specify a static transform publisher between the laser_link and base_link frames • Add this publisher to one_robot. launch file (C)2014 Roi Yehoshua 19

Laser Scanner Controller <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"> … </node> <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="link_broadcaster" args="0 0 1 0 0 0 $(arg robot_name)/base_link $(arg robot_name)/laser_link 100" /> </launch> (C)2014 Roi Yehoshua 20

TF Tree • The TF tree should now look like this: (C)2014 Roi Yehoshua

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)2014 Roi Yehoshua 22

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 $ roscd gazebo_navigation_multi/worlds $ cp /usr/share/gazebo-1. 9/worlds/willowgarage. world. • 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 (C)2014 Roi Yehoshua 23

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)2014 Roi Yehoshua 24

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 $ roscd gazebo_ros/launch $ cp willowgarage_world. launch ~/catkin_ws/src/gazebo_navigation_multi/launch • Now edit this file (C)2014 Roi Yehoshua 25

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> (C)2014 Roi Yehoshua 26

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

Set Navigation Goal • Open rviz menu – Tool Properties • Change the topic

Set Navigation Goal (C)2014 Roi Yehoshua 31

Following the Global Plan (C)2014 Roi Yehoshua 32

Final Pose in Gazebo (C)2014 Roi Yehoshua 34

Change Speed Limits • To change the speed limits of the robot used by the navigation stack, open the file move_base_config/base_local_planner_params. yaml • There are 4 parameters that determine the speed limits – max_vel_x: maximum linear velocity – min_vel_x: minimum linear velocity – max_rotational_vel: maximum angular velocity – min_in_place_rotational_vel: minimum angular velocity (C)2014 Roi Yehoshua 35

Change Speed Limits • For the lizi robot we are going to change the following speed limits: #Set the velocity limits of the robot max_vel_x: 1. 5 min_vel_x: 0. 5 max_rotational_vel: 1. 5 min_in_place_rotational_vel: 1. 0 (C)2014 Roi Yehoshua 36

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 lizi 2: $ rostopic pub /lizi 2/move_base_simple/goal geometry_msgs/Pose. Stamped '{header: {frame_id: "map"}, pose: {position: {x: 22, y: 17, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 1}}}' (C)2014 Roi Yehoshua 37

Sending Goals From Terminal (C)2014 Roi Yehoshua 38

Sending Goals From Code • Copy send_goals. cpp from the package navigation_multi • Add send_goals. cpp to CMake. Lists. 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)2014 Roi Yehoshua 39

Sending Goals From Code (C)2014 Roi Yehoshua 40

AMCL • amcl is a probabilistic localization system for a robot moving in 2 D • It implements the Adaptive Monte Carlo localization approach, which uses a particle filter to track the pose of a robot against a known map • To use amcl, in one_robot. launch remove fake_localization node and include amcl_node. xml from move_base_config directory (C)2013 Roi Yehoshua

AMCL <launch> …  <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen"> … </node>  <include file="$(find gazebo_navigation_multi)/move_base_config/amcl_node. xml"/> <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)2013 Roi Yehoshua

AMCL • Localization made by AMCL needs some help in providing the initial position of the robots on the map • For this purpose we will introduce 3 params carrying those values: initial_pose_x, initial_pose_y and initial_pose_a (their default value is 0) – We will define them in robots. launch file • Amcl node runs is in the robot namespace and thus the whole path of these params would be like /robot. X/amcl/initial_pose_x (C)2013 Roi Yehoshua

robots. launch <launch>  <group ns="lizi 1"> <param name="tf_prefix" value="lizi 1" /> <param name="amcl/initial_pose_x" value="20" /> <param name="amcl/initial_pose_y" value="15" /> <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="lizi 1" /> </include> </group>  … </launch> (C)2013 Roi Yehoshua

Set Initial Pose in rviz • You can also set the initial pose in rviz • First open Tool Properties and change the 2 D Pose Estimate topic to the specific robot’s initialpose topic • Then you can use the 2 D Pose Estimate button to specify the initial pose of the robot (C)2013 Roi Yehoshua

Multi Robot Collision Avoidance • http: //wiki. ros. org/multi_robot_collision_avoidance • This package includes a patch to AMCL that can take into account other robots navigation paths • The AMCL patch is necessary to publish a Weighted Point Cloud needed for the calculation • Multi Robot Collision Avoidance Demo • No version of this package for ROS Hydro yet – Opportunity to make some contribution to the community! (C)2014 Roi Yehoshua 46

Homework (not for submission) • Spawn 3 Pioneer robots in Gazebo • Add all the necessary Gazebo controllers and definitions to the robot’s URDF file • Run ROS navigation stack on these robots (without amcl and with amcl) (C)2014 Roi Yehoshua