在ROS下控制越疆科技dobot(magician)机械手的IO口输出
这里就使用IO17号口来进行输出12V
需要先设置IO复用,在设置IO输出
#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;
}