MultiRobot Systems with ROS Lesson 6 Teaching Assistant

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

Agenda • ROS navigation stack with multiple robots • Using rviz • Sending goals

ROS Navigation Stack • http: //wiki. ros. org/navigation • The navigation stack handles moving a robot from one position to another position safely (without crashing or getting lost) • It takes in information from odometry and the sensors, and a goal pose and outputs safe velocity commands (C)2014 Roi Yehoshua

ROS Navigation Stack (C)2014 Roi Yehoshua

Navigation Stack Main Components • map_server - offers map data as a ROS Service • gmapping - provides laser-based SLAM (Simultaneous Localization and Mapping) • amcl - a probabilistic localization system • global_planner - implementation of a fast global planner for navigation • local_planner - implementations of the Trajectory Rollout and Dynamic Window approaches to local robot navigation • move_base - links together the global and local planner to accomplish the navigation task (C)2014 Roi Yehoshua

Navigation Stack Requirements • There are three main hardware requirements: – The navigation stack can only handle a differential drive and holonomic wheeled robots. – A planar laser must be mounted on the mobile base of the robot to create the map and localization – Its performance will be best on robots that are nearly square or circular (C)2014 Roi Yehoshua

Navigation Stack with Multiple Robots • Download the navigation tutorials from git – https: //github. com/ros-planning/navigation_tutorials $ cd ~/ros/stacks $ git clone https: //github. com/ros-planning/navigation_tutorials. git • This will create a navigation_stack package • In the launch directory of the package you will find move_base_multi_robot. launch • This is an example launch file for running the navigation stack with multiple robots in stage (C)2014 Roi Yehoshua

move_base_multi_robot. launch (1) • Nodes that are common to all robots: <launch> <master auto="start"/> <param name="/use_sim_time" value="true"/> <node pkg="map_server" type="map_server" name="map_server" args="$(find navigation_stage)/stage_config/maps/willow-full. pgm 0. 1" respawn="false" > <param name="frame_id" value="/map" /> </node> <node pkg="stage_ros" type="stageros" name="stageros" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find navigation_stage)/stage_config/worlds/willow-pr 2 multi. world" respawn="false"> <param name="base_watchdog_timeout" value="0. 2"/> </node> – Remove the first argument in the stage_ros node to make stage’s window visible (C)2014 Roi Yehoshua

move_base_multi_robot. launch (2) • Nodes for controlling robot 0:  <group ns="robot_0"> <param name="tf_prefix" value="robot_0" /> <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 navigation_stage)/move_base_config/costmap_common_params. yaml" command="load" ns="global_costmap" /> <rosparam file="$(find navigation_stage)/move_base_config/costmap_common_params. yaml" command="load" ns="local_costmap" /> <rosparam file="$(find navigation_stage)/move_base_config/local_costmap_params. yaml" command="load" /> <rosparam file="$(find navigation_stage)/move_base_config/global_costmap_params. yaml" command="load" /> <rosparam file="$(find navigation_stage)/move_base_config/base_local_planner_params. yaml" command="load" /> </node> (C)2014 Roi Yehoshua

move_base_multi_robot. launch (3) <node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false" output="screen"> <param name="odom_frame_id" value="robot_0/odom" /> <param name="base_frame_id" value="robot_0/base_link" /> </node> </group>  (C)2014 Roi Yehoshua

move_base_multi_robot. launch (4) • Nodes for controlling robot 1:  <group ns="robot_1"> <param name="tf_prefix" value="robot_1" /> <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 navigation_stage)/move_base_config/costmap_common_params. yaml" command="load" ns="global_costmap" /> <rosparam file="$(find navigation_stage)/move_base_config/costmap_common_params. yaml" command="load" ns="local_costmap" /> <rosparam file="$(find navigation_stage)/move_base_config/local_costmap_params. yaml" command="load" /> <rosparam file="$(find navigation_stage)/move_base_config/global_costmap_params. yaml" command="load" /> <rosparam file="$(find navigation_stage)/move_base_config/base_local_planner_params. yaml" command="load" /> </node> (C)2014 Roi Yehoshua

move_base_multi_robot. launch (5) <node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false"> <param name="odom_frame_id" value="robot_1/odom" /> <param name="base_frame_id" value="robot_1/base_link" /> </node> </group>  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation_stage)/multi_robot. rviz" /> </launch> (C)2014 Roi Yehoshua

Running the Navigation Stack • To run this launch file type: $ roslaunch navigation_stage

Running the Navigation Stack (C)2014 Roi Yehoshua

Using rviz with Navigation Stack • rviz is a ROS 3 D visualization tool that lets you see the world from a robot's perspective • rviz can help you work with the navigation stack, including: – Displaying all the visualization information that the navigation stack provides, such as the global and local plans and the costmaps – Sending goals to the navigation stack – Setting the initial pose of the robot for a localization system like amcl (C)2014 Roi Yehoshua

Map in Stage and rviz (C)2014 Roi Yehoshua

Map in Stage and rviz • By default the origin of the map is different in Stage and rviz • In Stage the origin is by default at the center of the map while in rviz it is at the lower-left corner • The map’s origin in Stage can be changed by adjusting the floorplan pose in its world file • rviz reads the map from the /map topic that is published by map_server • Its origin can be changed in the map’s yaml file (C)2014 Roi Yehoshua

Map in Stage and rviz • Change the map’s pose in Stage world file so the map’s origin will be adjusted to its origin in rviz • Also change the robots’ positions accordingly # load an environment bitmap floorplan ( name "willow" bitmap ". . /maps/willow-full. pgm" size [58. 4 52. 6 0. 5] #pose [ -26. 300 29. 200 0 90. 000 ] pose [ 29. 2 26. 2 0 0 ] ) # throw in a robot #pr 2( pose [ -21. 670 47. 120 0 28. 166 ] name "pr 2_0" color "blue") #pr 2( pose [ -21. 670 48. 120 0 28. 166 ] name "pr 2_1" color "green") #block( pose [ -24. 269 48. 001 0 180. 000 ] color "red") pr 2( pose [ 9. 5 14. 5 0 28. 166 ] name "pr 2_0" color "blue") pr 2( pose [ 9. 5 15. 5 0 28. 166 ] name "pr 2_1" color "green") block( pose [ 12. 5 15. 5 0 180. 000 ] color "red") (C)2014 Roi Yehoshua

Robot Footprint • To see the robot’s footprint in rviz change the robot footprint topic to: /robot_N/move_base_node/local_costmap/footprint_ layer/footprint_stamped • In our case, the robots have a pentagon-shape – Defined in move_base_config/costmap_common_params. yaml (C)2014 Roi Yehoshua

TF • Add the TF display to watch the TF tree (C)2014 Roi Yehoshua

Sending Goals • The 2 D nav goal button allows you to send a goal to the navigation by setting a desired pose for the robot to achieve • By default the goal is published on the topic /move_base_simple/goal • However, when having multiple robots, the topic is /robot_N/move_base_simple/goal • To change the topic name, first enable the Tool Properties panel via the Panels menu (C)2014 Roi Yehoshua

Sending Goals • Change the 2 D Nav Goal topic to /robot_0/move_base_simple/goal • Click on the 2 D Nav Goal button (or press G) and select the map and the goal for the first robot • You can select the x and y position and the end orientation for the robot (C)2014 Roi Yehoshua

Collision Avoidance • To avoid the robots from colliding into each other, change the following definition in willowpr 2 -multi. world: define pr 2 position ( size [0. 65 0. 25] origin [-0. 05 0 0 0] gui_nose 1 drive "omni” topurg(pose [ 0. 275 0. 000 -0. 1 0. 000 ]) ) (C)2014 Roi Yehoshua

Using the Navigation Stack • We will now create a node that will make a given robot to move to a specific location on the map • First create a package called navigation_multi that depends on roscpp, rospy, tf, action_lib and move_base_msgs $ cd ~/catkin_ws/src $ catkin_create_pkg navigation_multi roscpp rospy tf actionlib move_base_msgs • Build the package by calling catkin_make • Open the package in Eclipse and add a new source file called print_location. cpp (C)2014 Roi Yehoshua

Integrating with move_base • Copy the following directories and files from the navigation_stage package to your package: – Copy the entire directory move_base_config – From the launch directory copy move_base_multi_robot. launch – From stage_config/maps copy willow-full. pgm – From stage_config/worlds copy willow-pr 2 multi. world – From the root directory copy multi_robot. rviz (C)2014 Roi Yehoshua

Package Directory Structure (C)2014 Roi Yehoshua

Integrating with move_base • move_base_config files: (C)2014 Roi Yehoshua

Integrating with move_base • Fix move_base. xml to use the correct package: (C)2014 Roi

Integrating with move_base • Fix the package name also in the launch file: (C)2014

Check Package Configuration • Test that all the configuration is correct by running the

Sending Goals From Code • Open the project file in Eclipse • Under the

Using the Navigation Stack • Open the package in Eclipse and add a new

send_goal. cpp (1) #include <ros/ros. h> <move_base_msgs/Move. Base. Action. h> <actionlib/client/simple_action_client. h> <tf/transform_datatypes. h> typedef actionlib: : Simple. Action. Client<move_base_msgs: : Move. Base. Action> Move. Base. Client; using namespace std; int main(int argc, char** argv) { if (argc < 2) { ROS_ERROR("You must specify leader robot id. "); return -1; } char *robot_id = argv[1]; ros: : init(argc, argv, "send_goals"); ros: : Node. Handle nh; // Define the goal double goal_x = 7. 45; double goal_y = 18. 5; double goal_theta = 0; (C)2014 Roi Yehoshua

send_goal. cpp (2) // Create the string "robot_X/move_base" string move_base_str = "/robot_"; move_base_str += robot_id; move_base_str += "/move_base"; // create the action client Move. Base. Client ac(move_base_str, true); // Wait for the action server to become available ROS_INFO("Waiting for the move_base action server"); ac. wait. For. Server(ros: : Duration(5)); ROS_INFO("Connected to move base server"); // Send a goal to move_base_msgs: : Move. Base. Goal goal; goal. target_pose. header. frame_id = "map"; goal. target_pose. header. stamp = ros: : Time: : now(); goal. target_pose. position. x = goal_x; goal. target_pose. position. y = goal_y; (C)2014 Roi Yehoshua

send_goal. cpp (3) // Convert the Euler angle to quaternion double radians = goal_theta * (M_PI/180); tf: : Quaternion quaternion; quaternion = tf: : create. Quaternion. From. Yaw(radians); geometry_msgs: : Quaternion q. Msg; tf: : quaternion. TFTo. Msg(quaternion, q. Msg); goal. target_pose. orientation = q. Msg; ROS_INFO("Sending goal to robot no. %s: x = %f, y = %f, theta = %f", robot_id, goal_x, goal_y, goal_theta); ac. send. Goal(goal); // Wait for the action to return ac. wait. For. Result(); if (ac. get. State() == actionlib: : Simple. Client. Goal. State: : SUCCEEDED) ROS_INFO("You have reached the goal!"); else ROS_INFO("The base failed for some reason"); return 0; } (C)2014 Roi Yehoshua

Compiling the Node • Change the following lines in CMake. Lists. txt: cmake_minimum_required(VERSION 2. 8. 3) project(tf_multi) … ## Declare a cpp executable add_executable(send_goal src/send_goal. cpp) … ## Specify libraries to link a library or executable target against target_link_libraries(send_goal ${catkin_LIBRARIES}) • Then call catkin_make • For example, to send a goal to robot no. 1 type: $ rosrun navigation_multi send_goal 1 (C)2014 Roi Yehoshua

Running send_goal node (C)2014 Roi Yehoshua

Running send_goal node • Initial position: (C)2014 Roi Yehoshua

Running send_goal node • In the middle of the path: (C)2014 Roi Yehoshua

Running send_goal node • Final position: (C)2014 Roi Yehoshua

send_goal Parameters • Now let us make the desired pose of the robot configurable in a launch file, so we can send different goals to the robots from the terminal • You can define parameters for a node by using the <param> tag in the ROS launch file • Create the following send_goals. launch file (C)2014 Roi Yehoshua

send_goal Parameters <launch>  <group ns="robot_0"> <param name="goal_x" value="6. 32" /> <param name="goal_y" value="17. 67" /> <param name="goal_theta" value="0" /> <node pkg="navigation_multi" type="send_goal" respawn="false" name="send_goal" output="screen" args="0"/> </group>   <group ns="robot_1"> <param name="goal_x" value="10. 12" /> <param name="goal_y" value="12. 97" /> <param name="goal_theta" value="45" /> <node pkg="navigation_multi" type="send_goal" respawn="false" name="send_goal" output="screen" args="1"/> </group>  </launch> (C)2014 Roi Yehoshua

send_goal Parameters • Now roslaunch send_goal. launch: (C)2014 Roi Yehoshua

send_goal Parameters (C)2014 Roi Yehoshua

ROS Graph • Graph of nodes running in the system: (C)2014 Roi Yehoshua

Homework – Assignment 1 • Implement a simple line formation control for a team of robots • More details can be found at: http: //u. cs. biu. ac. il/~yehoshr 1/89689/assignment 1/Assignment 1. html (C)2014 Roi Yehoshua