DOBOT magician魔术师在ROS下使用moveit编写代码控制（笛卡尔空间控制气泵抓取功能）

#include <string>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <iostream>
#include <magician_hardware/SetEndEffectorSuctionCup.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "moveit_magician_pick_demo");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    ros::NodeHandle n;

   	ros::ServiceClient client;
	client = n.serviceClient<magician_hardware::SetEndEffectorSuctionCup>("/DobotServer/SetEndEffectorSuctionCup");
	//设置气泵吸取suck1
	magician_hardware::SetEndEffectorSuctionCup suck1;
	suck1.request.enableCtrl = 1;
	suck1.request.suck =1;
	suck1.request.isQueued = true;
	//设置气泵关闭suck2
	magician_hardware::SetEndEffectorSuctionCup suck2;
	suck2.request.enableCtrl = 0;
	suck2.request.suck =0;
	suck2.request.isQueued = true;


    moveit::planning_interface::MoveGroupInterface arm("magician_arm");

    //获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();

    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "magician_origin";
    arm.setPoseReferenceFrame(reference_frame);

    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.0002);
    arm.setGoalOrientationTolerance(0.001);

    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.8);
    arm.setMaxVelocityScalingFactor(0.8);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);

    // 设置机器人终端的目标位置
    geometry_msgs::Pose target_pose;
    target_pose.orientation.x = 0.0;
    target_pose.orientation.y = 0.0;
    target_pose.orientation.z = 0.0;
    target_pose.orientation.w = 0.1;

    target_pose.position.x = 0.13731;
    target_pose.position.y = -0.12658;
    target_pose.position.z = -0.01313;

    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();

    arm.setPoseTarget(target_pose);

    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::planning_interface::MoveItErrorCode success = arm.plan(plan);

    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    //让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan);
    sleep(1);

    // 设置机器人终端的目标位置
    target_pose.orientation.x = 0.0;
    target_pose.orientation.y = 0.0;
    target_pose.orientation.z = 0.0;
    target_pose.orientation.w = 0.1;

    target_pose.position.x = 0.13731;
    target_pose.position.y = -0.12658;
    target_pose.position.z = -0.03313;

    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();

    arm.setPoseTarget(target_pose);

    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);

    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    //让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan);
    sleep(1);

	//开启气泵
	client.call(suck1);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);

    // 设置机器人终端的目标位置
    target_pose.orientation.x = 0.0;
    target_pose.orientation.y = 0.0;
    target_pose.orientation.z = 0.0;
    target_pose.orientation.w = 0.1;

    target_pose.position.x = 0.13731;
    target_pose.position.y = 0.13265;
    target_pose.position.z = -0.03313;

    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();

    arm.setPoseTarget(target_pose);

    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);

    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    //让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan);
    sleep(1);
	//关闭气泵
	client.call(suck2);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);

    ros::shutdown(); 

    return 0;
}