ROS下使用dobot越疆科技的M1-B1机器人进行定点抓取代码

 ROS下使用越疆科技的M1-B1机器人进行定点抓取代码

#include 
#include "ros/console.h"
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#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/SetEndEffectorGripper.h"
#include "dobot/SetHOMECmd.h"
#include "dobot/SetHOMEParams.h"
#include "dobot/SetIODO.h"

using namespace std;


void navigation_goal(const std_msgs::Bool::ConstPtr& flag)
{
	
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "send_goal");
	ros::NodeHandle n;

	//自动回充数据
	ros::Publisher recharge_pub = n.advertise("/recharge_handle",10);
	std_msgs::Int16 msg1;
	msg1.data = 1;
	std_msgs::Int16 msg0;
	msg0.data = 0;	

///机械臂初始化
    ros::ServiceClient client;
//#if 0
    // SetCmdTimeout
    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 end effector parameters
    client = n.serviceClient("/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("/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("/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("/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("/DobotServer/SetPTPCommonParams");
        dobot::SetPTPCommonParams srv;

        srv.request.velocityRatio = 50;
        srv.request.accelerationRatio = 50;
        client.call(srv);
    } while (0);
//#endif
///机械臂初始化

	//订阅move_base操作服务器
	actionlib::SimpleActionClient ac("move_base",true);

	//设置我们要机器人走的几个点。
	geometry_msgs::Point point;
	geometry_msgs::Quaternion quaternion;
	geometry_msgs::Pose pose_list1;
	geometry_msgs::Pose pose_list2;
	geometry_msgs::Pose pose_list3;

	point.x = 1.30395;
	point.y = 3.70786;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.70068;
	quaternion.w = 0.71347;
	pose_list1.position = point;
	pose_list1.orientation = quaternion;
	
	point.x = 3.60977;
	point.y = -0.72723;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = -0.0235;
	quaternion.w = 0.99972;
	pose_list2.position = point;
	pose_list2.orientation = quaternion;

	point.x = 0.50;
	point.y = 0.10;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.709273408676;
	quaternion.w = 0.704933494555;
	pose_list3.position = point;
	pose_list3.orientation = quaternion;


 	 ROS_INFO("Waiting for move_base action server...");
  	//等待60秒以使操作服务器可用
  	if(!ac.waitForServer(ros::Duration(60)))
  	{
   	 	ROS_INFO("Can't connected to move base server");
		exit(-1);
 	 }
 	 ROS_INFO("Connected to move base server");
 	 ROS_INFO("Starting navigation test");

         //初始化航点目标
         move_base_msgs::MoveBaseGoal goal1;

         //使用地图框定义目标姿势
         goal1.target_pose.header.frame_id = "map";

         //将时间戳设置为“now”
         goal1.target_pose.header.stamp = ros::Time::now();

        //将目标姿势设置为第i个航点
        goal1.target_pose.pose = pose_list1;

        //让机器人向目标移动
        //将目标姿势发送到MoveBaseAction服务器
        ac.sendGoal(goal1);

        //等3分钟到达那里
        bool finished_within_time1 = ac.waitForResult(ros::Duration(180));

        //如果我们没有及时赶到那里，就会中止目标
        if(!finished_within_time1)
        {
            ac.cancelGoal();
            ROS_INFO("Timed out achieving goal");
        }
	else
        {
            //We made it!
            if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                ROS_INFO("Goal A succeeded!");
		recharge_pub.publish(msg1);  //启动回充
	    	sleep(15);
		recharge_pub.publish(msg0);  //关闭回充
		ROS_INFO("begin M1 pick");
		////移动到抓取点
    		client = n.serviceClient("/DobotServer/SetPTPCmd");
  		dobot::SetPTPCmd srv;
            	srv.request.ptpMode = 1;
            	srv.request.x = 323;
            	srv.request.y = 74.4;
            	srv.request.z = 188.18;
            	srv.request.r = 222.6505;
            	client.call(srv);
            	if (srv.response.result == 0) 
	    	{
            	    ROS_INFO("PTP 2");
            	} 
		///开启气泵
	   	client = n.serviceClient("/DobotServer/SetIODO");
		dobot::SetIODO IO1;
		do{
			IO1.request.address = 18;
			IO1.request.level = 0;
			IO1.request.isQueued = 0;
			client.call(IO1);
		 	ROS_INFO("SetIODO1 ok, %d", IO1.response.queuedCmdIndex);
		}while(0);
		dobot::SetIODO IO2;
		do{
			IO2.request.address = 17;
			IO2.request.level = 0;
			IO2.request.isQueued = 0;
			client.call(IO2);
		 	ROS_INFO("SetIODO2 ok, %d", IO2.response.queuedCmdIndex);
		}while(0);
	    	sleep(5);
		///
    		client = n.serviceClient("/DobotServer/SetPTPCmd");
		srv.request.ptpMode = 1;
		srv.request.x = 206;
		srv.request.y = 27;
		srv.request.z = 226;
		srv.request.r = 222.6505;
            	client.call(srv);
            	if (srv.response.result == 0) 
	    	{
            	    ROS_INFO("PTP 3");
            	} 
            }
            else
            {
                ROS_INFO("The base failed for some reason");
            }
        }

        //初始化航点目标
        move_base_msgs::MoveBaseGoal goal2;

        //使用地图框定义目标姿势
        goal2.target_pose.header.frame_id = "map";

        //将时间戳设置为“now”
        goal2.target_pose.header.stamp = ros::Time::now();

        //将目标姿势设置为第i个航点
        goal2.target_pose.pose = pose_list2;

        //让机器人向目标移动
        //将目标姿势发送到MoveBaseAction服务器
        ac.sendGoal(goal2);

        //等3分钟到达那里
        bool finished_within_time2 = ac.waitForResult(ros::Duration(180));

        //如果我们没有及时赶到那里，就会中止目标
        if(!finished_within_time2)
        {
            ac.cancelGoal();
            ROS_INFO("Timed out achieving goal");
        }
	else
        {
            //We made it!
            if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                ROS_INFO("Goal B succeeded!");
		////移动到放置点
    		client = n.serviceClient("/DobotServer/SetPTPCmd");
  		dobot::SetPTPCmd srv;
             	srv.request.ptpMode = 1;
            	srv.request.x = 314;
            	srv.request.y = 104.4;
            	srv.request.z = 186.18;
            	srv.request.r = 222.6505;
            	client.call(srv);
            	if (srv.response.result == 0) 
	    	{
            	    ROS_INFO("PTP 2");
            	} 
		sleep(3);
		///关闭气泵
	   	client = n.serviceClient("/DobotServer/SetIODO");
		dobot::SetIODO IO1;
		do{
			IO1.request.address = 18;
			IO1.request.level = 1;
			IO1.request.isQueued = 0;
			client.call(IO1);
		 	ROS_INFO("SetIODO3 ok, %d", IO1.response.queuedCmdIndex);
		}while(0);
		dobot::SetIODO IO2;
		do{
			IO2.request.address = 17;
			IO2.request.level = 1;
			IO2.request.isQueued = 0;
			client.call(IO2);
		 	ROS_INFO("SetIODO4 ok, %d", IO2.response.queuedCmdIndex);
		}while(0);
		///
    		client = n.serviceClient("/DobotServer/SetPTPCmd");
		srv.request.ptpMode = 1;
		srv.request.x = 206;
		srv.request.y = 27;
		srv.request.z = 226;
		srv.request.r = 222.6505;
            	client.call(srv);
            	if (srv.response.result == 0) 
	    	{
            	    ROS_INFO("PTP 3");
            	} 
            }
            else
            {
                ROS_INFO("The base failed for some reason");
            }
        }

        //初始化航点目标
        move_base_msgs::MoveBaseGoal goal3;

        //使用地图框定义目标姿势
        goal3.target_pose.header.frame_id = "map";

        //将时间戳设置为“now”
        goal3.target_pose.header.stamp = ros::Time::now();

        //将目标姿势设置为第i个航点
        goal3.target_pose.pose = pose_list3;

        //让机器人向目标移动
        //将目标姿势发送到MoveBaseAction服务器
        ac.sendGoal(goal3);

        //等3分钟到达那里
        bool finished_within_time3 = ac.waitForResult(ros::Duration(180));

        //如果我们没有及时赶到那里，就会中止目标
        if(!finished_within_time3)
        {
            ac.cancelGoal();
            ROS_INFO("Timed out achieving goal");
        }
	else
        {
            //We made it!
            if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                ROS_INFO("end!");
            }
            else
            {
                ROS_INFO("The base failed for some reason");
            }
        }

	ros::spin();

	return 0;
	
			
}