1. 测试官方样例
我这里用的是CANalyst-II分析仪(创芯科技) Linux版
我的系统是:Ubuntu18.04.6 LTS
我的ROS版本:melodic
![在这里插入图片描述](https://img-blog.csdnimg.cn/f8eac514bed246e3a19824dac12eaf80.png#pic_center)
这个CAN设备是不用装驱动的,直接在终端输入lsusb就能查到CAN设备ID:04d8:0053 Microchip Technology, Inc.
lsusb
![在这里插入图片描述](https://img-blog.csdnimg.cn/abb4a3e8c53b4f8e873af4a2f401cd36.png#pic_center)
官方提供的Linux资料包里面就有二次开发样例
最重要的是libcontrolcan.so是动态链接库,调用库中的函数需要用到controlcan.h
![在这里插入图片描述](https://img-blog.csdnimg.cn/623f9319f10e4fba8ca0d07797eb4b7c.png)
将这4个文件复制到一个不含中文路径的目录下,我这里是复制到can_test这个文件夹里
![在这里插入图片描述](https://img-blog.csdnimg.cn/ec0873faa1ac4a15bf5190f33f3beed0.png#pic_center)
修改main.cpp的内容:读取CAN1通道的数据(遥控器发给CAN1),并打印到终端
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <pthread.h>
#include "controlcan.h"
#include <ctime>
#include <cstdlib>
#include "unistd.h"
main()
{
printf(">>this is mytestdemo !\r\n");
if(VCI_OpenDevice(VCI_USBCAN2,0,0)==1)
{
printf(">>open deivce success!\n");
}else
{
printf(">>open deivce error!\n");
exit(1);
}
VCI_INIT_CONFIG config;
config.AccCode = 0x80000000;
config.AccMask = 0xFFFFFFFF;
config.Filter = 2;
config.Timing0 = 0x01;
config.Timing1 = 0x1C;
config.Mode = 0;
if(VCI_InitCAN(VCI_USBCAN2,0,0,&config)!=1)
{
printf(">>Init CAN1 error\n");
VCI_CloseDevice(VCI_USBCAN2,0);
}
if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
{
printf(">>Start CAN1 error\n");
VCI_CloseDevice(VCI_USBCAN2,0);
}
VCI_CAN_OBJ rec[50];
int reclen=0;
int i,j;
while(1)
{
if((reclen = VCI_Receive(VCI_USBCAN2,0,0,rec,50,200)) >= 0)
{
for(i=0; i<reclen; i++)
{
printf("data:0x");
for( j= 0; j < rec[i].DataLen; j++ )
{
printf(" %02X", rec[i].Data[j]);
}
printf("\n");
}
}
}
}
然后将libcontrolcan.so动态链接库的绝对路径添加到Makefile中
就可以将这个main.cpp编译成可执行文件test2333,可执行文件名可以自定义
all:
g++ -o test2333 main.cpp /home/yao/My_Ros_WorkSpace/can_test/libcontrolcan.so -lpthread
clean:
rm -f *.o
最后在终端中编译和执行即可
lsusb
cd /home/yao/My_Ros_WorkSpace/can_test
rm test2333
make clean && make
sudo ./test2333
![在这里插入图片描述](https://img-blog.csdnimg.cn/677497b63b73420591be51cc6450f449.png#pic_center)
遥控器↑:02 01 00 00 00 00 00 00
遥控器↓:02 02 00 00 00 00 00 00
遥控器←:02 04 00 00 00 00 00 00
遥控器→:02 08 00 00 00 00 00 00
OK,到这里说明我们的设备和程序都是可以正常工作的,没有问题!
特别注意:CAN1口旁边的2个120Ω上拉电阻,必须保证至少一个
是拨到ON档
否则没办法接收CAN1口发送过来的数据
![在这里插入图片描述](https://img-blog.csdnimg.cn/bd4da95255544839951409ba40ac3579.png#pic_center)
2. 移植到ROS
接下来,将这部分代码移植到ROS系统下
- 首先创建工作空间
用VScode打开工作空间,需要进行配置:看我前面的文章
cd ~/My_Ros_WorkSpace
mkdir can_workspace
cd can_workspace
mkdir src
cd src
catkin_init_workspace
cd ..
catkin_make
catkin_make install
code .
-
然后创建功能包
选中src,右键Create Catkin Package
录入功能包名字"can_pkg",回车
录入依赖"roscpp rospy std_msgs",再回车
-
创建节点:读取CAN1的数据
选中can_pkg→src,右键新建一个发布者的实现代码mycan_pub.cpp
修改 .vscode/c_cpp_properties.json,设置 "cppStandard"为 “c++17”并保存,否则会报错
重中之重:
因为要用到libcontrolcan.so和controlcan.h,所以要将这两个文件添加到这个工作空间下
controlcan.h:
-
先放到can_workspace/src/can_pkg/include/can_pkg目录下
![在这里插入图片描述](https://img-blog.csdnimg.cn/167daae98b75439ab3f2e1039052cc3d.png#pic_center)
-
然后配置includePath路径,在c_cpp_properties.json文件中添加controlcan.h所在的绝对路径 “/home/yao/My_Ros_WorkSpace/can_workspace/src/can_pkg/include/**”
![在这里插入图片描述](https://img-blog.csdnimg.cn/347f501c66cb4798b3632afd9471497f.png#pic_center)
-
然后就可以在mycan_pub.cpp中调用这个头文件了,注意引用头文件的写法,需要将头文件的相对路径加进来,否则会报错,应该这样写:#include “can_pkg/controlcan.h”
-
这样就可以调用controlcan.h中声明的函数编写代码了:
#include <ros/ros.h>
#include <std_msgs/String.h>
#include "can_pkg/controlcan.h"
using namespace std;
int main(int argc, char** argv)
{
ros::init(argc, argv,"can_publisher");
if(VCI_OpenDevice(VCI_USBCAN2,0,0)==1)
{
ROS_INFO_STREAM(">>open deivce success!");
}else
{
ROS_INFO_STREAM(">>open deivce error!");
exit(1);
}
VCI_INIT_CONFIG config;
config.AccCode = 0x80000000;
config.AccMask = 0xFFFFFFFF;
config.Filter = 2;
config.Timing0 = 0x01;
config.Timing1 = 0x1C;
config.Mode = 0;
if(VCI_InitCAN(VCI_USBCAN2,0,0,&config)!=1)
{
ROS_INFO_STREAM(">>Init CAN1 error");
VCI_CloseDevice(VCI_USBCAN2,0);
}
if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
{
ROS_INFO_STREAM(">>Start CAN1 error");
VCI_CloseDevice(VCI_USBCAN2,0);
}
VCI_CAN_OBJ rec[50];
int reclen=0;
int i,j;
while(1)
{
if((reclen = VCI_Receive(VCI_USBCAN2,0,0,rec,50,200)) >= 0)
{
for(i=0; i < reclen; i++)
{
printf("data:0x");
for( j= 0; j < rec[i].DataLen; j++ )
{
printf(" %02X", rec[i].Data[j]);
}
printf("\n");
}
}
}
return 0;
}
libcontrolcan.so:
- 在功能包can_pkg目录下创建一个lib文件夹
- 将libcontrolcan.so复制到刚刚新建的lib文件夹下面
![在这里插入图片描述](https://img-blog.csdnimg.cn/4117146eac8e46b6bbfabbddb45a7405.png#pic_center)
- 解除#include的注释:可以让编译器去include下找到can_pkg/controlcan.h
include_directories(
include
${catkin_INCLUDE_DIRS}
)
- 指定动态链接库libcontrolcan.so的访问路径
link_directories(
lib
${catkin_LIB_DIRS}
)
- 最后就是指定生成可执行文件和链接库
add_executable(mycan_pub src/mycan_pub.cpp)
target_link_libraries(mycan_pub
${catkin_LIBRARIES}
controlcan
)
- 编译执行
Ctrl + S保存,Ctrl + Shift +B编译
- 终端1
roscore
- 终端2
source devel/setup.bash
rosrun can_pkg mycan_pub
报错:error setting config #1: could not set config 1: Operation not permitted
原因:这是因为Linux系统下将涉及到usb底层驱动的调用,运行时,一定要加sudo获取权限运行,否则USB
设备没有权限操作,比如之前运行那个用gcc编译的可执行文件test2333时,我也是加了sudo的
sudo ./test2333
但是用rosrun can_pkg mycan_pub启动节点的时候,貌似没办法加sudo,所以需要配置USB权限:
方法一:
- 创建一个新的 udev 规则,名称取为:99-myusb.rules
注意:数字 99 最好不要改动,否则可能设置失败,而且要加 sudo
sudo vi /etc/udev/rules.d/99-myusb.rules
- 把以下两行代码复制到新建的 99-myusb.rules 文件中
注意:按键盘上 Insert 键切换到“代替”输入模式,然后粘贴
ACTION=="add",SUBSYSTEMS=="usb", ATTRS{idVendor}=="04d8", ATTRS{idProduct}=="0053",
GROUP="users", MODE="0777"
- 按一次“Esc”键
- 直接输入“:wq”回车,即保存退出
- 插拔一下 USBCAN 设备或重启一下电脑
- 就可以不加 sudo 权限运行程序了,或者ROS下用rosrun启动节点也不会报错了
方法二:
cd /etc/udev/rules.d
sudo touch 99-myusb.rules
sudo gedit 99-myusb.rules
ACTION=="add",SUBSYSTEMS=="usb", ATTRS{idVendor}=="04d8", ATTRS{idProduct}=="0053",
GROUP="users", MODE="0777"
- 升级节点:读取完CAN1的数据,根据遥控器发过来的数据,将控制指令输出到CAN2,以便后续的控制操作
- 硬件连接:CANalyst-II的CAN1连接遥控器、CAN2可以再外接一个CANalyst-II并连到Windows系统,这样就可以将CAN2输出的控制指令在Windows系统上的USB_CAN TOOL调试软件上显示出来。注意:所有CAN口的上拉电阻都需要拨到ON档
- 代码如下:
#include <ros/ros.h>
#include <std_msgs/String.h>
#include "can_pkg/controlcan.h"
using namespace std;
bool Init_CAN1(int nDeviceType, int nDeviceInd, int nCANInd, VCI_INIT_CONFIG config) {
VCI_ResetCAN(nDeviceType, nDeviceInd, nCANInd);
if (VCI_InitCAN(nDeviceType, nDeviceInd, nCANInd, &config) != 1)
{
VCI_CloseDevice(nDeviceType, nDeviceInd);
ROS_INFO_STREAM(">>Init CAN1 error");
return(0);
}
VCI_ClearBuffer(nDeviceType, nDeviceInd, nCANInd);
if (VCI_StartCAN(nDeviceType, nDeviceInd, nCANInd) != 1)
{
VCI_CloseDevice(nDeviceType, nDeviceInd);
ROS_INFO_STREAM(">>Start CAN1 error");
return(0);
}
else
{
ROS_INFO_STREAM(">>Start CAN1 success");
return(1);
}
}
bool Init_CAN2(int nDeviceType, int nDeviceInd, int nCANInd, VCI_INIT_CONFIG config) {
VCI_ResetCAN(nDeviceType, nDeviceInd, nCANInd);
if (VCI_InitCAN(nDeviceType, nDeviceInd, nCANInd, &config) != 1)
{
VCI_CloseDevice(nDeviceType, nDeviceInd);
ROS_INFO_STREAM(">>Init CAN2 error");
return(0);
}
VCI_ClearBuffer(nDeviceType, nDeviceInd, nCANInd);
if (VCI_StartCAN(nDeviceType, nDeviceInd, nCANInd) != 1)
{
VCI_CloseDevice(nDeviceType, nDeviceInd);
ROS_INFO_STREAM(">>Start CAN2 error");
return(0);
}
else
{
ROS_INFO_STREAM(">>Start CAN2 success");
return(1);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv,"can_publisher");
const unsigned char a0[] = { 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 };
const unsigned char a1[] = { 0xF4,0x01,0x00,0x00,0x00,0x00,0x00,0x00 };
const unsigned char a2[] = { 0x00,0x00,0xF4,0x01,0x00,0x00,0x00,0x00 };
const unsigned char a3[] = { 0x00,0x00,0x00,0x00,0xF4,0x01,0x00,0x00 };
const unsigned char a4[] = { 0x00,0x00,0x00,0x00,0x00,0x00,0xF4,0x01 };
int nDeviceType = 4;
int nDeviceInd = 0;
int nCANInd0 = 0;
int nCANInd1 = 1;
if(VCI_OpenDevice(nDeviceType,nDeviceInd,0)==1)
{
ROS_INFO_STREAM(">>open deivce success!");
}else
{
ROS_INFO_STREAM(">>open deivce error!");
exit(1);
}
VCI_INIT_CONFIG config;
config.AccCode = 0x80000000;
config.AccMask = 0xFFFFFFFF;
config.Filter = 2;
config.Timing0 = 0x01;
config.Timing1 = 0x1C;
config.Mode = 0;
int num = 50;
VCI_CAN_OBJ rec[num];
int reclen = 0;
int i,j,k, order;
VCI_CAN_OBJ send[1];
send[0].ID = 0;
send[0].SendType = 0;
send[0].RemoteFlag = 0;
send[0].ExternFlag = 0;
send[0].DataLen = 8;
if (Init_CAN1(nDeviceType, nDeviceInd, nCANInd0, config) && Init_CAN2(nDeviceType, nDeviceInd, nCANInd1, config))
{
while(1)
{
usleep(20000);
if((reclen = VCI_Receive(nDeviceType,nDeviceInd,nCANInd0,rec,num,200)) >= 0)
{
for(i=0; i < reclen; i++)
{
order = (int)rec[i].Data[1] + (int)rec[i].Data[2];
printf("data:0x");
for( j= 0; j < rec[i].DataLen; j++ )
{
printf(" %02X", rec[i].Data[j]);
}
printf("\n");
switch (order)
{
case 0:
{
for (k = 0; k < 8; k++)
{
send[0].Data[k] = a0[k];
}
VCI_Transmit(nDeviceType, nDeviceInd, nCANInd1, send, 1);
break;
}
case 1:
{
for (k = 0; k < 8; k++)
{
send[0].Data[k] = a1[k];
}
VCI_Transmit(nDeviceType, nDeviceInd, nCANInd1, send, 1);
break;
}
case 2:
{
for (k = 0; k < 8; k++)
{
send[0].Data[k] = a2[k];
}
VCI_Transmit(nDeviceType, nDeviceInd, nCANInd1, send, 1);
break;
}
case 4:
{
for (k = 0; k < 8; k++)
{
send[0].Data[k] = a3[k];
}
VCI_Transmit(nDeviceType, nDeviceInd, nCANInd1, send, 1);
break;
}
case 8:
{
for (k = 0; k < 8; k++)
{
send[0].Data[k] = a4[k];
}
VCI_Transmit(nDeviceType, nDeviceInd, nCANInd1, send, 1);
break;
}
default:
{
for (k = 0; k < 8; k++)
{
send[0].Data[k] = a0[k];
}
VCI_Transmit(nDeviceType, nDeviceInd, nCANInd1, send, 1);
break;
}
}
}
}
}
}
return 0;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)