在ROS下控制dobot(magician)机械手的IO口输出ｄｅｍｏ

这里就使用IO17号口来进行输出12V

 #include "ros/ros.h" #include "std_msgs/String.h" #include "dobot/SetCmdTimeout.h" #include "dobot/SetQueuedCmdClear.h" #include "dobot/SetQueuedCmdStartExec.h" #include "dobot/SetQueuedCmdForceStopExec.h" #include "dobot/GetDeviceVersion.h"  #include "dobot/SetEndEffectorParams.h" #include "dobot/SetPTPJointParams.h" #include "dobot/SetPTPCoordinateParams.h" #include "dobot/SetPTPJumpParams.h" #include "dobot/SetPTPCommonParams.h" #include "dobot/SetPTPCmd.h" #include "dobot/SetIODO.h" #include "dobot/GetIODO.h" #include "dobot/SetIOMultiplexing.h" #include "iostream"  using namespace std;   int main(int argc, char **argv) {     ros::init(argc, argv, "DobotClient");     ros::NodeHandle n;      ros::ServiceClient client;      // SetCmdTimeout     client = n.serviceClient<dobot::SetCmdTimeout>("/DobotServer/SetCmdTimeout");     dobot::SetCmdTimeout srv1;     srv1.request.timeout = 3000;     if (client.call(srv1) == false) {         ROS_ERROR("Failed to call SetCmdTimeout. Maybe DobotServer isn't started yet!");         return -1;     }      // Clear the command queue     client = n.serviceClient<dobot::SetQueuedCmdClear>("/DobotServer/SetQueuedCmdClear");     dobot::SetQueuedCmdClear srv2;     client.call(srv2);      // Start running the command queue     client = n.serviceClient<dobot::SetQueuedCmdStartExec>("/DobotServer/SetQueuedCmdStartExec");     dobot::SetQueuedCmdStartExec srv3;     client.call(srv3);      // Get device version information     client = n.serviceClient<dobot::GetDeviceVersion>("/DobotServer/GetDeviceVersion");     dobot::GetDeviceVersion srv4;     client.call(srv4);     if (srv4.response.result == 0) {         ROS_INFO("Device version:%d.%d.%d", srv4.response.majorVersion, srv4.response.minorVersion, srv4.response.revision);     } else {         ROS_ERROR("Failed to get device version information!");     }      // Set end effector parameters     client = n.serviceClient<dobot::SetEndEffectorParams>("/DobotServer/SetEndEffectorParams");     dobot::SetEndEffectorParams srv5;     srv5.request.xBias = 70;     srv5.request.yBias = 0;     srv5.request.zBias = 0;     client.call(srv5);      // Set PTP joint parameters     do {         client = n.serviceClient<dobot::SetPTPJointParams>("/DobotServer/SetPTPJointParams");         dobot::SetPTPJointParams srv;          for (int i = 0; i < 4; i++) {             srv.request.velocity.push_back(100);         }         for (int i = 0; i < 4; i++) {             srv.request.acceleration.push_back(100);         }         client.call(srv);     } while (0);      // Set PTP coordinate parameters     do {         client = n.serviceClient<dobot::SetPTPCoordinateParams>("/DobotServer/SetPTPCoordinateParams");         dobot::SetPTPCoordinateParams srv;          srv.request.xyzVelocity = 100;         srv.request.xyzAcceleration = 100;         srv.request.rVelocity = 100;         srv.request.rAcceleration = 100;         client.call(srv);     } while (0);      // Set PTP jump parameters     do {         client = n.serviceClient<dobot::SetPTPJumpParams>("/DobotServer/SetPTPJumpParams");         dobot::SetPTPJumpParams srv;          srv.request.jumpHeight = 20;         srv.request.zLimit = 200;         client.call(srv);     } while (0);      // Set PTP common parameters     do {         client = n.serviceClient<dobot::SetPTPCommonParams>("/DobotServer/SetPTPCommonParams");         dobot::SetPTPCommonParams srv;          srv.request.velocityRatio = 50;         srv.request.accelerationRatio = 50;         client.call(srv);     } while (0);      client = n.serviceClient<dobot::SetIOMultiplexing>("/DobotServer/SetIOMultiplexing");     dobot::SetIOMultiplexing  IOmul;     IOmul.request.address = 17;     IOmul.request.multiplex = 1;     IOmul.request.isQueued = 1;     client.call(IOmul);       client = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");     dobot::SetPTPCmd srv;      while (ros::ok()) {         client = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");         // The first point         do {             srv.request.ptpMode = 1;             srv.request.x = 200;             srv.request.y = 0;             srv.request.z = 0;             srv.request.r = 0;             client.call(srv);             if (srv.response.result == 0) {                 break;             }                  ros::spinOnce();             if (ros::ok() == false) {                 break;             }         } while (1);          client = n.serviceClient<dobot::SetIODO>("/DobotServer/SetIODO"); 	dobot::SetIODO IO; 	IO.request.address = 17; 	IO.request.level = 1;         IO.request.isQueued = 0; 	client.call(IO);          client = n.serviceClient<dobot::GetIODO>("/DobotServer/GetIODO"); 	dobot::GetIODO GetIO; 	GetIO.request.address = 17; 	client.call(GetIO); 	ROS_INFO("GetIODO.level:%d", GetIO.response.level); 	cout<<"--"<<GetIO.response.level<<"--"<<endl; 	sleep(30);          client = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");         // The first point         do {             srv.request.ptpMode = 1;             srv.request.x = 250;             srv.request.y = 0;             srv.request.z = 0;             srv.request.r = 0;             client.call(srv);             if (srv.response.result == 0) {                 break;             }             ros::spinOnce();             if (ros::ok() == false) {                 break;             }         } while (1);            ros::spinOnce();     }      return 0; } 

文章来源: https://blog.csdn.net/qq_42145185/article/details/90289002