Nvidia Jetson AGX Orin使用CAN与底盘通信（ROS C++ 驱动）

---

一、Nvidia Jetson AGX Orin使用CAN通信

参考：https://blog.csdn.net/vonct/article/details/129055892，使用杜邦线将Nvidia Jetson AGX Orin的CAN口（底盘CANL和CANH连接对应Orin的CANL和CANH，具体底盘CANL和CANH接口可能要看Nvidia Jetson AGX Orin说明书）接出来。

1.1 CAN使能配置修改GPIO口功能

由于默认的CAN引脚不是配置为CAN，因此需要修改4个寄存器的值。具体可以从文档看到：

以Orin为例，图中Addr就是寄存器地址，value就是需要写入的值
（1）使用busybox修改寄存器的值

sudo apt-get install busybox
sudo busybox devmem 0x0c303018 w 0xc458
sudo busybox devmem 0x0c303010 w 0xc400
sudo busybox devmem 0x0c303008 w 0xc458
sudo busybox devmem 0x0c303000 w 0xc400

（2）挂载CAN内核

sudo modprobe can
sudo modprobe can_raw
sudo modprobe mttcan

（3）CAN属性设置
例如将CAN0波特率设置成500k，注意，配置前需要先关闭can

sudo ip link set down can0
sudo ip link set can0 type can bitrate 500000
sudo ip link set up can0

将上诉代码写进shell文件，每次使用之前要运行：

sudo busybox devmem 0x0c303018 w 0xc458
sudo busybox devmem 0x0c303010 w 0xc400
sudo busybox devmem 0x0c303008 w 0xc458
sudo busybox devmem 0x0c303000 w 0xc400
sudo modprobe can
sudo modprobe can_raw
sudo modprobe mttcan
sudo ip link set down can0
sudo ip link set can0 type can bitrate 500000
sudo ip link set up can0

1.2 can收发测试

用can-utils进行简单的can收发测试，后续将用SocketCan结合ROS。

sudo apt-get install can-utils
cansend can0 0E2#05.00.00.00.00.00.00.00  #cansend can0/1 [can_id]#[八字节数据]
cangen -v can0  #随机发送
candump can0 #接受can帧

二、通过CAN协议编写CAN的SocketCan ROS1驱动程序

2.1 通讯协议

（1）小车反馈的整车数据

（2）线控指令


不同于UDP，类似于串口通信，可以参考我串口通信的文章
代码写好之后遇到的问题：
（1）接收数据的时候反馈慢
解决：去掉代码中的 spin频率控制;
（2）速度控制时无法换档
解决：每次发速度之前，发送速度指令清零；
（3）节点关闭时速度仍在
解决：在析构函数中发送退出线控指令；
（4）将Autoware参数（TwistStamped）写进Launch方便调参
优化并调试后的代码如下（其中协议中写的角度和速度指定bit与实际值的比例关系似乎不太对，需要实际调试的时候优化）：

2.2 接收数据节点

can_receive_node.cpp

#include "can_ros1_driver/can_receive.hpp"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "can_receive_node");
    canControl can_control;

    ros::spin();

    return 0;
}

can_receive.hpp

#ifndef CAN_H_
#define CAN_H_
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include 
#include 

class canControl
{
public:
    // 0 初始化
    canControl();

    // 1 接收CAN消息
    void receiveCanData();

    // 1.1 解析数据
    std::vector<int16_t> hexToShort(const std::vector<uint8_t> &hex_data);

    // 1.2 推算里程计并发布
    void publishOdometry(double linear_velocity, double angular_velocity, double Steer_Angle);

    // 2 析构函数
    ~canControl();

private:
    ros::NodeHandle nh_;
    // 轴距
    double wheel_base;
    // 套接字参数
    struct ifreq ifr;
    struct sockaddr_can can_addr;
    int sockfd;
    int ret;
    // 接收的数据
    struct can_frame frame;

    // 当前里程计
    double position_x_;
    double position_y_;
    double yaw;

    // 发布速度和里程计
    ros::Publisher pub_;
    ros::Publisher pub_Odom;
    tf::TransformBroadcaster tf_broadcaster_;
};

#endif // CAN_H_

can_receive.cpp

#include "can_ros1_driver/can_receive.hpp"

// 0 初始化
canControl::canControl() : nh_("~")
{
    // 1 初始化ROS节点
    nh_.param<double>("wheel_base", wheel_base, 1.0);
    position_x_ = 0.0;
    position_y_ = 0.0;
    yaw = 0.0;

    /* 打开套接字 */
    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (0 > sockfd)
    {
        perror("socket error");
        exit(EXIT_FAILURE);
    }

    /* 指定can0设备 */
    ifr = {0};
    can_addr = {0};
    frame = {0};
    strcpy(ifr.ifr_name, "can0");
    ioctl(sockfd, SIOCGIFINDEX, &ifr);
    can_addr.can_family = AF_CAN;
    can_addr.can_ifindex = ifr.ifr_ifindex;

    /* 将can0与套接字进行绑定 */
    if (0 > bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr)))
    {
        perror("bind error");
        close(sockfd);
        exit(EXIT_FAILURE);
    }

    // 创建用于反馈速度指令的发布者
    pub_ = nh_.advertise<geometry_msgs::Twist>("/feedback_twist", 1000);
    pub_Odom = nh_.advertise<nav_msgs::Odometry>("/panda_odom", 100);

    receiveCanData();
}

// 2 接收CAN串口消息
void canControl::receiveCanData()
{
    while (ros::ok())
    {
        /* 接收数据 */
        if (0 > read(sockfd, &frame, sizeof(struct can_frame)))
        {
            perror("read error");
            break;
        }

        /* 校验是否接收到错误帧 */
        if (frame.can_id & CAN_ERR_FLAG)
        {
            printf("Error frame!\n");
            break;
        }

        /* 校验帧格式 */
        // if (frame.can_id & CAN_EFF_FLAG) // 扩展帧
        //     printf("扩展帧 <0x%08x> ", frame.can_id & CAN_EFF_MASK);
        // else // 标准帧
        //     printf("标准帧 <0x%03x> ", frame.can_id & CAN_SFF_MASK);

        /* 校验帧类型：数据帧还是远程帧 */
        if (frame.can_id & CAN_RTR_FLAG)
        {
            printf("remote request\n");
            continue;
        }

        /* 打印数据长度 */
        // printf("[%d] ", frame.can_dlc);

        /* 打印数据 */
        std::vector<uint8_t> buff;
        for (int i = 0; i < frame.can_dlc; i++)
        {
            // 这里的%02x表示以两位十六进制数的形式输出，并且若不足两位则在前面补0。
            // printf("%02x ", frame.data[i]);
            // 一位一位字节地添加到队列
            buff.push_back(static_cast<int8_t>(frame.data[i]));
        }
        // printf("\n");

        /* 解析数据 */
        if (frame.can_id == 0x116 && frame.can_dlc == 8)
        {
            // printf("扩展帧 <0x%08x> ", frame.can_id & CAN_EFF_MASK);
            // printf("%02x ", frame.can_dlc);
            // printf("\n---------------------\n");
            // printf("Receive状态值为:%02x \n", buff[0]);
            // printf("Receive打角值L为:%02x \n", buff[2]);
            // printf("Receive打角值H为:%02x \n", buff[3]);
            // printf("Receive速度值L为:%02x \n", buff[4]);
            // printf("Receive速度值H为:%02x \n", buff[5]);
            // printf("Receive电量0x为:%02x \n", buff[6]);

            std::vector<int16_t> Velocity_feedback(4, 0);
            Velocity_feedback = hexToShort(buff);

            // 获取完一帧的标志位,可以输出数据
            geometry_msgs::Twist feedback_twist;

            // 1 获取档位
            int gear = static_cast<int>(Velocity_feedback[0]);

            // 3 获取车速
            // TODO  ± 5m/s 0.001(m/s)/bit  有符号 
            double DM_Speed = static_cast<double>(Velocity_feedback[2]) / 185;
            if (gear == 2)
                DM_Speed = -DM_Speed;
            feedback_twist.linear.x = DM_Speed;

            // 2 获取舵机转向角度（打角值）
            // TODO 524-1024-1524   0.1°/bit    0对应最左,1024对应中间角度
            double Steer_Angle = (static_cast<double>(Velocity_feedback[1]) - 1024) / 3.6;
            // 打角值满足：tan(打角值) = 前后轮轴距 / 转弯半径
            // 角速度 = 线速度 / 转弯半径 = 线速度 * tan(打角值) / 前后轮轴距
            double Velocity_Angle = DM_Speed * tan(fabs(M_PI * Steer_Angle / 180)) / wheel_base;
            feedback_twist.angular.z = Velocity_Angle;

            // 4 获取电池电量
            int power = static_cast<double>(Velocity_feedback[3]);
            if (power < 20)
                ROS_WARN("Battery power (%d) is less than 10%!", power);

            // std::cout << "Receive档位值为: " << gear << std::endl;
            // std::cout << "Receive角速度值为: " << Velocity_Angle << std::endl;
            // std::cout << "Receive速度值为: " << DM_Speed << std::endl;
            // std::cout << "Receive电量为: " << power << std::endl;

            // 5 发布速度
            pub_.publish(feedback_twist);

            // 6 发布里程计，左转是rZ正方向，而这个底盘最左是负，因此加-号
            publishOdometry(DM_Speed, Velocity_Angle, -M_PI * Steer_Angle / 180);
        }
    }
}

// 2.1 解析数据
std::vector<int16_t> canControl::hexToShort(const std::vector<uint8_t> &hex_data)
{
    std::vector<int16_t> short_data(4, 0);
    // 1 获取档位
    uint8_t byte = hex_data[0], mask = 0b00001100;
    // 使用位与操作符&来获取第3-4位的值,将结果向右位移3位
    uint8_t gear = (byte & mask) >> 3;
    short_data[0] = static_cast<int16_t>(gear);

    // 2 获取舵机转向角度
    // 将两个连续的字节（低位hex_data[i] 和 高位hex_data[i+1]）组合为一个 int16_t 类型的数值：千万注意高低顺序，仔细看通讯协议
    // 高位hex_data[i+1]需要先强制转换为一个有符号的short类型的数据以后再移位
    short Steer_Angle_H = static_cast<int16_t>(hex_data[3]);
    // 左移运算符 << 将 high 的二进制表示向左移动 8 位。这样做是因为 int16_t 类型占用 2 个字节，而我们希望将 high 的数据放置在最高的 8 位上。
    // |: 按位或运算符 | 将经过左移的 high 数据和 hex_data[i] 数据进行按位或操作，将它们组合为一个 16 位的数值
    short_data[1] = static_cast<int16_t>((Steer_Angle_H << 8) | hex_data[2]);

    // 3 获取车速
    short DM_Speed_H = static_cast<int16_t>(hex_data[5]);
    short_data[2] = static_cast<int16_t>((DM_Speed_H << 8) | hex_data[4]);

    // 4 获取电池电量
    short_data[3] = static_cast<int16_t>(hex_data[6]);

    return short_data;
}

// 2.2 推算里程计并发布
void canControl::publishOdometry(double linear_velocity, double angular_velocity, double Steer_Angle)
{
    // 计算与上一时刻的时间差
    static double last_stamp = ros::Time::now().toSec();
    double dt = ros::Time::now().toSec() - last_stamp;
    last_stamp = ros::Time::now().toSec();

    // 1 推算里程
    double wz = angular_velocity, vx = 0.0, vy = 0.0;
    // 计算偏航角（注意是相对于初始0，要绝对的需要IMU）
    yaw += angular_velocity * dt;
    // 根据线速度以及航向角计算X，Y方向上的位移
    vx = linear_velocity * std::cos(Steer_Angle);
    vy = linear_velocity * (linear_velocity < 0.0 ? sin(-Steer_Angle) : sin(Steer_Angle));
    // 计算相对与上一时刻的位，根据偏航变换到初始坐标系
    position_x_ += cos(yaw) * vx * dt - sin(yaw) * vy * dt;
    position_y_ += sin(yaw) * vx * dt + cos(yaw) * vy * dt;

    // 2 发布tf
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(yaw);
    // std::cout<< "odom_quat:" << odom_quat<
    geometry_msgs::TransformStamped tf_msg;
    tf_msg.header.stamp = ros::Time::now();
    tf_msg.header.frame_id = "odom";
    tf_msg.child_frame_id = "base_link";
    tf_msg.transform.translation.x = position_x_;
    tf_msg.transform.translation.y = position_y_;
    tf_msg.transform.translation.z = 0.0;
    tf_msg.transform.rotation = odom_quat;
    tf_broadcaster_.sendTransform(tf_msg);

    // 3 发布odom
    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = ros::Time::now();
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "base_link";

    odom_msg.pose.pose.position.x = position_x_;
    odom_msg.pose.pose.position.y = position_y_;
    odom_msg.pose.pose.position.z = 0.0;
    odom_msg.pose.pose.orientation = odom_quat;

    odom_msg.twist.twist.linear.x = vx;
    odom_msg.twist.twist.linear.y = vy;
    odom_msg.twist.twist.angular.z = wz;

    odom_msg.pose.covariance[0] = 0.1;
    odom_msg.pose.covariance[7] = 0.1;
    odom_msg.pose.covariance[14] = 0.1;
    odom_msg.pose.covariance[21] = 1.0;
    odom_msg.pose.covariance[28] = 1.0;
    odom_msg.pose.covariance[35] = 1.0;

    pub_Odom.publish(odom_msg);
}

// 3 析构函数
canControl::~canControl()
{
    /* 关闭套接字 */
    close(sockfd);
    exit(EXIT_SUCCESS);
}

2.3 发送数据节点

can_send_node.cpp

#include "can_ros1_driver/can_send.hpp"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "can_send_node");
    canControl can_control;

    ros::spin();

    return 0;
}

can_send.hpp

#ifndef CAN_H_
#define CAN_H_
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include "nav_msgs/Odometry.h"

class canControl
{
public:
    // 0 初始化
    canControl();

    // 1 下发CAN消息 速度控制节点消息的回调函数
    void velocityCallback(const geometry_msgs::Twist::ConstPtr &msg);
    void velocityCallback_Autoware(const geometry_msgs::TwistStamped::ConstPtr &msg);

    // 1.1 向底盘发送控制指令
    void publishCmd(float linear_vel, float angular_vel);

    // 2 析构函数
    ~canControl();

private:
    ros::NodeHandle nh_;
    // 是否为Autoware速度
    bool Autoware;
    // 接收的速度话题
    std::string twistTopic;
    // 轴距
    double wheel_base;
    // 套接字参数
    struct ifreq ifr;
    struct sockaddr_can can_addr;
    int sockfd;
    int ret;
    // 发送的数据
    struct can_frame frameE2;
    struct can_frame frame469;

    // 当前里程计
    double position_x_;
    double position_y_;
    double yaw;

    // 订阅速度控制
    ros::Subscriber sub_;
};

#endif // CAN_H_

can_send.cpp

#include "can_ros1_driver/can_send.hpp"

// 0 初始化
canControl::canControl() : nh_("~")
{
    // 1 初始化ROS节点
    nh_.param<double>("wheel_base", wheel_base, 1.0);
    nh_.param<bool>("Autoware", Autoware, 1.0);
    nh_.param<std::string>("twistTopic", twistTopic, "/cmd_vel");

    position_x_ = 0.0;
    position_y_ = 0.0;
    yaw = 0.0;

    /* 打开套接字 */
    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (0 > sockfd)
    {
        perror("socket error");
        exit(EXIT_FAILURE);
    }

    /* 指定can0设备 */
    ifr = {0};
    can_addr = {0};
    strcpy(ifr.ifr_name, "can0");
    ioctl(sockfd, SIOCGIFINDEX, &ifr);
    can_addr.can_family = AF_CAN;
    can_addr.can_ifindex = ifr.ifr_ifindex;

    /* 将can0与套接字进行绑定 */
    if (0 > bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr)))
    {
        perror("bind error");
        close(sockfd);
        exit(EXIT_FAILURE);
    }

    /* 设置过滤规则 */
    setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);

    /* 发送数据格式 */
    frameE2.can_dlc = 8;    // 一次发送8个字节数据
    frameE2.can_id = 0x0E2; // 帧ID为0x0E2,标准帧
    frameE2.data[4] = 0x00;
    frameE2.data[5] = 0x00;
    frameE2.data[6] = 0x00;

    frame469.can_dlc = 8;    // 一次发送8个字节数据
    frame469.can_id = 0x469; // 帧ID为0x469,标准帧
    frame469.data[0] = 0x20;
    frame469.data[1] = 0x00;
    frame469.data[2] = 0x00;
    frame469.data[5] = 0x00;
    frame469.data[6] = 0x00;

    // 创建订阅控制底盘消息的订阅者
    if (Autoware)
        sub_ = nh_.subscribe(twistTopic, 1000, &canControl::velocityCallback_Autoware, this);
    else
        sub_ = nh_.subscribe(twistTopic, 1000, &canControl::velocityCallback, this);
}

// 1 下发CAN消息 根据接收到的速度消息生成控制指令并发送
// 其他节点消息的回调函数
void canControl::velocityCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
    publishCmd(msg->linear.x, msg->angular.z);
}
// Autoware发送的
void canControl::velocityCallback_Autoware(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    publishCmd(msg->twist.linear.x, msg->twist.angular.z);
}

// 1.1 向底盘发送控制指令
void canControl::publishCmd(float linear_vel, float angular_vel)
{
    // 根据线速度和角速度生成控制指令的逻辑,将 linear_vel、angular_vel 转换为控制指令
    // 1. 计算挡位Gear
    int gear = 0; // 默认值
    if (linear_vel > 0)
    {
        gear = 1; // 前进挡
    }
    else if (linear_vel < 0)
    {
        gear = 2; // 倒挡
    }
    // 2. 线控驾驶标志位，通过CAN线控
    int line_control = 1;
    // 3. 驻车P标志位
    int parking_brake = 0;
    // 速度为0或速度反向的时候刹车
    static int last_gear = 0;
    if (linear_vel == 0.0 || last_gear + gear == 3)
        parking_brake = 1;
    last_gear = gear;
    // 4. 循环计数，默认为0
    int loop_count = 0;
    uint8_t gear_contrl = static_cast<uint8_t>(loop_count + parking_brake * 8 + line_control * 4 + gear);

    // 5 计算速度byte
    if (linear_vel > 5)
    {
        ROS_WARN("Speed is too fast!");
        linear_vel = 5;
    }
    // TODO 1000有待优化
    // printf("Send档位值为:%02x \n", gear_contrl);
    // std::cout << "Send速度值为: " << linear_vel << std::endl;
    uint8_t Speed_L = static_cast<uint8_t>((int)fabs(linear_vel) * 1000 % 256);
    uint8_t Speed_H = static_cast<uint8_t>((int)fabs(linear_vel) * 1000 / 256);

    // 6. 计算低压电器开关和速度校验和
    int brakeLight = 0;
    if (parking_brake)
        brakeLight = 1;
    int headlight = 0;
    int trafficIndicatorL = 0;
    int trafficIndicatorR = 0;
    if (angular_vel > 0)
        trafficIndicatorL = 1;
    else if (angular_vel < 0)
        trafficIndicatorR = 1;
    uint8_t LowVoltageEle = static_cast<uint8_t>(brakeLight + headlight * 2 + trafficIndicatorL * 4 + trafficIndicatorR * 8);

    uint8_t checksumSpeed = gear_contrl + Speed_L + Speed_H + LowVoltageEle;

    /* 发送数据 进入线控 协议需要每次先要发速度清空 */
    frameE2.data[0] = gear_contrl;
    frameE2.data[1] = 0x00;
    frameE2.data[2] = 0x00;
    frameE2.data[3] = LowVoltageEle;
    frameE2.data[7] = checksumSpeed;
    ret = write(sockfd, &frameE2, sizeof(frameE2)); // 发送数据

    // 7. 生成速度控制指令 发送数据
    frameE2.data[0] = gear_contrl;
    frameE2.data[1] = Speed_L;
    frameE2.data[2] = Speed_H;
    frameE2.data[3] = LowVoltageEle;
    frameE2.data[7] = checksumSpeed;
    ret = write(sockfd, &frameE2, sizeof(frameE2)); // 发送数据

    // 8. 计算角度byte
    // 转弯的角度 = arctan(( 角速度 / 线速度 ) * 轴距 )
    // 左转是rZ正方向，而这个底盘最左是负，因此加-号
    // TODO
    double angular = -atan((angular_vel / fabs(linear_vel)) * wheel_base);
    int angular_byte = 3.6 * angular * 180 / M_PI + 1024;
    // std::cout << "Send打角值为: " << angular_byte << std::endl;
    uint8_t angular_L = static_cast<uint8_t>((int)angular_byte % 256);
    uint8_t angular_H = static_cast<uint8_t>((int)angular_byte / 256);

    // 9. 计算角度校验和
    uint8_t checksumAng = 0x20 + angular_H + angular_L;

    // 10. 生成角度控制指令 发送数据
    frame469.data[3] = angular_H;
    frame469.data[4] = angular_L;
    frame469.data[7] = checksumSpeed;
    ret = write(sockfd, &frame469, sizeof(frame469)); // 发送数据
}

// 2 析构函数
canControl::~canControl()
{
    /* 发送数据 退出线控 */
    for (int i = 0; i < 8; i++)
        frameE2.data[i] = 0x00;
    ret = write(sockfd, &frameE2, sizeof(frameE2)); // 发送数据

    /* 关闭套接字 */
    close(sockfd);
    exit(EXIT_SUCCESS);
}

2.4 功能包配置

can_control.launch

<launch>
    <node name="can_receive_node" pkg="can_ros1_driver" type="can_receive_node" output="screen">
        
        <param name="wheel_base" value="0.35"/>
    node>
    <node name="can_send_node" pkg="can_ros1_driver" type="can_send_node" output="screen">
        
        <param name="wheel_base" value="0.35"/>
        
        <param name="Autoware" value="false"/>
        
        <param name="twistTopic" value="/turtle1/cmd_vel"/>
    node>
launch>

cmakeLists

cmake_minimum_required(VERSION 3.0.2)
project(can_ros1_driver)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  tf
  std_msgs
  nav_msgs
  geometry_msgs
)

catkin_package( CATKIN_DEPENDS roscpp tf std_msgs nav_msgs geometry_msgs)

include_directories(
  include
  ${catkin_INCLUDE_DIRS})

add_executable(can_receive_node src/can_receive.cpp src/can_receive_node.cpp)
# ROS相关库
target_link_libraries(can_receive_node ${catkin_LIBRARIES})

add_executable(can_send_node src/can_send.cpp src/can_send_node.cpp)
target_link_libraries(can_send_node ${catkin_LIBRARIES})

package.xml

<package format="2">
  <name>can_ros1_drivername>
  <version>0.0.0version>
  <description>The can_ros1_driver packagedescription>

  <maintainer email="[email protected]">WangBinmaintainer>

  <license>TODOlicense>

  <buildtool_depend>catkinbuildtool_depend>
  <depend>roscppdepend>
  <depend>tfdepend>
  <depend>std_msgsdepend>
  <depend>nav_msgsdepend>
  <depend>geometry_msgsdepend>
package>

三、ROS2驱动程序

TODO