（带TCP启动信号V1版-简单信号启动，停止在取水导航点位，不带接收到的数据处理功能）
#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 <stdlib.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <netinet/in.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 = 116.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 robot_place();
void *pthread_pub(void *agr);
void *sub_start_msg(void *agr);
//红外校准数据变量
int recharge_status = 0;
//光电IO数据变量 亮的时候为0（有水在） 不亮时为1（没有水了）
int IO_water = 0;
//导航结束后成功和失败的flag
int navigation_flag = 0;
//机器人启动信号flag('A'表示启动，'B'表示停止，默认停止状态)
char start_flag = 'B';
//回调函数获取二维码的位置
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+10 ,Z_pose ,200.3824 , clientPTP);
//运行到水位下位PTP
setPTP(2 ,X_pose + 70 ,Y_pose+10 ,Z_pose ,200.3824 , clientPTP);
//抬起水来
setPTP(2 ,X_pose + 70 ,Y_pose+10 ,Z_pose + 80 ,200.3824 , clientPTP);
//缩回来
setPTP(2 ,X_pose ,Y_pose+10 ,Z_pose + 80 ,200.3824 , clientPTP);
//完成送水
setPTP(2 ,picture_x ,picture_y ,picture_z ,200.3824 , clientPTP);
//睡眠8秒等待抓取完成
sleep(17);
}
相机区域
/机械臂区域
//清除系统所有报警
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);
}
//监听开始信号线程(此线程中为TCP—socket服务器)
void *sub_start_msg(void *agr)
{
//1.socket（）获得一个sockfd。注意第二个参数必须SOCK_STREAM.
//第一个参数是地址描述符，常用AF_INET（表示IPV4）
//第三个参数是套接口所用的协议，不想调用就用0
int socket_fd = socket(AF_INET,SOCK_STREAM,0);
if(socket_fd == -1)
{
cout<<"socket 创建失败："<<endl;
exit(-1);
}
//2.准备通讯地址（必须是服务器的）
//sin_family：协议簇一般用AF_INET（表示IPV4）
//sin_port: 端口，一般用htons(填端口：数字随便要和服务器的一样)将十进制转为网络进制
//sin_addr.s_addr:一般用inet_addr(" "填IP地址)
struct sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_port = htons(8888);
addr.sin_addr.s_addr = inet_addr("192.168.31.202");
int reuseaddr = 1;//解决地址已被占用问题
setsockopt(socket_fd,SOL_SOCKET,SO_REUSEADDR,&reuseaddr,sizeof(reuseaddr));
//3.bind() 绑定。（开发了端口，允许客服端链接）
//参数一：0的返回值（socket_fd）
//参数二：(struct sockaddr*)&addr 前面结构体，即地址
//参数三: addr结构体的长度
int res = bind(socket_fd,(struct sockaddr*)&addr,sizeof(addr));
if(res == -1)
{
cout<<"bind 创建失败："<<endl;
exit(-1);
}
cout<<"bind ok 等待客户端的连接"<<endl;
while(1)
{
//4.监听客户端 listen()函数
//参数二：进程上限，一般小于30
listen(socket_fd,30);
//5.等待客户端的连接 accept(),返回用于交互的socket描述符
struct sockaddr_in client;
socklen_t len = sizeof(client);
int fd = accept(socket_fd,(struct sockaddr*)&client,&len);//阻塞函数
if(fd == -1)
{
printf("accept 错误\n");
exit(-1);
}
//6.使用第5步返回sockt描述符，进行读写通信。
char *ip = inet_ntoa(client.sin_addr);
cout<<"客户：【"<<ip<<"】链接成功"<<endl;
char read_msg[40];
int size = read(fd, read_msg, sizeof(read_msg));
//第一个参数：accept 返回的文件描述符
//第二个参数：存放读取的内容
//第三个参数：内容的大小
start_flag = read_msg[0];
cout<<"接收到字节数为："<<size<<endl;
cout<<"启动信号："<<start_flag<<endl;
//判断水是否被取走，水被取走则会反馈给客户端
while (start_flag =='A')
{
cout<<"判断水是否被取走，水被取走则会反馈给客户端"<<endl;
if(IO_water == 1 && start_flag == 'B')
{
cout<<"水被取走!!"<<endl;
//向客户端发送数据
char str[] = "BBBBBBBBBBBBBBBBBBBBBBB";
write(fd, str, sizeof(str));
cout<<"向客户端发送数据:"<<str<<endl;
sleep(3);
break;
}
}
cout<<"关闭sockfd"<<endl;
//7.关闭sockfd。
close(fd);
}
close(socket_fd);
}
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;
geometry_msgs::Pose pose_list5;
geometry_msgs::Pose pose_list6;
geometry_msgs::Pose pose_list7;
/*/展厅点位
//抓取点
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 = 5.61041688919;
point.y = -0.613996267319;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = -0.0124017467134;
quaternion.w = 0.999923095382;
pose_list1.position = point;
pose_list1.orientation = quaternion;
//放置点
point.x = 6.08865070343;
point.y = -0.688117861748;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = 0.0;
quaternion.w = 1.0;
pose_list2.position = point;
pose_list2.orientation = quaternion;
//放置点
point.x = 2.3586294651;
point.y = 1.0899682045;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = 0.999369155804;
quaternion.w = 0.0355146508812;
pose_list3.position = point;
pose_list3.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;
//创建监听开始信号线程
pthread_t start_thread;
//运行监听开始信号线程
pthread_create(&start_thread,NULL,sub_start_msg,NULL);
cout<<"机械臂准备完成请按下空格键开始检测"<<endl;
getchar();
//矩阵变换函数
solve_equation(A, B);
cout<<"sub"<<endl;
while(ros::ok())
{
cout<<"等待启动信号！"<<endl;
sleep(3);
if(start_flag == 'A')
{
cout<<"接收到启动信号！"<<endl;
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 != 5 && 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_list3.position, pose_list3.orientation);
if(IO_water == 1)
{
cout<<"车上水位状态IO_water:"<<IO_water<<endl;
break;
}
navigation(ac, pose_list2.position, pose_list2.orientation);
}
//结束线程
cout<<"线程结束前"<<endl;
pthread_join(tid1,NULL);
cout<<"线程结束后"<<endl;
//放置水位
robot_place();
//回到取水点
navigation(ac, pose_list1.position, pose_list1.orientation);
//设置启动变量为B
start_flag = 'B';
sleep(1);
//水被取走后标志继续置0
IO_water = 0;
}
}
return 0;
}（带TCP启动信号V1.1版-简单信号启动，停止在回冲点位，不带接收到的数据处理功能）
#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 <stdlib.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <netinet/in.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 = 116.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 robot_place();
void *pthread_pub(void *agr);
void *sub_start_msg(void *agr);
//红外校准数据变量
int recharge_status = 0;
//光电IO数据变量 亮的时候为0（有水在） 不亮时为1（没有水了）
int IO_water = 0;
//导航结束后成功和失败的flag
int navigation_flag = 0;
//机器人启动信号flag('A'表示启动，'B'表示停止，默认停止状态)
char start_flag = 'B';
//调用回充次数flag
int recharge_flag = 1;
//回调函数获取二维码的位置
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+10 ,Z_pose ,200.3824 , clientPTP);
//运行到水位下位PTP
setPTP(2 ,X_pose + 70 ,Y_pose+10 ,Z_pose ,200.3824 , clientPTP);
//抬起水来
setPTP(2 ,X_pose + 70 ,Y_pose+10 ,Z_pose + 80 ,200.3824 , clientPTP);
//缩回来
setPTP(2 ,X_pose ,Y_pose+10 ,Z_pose + 80 ,200.3824 , clientPTP);
//完成送水
setPTP(2 ,picture_x ,picture_y ,picture_z ,200.3824 , clientPTP);
//睡眠8秒等待抓取完成
sleep(17);
}
相机区域
/机械臂区域
//清除系统所有报警
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);
}
//监听开始信号线程(此线程中为TCP—socket服务器)
void *sub_start_msg(void *agr)
{
//1.socket（）获得一个sockfd。注意第二个参数必须SOCK_STREAM.
//第一个参数是地址描述符，常用AF_INET（表示IPV4）
//第三个参数是套接口所用的协议，不想调用就用0
int socket_fd = socket(AF_INET,SOCK_STREAM,0);
if(socket_fd == -1)
{
cout<<"socket 创建失败："<<endl;
exit(-1);
}
//2.准备通讯地址（必须是服务器的）
//sin_family：协议簇一般用AF_INET（表示IPV4）
//sin_port: 端口，一般用htons(填端口：数字随便要和服务器的一样)将十进制转为网络进制
//sin_addr.s_addr:一般用inet_addr(" "填IP地址)
struct sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_port = htons(8888);
addr.sin_addr.s_addr = inet_addr("192.168.31.202");
int reuseaddr = 1;//解决地址已被占用问题
setsockopt(socket_fd,SOL_SOCKET,SO_REUSEADDR,&reuseaddr,sizeof(reuseaddr));
//3.bind() 绑定。（开发了端口，允许客服端链接）
//参数一：0的返回值（socket_fd）
//参数二：(struct sockaddr*)&addr 前面结构体，即地址
//参数三: addr结构体的长度
int res = bind(socket_fd,(struct sockaddr*)&addr,sizeof(addr));
if(res == -1)
{
cout<<"bind 创建失败："<<endl;
exit(-1);
}
cout<<"bind ok 等待客户端的连接"<<endl;
while(1)
{
//4.监听客户端 listen()函数
//参数二：进程上限，一般小于30
listen(socket_fd,30);
//5.等待客户端的连接 accept(),返回用于交互的socket描述符
struct sockaddr_in client;
socklen_t len = sizeof(client);
int fd = accept(socket_fd,(struct sockaddr*)&client,&len);//阻塞函数
if(fd == -1)
{
printf("accept 错误\n");
exit(-1);
}
//6.使用第5步返回sockt描述符，进行读写通信。
char *ip = inet_ntoa(client.sin_addr);
cout<<"客户：【"<<ip<<"】链接成功"<<endl;
char read_msg[40];
int size = read(fd, read_msg, sizeof(read_msg));
//第一个参数：accept 返回的文件描述符
//第二个参数：存放读取的内容
//第三个参数：内容的大小
start_flag = read_msg[0];
cout<<"接收到字节数为："<<size<<endl;
cout<<"启动信号："<<start_flag<<endl;
//判断水是否被取走，水被取走则会反馈给客户端
while (start_flag =='A')
{
cout<<"判断水是否被取走，水被取走则会反馈给客户端"<<endl;
if(IO_water == 1 && start_flag == 'B')
{
cout<<"水被取走!!"<<endl;
//向客户端发送数据
char str[] = "BBBBBBBBBBBBBBBBBBBBBBB";
write(fd, str, sizeof(str));
cout<<"向客户端发送数据:"<<str<<endl;
sleep(3);
break;
}
}
cout<<"关闭sockfd"<<endl;
//7.关闭sockfd。
close(fd);
}
close(socket_fd);
}
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;
geometry_msgs::Pose pose_list5;
geometry_msgs::Pose pose_list6;
geometry_msgs::Pose pose_list7;
/*/展厅点位
//抓取点
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 = 5.61041688919;
point.y = -0.613996267319;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = -0.0124017467134;
quaternion.w = 0.999923095382;
pose_list1.position = point;
pose_list1.orientation = quaternion;
//放置点
point.x = 6.08865070343;
point.y = -0.688117861748;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = 0.0;
quaternion.w = 1.0;
pose_list2.position = point;
pose_list2.orientation = quaternion;
//放置点
point.x = 2.3586294651;
point.y = 1.0899682045;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = 0.999369155804;
quaternion.w = 0.0355146508812;
pose_list3.position = point;
pose_list3.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;
//创建监听开始信号线程
pthread_t start_thread;
//运行监听开始信号线程
pthread_create(&start_thread,NULL,sub_start_msg,NULL);
cout<<"机械臂准备完成请按下空格键开始检测"<<endl;
getchar();
//矩阵变换函数
solve_equation(A, B);
cout<<"sub"<<endl;
//到取水点位
navigation(ac, pose_list1.position, pose_list1.orientation);
while(ros::ok())
{
//整个if为navigation_flag判断导航是否完成,recharge_flag判断能否进行回充
if(navigation_flag == 1 && ros::ok() && recharge_flag == 1)
{
//取水部分
for(int i =0;i<3;i++)
{
recharge_pub.publish(msg1); //启动红外校准
}
while(recharge_status != 5 && 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_flag = 0;
//校准状态位置零
recharge_status = 0;
cout<<"回充完成"<<endl;
}
cout<<"等待启动信号！"<<endl;
sleep(3);
if(start_flag == 'A')
{
cout<<"接收到启动信号！"<<endl;
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_list3.position, pose_list3.orientation);
if(IO_water == 1)
{
cout<<"车上水位状态IO_water:"<<IO_water<<endl;
break;
}
navigation(ac, pose_list2.position, pose_list2.orientation);
}
//结束线程
cout<<"线程结束前"<<endl;
pthread_join(tid1,NULL);
cout<<"线程结束后"<<endl;
//放置水位
robot_place();
//回到取水点
navigation(ac, pose_list1.position, pose_list1.orientation);
//设置启动变量为B
start_flag = 'B';
sleep(1);
//水被取走后标志继续置0
IO_water = 0;
//完成取水之后，回充次数flag赋值为1,准备回去进行充电
recharge_flag = 1;
}
}
return 0;
}(带TCP启动信号V1.2版-信号启动，接收到导航数据点并进行送水行为)
#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 <stdlib.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <cstdlib>
#include <vector>
#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 = 116.4510;
//接收到的点位数据
geometry_msgs::Point receive_point1;
geometry_msgs::Quaternion receive_quaternion1;
geometry_msgs::Pose receive_pose1;
//为了车子不停下来，增加接收到的点位数据附近的一个点位
geometry_msgs::Point receive_point2;
geometry_msgs::Quaternion receive_quaternion2;
geometry_msgs::Pose receive_pose2;
//为接受到的数据String类的字符串分割函数准备的字符串，容器
string receive_s;
vector<string> receive_v;
//服务注册
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 robot_place();
void *pthread_pub(void *agr);
void *sub_start_msg(void *agr);
void SplitString(const string& s, vector<string>& v, const string& c);
//红外校准数据变量
int recharge_status = 0;
//光电IO数据变量 亮的时候为0（有水在） 不亮时为1（没有水了）
int IO_water = 0;
//导航结束后成功和失败的flag
int navigation_flag = 0;
//机器人启动信号flag('A'表示启动，'B'表示停止，默认停止状态)
char start_flag = 'B';
//调用回充次数flag
int recharge_flag = 1;
//回调函数获取二维码的位置
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+10 ,Z_pose ,200.3824 , clientPTP);
//运行到水位下位PTP
setPTP(2 ,X_pose + 70 ,Y_pose+10 ,Z_pose ,200.3824 , clientPTP);
//抬起水来
setPTP(2 ,X_pose + 70 ,Y_pose+10 ,Z_pose + 80 ,200.3824 , clientPTP);
//缩回来
setPTP(2 ,X_pose ,Y_pose+10 ,Z_pose + 80 ,200.3824 , clientPTP);
//完成送水
setPTP(2 ,picture_x ,picture_y ,picture_z ,200.3824 , clientPTP);
//睡眠8秒等待抓取完成
sleep(17);
}
相机区域
/机械臂区域
//清除系统所有报警
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);
}
//监听开始信号线程(此线程中为TCP—socket服务器)
void *sub_start_msg(void *agr)
{
//1.socket（）获得一个sockfd。注意第二个参数必须SOCK_STREAM.
//第一个参数是地址描述符，常用AF_INET（表示IPV4）
//第三个参数是套接口所用的协议，不想调用就用0
int socket_fd = socket(AF_INET,SOCK_STREAM,0);
if(socket_fd == -1)
{
cout<<"socket 创建失败："<<endl;
exit(-1);
}
//2.准备通讯地址（必须是服务器的）
//sin_family：协议簇一般用AF_INET（表示IPV4）
//sin_port: 端口，一般用htons(填端口：数字随便要和服务器的一样)将十进制转为网络进制
//sin_addr.s_addr:一般用inet_addr(" "填IP地址)
struct sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_port = htons(8888);
addr.sin_addr.s_addr = inet_addr("192.168.31.93");
int reuseaddr = 1;//解决地址已被占用问题
setsockopt(socket_fd,SOL_SOCKET,SO_REUSEADDR,&reuseaddr,sizeof(reuseaddr));
//3.bind() 绑定。（开发了端口，允许客服端链接）
//参数一：0的返回值（socket_fd）
//参数二：(struct sockaddr*)&addr 前面结构体，即地址
//参数三: addr结构体的长度
int res = bind(socket_fd,(struct sockaddr*)&addr,sizeof(addr));
if(res == -1)
{
cout<<"bind 创建失败："<<endl;
exit(-1);
}
cout<<"bind ok 等待客户端的连接"<<endl;
while(1)
{
//4.监听客户端 listen()函数
//参数二：进程上限，一般小于30
listen(socket_fd,30);
//5.等待客户端的连接 accept(),返回用于交互的socket描述符
struct sockaddr_in client;
socklen_t len = sizeof(client);
int fd = accept(socket_fd,(struct sockaddr*)&client,&len);//阻塞函数
if(fd == -1)
{
printf("accept 错误\n");
exit(-1);
}
//6.使用第5步返回sockt描述符，进行读写通信。
char *ip = inet_ntoa(client.sin_addr);
cout<<"客户：【"<<ip<<"】链接成功"<<endl;
char read_msg[40];
int size = read(fd, read_msg, sizeof(read_msg));
//第一个参数：accept 返回的文件描述符
//第二个参数：存放读取的内容
//第三个参数：内容的大小
receive_s = read_msg;
cout<<"接收到字节数为："<<size<<endl;
cout<<"接收到的数据："<<read_msg<<endl;
//把接收到的string数据转换为float并赋值给导航点位
//第一个参数为要分割的sting，第二个为接收分割好的容器，第三为分割符号
SplitString(receive_s, receive_v, ",");
//start_flag置A准备启动
start_flag ='A';
cout<<"启动信号："<<start_flag<<endl;
//判断水是否被取走，水被取走则会反馈给客户端
while (start_flag =='A')
{
//cout<<"判断水是否被取走，水被取走则会反馈给客户端"<<endl;
if(IO_water == 1 && start_flag == 'B')
{
cout<<"水被取走!!"<<endl;
//向客户端发送数据
char str[] = "I have completed!";
write(fd, str, sizeof(str));
cout<<"向客户端发送数据:"<<str<<endl;
sleep(1);
break;
}
}
//清除vector里面数据
receive_v.clear();
cout<<"关闭sockfd"<<endl;
//7.关闭sockfd。
close(fd);
}
close(socket_fd);
}
//接受到的数据String类的字符串分割为double型数据点位的函数
void SplitString(const string& s, vector<string>& v, const string& c)
{
string::size_type pos1, pos2;
pos2 = s.find(c);
pos1 = 0;
while(string::npos != pos2)
{
v.push_back(s.substr(pos1, pos2-pos1));
pos1 = pos2 + c.size();
pos2 = s.find(c, pos1);
}
if(pos1 != s.length())
{
v.push_back(s.substr(pos1));
}
//分割完成够，赋值给点位
receive_point1.x = atof(v[0].c_str());
receive_point1.y = atof(v[1].c_str());
receive_point1.z = 0.0;
receive_quaternion1.x = 0.000;
receive_quaternion1.y = 0.000;
receive_quaternion1.z = 0.0;
receive_quaternion1.w = 1.0;
cout <<"分割完成数据点位:"<<receive_point1<<endl;
receive_point2.x = receive_point1.x+0.3;
receive_point2.y = receive_point1.y;
receive_point2.z = 0.0;
receive_quaternion2.x = 0.000;
receive_quaternion2.y = 0.000;
receive_quaternion2.z = -0.99931;
receive_quaternion2.w = 0.03687;
cout <<receive_point2<<endl;
receive_pose1.position = receive_point1;
receive_pose1.orientation = receive_quaternion1;
receive_pose2.position = receive_point2;
receive_pose2.orientation = receive_quaternion2;
//清除vector里面的数据
v.clear();
}
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;
geometry_msgs::Pose pose_list5;
geometry_msgs::Pose pose_list6;
geometry_msgs::Pose pose_list7;
//抓取点
point.x = 5.91041688919;
point.y = -0.613996267319;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = -0.0124017467134;
quaternion.w = 0.999923095382;
pose_list1.position = point;
pose_list1.orientation = quaternion;
//放置点
point.x = 6.08865070343;
point.y = -0.688117861748;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = 0.0;
quaternion.w = 1.0;
pose_list2.position = point;
pose_list2.orientation = quaternion;
//放置点
point.x = 2.3586294651;
point.y = 1.0899682045;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = 0.999369155804;
quaternion.w = 0.0355146508812;
pose_list3.position = point;
pose_list3.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;
//创建监听开始信号线程
pthread_t start_thread;
//运行监听开始信号线程
pthread_create(&start_thread,NULL,sub_start_msg,NULL);
cout<<"机械臂准备完成请按下空格键开始检测"<<endl;
getchar();
//矩阵变换函数
solve_equation(A, B);
cout<<"sub"<<endl;
//到取水点位
navigation(ac, pose_list1.position, pose_list1.orientation);
while(ros::ok())
{
//整个if为navigation_flag判断导航是否完成,recharge_flag判断能否进行回充
if(navigation_flag == 1 && ros::ok() && recharge_flag == 1)
{
//取水部分
for(int i =0;i<3;i++)
{
recharge_pub.publish(msg1); //启动红外校准
}
while(recharge_status != 5 && 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_flag = 0;
//校准状态位置零
recharge_status = 0;
cout<<"回充完成"<<endl;
//设置启动变量为B
start_flag = 'B';
//水被取走后标志继续置0
IO_water = 0;
}
cout<<"等待启动信号！"<<endl;
sleep(3);
if(start_flag == 'A')
{
cout<<"接收到启动信号！"<<endl;
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, receive_pose1.position, receive_pose1.orientation);
if(IO_water == 1)
{
cout<<"车上水位状态IO_water:"<<IO_water<<endl;
break;
}
navigation(ac, receive_pose2.position, receive_pose2.orientation);
}
//结束线程
cout<<"线程结束前"<<endl;
pthread_join(tid1,NULL);
cout<<"线程结束后"<<endl;
//放置水位
robot_place();
//回到取水点
navigation(ac, pose_list1.position, pose_list1.orientation);
//完成取水之后，回充次数flag赋值为1,准备回去进行充电
recharge_flag = 1;
}
}
return 0;
}
568
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
