Multi-Robot Systems with ROS Lesson 4
description
Transcript of Multi-Robot Systems with ROS Lesson 4
(C)2015 Roi Yehoshua
Agenda• ROS navigation stack with multiple robots• Sending goals to robots
(C)2015 Roi Yehoshua
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)2015 Roi Yehoshua
ROS Navigation Stack
(C)2015 Roi Yehoshua
Navigation Stack Main Componentscomponent
offers map data as a ROS Service map_server
provides laser-based SLAM (Simultaneous Localization and Mapping)
gmapping
a probabilistic localization system amcl
implementation of a fast global planner for navigation global_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
(C)2015 Roi Yehoshua
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)2015 Roi Yehoshua
move_base_multi_robot.launch (1)
<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>
– Remove the first argument in the stage_ros node to make stage’s window visible
• Nodes that are common to all robots:
(C)2015 Roi Yehoshua
move_base_multi_robot.launch (2)
<!-- 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>
• Nodes for controlling robot 0:
(C)2015 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> <!-- END ROBOT 0 -->
(C)2015 Roi Yehoshua
move_base_multi_robot.launch (4)
<!-- 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>
• Nodes for controlling robot 1:
(C)2015 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> <!-- END ROBOT 1 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation_stage)/multi_robot.rviz" />
</launch>
(C)2015 Roi Yehoshua
Fake Localization• 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
Running the Navigation Stack• To run this launch file type:
$ roslaunch navigation_stage move_base_multi_robot.launch
(C)2015 Roi Yehoshua
Running the Navigation Stack
(C)2015 Roi Yehoshua
rviz
(C)2015 Roi Yehoshua
Map in Stage and rviz
(C)2015 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)2015 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 bitmapfloorplan( 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")
(C)2015 Roi Yehoshua
Map in Stage and rviz
(C)2015 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)2015 Roi Yehoshua
Robot Footprint
(C)2015 Roi Yehoshua
TF• Add the TF display to watch the TF tree
(C)2015 Roi Yehoshua
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)2015 Roi Yehoshua
Sending Goals
(C)2015 Roi Yehoshua
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)2015 Roi Yehoshua
Sending Goals
(C)2015 Roi Yehoshua
Sending Goals
(C)2015 Roi Yehoshua
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)2015 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
• 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_pkg navigation_multi roscpp rospy tf actionlib move_base_msgs
(C)2015 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-pr2-
multi.world– From the root directory copy multi_robot.rviz
(C)2015 Roi Yehoshua
Package Directory Structure
(C)2015 Roi Yehoshua
Integrating with move_base• move_base_config files:
(C)2015 Roi Yehoshua
• Fix move_base.xml to use the correct package:
Integrating with move_base
(C)2015 Roi Yehoshua
Integrating with move_base• Fix the package name also in the launch file:
(C)2015 Roi Yehoshua
Check Package Configuration• Test that all the configuration is correct by
running the launch file:$ roslaunch navigation_multi navigation_multi.launch
(C)2015 Roi Yehoshua
Sending Goals From Code• Open the project file in Eclipse• Under the src subdirectory, create a new file
called send_goal.cpp
(C)2015 Roi Yehoshua
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)2015 Roi Yehoshua
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> typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> 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;
(C)2015 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 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)2015 Roi Yehoshua
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)2015 Roi Yehoshua
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 executableadd_executable(send_goal src/send_goal.cpp)…## Specify libraries to link a library or executable target againsttarget_link_libraries(send_goal ${catkin_LIBRARIES})
$ rosrun navigation_multi send_goal 1
(C)2015 Roi Yehoshua
Running send_goal node
(C)2015 Roi Yehoshua
Running send_goal node• Initial position:
(C)2015 Roi Yehoshua
Running send_goal node• In the middle of the path:
(C)2015 Roi Yehoshua
Running send_goal node• Final position:
(C)2015 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)2015 Roi Yehoshua
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)2015 Roi Yehoshua
send_goal Parameters• Now roslaunch send_goal.launch:
(C)2015 Roi Yehoshua
send_goal Parameters
(C)2015 Roi Yehoshua
ROS Graph• Graph of nodes running in the system: