Teaching Assistant: Roi Yehoshua

Running navigation stack in Gazebo with multiple robots Sending goals to robots in Gazebo Using amcl Multi robot collision avoidance in navigation stack (C)2014 Roi Yehoshua

Copy the package navigation_multi into a new package called gazebo_navigation_multi Change the name of the package in package.xml, CMakeLists.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 5(C)2014 Roi Yehoshua

6

7

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

9

10(C)2014 Roi Yehoshua

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

12(C)2014 Roi Yehoshua <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/> <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/>

13(C)2014 Roi Yehoshua

For the navigation stack to work properly, the robot needs to publish the following transformations: 14(C)2014 Roi Yehoshua 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

_and_Sensor_Plugins _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 3AT) The driver controller is automatically publishing the transformation from odom to base_link 15(C)2014 Roi Yehoshua

16(C)2014 Roi Yehoshua front_left_joint front_right_joint rear_left_joint rear_right_joint odom base_link 500 cmd_vel front_left_joint front_right_joint rear_left_joint rear_right_joint odom base_link 500 cmd_vel 1

In addition, you need to make Gazebo publish the robot’s position to ROS The easiest way to do this is with the position_3d controller Add the following to your URDF file: 17(C)2014 Roi Yehoshua true base_link base_pose_ground_truth 0 /map true base_link base_pose_ground_truth 0 /map 0 0 0

The gazebo model, written in URDF, makes use of an Hokuyo laser scanner through the libgazebo_ros_laser.so plugin The frameName property of the plugin specifies the TF frame that corresponds to the laser link 18(C)2014 Roi Yehoshua true 40 base_scan laser_link true 40 base_scan laser_link

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

20(C)2014 Roi Yehoshua <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/> … <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/> …

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

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

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: – Angles are specified in degrees 23(C)2014 Roi Yehoshua $ roscd gazebo_navigation_multi/worlds $ cp /usr/share/gazebo-1.9/worlds/willowgarage.world. $ roscd gazebo_navigation_multi/worlds $ cp /usr/share/gazebo-1.9/worlds/willowgarage.world.

24(C)2014 Roi Yehoshua model://ground_plane model://sun model://willowgarage model://ground_plane model://sun model://willowgarage

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

26(C)2014 Roi Yehoshua

Now we are ready to run the navigation stack (C)2014 Roi Yehoshua $ roslaunch gazebo_navigation_multi navigation_multi.launch

28(C)2014 Roi Yehoshua

29(C)2014 Roi Yehoshua

Open rviz menu – Tool Properties Change the topic name for the 2D Nav Goal according to the robot that you want to activate: 30(C)2014 Roi Yehoshua

31(C)2014 Roi Yehoshua

32(C)2014 Roi Yehoshua

33(C)2014 Roi Yehoshua

34(C)2014 Roi Yehoshua

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

For the lizi robot we are going to change the following speed limits: 36(C)2014 Roi Yehoshua #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 #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

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: 37(C)2014 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}}}'

38(C)2014 Roi Yehoshua

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

40(C)2014 Roi Yehoshua

amcl is a probabilistic localization system for a robot moving in 2D 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

… … --> … … -->

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 /robotX/amcl/initial_pose_x (C)2013 Roi Yehoshua

… …

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

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

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