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

本文主要是介绍Nvidia Jetson AGX Orin使用CAN与底盘通信（ROS C++ 驱动），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

文章目录

一、Nvidia Jetson AGX Orin使用CAN通信
- 1.1 CAN使能配置修改GPIO口功能
- 1.2 can收发测试
二、通过CAN协议编写CAN的SocketCan ROS1驱动程序
- 2.1 通讯协议
- 2.2 接收数据节点
- 2.3 发送数据节点
- 2.4 功能包配置
三、ROS2驱动程序

一、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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>#include <iostream>
#include <string>
#include <cmath>#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>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 发布tfgeometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(yaw);// std::cout<< "odom_quat:" << odom_quat<<std::endl;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 发布odomnav_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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>#include <iostream>
#include <string>
#include <cmath>#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);elsesub_ = 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. 计算挡位Gearint 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. 循环计数，默认为0int loop_count = 0;uint8_t gear_contrl = static_cast<uint8_t>(loop_count + parking_brake * 8 + line_control * 4 + gear);// 5 计算速度byteif (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正方向，而这个底盘最左是负，因此加-号// TODOdouble 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"/><!-- 是否为Autoware速度 --><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 COMPONENTSroscpptfstd_msgsnav_msgsgeometry_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

<?xml version="1.0"?>
<package format="2"><name>can_ros1_driver</name><version>0.0.0</version><description>The can_ros1_driver package</description><maintainer email="WangBin@todo.todo">WangBin</maintainer><license>TODO</license><buildtool_depend>catkin</buildtool_depend><depend>roscpp</depend><depend>tf</depend><depend>std_msgs</depend><depend>nav_msgs</depend><depend>geometry_msgs</depend>
</package>