自动驾驶系列（十一）编写电动车ROS节点（转向）
助力转向机通过CAN总线控制，因此，需要利用CAN盒子来进行。本项目选用的时创芯科技的CAN分析仪。首先需要完成盒子的驱动安装。创芯科技的盒子需要注意linux和windows版本硬件不兼容，广成科技的盒子两个平台都兼容。
一、驱动文件测试
驱动文件main.cpp内容：
//样例只是提供一个简单的调用so库的方法供参考，程序接收，与发送函数设置在两个线程中，并且线程没有同步。
//现实中客户编程中，发送与接收函数不能同时调用（不支持多线程），如果在多线程中，一定需要互锁。需要客户自行完善代码。
#include
#include
#include
#include
#include
#include "controlcan.h"
#include
#include
#include "unistd.h"
VCI_BOARD_INFO pInfo;//用来获取设备信息。
int count=0;//数据列表中，用来存储列表序号。
VCI_BOARD_INFO pInfo1 [50];
int num=0;
void *receive_func(void* param) //接收线程。
{
int reclen=0;
VCI_CAN_OBJ rec[3000];//接收缓存，设为3000为佳。
int i,j;
int *run=(int*)param;//线程启动，退出控制。
int ind=0;
while((*run)&0x0f)
{
if((reclen=VCI_Receive(VCI_USBCAN2,0,ind,rec,3000,100))>0)//调用接收函数，如果有数据，进行数据处理显示。
{
for(j=0;j>this is hello !\r\n");//指示程序已运行
num=VCI_FindUsbDevice2(pInfo1);
printf(">>USBCAN DEVICE NUM:");printf("%d", num);printf(" PCS");printf("\n");
for(int i=0;i>Get VCI_ReadBoardInfo success!\n");
printf(">>Serial_Num:%c", pInfo1[i].str_Serial_Num[0]);
printf("%c", pInfo1[i].str_Serial_Num[1]);
printf("%c", pInfo1[i].str_Serial_Num[2]);
printf("%c", pInfo1[i].str_Serial_Num[3]);
printf("%c", pInfo1[i].str_Serial_Num[4]);
printf("%c", pInfo1[i].str_Serial_Num[5]);
printf("%c", pInfo1[i].str_Serial_Num[6]);
printf("%c", pInfo1[i].str_Serial_Num[7]);
printf("%c", pInfo1[i].str_Serial_Num[8]);
printf("%c", pInfo1[i].str_Serial_Num[9]);
printf("%c", pInfo1[i].str_Serial_Num[10]);
printf("%c", pInfo1[i].str_Serial_Num[11]);
printf("%c", pInfo1[i].str_Serial_Num[12]);
printf("%c", pInfo1[i].str_Serial_Num[13]);
printf("%c", pInfo1[i].str_Serial_Num[14]);
printf("%c", pInfo1[i].str_Serial_Num[15]);
printf("%c", pInfo1[i].str_Serial_Num[16]);
printf("%c", pInfo1[i].str_Serial_Num[17]);
printf("%c", pInfo1[i].str_Serial_Num[18]);
printf("%c", pInfo1[i].str_Serial_Num[19]);printf("\n");
printf(">>hw_Type:%c", pInfo1[i].str_hw_Type[0]);
printf("%c", pInfo1[i].str_hw_Type[1]);
printf("%c", pInfo1[i].str_hw_Type[2]);
printf("%c", pInfo1[i].str_hw_Type[3]);
printf("%c", pInfo1[i].str_hw_Type[4]);
printf("%c", pInfo1[i].str_hw_Type[5]);
printf("%c", pInfo1[i].str_hw_Type[6]);
printf("%c", pInfo1[i].str_hw_Type[7]);
printf("%c", pInfo1[i].str_hw_Type[8]);
printf("%c", pInfo1[i].str_hw_Type[9]);printf("\n");
printf(">>Firmware Version:V");
printf("%x", (pInfo1[i].fw_Version&0xF00)>>8);
printf(".");
printf("%x", (pInfo1[i].fw_Version&0xF0)>>4);
printf("%x", pInfo1[i].fw_Version&0xF);
printf("\n");
}
printf(">>\n");
printf(">>\n");
printf(">>\n");
if(VCI_OpenDevice(VCI_USBCAN2,0,0)==1)//打开设备
{
printf(">>open deivce success!\n");//打开设备成功
}else
{
printf(">>open deivce error!\n");
exit(1);
}
if(VCI_ReadBoardInfo(VCI_USBCAN2,0,&pInfo)==1)//读取设备序列号、版本等信息。
{
printf(">>Get VCI_ReadBoardInfo success!\n");
//printf(" %08X", pInfo.hw_Version);printf("\n");
//printf(" %08X", pInfo.fw_Version);printf("\n");
//printf(" %08X", pInfo.dr_Version);printf("\n");
//printf(" %08X", pInfo.in_Version);printf("\n");
//printf(" %08X", pInfo.irq_Num);printf("\n");
//printf(" %08X", pInfo.can_Num);printf("\n");
printf(">>Serial_Num:%c", pInfo.str_Serial_Num[0]);
printf("%c", pInfo.str_Serial_Num[1]);
printf("%c", pInfo.str_Serial_Num[2]);
printf("%c", pInfo.str_Serial_Num[3]);
printf("%c", pInfo.str_Serial_Num[4]);
printf("%c", pInfo.str_Serial_Num[5]);
printf("%c", pInfo.str_Serial_Num[6]);
printf("%c", pInfo.str_Serial_Num[7]);
printf("%c", pInfo.str_Serial_Num[8]);
printf("%c", pInfo.str_Serial_Num[9]);
printf("%c", pInfo.str_Serial_Num[10]);
printf("%c", pInfo.str_Serial_Num[11]);
printf("%c", pInfo.str_Serial_Num[12]);
printf("%c", pInfo.str_Serial_Num[13]);
printf("%c", pInfo.str_Serial_Num[14]);
printf("%c", pInfo.str_Serial_Num[15]);
printf("%c", pInfo.str_Serial_Num[16]);
printf("%c", pInfo.str_Serial_Num[17]);
printf("%c", pInfo.str_Serial_Num[18]);
printf("%c", pInfo.str_Serial_Num[19]);printf("\n");
printf(">>hw_Type:%c", pInfo.str_hw_Type[0]);
printf("%c", pInfo.str_hw_Type[1]);
printf("%c", pInfo.str_hw_Type[2]);
printf("%c", pInfo.str_hw_Type[3]);
printf("%c", pInfo.str_hw_Type[4]);
printf("%c", pInfo.str_hw_Type[5]);
printf("%c", pInfo.str_hw_Type[6]);
printf("%c", pInfo.str_hw_Type[7]);
printf("%c", pInfo.str_hw_Type[8]);
printf("%c", pInfo.str_hw_Type[9]);printf("\n");
printf(">>Firmware Version:V");
printf("%x", (pInfo.fw_Version&0xF00)>>8);
printf(".");
printf("%x", (pInfo.fw_Version&0xF0)>>4);
printf("%x", pInfo.fw_Version&0xF);
printf("\n");
}else
{
printf(">>Get VCI_ReadBoardInfo error!\n");
exit(1);
}
//初始化参数，严格参数二次开发函数库说明书。
VCI_INIT_CONFIG config;
config.AccCode=0;
config.AccMask=0xFFFFFFFF;
config.Filter=1;//接收所有帧
config.Timing0=0x03;/*波特率125 Kbps 0x03 0x1C*/
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);
}
if(VCI_InitCAN(VCI_USBCAN2,0,1,&config)!=1)
{
printf(">>Init can2 error\n");
VCI_CloseDevice(VCI_USBCAN2,0);
}
if(VCI_StartCAN(VCI_USBCAN2,0,1)!=1)
{
printf(">>Start can2 error\n");
VCI_CloseDevice(VCI_USBCAN2,0);
}
//需要发送的帧，结构体设置
VCI_CAN_OBJ send[1];
send[0].ID=0;
send[0].SendType=0;
send[0].RemoteFlag=0;
send[0].ExternFlag=1;
send[0].DataLen=8;
int i=0;
for(i = 0; i < send[0].DataLen; i++)
{
send[0].Data[i] = i;
}
int m_run0=1;
pthread_t threadid;
int ret;
ret=pthread_create(&threadid,NULL,receive_func,&m_run0);
int times = 5;
while(times--)
{
if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
{
printf("Index:%04d ",count);count++;
printf("CAN1 TX ID:0x%08X",send[0].ID);
if(send[0].ExternFlag==0) printf(" Standard ");
if(send[0].ExternFlag==1) printf(" Extend ");
if(send[0].RemoteFlag==0) printf(" Data ");
if(send[0].RemoteFlag==1) printf(" Remote ");
printf("DLC:0x%02X",send[0].DataLen);
printf(" data:0x");
for(i=0;i 打开给定的驱动例程文件夹，打开makefile文件，修改内容如下：
all:
g++ -o hello_cpp main.cpp /home/bjtu/Desktop/controlcan/libcontrolcan.so -lpthread
clean:
rm -f *.o hello在文件路径下，新建终端
make生成新的可执行文件后，运行，可以看到执行成功。
二、程序编写
修改bjtucar代码包中的主文件，含油门控制代码，加入转向控制程序。
//----serial---------
#include
#include
#include
//----can------------
#include
#include
#include
#include
#include
#include "controlcan.h"
#include
#include
#include "unistd.h"
//----serial---------
serial::Serial ser; //声明串口对象 1
serial::Serial ser1; //声明串口对象 1
//----can------------
VCI_BOARD_INFO pInfo;//用来获取设备信息。
VCI_BOARD_INFO pInfo1 [50];
unsigned char turnLeft[8]={0x00, 0x31, 0x01, 0x90, 0x16, 0x00, 0x00, 0x00};
unsigned char turnRight[8]={0x00, 0x31, 0x3A, 0x98, 0x16, 0x00, 0x00, 0x00};
unsigned char turnMidldle[8]={0x00, 0x31, 0x1e, 0x78, 0x16, 0x00, 0x00, 0x00};
//----serial---------
//static unsigned char doBrake[8]; //分配静态存储空间
unsigned char doBrake[8]={0x01, 0x06, 0x00, 0x00, 0x0c, 0xcd, 0x4d, 0x5f};
unsigned char noBrake[8]={0x01, 0x06, 0x00, 0x00, 0x00, 0x00, 0x89, 0xca};
unsigned char mySpeed0[8]={0x01, 0x06, 0x00, 0x00, 0x00, 0x00, 0x89, 0xca}; //0km/h
unsigned char mySpeed1[8]={0x01, 0x06, 0x00, 0x00, 0x00, 0x51, 0x48, 0x36}; //1km/h
unsigned char mySpeed2[8]={0x01, 0x06, 0x00, 0x00, 0x00, 0xa3, 0xc9, 0xb3}; //2km/h
unsigned char mySpeed3[8]={0x01, 0x06, 0x00, 0x00, 0x01, 0x99, 0x48, 0x30}; //5km/h
unsigned char mySpeed4[8]={0x01, 0x06, 0x00, 0x00, 0x03, 0x33, 0xc9, 0x2f}; //10km/h
unsigned char mySpeed5[8]={0x01, 0x06, 0x00, 0x00, 0x06, 0x66, 0x0a, 0x40}; //20km/h
unsigned char mySpeed6[8]={0x01, 0x06, 0x00, 0x00, 0x09, 0x99, 0x4f, 0xf0}; //30km/h
void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd)
{
ROS_INFO("I heard: [%f]", vel_cmd.linear.y);
ROS_INFO("I heard: [%f]", vel_cmd.linear.x);
std::cout << "Twist Received " << std::endl;
VCI_CAN_OBJ send[1];
send[0].ID=0x215;
send[0].SendType=0;
send[0].RemoteFlag=0;
send[0].ExternFlag=0;
send[0].DataLen=8;
if(vel_cmd.linear.x>0)
{
ser.write(noBrake,8);
ser1.write(mySpeed5,8);
}
else if(vel_cmd.linear.x<0)
{
ser.write(doBrake,8);
ser1.write(mySpeed0,8);
}
else;
if(vel_cmd.linear.y>0)
{
int i=0;
for(i = 0; i < send[0].DataLen; i++)
{
send[0].Data[i] = turnRight[i];
}
if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
{
ROS_INFO_STREAM(">>send turn right success !\n");
}
}
else if(vel_cmd.linear.y<0)
{
int i=0;
for(i = 0; i < send[0].DataLen; i++)
{
send[0].Data[i] = turnLeft[i];
}
if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
{
ROS_INFO_STREAM(">>send turn left success !\n");
}
}
else
{
int i=0;
for(i = 0; i < send[0].DataLen; i++)
{
send[0].Data[i] = turnMidldle[i];
}
if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
{
ROS_INFO_STREAM(">>send turn Midldle success !\n");
}
}
}
int main( int argc, char* argv[] )
{
//----serial---------
ros::init(argc,argv,"serial_port");
//设置串口属性，并打开串口
ser.setPort("/dev/ttyUSB0");
ser.setBaudrate(9600);
serial::Timeout to = serial::Timeout::simpleTimeout(100);
ser.setTimeout(to);
try
{
ser.open();
}
catch (serial::IOException & e)
{
ROS_ERROR_STREAM("Unable to open port ttyUSB0");
return -1;
}
//检测串口是否已经打开，并给出提示信息
if(ser.isOpen())
{
ROS_INFO_STREAM("Serial Port0 initialized");
}
else
{
return -1;
}
ser1.setPort("/dev/ttyUSB0");
ser1.setBaudrate(9600);
serial::Timeout to1 = serial::Timeout::simpleTimeout(100);
ser1.setTimeout(to1);
try
{
ser1.open();
}
catch (serial::IOException & e)
{
ROS_ERROR_STREAM("Unable to open port ttyUSB1");
return -1;
}
//检测串口是否已经打开，并给出提示信息
if(ser1.isOpen())
{
ROS_INFO_STREAM("Serial Port1 initialized");
}
else
{
return -1;
}
//----can------------
ROS_INFO_STREAM(">>this is hello !\r\n");//指示程序已运行
if(VCI_OpenDevice(VCI_USBCAN2,0,0)==1)//打开设备
{
ROS_INFO_STREAM(">>open deivce success!\n");//打开设备成功
}else
{
ROS_ERROR_STREAM(">>open deivce error!\n");
exit(1);
}
if(VCI_ReadBoardInfo(VCI_USBCAN2,0,&pInfo)==1)//读取设备序列号、版本等信息。
{
ROS_INFO_STREAM(">>Get VCI_ReadBoardInfo success!\n");
}else
{
ROS_ERROR_STREAM(">>Get VCI_ReadBoardInfo error!\n");
exit(1);
}
//初始化参数，严格参数二次开发函数库说明书。
VCI_INIT_CONFIG config;
config.AccCode=0;
config.AccMask=0xFFFFFFFF;
config.Filter=1;//接收所有帧
//500 kbps --- 00 1C
config.Timing0=0x00;
config.Timing1=0x1C;
config.Mode=0;//正常模式
if(VCI_InitCAN(VCI_USBCAN2,0,0,&config)!=1)
{
ROS_INFO_STREAM(">>Init CAN1 error\n");
VCI_CloseDevice(VCI_USBCAN2,0);
}
if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
{
ROS_ERROR_STREAM(">>Start CAN1 error\n");
VCI_CloseDevice(VCI_USBCAN2,0);
}
//需要发送的帧，结构体设置
ros::init(argc, argv, "myCar" );
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/turtle1/cmd_vel", 1000, cmd_vel_callback);
while( ros::ok() )
{
ros::spin();
}
return 1;
}
在myCar.cpp文件夹下，添加h文件和so文件，使程序能找到CAN的函数。
修改CmakeLists.txt:
cmake_minimum_required(VERSION 3.0.2)
project(bjtucar)
find_package(catkin REQUIRED COMPONENTS
serial
roscpp
std_msgs
std_msgs
)
catkin_package(
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(bjtucar ./src/myCar.cpp)
target_link_libraries(bjtucar ${catkin_LIBRARIES} controlcan)
然后编译:
catkin_make三、实验验证
插入两个串口设备，分别保证使用ttyUSB0，ttyUSB1，插入CAN盒子。
修改USB设备权限
sudo vi /etc/udev/rules.d/99-myusb.rules
在其中粘贴以下内容：
ACTION=="add",SUBSYSTEMS=="usb", ATTRS{idVendor}=="04d8", ATTRS{idProduct}=="0053", GROUP="users", MODE="0777" 执行rosrun
#新终端
roscore
#新终端
rosrun bjtucar bjtucar
#新终端
rosrun turtlesim turtle_teleop_key操作方向键可以看到不同的输出打印信息：
