改进ur_modern_driver包，提供ur_driver/URScript_srv服务

虽然ur_modern_driver包订阅了话题ur_driver/URScript（消息类型std_msgs/String），容许咱们向该话题发布URScript脚本命令，可是因为ros话题“有去无回”，咱们不知道ur_driver节点有没有接收到消息以及何时接收到的消息；为了保证机器人接收到运动指令，只能经过循环不断发送消息，这是十分不方便的。所以咱们在ur_modern_driver包的基础上，建立一个ros服务。

• 首先建立service

//在ur_msgs包中添加srv/urscript.srv，内容以下：

string script
---
bool success

//改写ur_msgs包的CMakeLists.txt文件，添加以下内容：

add_service_files(
   FILES   
   urscript.srv
)

//编译ur_msgs包

catkin_make -DCATKIN_WHITELIST_PACKAGES="ur_msgs"

//此时，在catkin_ws/devel/include目录下会生成urscript.h，在要用到这个服务的文件中添加以下头文件：

#include "ur_msgs/urscript.h"

• 而后在ur_modern_driver包中添加服务器

//在ur_modern_driver包中的urscript_handler.h文件中添加头文件：

#include "ur_msgs/urscript.h"

//在URScriptHandler类中添加成员变量

ros::ServiceServer urscript_srv_;

//在类的实现文件urscript_handler.cpp中的构造函数中初始化

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

//此处，“ur_driver/URScript_srv”为该服务的名称，URScriptHandler::urscriptInterface_srv为服务的回调函数。

//定义回调函数

bool URScriptHandler::urscriptInterface_srv(ur_msgs::urscript::Request &req, ur_msgs::urscript::Response &res){
  //robot_.rt_interface_->addCommandToQueue(req.script);
  std::string str = req.script;
  if (str.back() != '\n')
    str.append("\n");

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

//记得要在类的头文件urscript_handler.h中声明该成员函数。

• 最后定义相应的客户端

//定义客户端并向服务器请求相应

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

//消息回调函数
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;
}

//定义须要发送的URScript脚本
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";
  /*cmd_str_ = "def myProg():\n";
  cmd_str_ += "\tsleep(3)\n";
  cmd_str_ += "\tmovel(p[-0.17993, -0.60687, 0.27638, 0.00100, -3.16600, -0.04000])\n";
  cmd_str_ += "\tsleep(3)\n";
  cmd_str_ += "\tmovel(pose_trans(p[-0.17993,-0.60687,0.27638,0.00100,-3.16600,-0.04000],p[0.01324,-0.17592,-0.00477,0.00041,-0.91694,-0.02321]))\n";
  cmd_str_ += "end\n";*/
  return cmd_str_;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "URScript_talker"); //定义节点名称
  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);
  //ros::spin();
  //std_msgs::String temp_;
  ur_msgs::urscript srv_;
  std::string cmd_str_ = movej_test();
  srv_.request.script = cmd_str_;
  while(!script_client_.call(srv_));//成功调用服务后中止循环
  //sleep(5);
  return 0;
}