实验四 综合任务开发 - RLi43/Baxter-Experimental-Guide GitHub Wiki
- 熟悉Baxter机器人的API，掌握调用方法。
- 巩固对ROS的理解，并熟悉有关的代码。
- 通过实现一个综合系统，具备完成自拟综合任务的能力
- 实验前了解实验指导书中的相关内容
- 实验前做好实验计划，做好前期准备，能按实验指导书完成相应内容，并进行适当探索。
- 注意实验安全
1. 人体动作跟随：Baxter机能够跟随人的手臂做出相同的动作
2. 人机五子棋对战：运行程序后无需干预，Baxter能够与人进行五子棋对战，且对环境有一定的适应能力。
人体动作跟随，也就是让Baxter机器人的两只机械臂能够跟随人的两只手臂的运动做出类似的运动。
总体来说，为了完成这一任务，需要分三步进行，首先要能够获得人的手臂在三维空间中的位姿信息，其次要有一套人的手臂到机器人的手臂的转换关系，最后根据人手臂的信息和转换关系，控制机器人达到相应的位姿即可。
本项目中实现动作跟随的方法是使用Kinect识别人体骨架，根据关节位置得到各个关节角度，进而映射到Baxter机械臂的关节角，实现动作跟随。
这是参考工程链接，建议加入一定的创新元素。
获取人手臂在三维空间中的位姿，我们采用的是视觉方法，利用Kinect V2深度摄像头其内部集成好的算法模块识别出人体骨架，并通过OpenNI开发库获取了人体25个关节点的三维坐标信息。人体的25个关节点如下图所示：
事实上为获取人手臂的位姿，只需用到两只手臂上共8个节点的坐标信息即可，分别为人的肩关节、肘关节、腕关节和指尖。
本项目在Windows系统上安装了Kinect SDK（如果用Ubuntu下的SDK当然整个系统会简洁一些，但是没有那么多资料），并通过socket连接将数据发送给Ubuntu系统。
Kinect SDK和socket连接不是本实验的重点，故给出参考代码：
#include "pch.h"
#include <iostream>
#include <string>
#include <sstream>
#include <time.h>
//网络连接
#define _WINSOCK_DEPRECATED_NO_WARNINGS
#include <WinSock2.h>
#include <stdio.h>
#define SERVER_PORT 5099 //Keep it same with the program running on Ubuntu
#define MAX_BUFF_SIZE 4096
// OpenCV 头文件
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
// Kinect for Windows SDK 头文件
#include <Kinect.h>
using namespace std;
using namespace cv;
#pragma comment(lib, "ws2_32.lib")
const string get_name(int n); //此函数判断出关节点的名字
void DrawLine(Mat& Img, const Joint& r1, const Joint& r2, ICoordinateMapper* pMapper);
int main()
{
//--------TCP/IP服务---------------
char sever_IP[20] = "192.168.0.7";// Keep it same with Ubuntu's IP
//加载套接字
WSADATA wsaData;
char buff[MAX_BUFF_SIZE];
memset(buff, 0, sizeof(buff));
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0)
{
cout << "Failed to load Winsock" << endl;
return 0;
}
SOCKADDR_IN addrSrv;
addrSrv.sin_family = AF_INET;
addrSrv.sin_port = htons(SERVER_PORT);
addrSrv.sin_addr.S_un.S_addr = inet_addr(sever_IP);//127.0.0.1
//创建套接字
SOCKET sockClient = socket(AF_INET, SOCK_STREAM, 0);
if (SOCKET_ERROR == sockClient) {
cout << "Socket() error:" << WSAGetLastError() << endl;
return 0;
}
//向服务器发出连接请求
if (connect(sockClient, (struct sockaddr*)&addrSrv, sizeof(addrSrv)) == INVALID_SOCKET) {
cout << "Connect failed:" << WSAGetLastError() << endl;
return 0;
}
//发送数据
/*
char buffSend[MAX_BUFF_SIZE];
cin.getline(buffSend, 100);
send(sockClient, buffSend, strlen(buffSend) + 1, 0);
cout << "send!"<<buffSend << endl;
*/
//--------End of TCP/IP服务---------------*/
/*--- Kinect ----*/
//******************* 1. 初始化******************
// 1a. 获取传感器
IKinectSensor* pSensor = nullptr;
GetDefaultKinectSensor(&pSensor);
// 1b. 打开传感器
pSensor->Open();
//******************* 2. 彩色图像读取到图像矩阵中******************
IColorFrameSource* pFrameSource = nullptr;
pSensor->get_ColorFrameSource(&pFrameSource);
int iWidth = 0, iHeight = 0;
IFrameDescription* pFrameDescription = nullptr;
pFrameSource->get_FrameDescription(&pFrameDescription);
pFrameDescription->get_Width(&iWidth);
pFrameDescription->get_Height(&iHeight);
IColorFrameReader* pColorFrameReader = nullptr;
pFrameSource->OpenReader(&pColorFrameReader);
pFrameDescription->Release();
pFrameDescription = nullptr;
pFrameSource->Release();
pFrameSource = nullptr;
// Prepare OpenCV data
UINT uBufferSize = 0;
Mat mColorImg(iHeight, iWidth, CV_8UC4);
uBufferSize = iHeight * iWidth * 4 * sizeof(BYTE);
// *******************3. 读取关节数据************************
IBodyFrameReader* pBodyFrameReader = nullptr;
IBody** aBodyData = nullptr;
INT32 iBodyCount = 0;
IBodyFrameSource* pBodySource = nullptr;
pSensor->get_BodyFrameSource(&pBodySource);
pBodySource->get_BodyCount(&iBodyCount);
aBodyData = new IBody*[iBodyCount];
for (int i = 0; i < iBodyCount; ++i)
aBodyData[i] = nullptr;
pBodySource->OpenReader(&pBodyFrameReader);
pBodySource->Release();
pBodySource = nullptr;
// *************************4.准备坐标转换*************************
ICoordinateMapper* pCoordinateMapper = nullptr;
pSensor->get_CoordinateMapper(&pCoordinateMapper);
namedWindow("Body Image");
namedWindow("colorImage");
time_t time_old = clock();
while (1)
{
time_t time_now = clock();
if (difftime(time_now, time_old) < SLEEP_TIME)continue;
time_old = time_now;
// 4a. 读取彩色图像并输出到矩阵
IColorFrame* pColorFrame = nullptr;
if (pColorFrameReader->AcquireLatestFrame(&pColorFrame) == S_OK)
{
pColorFrame->CopyConvertedFrameDataToArray(uBufferSize, mColorImg.data, ColorImageFormat_Bgra);
pColorFrame->Release();
}
//Mat mImg = mColorImg.clone();
Mat mImg(iHeight, iWidth, CV_8UC4);;
// 4b. 读取Body数据并输出到数组
IBodyFrame* pBodyFrame = nullptr;
if (pBodyFrameReader->AcquireLatestFrame(&pBodyFrame) == S_OK)
{
// 4b1. 获取身体数据
if (pBodyFrame->GetAndRefreshBodyData(iBodyCount, aBodyData) == S_OK)
{
// 4b2. 遍历每个人
for (int i = 0; i < iBodyCount; ++i)
{
IBody* pBody = aBodyData[i];
// 4b3. 确认追踪状态
BOOLEAN bTracked = false;
if ((pBody->get_IsTracked(&bTracked) == S_OK) && bTracked)
{
// 4b4.获取关节
Joint aJoints[JointType::JointType_Count];
if (pBody->GetJoints(JointType::JointType_Count, aJoints) == S_OK)
{
// 绘制关节图像 - 便于调试
DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_SpineMid], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_SpineMid], aJoints[JointType_SpineShoulder], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_Neck], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_Neck], aJoints[JointType_Head], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_ShoulderLeft], aJoints[JointType_ElbowLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_ElbowLeft], aJoints[JointType_WristLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_WristLeft], aJoints[JointType_HandLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_HandLeft], aJoints[JointType_HandTipLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_HandLeft], aJoints[JointType_ThumbLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_ShoulderRight], aJoints[JointType_ElbowRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_ElbowRight], aJoints[JointType_WristRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_WristRight], aJoints[JointType_HandRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_HandRight], aJoints[JointType_HandTipRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_HandRight], aJoints[JointType_ThumbRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_HipLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_HipLeft], aJoints[JointType_KneeLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_KneeLeft], aJoints[JointType_AnkleLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_AnkleLeft], aJoints[JointType_FootLeft], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_HipRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_HipRight], aJoints[JointType_KneeRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_KneeRight], aJoints[JointType_AnkleRight], pCoordinateMapper);
DrawLine(mImg, aJoints[JointType_AnkleRight], aJoints[JointType_FootRight], pCoordinateMapper);
//----关节角输出与映射---------
JointOrientation aOrientations[JointType::JointType_Count];
if (pBody->GetJointOrientations(JointType::JointType_Count, aOrientations) == S_OK)
{
string str;
stringstream ss;
//Joint information Left Limb and Right Limb
for (int i = 4; i <12; i++) {
cout << "J" << i << "(" << get_name(i) << ")" << endl;
cout << " pos[" << aJoints[i].Position.X << "," << aJoints[i].Position.Y << "," << aJoints[i].Position.Z << "]" << endl;
cout << " ori[" << aOrientations[i].Orientation.w<<"," << aOrientations[i].Orientation.x << "," << aOrientations[i].Orientation.y << "," << aOrientations[i].Orientation.z <<"]"<< endl;
cout << endl;
}
//mapping
//************ 请添加映射代码 *************
//************
//发送消息，可以自己改格式
ss << s0_L << "," << s1_L << "," << e0_L << "," << e1_L << "," << w1_L << ",";
ss << s0_R << "," << s1_R << "," << e0_R << "," << e1_R << "," << w1_R << endl;
str = ss.str();
str.copy(buffSend, str.length(), 0);
buffSend[str.length() + 1] = '\0';
send(sockClient, buffSend, strlen(buffSend) + 1, 0);
//Sleep(SLEEP_TIME);
}
}
}
}
}
else
{
cerr << "Can't read body data" << endl;
}
// 4e. 释放bodyframe
pBodyFrame->Release();
}
// 输出图像
imshow("Body Image", mImg);
imshow("colorImage", mColorImg);
if (waitKey(30) == VK_ESCAPE) {
break;
}
}
delete[] aBodyData;
// 3.释放frame reader
cout << "Release body frame reader" << endl;
pBodyFrameReader->Release();
pBodyFrameReader = nullptr;
// 2. 释放 color frame reader
cout << "Release color frame reader" << endl;
pColorFrameReader->Release();
pColorFrameReader = nullptr;
// 1c.关闭Sensor
cout << "close sensor" << endl;
pSensor->Close();
// 1d. 释放Sensor
cout << "Release sensor" << endl;
pSensor->Release();
pSensor = nullptr;
//
//关闭套接字
closesocket(sockClient);
WSACleanup();
return 0;
}
const string get_name(int n)
{
switch (n)
{
case 0:return "Spine Base"; break;
case 1:return "Spine Mid"; break;
case 2:return "Neck"; break;
case 3:return "Head"; break;
case 4:return "Left Shoulder"; break;
case 5:return "Left Elbow"; break;
case 6:return "Left Wrist"; break;
case 7:return "Left Hand"; break;
case 8:return "Right Shoulder"; break;
case 9:return "Right Elbow"; break;
case 10:return "Right Wrist"; break;
case 11:return "Right Hand"; break;
case 12:return "Left Hip"; break;
case 13:return "Left Knee"; break;
case 14:return "Left Ankle"; break;
case 15:return "Left Foot"; break;
case 16:return "Right Hip"; break;
case 17:return "Right Knee"; break;
case 18:return "Right Ankle"; break;
case 19:return "Right Foot"; break;
case 20:return "Spine Shoulder"; break;
case 21:return "Left HandTip"; break;
case 22:return "Left Thumb"; break;
case 23:return "Right HandTip"; break;
case 24:return "Right Thumb"; break;
default:return "NULL";
}
}
void DrawLine(Mat& Img, const Joint& r1, const Joint& r2, ICoordinateMapper* pMapper)
{
//用两个关节点来做线段的两端，并且进行状态过滤
if (r1.TrackingState == TrackingState_NotTracked || r2.TrackingState == TrackingState_NotTracked)
return;
//要把关节点用的摄像机坐标下的点转换成彩色空间的点
ColorSpacePoint p1, p2;
pMapper->MapCameraPointToColorSpace(r1.Position, &p1);
pMapper->MapCameraPointToColorSpace(r2.Position, &p2);
line(Img, Point(p1.X, p1.Y), Point(p2.X, p2.Y), Vec3b(0, 0, 255), 5);
}Ubuntu上接收socket部分参考代码
#include<stdio.h>
#include<stdlib.h>
#include<errno.h>
#include<sys/types.h>
#include<sys/socket.h>
#include<netinet/in.h>
#include<arpa/inet.h>
#include<unistd.h>
#include <string>
#include<sstream>
#include<iostream>
#include <vector>
#define MAXLINE 4096
#define SEVER_PORT 5099
using namespace std;
int main(int argc, char** argv){
int listenfd, connfd;
struct sockaddr_in servaddr;
char buff[4096];
int n;
cout<<"Initiallizing network..."<<endl;
if( (listenfd = socket(AF_INET, SOCK_STREAM, 0)) == -1 ){
printf("create socket error: %s(errno: %d)\n",strerror(errno),errno);
return 0;
}
memset(&servaddr, 0, sizeof(servaddr)); //置零
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
servaddr.sin_port = htons(SEVER_PORT);
if( bind(listenfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) == -1){
//printf("bind socket error: %s(errno: %d)\n",strerror(errno),errno);
return 0;
}
if( listen(listenfd, 10) == -1){
//printf("listen socket error: %s(errno: %d)\n",strerror(errno),errno);
return 0;
}
//printf("======waiting for client's request======\n");
while(true){
if( (connfd = accept(listenfd, (struct sockaddr*)NULL, NULL)) == -1){
//printf("accept socket error: %s(errno: %d)",strerror(errno),errno);
continue;
}else{
//printf("accept client %s \n",inet_ntoa(servaddr.sin_addr));
send(connfd,"Welcome to ubuntu server\n",25,0);//发送欢迎信息
}
while((n = recv(connfd,buff,MAXLINE,0))>0){
buff[n] = '\0';
printf("msg from client: %s\n",buff);
std_msgs::Float64 angle_ls0;
std_msgs::Float64 angle_ls1;
std_msgs::Float64 angle_le0;
std_msgs::Float64 angle_le1;
std_msgs::Float64 angle_lw1;
std_msgs::Float64 angle_rs0;
std_msgs::Float64 angle_rs1;
std_msgs::Float64 angle_re0;
std_msgs::Float64 angle_re1;
std_msgs::Float64 angle_rw1;
vector<double> angles;
char *p;
char *split = ",";
p = strtok(buff,split);
angles.clear(); //Clear array
char* pEnd;
int length = 1;
angles.push_back(strtod(p,&pEnd));
while(p = strtok(NULL,split)){
angles.push_back(strtod(p,&pEnd));
length++;
}
}
close(connfd);
}
close(listenfd);
return 0;
}为能够对机器人进行精确的控制，需要对其进行运动学建模。因此我们采用D-H法对Baxter机器人进行建模，D-H法是一种描述串联式链路上连杆和关节几何关系的系统方法。
Baxter机器人有两个手臂，每个手臂由7个关节组成，分别是s0、s1、e0、e1、w0、w1、w2，由D-H法我们建立各个关节的坐标系，如下图所示：
由官方手册上的数据，以Baxter机器人右臂为例，其D-H参数如下表所示：
通过这些参数，我们在的Matlab中利用Robotics toolbox工具箱可以得到Baxter机器人的模型,并且利用这一工具箱，我们能够方便地对Baxter机器人进行正逆运动学解算，并可进行简单的轨迹规划。
现在我们得到了人体的关节信息，又对Baxter机器人进行了建模，那么就需要建立一个从人体关节的三维空间位置到机器人手臂关节角度的映射关系，从而能够依据人的动作来控制Baxter机械臂的运动。
以人的左臂为例，α为水平方向上大臂的转动角度，β为竖直方向上大臂的转动角度，μ为大臂绕自身的旋转角度，γ表示大臂和小臂间的夹角。
- α即为大臂所在的向量在平面XBOZB上的投影与OZB的夹角。
- β即为大臂所在的向量与OYB的夹角。
- μ即为大臂所在的向量与OYB构成的平面和大臂小臂所在的平面的法向量间的夹角。
- γ即为大臂所在的向量与小臂所在向量的夹角。
由于Baxter的机械臂与人的手臂结构高度相似，将上述四个角度分别作为Baxter的s0、s1、e0、e1的转动角度输入即可让Baxter模仿人此时的手臂位姿，同时为了防止Baxter超出工作空间，将所得的角度线性插入Baxter的每一个关节的工作区间范围即可。
通过之前的所有工作，我们就能够将Kinect摄像头前一个人的手臂动作转换为Baxter机器人的关节角度信息了，于是在ROS中我们建立了两个节点，分别读取Baxter左右两只手臂的关节角度时间序列，并控制Baxter的两只手臂依据控制这一信息进行相应的运动，即可完成人体动作跟随任务。
本项目将使用多个节点，更能深入了解ROS的通信机制。
实现五子棋对战需要
- 获取棋盘信息
- 计算下一步位置
- 移动棋子
系统设计如下：
将左机械臂末端朝向棋盘拍摄棋盘，获取棋盘上的布局信息。根据当前布局信息和历史布局信息可以得到人类下的位置，将此位置/posi/human传递给五子棋计算引擎，通过策略搜索，生成ai方落子位置/posi/ai。右机械臂根据开始时的标定以及落子的行列位置，得到放置棋子的物理位置；放置之后，回到取棋子处，利用图像识别抓取ai方颜色的棋子。
这是参考工程链接，建议加入一定的创新元素。
为了在节点间传递行棋信息，定义了ChessMove.msg消息。
uint16 row
uint16 col
char state
字符state表示当前状态。N表示正常下棋，Q表示退出棋局，U表示悔棋，H表示困难模式（即将搜索深度加深），E表示简单模式。
关于如何使用自定义消息，可以在ROS wiki中找到帮助。
为了使用matlab的图像处理函数，开发时用matlab实现了棋盘的自动识别，为了程序的统一性起见，也可迁移到python用opencv实现。
主要代码在参考链接中的statepub.m, Getstate.m, JudgeInside.m
参考链接中教程完成Matlab端的准备工作
参考链接中教程完成Matlab端的自定义消息类
注：如果无法成功建立ros节点，试试是否能够ping通011606P0006.local，若无法ping通，在windows的host文件中加入一行
192.168.0.2 011606P0006.local
其中192.168.0.2为baxter实际的ip地址，可能会有所改变。
这样即可解决问题。
图像识别处理棋盘
准备工作完成，下面正式开始进行图像识别处理棋盘的工作
为方便图像处理，棋盘如下，在其四角标上用于定标的紫色色块，棋子采用蓝色和黄色的泡沫小方块，方便抓取。
为获取当前棋盘的状态，我们通过读取机器人左手手臂上的摄像机的图像，分别对其进行前景分割、颜色阈值分割、形态学处理、透视变换最终得到当前的棋盘对局信息。
由于棋盘整体呈白色，通过对hsv空间中的亮度v空间进行分割，并进一步进行形态学填充和形态学开运算，可以得到只含棋盘的图片，减少后续计算量。
另一方面，后面将会说到，通过颜色阈值分割得到了紫色色块的位置后，即可确定棋盘内部在图像中的位置。我们容易知道，四个顶点构成的凸四边形内部一点到各顶点之间的向量间的夹角和为360°，而外部一点的夹角和小于360°，通过这一方法可以得到棋盘内部的区域，再结合亮度空间分割得到的结果，即可判断此时棋盘上是否有障碍物，从而可以让识别棋盘不受障碍物的干扰。
%得到前景分割区域
se = strel('square',4);
mask = hsv(:,:,3) > thresholdmask;
mask = imopen(mask,se);
mask = imfill(mask,'holes');
[mx my] = find(mask);
maskn = mask(min(mx):max(mx),min(my):max(my));%将前景分割区域提出来此处分别对紫色色块、蓝色棋子、黄色棋子进行颜色阈值分割，从而在各个图像上得到其所在的位置，但由于实验室光照条件经常有所改变，故每次实验前仍需依照当前光照条件调整阈值。
Blue_h = 0.5;
Blue_s = 0.4;
Blue_v = 0.35;
Yellow_h = 0.15;
Yellow_s = 0.1;
Purple_h = 0.6;
Purple_s = 0.2;
Purple_v = 0.3;
%得到蓝色、黄色、紫色三个区域
ChessBlue = (hsv(min(mx):max(mx),min(my):max(my),1)>Blue_h).*(hsv(min(mx):max(mx),min(my):max(my),2)>Blue_s).*(hsv(min(mx):max(mx),min(my):max(my),3)>Blue_v);
ChessYellow = (hsv(min(mx):max(mx),min(my):max(my),1)<Yellow_h).*(hsv(min(mx):max(mx),min(my):max(my),2)>Yellow_s);
Purple = (hsv(min(mx):max(mx),min(my):max(my),1)>Purple_h).*(hsv(min(mx):max(mx),min(my):max(my),2)>Purple_s).*(hsv(min(mx):max(mx),min(my):max(my),3)<Purple_v);
ChessBlue = ChessBlue.*maskn;
ChessYellow = ChessYellow.*maskn;
Purple = Purple.*maskn;
%开运算去除孤立点
se = strel('square',5);
ChessBlue = imopen(ChessBlue,se);
se = strel('square',3);
ChessYellow = imopen(ChessYellow,se);
% se = strel('square',6);
Purple = imopen(Purple,se);通过颜色阈值分割得到紫色色块所在的位置后，由于存在一些噪点的影响，分别提取最大的四个连通分量，并通过四个区域的位置关系，得到其不同的边界点，进而得到棋盘真正的角的位置。
通过之前的棋盘图片可以看到，由于摄像机并不在棋盘的正上方，故棋盘在图片中是倾斜的，这里我们通过透视变换，利用四角顶点的位置，将棋盘及棋盘上的棋子校正为正视的状态，虽然棋盘的格点间距横竖方向在矫正后有微小差异，但不影响读取棋盘的信息
% 透视变换
function [imgn ,dot2] = Switch(img , dot)
[M N] = size(img);
dot2 = zeros(2,2);
w=round(sqrt((dot(1,1)-dot(2,1))^2+(dot(1,2)-dot(2,2))^2)); %从原四边形获得新矩形宽
h=round(sqrt((dot(1,1)-dot(3,1))^2+(dot(1,2)-dot(3,2))^2)); %从原四边形获得新矩形高
y=[dot(1,1) dot(2,1) dot(3,1) dot(4,1)]; %四个原顶点
x=[dot(1,2) dot(2,2) dot(3,2) dot(4,2)];
%这里是新的顶点，我取的矩形,也可以做成其他的形状
%大可以原图像是矩形，新图像是从dot中取得的点组成的任意四边形.:)
Y=[dot(1,1) dot(1,1) dot(1,1)+h dot(1,1)+h];
X=[dot(1,2) dot(1,2)+w dot(1,2) dot(1,2)+w];
B=[X(1) Y(1) X(2) Y(2) X(3) Y(3) X(4) Y(4)]'; %变换后的四个顶点，方程右边的值
%联立解方程组，方程的系数
A=[x(1) y(1) 1 0 0 0 -X(1)*x(1) -X(1)*y(1);
0 0 0 x(1) y(1) 1 -Y(1)*x(1) -Y(1)*y(1);
x(2) y(2) 1 0 0 0 -X(2)*x(2) -X(2)*y(2);
0 0 0 x(2) y(2) 1 -Y(2)*x(2) -Y(2)*y(2);
x(3) y(3) 1 0 0 0 -X(3)*x(3) -X(3)*y(3);
0 0 0 x(3) y(3) 1 -Y(3)*x(3) -Y(3)*y(3);
x(4) y(4) 1 0 0 0 -X(4)*x(4) -X(4)*y(4);
0 0 0 x(4) y(4) 1 -Y(4)*x(4) -Y(4)*y(4)];
fa=inv(A)*B; %用四点求得的方程的解，也是全局变换系数
a=fa(1);b=fa(2);c=fa(3);
d=fa(4);e=fa(5);f=fa(6);
g=fa(7);h=fa(8);
rot=[d e f;
a b c;
g h 1]; %公式中第一个数是x,Matlab第一个表示y，所以我矩阵1,2行互换了
pix1=rot*[1 1 1]'/(g*1+h*1+1); %变换后图像左上点
pix2=rot*[1 N 1]'/(g*1+h*N+1); %变换后图像右上点
pix3=rot*[M 1 1]'/(g*M+h*1+1); %变换后图像左下点
pix4=rot*[M N 1]'/(g*M+h*N+1); %变换后图像右下点
height=round(max([pix1(1) pix2(1) pix3(1) pix4(1)])-min([pix1(1) pix2(1) pix3(1) pix4(1)])); %变换后图像的高度
width=round(max([pix1(2) pix2(2) pix3(2) pix4(2)])-min([pix1(2) pix2(2) pix3(2) pix4(2)])); %变换后图像的宽度
delta_y=round(abs(min([pix1(1) pix2(1) pix3(1) pix4(1)]))); %取得y方向的负轴超出的偏移量
delta_x=round(abs(min([pix1(2) pix2(2) pix3(2) pix4(2)]))); %取得x方向的负轴超出的偏移量
inv_rot=inv(rot);
imgn=zeros(height,width);
dot2(1,1) = 0;
dot2(2,2) = width;
for i = height-delta_y:-1:1-delta_y %从变换图像中反向寻找原图像的点，以免出现空洞，和旋转放大原理一样
for j = 1-delta_x:width-delta_x
if i > dot2(1,1) -delta_y - 3 && j < dot2(2,2) - delta_x + 2
pix=inv_rot*[i j 1]'; %求原图像中坐标，因为[YW XW W]=fa*[y x 1],所以这里求的是[YW XW W],W=gy+hx+1;
pix=inv([g*pix(1)-1 h*pix(1);g*pix(2) h*pix(2)-1])*[-pix(1) -pix(2)]'; %相当于解[pix(1)*(gy+hx+1) pix(2)*(gy+hx+1)]=[y x],这样一个方程，求y和x，最后pix=[y x];
if pix(1)>=0.5 && pix(2)>=0.5 && pix(1)<=M && pix(2)<=N
imgn(i+delta_y,j+delta_x)=img(round(pix(1)),round(pix(2))); %最邻近插值,也可以用双线性或双立方插值
end
if abs(round(pix(1)) - dot(1, 2)) <= 1 && abs(round(pix(2)) - dot(1, 1)) <= 1
dot2(1,1) = i+delta_y;
dot2(1,2) = j+delta_x;
else
if abs(round(pix(1)) - dot(4, 2)) <= 1 && abs(round(pix(2)) - dot(4, 1)) <= 1
dot2(2,1) = i+delta_y;
dot2(2,2) = j+delta_x;
end
end
end
end
end通过上面的所有运算，最终我们就将一张较为模糊、倾斜的棋盘图片，转换为清晰可见的棋盘对局信息。
矫正前
矫正后
棋盘对局信息
主要代码在参考链接中的gobang_ai.py
我们使用了GitHub上的一个开源项目，添加修改了一些代码，将其作为一个gobang_ai节点，接收棋局图像处理得到的人类下棋位置，并发布AI下棋位置。
该项目用的是策略搜索的方法，根据各种情况（竖直水平左斜右斜的三连，四连等）为每一处落子计算得分；可以设定迭代的层数，递归搜索，选择最优解作为落子地点。
实验室提供了蓝色和黄色的泡沫小方块，2*2*2cm大小，另外可以调整抓手的安装位置，以实现抓取/放置。
主要代码在参考链接中的controller.py
右机械臂节点arm_controller，接收posi/ai主题的消息，控制机械臂将棋子放置在指定的位置。在放置完成后执行抓取并等待执行下一个放置命令。
利用Baxter手臂末端的摄像头，可以确定附近的棋子位置。为了得到较高的传输速率，这里选择了480×300像素分辨的相机模式。
class image_converter实现右手臂的图像处理，核心函数为_image_process
经过颜色阈值分割和形态学操作得到蓝色棋子的轮廓，就可以确定棋子的位置与角度，并选择离抓手中心最近的一个棋子作为目标。下图中，绿色为识别的棋子，红色圈出为选定的棋子。
这里需要注意的是，摄像头并不在机械臂末端的中心，如上图中蓝点（红色方框右上角）才是图片中抓手的中心。图像处理返回目标棋子与抓手中心的位置差距bias_x,bias_y，有
当目标棋子的位置和角度的差小于一定阈值，就进行抓取。
为了尽可能避免碰到其他棋子，在抓取之前先收拢抓手一定程度。同样在放置时也先放开一点，上升之后才全部放开抓手。
如果你可以通过对棋盘观察的图像处理就能得到坐标那当然再好不过啦。如果实现不了可以考虑在开始前进行一次定标，棋盘上也提供了四角上的紫色方块方便实现。
将接收的棋盘位置转换为机器人坐标下的位置，可以事先确定棋盘的位置，并利用已知的棋盘属性，即可计算出各个棋盘坐标在机器人坐标下的位置。为此，首先利用四角的紫色方块进行定标。这里使用的图像处理方法与抓取部分的相似，只是换了识别的颜色。
这里存储的是计算得到的棋盘左上角点位置bx,by与棋盘的偏斜角度bt
x = self._board_cfg._bx + (col*math.sin(self._board_cfg._bt)-(row)*math.cos(self._board_cfg._bt))*self._board_cfg._bg
y = self._board_cfg._by - (col*math.cos(self._board_cfg._bt)+(row)*math.sin(self._board_cfg._bt))*self._board_cfg._bg这里的bg是每个方格的大小即0.04m。
Baxter拥有一个720P的显示屏和一些声纳传感器，你可以用它们来实现一些人机交互。
为了使人机交互的体验更佳，我们定制了一些适合在屏幕显示的图片动态显示在头部的显示屏上。
使用一张睁眼和一张闭眼的图片即可制造眨眼的效果。并加入随机睁眼时间，显得不那么机械。
其中睁眼包括向上下左右各个方向，可以根据当前的系统状态决定。
下面是一个使其眨眼的示例：
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import os
import sys
import argparse
import rospy
import cv2
import cv_bridge
import PIL #加载和集成PNG等图像
#from baxter_demo_ui import img_proc
import numpy as np
import random
import time
from sensor_msgs.msg import (
Image,
)
from std_msgs.msg import Char
class face(object):
def __init__(self):
rospy.init_node('baxter_emotion', anonymous=True)
rospy.Subscriber('ai_state',Char,self.cb_face)
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True,queue_size=2)
def load(self,name):
print name
path = 'img/'+name+'.jpg'
if not os.access(path, os.R_OK):
rospy.logerr("Cannot read file at '%s'" % (path,))
return False
img = cv2.imread(path)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
return msg
def cb_face(self,ai_s):
self.state = ai_s
print ai_s
def show_image(self,name):
msg = self.load(name)
self.pub.publish(msg)
rospy.sleep(1)
def go(self):
winkle_delay = 3+random.randint(0,100)/50.0
time_last = time.time()
rate = rospy.Rate(20)
open_eye = True
while not rospy.is_shutdown():
if open_eye:
if time.time()-time_last >= winkle_delay:
time_last = time.time()
self.show_image('close')
winkle_delay = 3+random.randint(0,100)/50.0
open_eye = False
else:
if time.time()-time_last >= 0.5:
time_last = time.time()
self.show_image('normal')
open_eye = True
rate.sleep()
if __name__ == '__main__':
def shut():
print "exiting..."
rospy.on_shutdown(shut)
fa = face()
sys.exit(fa.go())至此，你已经完成了人体动作跟随、人机五子棋对战综合任务的开发，并具备完成自拟综合任务的能力，利用学习到的知识，完成一个自拟综合任务吧！