ROS CAN總線設備接入（二）can總線數據提取和以ros topic形式發佈

原創 信雪神话 2020-07-04 15:40

【簡介】基於前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\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()");
        }

以上程序片段即完成了打開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文件內容如下： 

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  :[email protected];[email protected]
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);
}
實際實驗的結果給個圖咯！



發表評論
所有評論
還沒有人評論，想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章

ROS CAN總線設備接入（一）Linux動態庫的顯式調用

前提： （1），如果在libpcan安裝正常的話，那麼可以用以下命令查找到libpcan.so ls /usr/lib/libpcan* 查找到方可進行api載入。 （2），確保CMakeList.txt 當前目標文件已加入add_exe

信雪神话 2020-07-04 15:00:22

ROS多進程回調的實現 ROS Using different ros::CallbackQueue

總體上分爲一下步驟： 1 用 ros::CallbackQueue定義需要的回調隊列； ros::CallbackQueue state_callback_queue; 2 將回調隊列初始化進subscriber中。例如下面所示： r

信雪神话 2020-07-04 15:00:22

ROS node調用service: ros::service::call/client.call()

信雪神话 2020-02-25 10:17:16

Ubuntu14.04系統下安裝ros_barrett_package

信雪神话 2020-02-25 10:17:16

ros::NodeHandle成員介紹

信雪神话 2020-02-25 10:17:16

ros talker listener主要基本api 介紹

信雪神话 2020-02-25 10:17:16

ROS .so載入undefined reference to `dlopen'問題

信雪神话 2020-02-25 10:17:16

ROS CAN總線設備接入（一）Linux動態庫的顯式調用

前提： （1），如果在libpcan安裝正常的話，那麼可以用以下命令查找到libpcan.so ls /usr/lib/libpcan* 查找到方可進行api載入。 （2），確保CMakeList.txt 當前目標文件已加入add_exe

信雪神话 2020-07-04 15:00:22

ROS多進程回調的實現 ROS Using different ros::CallbackQueue

總體上分爲一下步驟： 1 用 ros::CallbackQueue定義需要的回調隊列； ros::CallbackQueue state_callback_queue; 2 將回調隊列初始化進subscriber中。例如下面所示： r

信雪神话 2020-07-04 15:00:22

ROS的學習（十五）驗證publisher和subscriber

     在前面的兩篇博客中我們用C++在ROS中創建了一個發佈者和接收者，並使用catkin_make構建了新的節點，下面就需要驗證一下，我們寫的是否正確。      首先運行roscore roscore      打開一個新的終端

BigHerbert 2020-06-24 14:08:39

ROS的學習（十六）用C++寫一個簡單的服務器(service)和客戶端(client)

      我們將創建一個服務器節點add_two_ints_server，它將會收到兩個整數，並且返回它們的和。切換目錄到之前建立的beginner_tutorials包下： cd ~/catkin_ws/src/beginner_tu

BigHerbert 2020-06-24 14:08:37

ROS的學習（二十）rosserial中的Publisher和Subscriber中的編程步驟

     1.在一個話題上發佈消息。      可以使用ros::Publisher類創建一個句柄來在一個話題上發佈消息，應該首先創建一個publisher將要使用的消息，然後在setup函數中調用advertise。整個程序的結構如下：

BigHerbert 2020-06-24 14:08:37

ROS node調用service: ros::service::call/client.call()

信雪神话 2020-02-25 10:17:16

Ubuntu14.04系統下安裝ros_barrett_package

信雪神话 2020-02-25 10:17:16

ros::NodeHandle成員介紹

信雪神话 2020-02-25 10:17:16