ROS CAN总线设备接入（二）can总线数据提取和以ros topic形式发布

【简介】基于前ROS CAN总线设备接入（一），我们成功实现了对于libpcan库的使用，本次将实现对于can总线的初始化以及对于can总线上有效数据提取，并将其以topic形式发布到ros节点中。

【正文】

1，要完成数据读取，需要对以下函数进行映射。

//one-to-one mapping using dlsym function,if return null,mapping would be failed
    funCAN_Init         =(funCAN_Init_TYPE)         dlsym(libm_handle,"CAN_Init");
    funLINUX_CAN_Open   =(funLINUX_CAN_Open_TYPE)   dlsym(libm_handle,"LINUX_CAN_Open");
    funCAN_Close        =(funCAN_Close_TYPE)        dlsym(libm_handle,"CAN_Close");
    funCAN_VersionInfo  =(funCAN_VersionInfo_TYPE)  dlsym(libm_handle,"CAN_VersionInfo");
    funLINUX_CAN_Read   =(funLINUX_CAN_Read_TYPE)   dlsym(libm_handle,"LINUX_CAN_Read");
    funCAN_Status       =(funCAN_Status_TYPE)       dlsym(libm_handle,"CAN_Status");
    funnGetLastError    =(funnGetLastError_TYPE)    dlsym(libm_handle,"nGetLastError");

映射的方法在设备接入（一）教程中有详细介绍。程序中用了多处dlerror()进行dlfcn函数调用中的错误处理，该函数每次调用会清除之前错误队列，返回当前函数调用出现的错误。

2，pcan使用时可以先打开，再进行波特率、标准模式或扩展模式的设置等操作。

    char txt[VERSIONSTRING_LEN];            //store information of can version，存储信息
    unsigned short wBTR0BTR1 = CAN_BAUD_1M; //set the communicate baud rate of can bus，波特率
    int nExtended = CAN_INIT_TYPE_ST;       //set can message int standard model，标准模式
    const char  *szDevNode = DEFAULT_NODE;  //define const pointer point to device name，定义设备地址
    pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR );//use mapping function

如果打开的pcan_handle非空，则函数调用成功，进行下一步处理。此处进行了打开成功的提示，并通过读取驱动can的版本号来验证是否驱动成功。

    //judge whether the call is success.if pcan_handle=null,the call would be failed
    if(pcan_handle){
        printf("CAN Bus test: %s have been opened
", szDevNode);
        errno = funCAN_VersionInfo(pcan_handle, txt);
        if (!errno)
            printf("CAN Bus test: driver version = %s
", txt);
        else {
            perror("CAN Bus test: CAN_VersionInfo()");
        }

以上程序片段即完成了打开can是否成功的验证，运行时会输出以下类似信息。

robot@robot-ThinkPad-T410:~$ rosrun beginner_tutorials can_test 
CAN Bus test: /dev/pcan0 have been opened
CAN Bus test: driver version = Release_20130814_n
Device Info: /dev/pcan0; CAN_BAUD_1M; CAN_INIT_TYPE_ST

然后进行初始化函数的调用，wBTR0BTR1代表了can的波特率，引用此名称主要参考lipcan官方给的测试源码，这样后期其许多函数移植会比较方便。

        if (wBTR0BTR1) {
                errno = funCAN_Init(pcan_handle, wBTR0BTR1, nExtended);
                if (errno) {
                    perror("CAN Bus test: CAN_Init()");
                }
                else
                    printf("Device Info: %s; CAN_BAUD_1M; CAN_INIT_TYPE_ST
", szDevNode);
            }
    }
    else
        printf("CAN Bus test: can't open %s
", szDevNode);

3，数据的提取和发布。要实现数据的提取和发布需要以下函数进行辅助，将分别对其做介绍。

//function declaration
int  read_loop(HANDLE h,ros::Publisher pub,bool display_on,bool publish_on);<span style="white-space:pre">//循环读取函数，可选择是否终端打印和topic发布
void print_message(TPCANMsg *m);<span style="white-space:pre">						//打印接收到的消息，主要打印数据
void print_message_ex(TPCANRdMsg *mr);<span style="white-space:pre">					//打印消息的预处理打印，打印时间戳等信息
void do_exit(void *file,HANDLE h,int error);<span style="white-space:pre">				//退出处理，包括关闭文件指针等操作
void signal_handler(int signal);<span style="white-space:pre">						//异常事件（ctrl+c等）发生时的中断服务程序
void init();<span style="white-space:pre">								//初始化事件signal相关操作，即指定中断服务函数
void publish_forcedata(TPCANRdMsg *mr,ros::Publisher force_pub);<span style="white-space:pre">		//以topic形式进行数据发布

跟异常事件相关的函数不是本文重点，其中do_exit()部分的工作在结束终端和进程时系统也会自动执行，这样主要是比较安全而且是一个习惯问题。原型如下，读者感兴趣可以自己查找相关资料。

// exit handler
void do_exit(void *file,HANDLE h,int error)
{
    //Must close h handle firstly,then close file using dlclose
    if (h) {
        funCAN_Close(h);
    }
    printf("
CAN Bus test: finished (%d).

", error);
    //after call the target function in ELF object,close it using dlclose
    dlclose(file);
    exit(error);
}

// the signal handler for manual break Ctrl-C
void signal_handler(int signal)
{
    do_exit(libm_handle,pcan_handle,0);
}

// what has to be done at program start
void init()
{
    /* install signal handlers */
    signal(SIGTERM, signal_handler);
    signal(SIGINT, signal_handler);
}

主要相关的函数为read_loop()，print_message(),print_message_ex(),publish_forcedata().，下面介绍主要函数：

3.1 read_loop()，该函数需要传入设备句柄，ros publisher对象，是否终端显示，是否topic发布，调用实例如下，调用时要在死循环中调用，目前暂未实现基于事件方式的驱动。

//HANDLE h,ros::Publisher pub, bool display_on,bool publish_on
read_loop(pcan_handle,force_pub,true,true);

下面为函数原型：

//read from CAN forever - until manual break

int read_loop(HANDLE h,ros::Publisher pub, bool display_on,bool publish_on)
{
    TPCANRdMsg m;
    __u32 status;

    if (funLINUX_CAN_Read(h, &m)) {
        perror("receivetest: LINUX_CAN_Read()");//数据读取错误显示，如果调用无错误，则数据存储到 m 对象中
        return errno;
    }

    if (display_on)
        print_message_ex(&m);
    if (publish_on)
        publish_forcedata(&m,pub);

    // check if a CAN status is pending，检测can总线状态，状态异常则终端会给出提示
    if (m.Msg.MSGTYPE & MSGTYPE_STATUS) {
        status = funCAN_Status(h);
        if ((int)status < 0) {
            errno = funnGetLastError();
            perror("receivetest: CAN_Status()");
            return errno;
        }

        printf("receivetest: pending CAN status 0x%04x read.
",
               (__u16)status);
    }
    return 0;
}

3.2 print_message(),print_message_ex().该函数主要负责数据打印到终端。

void print_message(TPCANMsg *m)
{
    int i;

    //print RTR, 11 or 29, CAN-Id and datalength
    printf("receivetest: %c %c 0x%08x %1d ",
            ((m->MSGTYPE & MSGTYPE_RTR) ? 'r' : 'm') -<span style="white-space:pre">	//是否远程帧
                ((m->MSGTYPE ) ? 0x20 : 0),<span style="white-space:pre">		//原味检测是否回环模式，此处去掉检测
            (m->MSGTYPE & MSGTYPE_EXTENDED) ? 'e' : 's',<span style="white-space:pre">	//标准模式还是扩展模式
             m->ID,<span style="white-space:pre">					//接收到的数据ID
             m->LEN);<span style="white-space:pre">					//数据长度

    //don't print any telegram contents for remote frames，打印非远程帧的数据区
    if (!(m->MSGTYPE & MSGTYPE_RTR))
        for (i = 0; i < m->LEN; i++)
            printf("%02x ", m->DATA[i]);
          //printf("%3d ", m->DATA[i]);//decimal format print.可十进制打印
    printf("
");
}
//打印接受到数据的时间戳，并将有用数据区传入print_message()函数
void print_message_ex(TPCANRdMsg *mr)
{
    printf("%u.%3u ", mr->dwTime, mr->wUsec);
    print_message(&mr->Msg);
}

3.3 publish_forcedata().要实现数据到topic发布，需要定义msg文件，在当前包的source目录下新建msg文件夹，我新建了ForceData.msg，因为我们只关注数据的id和内容，且数据长度固定为8，因此msg文件内容如下：

int32 id
int16[8] data

然后需要将msg文件添加到当前包内，在CMakeList文件中确保有以下代码段

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

......

 add_message_files(
   FILES
   Num.msg
   ForceData.msg
 )

然后，进行topic的建立和调用，该部分为ros基本内容，如下

    ros::NodeHandle force_handle;
    ros::Publisher force_pub=force_handle.advertise<beginner_tutorials::ForceData>("ArrayForcePUB",1);//advertise a topic named     //"ArrayForceDEV"
    ros::Rate loop_rate(1000);
    //data receive test
    while(ros::ok())
    {   //HANDLE h,ros::Publisher pub, bool display_on,bool publish_on
        read_loop(pcan_handle,force_pub,true,true);
        ros::spinOnce();
        loop_rate.sleep();
    }

应该没有其他问题，此处loop_rate设置频率若低于设备发送数据的频率会导致接受、刷新跟不上，即设备停止后显示或者topic还在发布或者打印数据区，而且持续很长时间。

该篇的完整代码如下所示，当然宝宝的qt还是不能输入中文，凑合看^_^。。。。。。。

#include <ros/ros.h>
#include <stdio.h>
#include <dlfcn.h>
#include <libpcan.h>
#include <fcntl.h>
#include <errno.h>
#include <signal.h>
#include "beginner_tutorials/ForceData.h"

/*
 *
Author  :Hekai SIA
Date    :2016,10,12
Usage   :acquire the message on can bus,and publish the id and data of the message as a ros
        :topic.
E-mail  :hekai@sia.cn;susthekai@qq.com
Debug note:
1,the can_test process can not be ended when press ctrl+c, and the do_exit() can not be run.
2,when run the command-"rostopic echo /ArrayForceDEV",there are some negtive value even if i
added abs function when i copy the data from can message.
result:i use the int8 errorly, and the riginal data was unsigned char,i correct it as int16.

WARNING:Please respect the author the work,Don't use it as business application.
*
*/


//define mapping function according to target function in libpcan.h
typedef DWORD   (*funCAN_Init_TYPE)(HANDLE hHandle, WORD wBTR0BTR1, int nCANMsgType);
typedef HANDLE  (*funLINUX_CAN_Open_TYPE)(const char *szDeviceName, int nFlag);
typedef DWORD   (*funCAN_VersionInfo_TYPE)(HANDLE hHandle, LPSTR lpszTextBuff);
typedef DWORD   (*funCAN_Close_TYPE)(HANDLE hHandle);
typedef DWORD   (*funLINUX_CAN_Read_TYPE)(HANDLE hHandle, TPCANRdMsg* pMsgBuff);
typedef DWORD   (*funCAN_Status_TYPE)(HANDLE hHandle);
typedef int     (*funnGetLastError_TYPE)(void);

//the target device name
#define DEFAULT_NODE "/dev/pcan0"

//GLOBALS
void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
HANDLE pcan_handle =NULL;//void *pcan_handle


beginner_tutorials::ForceData force_msg;

//function declaration
int  read_loop(HANDLE h,ros::Publisher pub,bool display_on,bool publish_on);
void print_message(TPCANMsg *m);
void print_message_ex(TPCANRdMsg *mr);
void do_exit(void *file,HANDLE h,int error);
void signal_handler(int signal);
void init();
void publish_forcedata(TPCANRdMsg *mr,ros::Publisher force_pub);

//define function pointer,there is a one-to-one mapping between target function and your defined function
funCAN_Init_TYPE        funCAN_Init;
funLINUX_CAN_Open_TYPE  funLINUX_CAN_Open;
funCAN_VersionInfo_TYPE funCAN_VersionInfo;
funCAN_Close_TYPE       funCAN_Close;
funLINUX_CAN_Read_TYPE  funLINUX_CAN_Read;
funCAN_Status_TYPE      funCAN_Status;
funnGetLastError_TYPE   funnGetLastError;


int main(int argc, char *argv[])
{
    ros::init(argc,argv,"CAN_Test");
    init();
    //load libpcan.so using dlopen function,return handle for further use
    libm_handle = dlopen("libpcan.so", RTLD_LAZY );
    if (!libm_handle){
        printf("Open Error:%s.
",dlerror());//if file can't be loaded,return null,get reason using dlerror function
        return 0;
    }

    char *errorInfo;//error information pointer
    //one-to-one mapping using dlsym function,if return null,mapping would be failed
    funCAN_Init         =(funCAN_Init_TYPE)         dlsym(libm_handle,"CAN_Init");
    funLINUX_CAN_Open   =(funLINUX_CAN_Open_TYPE)   dlsym(libm_handle,"LINUX_CAN_Open");
    funCAN_Close        =(funCAN_Close_TYPE)        dlsym(libm_handle,"CAN_Close");
    funCAN_VersionInfo  =(funCAN_VersionInfo_TYPE)  dlsym(libm_handle,"CAN_VersionInfo");
    funLINUX_CAN_Read   =(funLINUX_CAN_Read_TYPE)   dlsym(libm_handle,"LINUX_CAN_Read");
    funCAN_Status       =(funCAN_Status_TYPE)       dlsym(libm_handle,"CAN_Status");
    funnGetLastError    =(funnGetLastError_TYPE)    dlsym(libm_handle,"nGetLastError");

    errorInfo = dlerror();//get error using dlerror function,and clear the error list in memory
    if (errorInfo != NULL){
        printf("Dlsym Error:%s.
",errorInfo);
        return 0;
    }

    char txt[VERSIONSTRING_LEN];            //store information of can version
    unsigned short wBTR0BTR1 = CAN_BAUD_1M; //set the communicate baud rate of can bus
    int nExtended = CAN_INIT_TYPE_ST;       //set can message int standard model
    const char  *szDevNode = DEFAULT_NODE;  //define const pointer point to device name
    pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR );//use mapping function

    //judge whether the call is success.if pcan_handle=null,the call would be failed
    if(pcan_handle){
        printf("CAN Bus test: %s have been opened
", szDevNode);
        errno = funCAN_VersionInfo(pcan_handle, txt);
        if (!errno)
            printf("CAN Bus test: driver version = %s
", txt);
        else {
            perror("CAN Bus test: CAN_VersionInfo()");
        }
        if (wBTR0BTR1) {
                errno = funCAN_Init(pcan_handle, wBTR0BTR1, nExtended);
                if (errno) {
                    perror("CAN Bus test: CAN_Init()");
                }
                else
                    printf("Device Info: %s; CAN_BAUD_1M; CAN_INIT_TYPE_ST
", szDevNode);
            }
    }
    else
        printf("CAN Bus test: can't open %s
", szDevNode);

    //initial a talker to publish the force data and can id.
    ros::NodeHandle force_handle;
    ros::Publisher force_pub=force_handle.advertise<beginner_tutorials::ForceData>("ArrayForcePUB",1);//advertise a topic named "ArrayForceDEV"
    ros::Rate loop_rate(1000);
    //data receive test
    while(ros::ok())
    {   //HANDLE h,ros::Publisher pub, bool display_on,bool publish_on
        read_loop(pcan_handle,force_pub,true,true);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}


//read from CAN forever - until manual break
int read_loop(HANDLE h,ros::Publisher pub, bool display_on,bool publish_on)
{
    TPCANRdMsg m;
    __u32 status;

    if (funLINUX_CAN_Read(h, &m)) {
        perror("receivetest: LINUX_CAN_Read()");
        return errno;
    }

    if (display_on)
        print_message_ex(&m);
    if (publish_on)
        publish_forcedata(&m,pub);

    // check if a CAN status is pending
    if (m.Msg.MSGTYPE & MSGTYPE_STATUS) {
        status = funCAN_Status(h);
        if ((int)status < 0) {
            errno = funnGetLastError();
            perror("receivetest: CAN_Status()");
            return errno;
        }

        printf("receivetest: pending CAN status 0x%04x read.
",
               (__u16)status);
    }
    return 0;
}

void publish_forcedata(TPCANRdMsg *mr,ros::Publisher force_pub)
{
    force_msg.id=mr->Msg.ID;
    for(__u8 i=0;i<8;i++)
    force_msg.data[i]=mr->Msg.DATA[i];
    force_pub.publish(force_msg);

}


void print_message(TPCANMsg *m)
{
    int i;

    //print RTR, 11 or 29, CAN-Id and datalength
    printf("receivetest: %c %c 0x%08x %1d ",
            ((m->MSGTYPE & MSGTYPE_RTR) ? 'r' : 'm') -
                ((m->MSGTYPE ) ? 0x20 : 0),
            (m->MSGTYPE & MSGTYPE_EXTENDED) ? 'e' : 's',
             m->ID,
             m->LEN);

    //don't print any telegram contents for remote frames
    if (!(m->MSGTYPE & MSGTYPE_RTR))
        for (i = 0; i < m->LEN; i++)
            printf("%02x ", m->DATA[i]);
          //printf("%3d ", m->DATA[i]);//decimal format print.
    printf("
");
}

void print_message_ex(TPCANRdMsg *mr)
{
    printf("%u.%3u ", mr->dwTime, mr->wUsec);
    print_message(&mr->Msg);
}

// exit handler
void do_exit(void *file,HANDLE h,int error)
{
    //Must close h handle firstly,then close file using dlclose
    if (h) {
        funCAN_Close(h);
    }
    printf("
CAN Bus test: finished (%d).

", error);
    //after call the target function in ELF object,close it using dlclose
    dlclose(file);
    exit(error);
}

// the signal handler for manual break Ctrl-C
void signal_handler(int signal)
{
    do_exit(libm_handle,pcan_handle,0);
}

// what has to be done at program start
void init()
{
    /* install signal handlers */
    signal(SIGTERM, signal_handler);
    signal(SIGINT, signal_handler);
}

实际实验的结果给个图咯！