A header file to wait for UR robots to complete a UR script program sent via ROS.

wait_for_program.h

#pragma once

// This header file can be copied into the folder ur_modern_driver/include/ur_modern_driver 
// of the ur_modern_driver ROS package and included in your program for convenience.
// It waits for a program to be completed on the UR controller.
// Note that this is useful mostly for custom scripts. A move command sent through
// ur_modern_driver/MoveIt will stay active on the UR controller, and the wait will time out.

#include "ros/ros.h"
#include "ur_msgs/RobotModeDataMsg.h"
// Do not forget to add ur_modern_driver to your CMakeLists.txt

// Note: topic_namespace needs a leading slash.
bool isProgramRunning(std::string topic_namespace = "")
{
  ur_msgs::RobotModeDataMsg robot_mode_state;
  auto msg = ros::topic::waitForMessage<ur_msgs::RobotModeDataMsg>(topic_namespace + "/ur_driver/robot_mode_state",
                                                                   ros::Duration(2));
  if (msg)
    return msg->is_program_running;
  else
  {
    ROS_ERROR("No message received from the robot. Is everything running? Is the namespace entered correctly with a "
              "leading slash?");
    ros::Exception e("No message received from the robot.");
    throw(e);
  }
}

// Note: topic_namespace needs a leading slash.
bool waitForURProgram(std::string topic_namespace = "", ros::Duration timeout = ros::Duration(60.0))
{
  ROS_DEBUG("Waiting for UR program to finish. Only run this after sending custom URScripts and not the regular motion "
            "commands, or this call will not terminate before the timeout.");
  ros::Time t_start = ros::Time::now();
  ros::Duration time_passed = ros::Time::now() - t_start;
  while (isProgramRunning(topic_namespace) && (time_passed < timeout))
  {
    ros::Duration(.05).sleep();
    time_passed = ros::Time::now() - t_start;
  }
  ROS_DEBUG("UR Program has terminated.");
  return true;
}