Teaching Assistant: Roi Yehoshua

Spanning multiple robots in Gazebo Controlling multiple robots in Gazebo Running navigation stack in Gazebo with multiple robots Sending goals to robots in Gazebo (C)2015 Roi Yehoshua2

Gazebo is a 3D multi-robot simulator Gazebo ROS Indigo comes with Gazebo V2 Composed of two processes: – Server: Runs the physics loop and generates sensor data – Client: Provides user interaction and visualization of a simulation (C)2015 Roi Yehoshua3

You can use roslaunch to load world models For example, to open willowgarage_world type: There are other 5 world files examples in the /launch subdirectory of gazebo_ros package (C)2015 Roi Yehoshua $ roslaunch gazebo_ros willowgarage_world.launch 4

Typically, each robot comes with a robot description package that contains its URDF and mesh files for simulation and visualization We will use r2d2_description package that we have built in the Introduction course This package contains r2d2 URDF file and the mesh file for the Hokuyo laser You can download this package from the link: – 685/demos/lesson11/r2d2_description.zip 685/demos/lesson11/r2d2_description.zip (C)2015 Roi Yehoshua5

6

Make sure that all the topics and frames specified in the URDF file are without the /, otherwise they will be shared, e.g.: base_scan hokuyo_link base_scan hokuyo_link 7

Let’s create a new package called gazebo_multi Create a launch subdirectory within the package and add the following launch file called one_robot.launch (C)2015 Roi Yehoshua $ cd ~/catkin_ws/src $ catkin_create_pkg gazebo_multi std_msgs rospy roscpp $ cd ~/catkin_ws/src $ catkin_create_pkg gazebo_multi std_msgs rospy roscpp 8

To work with the robot model in ROS, we need to publish its joint states and TF tree For that purpose we need to start two nodes: – a joint_state_publisher node that reads the robot’s model from its URDF file and publishes /joint_states messages – a robot_state_publisher node that listens to /joint_states messages and publishes the transforms to /tf (C)2015 Roi Yehoshua9

<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"/> There are two arguments to pass for each robot’s initialization: name and position 10

(C)2015 Roi Yehoshua 11

gazebo_multi.launch (C)2015 Roi Yehoshua 12

To launch the simulation run: This will open an empty world with the two r2d2 robots (C)2015 Roi Yehoshua $ roslaunch gazebo_multi gazebo_multi.launch 13

(C)2015 Roi Yehoshua14

(C)2015 Roi Yehoshua15

You can see that both robots publish the base_scan and cmd_vel topics in the correct namespaces (C)2015 Roi Yehoshua16

Now we are going to move one of the robots using the teleop_twist_keyboard node Run the following command: You should see console output that gives you the key-to-control mapping (C)2015 Roi Yehoshua $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/robot2/cmd_vel 17

(C)2015 Roi Yehoshua18

(C)2015 Roi Yehoshua We will now add a node that will make one of the robots start random walking in the environment The code of the node is the same as the one we used to control the robot in Stage simulator – Since the robots are using the same topics Add random_walk.cpp to your package 19

(C)2015 Roi Yehoshua #include using namespace std; #define MIN_SCAN_ANGLE_RAD -45.0/180*M_PI #define MAX_SCAN_ANGLE_RAD +45.0/180*M_PI bool obstacleFound = false; void readSensorCallback(const sensor_msgs::LaserScan::ConstPtr &sensor_msg); #include using namespace std; #define MIN_SCAN_ANGLE_RAD -45.0/180*M_PI #define MAX_SCAN_ANGLE_RAD +45.0/180*M_PI bool obstacleFound = false; void readSensorCallback(const sensor_msgs::LaserScan::ConstPtr &sensor_msg); 20

(C)2015 Roi Yehoshua int main(int argc, char **argv) { if (argc < 2) { ROS_ERROR("You must specify robot id."); return -1; } char *robot_name = argv[1]; ROS_INFO("Moving robot %s", robot_name); ros::init(argc, argv, "random_walk"); ros::NodeHandle nh; string cmd_vel_topic_name = robot_name; cmd_vel_topic_name += "/cmd_vel"; ros::Publisher cmd_vel_pub = nh.advertise (cmd_vel_topic_name, 10); string laser_topic_name = robot_name; laser_topic_name += "/base_scan"; ros::Subscriber base_scan_sub = nh.subscribe (laser_topic_name, 1, &readSensorCallback); int main(int argc, char **argv) { if (argc < 2) { ROS_ERROR("You must specify robot id."); return -1; } char *robot_name = argv[1]; ROS_INFO("Moving robot %s", robot_name); ros::init(argc, argv, "random_walk"); ros::NodeHandle nh; string cmd_vel_topic_name = robot_name; cmd_vel_topic_name += "/cmd_vel"; ros::Publisher cmd_vel_pub = nh.advertise (cmd_vel_topic_name, 10); string laser_topic_name = robot_name; laser_topic_name += "/base_scan"; ros::Subscriber base_scan_sub = nh.subscribe (laser_topic_name, 1, &readSensorCallback); 21

(C)2015 Roi Yehoshua geometry_msgs::Twist moveForwardCommand; moveForwardCommand.linear.x = 1.0; geometry_msgs::Twist turnCommand; turnCommand.angular.z = 1.0; ros::Rate loop_rate(10); while (ros::ok()) { if (obstacleFound) { ROS_INFO("Turning around"); cmd_vel_pub.publish(turnCommand); } else { ROS_INFO("Moving forward"); cmd_vel_pub.publish(moveForwardCommand); } ros::spinOnce(); // let ROS process incoming messages loop_rate.sleep(); } return 0; } geometry_msgs::Twist moveForwardCommand; moveForwardCommand.linear.x = 1.0; geometry_msgs::Twist turnCommand; turnCommand.angular.z = 1.0; ros::Rate loop_rate(10); while (ros::ok()) { if (obstacleFound) { ROS_INFO("Turning around"); cmd_vel_pub.publish(turnCommand); } else { ROS_INFO("Moving forward"); cmd_vel_pub.publish(moveForwardCommand); } ros::spinOnce(); // let ROS process incoming messages loop_rate.sleep(); } return 0; } 22

(C)2015 Roi Yehoshua void readSensorCallback(const sensor_msgs::LaserScan::ConstPtr &scan) { bool isObstacle = false; int minIndex = ceil((MIN_SCAN_ANGLE_RAD - scan->angle_min) / scan- >angle_increment); int maxIndex = floor((MAX_SCAN_ANGLE_RAD - scan->angle_min) / scan- >angle_increment); for (int i = minIndex; i <= maxIndex; i++) { if (scan->ranges[i] < 0.5) { isObstacle = true; } if (isObstacle) { ROS_INFO("Obstacle in front of you!"); obstacleFound = true; } else { obstacleFound = false; } void readSensorCallback(const sensor_msgs::LaserScan::ConstPtr &scan) { bool isObstacle = false; int minIndex = ceil((MIN_SCAN_ANGLE_RAD - scan->angle_min) / scan- >angle_increment); int maxIndex = floor((MAX_SCAN_ANGLE_RAD - scan->angle_min) / scan- >angle_increment); for (int i = minIndex; i <= maxIndex; i++) { if (scan->ranges[i] < 0.5) { isObstacle = true; } if (isObstacle) { ROS_INFO("Obstacle in front of you!"); obstacleFound = true; } else { obstacleFound = false; } 23

(C)2015 Roi Yehoshua To launch the random walk node type: $ rosrun gazebo_multi random_walk [robot name] 24

(C)2015 Roi Yehoshua To make all robots random walk, you can add the random_walk node to one_robot.launch file <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"/> 25

We will now see how to run the navigation stack in Gazebo with multiple robots Create a new package gazebo_navigation_multi Copy launch files (gazebo_multi.launch, robots.launch, one_robot.launch) from the launch directory in gazebo_multi package Rename gazebo_multi.launch to navigation_multi.launch Copy the folder move_base_config from navigation_multi package 26(C)2015 Roi Yehoshua

27(C)2015 Roi Yehoshua

28(C)2015 Roi Yehoshua

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 29(C)2015 Roi Yehoshua

30(C)2015 Roi Yehoshua

31(C)2015 Roi Yehoshua

Copy navigation stack nodes (move_base, fake_localization or amcl) to one_robot.launch Add a static transform publisher between base_link and laser_link frames 32(C)2015 Roi Yehoshua

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

34(C)2015 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"/>

35(C)2015 Roi Yehoshua

36(C)2015 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 37(C)2015 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 38(C)2015 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.

39(C)2015 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 40(C)2015 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

41(C)2015 Roi Yehoshua

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

43(C)2015 Roi Yehoshua

44(C)2015 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: 45(C)2015 Roi Yehoshua

46(C)2015 Roi Yehoshua

47(C)2015 Roi Yehoshua

48(C)2015 Roi Yehoshua

49(C)2015 Roi Yehoshua

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: 50(C)2015 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}}}'

51(C)2015 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 52(C)2015 Roi Yehoshua

53(C)2015 Roi Yehoshua