自动驾驶系列（十）编写电动车ROS节点（刹车）

一、硬件控制协议

       对于不同的设备，底层的通讯方式都不一致，因此需要根据具体硬件具体分析。本系统 采用了1个CAN盒子控制转向，1路DA控制油门，另外一路DA控制刹车。

1，转向协议

      CAN协议获取当前状态的帧ID是0x322,发送控制指令的ID是0x215。500kbps。

     一条十六进制控制码是 00 31 X1 X2 Y1 00 00 00。

     其中X1*256+X2=控制角度 Y1控制角速度。

2，油门协议

      串口转DA控制模块，设置波特率9600bps。

      一条十六进制控制码是 01 06 00 00 X1 X2 Y1 Y2。

     其中，X1*256+X2控制DA输出电压。空载测试大约 5km/h对应 01 99，10km/h对应 03 33。

     Y1和Y2分别是对前八位数据进行CRC校验后的输出结果。

3，刹车协议

     串口转DA控制模块，设置波特率9600bps。

     启动刹车：01 06 00 00 0c cd 4d 5f。

     取消刹车：01 06 00 00 00 00 89 ca。

二、ROS节点接收速度控制消息

     对于ROS节点的编写规则，可以参考本系列第二讲中推荐的ros基本教程。

    首先创建功能包，实现监听cmd_vel消息，命名为bjtucar：

cd ~/catkin_ws/src
catkin_create_pkg bjtucar std_msgs std_msgs roscpp

 创建节点订阅文件myCar.cpp

cd ~/catkin_ws/src/bjtucar/src/
gedit myCar.cpp

在文件中粘贴以下内容：

#include <ros/ros.h>
#include <geometry_msgs/Twist.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;
}
int main( int argc, char* argv[] )
{
    ros::init(argc, argv, "myCar" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/turtle1/cmd_vel", 1000, cmd_vel_callback);
    while( n.ok() )
    {
        ros::spin();
    }
    return 1;
}

在CMakeList.txt文件最后增加代码：

add_executable(bjtucar ./src/myCar.cpp)
target_link_libraries(bjtucar ${catkin_LIBRARIES})

重新编译

cd ~/catkin_ws
catkin_make

测试：打开4个终端，分别运行以下节点

启动内核：   roscore

启动仿真乌龟节点： rosrun turtlesim turtlesim_node

启动键盘控制节点： rosrun turtlesim turtle_teleop_key

启动myCar节点: rosrun bjtucar bjtucar

可以通过输入方向键，发送cmd_vel消息，myCar节点监听该消息后打印出来。

 新终端，rqt_graph查看当前节点状态

 三、ROS节点控制刹车

控制油门和刹车，都需要添加串口节点，我们先假设配置油门是串口1，刹车是串口2。首先按照一个串口添加。

3.1添加串口功能

查看串口设备，修改串口名称：ls /dev/tty*。  一般是ttyACM0 或者 ttyUSB0。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#include <serial/serial.h>
serial::Serial ser; //声明串口对象 1
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;
}
int main( int argc, char* argv[] )
{
    ros::init(argc,argv,"serial_port");
    //设置串口属性，并打开串口
    ser.setPort("/dev/ttyACM0");
    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 ");
        return -1;
    }
    //检测串口是否已经打开，并给出提示信息
    if(ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }

    ros::init(argc, argv, "myCar" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/turtle1/cmd_vel", 1000, cmd_vel_callback);
    while( n.ok() )
    {
        ros::spin();
    }
    return 1;
}

修改CMakeList.txt文件，添加了serial的包依赖。

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})

安装serial包，再次编译通过。

sudo apt-get install ros-melodic-serial
cd ~/catkin
catkin_make

启动内核节点：roscore

启动myCar节点: rosrun bjtucar bjtucar

修改USB权限： sudo chmod 666 /dev/ttyUSB0
再次运行，成功。

 3.2串口调试工具cutecom

　（1）终端下载命令：sudo apt-get install cutecom 

　（2）通过终端打开软件：sudo cutecom (需要root权限，否则无法打开串口）

 3.3输出控制指令

当键盘按下”↑“时，刹车踏板松开。当键盘按下”↓“时，刹车踩下。

修改myCar.cpp文件内容如下：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#include <serial/serial.h>
serial::Serial ser; //声明串口对象 1

//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};

//int doBrake[8]={1, 6, 0, 0, 12, 205, 77, 95};
//int noBrake[8]={1, 6, 0, 0, 0, 0, 137, 202};
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;

    if(vel_cmd.linear.x>0)
        ser.write(noBrake,8);
    else if(vel_cmd.linear.x<0)
        ser.write(doBrake,8);
    else;
}
int main( int argc, char* argv[] )
{
    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 ");
        return -1;
    }
    //检测串口是否已经打开，并给出提示信息
    if(ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }

    ros::init(argc, argv, "myCar" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/turtle1/cmd_vel", 1000, cmd_vel_callback);
    while( n.ok() )
    {
        ros::spin();
    }
    return 1;
}

重新编译

cd ~/catkin
catkin_make

测试：

1，用两个串口模块连接到电脑端。并查看串口号。ttyUSB0的输出用导线连接到ttyUSB1的输入。

 2，打开串口调试工具，监控串口ttyUSB1。

sudo cutecom

 3，依次启动节点

启动内核：   roscore

启动键盘控制节点： rosrun turtlesim turtle_teleop_key

启动myCar节点: rosrun bjtucar bjtucar

 

 在键盘输入的终端，分别输入方向按键，可以监控到串口出不同的刹车指令。