使用越疆科技的M1-B1机器人进行ROS下移动加机械臂加视觉抓取代码

使用越疆科技的M1-B1机器人进行ROS下移动加机械臂加视觉抓取代码 

#include "ros/ros.h"
#include "ar_track_alvar_msgs/AlvarMarkers.h"
#include "iostream"
#include "stdio.h"
#include "cv.h"
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"
#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"
#include "dobot/GetAlarmsState.h"
#include "dobot/ClearAllAlarmsState.h"


using namespace std;
using namespace cv;



////////////////////////相机区域
//机械臂拍照点位
float picture_x = 237.633
    , picture_y = 60.6892
    , picture_z = 230.7879
;

//标定像素坐标 3个点
float cam_x1 = 159.387
    , cam_y1 = -21.6633

    , cam_x2 = 34.927
    , cam_y2 = 40.634

    , cam_x3 = 165.001
    , cam_y3 = 81.35
;

//标定机械包坐标 3个点
float rob_x1 = 327.7778
    , rob_y1 = -78.2467

    , rob_x2 = 357.0341
    , rob_y2 = 43.5529
    
    , rob_x3 = 250.8334
    , rob_y3 = -21.1507
;

//标定像素坐标 3个点
Mat A = (Mat_(3,3) <<
	cam_x1, cam_y1, 1,
	cam_x2, cam_y2, 1,
	cam_x3, cam_y3, 1
	); //3×3
	
//标定机械包坐标 3个点
Mat B = (Mat_(3,3) <<
	rob_x1, rob_y1, 1,
	rob_x2, rob_y2, 1,
	rob_x3, rob_y3, 1
	); //3×3

//矩阵参数
Mat X;


//相机得到图像的坐标和ID
int   ar_ID;
float cam_getX;
float cam_getY;
	
//机械臂最后执行的点位
float X_pose;
float Y_pose;
float Z_pose = 155.9403;

//服务注册
ros::ServiceClient client;
ros::ServiceClient clientPTP;
ros::ServiceClient clientIO;

//函数声明
void robot_pose(Mat X);
void setIO(uint8_t address, uint8_t level, bool isQueued, ros::ServiceClient client);
void setPTP(uint8_t ptpMode ,float x, float y, float z, float r, ros::ServiceClient client);
int getState(ros::ServiceClient client);
int ClearState(ros::ServiceClient client);
void sub_ar_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg);

//回调函数获取二维码的位置
void sub_ar_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
ros::ServiceClient client;
 	cout<<"回调函数获取二维码的位置"	<markers.size() > 0) && (msg->markers[0].id < 30) )
	if(msg->markers.size() > 0 && (msg->markers[0].id < 30) )
	{
	ar_ID = msg->markers[0].id;
	cam_getX = msg->markers[0].pose.pose.position.x*1000;
	cam_getY = msg->markers[0].pose.pose.position.y*1000;

	cout<<"获取二维码的位置"<(1,3) << cam_getX, cam_getY, 1);
	//通过矩阵变换得到机器人抓取的物理坐标
	Mat rot_pose = cam_pose*X; 
	
	//机械臂抓取点位输出
	X_pose = rot_pose.at(0);
	Y_pose = rot_pose.at(1);
	cout<<"抓取点位："<= 0 && ros::ok())
	{
		ros::spinOnce();
		i = i-1;
		cout<<"call :"< ac("move_base",true);

	//设置我们要机器人走的几个点。
	geometry_msgs::Point point;
	geometry_msgs::Quaternion quaternion;
	geometry_msgs::Pose pose_list1;
	geometry_msgs::Pose pose_list2;
	
	point.x = -0.174;
	point.y = -0.921;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = -0.728;
	quaternion.w = 0.684;
	pose_list1.position = point;
	pose_list1.orientation = quaternion;
	
	point.x = 1.842;
	point.y = 0.893;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = -0.0331;
	quaternion.w = 0.999;
	pose_list2.position = point;
	pose_list2.orientation = quaternion;


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


	//运行到拍照点位PTP
	client = n.serviceClient("/DobotServer/SetPTPCmd");
	setPTP(0 ,picture_x ,picture_y ,picture_z ,0 , client);
	//运行到抓取点位PTP
	clientPTP = n.serviceClient("/DobotServer/SetPTPCmd");
	//开启气泵setIO	
	clientIO = n.serviceClient("/DobotServer/SetIODO");


	cout<<"机械臂准备完成请按下空格键开始检测"<