使用越疆科技的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 <unistd.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>
#include <actionlib_msgs/GoalID.h>
#include <pthread.h>

#include "std_msgs/String.h"
#include "std_msgs/Int16.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"
#include "dobot/SetArmOrientation.h"
#include "dobot/GetIODI.h"




using namespace std;
using namespace cv;



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

//标定像素坐标 3个点
float cam_x1 = 168.993
    , cam_y1 = -3.14271

    , cam_x2 = 75.9422
    , cam_y2 = 3.23216

    , cam_x3 = 132.492
    , cam_y3 = -50.4209
;

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

    , rob_x2 = 328.4742
    , rob_y2 = 32.0810
    
    , rob_x3 = 329.2900
    , rob_y3 = -37.6387
;

//标定像素坐标 3个点
Mat A = (Mat_<float>(3,3) <<
	cam_x1, cam_y1, 1,
	cam_x2, cam_y2, 1,
	cam_x3, cam_y3, 1
	); //3×3
	
//标定机械包坐标 3个点
Mat B = (Mat_<float>(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 = 101.4510;

//服务注册
ros::ServiceClient client;
ros::ServiceClient clientPTP;
ros::ServiceClient clientIO;
ros::ServiceClient clientClearState;
ros::ServiceClient clientGetIO;
ros::Publisher cancel_pub;

//函数声明
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_recharge_status_callback(const std_msgs::Int16::ConstPtr& status_msg);
void getIO(uint8_t address, ros::ServiceClient client);
void navigation(actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> &ac, geometry_msgs::Point &point, geometry_msgs::Quaternion &quaternion);
void *pthread_pub(void *agr);

//红外校准数据变量
int recharge_status = 0;

//光电IO数据变量 亮的时候为0（有水在） 不亮时为1（没有水了）
int IO_water = 0;

//导航结束后成功和失败的flag
int navigation_flag = 0;

//回调函数获取二维码的位置
void sub_ar_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
ros::ServiceClient client;
 	cout<<"回调函数获取二维码的位置"	<<endl;
	//if((msg->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<<"获取二维码的位置"<<endl<<"二维码ID:"<<ar_ID<<endl;
	cout<<"Point position:"<<endl;
	cout<<"cam_getX: "<<cam_getX<<endl;
	cout<<"cam_getY: "<<cam_getY<<endl;
	cout<<"-----------"<<endl;
	} 

	//获取机器人抓取位置
	robot_pose(X);	
}

//回调函数获取红外校准数据
void sub_recharge_status_callback(const std_msgs::Int16::ConstPtr& status_msg)
{
	cout<<"回调函数处理数据:"<<status_msg->data<<endl;
	recharge_status = status_msg->data;
}

//矩阵变换函数(只是开始调用一次，求出矩阵参数X)
Mat solve_equation(Mat A, Mat B)
{
	//矩阵转换公式
	solve(A, B, X, CV_SVD);
	cout<<"矩阵转换完成,参数X："<<endl<<X<<endl;
	return X;
}

//机器人抓取位置函数
void robot_pose(Mat X)
{
	cout<<"cam_getX"<<cam_getX<<endl;
	cout<<"cam_getY"<<cam_getY<<endl;
	//相机得到图像的坐标来计算机械臂点位
	Mat cam_pose = (Mat_<float>(1,3) << cam_getX, cam_getY, 1);
	//通过矩阵变换得到机器人抓取的物理坐标
	Mat rot_pose = cam_pose*X; 
	
	//机械臂抓取点位输出
	X_pose = rot_pose.at<float>(0);
	Y_pose = rot_pose.at<float>(1);
	cout<<"抓取点位："<<endl;
	cout<<"X_pose:"<<X_pose<<endl;
	cout<<"Y_pose:"<<Y_pose<<endl;	

	//清除报警状态
	ClearState(clientClearState);

	//运行到二维码点位PTP
	setPTP(2 ,X_pose ,Y_pose ,Z_pose ,200.3824 , clientPTP);

	//运行到水位下位PTP
	setPTP(2 ,X_pose + 90 ,Y_pose ,Z_pose ,200.3824 , clientPTP);

	//抬起水来
	setPTP(2 ,X_pose + 90 ,Y_pose ,Z_pose + 105 ,200.3824 , clientPTP);

	//完成送水
	setPTP(2 ,picture_x ,picture_y ,picture_z ,200.3824 , clientPTP);


	//睡眠8秒等待抓取完成
	sleep(12);
		
}
相机区域
/机械臂区域


//清除系统所有报警
int ClearState(ros::ServiceClient clientClearState) 
{
	dobot::ClearAllAlarmsState srv;
	clientClearState.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);
}

//读取 I/O 输入电平
void getIO(int address, ros::ServiceClient client)
{
	dobot::GetIODI DI;
	DI.request.address = address;
	client.call(DI);
	IO_water = DI.response.level;
}
/机械臂区域

//调用拍照和机械臂抓取部分  回调两次
void cam_pick()
{
	ros::Rate loop_rate(0.5);
	int i = 1;
	while(i >= 0 && ros::ok())
	{
		ros::spinOnce();
		i = i-1;
		//cout<<"call :"<<i<<endl;
		loop_rate.sleep();
	}	
}

//导航函数
void navigation(actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> &ac, geometry_msgs::Point &point, geometry_msgs::Quaternion &quaternion)
{
	//设置我们要机器人走的几个点。
	geometry_msgs::Pose pose_list;

	pose_list.position = point;
	pose_list.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_list;

        //让机器人向目标移动
        //将目标姿势发送到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");
	    navigation_flag = 0;
        }
	else
        {
            //We made it!
            if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                ROS_INFO("Goal succeeded!");
		navigation_flag =1;
            }
            else
            {
                ROS_INFO("The base failed for some reason");
		navigation_flag = 0;
            }
        }
}

//机械臂放置物料函数
void robot_place()
{
	//运行到拍照点位PTP
	setPTP(0 ,picture_x ,picture_y ,picture_z ,0 , clientPTP);

	//清除报警状态
	ClearState(clientClearState);
		
}

//发布停止的线程函数
void *pthread_pub(void *agr)
{	
	actionlib_msgs::GoalID first_goal;
	
	while(ros::ok() && IO_water == 0)
	{
		getIO(21,clientGetIO);
		if(IO_water == 1)
		{
			cancel_pub.publish(first_goal);
			cout<<"发布停止"<<endl;
			break;
		}
	}
	pthread_exit(0);
}


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

	//订阅move_base操作服务器
	actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);
	//红外校准发布器
	ros::Publisher recharge_pub = n.advertise<std_msgs::Int16>("/recharge_handle",10);
	std_msgs::Int16 msg1;
	msg1.data = 1;
	std_msgs::Int16 msg0;
	msg0.data = 0;	

	//设置我们要机器人走的几个点。
	geometry_msgs::Point point;
	geometry_msgs::Quaternion quaternion;
	geometry_msgs::Pose pose_list1;
	geometry_msgs::Pose pose_list2;
	geometry_msgs::Pose pose_list3;
	geometry_msgs::Pose pose_list4;
/*/展厅点位	
	//抓取点
	point.x = 4.81385993958;
	point.y = -0.170049920678;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = -0.102646709935;
	quaternion.w = 0.994717876053;
	pose_list1.position = point;
	pose_list1.orientation = quaternion;
	
	//放置点
	point.x = -0.0495810583234;
	point.y = 0.942381739616;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.993664720651;
	quaternion.w = 0.112385154418;
	pose_list2.position = point;
	pose_list2.orientation = quaternion;
*/
//办公区点位
	//抓取点
	point.x = 2.02914786339;
	point.y = -0.0180041491985;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.00862752292321;
	quaternion.w = 0.999962782232;
	pose_list1.position = point;
	pose_list1.orientation = quaternion;
	
	//放置点
	point.x = -1.06888437271;
	point.y = -0.344112813473;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.999921828282;
	quaternion.w = -0.0125034925458;
	pose_list2.position = point;
	pose_list2.orientation = quaternion;

	//放置点
	point.x = -2.06888437271;
	point.y = -0.344112813473;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.999921828282;
	quaternion.w = -0.0125034925458;
	pose_list3.position = point;
	pose_list3.orientation = quaternion;

	//放置点
	point.x = -3.06888437271;
	point.y = -0.344112813473;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.999921828282;
	quaternion.w = -0.0125034925458;
	pose_list4.position = point;
	pose_list4.orientation = quaternion;



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


	//设置机械臂方向
	client = n.serviceClient<dobot::SetArmOrientation>("/DobotServer/SetArmOrientation");
	dobot::SetArmOrientation srv_arm;
	srv_arm.request.tagArmOrientation = 0;
	srv_arm.request.isQueued =1;
	client.call(srv_arm);

	//运行到拍照点位PTP
	client = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");
	setPTP(0 ,picture_x ,picture_y ,picture_z ,0 , client);
	//运行到抓取点位PTP的Client设置
	clientPTP = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");
	//开启气泵setIO的Client设置
	clientIO = n.serviceClient<dobot::SetIODO>("/DobotServer/SetIODO");
	//报警状态的client设置
	clientClearState = n.serviceClient<dobot::ClearAllAlarmsState>("/DobotServer/ClearAllAlarmsState");
	//获取IO数据设置
	clientGetIO = n.serviceClient<dobot::GetIODI>("/DobotServer/GetIODI");
	//导航取消发布器
	cancel_pub = n.advertise<actionlib_msgs::GoalID>("move_base/cancel",1);
	//创建线程
	pthread_t tid1;
	cout<<"机械臂准备完成请按下空格键开始检测"<<endl;
	getchar();

	//矩阵变换函数
	solve_equation(A, B);
	cout<<"sub"<<endl;

while(ros::ok())
{
	navigation(ac, pose_list1.position, pose_list1.orientation);
	if(navigation_flag == 1 && ros::ok())
	{
		//取水部分
		for(int i =0;i<3;i++)
		{
			recharge_pub.publish(msg1);  //启动红外校准
		}
		while(recharge_status != 2 && recharge_status != 3 && recharge_status != 4 && ros::ok())
		{
			cout<<"开始订阅对准状态:"<<recharge_status<<endl;
			//订阅红外校准状态，成功后返回值为int 3
	    		ros::Subscriber sub1 = n.subscribe("/recharge_status", 1, sub_recharge_status_callback);
			cam_pick();		
		}
		//校准状态位置零
		recharge_status = 0;
		recharge_pub.publish(msg0);  //关闭红外校准
		ROS_INFO("begin M1 pick");
		//sleep(1);
		//订阅相机获取二维码的位置
		ros::Subscriber sub2 = n.subscribe("/ar_pose_marker", 1, sub_ar_callback);
		//调用拍照和机械臂抓取部分
		cam_pick();
	}

	while(ros::ok())
	{
		//运行线程
		pthread_create(&tid1,NULL,pthread_pub,NULL);
     
		if(IO_water == 1)
		{
			cout<<"车上水位状态IO_water:"<<IO_water<<endl;
			break;
		}
		navigation(ac, pose_list2.position, pose_list2.orientation);

		if(IO_water == 1)
		{
			cout<<"车上水位状态IO_water:"<<IO_water<<endl;
			break;
		}
		navigation(ac, pose_list3.position, pose_list3.orientation);
		
		if(IO_water == 1)
		{
			cout<<"车上水位状态IO_water:"<<IO_water<<endl;
			break;
		}
		navigation(ac, pose_list4.position, pose_list4.orientation);
	}
	//结束线程	
	cout<<"线程结束前"<<endl;
	
	pthread_join(tid1,NULL);
	cout<<"线程结束后"<<endl;
	//水被取走后标志继续置0
	IO_water = 0;
	//放置水位
	robot_place();
}

	return 0;

}

不带线程版：

#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 <unistd.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>

#include "std_msgs/String.h"
#include "std_msgs/Int16.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"
#include "dobot/SetArmOrientation.h"
#include "dobot/GetIODI.h"




using namespace std;
using namespace cv;



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

//标定像素坐标 3个点
float cam_x1 = 168.993
    , cam_y1 = -3.14271

    , cam_x2 = 75.9422
    , cam_y2 = 3.23216

    , cam_x3 = 132.492
    , cam_y3 = -50.4209
;

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

    , rob_x2 = 328.4742
    , rob_y2 = 32.0810
    
    , rob_x3 = 329.2900
    , rob_y3 = -37.6387
;

//标定像素坐标 3个点
Mat A = (Mat_<float>(3,3) <<
	cam_x1, cam_y1, 1,
	cam_x2, cam_y2, 1,
	cam_x3, cam_y3, 1
	); //3×3
	
//标定机械包坐标 3个点
Mat B = (Mat_<float>(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 = 101.4510;

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

//函数声明
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_recharge_status_callback(const std_msgs::Int16::ConstPtr& status_msg);
void getIO(uint8_t address, ros::ServiceClient client);

//红外校准数据变量
int recharge_status = 0;

//光电IO数据变量 亮的时候为0（有水在） 不亮时为1（没有水了）
int IO_water = 0;

//回调函数获取二维码的位置
void sub_ar_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
ros::ServiceClient client;
 	cout<<"回调函数获取二维码的位置"	<<endl;
	//if((msg->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<<"获取二维码的位置"<<endl<<"二维码ID:"<<ar_ID<<endl;
	cout<<"Point position:"<<endl;
	cout<<"cam_getX: "<<cam_getX<<endl;
	cout<<"cam_getY: "<<cam_getY<<endl;
	cout<<"-----------"<<endl;
	} 

	//获取机器人抓取位置
	robot_pose(X);	
}

//回调函数获取红外校准数据
void sub_recharge_status_callback(const std_msgs::Int16::ConstPtr& status_msg)
{
	cout<<"回调函数处理数据:"<<status_msg->data<<endl;
	recharge_status = status_msg->data;
}

//矩阵变换函数(只是开始调用一次，求出矩阵参数X)
Mat solve_equation(Mat A, Mat B)
{
	//矩阵转换公式
	solve(A, B, X, CV_SVD);
	cout<<"矩阵转换完成,参数X："<<endl<<X<<endl;
	return X;
}

//机器人抓取位置函数
void robot_pose(Mat X)
{
	cout<<"cam_getX"<<cam_getX<<endl;
	cout<<"cam_getY"<<cam_getY<<endl;
	//相机得到图像的坐标来计算机械臂点位
	Mat cam_pose = (Mat_<float>(1,3) << cam_getX, cam_getY, 1);
	//通过矩阵变换得到机器人抓取的物理坐标
	Mat rot_pose = cam_pose*X; 
	
	//机械臂抓取点位输出
	X_pose = rot_pose.at<float>(0);
	Y_pose = rot_pose.at<float>(1);
	cout<<"抓取点位："<<endl;
	cout<<"X_pose:"<<X_pose<<endl;
	cout<<"Y_pose:"<<Y_pose<<endl;	

	//清除报警状态
	ClearState(clientClearState);

	//运行到二维码点位PTP
	setPTP(2 ,X_pose ,Y_pose ,Z_pose ,200.3824 , clientPTP);

	//运行到水位下位PTP
	setPTP(2 ,X_pose + 90 ,Y_pose ,Z_pose ,200.3824 , clientPTP);

	//抬起水来
	setPTP(2 ,X_pose + 90 ,Y_pose ,Z_pose + 105 ,200.3824 , clientPTP);

	//完成送水
	setPTP(2 ,picture_x ,picture_y ,picture_z ,200.3824 , clientPTP);


	//睡眠8秒等待抓取完成
	sleep(12);
		
}
相机区域
/机械臂区域


//清除系统所有报警
int ClearState(ros::ServiceClient clientClearState) 
{
	dobot::ClearAllAlarmsState srv;
	clientClearState.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);
}

//读取 I/O 输入电平
void getIO(int address, ros::ServiceClient client)
{
	dobot::GetIODI DI;
	DI.request.address = address;
	client.call(DI);
	IO_water = DI.response.level;
}
/机械臂区域

//调用拍照和机械臂抓取部分  回调两次
void cam_pick()
{
	ros::Rate loop_rate(0.5);
	int i = 1;
	while(i >= 0 && ros::ok())
	{
		ros::spinOnce();
		i = i-1;
		//cout<<"call :"<<i<<endl;
		loop_rate.sleep();
	}	
}

//机械臂放置物料函数
void robot_place()
{
	//运行到拍照点位PTP
	setPTP(0 ,picture_x ,picture_y ,picture_z ,0 , clientPTP);

	//清除报警状态
	ClearState(clientClearState);
		
}

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

	//订阅move_base操作服务器
	actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);
	//红外校准发布器
	ros::Publisher recharge_pub = n.advertise<std_msgs::Int16>("/recharge_handle",10);
	std_msgs::Int16 msg1;
	msg1.data = 1;
	std_msgs::Int16 msg0;
	msg0.data = 0;	

	//设置我们要机器人走的几个点。
	geometry_msgs::Point point;
	geometry_msgs::Quaternion quaternion;
	geometry_msgs::Pose pose_list1;
	geometry_msgs::Pose pose_list2;
//展厅点位	
	//抓取点
	point.x = 4.81385993958;
	point.y = -0.170049920678;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = -0.102646709935;
	quaternion.w = 0.994717876053;
	pose_list1.position = point;
	pose_list1.orientation = quaternion;
	
	//放置点
	point.x = -0.0495810583234;
	point.y = 0.942381739616;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.993664720651;
	quaternion.w = 0.112385154418;
	pose_list2.position = point;
	pose_list2.orientation = quaternion;
/*
//办公区点位
	//抓取点
	point.x = 2.02914786339;
	point.y = -0.0180041491985;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.00862752292321;
	quaternion.w = 0.999962782232;
	pose_list1.position = point;
	pose_list1.orientation = quaternion;
	
	//放置点
	point.x = -1.06888437271;
	point.y = -0.344112813473;
	point.z = 0.000;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.999921828282;
	quaternion.w = -0.0125034925458;
	pose_list2.position = point;
	pose_list2.orientation = quaternion;
*/


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


	//设置机械臂方向
	client = n.serviceClient<dobot::SetArmOrientation>("/DobotServer/SetArmOrientation");
	dobot::SetArmOrientation srv_arm;
	srv_arm.request.tagArmOrientation = 0;
	srv_arm.request.isQueued =1;
	client.call(srv_arm);

	//运行到拍照点位PTP
	client = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");
	setPTP(0 ,picture_x ,picture_y ,picture_z ,0 , client);
	//运行到抓取点位PTP的Client设置
	clientPTP = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");
	//开启气泵setIO的Client设置
	clientIO = n.serviceClient<dobot::SetIODO>("/DobotServer/SetIODO");
	//报警状态的client设置
	clientClearState = n.serviceClient<dobot::ClearAllAlarmsState>("/DobotServer/ClearAllAlarmsState");
	//获取IO数据设置
	clientGetIO = n.serviceClient<dobot::GetIODI>("/DobotServer/GetIODI");
	cout<<"机械臂准备完成请按下空格键开始检测"<<endl;
	getchar();

	//矩阵变换函数
	solve_equation(A, B);
	cout<<"sub"<<endl;

while(ros::ok())
{
///导航开始部分
 	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!");
		for(int i =0;i<3;i++)
		{
			recharge_pub.publish(msg1);  //启动红外校准
		}
		while(recharge_status != 2 && recharge_status != 3 && recharge_status != 4 && ros::ok())
		{
			cout<<"开始订阅对准状态:"<<recharge_status<<endl;
			//订阅红外校准状态，成功后返回值为int 3
	    		ros::Subscriber sub1 = n.subscribe("/recharge_status", 1, sub_recharge_status_callback);
			cam_pick();		
		}
		//校准状态位置零
		recharge_status = 0;
		recharge_pub.publish(msg0);  //关闭红外校准
		ROS_INFO("begin M1 pick");
		//sleep(1);
		//订阅相机获取二维码的位置
		ros::Subscriber sub2 = n.subscribe("/ar_pose_marker", 1, sub_ar_callback);
		//调用拍照和机械臂抓取部分
		cam_pick();
            }
            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!");
		cout<<"取水状态IO_water:"<<IO_water<<endl;
		//等待水被取走
		while(IO_water == 0 && ros::ok())
		{
			getIO(21,clientGetIO);
		}
		cout<<"取水状态IO_water:"<<IO_water<<endl;
		//水被取走后标志继续置0
		IO_water = 0;
		//放置水位
		robot_place();
            }
            else
            {
                ROS_INFO("The base failed for some reason");
            }
        }

}
	return 0;

}