ROS-Serial-Crazypony通信

從第二次開始學(xué)ros到成功進(jìn)行串口通信，讀取vicon數(shù)據(jù)，再將高度信息發(fā)送給crazypony花了差不多十天的時(shí)間，最后的主控程序其實(shí)很短很短，但是終于能完成最基本的功能了，可喜可賀可喜可賀:)。

首先寫出代碼

#include     <stdio.h>      /*標(biāo)準(zhǔn)輸入輸出定義*/
#include     <stdlib.h>     /*標(biāo)準(zhǔn)函數(shù)庫定義*/
#include     <unistd.h>     /*Unix 標(biāo)準(zhǔn)函數(shù)定義*/
#include     <sys/types.h>  
#include     <sys/stat.h>   
#include     <fcntl.h>      /*文件控制定義*/
#include     <termios.h>    /*PPSIX 終端控制定義*/
#include     <errno.h>      /*錯(cuò)誤號定義*/
#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;  //臨時(shí)字符，用做通信協(xié)議
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是高度，于實(shí)際同號
    //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(" 提示錯(cuò)誤！\n"); 
        return 1;
    }
    else
        printf("success open USB0\n");
    //設(shè)置串口，termios.h中定義了終端控制結(jié)構(gòu)體和POSIX控制函數(shù)
    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);

    //保證程序不會(huì)成為端口所有者
    options.c_cflag |= (CLOCAL | CREAD);

    //無校驗(yàn)，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);  //原始數(shù)據(jù)輸入
    options.c_oflag  &= ~OPOST;   //原始數(shù)據(jù)輸出

    tcflush(fd,TCIFLUSH);  //數(shù)據(jù)溢出時(shí)接受數(shù)據(jù)但不讀取

        tcsetattr(fd,TCSANOW,&options);
        status=tcsetattr(fd,TCSANOW,&options);
    if(status!=0)
    {
        perror("com set error");
        return 1;
    }
    
    pthread_t tids[3];  
    //ROS 訂閱發(fā)布消息 
    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函數(shù)：

初始化串口設(shè)置：115200B，8位數(shù)據(jù)位，無控制流，無校驗(yàn)，這個(gè)程序在很多人的博客已經(jīng)寫到了，我 僅僅寫出了與我的無人機(jī)匹配的設(shè)置模式，沒有其他設(shè)置模式的代碼。

打開了三個(gè)線程：RX用于讀取串口，TX用于寫串口，Sub用于訂閱vicon節(jié)點(diǎn)并且通過無線發(fā)送，之所于 要使用者三個(gè)線程，我的想法是因?yàn)閞ead函數(shù)、write函數(shù)、ROSmsg函數(shù)都要寫到循環(huán)里，但是如果寫到一個(gè)函數(shù)里就會(huì)陷入等待中，所以我開了一個(gè)線程專門用來讀緩沖區(qū)，一個(gè)線程等待輸入，第三個(gè)線程是因?yàn)閞os::spins會(huì)一直自循環(huán)，所以額外開了一個(gè)線程并把功能都寫入了回調(diào)函數(shù)。

關(guān)于是開多線程還是利用ROS的優(yōu)勢使用topic通信進(jìn)行控制，我的考慮是因?yàn)閒d這個(gè)文件指針會(huì)被占用，所以應(yīng)該把write和read寫進(jìn)一個(gè)程序，這樣控制程序也自然應(yīng)寫在同一個(gè)函數(shù)，但是因?yàn)槲彝耆珱]有學(xué)習(xí)過C++，也對串口的API細(xì)節(jié)不了解，所以也并不確定。

有一個(gè)要注意的點(diǎn)是要記得及時(shí)銷毀文件指針，在*rx和*tx里我使用test和flag就是想要控制銷毀該指針（但是最后的結(jié)果很奇怪，在我輸入s后tx確實(shí)不再寫串口也打印了Tx Exit，但是但我輸入時(shí)仍然會(huì)打印write），如果不及時(shí)銷毀，直接關(guān)閉終端會(huì)使ttyUSB0被占用（我也不確定是不是被占用，因?yàn)槲以?dev/里找不到ttyUSB0了，只有ttyUSB1，如果繼續(xù)使用ttyUSB1，那么下一次dev里就只有ttyUSB2了）

最后，整個(gè)過程中最蛋疼的一點(diǎn)是，我使用crazypony做地面站時(shí)，怎么都無法串口通信，我檢查程序檢查驅(qū)動(dòng)一切都似乎沒有問題，然后我嘗試把地面站程序移植到了一塊開發(fā)板，就直接可以通信了。我猜想是因?yàn)閏p2102驅(qū)動(dòng)的問題(雖然我系統(tǒng)顯示我驅(qū)動(dòng)沒問題)，開發(fā)板是ch340的就直接可以用了。真的有毒。