ROS下使用dobot越疆科技的M1-B1机器人报警的信息测试

#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/GetAlarmsState.h"
#include "dobot/ClearAllAlarmsState.h"
#include "iostream"

using namespace std;

//清除系统所有报警
int ClearState(ros::ServiceClient client) 
{
	dobot::ClearAllAlarmsState srv;
	client.call(srv);
}

//获取系统报警状态
int getState(ros::ServiceClient client)
{
	dobot::GetAlarmsState srv;
	client.call(srv);
	cout<<"State result: "<<srv.response.result<<endl;
	cout<<"size_alarmsState:"<<srv.response.alarmsState.size()<<endl;
	for(int i = 0; i<srv.response.alarmsState.size(); i++)
	{
		cout<<srv.response.alarmsState[i];
	}
	cout<<"end  "<<endl;
	//cout<<"State alarmsState: "<<srv.response.alarmsState[0]<<endl;
}

//执行 PTP 指令
void setPTP(uint8_t ptpMode ,float x, float y, float z, float r, ros::ServiceClient client)
{
	dobot::SetPTPCmd srv;
	srv.request.ptpMode = ptpMode;
        srv.request.x = x;
        srv.request.y = y;
        srv.request.z = z;
        srv.request.r = r;
        client.call(srv);   
}

//设置 I/O 输出电平
void setIO(uint8_t address, uint8_t level, bool isQueued, ros::ServiceClient client)
{
	dobot::SetIODO IO;
	IO.request.address = address;
	IO.request.level = level;
	IO.request.isQueued = isQueued;
	client.call(IO);
	ROS_INFO("SetIODO%d ok, %d", IO.request.address , IO.response.queuedCmdIndex);
}

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 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);


    while (ros::ok()) {

	client = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");
	setPTP(230 ,2300 ,246 ,180 ,314 , client);

	sleep(5);
	
	//获取系统报警状态
	client = n.serviceClient<dobot::GetAlarmsState>("/DobotServer/GetAlarmState");
	getState(client);
	
	//清除系统所有报警
	client = n.serviceClient<dobot::ClearAllAlarmsState>("/DobotServer/ClearAllAlarmsState");
	ClearState(client);
	
	//PTP
	client = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");
	setPTP(1 ,263 ,-11 ,160 ,-28 , client);

	client = n.serviceClient<dobot::GetAlarmsState>("/DobotServer/GetAlarmState");
	getState(client);

	//setIO
	client = n.serviceClient<dobot::SetIODO>("/DobotServer/SetIODO");	
	setIO(18,0,0,client);
	setIO(17,0,0,client);

	//PTP
	client = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");
	setPTP(1 ,263 ,-11 ,200 ,-28 , client);	

	sleep(5);

        ros::spinOnce();
    }

    return 0;
}