Teaching Assistant: Roi Yehoshua

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

One important aspect of multi-robot systems is the need of coordination and synchronization of behavior between 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 3(C)2014 Roi Yehoshua

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 4(C)2014 Roi Yehoshua

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 5(C)2014 Roi Yehoshua Header header int32 robot_id bool is_ready

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

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

8

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 9(C)2014 Roi Yehoshua

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 10(C)2014 Roi Yehoshua $ cd ~/catkin_ws/src $ catkin_create_pkg multi_sync std_msgs rospy roscpp $ cd ~/catkin_ws/src $ catkin_create_pkg multi_sync std_msgs rospy roscpp $ cd ~/catkin_ws/src/multi_sync $ mkdir msg $ gedit msg/RobotStatus.msg $ cd ~/catkin_ws/src/multi_sync $ mkdir msg $ gedit msg/RobotStatus.msg

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 11(C)2014 Roi Yehoshua Header header int32 robot_id bool is_ready Header header int32 robot_id bool is_ready

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" 12(C)2014 Roi Yehoshua roscpp rospy std_msgs message_generation roscpp rospy std_msgs message_runtime roscpp rospy std_msgs message_generation roscpp rospy std_msgs message_runtime

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: 13(C)2014 Roi Yehoshua find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation ) find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation ) catkin_package( # INCLUDE_DIRS include # LIBRARIES multi_sync CATKIN_DEPENDS roscpp rospy std_msgs message_runtime # DEPENDS system_lib ) catkin_package( # INCLUDE_DIRS include # LIBRARIES multi_sync CATKIN_DEPENDS roscpp rospy std_msgs message_runtime # DEPENDS system_lib )

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: 14(C)2014 Roi Yehoshua ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) add_message_files( FILES RobotStatus.msg ) add_message_files( FILES RobotStatus.msg )

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

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: 16(C)2014 Roi Yehoshua $ rosmsg show [message type]

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

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/ 18(C)2014 Roi Yehoshua

19(C)2014 Roi Yehoshua #include namespace multi_sync { template struct RobotStatus_ { typedef RobotStatus_ Type; RobotStatus_() : header(), robot_id(0), is_ready(false) { } typedef ::std_msgs::Header_ _header_type; _header_type header; typedef uint32_t _robot_id_type; _robot_id_type robot_id; typedef uint8_t _is_ready_type; _is_ready_type is_ready; typedef boost::shared_ptr > Ptr; typedef boost::shared_ptr const> ConstPtr; boost::shared_ptr > __connection_header; }; // struct RobotStatus_ typedef ::multi_sync::RobotStatus_ > RobotStatus; #include namespace multi_sync { template struct RobotStatus_ { typedef RobotStatus_ Type; RobotStatus_() : header(), robot_id(0), is_ready(false) { } typedef ::std_msgs::Header_ _header_type; _header_type header; typedef uint32_t _robot_id_type; _robot_id_type robot_id; typedef uint8_t _is_ready_type; _is_ready_type is_ready; typedef boost::shared_ptr > Ptr; typedef boost::shared_ptr const> ConstPtr; boost::shared_ptr > __connection_header; }; // struct RobotStatus_ typedef ::multi_sync::RobotStatus_ > RobotStatus;

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 20(C)2014 Roi Yehoshua #include

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: 21(C)2014 Roi Yehoshua ros::Subscriber team_status_sub; ros::Publisher team_status_pub; ros::Subscriber team_status_sub; ros::Publisher team_status_pub;

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: 22(C)2014 Roi Yehoshua team_status_pub = nh.advertise ("team_status", 10); team_status_sub = nh.subscribe("team_status", 1, &teamStatusCallback); team_status_pub = nh.advertise ("team_status", 10); team_status_sub = nh.subscribe("team_status", 1, &teamStatusCallback); publishReadyStatus(); waitForTeam();

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 23(C)2014 Roi Yehoshua void publishReadyStatus() { multi_sync::RobotStatus status_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); } void publishReadyStatus() { multi_sync::RobotStatus status_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); }

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. 24(C)2014 Roi Yehoshua void waitForTeam() { 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(); } void waitForTeam() { 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(); } bool teamReady = false;

Add the following callback function to move_robot.cpp that will receive a team ready message from the monitor node: 25(C)2014 Roi Yehoshua void teamStatusCallback(const multi_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; } void teamStatusCallback(const multi_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; }

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

27 Create a launch file named multi_sync.launch in /launch subdirectory and add the following lines: (C)2014 Roi Yehoshua

28(C)2014 Roi Yehoshua

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

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 30(C)2014 Roi Yehoshua

31(C)2014 Roi Yehoshua #include "ros/ros.h" #include #define MAX_ROBOTS_NUM 20 unsigned int teamSize; unsigned int robotsCount = 0; bool robotsReady[MAX_ROBOTS_NUM]; ros::Subscriber team_status_sub; ros::Publisher team_status_pub; void teamStatusCallback(const multi_sync::RobotStatus::ConstPtr& status_msg); #include "ros/ros.h" #include #define MAX_ROBOTS_NUM 20 unsigned int teamSize; unsigned int robotsCount = 0; bool robotsReady[MAX_ROBOTS_NUM]; ros::Subscriber team_status_sub; ros::Publisher team_status_pub; void teamStatusCallback(const multi_sync::RobotStatus::ConstPtr& status_msg);

32(C)2014 Roi Yehoshua int main(int argc, 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::NodeHandle nh; team_status_pub = nh.advertise ("team_status", 10); team_status_sub = nh.subscribe("team_status", 1, &teamStatusCallback); ROS_INFO("Waiting for robots to connect..."); ros::spin(); } int main(int argc, 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::NodeHandle nh; team_status_pub = nh.advertise ("team_status", 10); team_status_sub = nh.subscribe("team_status", 1, &teamStatusCallback); ROS_INFO("Waiting for robots to connect..."); ros::spin(); }

33(C)2014 Roi Yehoshua void teamStatusCallback(const multi_sync::RobotStatus::ConstPtr& status_msg) { int robot_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::RobotStatus status_msg; status_msg.header.stamp = ros::Time::now(); status_msg.header.frame_id = "monitor"; team_status_pub.publish(status_msg); } void teamStatusCallback(const multi_sync::RobotStatus::ConstPtr& status_msg) { int robot_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::RobotStatus status_msg; status_msg.header.stamp = ros::Time::now(); status_msg.header.frame_id = "monitor"; team_status_pub.publish(status_msg); }

34 Add the following lines to CMakeLists.txt: Then call catkin_make (C)2014 Roi Yehoshua ## Declare a cpp executable add_executable(move_robot_sync src/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_node multi_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} ) ## Declare a cpp executable add_executable(move_robot_sync src/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_node multi_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} )

35 Open a new terminal and run (C)2014 Roi Yehoshua rosrun multi_sync monitor 4

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

37(C)2014 Roi Yehoshua

Make the robots share their position as they move in the environment 38(C)2014 Roi Yehoshua