1. Teaching Assistant: Roi Yehoshua roiyeho@gmail.com

2. ROS navigation stack with multiple robots Sending goals to robots (C)2015 Roi Yehoshua

3. 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)2015 Roi Yehoshua

5. component offers map data as a ROS Servicemap_server provides laser-based SLAM (Simultaneous Localization and Mapping) gmapping a probabilistic localization systemamcl implementation of a fast global planner for navigationglobal_planner implementations of the Trajectory Rollout and Dynamic Window approaches to local robot navigation local_planner links together the global and local planner to accomplish the navigation task move_base

6. Download the navigation tutorials from git – https://github.com/ros-planning/navigation_tutorials https://github.com/ros-planning/navigation_tutorials 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)2015 Roi Yehoshua $ cd ~/ros/stacks $ git clone https://github.com/ros-planning/navigation_tutorials.git $ cd ~/ros/stacks $ git clone https://github.com/ros-planning/navigation_tutorials.git

7. (C)2015 Roi Yehoshua – Remove the first argument in the stage_ros node to make stage’s window visible Nodes that are common to all robots:

8. (C)2015 Roi Yehoshua Nodes for controlling robot 0:

9. (C)2015 Roi Yehoshua

10. (C)2015 Roi Yehoshua Nodes for controlling robot 1:

11. (C)2015 Roi Yehoshua

12. This node is most frequently used during simulation as a method to provide perfect localization in a computationally inexpensive manner It creates a link between the global /map frame and the robots /odom frames so they actually become equal – no localization error = pose of robot in its /odom frame equals its pose in the global /map frame Parameters: – odom_frame_id - the name of the odometric frame of the robot (default = “odom”) – base_frame_id – the base frame of the robot (default = “base_link”) (C)2015 Roi Yehoshua

13. To run this launch file type: (C)2015 Roi Yehoshua $ roslaunch navigation_stage move_base_multi_robot.launch

14. (C)2015 Roi Yehoshua

17. 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)2015 Roi Yehoshua

18. 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 (C)2015 Roi Yehoshua # 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.3 0 0 ] ) # throw in a robot #pr2( pose [ -21.670 47.120 0 28.166 ] name "pr2_0" color "blue") #pr2( pose [ -21.670 48.120 0 28.166 ] name "pr2_1" color "green") #block( pose [ -24.269 48.001 0 180.000 ] color "red") pr2( pose [ 9.5 14.5 0 28.166 ] name "pr2_0" color "blue") pr2( pose [ 9.5 15.5 0 28.166 ] name "pr2_1" color "green") block( pose [ 12.5 15.5 0 180.000 ] color "red") # 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.3 0 0 ] ) # throw in a robot #pr2( pose [ -21.670 47.120 0 28.166 ] name "pr2_0" color "blue") #pr2( pose [ -21.670 48.120 0 28.166 ] name "pr2_1" color "green") #block( pose [ -24.269 48.001 0 180.000 ] color "red") pr2( pose [ 9.5 14.5 0 28.166 ] name "pr2_0" color "blue") pr2( pose [ 9.5 15.5 0 28.166 ] name "pr2_1" color "green") block( pose [ 12.5 15.5 0 180.000 ] color "red")

19. (C)2015 Roi Yehoshua

20. 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)2015 Roi Yehoshua

22. Add the TF display to watch the TF tree (C)2015 Roi Yehoshua

23. The 2D 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)2015 Roi Yehoshua

25. Change the 2D Nav Goal topic to /robot_0/move_base_simple/goal Click on the 2D 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)2015 Roi Yehoshua

28. To avoid the robots from colliding into each other, change the following definition in willow- pr2-multi.world: (C)2015 Roi Yehoshua define pr2 position ( size [0.65 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 ]) ) define pr2 position ( size [0.65 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 ]) )

29. 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 Build the package by calling catkin_make Open the package in Eclipse and add a new source file called print_location.cpp (C)2015 Roi Yehoshua $ cd ~/catkin_ws/src $ catkin_create_pkg navigation_multi roscpp rospy tf actionlib move_base_msgs $ cd ~/catkin_ws/src $ catkin_create_pkg navigation_multi roscpp rospy tf actionlib move_base_msgs

30. 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-pr2- multi.world – From the root directory copy multi_robot.rviz (C)2015 Roi Yehoshua

32. move_base_config files: (C)2015 Roi Yehoshua

33. Fix move_base.xml to use the correct package: (C)2015 Roi Yehoshua

34. Fix the package name also in the launch file: (C)2015 Roi Yehoshua

35. Test that all the configuration is correct by running the launch file: (C)2015 Roi Yehoshua $ roslaunch navigation_multi navigation_multi.launch

36. Open the project file in Eclipse Under the src subdirectory, create a new file called send_goal.cpp (C)2015 Roi Yehoshua

37. Open the package in Eclipse and add a new source file called send_goal.cpp Copy the following code into it (C)2015 Roi Yehoshua

38. #include typedef actionlib::SimpleActionClient MoveBaseClient; 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::NodeHandle nh; // Define the goal double goal_x = 7.45; double goal_y = 18.5; double goal_theta = 0; #include typedef actionlib::SimpleActionClient MoveBaseClient; 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::NodeHandle nh; // Define the goal double goal_x = 7.45; double goal_y = 18.5; double goal_theta = 0;

39. (C)2015 Roi Yehoshua // 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 MoveBaseClient ac(move_base_str, true); // Wait for the action server to become available ROS_INFO("Waiting for the move_base action server"); ac.waitForServer(ros::Duration(5)); ROS_INFO("Connected to move base server"); // Send a goal to move_base move_base_msgs::MoveBaseGoal goal; goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = goal_x; goal.target_pose.pose.position.y = goal_y; // 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 MoveBaseClient ac(move_base_str, true); // Wait for the action server to become available ROS_INFO("Waiting for the move_base action server"); ac.waitForServer(ros::Duration(5)); ROS_INFO("Connected to move base server"); // Send a goal to move_base move_base_msgs::MoveBaseGoal goal; goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = goal_x; goal.target_pose.pose.position.y = goal_y;

40. (C)2015 Roi Yehoshua // Convert the Euler angle to quaternion double radians = goal_theta * (M_PI/180); tf::Quaternion quaternion; quaternion = tf::createQuaternionFromYaw(radians); geometry_msgs::Quaternion qMsg; tf::quaternionTFToMsg(quaternion, qMsg); goal.target_pose.pose.orientation = qMsg; ROS_INFO("Sending goal to robot no. %s: x = %f, y = %f, theta = %f", robot_id, goal_x, goal_y, goal_theta); ac.sendGoal(goal); // Wait for the action to return ac.waitForResult(); if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) ROS_INFO("You have reached the goal!"); else ROS_INFO("The base failed for some reason"); return 0; } // Convert the Euler angle to quaternion double radians = goal_theta * (M_PI/180); tf::Quaternion quaternion; quaternion = tf::createQuaternionFromYaw(radians); geometry_msgs::Quaternion qMsg; tf::quaternionTFToMsg(quaternion, qMsg); goal.target_pose.pose.orientation = qMsg; ROS_INFO("Sending goal to robot no. %s: x = %f, y = %f, theta = %f", robot_id, goal_x, goal_y, goal_theta); ac.sendGoal(goal); // Wait for the action to return ac.waitForResult(); if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) ROS_INFO("You have reached the goal!"); else ROS_INFO("The base failed for some reason"); return 0; }

41. Change the following lines in CMakeLists.txt: Then call catkin_make For example, to send a goal to robot no.1 type: (C)2015 Roi Yehoshua 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}) 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}) $ rosrun navigation_multi send_goal 1

42. (C)2015 Roi Yehoshua

43. Initial position:

44. (C)2015 Roi Yehoshua In the middle of the path:

45. (C)2015 Roi Yehoshua Final position:

46. 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 tag in the ROS launch file Create the following send_goals.launch file (C)2015 Roi Yehoshua

48. Now roslaunch send_goal.launch: (C)2015 Roi Yehoshua

50. Graph of nodes running in the system: (C)2015 Roi Yehoshua