Multi-Robot Systems with ROS Lesson 4

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

2. Agenda • Robots synchronization • Sharing robots’ status • Creating ROS custom messages (C)2014 Roi Yehoshua

3. Robots Synchronization • One important aspect of multi-robot systems is the need of coordination and synchronization of behaviorbetween robots in the team • In this lesson we will learn how to use ROS built-in mechanisms to synchronize robots actions • In the first example we will make our robots wait for each other before they start moving (C)2014 Roi Yehoshua

4. Robots Synchronization • First we will create a shared topic called team_status • Each robot when ready – will publish a ready status message to the shared topic • In addition, we will create a monitor node that will listen for the ready messages • When all ready messages arrive, the monitor node will publish a broadcast message that will let the robots know that the team is ready and they can start moving (C)2014 Roi Yehoshua

5. RobotStatus Message • We will start by creating a new ROS message type for the ready messages called RobotStatus • The structure of the message will be: • The header contains a timestamp and coordinate frame information that are commonly used in ROS Header header int32 robot_id boolis_ready (C)2014 Roi Yehoshua

6. Message Header • stamp specifies the publishing time • frame_id specifies the point of reference for data contained in that message (C)2014 Roi Yehoshua

7. Message Field Types Field types can be: • a built-in type, such as "float32 pan" or "string name" • names of Message descriptions defined on their own, such as "geometry_msgs/PoseStamped" • fixed- or variable-length arrays (lists) of the above, such as "float32[] ranges" or "Point32[10] points“ • the special Header type, which maps to std_msgs/Header (C)2014 Roi Yehoshua

8. Built-In Types (C)2014 Roi Yehoshua

9. msg Files • msg files are simple text files that describe the fields of a ROS message • They are used to generate source code for messages in different languages • Stored in the msg directory of your package (C)2014 Roi Yehoshua

10. Create a Message • First create a new package called multi_sync • Copy the world subdirectory from the stage_multi package we created in the last lesson to the new package • Now create a subdirectory msg and the file RobotStatus.msg within it • $ cd ~/catkin_ws/src • $ catkin_create_pkgmulti_syncstd_msgsrospyroscpp • $ cd ~/catkin_ws/src/multi_sync • $ mkdirmsg • $ geditmsg/RobotStatus.msg (C)2014 Roi Yehoshua

11. Create a Message Type • Add the following lines to RobotStatus.msg: • Now we need to make sure that the msg files are turned into source code for C++, Python, and other languages • Header header • int32 robot_id • boolis_ready (C)2014 Roi Yehoshua

12. package.xml • Open package.xml, and add the following two lines to it • Note that at build time, we need "message_generation", while at runtime, we need "message_runtime" • <build_depend>roscpp</build_depend> • <build_depend>rospy</build_depend> • <build_depend>std_msgs</build_depend> • <build_depend>message_generation</build_depend> • <run_depend>roscpp</run_depend> • <run_depend>rospy</run_depend> • <run_depend>std_msgs</run_depend> • <run_depend>message_runtime</run_depend> (C)2014 Roi Yehoshua

13. CMakeLists • Add the message_generation dependency to the find package call so that you can generate messages:  • Also make sure you export the message runtime dependency: • find_package(catkin REQUIRED COMPONENTS • roscpp • rospy • std_msgs • message_generation • ) catkin_package( # INCLUDE_DIRS include # LIBRARIES multi_sync CATKIN_DEPENDS roscpprospystd_msgsmessage_runtime # DEPENDS system_lib ) (C)2014 Roi Yehoshua

14. CMakeLists • Find the following block of code: • Uncomment it by removing the # symbols and then replace the stand in Message*.msg files with your .msg file, such that it looks like this: ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) add_message_files( FILES RobotStatus.msg ) (C)2014 Roi Yehoshua

15. CMakeLists • Now we must ensure the generate_messages() function is called • Uncomment these lines: • So it looks like: # generate_messages( # DEPENDENCIES # std_msgs # ) generate_messages( DEPENDENCIES std_msgs ) (C)2014 Roi Yehoshua

16. Using rosmsg • That's all you need to do to create a msg • Let's make sure that ROS can see it using the rosmsg show command: • $ rosmsg show [message type] (C)2014 Roi Yehoshua

17. Create a Message •  Now we need to make our package: • During the build, source files will be created from the .msg file: • $ cd ~/catkin_ws • $ catkin_make (C)2014 Roi Yehoshua

18. Create a Message • Any .msg file in the msg directory will generate code for use in all supported languages. • The C++ message header file will be generated in ~/catkin_ws/devel/include/multi_sync/ (C)2014 Roi Yehoshua

19. RobotStatus.h #include <std_msgs/Header.h> namespace multi_sync { template <classContainerAllocator> structRobotStatus_ { typedefRobotStatus_<ContainerAllocator> Type; RobotStatus_() : header() , robot_id(0) , is_ready(false) { } typedef ::std_msgs::Header_<ContainerAllocator> _header_type; _header_type header; typedef uint32_t _robot_id_type; _robot_id_typerobot_id; typedef uint8_t _is_ready_type; _is_ready_typeis_ready; typedef boost::shared_ptr< ::multi_sync::RobotStatus_<ContainerAllocator> > Ptr; typedef boost::shared_ptr< ::multi_sync::RobotStatus_<ContainerAllocator> const> ConstPtr; boost::shared_ptr<std::map<std::string, std::string> > __connection_header; }; // structRobotStatus_ typedef ::multi_sync::RobotStatus_<std::allocator<void> > RobotStatus; (C)2014 Roi Yehoshua

20. Importing Messages • Copy move_robot.cpp from the last lesson to the package • Now import the RobotStatus header file by adding the following line: • Note that messages are put into a namespace that matches the name of the package #include <multi_sync/RobotStatus.h> (C)2014 Roi Yehoshua

21. Team Status Messages • We will use a shared topic called team_status to receive status messages from other team members • Each robot will have both a publisher and a listener object to this topic • Add the following global (class) variables: ros::Subscriber team_status_sub; ros::Publisher team_status_pub; (C)2014 Roi Yehoshua

22. Team Status Messages • In the main() function initialize the publisher and the listener: • Then call a function that will publish a ready status message (before calling move_forward): • After that call a function that will wait for all the other team members to become ready: team_status_pub = nh.advertise<multi_sync::RobotStatus>("team_status", 10); team_status_sub = nh.subscribe("team_status", 1, &teamStatusCallback); publishReadyStatus(); waitForTeam(); (C)2014 Roi Yehoshua

23. Publishing Ready Status Message • Add the following function that will publish a status message when the robot is ready: • Note that the publisher needs some time to connect to the subscribers, thus you need to wait between creating the publisher and sending the first message voidpublishReadyStatus() { multi_sync::RobotStatusstatus_msg; status_msg.header.stamp = ros::Time::now(); status_msg.robot_id = robot_id; status_msg.is_ready = true; // Wait for the publisher to connect to subscribers sleep(1.0); team_status_pub.publish(status_msg); ROS_INFO("Robot %d published ready status", robot_id); } (C)2014 Roi Yehoshua

24. Waiting for Team • First add a global (class) boolean variable that will indicate that all robots are ready: • Now add the following function: • ros::spinOnce() will call all the team status callbacks waiting to be called at that point in time. boolteamReady = false; voidwaitForTeam(){ ros::Rate loopRate(1); // Wait until all robots are ready... while (!teamReady) { ROS_INFO("Robot %d: waiting for team", robot_id); ros::spinOnce(); loopRate.sleep(); } } (C)2014 Roi Yehoshua

25. Receiving Team Ready Status • Add the following callback function to move_robot.cpp that will receive a team ready message from the monitor node: voidteamStatusCallback(constmulti_sync::RobotStatus::ConstPtr& status_msg) { // Check if message came from monitor if (status_msg->header.frame_id == "monitor") { ROS_INFO("Robot %d: Team is ready. Let's move!", robot_id); teamReady = true; } } (C)2014 Roi Yehoshua

26. Compiling the Node • Uncomment the following lines in CMakeLists.txt: • Then call catkin_make • cmake_minimum_required(VERSION 2.8.3) • project(multi_sync) • … • ## Declare a cpp executable • add_executable(move_robot_syncsrc/move_robot.cpp) • ## Specify libraries to link a library or executable target against • target_link_libraries(move_robot_sync • ${catkin_LIBRARIES} • ) (C)2014 Roi Yehoshua

27. Launch File • Create a launch file named multi_sync.launch in /launch subdirectory and add the following lines: • <launch> • <node name="stage" pkg="stage_ros" type="stageros" args="$(find multi_sync)/world/willow-multi-erratic.world"/> • <node name="move_robot_0" pkg="multi_sync" type="move_robot" args="0" output="screen"/> • <node name="move_robot_1" pkg="multi_sync" type="move_robot" args="1" output="screen"/> • <node name="move_robot_2" pkg="multi_sync" type="move_robot" args="2" output="screen"/> • <node name="move_robot_3" pkg="multi_sync" type="move_robot" args="3" output="screen"/> • </launch> (C)2014 Roi Yehoshua

28. Running the move_robot nodes (C)2014 Roi Yehoshua

29. Ready status messages • Type rostopic echo /team_status to watch the ready messages: (C)2014 Roi Yehoshua

30. Creating the monitor node • Now we will create the monitor node that will listen for the ready messages and announce when all robots are ready • The monitor will receive the team size as an argument • Add the file monitor.cpp to the package (C)2014 Roi Yehoshua

31. monitor.cpp (1) #include "ros/ros.h" #include <multi_sync/RobotStatus.h> #define MAX_ROBOTS_NUM 20 unsigned intteamSize; unsigned introbotsCount = 0; boolrobotsReady[MAX_ROBOTS_NUM]; ros::Subscriber team_status_sub; ros::Publisher team_status_pub; voidteamStatusCallback(constmulti_sync::RobotStatus::ConstPtr& status_msg); (C)2014 Roi Yehoshua

32. monitor.cpp (2) int main(intargc, char **argv) { if (argc < 2) { ROS_ERROR("You must specify team size."); return -1; } char *teamSizeStr = argv[1]; teamSize = atoi(teamSizeStr); // Check that robot id is between 0 and MAX_ROBOTS_NUM if (teamSize > MAX_ROBOTS_NUM || teamSize < 1 ) { ROS_ERROR("The team size must be an integer number between 1 and %d", MAX_ROBOTS_NUM); return -1; } ros::init(argc, argv, "monitor"); ros::NodeHandlenh; team_status_pub = nh.advertise<multi_sync::RobotStatus>("team_status", 10); team_status_sub = nh.subscribe("team_status", 1, &teamStatusCallback); ROS_INFO("Waiting for robots to connect..."); ros::spin(); } (C)2014 Roi Yehoshua

33. monitor.cpp (3) voidteamStatusCallback(constmulti_sync::RobotStatus::ConstPtr& status_msg) { introbot_id = status_msg->robot_id; if (!robotsReady[robot_id]) { ROS_INFO("Robot %d is ready!\n", robot_id); robotsReady[robot_id] = true; robotsCount++; if (robotsCount == teamSize) { ROS_INFO("All robots GO!"); multi_sync::RobotStatusstatus_msg; status_msg.header.stamp = ros::Time::now(); status_msg.header.frame_id = "monitor"; team_status_pub.publish(status_msg); } } } (C)2014 Roi Yehoshua

34. Compiling the Node • Add the following lines to CMakeLists.txt: • Then call catkin_make • ## Declare a cpp executable • add_executable(move_robot_syncsrc/move_robot.cpp) • add_executable(monitor src/monitor.cpp) • ## Add cmake target dependencies of the executable/library • ## as an example, message headers may need to be generated before nodes • # add_dependencies(multi_sync_nodemulti_sync_generate_messages_cpp) • ## Specify libraries to link a library or executable target against • target_link_libraries(move_robot_sync • ${catkin_LIBRARIES} • ) • target_link_libraries(monitor • ${catkin_LIBRARIES} • ) (C)2014 Roi Yehoshua

35. Running the monitor • Open a new terminal and run • rosrunmulti_sync monitor 4 (C)2014 Roi Yehoshua

36. Running the monitor • You should now see the robots start to move after receiving the team ready signal (C)2014 Roi Yehoshua

37. Running the monitor (C)2014 Roi Yehoshua

38. Homework (not for submission) • Make the robots share their position as they move in the environment (C)2014 Roi Yehoshua