1 / 52

Multi-Robot Systems with ROS Lesson 6

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 to robots. ROS Navigation Stack. http://wiki.ros.org/navigation

xia
Download Presentation

Multi-Robot Systems with ROS Lesson 6

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

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

  2. Agenda • ROS navigation stack with multiple robots • Using rviz • Sending goals to robots (C)2014 Roi Yehoshua

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

  4. ROS Navigation Stack (C)2014 Roi Yehoshua

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

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

  7. Navigation Stack with Multiple Robots • Download the navigation tutorials from git • 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 • $ cd ~/ros/stacks • $ git clone https://github.com/ros-planning/navigation_tutorials.git (C)2014 Roi Yehoshua

  8. move_base_multi_robot.launch (1) • Remove the first argument in the stage_ros node to make stage’s window visible • 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-pr2-multi.world" respawn="false"> • <param name="base_watchdog_timeout" value="0.2"/> • </node> (C)2014 Roi Yehoshua

  9. move_base_multi_robot.launch (2) • Nodes for controlling robot 0: • <!-- BEGIN 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

  10. 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> • <!-- END ROBOT 0 --> (C)2014 Roi Yehoshua

  11. move_base_multi_robot.launch (4) • Nodes for controlling robot 1: • <!-- BEGIN 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

  12. 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> • <!-- END ROBOT 1 --> • <node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation_stage)/multi_robot.rviz" /> • </launch> (C)2014 Roi Yehoshua

  13. Running the Navigation Stack • To run this launch file type: • $ roslaunchnavigation_stagemove_base_multi_robot.launch (C)2014 Roi Yehoshua

  14. Running the Navigation Stack (C)2014 Roi Yehoshua

  15. rviz (C)2014 Roi Yehoshua

  16. Using rviz with Navigation Stack • rviz is a ROS 3D 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

  17. Map in Stage and rviz (C)2014 Roi Yehoshua

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

  19. 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 • #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") (C)2014 Roi Yehoshua

  20. Map in Stage and rviz (C)2014 Roi Yehoshua

  21. 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 inmove_base_config/costmap_common_params.yaml (C)2014 Roi Yehoshua

  22. Robot Footprint (C)2014 Roi Yehoshua

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

  24. Sending Goals • 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)2014 Roi Yehoshua

  25. Sending Goals (C)2014 Roi Yehoshua

  26. Sending Goals • 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)2014 Roi Yehoshua

  27. Sending Goals (C)2014 Roi Yehoshua

  28. Sending Goals (C)2014 Roi Yehoshua

  29. Collision Avoidance • To avoid the robots from colliding into each other, change the following definition in willow-pr2-multi.world: • 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 ]) • ) (C)2014 Roi Yehoshua

  30. 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 • Build the package by calling catkin_make • Open the package in Eclipse and add a new source file called print_location.cpp • $ cd ~/catkin_ws/src • $ catkin_create_pkgnavigation_multiroscpprospytfactionlibmove_base_msgs (C)2014 Roi Yehoshua

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

  32. Package Directory Structure (C)2014 Roi Yehoshua

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

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

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

  36. Check Package Configuration • Test that all the configuration is correct by running the launch file: $ roslaunchnavigation_multinavigation_multi.launch (C)2014 Roi Yehoshua

  37. Sending Goals From Code • Open the project file in Eclipse • Under the src subdirectory, create a new file called send_goal.cpp (C)2014 Roi Yehoshua

  38. Using the Navigation Stack • Open the package in Eclipse and add a new source file called send_goal.cpp • Copy the following code into it (C)2014 Roi Yehoshua

  39. send_goal.cpp (1) #include <ros/ros.h> #include <move_base_msgs/MoveBaseAction.h> #include <actionlib/client/simple_action_client.h> #include <tf/transform_datatypes.h> typedefactionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; using namespace std; int main(intargc, 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::NodeHandlenh; // Define the goal doublegoal_x = 7.45; doublegoal_y = 18.5; doublegoal_theta = 0; (C)2014 Roi Yehoshua

  40. 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 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; (C)2014 Roi Yehoshua

  41. send_goal.cpp (3) // 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; } (C)2014 Roi Yehoshua

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

  43. Running send_goal node (C)2014 Roi Yehoshua

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

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

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

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

  48. send_goal Parameters <launch> <!-- BEGIN ROBOT 0 --> <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> <!-- END ROBOT 0 --> <!-- BEGIN ROBOT 1 --> <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> <!-- END ROBOT 1 --> </launch> (C)2014 Roi Yehoshua

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

  50. send_goal Parameters (C)2014 RoiYehoshua

More Related