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");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 //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);//function declaration
int read_loop(HANDLE h,ros::Publisher pub,bool display_on,bool publish_on);//循环读取函数，可选择是否终端打印和topic发布
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); //异常事件（ctrl+c等）发生时的中断服务程序
void init(); //初始化事件signal相关操作，即指定中断服务函数
void publish_forcedata(TPCANRdMsg *mr,ros::Publisher force_pub); //以topic形式进行数据发布// 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);
}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;
}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, //接收到的数据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");
}
//打印接受到数据的时间戳，并将有用数据区传入print_message()函数
void print_message_ex(TPCANRdMsg *mr)
{
printf("%u.%3u ", mr->dwTime, mr->wUsec);
print_message(&mr->Msg);
}int32 id
int16[8] datafind_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
)......
add_message_files(
FILES
Num.msg
ForceData.msg
) ros::NodeHandle force_handle;
ros::Publisher force_pub=force_handle.advertise("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();
} 该篇的完整代码如下所示，当然宝宝的qt还是不能输入中文，凑合看^_^。。。。。。。
#include
#include
#include
#include
#include
#include
#include
#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("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);
}
