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

发布时间:2017年08月10日 17:09:03    浏览数:533次    来自:信雪神话
我们成功实现了对于libpcan库的使用,本次将实现对于can总线的初始化以及对于can总线上有效数据提取,并将其以topic形式发布到ros节点中。

【简介】基于前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

以上程序片段即完成了打开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\n", szDevNode);  
        }  
}  
else  
    printf("CAN Bus test: can't open %s\n", 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("\nCAN Bus test: finished (%d).\n\n", 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.\n",  
               (__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("\n");  
}  
//打印接受到数据的时间戳,并将有用数据区传入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文件内容如下:2 id  

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.\n",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.\n",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\n", szDevNode);  
        errno = funCAN_VersionInfo(pcan_handle, txt);  
        if (!errno)  
            printf("CAN Bus test: driver version = %s\n", 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\n", szDevNode);  
            }  
    }  
    else  
        printf("CAN Bus test: can't open %s\n", 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.\n",  
               (__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("\n");  
}  
  
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("\nCAN Bus test: finished (%d).\n\n", 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);  
}

1.png





标签: ROSClubcanros_can

评论共0条评论

登录后再评论!

全部评论

目前没有评论