Improve the ur_modern_driver package to provide the ur_driver/URScript_srv service

Although the ur_modern_driver package subscribes to the topic ur_driver/URScript (message type std_msgs/String), which allows us to issue URScript script commands to the topic, we do not know if and when the ur_driver node receives messages because the ros topic "Goes or Goes Back"; in order to ensure that the robot receives motion instructions, it can only do so through loopsIt is inconvenient to interrupt the sending of messages.So we create a ros service based on the ur_modern_driver package.

  • First create the service

//Add srv/urscript.srv to the ur_msgs package as follows:

string script
bool success

//Rewrite the CMakeLists.txt file of the ur_msgs package, adding the following:


//Compile ur_msgs package

catkin_make -DCATKIN_WHITELIST_PACKAGES="ur_msgs"

//At this point, urscript.h is generated in the catkin_ws/devel/include directory, and the following header file is added to the file where the service is to be used:

#include "ur_msgs/urscript.h"
  • Then add the server to the ur_modern_driver package

//Add a header file to the urscript_handler.h file in the ur_modern_driver package:

#include "ur_msgs/urscript.h"

//Add member variables to the URScriptHandler class

ros::ServiceServer urscript_srv_;

//Initialized in constructor in class implementation file urscript_handler.cpp

urscript_srv_ = nh_.advertiseService("ur_driver/URScript_srv", &URScriptHandler::urscriptInterface_srv,this);

//Here,'ur_driver/URScript_srv'is the name of the service and URScriptHandler::urscriptInterface_srv is the callback function for the service.

//Define callback function

bool URScriptHandler::urscriptInterface_srv(ur_msgs::urscript::Request &req, ur_msgs::urscript::Response &res){
  std::string str = req.script;
  if (str.back() != '\n')

  switch (state_)
    case RobotState::Running:
      if (!commander_.uploadProg(str))
        LOG_ERROR("Program upload failed!");
    case RobotState::EmergencyStopped:
      LOG_ERROR("Robot is emergency stopped");
    case RobotState::ProtectiveStopped:
      LOG_ERROR("Robot is protective stopped");
    case RobotState::Error:
      LOG_ERROR("Robot is not ready, check robot_mode");
      LOG_ERROR("Robot is in undefined state");
  res.success = true;
  return res.success;

//Remember to declare the member function in the header file urscript_handler.h of the class.

  • Finally, define the appropriate client

//Define the client and request the appropriate response from the server

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "ur_msgs/urscript.h"
#include "sensor_msgs/JointState.h"

//Message Callback Function
void joint_state_callback(const sensor_msgs::JointState::ConstPtr& msg){
  std::cout<<msg->position[0]<<" "<<msg->position[1]<<" "<<msg->position[2]<<" "<<
             msg->position[3]<<" "<<msg->position[4]<<" "<<msg->position[5]<<std::endl;

//Define the URScript script that needs to be sent
std::string movej_test(){
  std::string cmd_str_;
  //std::vector<float> target_q_(6);
  float target_q_[6] = {0, 0, 0, 0, 0, 0};
  std::string move_str_ = "\tmovej([" + 
          std::to_string(target_q_[0]) + "," + std::to_string(target_q_[1]) + "," + 
          std::to_string(target_q_[2]) + "," + std::to_string(target_q_[3]) + "," + 
          std::to_string(target_q_[4]) + "," + std::to_string(target_q_[5]) + "]," + 
          std::to_string(1.0) + "," + std::to_string(1.5) + ")\n";
  cmd_str_ = "def myProg():\n";
  cmd_str_ += "\tmovej([1,0,0,0,0,0],1.0,1.5)\n";
  cmd_str_ += "\tsleep(3)\n";
  cmd_str_ += "\tmovej([0,0,2.5,0,0,0],1.0,1.5)\n";
  cmd_str_ += "\tsleep(3)\n";
  cmd_str_ += move_str_;
  cmd_str_ += "end\n";
  return cmd_str_;

int main(int argc, char **argv)
  ros::init(argc, argv, "URScript_talker"); //Define Node Name
  ros::NodeHandle nh_;
  //ros::Publisher script_pub_ = nh_.advertise<std_msgs::String>("ur_driver/URScript", 100);
  ros::ServiceClient script_client_ = nh_.serviceClient<ur_msgs::urscript>("ur_driver/URScript_srv");
  ros::Subscriber joint_state_sub_ = nh_.subscribe("/joint_states", 5, joint_state_callback);
  //std_msgs::String temp_;
  ur_msgs::urscript srv_;
  std::string cmd_str_ = movej_test();
  srv_.request.script = cmd_str_;
  while(!;//Stop loop after successful service call
  return 0;


Posted by konrados on Sun, 01 Dec 2019 11:41:34 -0800