雖然ur_modern_driver包訂閱了話題ur_driver/URScript（消息類型std_msgs/String），允許我們向該話題發(fā)布URScript腳本命令，但是由于ros話題“有去無回”，我們不知道ur_driver節(jié)點有沒有接收到消息以及什么時候接收到的消息；為了保證機器人接收到運動指令，只能通過循環(huán)不斷發(fā)送消息，這是十分不方便的。因此我們在ur_modern_driver包的基礎(chǔ)上，創(chuàng)建一個ros服務(wù)。

• 首先創(chuàng)建service

//在ur_msgs包中添加srv/urscript.srv，內(nèi)容如下：

string script --- bool success

//改寫ur_msgs包的CMakeLists.txt文件，添加如下內(nèi)容：

add_service_files(FILES urscript.srv )

//編譯ur_msgs包

catkin_make -DCATKIN_WHITELIST_PACKAGES="ur_msgs"

//此時，在catkin_ws/devel/include目錄下會生成urscript.h，在要用到這個服務(wù)的文件中添加如下頭文件：

#include "ur_msgs/urscript.h"

• 然后在ur_modern_driver包中添加服務(wù)器

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

#include "ur_msgs/urscript.h"

//在URScriptHandler類中添加成員變量

ros::ServiceServer urscript_srv_;

//在類的實現(xiàn)文件urscript_handler.cpp中的構(gòu)造函數(shù)中初始化

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

//此處，“ur_driver/URScript_srv”為該服務(wù)的名稱，URScriptHandler::urscriptInterface_srv為服務(wù)的回調(diào)函數(shù)。

//定義回調(diào)函數(shù)

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中聲明該成員函數(shù)。

• 最后定義相應(yīng)的客戶端

//定義客戶端并向服務(wù)器請求相應(yīng)

#include "ros/ros.h" #include "std_msgs/String.h" #include "ur_msgs/urscript.h" #include "sensor_msgs/JointState.h"//消息回調(diào)函數(shù) 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; }//定義需要發(fā)送的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"); //定義節(jié)點名稱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_));//成功調(diào)用服務(wù)后停止循環(huán)//sleep(5);return 0; }

總結(jié)

以上是生活随笔為你收集整理的改进ur_modern_driver包，提供ur_driver/URScript_srv服务的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。