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的就直接可以用了。真的有毒。