ROS-Serial-Crazypony通信
从第二次开始学ros到成功进行串口通信，读取vicon数据，再将高度信息发送给crazypony花了差不多十天的时间，最后的主控程序其实很短很短，但是终于能完成最基本的功能了，可喜可贺可喜可贺:)。
首先写出代码
#include <stdio.h> /*标准输入输出定义*/
#include <stdlib.h> /*标准函数库定义*/
#include <unistd.h> /*Unix 标准函数定义*/
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h> /*文件控制定义*/
#include <termios.h> /*PPSIX 终端控制定义*/
#include <errno.h> /*错误号定义*/
#include <iostream>
#include <pthread.h>
#include <cstdlib>
#include <string.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>
#include <vicon_bridge/Marker.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
using namespace std;
int fd=open( "/dev/ttyUSB0", O_RDWR|O_NOCTTY|O_NDELAY);
int nread;
char buff[512];
char tempbuff[512];
int buffnum=0;
char test;
char tempone; //临时字符，用做通信协议
char temptwo;
int status;
int flag=0;
void *tx(void*)
{
while(1)
{
if(fscanf(stdin,"%c",&test)>0)
{
printf("write\n");
if(test=='s')
{
flag=1;
close(fd);
printf("Exit Tx");
}
tempone='c';
write(fd,&tempone,1);
write(fd,&test,1);
tempone='s';
write(fd,&tempone,1);
}
}
printf("Exit Tx");
}
void *rx(void*)
{
int i=0;
while(flag==0)
{
if((nread=read(fd,buff,512))>0)
{
printf("%s\n",buff);
for(i=0;i<nread;i++)
{
tempbuff[buffnum]=buff[i];
// printf("%s\n",tempbuff);
buffnum++;
if(tempbuff[buffnum-1]=='!')
{
// printf("%s\n",tempbuff);
tempbuff[0]='\0';
buffnum=0;
}
}
}
}
printf("Exit Rx");
}
int height;
void callback(const geometry_msgs::TransformStamped& msg)
{
//测试用,Z是高度，于实际同号
//printf("translation.x %f ",msg.transform.translation.x);
//printf("translation.y %f ",msg.transform.translation.y);
//printf("translation.z %f\n",msg.transform.translation.z);
height=(int)(msg.transform.translation.z*100.0);
//printf("send %d,height %f\n",height,height/100.0);
temptwo='h';
write(fd,&temptwo,1);
write(fd,&height,1);
temptwo='s';
write(fd,&temptwo,1);
}
void *spin(void*)
{
ros::spin();
}
int main(int argc,char **argv)
{
//打开串口
int count;
printf("begin\n");
if (fcntl(fd,F_SETFL,0)<0)
{
perror(" 提示错误！\n");
return 1;
}
else
printf("success open USB0\n");
//设置串口，termios.h中定义了终端控制结构体和POSIX控制函数
if(0==isatty(STDIN_FILENO))
{
printf("standard input is not terminal device\n");
return 1;
}
struct termios options;
tcgetattr(fd, &options);
//set speed
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
//保证程序不会成为端口所有者
options.c_cflag |= (CLOCAL | CREAD);
//无校验，8位
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
options.c_cflag &= ~CRTSCTS; //不使用流控制
options.c_iflag &= ~INPCK;
//原始模式通信
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); //原始数据输入
options.c_oflag &= ~OPOST; //原始数据输出
tcflush(fd,TCIFLUSH); //数据溢出时接受数据但不读取
tcsetattr(fd,TCSANOW,&options);
status=tcsetattr(fd,TCSANOW,&options);
if(status!=0)
{
perror("com set error");
return 1;
}
pthread_t tids[3];
//ROS 订阅发布消息
ros::init(argc,argv,"CP_Posture");
ros::NodeHandle n;
ros::Publisher chatter_pub=n.advertise<std_msgs::String>("posture",1000);
ros::Subscriber sub=n.subscribe("vicon/CP1_1/CP1_1",1000,callback);
ros::Rate loop_rate(1);
int retRx=pthread_create(&tids[0],NULL,rx,NULL);
int retTx=pthread_create(&tids[1],NULL,tx,NULL);
int retSub=pthread_create(&tids[2],NULL,spin,NULL);
while(1)
{
if(ros::ok)
{
//printf("hello\n");
count++;
std_msgs::String msg;
std::stringstream ss;
ss << "hello" << count;
msg.data = ss.str();
chatter_pub.publish(msg);
loop_rate.sleep();
ros::spinOnce();
}
}
//异常检测
if(retRx!=0)
{
printf("retRX error\n");
}
if(retTx!=0)
{
printf("retTX error\n");
}
pthread_exit(NULL);
close(fd);
return 0;
}
代码流程
mian函数：
初始化串口设置：115200B，8位数据位，无控制流，无校验，这个程序在很多人的博客已经写到了，我 仅仅写出了与我的无人机匹配的设置模式，没有其他设置模式的代码。
打开了三个线程：RX用于读取串口，TX用于写串口，Sub用于订阅vicon节点并且通过无线发送，之所于 要使用者三个线程，我的想法是因为read函数、write函数、ROSmsg函数都要写到循环里，但是如果写到一个函数里就会陷入等待中，所以我开了一个线程专门用来读缓冲区，一个线程等待输入，第三个线程是因为ros::spins会一直自循环，所以额外开了一个线程并把功能都写入了回调函数。
关于是开多线程还是利用ROS的优势使用topic通信进行控制，我的考虑是因为fd这个文件指针会被占用，所以应该把write和read写进一个程序，这样控制程序也自然应写在同一个函数，但是因为我完全没有学习过C++，也对串口的API细节不了解，所以也并不确定。
有一个要注意的点是要记得及时销毁文件指针，在*rx和*tx里我使用test和flag就是想要控制销毁该指针（但是最后的结果很奇怪，在我输入s后tx确实不再写串口也打印了Tx Exit，但是但我输入时仍然会打印write），如果不及时销毁，直接关闭终端会使ttyUSB0被占用（我也不确定是不是被占用，因为我在/dev/里找不到ttyUSB0了，只有ttyUSB1，如果继续使用ttyUSB1，那么下一次dev里就只有ttyUSB2了）
最后，整个过程中最蛋疼的一点是，我使用crazypony做地面站时，怎么都无法串口通信，我检查程序检查驱动一切都似乎没有问题，然后我尝试把地面站程序移植到了一块开发板，就直接可以通信了。我猜想是因为cp2102驱动的问题(虽然我系统显示我驱动没问题)，开发板是ch340的就直接可以用了。真的有毒。
