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

2. Gazebo 3D Simulator Spanning multiple robots in Gazebo Controlling multiple robots in Gazebo (C)2014 Roi Yehoshua

3. Gazebo is a 3D multi-robot simulator Gazebo ROS Hydro comes with Gazebo V1.9.1 Composed of two processes: – Server: Runs the physics loop and generates sensor data – Client: Provides user interaction and visualization of a simulation (C)2014 Roi Yehoshua

4. To launch gazebo (both client and server components) run: (C)2013 Roi Yehoshua $ gazebo

5. 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)2014 Roi Yehoshua $ roslaunch gazebo_ros willowgarage_world.launch

6. To add a model to the world: – Move to the Insert tab – left-click on the desired model in the Insert Tab – move the cursor to the desired location in World View – left-click again to release – Use the Translate and Rotate modes to orient the model more precisely (C)2013 Roi Yehoshua

7. For example, we will add two Pioneer robots to our world (C)2013 Roi Yehoshua

8. 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: – http://u.cs.biu.ac.il/~yehoshr1/89- 685/demos/lesson11/r2d2_packages.zip http://u.cs.biu.ac.il/~yehoshr1/89- 685/demos/lesson11/r2d2_packages.zip (C)2014 Roi Yehoshua

9. Make sure that the laser topic used in the URDF file does is without the /, otherwise it will be shared base_scan hokuyo_link base_scan hokuyo_link

10. 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)2014 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

11. 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)2014 Roi Yehoshua

12. <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 robot initialization: name and position Notice that -param /robot_description is with the slash i.e. fully-qualified, as it will be shared

13. (C)2014 Roi Yehoshua <!-- No namespace here as we will share this description. Access with slash at the beginning --> <!-- No namespace here as we will share this description. Access with slash at the beginning -->

14. gazebo_multi.launch (C)2014 Roi Yehoshua

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

16. (C)2014 Roi Yehoshua

17. You can see that both robots publish the base_scan and cmd_vel topics in the correct namespace (C)2014 Roi Yehoshua

18. 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)2014 Roi Yehoshua $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/robot2/cmd_vel

19. (C)2014 Roi Yehoshua

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

21. (C)2014 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);

22. (C)2014 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);

23. (C)2014 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; }

24. (C)2014 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; }

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

26. (C)2014 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"/>

27. Pioneer 3-AT (P3AT) is one of the world's most popular robot for research and teaching university robotics Download Pioneer’s URDF file from herehere Spawn 3 Pioneer robots in Gazebo and move them around with the keyboard (C)2014 Roi Yehoshua