在ROS下控制dobot(magician)机械手的滑轨
在ROS下控制越疆科技dobot(magician)机械手的滑轨
GetQueuedCmdCurrentIndex.srv代码：
---
int32 result
uint64 queuedCmdIndex
SetPTPWithLCmd.srv代码：
uint8 ptpMode
float32 x
float32 y
float32 z
float32 r
float32 l
---
int32 result
uint64 queuedCmdIndexSetDeviceWithL.srv代码：
uint8 isWithL
---
int32 result
uint64 queuedCmdIndexClient代码：
#include "ros/ros.h"
#include "ros/console.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
#include
#include
#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/SetHOMEParams.h"
#include "dobot/SetHOMECmd.h"
#include "dobot/GetQueuedCmdCurrentIndex.h"
#include "dobot/SetQueuedCmdStopExec.h"
#include "dobot/SetDeviceWithL.h"
#include "dobot/SetPTPWithLCmd.h"
#include "dobot/SetDeviceWithL.h"
static volatile int keepRunning = 1;
void sigint(int);
void sigint(int a)
{
keepRunning = 0;
exit(1);
}
int main(int argc, char **argv)
{
signal(SIGINT, sigint);
ros::init(argc, argv, "DobotClient");
ros::NodeHandle n;
ros::ServiceClient client;
ros::Rate loop(0.1);
// Set command timeout
client = n.serviceClient("/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("/DobotServer/SetQueuedCmdClear");
dobot::SetQueuedCmdClear srv2;
client.call(srv2);
// Start running the command queue
client = n.serviceClient("/DobotServer/SetQueuedCmdStartExec");
dobot::SetQueuedCmdStartExec srv3;
client.call(srv3);
// Get device version information
client = n.serviceClient("/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 PTP coordinate parameters
do {
client = n.serviceClient("/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 common parameters
do {
client = n.serviceClient("/DobotServer/SetPTPCommonParams");
dobot::SetPTPCommonParams srv;
srv.request.velocityRatio = 50;
srv.request.accelerationRatio = 50;
client.call(srv);
} while (0);
do{
client = n.serviceClient("/DobotServer/SetDeviceWithL");
dobot::SetDeviceWithL rail;
rail.request.isWithL = 1;
client.call(rail);
} while(0);
do{
client = n.serviceClient("/DobotServer/SetHOMEParams"); /* The client is using the nodehandler
* to access the advertised service SetHOMEParams
* within the DobotServer.
*/
dobot::SetHOMEParams home; // Sets the position of the home.
home.request.x = 200; // Setting the x coordinate through a client request.
home.request.y = 0; // Setting the y coordinate through a client request.
home.request.z = 0; // Setting the z coordinate through a client request.
home.request.r = 0; // Setting the r coordinate through a client request.
home.request.isQueued = 1; // Enabling the queue through a client request.
client.call(home); // Calls the service with all the requests made but won't be performed until a spin is called.
} while(0);
do{
client = n.serviceClient("/DobotServer/SetHOMECmd");
dobot::SetHOMECmd home1; // SetHomeCmd needs to be called after SetHOMEParams otherwise the magician will not move to the user defined positions.
client.call(home1); // Calls the service with all the requests made but won't be performed until a spin is called.
} while(0);
ros::spinOnce(); // All the callbacks will be called from spinOnce()
client = n.serviceClient("/DobotServer/SetPTPWithLCmd");
dobot::SetPTPWithLCmd srv;
//first point
do {
srv.request.ptpMode = 1; // This sets the mode of the movement, the value of 1 gives the movement option of MOVJ.
srv.request.x = 200; // Set the x cartesian coordinant through a client request.
srv.request.y = 0; // Set the y cartesian coordinant through a client request.
srv.request.z = 0; // Set the z cartesian coordinant through a client request.
srv.request.r = 0; // Set the r cartesian coordinant through a client request.
srv.request.l = 0;
client.call(srv); // Calls the service with all the requests made but won't be performed until a spin is called.
if(srv.response.result == 0) // The Client is asking for a response from the service and will check to see if the condition has been met.
{
break;
}
ros::spinOnce(); // All the callbacks will be called from spinOnce()
if (ros::ok() == false) {
break;
}
} while (1);
do {
srv.request.ptpMode = 1; // This sets the mode of the movement, the value of 1 gives the movement option of MOVJ.
srv.request.x = 200; //Set the x cartesian coordinant
srv.request.y = 0; //Set the y cartesian coordinant
srv.request.z = -30; //Set the z cartesian coordinant
srv.request.r = 0; //Set the r cartesian coordinant
srv.request.l = 30;
client.call(srv); //Once all the requests are made the client needs to call the advertised service
if(srv.response.result == 0)
{
break;
}
ros::spinOnce(); // All the callbacks will be called from spinOnce()
loop.sleep();
if (ros::ok() == false) {
break;
}
} while (1);
do {
srv.request.ptpMode = 1;
srv.request.x = 250; //x cartesian coordinant has been changed to 250.
srv.request.y = 0;
srv.request.z = 0; //z cartesian coordinant has been changed to 0.
srv.request.r = 0;
srv.request.l = 60;
client.call(srv);
if(srv.response.result == 0)
{
break;
}
ros::spinOnce(); // All the callbacks will be called from spinOnce()
loop.sleep();
if (ros::ok() == false)
{
break;
}
} while (1);
do {
srv.request.ptpMode = 1;
srv.request.x = 250;
srv.request.y = 0;
srv.request.z = -30; //z cartesian coordinant has been changed to -30.
srv.request.r = 0;
srv.request.l = 60;
client.call(srv);
if(srv.response.result == 0)
{
break;
}
ros::spinOnce(); // All the callbacks will be called from spinOnce()
loop.sleep();
if (ros::ok() == false)
{
break;
}
} while (1);
return 0;
}
server代码：
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32MultiArray.h"
#include "DobotDll.h"
/*
* Cmd timeout
*/
#include "dobot/SetCmdTimeout.h"
bool SetCmdTimeoutService(dobot::SetCmdTimeout::Request &req, dobot::SetCmdTimeout::Response &res)
{
res.result = SetCmdTimeout(req.timeout);
return true;
}
void InitCmdTimeoutServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetCmdTimeout", SetCmdTimeoutService);
serverVec.push_back(server);
}
/*
* Device information
*/
#include "dobot/GetDeviceSN.h"
#include "dobot/SetDeviceName.h"
#include "dobot/GetDeviceName.h"
#include "dobot/GetDeviceVersion.h"
#include "dobot/SetDeviceWithL.h"
bool GetDeviceSNService(dobot::GetDeviceSN::Request &req, dobot::GetDeviceSN::Response &res)
{
char deviceSN[256];
res.result = GetDeviceSN(deviceSN, sizeof(deviceSN));
if (res.result == DobotCommunicate_NoError) {
std::stringstream ss;
ss << deviceSN;
res.deviceSN.data = ss.str();
}
return true;
}
bool SetDeviceNameService(dobot::SetDeviceName::Request &req, dobot::SetDeviceName::Response &res)
{
res.result = SetDeviceName(req.deviceName.data.c_str());
return true;
}
bool GetDeviceNameService(dobot::GetDeviceName::Request &req, dobot::GetDeviceName::Response &res)
{
char deviceName[256];
res.result = GetDeviceName(deviceName, sizeof(deviceName));
if (res.result == DobotCommunicate_NoError) {
std::stringstream ss;
ss << deviceName;
res.deviceName.data = ss.str();
}
return true;
}
bool GetDeviceVersionService(dobot::GetDeviceVersion::Request &req, dobot::GetDeviceVersion::Response &res)
{
uint8_t majorVersion, minorVersion, revision;
res.result = GetDeviceVersion(&majorVersion, &minorVersion, &revision);
if (res.result == DobotCommunicate_NoError) {
res.majorVersion = majorVersion;
res.minorVersion = minorVersion;
res.revision = revision;
}
return true;
}
bool SetDeviceWithLService(dobot::SetDeviceWithL::Request &req, dobot::SetDeviceWithL::Response &res)
{
uint64_t queuedCmdIndex;
res.result = SetDeviceWithL(req.isWithL, true, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
void InitDeviceInfoServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/GetDeviceSN", GetDeviceSNService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetDeviceName", SetDeviceNameService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetDeviceName", GetDeviceNameService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetDeviceVersion", GetDeviceVersionService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetDeviceWithL", SetDeviceWithLService);
serverVec.push_back(server);
}
/*
* Pose
*/
#include "dobot/GetPose.h"
bool GetPoseService(dobot::GetPose::Request &req, dobot::GetPose::Response &res)
{
Pose pose;
res.result = GetPose(&pose);
if (res.result == DobotCommunicate_NoError) {
res.x = pose.x;
res.y = pose.y;
res.z = pose.z;
res.r = pose.r;
for (int i = 0; i < 4; i++) {
res.jointAngle.push_back(pose.jointAngle[i]);
}
}
return true;
}
void InitPoseServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/GetPose", GetPoseService);
serverVec.push_back(server);
}
/*
* Alarms
*/
#include "dobot/GetAlarmsState.h"
#include "dobot/ClearAllAlarmsState.h"
bool GetAlarmsStateService(dobot::GetAlarmsState::Request &req, dobot::GetAlarmsState::Response &res)
{
uint8_t alarmsState[128];
uint32_t len;
res.result = GetAlarmsState(alarmsState, &len, sizeof(alarmsState));
if (res.result == DobotCommunicate_NoError) {
for (int i = 0; i < len; i++) {
res.alarmsState.push_back(alarmsState[i]);
}
}
return true;
}
bool ClearAllAlarmsStateService(dobot::ClearAllAlarmsState::Request &req, dobot::ClearAllAlarmsState::Response &res)
{
res.result = ClearAllAlarmsState();
return true;
}
void InitAlarmsServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/GetAlarmsState", GetAlarmsStateService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/ClearAllAlarmsState", ClearAllAlarmsStateService);
serverVec.push_back(server);
}
/*
* HOME
*/
#include "dobot/SetHOMEParams.h"
#include "dobot/GetHOMEParams.h"
#include "dobot/SetHOMECmd.h"
bool SetHOMEParamsService(dobot::SetHOMEParams::Request &req, dobot::SetHOMEParams::Response &res)
{
HOMEParams params;
uint64_t queuedCmdIndex;
params.x = req.x;
params.y = req.y;
params.z = req.z;
params.r = req.r;
res.result = SetHOMEParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetHOMEParamsService(dobot::GetHOMEParams::Request &req, dobot::GetHOMEParams::Response &res)
{
HOMEParams params;
res.result = GetHOMEParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
res.x = params.x;
res.y = params.y;
res.z = params.z;
res.r = params.r;
}
return true;
}
bool SetHOMECmdService(dobot::SetHOMECmd::Request &req, dobot::SetHOMECmd::Response &res)
{
HOMECmd cmd;
uint64_t queuedCmdIndex;
res.result = SetHOMECmd(&cmd, true, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
void InitHOMEServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetHOMEParams", SetHOMEParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetHOMEParams", GetHOMEParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetHOMECmd", SetHOMECmdService);
serverVec.push_back(server);
}
/*
* End effector
*/
#include "dobot/SetEndEffectorParams.h"
#include "dobot/GetEndEffectorParams.h"
#include "dobot/SetEndEffectorLaser.h"
#include "dobot/GetEndEffectorLaser.h"
#include "dobot/SetEndEffectorSuctionCup.h"
#include "dobot/GetEndEffectorSuctionCup.h"
#include "dobot/SetEndEffectorGripper.h"
#include "dobot/GetEndEffectorGripper.h"
bool SetEndEffectorParamsService(dobot::SetEndEffectorParams::Request &req, dobot::SetEndEffectorParams::Response &res)
{
EndEffectorParams params;
uint64_t queuedCmdIndex;
params.xBias = req.xBias;
params.yBias = req.yBias;
params.zBias = req.zBias;
res.result = SetEndEffectorParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetEndEffectorParamsService(dobot::GetEndEffectorParams::Request &req, dobot::GetEndEffectorParams::Response &res)
{
EndEffectorParams params;
res.result = GetEndEffectorParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
res.xBias = params.xBias;
res.yBias = params.yBias;
res.zBias = params.zBias;
}
return true;
}
bool SetEndEffectorLaserService(dobot::SetEndEffectorLaser::Request &req, dobot::SetEndEffectorLaser::Response &res)
{
uint64_t queuedCmdIndex;
res.result = SetEndEffectorLaser(req.enableCtrl, req.on, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetEndEffectorLaserService(dobot::GetEndEffectorLaser::Request &req, dobot::GetEndEffectorLaser::Response &res)
{
bool enableCtrl, on;
res.result = GetEndEffectorLaser(&enableCtrl, &on);
if (res.result == DobotCommunicate_NoError) {
res.enableCtrl = enableCtrl;
res.on = on;
}
return true;
}
bool SetEndEffectorSuctionCupService(dobot::SetEndEffectorSuctionCup::Request &req, dobot::SetEndEffectorSuctionCup::Response &res)
{
uint64_t queuedCmdIndex;
res.result = SetEndEffectorSuctionCup(req.enableCtrl, req.suck, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetEndEffectorSuctionCupService(dobot::GetEndEffectorSuctionCup::Request &req, dobot::GetEndEffectorSuctionCup::Response &res)
{
bool enableCtrl, suck;
res.result = GetEndEffectorLaser(&enableCtrl, &suck);
if (res.result == DobotCommunicate_NoError) {
res.enableCtrl = enableCtrl;
res.suck = suck;
}
return true;
}
bool SetEndEffectorGripperService(dobot::SetEndEffectorGripper::Request &req, dobot::SetEndEffectorGripper::Response &res)
{
uint64_t queuedCmdIndex;
res.result = SetEndEffectorGripper(req.enableCtrl, req.grip, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetEndEffectorGripperService(dobot::GetEndEffectorGripper::Request &req, dobot::GetEndEffectorGripper::Response &res)
{
bool enableCtrl, grip;
res.result = GetEndEffectorLaser(&enableCtrl, &grip);
if (res.result == DobotCommunicate_NoError) {
res.enableCtrl = enableCtrl;
res.grip = grip;
}
return true;
}
void InitEndEffectorServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetEndEffectorParams", SetEndEffectorParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetEndEffectorParams", GetEndEffectorParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetEndEffectorLaser", SetEndEffectorLaserService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetEndEffectorLaser", GetEndEffectorLaserService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetEndEffectorSuctionCup", SetEndEffectorSuctionCupService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetEndEffectorSuctionCup", GetEndEffectorSuctionCupService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetEndEffectorGripper", SetEndEffectorGripperService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetEndEffectorGripper", GetEndEffectorGripperService);
serverVec.push_back(server);
}
/*
* JOG
*/
#include "dobot/SetJOGJointParams.h"
#include "dobot/GetJOGJointParams.h"
#include "dobot/SetJOGCoordinateParams.h"
#include "dobot/GetJOGCoordinateParams.h"
#include "dobot/SetJOGCommonParams.h"
#include "dobot/GetJOGCommonParams.h"
#include "dobot/SetJOGCmd.h"
bool SetJOGJointParamsService(dobot::SetJOGJointParams::Request &req, dobot::SetJOGJointParams::Response &res)
{
JOGJointParams params;
uint64_t queuedCmdIndex;
for (int i = 0; i < req.velocity.size(); i++) {
params.velocity[i] = req.velocity[i];
}
for (int i = 0; i < req.acceleration.size(); i++) {
params.acceleration[i] = req.acceleration[i];
}
res.result = SetJOGJointParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetJOGJointParamsService(dobot::GetJOGJointParams::Request &req, dobot::GetJOGJointParams::Response &res)
{
JOGJointParams params;
res.result = GetJOGJointParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
for (int i = 0; i < 4; i++) {
res.velocity.push_back(params.velocity[i]);
res.acceleration.push_back(params.acceleration[i]);
}
}
return true;
}
bool SetJOGCoordinateParamsService(dobot::SetJOGCoordinateParams::Request &req, dobot::SetJOGCoordinateParams::Response &res)
{
JOGCoordinateParams params;
uint64_t queuedCmdIndex;
for (int i = 0; i < req.velocity.size(); i++) {
params.velocity[i] = req.velocity[i];
}
for (int i = 0; i < req.acceleration.size(); i++) {
params.acceleration[i] = req.acceleration[i];
}
res.result = SetJOGCoordinateParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetJOGCoordinateParamsService(dobot::GetJOGCoordinateParams::Request &req, dobot::GetJOGCoordinateParams::Response &res)
{
JOGCoordinateParams params;
res.result = GetJOGCoordinateParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
for (int i = 0; i < 4; i++) {
res.velocity.push_back(params.velocity[i]);
res.acceleration.push_back(params.acceleration[i]);
}
}
return true;
}
bool SetJOGCommonParamsService(dobot::SetJOGCommonParams::Request &req, dobot::SetJOGCommonParams::Response &res)
{
JOGCommonParams params;
uint64_t queuedCmdIndex;
params.velocityRatio = req.velocityRatio;
params.accelerationRatio = req.accelerationRatio;
res.result = SetJOGCommonParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetJOGCommonParamsService(dobot::GetJOGCommonParams::Request &req, dobot::GetJOGCommonParams::Response &res)
{
JOGCommonParams params;
res.result = GetJOGCommonParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
res.velocityRatio = params.velocityRatio;
res.accelerationRatio = params.accelerationRatio;
}
return true;
}
bool SetJOGCmdService(dobot::SetJOGCmd::Request &req, dobot::SetJOGCmd::Response &res)
{
JOGCmd cmd;
uint64_t queuedCmdIndex;
cmd.isJoint = req.isJoint;
cmd.cmd = req.cmd;
res.result = SetJOGCmd(&cmd, false, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
void InitJOGServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetJOGJointParams", SetJOGJointParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetJOGJointParams", GetJOGJointParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetJOGCoordinateParams", SetJOGCoordinateParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetJOGCoordinateParams", GetJOGCoordinateParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetJOGCommonParams", SetJOGCommonParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetJOGCommonParams", GetJOGCommonParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetJOGCmd", SetJOGCmdService);
serverVec.push_back(server);
}
/*
* PTP
*/
#include "dobot/SetPTPJointParams.h"
#include "dobot/GetPTPJointParams.h"
#include "dobot/SetPTPCoordinateParams.h"
#include "dobot/GetPTPCoordinateParams.h"
#include "dobot/SetPTPJumpParams.h"
#include "dobot/GetPTPJumpParams.h"
#include "dobot/SetPTPCommonParams.h"
#include "dobot/GetPTPCommonParams.h"
#include "dobot/SetPTPCmd.h"
#include "dobot/SetPTPWithLCmd.h"
bool SetPTPJointParamsService(dobot::SetPTPJointParams::Request &req, dobot::SetPTPJointParams::Response &res)
{
PTPJointParams params;
uint64_t queuedCmdIndex;
for (int i = 0; i < req.velocity.size(); i++) {
params.velocity[i] = req.velocity[i];
}
for (int i = 0; i < req.acceleration.size(); i++) {
params.acceleration[i] = req.acceleration[i];
}
res.result = SetPTPJointParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetPTPJointParamsService(dobot::GetPTPJointParams::Request &req, dobot::GetPTPJointParams::Response &res)
{
PTPJointParams params;
res.result = GetPTPJointParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
for (int i = 0; i < 4; i++) {
res.velocity.push_back(params.velocity[i]);
res.acceleration.push_back(params.acceleration[i]);
}
}
return true;
}
bool SetPTPCoordinateParamsService(dobot::SetPTPCoordinateParams::Request &req, dobot::SetPTPCoordinateParams::Response &res)
{
PTPCoordinateParams params;
uint64_t queuedCmdIndex;
params.xyzVelocity = req.xyzVelocity;
params.rVelocity = req.rVelocity;
params.xyzAcceleration = req.xyzAcceleration;
params.rAcceleration = req.rAcceleration;
res.result = SetPTPCoordinateParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetPTPCoordinateParamsService(dobot::GetPTPCoordinateParams::Request &req, dobot::GetPTPCoordinateParams::Response &res)
{
PTPCoordinateParams params;
res.result = GetPTPCoordinateParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
res.xyzVelocity = params.xyzVelocity;
res.rVelocity = params.rVelocity;
res.xyzAcceleration = params.xyzAcceleration;
res.rAcceleration = params.rAcceleration;
}
return true;
}
bool SetPTPJumpParamsService(dobot::SetPTPJumpParams::Request &req, dobot::SetPTPJumpParams::Response &res)
{
PTPJumpParams params;
uint64_t queuedCmdIndex;
params.jumpHeight = req.jumpHeight;
params.zLimit = req.zLimit;
res.result = SetPTPJumpParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetPTPJumpParamsService(dobot::GetPTPJumpParams::Request &req, dobot::GetPTPJumpParams::Response &res)
{
PTPJumpParams params;
res.result = GetPTPJumpParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
res.jumpHeight = params.jumpHeight;
res.zLimit = params.zLimit;
}
return true;
}
bool SetPTPCommonParamsService(dobot::SetPTPCommonParams::Request &req, dobot::SetPTPCommonParams::Response &res)
{
PTPCommonParams params;
uint64_t queuedCmdIndex;
params.velocityRatio = req.velocityRatio;
params.accelerationRatio = req.accelerationRatio;
res.result = SetPTPCommonParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetPTPCommonParamsService(dobot::GetPTPCommonParams::Request &req, dobot::GetPTPCommonParams::Response &res)
{
PTPCommonParams params;
res.result = GetPTPCommonParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
res.velocityRatio = params.velocityRatio;
res.accelerationRatio = params.accelerationRatio;
}
return true;
}
bool SetPTPCmdService(dobot::SetPTPCmd::Request &req, dobot::SetPTPCmd::Response &res)
{
PTPCmd cmd;
Pose pose;
uint64_t queuedCmdIndex;
cmd.ptpMode = req.ptpMode; // Sets the cmd ptpMode to the request made by a client.
cmd.x = req.x; // Sets the cmd x point to the request made by a client.
cmd.y = req.y; // Sets the cmd y point to the request made by a client.
cmd.z = req.z; // Sets the cmd z point to the request made by a client.
cmd.r = req.r; // Sets the cmd r point to the request made by a client.
res.result = SetPTPCmd(&cmd, true, &queuedCmdIndex); // The result of the request is assigned to the response.
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex; // The
}
return true;
}
bool SetPTPWithLCmdService(dobot::SetPTPWithLCmd::Request &req, dobot::SetPTPWithLCmd::Response &res)
{
PTPWithLCmd cmd;
Pose pose;
uint64_t queuedCmdIndex;
cmd.ptpMode = req.ptpMode; // Sets the cmd ptpMode to the request made by a client.
cmd.x = req.x; // Sets the cmd x point to the request made by a client.
cmd.y = req.y; // Sets the cmd y point to the request made by a client.
cmd.z = req.z; // Sets the cmd z point to the request made by a client.
cmd.r = req.r; // Sets the cmd r point to the request made by a client.
cmd.l = req.l; // Sets the cmd l point to the request made by a client. The l variable is the rail.
res.result = SetPTPWithLCmd(&cmd, true, &queuedCmdIndex); // The result of the request is assigned to the response.
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex; // The
}
// int num;
// while(1){
// if(DobotCommunicate_NoError == GetPose(&pose)){
// if(pose.x == req.x && pose.y == req.y && pose.z == req.z && pose.r == req.r){
// break;
// }
// }else{
// printf("set query error/n");
// }
// num++;
// }
// printf("----------\n");
// printf("num = %d\n",num);
return true;
}
void InitPTPServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetPTPJointParams", SetPTPJointParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetPTPJointParams", GetPTPJointParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetPTPCoordinateParams", SetPTPCoordinateParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetPTPCoordinateParams", GetPTPCoordinateParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetPTPJumpParams", SetPTPJumpParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetPTPJumpParams", GetPTPJumpParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetPTPCommonParams", SetPTPCommonParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetPTPCommonParams", GetPTPCommonParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetPTPCmd", SetPTPCmdService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetPTPWithLCmd", SetPTPWithLCmdService);
serverVec.push_back(server);
}
/*
* CP
*/
#include "dobot/SetCPParams.h"
#include "dobot/GetCPParams.h"
#include "dobot/SetCPCmd.h"
bool SetCPParamsService(dobot::SetCPParams::Request &req, dobot::SetCPParams::Response &res)
{
CPParams params;
uint64_t queuedCmdIndex;
params.planAcc = req.planAcc;
params.juncitionVel = req.junctionVel;
params.acc = req.acc;
params.realTimeTrack = req.realTimeTrack;
res.result = SetCPParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetCPParamsService(dobot::GetCPParams::Request &req, dobot::GetCPParams::Response &res)
{
CPParams params;
res.result = GetCPParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
res.planAcc = params.planAcc;
res.junctionVel = params.juncitionVel;
res.acc = params.acc;
res.realTimeTrack = params.realTimeTrack;
}
return true;
}
bool SetCPCmdService(dobot::SetCPCmd::Request &req, dobot::SetCPCmd::Response &res)
{
CPCmd cmd;
uint64_t queuedCmdIndex;
cmd.cpMode = req.cpMode;
cmd.x = req.x;
cmd.y = req.y;
cmd.z = req.z;
cmd.velocity = req.velocity;
res.result = SetCPCmd(&cmd, true, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
void InitCPServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetCPParams", SetCPParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetCPParams", GetCPParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetCPCmd", SetCPCmdService);
serverVec.push_back(server);
}
/*
* ARC
*/
#include "dobot/SetARCParams.h"
#include "dobot/GetARCParams.h"
#include "dobot/SetARCCmd.h"
bool SetARCParamsService(dobot::SetARCParams::Request &req, dobot::SetARCParams::Response &res)
{
ARCParams params;
uint64_t queuedCmdIndex;
params.xyzVelocity = req.xyzVelocity;
params.rVelocity = req.rVelocity;
params.xyzAcceleration = req.xyzAcceleration;
params.rAcceleration = req.rAcceleration;
res.result = SetARCParams(¶ms, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetARCParamsService(dobot::GetARCParams::Request &req, dobot::GetARCParams::Response &res)
{
ARCParams params;
res.result = GetARCParams(¶ms);
if (res.result == DobotCommunicate_NoError) {
res.xyzVelocity = params.xyzVelocity;
res.rVelocity = params.rVelocity;
res.xyzAcceleration = params.xyzAcceleration;
res.rAcceleration = params.rAcceleration;
}
return true;
}
bool SetARCCmdService(dobot::SetARCCmd::Request &req, dobot::SetARCCmd::Response &res)
{
ARCCmd cmd;
uint64_t queuedCmdIndex;
cmd.cirPoint.x = req.x1;
cmd.cirPoint.y = req.y1;
cmd.cirPoint.z = req.z1;
cmd.cirPoint.r = req.r1;
cmd.toPoint.x = req.x2;
cmd.toPoint.y = req.y2;
cmd.toPoint.z = req.z2;
cmd.toPoint.r = req.r2;
res.result = SetARCCmd(&cmd, true, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
void InitARCServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetARCParams", SetARCParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetARCParams", GetARCParamsService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetARCCmd", SetARCCmdService);
serverVec.push_back(server);
}
/*
* WAIT
*/
#include "dobot/SetWAITCmd.h"
bool SetWAITCmdService(dobot::SetWAITCmd::Request &req, dobot::SetWAITCmd::Response &res)
{
WAITCmd cmd;
uint64_t queuedCmdIndex;
cmd.timeout = req.timeout;
res.result = SetWAITCmd(&cmd, true, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
void InitWAITServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetWAITCmd", SetWAITCmdService);
serverVec.push_back(server);
}
/*
* TRIG
*/
#include "dobot/SetTRIGCmd.h"
bool SetTRIGCmdService(dobot::SetTRIGCmd::Request &req, dobot::SetTRIGCmd::Response &res)
{
TRIGCmd cmd;
uint64_t queuedCmdIndex;
cmd.address = req.address;
cmd.mode = req.mode;
cmd.condition = req.condition;
cmd.threshold = req.threshold;
res.result = SetTRIGCmd(&cmd, true, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
void InitTRIGServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetTRIGCmd", SetTRIGCmdService);
serverVec.push_back(server);
}
/*
* EIO
*/
#include "dobot/SetIOMultiplexing.h"
#include "dobot/GetIOMultiplexing.h"
#include "dobot/SetIODO.h"
#include "dobot/GetIODO.h"
#include "dobot/SetIOPWM.h"
#include "dobot/GetIOPWM.h"
#include "dobot/GetIODI.h"
#include "dobot/GetIOADC.h"
#include "dobot/SetEMotor.h"
#include "dobot/SetInfraredSensor.h"
#include "dobot/GetInfraredSensor.h"
#include "dobot/SetColorSensor.h"
#include "dobot/GetColorSensor.h"
bool SetIOMultiplexingService(dobot::SetIOMultiplexing::Request &req, dobot::SetIOMultiplexing::Response &res)
{
IOMultiplexing ioMultiplexing;
uint64_t queuedCmdIndex;
ioMultiplexing.address = req.address;
ioMultiplexing.multiplex = req.multiplex;
res.result = SetIOMultiplexing(&ioMultiplexing, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetIOMultiplexingService(dobot::GetIOMultiplexing::Request &req, dobot::GetIOMultiplexing::Response &res)
{
IOMultiplexing ioMultiplexing;
ioMultiplexing.address = req.address;
res.result = GetIOMultiplexing(&ioMultiplexing);
if (res.result == DobotCommunicate_NoError) {
res.multiplex = ioMultiplexing.multiplex;
}
return true;
}
bool SetIODOService(dobot::SetIODO::Request &req, dobot::SetIODO::Response &res)
{
IODO ioDO;
uint64_t queuedCmdIndex;
ioDO.address = req.address;
ioDO.level = req.level;
res.result = SetIODO(&ioDO, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetIODOService(dobot::GetIODO::Request &req, dobot::GetIODO::Response &res)
{
IODO ioDO;
ioDO.address = req.address;
res.result = GetIODO(&ioDO);
if (res.result == DobotCommunicate_NoError) {
res.level = ioDO.level;
}
return true;
}
bool SetIOPWMService(dobot::SetIOPWM::Request &req, dobot::SetIOPWM::Response &res)
{
IOPWM ioPWM;
uint64_t queuedCmdIndex;
ioPWM.address = req.address;
ioPWM.frequency = req.frequency;
ioPWM.dutyCycle = req.dutyCycle;
res.result = SetIOPWM(&ioPWM, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool GetIOPWMService(dobot::GetIOPWM::Request &req, dobot::GetIOPWM::Response &res)
{
IOPWM ioPWM;
ioPWM.address = req.address;
res.result = GetIOPWM(&ioPWM);
if (res.result == DobotCommunicate_NoError) {
res.frequency = ioPWM.frequency;
res.dutyCycle = ioPWM.dutyCycle;
}
return true;
}
bool GetIODIService(dobot::GetIODI::Request &req, dobot::GetIODI::Response &res)
{
IODI ioDI;
ioDI.address = req.address;
res.result = GetIODI(&ioDI);
if (res.result == DobotCommunicate_NoError) {
res.level = ioDI.level;
}
return true;
}
bool GetIOADCService(dobot::GetIOADC::Request &req, dobot::GetIOADC::Response &res)
{
IOADC ioADC;
ioADC.address = req.address;
res.result = GetIOADC(&ioADC);
if (res.result == DobotCommunicate_NoError) {
res.value = ioADC.value;
}
return true;
}
bool SetEMotorService(dobot::SetEMotor::Request &req, dobot::SetEMotor::Response &res)
{
EMotor eMotor;
uint64_t queuedCmdIndex;
eMotor.index = req.index;
eMotor.isEnabled = req.isEnabled;
eMotor.speed = req.speed;
res.result = SetEMotor(&eMotor, req.isQueued, &queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
bool SetInfraredSensorService(dobot::SetInfraredSensor::Request &req, dobot::SetInfraredSensor::Response &res)
{
InfraredPort infraredPort = InfraredPort(req.infraredPort);
res.result = SetInfraredSensor(req.enableCtrl, infraredPort);
return true;
}
bool GetInfraredSensorService(dobot::GetInfraredSensor::Request &req, dobot::GetInfraredSensor::Response &res)
{
uint8_t value;
InfraredPort infraredPort = InfraredPort(req.infraredPort);
res.result = GetInfraredSensor(infraredPort, &value);
if (res.result == DobotCommunicate_NoError) {
res.value = value;
}
return true;
}
bool SetColorSensorService(dobot::SetColorSensor::Request &req, dobot::SetColorSensor::Response &res)
{
ColorPort colorPort = ColorPort(req.colorPort);
res.result = SetColorSensor(req.enableCtrl, colorPort);
return true;
}
bool GetColorSensorService(dobot::GetColorSensor::Request &req, dobot::GetColorSensor::Response &res)
{
uint8_t r;
uint8_t g;
uint8_t b;
res.result = GetColorSensor(&r, &g, &b);
if (res.result == DobotCommunicate_NoError) {
res.r = r;
res.g = g;
res.b = b;
}
return true;
}
void InitEIOServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetIOMultiplexing", SetIOMultiplexingService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetIOMultiplexing", GetIOMultiplexingService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetIODO", SetIODOService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetIODO", GetIODOService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetIOPWM", SetIOPWMService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetIOPWM", GetIOPWMService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetIODI", GetIODIService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetIOADC", GetIOADCService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetEMotor", SetEMotorService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetInfraredSensor", SetInfraredSensorService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetInfraredSensor", GetInfraredSensorService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetColorSensor", SetColorSensorService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetColorSensor", GetColorSensorService);
serverVec.push_back(server);
}
/*
* Queued command control
*/
#include "dobot/SetQueuedCmdStartExec.h"
#include "dobot/SetQueuedCmdStopExec.h"
#include "dobot/SetQueuedCmdForceStopExec.h"
#include "dobot/SetQueuedCmdClear.h"
#include "dobot/GetQueuedCmdCurrentIndex.h"
bool SetQueuedCmdStartExecService(dobot::SetQueuedCmdStartExec::Request &req, dobot::SetQueuedCmdStartExec::Response &res)
{
res.result = SetQueuedCmdStartExec();
return true;
}
bool SetQueuedCmdStopExecService(dobot::SetQueuedCmdStopExec::Request &req, dobot::SetQueuedCmdStopExec::Response &res)
{
res.result = SetQueuedCmdStopExec();
return true;
}
bool SetQueuedCmdForceStopExecService(dobot::SetQueuedCmdForceStopExec::Request &req, dobot::SetQueuedCmdForceStopExec::Response &res)
{
res.result = SetQueuedCmdForceStopExec();
return true;
}
bool SetQueuedCmdClearService(dobot::SetQueuedCmdClear::Request &req, dobot::SetQueuedCmdClear::Response &res)
{
res.result = SetQueuedCmdClear();
return true;
}
bool GetQueuedCmdCurrentIndexService(dobot::GetQueuedCmdCurrentIndex::Request &req, dobot::GetQueuedCmdCurrentIndex::Response &res)
{
uint64_t queuedCmdIndex;
res.result = GetQueuedCmdCurrentIndex(&queuedCmdIndex);
if (res.result == DobotCommunicate_NoError) {
res.queuedCmdIndex = queuedCmdIndex;
}
return true;
}
void InitQueuedCmdServices(ros::NodeHandle &n, std::vector &serverVec)
{
ros::ServiceServer server;
server = n.advertiseService("/DobotServer/SetQueuedCmdStartExec", SetQueuedCmdStartExecService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetQueuedCmdStopExec", SetQueuedCmdStopExecService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetQueuedCmdForceStopExec", SetQueuedCmdForceStopExecService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/SetQueuedCmdClear", SetQueuedCmdClearService);
serverVec.push_back(server);
server = n.advertiseService("/DobotServer/GetQueuedCmdCurrentIndex", GetQueuedCmdCurrentIndexService);
serverVec.push_back(server);
}
int main(int argc, char **argv)
{
if (argc < 2) {
ROS_ERROR("[USAGE]Application portName");
return -1;
}
// Connect Dobot before start the service
printf("---------%s",argv[1]);
int result = ConnectDobot(argv[1], 115200, 0, 0);
switch (result) {
case DobotConnect_NoError:
break;
case DobotConnect_NotFound:
ROS_ERROR("Dobot not found!");
return -2;
break;
case DobotConnect_Occupied:
ROS_ERROR("Invalid port name or Dobot is occupied by other application!");
return -3;
break;
default:
break;
}
ros::init(argc, argv, "DobotServer");
ros::NodeHandle n;
std::vector serverVec;
InitCmdTimeoutServices(n, serverVec);
InitDeviceInfoServices(n, serverVec);
InitPoseServices(n, serverVec);
InitAlarmsServices(n, serverVec);
InitHOMEServices(n, serverVec);
InitEndEffectorServices(n, serverVec);
InitJOGServices(n, serverVec);
InitPTPServices(n, serverVec);
InitCPServices(n, serverVec);
InitARCServices(n, serverVec);
InitWAITServices(n, serverVec);
InitTRIGServices(n, serverVec);
InitEIOServices(n, serverVec);
InitQueuedCmdServices(n, serverVec);
ROS_INFO("Dobot service running...");
ros::spin();
ROS_INFO("Dobot service exiting...");
// Disconnect Dobot
DisconnectDobot();
return 0;
}