Initial import of FaRui Campus ADS v3.2
This commit is contained in:
li-shihao-code
@@ -0,0 +1,3 @@
|
||||
__pycache__/
|
||||
*.pyc
|
||||
*.pyc.*
|
||||
+200
@@ -0,0 +1,200 @@
|
||||
# TEEMO Chassis Drive
|
||||
|
||||
基于 `dbc/X5_CAN2_VCU_500k.dbc` 和 `dbc/X5_CAN2_AD_500k.dbc` 实现的一套 ROS 2 底盘 CAN 驱动。
|
||||
|
||||
这套驱动做两件事：
|
||||
|
||||
- 订阅 CAN 卡上报的 `kvaser_input`，按 DBC 解析底盘状态并发布成 ROS 2 话题
|
||||
- 按 DBC 周期组包底盘控制报文，并发布到 `kvaser_output`
|
||||
|
||||
## 重点说明
|
||||
|
||||
`kvaser_input` 和 `kvaser_output` 使用的不是自定义消息，使用的是标准 ROS CAN 消息：
|
||||
|
||||
- 类型：`can_msgs/msg/Frame`
|
||||
|
||||
也就是说，你的 CAN 卡节点只要发布/订阅 `can_msgs/msg/Frame`，就能直接和这套驱动对接。
|
||||
|
||||
## 标准 CAN 消息格式
|
||||
|
||||
本驱动接收和发送的原始 CAN 话题如下：
|
||||
|
||||
- 订阅：`kvaser_input`
|
||||
- 发布：`kvaser_output`
|
||||
- 消息类型：`can_msgs/msg/Frame`
|
||||
|
||||
常用字段如下：
|
||||
|
||||
```text
|
||||
std_msgs/Header header
|
||||
uint32 id
|
||||
bool is_rtr
|
||||
bool is_extended
|
||||
bool is_error
|
||||
uint8 dlc
|
||||
uint8[8] data
|
||||
```
|
||||
|
||||
其中本驱动的使用方式是：
|
||||
|
||||
- 接收时间戳读取 `header.stamp`
|
||||
- CAN ID 读取 `id`
|
||||
- 数据长度读取 `dlc`
|
||||
- 8 字节数据读取 `data`
|
||||
- 当前按经典 CAN 8 字节处理
|
||||
|
||||
## 包结构
|
||||
|
||||
- `teemo_chassis_msgs`
|
||||
解析后的状态消息和控制消息定义
|
||||
- `teemo_chassis_drive`
|
||||
DBC 编解码、接收节点、发送节点、launch 和默认参数
|
||||
|
||||
## 接收侧
|
||||
|
||||
接收节点订阅 `kvaser_input`，识别并解析以下 DBC 状态报文：
|
||||
|
||||
- `0x300` `VCU_Vehicle_Status_3`
|
||||
- `0x3A4` `Vehicle_Mileage2`
|
||||
- `0x3A3` `Vehicle_Mileage1`
|
||||
- `0x30E` `VCU_Vehicle_Error_Status`
|
||||
- `0x3A2` `Vehicle_Odometer_Status`
|
||||
- `0x30A` `VCU_Vehicle_HVBat_Status`
|
||||
- `0x309` `VCU_RR_Wheel_Status`
|
||||
- `0x307` `VCU_FR_Wheel_Status`
|
||||
- `0x308` `VCU_RL_Wheel_Status`
|
||||
- `0x306` `VCU_FL_Wheel_Status`
|
||||
- `0x303` `VCU_Vehicle_Status_1`
|
||||
- `0x304` `VCU_Vehicle_Status_2`
|
||||
- `0x301` `VCU_Vehicle_Diagnosis`
|
||||
- `0x7F8` `VCU_Version`
|
||||
- `0x30C` `VCU_Vehicle_PwrCtrl_Status`
|
||||
|
||||
解析后会发布到这些话题：
|
||||
|
||||
- `status/VCU_Vehicle_Status_1`
|
||||
- `status/VCU_Vehicle_Status_2`
|
||||
- `status/VCU_Vehicle_Status_3`
|
||||
- `status/VCU_Vehicle_Diagnosis`
|
||||
- `status/VCU_Vehicle_Error_Status`
|
||||
- `status/VCU_Vehicle_HVBat_Status`
|
||||
- `status/Vehicle_Odometer_Status`
|
||||
- `status/Vehicle_Mileage1`
|
||||
- `status/Vehicle_Mileage2`
|
||||
- `status/VCU_Version`
|
||||
- `status/VCU_Vehicle_PwrCtrl_Status`
|
||||
- `status/VCU_FL_Wheel_Status`
|
||||
- `status/VCU_FR_Wheel_Status`
|
||||
- `status/VCU_RL_Wheel_Status`
|
||||
- `status/VCU_RR_Wheel_Status`
|
||||
|
||||
如需兼容旧版小写话题名（例如 `status/vehicle_status_1`），可在参数里设置：
|
||||
|
||||
- `use_legacy_status_topic_names: true`
|
||||
|
||||
每个解析后的状态消息都保留这些基础字段：
|
||||
|
||||
- `stamp`
|
||||
- `can_id`
|
||||
- `dlc`
|
||||
- `raw_data`
|
||||
|
||||
## 发送侧
|
||||
|
||||
发送节点按 DBC 周期自动发布以下控制帧到 `kvaser_output`：
|
||||
|
||||
- `0x1D6` `AD_OTAReq` 100 ms
|
||||
- `0x1D7` `AD_Setup_Control` 20 ms
|
||||
- `0x1DB` `System_Power_Control` 20 ms
|
||||
- `0x1DA` `Power_on_CAN` 20 ms
|
||||
- `0x1DE` `AD_Control_Body` 20 ms
|
||||
- `0x1D2` `AD_Control_Accelerate` 20 ms
|
||||
- `0x1D3` `AD_Control_Brake` 20 ms
|
||||
- `0x1D4` `AD_Control_Steering` 20 ms
|
||||
|
||||
发送侧已经实现：
|
||||
|
||||
- Rolling Counter 自动递增
|
||||
- `AD_CheckSum` 异或校验
|
||||
- DBC 缩放、偏移、限幅
|
||||
|
||||
## 默认控制参数
|
||||
|
||||
默认控制参数在：
|
||||
|
||||
- [teemo_chassis.yaml](/d:/Project/TEEMO_Chassis_Drive/teemo_chassis_drive/config/teemo_chassis.yaml:1)
|
||||
|
||||
你后面最常改的是这些：
|
||||
|
||||
- `ad_steering_angle_cmd`
|
||||
- `ad_steering_speed_cmd`
|
||||
- `ad_torque_control`
|
||||
- `ad_speedor_torque_control`
|
||||
- `ad_accelerate_gear`
|
||||
- `ad_accelerate_work_mode`
|
||||
- `ad_accelerate_valid`
|
||||
- `ad_brake_pressure_cmd`
|
||||
- `ad_dbs_valid`
|
||||
- `ad_steering_valid`
|
||||
|
||||
当前默认值是安全起步配置：
|
||||
|
||||
- 驱动有效位默认 `0`
|
||||
- 制动有效位默认 `0`
|
||||
- 转向有效位默认 `0`
|
||||
- 油门默认 `0`
|
||||
- 制动默认 `0`
|
||||
- 转角默认 `0.0`
|
||||
|
||||
## 可选控制话题
|
||||
|
||||
除了改 YAML，你也可以发布高层控制话题实时覆盖默认值：
|
||||
|
||||
- 话题：`control/vehicle_command`
|
||||
- 类型：`teemo_chassis_msgs/msg/VehicleControlCommand`
|
||||
|
||||
只要这个话题持续有消息，发送节点就优先使用它；超时后会自动回退到 YAML 默认值。
|
||||
|
||||
## 构建
|
||||
|
||||
先确保环境里有：
|
||||
|
||||
- `can_msgs`
|
||||
- `rclpy`
|
||||
- `colcon`
|
||||
|
||||
然后构建：
|
||||
|
||||
```bash
|
||||
colcon build --packages-select teemo_chassis_msgs teemo_chassis_drive
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
Windows PowerShell：
|
||||
|
||||
```powershell
|
||||
colcon build --packages-select teemo_chassis_msgs teemo_chassis_drive
|
||||
. .\install\setup.ps1
|
||||
```
|
||||
|
||||
## 启动
|
||||
|
||||
```bash
|
||||
ros2 launch teemo_chassis_drive driver.launch.py
|
||||
```
|
||||
|
||||
## 联调时最重要的一点
|
||||
|
||||
你的 Kvaser 节点必须满足下面这个接口约定：
|
||||
|
||||
- 往 `kvaser_input` 发布 `can_msgs/msg/Frame`
|
||||
- 从 `kvaser_output` 订阅 `can_msgs/msg/Frame`
|
||||
|
||||
如果它不是这个消息类型，而是你们自己封装过的消息，那就需要再加一层适配节点。
|
||||
|
||||
## 关键代码位置
|
||||
|
||||
- 接收节点：[receiver_node.py](/d:/Project/TEEMO_Chassis_Drive/teemo_chassis_drive/teemo_chassis_drive/receiver_node.py:41)
|
||||
- 发送节点：[sender_node.py](/d:/Project/TEEMO_Chassis_Drive/teemo_chassis_drive/teemo_chassis_drive/sender_node.py:94)
|
||||
- DBC 编解码：[dbc_codec.py](/d:/Project/TEEMO_Chassis_Drive/teemo_chassis_drive/teemo_chassis_drive/dbc_codec.py:1)
|
||||
- 控制参数：[teemo_chassis.yaml](/d:/Project/TEEMO_Chassis_Drive/teemo_chassis_drive/config/teemo_chassis.yaml:1)
|
||||
+320
@@ -0,0 +1,320 @@
|
||||
VERSION ""
|
||||
|
||||
|
||||
NS_ :
|
||||
NS_DESC_
|
||||
CM_
|
||||
BA_DEF_
|
||||
BA_
|
||||
VAL_
|
||||
CAT_DEF_
|
||||
CAT_
|
||||
FILTER
|
||||
BA_DEF_DEF_
|
||||
EV_DATA_
|
||||
ENVVAR_DATA_
|
||||
SGTYPE_
|
||||
SGTYPE_VAL_
|
||||
BA_DEF_SGTYPE_
|
||||
BA_SGTYPE_
|
||||
SIG_TYPE_REF_
|
||||
VAL_TABLE_
|
||||
SIG_GROUP_
|
||||
SIG_VALTYPE_
|
||||
SIGTYPE_VALTYPE_
|
||||
BO_TX_BU_
|
||||
BA_DEF_REL_
|
||||
BA_REL_
|
||||
BA_DEF_DEF_REL_
|
||||
BU_SG_REL_
|
||||
BU_EV_REL_
|
||||
BU_BO_REL_
|
||||
SG_MUL_VAL_
|
||||
|
||||
BS_:
|
||||
|
||||
BU_: AD VCU
|
||||
|
||||
|
||||
BO_ 471 AD_Setup_Control: 8 AD
|
||||
SG_ AD_CheckSum : 56|8@1+ (1,0) [0|255] "" VCU
|
||||
SG_ AD_RollingCounter : 52|4@1+ (1,0) [0|15] "" VCU
|
||||
SG_ AD_DenyPwrDownReqCmd : 5|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_MoveModeReq : 14|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Release_R_Bumper : 12|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Release_L_Bumper : 19|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Disable_R_Bumper : 4|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Disable_L_Bumper : 3|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Disable_B_Bumper : 2|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Disable_F_Bumper : 1|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Clear_TRIP : 0|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Release_B_Bumper : 18|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Release_F_Bumper : 17|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Release_Emergency_Button : 16|1@1+ (1,0) [0|1] "" VCU
|
||||
|
||||
BO_ 475 AD_Pwr_Control: 8 AD
|
||||
SG_ AD_RollingCounter : 52|4@1+ (1,0) [0|15] "" VCU
|
||||
SG_ AD_CheckSum : 56|8@1+ (1,0) [0|255] "" VCU
|
||||
SG_ ADU_Relay3_Ctrl : 4|2@1+ (1,0) [0|3] "" VCU
|
||||
SG_ Vehicle_Work_Mode_Control : 14|2@1+ (1,0) [0|3] "" VCU
|
||||
SG_ ADU_Relay2_Ctrl : 2|2@1+ (1,0) [0|3] "" VCU
|
||||
SG_ ADU_Relay5_Ctrl : 8|2@1+ (1,0) [0|3] "" VCU
|
||||
SG_ ADU_Relay1_Ctrl : 0|2@1+ (1,0) [0|3] "" VCU
|
||||
SG_ ADU_Relay4_Ctrl : 6|2@1+ (1,0) [0|3] "" VCU
|
||||
|
||||
BO_ 474 AD_VCU_Pwr_Control: 8 AD
|
||||
SG_ AD_RollingCounter : 52|4@1+ (1,0) [0|15] "" VCU
|
||||
SG_ AD_CheckSum : 56|8@1+ (1,0) [0|255] "" VCU
|
||||
SG_ AD_PwrDownCountInit : 0|6@1+ (1,0) [0|63] "S" VCU
|
||||
SG_ AD_Vehicle_PwrReq : 8|8@1+ (1,0) [0|255] "" VCU
|
||||
|
||||
BO_ 478 AD_Control_Body: 8 AD
|
||||
SG_ AD_Clean_Fan : 16|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_RollingCounter : 52|4@1+ (1,0) [0|15] "" VCU
|
||||
SG_ AD_CheckSum : 56|8@1+ (1,0) [0|255] "" VCU
|
||||
SG_ AD_VehLightSwtEmergFreq : 0|4@1+ (1,0) [0|15] "" VCU
|
||||
SG_ AD_Horn_2_Control : 15|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Fog_Light : 14|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Body_Valid : 6|2@1+ (1,0) [0|3] "" VCU
|
||||
SG_ AD_Low_Beam : 13|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Reversing_Lights : 12|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Double_Flash_Light : 11|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Brake_Light : 10|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Horn_1_Control : 9|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_High_Beam : 8|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Right_Turn_Light : 5|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Left_Turn_Light : 4|1@1+ (1,0) [0|1] "" VCU
|
||||
|
||||
BO_ 466 AD_Control_Accelerate: 8 AD
|
||||
SG_ AD_RollingCounter : 52|4@1+ (1,0) [0|15] "" VCU
|
||||
SG_ AD_CheckSum : 56|8@1+ (1,0) [0|255] "" VCU
|
||||
SG_ AD_Accelerate_Req : 32|8@1+ (0.1,-10) [-10|10] "m/ss" VCU
|
||||
SG_ AD_Energy_Recovery : 5|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_Speed_Req : 8|11@1+ (0.1,0) [0|40] "km/h" VCU
|
||||
SG_ AD_Torque_Pedal : 24|7@1+ (1,0) [0|100] "%" VCU
|
||||
SG_ AD_Accelerate_Gear : 3|2@1+ (1,0) [0|3] "" VCU
|
||||
SG_ AD_Accelerate_Work_Mode : 0|3@1+ (1,0) [0|2] "" VCU
|
||||
SG_ AD_Accelerate_Valid : 6|2@1+ (1,0) [0|1] "" VCU
|
||||
|
||||
BO_ 467 AD_Control_Brake: 8 AD
|
||||
SG_ AD_CheckSum : 56|8@1+ (1,0) [0|255] "" VCU
|
||||
SG_ AD_RollingCounter : 52|4@1+ (1,0) [0|15] "" VCU
|
||||
SG_ AD_DBS_WorkMode : 1|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_AWSC_Flag : 0|1@1+ (1,0) [0|1] "" VCU
|
||||
SG_ AD_BrakePressure_Req : 8|7@1+ (1,0) [0|100] "%" VCU
|
||||
SG_ AD_DBS_Valid : 6|2@1+ (1,0) [0|3] "" VCU
|
||||
|
||||
BO_ 468 AD_Control_Steering: 8 AD
|
||||
SG_ AD_CheckSum : 56|8@1+ (1,0) [0|255] "" VCU
|
||||
SG_ AD_RollingCounter : 52|4@1+ (1,0) [0|15] "" VCU
|
||||
SG_ AD_Steering_Speed_Cmd : 8|10@1+ (0.1,5) [5|107.3] "deg/s" VCU
|
||||
SG_ AD_Steering_Angle_Cmd : 18|11@1+ (0.1,-90) [-90|90] "deg" VCU
|
||||
SG_ AD_Steering_Valid : 6|2@1+ (1,0) [0|3] "" VCU
|
||||
|
||||
|
||||
|
||||
CM_ BO_ 471 "ADChassis控制";
|
||||
CM_ SG_ 471 AD_DenyPwrDownReqCmd "0x0:无效
|
||||
0x1:请求OTA";
|
||||
CM_ SG_ 471 AD_MoveModeReq "自动驾驶控制车辆挪车模式";
|
||||
CM_ SG_ 471 AD_Release_R_Bumper "0x0：无效
|
||||
0x1：解除右Bumper";
|
||||
CM_ SG_ 471 AD_Release_L_Bumper "0x0：无效
|
||||
0x1：解除左Bumper";
|
||||
CM_ SG_ 471 AD_Disable_R_Bumper "0x0：无效
|
||||
0x1：禁用右Bumper";
|
||||
CM_ SG_ 471 AD_Disable_L_Bumper "0x0：无效
|
||||
0x1：禁用左Bumper";
|
||||
CM_ SG_ 471 AD_Disable_B_Bumper "0x0：无效
|
||||
0x1：禁用后Bumper";
|
||||
CM_ SG_ 471 AD_Disable_F_Bumper "0x0：无效
|
||||
0x1：禁用前Bumper";
|
||||
CM_ SG_ 471 AD_Clear_TRIP "0无效，1清除小里程
|
||||
";
|
||||
CM_ SG_ 471 AD_Release_B_Bumper "0x0：无效
|
||||
0x1：解除后Bumper";
|
||||
CM_ SG_ 471 AD_Release_F_Bumper "0x0：无效
|
||||
0x1：解除前Bumper";
|
||||
CM_ SG_ 471 AD_Release_Emergency_Button "0x0：无效
|
||||
0x1：解除急停按钮";
|
||||
CM_ BO_ 475 "自动驾驶继电器控制
|
||||
";
|
||||
CM_ SG_ 475 ADU_Relay3_Ctrl "自驾继电器3
|
||||
0x0：下电
|
||||
0x1：上电
|
||||
0x2：保持
|
||||
0x3：重启";
|
||||
CM_ SG_ 475 Vehicle_Work_Mode_Control "0x0：正常工作模式
|
||||
0x1：临时停车
|
||||
0x2：长时间停车";
|
||||
CM_ SG_ 475 ADU_Relay2_Ctrl "自驾继电器2
|
||||
0x0：下电
|
||||
0x1：上电
|
||||
0x2：保持
|
||||
0x3：重启";
|
||||
CM_ SG_ 475 ADU_Relay5_Ctrl "自驾继电器5
|
||||
0x0：下电
|
||||
0x1：上电
|
||||
0x2：保持
|
||||
0x3：重启";
|
||||
CM_ SG_ 475 ADU_Relay1_Ctrl "自驾继电器1
|
||||
0x0：下电
|
||||
0x1：上电
|
||||
0x2：保持
|
||||
0x3：重启";
|
||||
CM_ SG_ 475 ADU_Relay4_Ctrl "自驾继电器4
|
||||
0x0：下电
|
||||
0x1：上电
|
||||
0x2：保持
|
||||
0x3：重启";
|
||||
CM_ BO_ 474 "远程上电信号";
|
||||
CM_ SG_ 474 AD_PwrDownCountInit "下电倒计时配置
|
||||
0约定为30s；无0x50A约定为5s";
|
||||
CM_ SG_ 474 AD_Vehicle_PwrReq "底盘总电源
|
||||
0：关机（底盘、货箱系统均断电）
|
||||
1：开机（底盘、货箱系统均上电）
|
||||
2：保持底盘当前状态
|
||||
3：重启（只能在底盘上电的情况下发重启才会生效）
|
||||
4：跳过下电倒计时（在下电倒计时阶段发送可跳过）
|
||||
";
|
||||
CM_ BO_ 478 "ADBody控制";
|
||||
CM_ SG_ 478 AD_Clean_Fan "清洁风机开关：
|
||||
0：不开启
|
||||
1：开启";
|
||||
CM_ SG_ 478 AD_VehLightSwtEmergFreq "双闪频率
|
||||
0x0:1.5hz
|
||||
0x1:3hz
|
||||
0x2:4hz
|
||||
0x3:5hz";
|
||||
CM_ SG_ 478 AD_Horn_2_Control "AD车载大喇叭控制使能
|
||||
0x0:车载大喇叭关
|
||||
0x1:车载大喇叭开";
|
||||
CM_ SG_ 478 AD_Fog_Light "AD雾灯控制使能
|
||||
0x0：雾灯灭
|
||||
0x1：雾灯亮";
|
||||
CM_ SG_ 478 AD_Body_Valid "AD车身部件控制使能
|
||||
0x0：车身部件去使能
|
||||
0x1：车身部件使能";
|
||||
CM_ SG_ 478 AD_Low_Beam "AD近光灯控制使能
|
||||
0x0：近光灯灭
|
||||
0x1：近光灯亮";
|
||||
CM_ SG_ 478 AD_Reversing_Lights "AD倒车灯控制使能
|
||||
0x0：倒车灯灭
|
||||
0x1：倒车灯亮";
|
||||
CM_ SG_ 478 AD_Double_Flash_Light "AD双闪灯控制使能
|
||||
0x0：双闪灯关
|
||||
0x1：双闪灯开";
|
||||
CM_ SG_ 478 AD_Brake_Light "AD制动灯使能
|
||||
0x0：制动灯灭
|
||||
0x1：制动灯亮";
|
||||
CM_ SG_ 478 AD_Horn_1_Control "AD内置喇叭控制使能
|
||||
0x0：低速报警器内置喇叭关
|
||||
0x1：低速报警器内置喇叭开";
|
||||
CM_ SG_ 478 AD_High_Beam "AD远光灯控制使能
|
||||
0x0：远光灯灭
|
||||
0x1：远光灯亮";
|
||||
CM_ SG_ 478 AD_Right_Turn_Light "AD右转灯控制使能
|
||||
0x0：右转灯灭
|
||||
0x1：右转灯亮";
|
||||
CM_ SG_ 478 AD_Left_Turn_Light "AD左转灯控制使能
|
||||
0x0：左转灯灭
|
||||
0x1：左转灯亮";
|
||||
CM_ BO_ 466 "AD驱动请求";
|
||||
CM_ SG_ 466 AD_Energy_Recovery "能量回收标志位
|
||||
0x0：无效
|
||||
0x1：启用能量回收";
|
||||
CM_ SG_ 466 AD_Speed_Req "请求车速";
|
||||
CM_ SG_ 466 AD_Torque_Pedal "驱动踏板量";
|
||||
CM_ SG_ 466 AD_Accelerate_Gear "AD请求档位
|
||||
0x0：P档
|
||||
0x1：D档
|
||||
0x2：N档
|
||||
0x3：R档";
|
||||
CM_ SG_ 466 AD_Accelerate_Work_Mode "AD请求控制模式
|
||||
0x0：扭矩控制
|
||||
0x1：速度控制
|
||||
0x2：加速度控制";
|
||||
CM_ SG_ 466 AD_Accelerate_Valid "AD纵向控制使能
|
||||
0x0：自动驾驶去使能
|
||||
0x1：自动驾驶使能";
|
||||
CM_ BO_ 467 "AD制动请求";
|
||||
CM_ SG_ 467 AD_DBS_WorkMode "AD控制排气模式接口";
|
||||
CM_ SG_ 467 AD_AWSC_Flag "EPB单边打滑标志位
|
||||
0：未开启
|
||||
1：开启";
|
||||
CM_ SG_ 467 AD_BrakePressure_Req "制动踏板量";
|
||||
CM_ SG_ 467 AD_DBS_Valid "AD制动控制使能
|
||||
0x0：无效
|
||||
0x1：自驾制动可控";
|
||||
CM_ BO_ 468 "AD转向请求";
|
||||
CM_ SG_ 468 AD_Steering_Speed_Cmd "轮端转向角速度控制";
|
||||
CM_ SG_ 468 AD_Steering_Angle_Cmd "AD模式下，-35到+35度 转角请求（左正右负）";
|
||||
CM_ SG_ 468 AD_Steering_Valid "AD横向控制使能
|
||||
0x0：无效
|
||||
0x1：自驾转向使能";
|
||||
BA_DEF_ BO_ "GenMsgCycleTime" INT 0 0;
|
||||
BA_DEF_ BO_ "GenMsgSendType" ENUM "Cyclic","not_used","not_used","not_used","not_used","Cyclic","not_used","IfActive","NoMsgSendType";
|
||||
BA_DEF_ BU_ "NmStationAddress" HEX 0 0;
|
||||
BA_DEF_ "DBName" STRING ;
|
||||
BA_DEF_ "BusType" STRING ;
|
||||
BA_DEF_DEF_ "GenMsgCycleTime" 0;
|
||||
BA_DEF_DEF_ "GenMsgSendType" "Cyclic";
|
||||
BA_DEF_DEF_ "NmStationAddress" 0;
|
||||
BA_DEF_DEF_ "DBName" "";
|
||||
BA_DEF_DEF_ "BusType" "CAN";
|
||||
BA_ "DBName" "X_SERIES_CAN2_AD";
|
||||
BA_ "GenMsgSendType" BO_ 471 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 471 20;
|
||||
BA_ "GenMsgSendType" BO_ 475 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 475 20;
|
||||
BA_ "GenMsgSendType" BO_ 474 7;
|
||||
BA_ "GenMsgCycleTime" BO_ 474 20;
|
||||
BA_ "GenMsgSendType" BO_ 478 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 478 20;
|
||||
BA_ "GenMsgSendType" BO_ 466 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 466 20;
|
||||
BA_ "GenMsgSendType" BO_ 467 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 467 20;
|
||||
BA_ "GenMsgSendType" BO_ 468 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 468 20;
|
||||
VAL_ 471 AD_DenyPwrDownReqCmd 1 "自驾OTA请求" 0 "无效" ;
|
||||
VAL_ 471 AD_MoveModeReq 1 "Valid" 0 "Invalid" ;
|
||||
VAL_ 471 AD_Release_R_Bumper 1 "解除触碰条" 0 "保持" ;
|
||||
VAL_ 471 AD_Release_L_Bumper 1 "解除触碰条" 0 "保持" ;
|
||||
VAL_ 471 AD_Disable_R_Bumper 1 "禁用触碰条" 0 "保持" ;
|
||||
VAL_ 471 AD_Disable_L_Bumper 1 "禁用触碰条" 0 "保持" ;
|
||||
VAL_ 471 AD_Disable_B_Bumper 1 "禁用触碰条" 0 "保持" ;
|
||||
VAL_ 471 AD_Disable_F_Bumper 1 "禁用触碰条" 0 "保持" ;
|
||||
VAL_ 471 AD_Clear_TRIP 1 "Valid" 0 "Invalid" ;
|
||||
VAL_ 471 AD_Release_B_Bumper 1 "解除触碰条" 0 "保持" ;
|
||||
VAL_ 471 AD_Release_F_Bumper 1 "解除触碰条" 0 "保持" ;
|
||||
VAL_ 471 AD_Release_Emergency_Button 1 "解除急停" 0 "保持" ;
|
||||
VAL_ 475 ADU_Relay3_Ctrl 3 "重启" 2 "保持" 1 "上电" 0 "下电" ;
|
||||
VAL_ 475 Vehicle_Work_Mode_Control 2 "长时间停车" 1 "临时停车" 0 "正常" ;
|
||||
VAL_ 475 ADU_Relay2_Ctrl 3 "重启" 2 "保持" 1 "上电" 0 "下电" ;
|
||||
VAL_ 475 ADU_Relay5_Ctrl 3 "重启" 2 "保持" 1 "上电" 0 "下电" ;
|
||||
VAL_ 475 ADU_Relay1_Ctrl 3 "重启" 2 "保持" 1 "上电" 0 "下电" ;
|
||||
VAL_ 475 ADU_Relay4_Ctrl 3 "重启" 2 "保持" 1 "上电" 0 "下电" ;
|
||||
VAL_ 474 AD_Vehicle_PwrReq 4 "立即下电" 3 "重启" 2 "保持" 1 "开机" 0 "关机" ;
|
||||
VAL_ 478 AD_Clean_Fan 1 "Valid" 0 "Invalid" ;
|
||||
VAL_ 478 AD_VehLightSwtEmergFreq 3 "5Hz" 2 "4Hz" 1 "3Hz" 0 "1.5Hz" ;
|
||||
VAL_ 478 AD_Horn_2_Control 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_Fog_Light 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_Body_Valid 1 "Valid" 0 "Invalid" ;
|
||||
VAL_ 478 AD_Low_Beam 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_Reversing_Lights 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_Double_Flash_Light 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_Brake_Light 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_Horn_1_Control 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_High_Beam 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_Right_Turn_Light 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 478 AD_Left_Turn_Light 1 "Enable" 0 "Disable" ;
|
||||
VAL_ 466 AD_Energy_Recovery 1 "Valid" 0 "Invalid" ;
|
||||
VAL_ 466 AD_Accelerate_Gear 3 "R档" 2 "N档" 1 "D档" 0 "P档" ;
|
||||
VAL_ 466 AD_Accelerate_Work_Mode 2 "加速度模式" 1 "速度模式" 0 "Pedal模式" ;
|
||||
VAL_ 466 AD_Accelerate_Valid 1 "Valid" 0 "Invalid" ;
|
||||
VAL_ 467 AD_DBS_WorkMode 1 "排气模式" 0 "线控模式" ;
|
||||
VAL_ 467 AD_AWSC_Flag 1 "Valid" 0 "Invalid" ;
|
||||
VAL_ 467 AD_DBS_Valid 1 "Valid" 0 "Invalid" ;
|
||||
VAL_ 468 AD_Steering_Valid 1 "Valid" 0 "Invalid" ;
|
||||
|
||||
+735
@@ -0,0 +1,735 @@
|
||||
VERSION ""
|
||||
|
||||
|
||||
NS_ :
|
||||
NS_DESC_
|
||||
CM_
|
||||
BA_DEF_
|
||||
BA_
|
||||
VAL_
|
||||
CAT_DEF_
|
||||
CAT_
|
||||
FILTER
|
||||
BA_DEF_DEF_
|
||||
EV_DATA_
|
||||
ENVVAR_DATA_
|
||||
SGTYPE_
|
||||
SGTYPE_VAL_
|
||||
BA_DEF_SGTYPE_
|
||||
BA_SGTYPE_
|
||||
SIG_TYPE_REF_
|
||||
VAL_TABLE_
|
||||
SIG_GROUP_
|
||||
SIG_VALTYPE_
|
||||
SIGTYPE_VALTYPE_
|
||||
BO_TX_BU_
|
||||
BA_DEF_REL_
|
||||
BA_REL_
|
||||
BA_DEF_DEF_REL_
|
||||
BU_SG_REL_
|
||||
BU_EV_REL_
|
||||
BU_BO_REL_
|
||||
SG_MUL_VAL_
|
||||
|
||||
BS_:
|
||||
|
||||
BU_: AD VCU
|
||||
|
||||
|
||||
BO_ 768 VCU_Vehicle_Status_3: 8 VCU
|
||||
SG_ Vehicle_Steering_Spd : 0|16@1- (0.01,0) [-327.68|327.67] "deg/s" Vector__XXX
|
||||
|
||||
BO_ 932 Vehicle_Mileage2: 8 VCU
|
||||
SG_ Vehicle_Mileage2_MsgCntr : 60|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ Vehicle_TRIP1 : 32|28@1+ (1,0) [0|268435455] "cm" AD
|
||||
SG_ Vehicle_Remote_Mileage1 : 0|20@1+ (0.1,0) [0|104857.5] "km" AD
|
||||
|
||||
BO_ 931 Vehicle_Mileage1: 8 VCU
|
||||
SG_ Vehicle_Mileage1_MsgCntr : 48|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ Vchicle_AD_Mileage1 : 24|24@1+ (0.1,0) [0|1677721.5] "km" AD
|
||||
SG_ Vehicle_ODO1 : 0|24@1+ (0.1,0) [0|1677721.5] "km" AD
|
||||
|
||||
BO_ 782 VCU_Vehicle_Error_Status: 8 VCU
|
||||
SG_ FlgWarning : 48|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Vehicle_Meter_Soc : 40|8@1+ (1,0) [0|100] "" Vector__XXX
|
||||
SG_ Residual_Pressure : 24|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Charge_Abnormal : 25|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Low_voltage : 16|8@1+ (0.1,0) [0|25.5] "V" Vector__XXX
|
||||
SG_ VCU_PwrSt : 14|2@1+ (1,0) [0|3] "" Vector__XXX
|
||||
SG_ VCU_CAN1_Fault : 37|3@1+ (1,0) [0|7] "" Vector__XXX
|
||||
SG_ VCU_CAN0_Fault : 29|3@1+ (1,0) [0|7] "" Vector__XXX
|
||||
SG_ VCU_EPBActinWhenEBSMF : 12|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ VCU_EEPROM_Fault : 10|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ VCU_EBSActinWhenEPBMF : 11|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ VCU_Error_Code : 0|10@1+ (1,0) [0|999] "" AD
|
||||
|
||||
BO_ 930 Vehicle_Odometer_Status: 8 VCU
|
||||
SG_ Vehicle_Odometer_MsgCntr : 60|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ Vehicle_AD_Mileage : 42|18@1+ (0.8,0) [0|209714.4] "km" AD
|
||||
SG_ Vehicle_Remote_Mileage : 30|12@1+ (0.8,0) [0|3276] "km" AD
|
||||
SG_ Vehicle_TRIP : 18|12@1+ (0.8,0) [0|3276] "km" AD
|
||||
SG_ Vehicle_ODO : 0|18@1+ (0.8,0) [0|209714.4] "km" AD
|
||||
|
||||
BO_ 778 VCU_Vehicle_HVBat_Status: 8 VCU
|
||||
SG_ Vehicle_Poweroff_Channel : 56|4@1+ (1,0) [0|0] "" AD
|
||||
SG_ Vehicle_Poweroff_Countdown_Time : 2|6@1+ (1,0) [0|63] "s" AD
|
||||
SG_ Battery_Work_State : 0|2@1+ (1,0) [0|3] "" AD
|
||||
SG_ Vehicle_Soc : 48|8@1+ (1,0) [0|100] "%" AD
|
||||
SG_ Vehicle_HVBat_MsgCntr : 60|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ High_Voltage_Battery_Voltage : 24|16@1+ (0.1,0) [0|1000] "V" AD
|
||||
SG_ High_Voltage_Battery_MaxTem : 40|8@1+ (1,-40) [-40|210] "℃" AD
|
||||
SG_ High_Voltage_Battery_Current : 8|16@1+ (0.1,-1000) [-1000|1000] "A" AD
|
||||
|
||||
BO_ 777 VCU_RR_Wheel_Status: 8 VCU
|
||||
SG_ RR_WhlSpdSensErr : 56|4@1+ (1,0) [0|15] "" Vector__XXX
|
||||
SG_ RR_Valid_Flag : 2|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ RR_Slip_Flag : 3|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ RR_Sensor_Attr : 0|2@1+ (1,0) [0|3] "" Vector__XXX
|
||||
SG_ RR_Tire_Leak_State : 4|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ RR_Wheel_Status_MsgCntr : 60|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ RR_Tire_Temperature : 48|8@1+ (1,-55) [-55|200] "℃" AD
|
||||
SG_ RR_Tire_Pressure : 16|16@1+ (0.1,0) [0|6553.5] "Kpa" AD
|
||||
SG_ RR_Sensor_state : 7|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ RR_Pressure_Warning : 5|2@1+ (1,0) [0|3] "" AD
|
||||
SG_ RR_WhlSpd : 32|16@1+ (0.01,0) [0|655.35] "km/h" AD
|
||||
SG_ RR_WSS_PulCnt : 8|8@1+ (1,0) [0|255] "" AD
|
||||
|
||||
BO_ 775 VCU_FR_Wheel_Status: 8 VCU
|
||||
SG_ FR_WhlSpdSensErr : 56|4@1+ (1,0) [0|15] "" Vector__XXX
|
||||
SG_ FR_Valid_Flag : 2|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ FR_Slip_Flag : 3|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ FR_Sensor_Attr : 0|2@1+ (1,0) [0|3] "" Vector__XXX
|
||||
SG_ FR_Tire_Leak_State : 4|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ FR_Pressure_Warning : 5|2@1+ (1,0) [0|3] "" AD
|
||||
SG_ FR_Wheel_Status_MsgCntr : 60|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ FR_Tire_Temperature : 48|8@1+ (1,-55) [-55|200] "℃" AD
|
||||
SG_ FR_Tire_Pressure : 16|16@1+ (0.1,0) [0|6553.5] "Kpa" AD
|
||||
SG_ FR_Sensor_state : 7|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ FR_WhlSpd : 32|16@1+ (0.01,0) [0|655.35] "km/h" AD
|
||||
SG_ FR_WSS_PulCnt : 8|8@1+ (1,0) [0|255] "" AD
|
||||
|
||||
BO_ 776 VCU_RL_Wheel_Status: 8 VCU
|
||||
SG_ RL_WhlSpdSensErr : 56|4@1+ (1,0) [0|15] "" Vector__XXX
|
||||
SG_ RL_Valid_Flag : 2|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ RL_Slip_Flag : 3|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ RL_Sensor_Attr : 0|2@1+ (1,0) [0|3] "" Vector__XXX
|
||||
SG_ RL_Pressure_Warning : 5|2@1+ (1,0) [0|3] "" AD
|
||||
SG_ RL_Tire_Temperature : 48|8@1+ (1,-55) [-55|200] "℃" AD
|
||||
SG_ RL_Tire_Pressure : 16|16@1+ (0.1,0) [0|6553.5] "Kpa" AD
|
||||
SG_ RL_Sensor_state : 7|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ RL_Tire_Leak_State : 4|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ RL_WhlSpd : 32|16@1+ (0.01,0) [0|655.35] "km/h" AD
|
||||
SG_ RL_WSS_PulCnt : 8|8@1+ (1,0) [0|255] "" AD
|
||||
SG_ RL_Wheel_Status_MsgCntr : 60|4@1+ (1,0) [0|15] "" AD
|
||||
|
||||
BO_ 774 VCU_FL_Wheel_Status: 8 VCU
|
||||
SG_ FL_WhlSpdSensErr : 56|4@1+ (1,0) [0|15] "" Vector__XXX
|
||||
SG_ FL_Valid_Flag : 2|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ FL_Slip_Flag : 3|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ FL_Sensor_Attr : 0|2@1+ (1,0) [0|3] "" Vector__XXX
|
||||
SG_ FL_Sensor_state : 7|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ FL_Tire_Temperature : 48|8@1+ (1,-55) [-55|200] "℃" AD
|
||||
SG_ FL_Pressure_Warning : 5|2@1+ (1,0) [0|3] "" AD
|
||||
SG_ FL_Tire_Leak_State : 4|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ FL_Tire_Pressure : 16|16@1+ (0.1,0) [0|6553.5] "Kpa" AD
|
||||
SG_ FL_WhlSpd : 32|16@1+ (0.01,0) [0|655.35] "km/h" AD
|
||||
SG_ FL_WSS_PulCnt : 8|8@1+ (1,0) [0|255] "" AD
|
||||
SG_ FL_Wheel_Status_MsgCntr : 60|4@1+ (1,0) [0|15] "" AD
|
||||
|
||||
BO_ 771 VCU_Vehicle_Status_1: 8 VCU
|
||||
SG_ VehicleChargeSts : 56|3@1+ (1,0) [0|7] "" Vector__XXX
|
||||
SG_ Vehicle_BrkConfig : 18|2@1+ (1,0) [0|3] "" Vector__XXX
|
||||
SG_ Vehicle_Range : 8|10@1+ (1,0) [0|1023] "Km" AD
|
||||
SG_ Drive_Mode_State : 4|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ EPB_Status : 3|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Accelerator_Pedal_Status : 40|8@1+ (1,0) [0|100] "%" AD
|
||||
SG_ Brake_Pedal_Status : 32|8@1+ (1,0) [0|100] "%" AD
|
||||
SG_ VCU_303_RollingCounter : 60|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ Vehicle_Gear : 0|2@1+ (1,0) [0|3] "" AD
|
||||
|
||||
BO_ 772 VCU_Vehicle_Status_2: 8 VCU
|
||||
SG_ WhlSpdSensLvlStsRR : 59|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ WhlSpdSensLvlStsRL : 58|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ WhlSpdSensLvlStsFR : 57|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ WhlSpdSensLvlStsFL : 56|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Vehicle_Speed_Vaild : 48|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_304_RollingCounter : 60|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ Vehicle_Steering_Angle : 32|16@1+ (0.1,-35) [-35|35] "deg" AD
|
||||
SG_ Vehicle_Brake_Pressure : 16|16@1+ (0.01,0) [0|10] "Mpa" AD
|
||||
SG_ Vehicle_Speed : 0|16@1+ (0.1,-80) [-80|80] "km/h" AD
|
||||
|
||||
BO_ 769 VCU_Vehicle_Diagnosis: 8 VCU
|
||||
SG_ Tire_Bur_St : 59|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Dropout_Voltage : 58|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ AD_Remote_Break : 57|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Clean_Fan_Flag : 51|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Slippery_slopes_Flag : 50|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Slip_Flag : 49|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_VehCannotPwrOff : 44|2@1+ (1,0) [0|3] "" Vector__XXX
|
||||
SG_ VCU_ADOTAFlag : 39|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Energy_Recovery_Flag : 36|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ Horn_2_State : 43|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_VehRdy : 38|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ ADS_Light_State : 35|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ KERS_Limited : 56|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Oil_pot_State : 4|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Business_Relay_State : 31|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Relay_4G_State : 30|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Radar_Relay_State : 29|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Orin_Relay_State : 28|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Motor_Temp_State : 27|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Fog_Light_state : 11|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Power_Button_State : 2|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ EPB_Button_State : 3|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Motor_Torque_Limit_State : 8|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ B_Press_Switch_Collision_State : 26|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Remo_Touch_Switch_Disable_State : 24|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ F_Press_Switch_Collision_State : 25|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ B_Touch_Switch_Disable_State : 21|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ L_Touch_Switch_Disable_State : 22|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ R_Touch_Switch_Disable_State : 23|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ F_Touch_Switch_Disable_State : 20|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ R_Touch_Switch_Collision_State : 19|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ L_Touch_Switch_Collision_State : 18|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ AD_FaultCode : 52|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ EPB_Diagnosis : 14|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Move_Switch : 47|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ LowBeam_State : 33|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Reversing_Lights_State : 34|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Tire_Sensor_State : 41|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Brake_Light_State : 42|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Vehicle_Fault_Grade : 16|2@1+ (1,0) [0|3] "" AD
|
||||
SG_ EPS_State : 10|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ VCU_301_RollingCounter : 60|4@1+ (1,0) [0|15] "" AD
|
||||
SG_ Horn_1_State : 40|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ HighBeam_State : 15|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Right_Turn_Light_State : 48|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Left_Turn_Light_State : 32|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ B_Touch_Switch_Collision_State : 13|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ F_Touch_Switch_Collision_State : 12|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ BMS_State : 9|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Emergency_Button_State : 0|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ DBS_State : 7|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ AD_State : 6|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Remote_State : 5|1@1+ (1,0) [0|1] "" AD
|
||||
SG_ Motor_State : 1|1@1+ (1,0) [0|1] "" AD
|
||||
|
||||
BO_ 2040 VCU_Version: 8 VCU
|
||||
SG_ CAR_Model : 47|5@1+ (1,0) [0|31] "" Vector__XXX
|
||||
SG_ Day : 56|6@1+ (1,0) [0|31] "日" Vector__XXX
|
||||
SG_ Month : 52|4@1+ (1,0) [0|12] "月" Vector__XXX
|
||||
SG_ Year : 40|7@1+ (1,0) [0|127] "年" Vector__XXX
|
||||
SG_ Byte5 : 32|8@1+ (1,0) [0|255] "" Vector__XXX
|
||||
SG_ Byte4 : 24|8@1+ (1,0) [0|255] "" Vector__XXX
|
||||
SG_ Byte3 : 16|8@1+ (1,0) [0|255] "" Vector__XXX
|
||||
SG_ Byte2 : 8|8@1+ (1,0) [0|255] "" Vector__XXX
|
||||
SG_ Byte1 : 0|8@1+ (1,0) [0|255] "" Vector__XXX
|
||||
|
||||
BO_ 780 VCU_Vehicle_PwrCtrl_Status: 8 VCU
|
||||
SG_ VCU_State_Flag : 28|4@1+ (1,0) [0|15] "" Vector__XXX
|
||||
SG_ VCU_Power_St : 21|3@1+ (1,0) [0|7] "" Vector__XXX
|
||||
SG_ VCU_Init : 32|8@1+ (1,0) [0|255] "" Vector__XXX
|
||||
SG_ Power_Button_State : 15|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_VehKL15Sts : 3|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_MCUKL15Sts : 5|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_BMSKL15Sts : 4|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_BTVersion2 : 48|8@1+ (1,0) [0|255] "" Vector__XXX
|
||||
SG_ VCU_BTVersion1 : 40|8@1+ (1,0) [0|255] "" Vector__XXX
|
||||
SG_ VCU_ABSoftStOld : 19|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_ABSoftStNew : 20|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_CP_Sts : 17|2@1+ (1,0) [0|3] "" Vector__XXX
|
||||
SG_ VCU_OTA_Flag : 16|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_LVMuteChargeSt : 12|3@1+ (1,0) [0|7] "" Vector__XXX
|
||||
SG_ VCU_AbnPwrOff : 11|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_PwrDownSt : 8|3@1+ (1,0) [0|7] "" Vector__XXX
|
||||
SG_ VCU_VehRdyDiagEnable : 7|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
SG_ VCU_LowBattVolt : 56|8@1+ (0.1,0) [6|15] "V" Vector__XXX
|
||||
SG_ VCU_WeakUpSig : 0|3@1+ (1,0) [0|7] "" Vector__XXX
|
||||
|
||||
|
||||
|
||||
CM_ SG_ 768 Vehicle_Steering_Spd "轮端转向角速度";
|
||||
CM_ BO_ 932 "VCU里程计反馈";
|
||||
CM_ SG_ 932 Vehicle_Mileage2_MsgCntr "里程2心跳计数：周期0-15";
|
||||
CM_ SG_ 932 Vehicle_TRIP1 "小计里程（精度1cm）";
|
||||
CM_ SG_ 932 Vehicle_Remote_Mileage1 "遥控模式下的行驶里程（精度0.1）";
|
||||
CM_ BO_ 931 "VCU里程计反馈";
|
||||
CM_ SG_ 931 Vehicle_Mileage1_MsgCntr "里程1心跳计数：周期0——15";
|
||||
CM_ SG_ 931 Vchicle_AD_Mileage1 "车辆自动驾驶行驶里（精度0.1）";
|
||||
CM_ SG_ 931 Vehicle_ODO1 "车辆行驶里程（精度0.1）";
|
||||
CM_ BO_ 782 "VCU车辆故障反馈";
|
||||
CM_ SG_ 782 FlgWarning "警告标志位，给仪表V3以上版本使用，置一时故障码变黄";
|
||||
CM_ SG_ 782 Vehicle_Meter_Soc "仪表校准SOC";
|
||||
CM_ SG_ 782 Residual_Pressure "残压判断
|
||||
0：没有残压
|
||||
1：有残压";
|
||||
CM_ SG_ 782 Charge_Abnormal "充电异常（插枪异常）
|
||||
0：正常充电
|
||||
1：插枪异常";
|
||||
CM_ SG_ 782 Low_voltage "小电池电压";
|
||||
CM_ SG_ 782 VCU_PwrSt "车辆电池状态：
|
||||
0x01：底盘上电，高压不在线
|
||||
0x02：高压在线
|
||||
0x03：车辆下电";
|
||||
CM_ SG_ 782 VCU_CAN0_Fault "0-Active
|
||||
1-TxWarning
|
||||
2-RxWarning
|
||||
3-Warning
|
||||
4-TxPassive
|
||||
5-RxPassive
|
||||
6-Passive
|
||||
7-Bus Off";
|
||||
CM_ SG_ 782 VCU_EPBActinWhenEBSMF "EBS故障，EPB介入标志位";
|
||||
CM_ SG_ 782 VCU_EEPROM_Fault "0x0:成功
|
||||
0x1:失败";
|
||||
CM_ SG_ 782 VCU_EBSActinWhenEPBMF "EPB故障，EBS介入标志位";
|
||||
CM_ SG_ 782 VCU_Error_Code "请查看故障列表";
|
||||
CM_ BO_ 930 "VCU里程计反馈";
|
||||
CM_ SG_ 930 Vehicle_Odometer_MsgCntr "心跳周期0-15";
|
||||
CM_ SG_ 930 Vehicle_AD_Mileage "车辆AD模式下里程";
|
||||
CM_ SG_ 930 Vehicle_Remote_Mileage "车辆遥控模式下里程";
|
||||
CM_ SG_ 930 Vehicle_TRIP "车辆小里程";
|
||||
CM_ SG_ 930 Vehicle_ODO "车辆行驶总里程（km）";
|
||||
CM_ BO_ 778 "VCU高压反馈";
|
||||
CM_ SG_ 778 Vehicle_Poweroff_Channel "0x0：表示当前无下电动作；
|
||||
0x1：表示当前所有系统下电状态；
|
||||
其他状态预留";
|
||||
CM_ SG_ 778 Vehicle_Poweroff_Countdown_Time "延时5秒下电倒计时
|
||||
";
|
||||
CM_ SG_ 778 Battery_Work_State "0x0：未知
|
||||
0x1：充电
|
||||
0x2：能量回收
|
||||
0x3：正常放电";
|
||||
CM_ SG_ 778 Vehicle_Soc "车辆剩余电量%";
|
||||
CM_ SG_ 778 Vehicle_HVBat_MsgCntr "心跳周期0-15";
|
||||
CM_ SG_ 778 High_Voltage_Battery_Voltage "High Voltage Battery Voltage
|
||||
高压电池当前电压";
|
||||
CM_ SG_ 778 High_Voltage_Battery_MaxTem "高压电池最高实时温度";
|
||||
CM_ SG_ 778 High_Voltage_Battery_Current "High Voltage Battery Current
|
||||
高压电池当前电流，正为放电，负为充电";
|
||||
CM_ BO_ 777 "右后轮子状态";
|
||||
CM_ SG_ 777 RR_WhlSpdSensErr "右后轮轮速传感器故障
|
||||
0x00:正常
|
||||
0x01:间隙大于2mm或者静止
|
||||
0x02:看门狗超时
|
||||
0x03:轮速传感器信号不匹配
|
||||
0x04:占空比不匹配
|
||||
0x05:采样错误
|
||||
0x06:阶跃超限
|
||||
0x07:最大速度超限
|
||||
0x08:占空比阶跃
|
||||
0x09:采样信号停滞
|
||||
0x0A:供电故障
|
||||
0x0B:占空比超限";
|
||||
CM_ SG_ 777 RR_Valid_Flag "右后轮轮速信号有效性标志位
|
||||
0：有效
|
||||
1：无效";
|
||||
CM_ SG_ 777 RR_Slip_Flag "右后轮打滑标志位";
|
||||
CM_ SG_ 777 RR_Sensor_Attr "右后轮轮速传感器属性
|
||||
0：不带方向
|
||||
1：带方向";
|
||||
CM_ SG_ 777 RR_Tire_Leak_State "快漏报警标识
|
||||
0：正常
|
||||
1：泄露
|
||||
";
|
||||
CM_ SG_ 777 RR_Wheel_Status_MsgCntr "心跳周期0-15";
|
||||
CM_ SG_ 777 RR_Tire_Temperature "轮胎温度";
|
||||
CM_ SG_ 777 RR_Tire_Pressure "RR轮胎压力";
|
||||
CM_ SG_ 777 RR_Sensor_state "胎压传感器丢失故障标识
|
||||
0：正常
|
||||
1：持续10分钟没有收到轮胎信号
|
||||
";
|
||||
CM_ SG_ 777 RR_Pressure_Warning "压力报警
|
||||
0：正常，
|
||||
1：胎压>=标准气压值*120%，
|
||||
2：胎压<=标准气压值*75%
|
||||
3：爆胎";
|
||||
CM_ SG_ 777 RR_WhlSpd "右后轮轮速";
|
||||
CM_ BO_ 775 "右前车轮状态";
|
||||
CM_ SG_ 775 FR_WhlSpdSensErr "右前轮轮速传感器故障
|
||||
0x00:正常
|
||||
0x01:间隙大于2mm或者静止
|
||||
0x02:看门狗超时
|
||||
0x03:轮速传感器信号不匹配
|
||||
0x04:占空比不匹配
|
||||
0x05:采样错误
|
||||
0x06:阶跃超限
|
||||
0x07:最大速度超限
|
||||
0x08:占空比阶跃
|
||||
0x09:采样信号停滞
|
||||
0x0A:供电故障
|
||||
0x0B:占空比超限";
|
||||
CM_ SG_ 775 FR_Valid_Flag "右前轮轮速信号有效性标志位
|
||||
0：有效
|
||||
1：无效";
|
||||
CM_ SG_ 775 FR_Slip_Flag "右前轮打滑标志位";
|
||||
CM_ SG_ 775 FR_Sensor_Attr "右前轮轮速传感器属性
|
||||
0：不带方向
|
||||
1：带方向";
|
||||
CM_ SG_ 775 FR_Tire_Leak_State "快漏报警标识
|
||||
0：正常
|
||||
1：泄露
|
||||
";
|
||||
CM_ SG_ 775 FR_Pressure_Warning "压力报警
|
||||
0：正常，
|
||||
1：胎压>=标准气压值*120%，
|
||||
2：胎压<=标准气压值*75%
|
||||
3：爆胎";
|
||||
CM_ SG_ 775 FR_Wheel_Status_MsgCntr "心跳周期0-15";
|
||||
CM_ SG_ 775 FR_Tire_Temperature "轮胎温度";
|
||||
CM_ SG_ 775 FR_Tire_Pressure "FR轮胎压力";
|
||||
CM_ SG_ 775 FR_Sensor_state "胎压传感器丢失故障标识
|
||||
0：正常
|
||||
1：持续10分钟没有收到轮胎信号
|
||||
";
|
||||
CM_ SG_ 775 FR_WhlSpd "右前轮轮速";
|
||||
CM_ BO_ 776 "左后轮子状态";
|
||||
CM_ SG_ 776 RL_WhlSpdSensErr "左后轮轮速传感器故障
|
||||
0x00:正常
|
||||
0x01:间隙大于2mm或者静止
|
||||
0x02:看门狗超时
|
||||
0x03:轮速传感器信号不匹配
|
||||
0x04:占空比不匹配
|
||||
0x05:采样错误
|
||||
0x06:阶跃超限
|
||||
0x07:最大速度超限
|
||||
0x08:占空比阶跃
|
||||
0x09:采样信号停滞
|
||||
0x0A:供电故障
|
||||
0x0B:占空比超限";
|
||||
CM_ SG_ 776 RL_Valid_Flag "左后轮轮速信号有效性标志位
|
||||
0：有效
|
||||
1：无效";
|
||||
CM_ SG_ 776 RL_Slip_Flag "左后轮打滑标志位";
|
||||
CM_ SG_ 776 RL_Sensor_Attr "左后轮轮速传感器属性
|
||||
0：不带方向
|
||||
1：带方向";
|
||||
CM_ SG_ 776 RL_Pressure_Warning "压力报警
|
||||
0：正常，
|
||||
1：胎压>=标准气压值*120%，
|
||||
2：胎压<=标准气压值*75%
|
||||
3：爆胎";
|
||||
CM_ SG_ 776 RL_Tire_Temperature "轮胎温度";
|
||||
CM_ SG_ 776 RL_Tire_Pressure "RL轮胎压力";
|
||||
CM_ SG_ 776 RL_Sensor_state "胎压传感器丢失故障标识
|
||||
0：正常
|
||||
1：持续10分钟没有收到轮胎信号
|
||||
";
|
||||
CM_ SG_ 776 RL_Tire_Leak_State "快漏报警标识
|
||||
0：正常
|
||||
1：泄露
|
||||
";
|
||||
CM_ SG_ 776 RL_WhlSpd "左后轮轮速";
|
||||
CM_ SG_ 776 RL_Wheel_Status_MsgCntr "心跳周期0-15";
|
||||
CM_ BO_ 774 "左前车轮状态";
|
||||
CM_ SG_ 774 FL_WhlSpdSensErr "左前轮轮速传感器故障
|
||||
0x00:正常
|
||||
0x01:间隙大于2mm或者静止
|
||||
0x02:看门狗超时
|
||||
0x03:轮速传感器信号不匹配
|
||||
0x04:占空比不匹配
|
||||
0x05:采样错误
|
||||
0x06:阶跃超限
|
||||
0x07:最大速度超限
|
||||
0x08:占空比阶跃
|
||||
0x09:采样信号停滞
|
||||
0x0A:供电故障
|
||||
0x0B:占空比超限";
|
||||
CM_ SG_ 774 FL_Valid_Flag "左前轮轮速信号有效性标志位
|
||||
0：有效
|
||||
1：无效";
|
||||
CM_ SG_ 774 FL_Slip_Flag "左前轮打滑标志位";
|
||||
CM_ SG_ 774 FL_Sensor_Attr "左前轮轮速传感器属性
|
||||
0：不带方向
|
||||
1：带方向";
|
||||
CM_ SG_ 774 FL_Sensor_state "胎压传感器丢失故障标识
|
||||
0：正常
|
||||
1：持续10分钟没有收到轮胎信号
|
||||
";
|
||||
CM_ SG_ 774 FL_Tire_Temperature "轮胎温度";
|
||||
CM_ SG_ 774 FL_Pressure_Warning "压力报警
|
||||
0：正常，
|
||||
1：胎压>=标准气压值*120%，
|
||||
2：胎压<=标准气压值*75%
|
||||
3：爆胎";
|
||||
CM_ SG_ 774 FL_Tire_Leak_State "快漏报警标识
|
||||
0：正常
|
||||
1：泄露
|
||||
";
|
||||
CM_ SG_ 774 FL_Tire_Pressure "FL轮胎压力";
|
||||
CM_ SG_ 774 FL_WhlSpd "左前轮轮速";
|
||||
CM_ SG_ 774 FL_Wheel_Status_MsgCntr "心跳周期0-15";
|
||||
CM_ BO_ 771 "VCU车辆状态反馈";
|
||||
CM_ SG_ 771 VehicleChargeSts "0x00: 非充电状态(比如休眠 放电中 放电故障等)
|
||||
0x01: 充电中
|
||||
0x02 : 加热中
|
||||
0x03 : 边充电边加热中
|
||||
0x04: 充电完成(在充电完成时，检测到充电故障发充电完成)
|
||||
0x05: 充电故障";
|
||||
CM_ SG_ 771 Vehicle_BrkConfig "整车制动类型配置
|
||||
0x0:无效;
|
||||
0x1:DBS配置;
|
||||
0x2:TWOBOX配置;";
|
||||
CM_ SG_ 771 Vehicle_Range "车辆剩余里程";
|
||||
CM_ SG_ 771 Drive_Mode_State "0x0：遥控模式，优先级最二
|
||||
0x1：AD模式，优先级第三
|
||||
0x2：空闲模式，优先级最低（遥控器关机，或者遥控器开机按着A键并且连续10s遥控器没有动作）
|
||||
0x3：故障模式，优先级最高，禁止运行";
|
||||
CM_ SG_ 771 EPB_Status "0x0：EPB反馈释放
|
||||
0x1：EPB反馈加紧";
|
||||
CM_ SG_ 771 Accelerator_Pedal_Status "油门踏板位移量反馈0-100%";
|
||||
CM_ SG_ 771 Brake_Pedal_Status "刹车踏板位移量反馈（0-100%）";
|
||||
CM_ SG_ 771 VCU_303_RollingCounter "心跳周期0-15";
|
||||
CM_ SG_ 771 Vehicle_Gear "车辆挡位状态
|
||||
0：P挡
|
||||
1：D挡
|
||||
2：N挡
|
||||
3：R挡";
|
||||
CM_ BO_ 772 "VCU车辆状态反馈";
|
||||
CM_ SG_ 772 WhlSpdSensLvlStsRR "右后轮速传感器电平信号状态
|
||||
0x0:下降沿
|
||||
0x1:上升沿";
|
||||
CM_ SG_ 772 WhlSpdSensLvlStsRL "左后轮速传感器电平信号状态
|
||||
0x0:下降沿
|
||||
0x1:上升沿";
|
||||
CM_ SG_ 772 WhlSpdSensLvlStsFR "右前轮速传感器电平信号状态
|
||||
0x0:下降沿
|
||||
0x1:上升沿";
|
||||
CM_ SG_ 772 WhlSpdSensLvlStsFL "左前轮速传感器电平信号状态
|
||||
0x0:下降沿
|
||||
0x1:上升沿";
|
||||
CM_ SG_ 772 Vehicle_Speed_Vaild "车辆速度可信标志位
|
||||
0x0:无效
|
||||
0x1:有效";
|
||||
CM_ SG_ 772 VCU_304_RollingCounter "心跳周期0-15";
|
||||
CM_ SG_ 772 Vehicle_Steering_Angle "车辆轮端转角（左正右负），转向角速度（暂定）";
|
||||
CM_ SG_ 772 Vehicle_Brake_Pressure "车辆制动管路压力";
|
||||
CM_ SG_ 772 Vehicle_Speed "车辆速度km/h（负为倒车）";
|
||||
CM_ BO_ 769 "VCU车辆状态反馈";
|
||||
CM_ SG_ 769 Tire_Bur_St "爆胎标志
|
||||
0：无爆胎
|
||||
1：爆胎";
|
||||
CM_ SG_ 769 Dropout_Voltage "电池压差过大标志位";
|
||||
CM_ SG_ 769 AD_Remote_Break "AD模式下遥控器制动介入标志
|
||||
0：未介入
|
||||
1：介入";
|
||||
CM_ SG_ 769 Clean_Fan_Flag "清洁风机状态：
|
||||
0：未开启
|
||||
1：开启";
|
||||
CM_ SG_ 769 Slippery_slopes_Flag "溜坡标志位:
|
||||
0：未溜坡
|
||||
1：溜坡";
|
||||
CM_ SG_ 769 Slip_Flag "打滑标志位：
|
||||
0：未打滑
|
||||
1：打滑
|
||||
";
|
||||
CM_ SG_ 769 VCU_VehCannotPwrOff "无法下电提示
|
||||
0x00:无效
|
||||
0x01:车辆行驶中无法下电
|
||||
0x02:挡位异常无法下电";
|
||||
CM_ SG_ 769 VCU_ADOTAFlag "VCU反馈AD能否OTA
|
||||
0表示不允许OTA，1表示允许OTA";
|
||||
CM_ SG_ 769 Energy_Recovery_Flag "能量回收标志
|
||||
0：未进行能量回收
|
||||
1：正在进行能量回收";
|
||||
CM_ SG_ 769 Horn_2_State "0x0：车载大喇叭关
|
||||
0x1：车载大喇叭开";
|
||||
CM_ SG_ 769 ADS_Light_State "0x0：ADS灯反馈灭
|
||||
0x1：ADS灯亮";
|
||||
CM_ SG_ 769 KERS_Limited "0x0：能量回收无限制
|
||||
0x1：能量回收限制状态";
|
||||
CM_ SG_ 769 Oil_pot_State "制动油液信号
|
||||
0x0：油壶传感器油位正常
|
||||
0x1：不正常";
|
||||
CM_ SG_ 769 Business_Relay_State "业务子系统继电器控制状态
|
||||
0x1：正常连接
|
||||
0x0：不正常连接";
|
||||
CM_ SG_ 769 Relay_4G_State "4G子系统继电器状态
|
||||
0x0：不正常连接
|
||||
0x1：正常连接";
|
||||
CM_ SG_ 769 Radar_Relay_State "雷达子系统继电器状态
|
||||
1：正常连接
|
||||
0：不正常";
|
||||
CM_ SG_ 769 Orin_Relay_State "Orin子系统继电器状态
|
||||
0x0：不正常连接
|
||||
0x1：正常连接";
|
||||
CM_ SG_ 769 Motor_Temp_State "0x0：电机温度正常
|
||||
0x1：电机温度大于150度";
|
||||
CM_ SG_ 769 Fog_Light_state "雾灯状态
|
||||
0x0：雾灯反馈灭
|
||||
0x1：雾灯反馈亮";
|
||||
CM_ SG_ 769 Power_Button_State "电源开关引脚反馈
|
||||
0x0：关断
|
||||
0x1：导通";
|
||||
CM_ SG_ 769 EPB_Button_State "EPB驻车开关
|
||||
0x0：未按下
|
||||
0x1：按下";
|
||||
CM_ SG_ 769 Motor_Torque_Limit_State "0x0：加速性能正常
|
||||
0x1：加速性能受限（电机限扭）";
|
||||
CM_ SG_ 769 B_Press_Switch_Collision_State "0x0：后压力波开关未触碰
|
||||
0x1：后压力波开关触碰";
|
||||
CM_ SG_ 769 Remo_Touch_Switch_Disable_State "0x0：遥控模式下触碰条未禁用
|
||||
0x1：遥控模式下触碰条禁用";
|
||||
CM_ SG_ 769 F_Press_Switch_Collision_State "0x0：前压力波开关未触碰
|
||||
0x1：前压力波开关触碰";
|
||||
CM_ SG_ 769 B_Touch_Switch_Disable_State "0x0：后触碰条未禁用
|
||||
0x1：后触碰条禁用";
|
||||
CM_ SG_ 769 L_Touch_Switch_Disable_State "0x0：左触碰条未禁用
|
||||
0x1：左触碰条禁用";
|
||||
CM_ SG_ 769 R_Touch_Switch_Disable_State "0x0：右触碰条未禁用
|
||||
0x1：右触碰条禁用";
|
||||
CM_ SG_ 769 F_Touch_Switch_Disable_State "0x0：前触碰条未禁用
|
||||
0x1：前触碰条禁用";
|
||||
CM_ SG_ 769 R_Touch_Switch_Collision_State "0x0：右触碰条未触发
|
||||
0x1：右触碰条触发";
|
||||
CM_ SG_ 769 L_Touch_Switch_Collision_State "0x0：左触碰条未触碰
|
||||
0x1：左触碰条触发";
|
||||
CM_ SG_ 769 AD_FaultCode "进入AD模式错误原因
|
||||
0x0：正常
|
||||
0x1：底盘插着充电枪，禁止进入AD";
|
||||
CM_ SG_ 769 EPB_Diagnosis "0x0：EPB无故障
|
||||
0x1：EPB有故障";
|
||||
CM_ SG_ 769 Move_Switch "0x0：未处于挪车模式
|
||||
0x1：挪车模式";
|
||||
CM_ SG_ 769 LowBeam_State "0x0：近光灯灭
|
||||
0x1：近光灯亮";
|
||||
CM_ SG_ 769 Reversing_Lights_State "0x0：倒车灯反馈灭
|
||||
0x1：倒车灯反馈亮";
|
||||
CM_ SG_ 769 Tire_Sensor_State "0x0：胎压传感器控制盒信号正常
|
||||
0x1：胎压传感器控制盒信号丢失";
|
||||
CM_ SG_ 769 Brake_Light_State "0x0：制动灯反馈灭
|
||||
0x1：制动灯反馈亮";
|
||||
CM_ SG_ 769 Vehicle_Fault_Grade "整车故障等级0、1、2、3级故障
|
||||
0级为无故障
|
||||
1级故障 不影响车辆行驶
|
||||
2级故障 速度环限制车速最高5km/h，扭矩环不做限制
|
||||
3级为最高级故障，车辆禁止行驶
|
||||
";
|
||||
CM_ SG_ 769 EPS_State "0x0：EPS无故障
|
||||
0x1：EPS有故障";
|
||||
CM_ SG_ 769 VCU_301_RollingCounter "心跳周期0-15";
|
||||
CM_ SG_ 769 Horn_1_State "0x0：低速报警器内置喇叭关
|
||||
0x1：低速报警器内置喇叭开";
|
||||
CM_ SG_ 769 HighBeam_State "0x0：远光灯反馈灭
|
||||
0x1：远光灯反馈亮";
|
||||
CM_ SG_ 769 Right_Turn_Light_State "0x0：右转灯灭
|
||||
0x1：右转灯亮";
|
||||
CM_ SG_ 769 Left_Turn_Light_State "0x0：左转灯灭
|
||||
0x1：左转灯亮";
|
||||
CM_ SG_ 769 B_Touch_Switch_Collision_State "0x0：后触碰条未触碰
|
||||
0x1：后触碰条触发";
|
||||
CM_ SG_ 769 F_Touch_Switch_Collision_State "0x0：前触碰条未触碰
|
||||
0x1：前触碰条触发";
|
||||
CM_ SG_ 769 BMS_State "0x0：BMS正常
|
||||
0x1：BMS信号丢失或有故障";
|
||||
CM_ SG_ 769 Emergency_Button_State "0x0：急停按钮未按下
|
||||
0x1：急停按钮按下";
|
||||
CM_ SG_ 769 DBS_State "1表示有故障，0表示无故障";
|
||||
CM_ SG_ 769 AD_State "0x0：自驾使能反馈
|
||||
0x1：自驾未使能反馈";
|
||||
CM_ SG_ 769 Remote_State "0x0：遥控器未连接
|
||||
0x1：遥控器已连接";
|
||||
CM_ SG_ 769 Motor_State "0x0：MCU无故障
|
||||
0x1：MCU故障";
|
||||
CM_ BO_ 2040 "VCU版本号";
|
||||
CM_ SG_ 2040 CAR_Model "车辆英文代号：车辆英文代号1-26表示A-Z";
|
||||
CM_ SG_ 2040 Day "dd";
|
||||
CM_ SG_ 2040 Month "MM";
|
||||
CM_ SG_ 2040 Year "yyyy";
|
||||
CM_ BO_ 780 "VCU电源管理反馈";
|
||||
CM_ SG_ 780 VCU_State_Flag "VCU状态标志位";
|
||||
CM_ SG_ 780 VCU_Power_St "VCU当前处于的状态：
|
||||
0、关机状态
|
||||
1、开机状态
|
||||
2、开机充电状态
|
||||
3、关机充电状态";
|
||||
CM_ SG_ 780 VCU_Init "VCU刷写完后开机次数";
|
||||
CM_ SG_ 780 Power_Button_State "电源开关引脚反馈
|
||||
0x0：关断
|
||||
0x1：导通";
|
||||
CM_ SG_ 780 VCU_VehKL15Sts "整车KL15继电器状态
|
||||
0x00:无效
|
||||
0x01:硬线唤醒";
|
||||
CM_ SG_ 780 VCU_MCUKL15Sts "MCU唤醒电源状态
|
||||
0x00:无效
|
||||
0x01:硬线唤醒";
|
||||
CM_ SG_ 780 VCU_BMSKL15Sts "BMS唤醒电源状态
|
||||
0x00:无效
|
||||
0x01:硬线唤醒";
|
||||
CM_ SG_ 780 VCU_BTVersion2 "BT小版本号，此为十进制，需要转16进制读取，24年8月份BT版本为52，即为0x34，故版本号为3.4";
|
||||
CM_ SG_ 780 VCU_BTVersion1 "BT大版本号，十进制直接读取，24年8月份为1";
|
||||
CM_ SG_ 780 VCU_ABSoftStOld "置1时表示该VCU未AB备份。TBOX不允许升级VCU以外的程序";
|
||||
CM_ SG_ 780 VCU_ABSoftStNew "置1时表示该VCU已AB备份。TBOX不允许升级VCU-AB程序以外的程序";
|
||||
CM_ SG_ 780 VCU_CP_Sts "车辆插枪判断：
|
||||
0：未插枪
|
||||
1：插枪
|
||||
2：预留";
|
||||
CM_ SG_ 780 VCU_OTA_Flag "VCU_OTA的标志：
|
||||
0：未升级
|
||||
1：即将进行升级";
|
||||
CM_ SG_ 780 VCU_LVMuteChargeSt "0x00:未补电
|
||||
0x01:补电进行中
|
||||
0x02:补电完成
|
||||
0x03:补电条件未满足
|
||||
0x04:无需补电";
|
||||
CM_ SG_ 780 VCU_AbnPwrOff "0x0：前一次下电为正常下电
|
||||
0x1：前一次下电为异常电源断开";
|
||||
CM_ SG_ 780 VCU_PwrDownSt "0x00:电源管理无操作
|
||||
0x01:关机进行中
|
||||
0x02:重启进行中";
|
||||
CM_ SG_ 780 VCU_VehRdyDiagEnable "车辆自检使能";
|
||||
CM_ SG_ 780 VCU_LowBattVolt "VCU AI28采集BATT电压结果";
|
||||
CM_ SG_ 780 VCU_WeakUpSig "0x01:Tbox远程唤醒
|
||||
0x02:按键唤醒
|
||||
0x03:未知原因（可能为未知CAN报文唤醒）
|
||||
0x04:OBC唤醒
|
||||
0x05:铅酸电池补电模式唤醒
|
||||
0x06:补电过程中开机唤醒
|
||||
0x07:补电过程中充电枪唤醒
|
||||
";
|
||||
BA_DEF_ SG_ "GenSigSendType" ENUM "Cyclic","OnWrite","OnWriteWithRepetition","OnChange","OnChangeWithRepetition","IfActive","IfActiveWithRepetition","NoSigSendType";
|
||||
BA_DEF_ SG_ "GenSigInactiveValue" INT 0 0;
|
||||
BA_DEF_ BO_ "GenMsgCycleTime" INT 0 0;
|
||||
BA_DEF_ BO_ "GenMsgSendType" ENUM "Cyclic","not_used","not_used","not_used","not_used","Cyclic","not_used","IfActive","NoMsgSendType";
|
||||
BA_DEF_ BU_ "NmStationAddress" HEX 0 0;
|
||||
BA_DEF_ "DBName" STRING ;
|
||||
BA_DEF_ "BusType" STRING ;
|
||||
BA_DEF_DEF_ "GenSigSendType" "Cyclic";
|
||||
BA_DEF_DEF_ "GenSigInactiveValue" 0;
|
||||
BA_DEF_DEF_ "GenMsgCycleTime" 0;
|
||||
BA_DEF_DEF_ "GenMsgSendType" "NoMsgSendType";
|
||||
BA_DEF_DEF_ "NmStationAddress" 0;
|
||||
BA_DEF_DEF_ "DBName" "";
|
||||
BA_DEF_DEF_ "BusType" "CAN";
|
||||
BA_ "DBName" "W2_CAN2_VCU";
|
||||
BA_ "GenMsgCycleTime" BO_ 768 10;
|
||||
BA_ "GenMsgSendType" BO_ 768 0;
|
||||
BA_ "GenMsgSendType" BO_ 932 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 932 1000;
|
||||
BA_ "GenMsgCycleTime" BO_ 931 1000;
|
||||
BA_ "GenMsgSendType" BO_ 931 0;
|
||||
BA_ "GenMsgSendType" BO_ 782 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 782 500;
|
||||
BA_ "GenMsgSendType" BO_ 930 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 930 1000;
|
||||
BA_ "GenMsgSendType" BO_ 778 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 778 20;
|
||||
BA_ "GenMsgSendType" BO_ 777 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 777 20;
|
||||
BA_ "GenMsgSendType" BO_ 775 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 775 20;
|
||||
BA_ "GenMsgSendType" BO_ 776 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 776 20;
|
||||
BA_ "GenMsgSendType" BO_ 774 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 774 20;
|
||||
BA_ "GenMsgSendType" BO_ 771 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 771 20;
|
||||
BA_ "GenMsgSendType" BO_ 772 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 772 10;
|
||||
BA_ "GenMsgSendType" BO_ 769 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 769 20;
|
||||
BA_ "GenMsgSendType" BO_ 2040 0;
|
||||
BA_ "GenMsgCycleTime" BO_ 2040 1000;
|
||||
BA_ "GenMsgCycleTime" BO_ 780 50;
|
||||
BA_ "GenMsgSendType" BO_ 780 0;
|
||||
|
||||
BIN
Binary file not shown.
+29
@@ -0,0 +1,29 @@
|
||||
序号,消息定义,数据类型,话题名称,对应autoware消息类型,易咖智车对应CAN报文,易咖对应CAN消息,尚元智行对应CAN报文,尚元智行对应CAN消息
|
||||
autoware需要底盘反馈的消息,,,,,,,,,
|
||||
1,控制模式反馈,int,/vehicle/status/control_mode,autoware_vehicle_msgs::msg::ControlModeReport,CDCU_VehState,CDCU_Veh_RunMode,VCU_Vehicle_Status_1,Drive_Mode_State,
|
||||
2,车辆纵向速度反馈,float,/vehicle/status/velocity_status,autoware_vehicle_msgs::msg::VelocityReport,CDCU_VehDyncState,CDCU_Veh_LongtdnalSpd,VCU_Vehicle_Status_2,Vehicle_Speed,
|
||||
3,车轮转角反馈,float,/vehicle/status/steering_status,autoware_vehicle_msgs::msg::SteeringReport,CDCU_SteerStatus,CDCU_EPS_StrWhlAngle,VCU_Vehicle_Status_2,Vehicle_Steering_Angle,
|
||||
4,实际挡位反馈,int,/vehicle/status/gear_status,autoware_vehicle_msgs::msg::GearReport,CDCU_DriveStatus,CDCU_MCU_GearAct,VCU_Vehicle_Status_1,Vehicle_Gear,
|
||||
5,踏板反馈（包括油门和刹车）,float,/vehicle/status/actuation_status,tier4_vehicle_msgs::msg::ActuationStatusStamped,"CDCU_DriveStatus
|
||||
CDCU_BrakeStatus","CDCU_MCU_ThrotAct
|
||||
CDCU_EHB_BrkPedpos",VCU_Vehicle_Status_1,"Accelerator_Pedal_Status
|
||||
Brake_Pedal_Status",
|
||||
,,,,,,,,,
|
||||
autoware给底盘发送的消息,,,,,,,,,
|
||||
1,控制模式,int,/control/control_mode_request,autoware_vehicle_msgs::srv::ControlModeCommand,"易咖智车需要ADCU_BrakeCmd、ADCU_ParkCmd、
|
||||
ADCU_SteerCmd、 ADCU_DriveCmd、ADCU_BodyCmd、ADCU_PowerCmd
|
||||
六帧自动驾驶指令报文必须按协议要求周期性发送，并且报文
|
||||
ADCU_BrakeCmd、ADCU_ParkCmd、ADCU_SteerCmd、ADCU_DriveCmd 中
|
||||
的激活信号 ADCU_Brk_Active、ADCU_Prk_Active、ADCU_Str_Active、
|
||||
ADCU_Drv_Active 必须在 500ms 时间窗内均完成上升沿触发，则底盘进入自动
|
||||
驾驶模式。",,AD_Control_Accelerate,AD_Accelerate_Valid置一则进入智驾模式,
|
||||
2,控制指令(包括油门/刹车/转向),float,/control/command/actuation_cmd,tier4_vehicle_msgs::msg::ActuationCommandStamped,"ADCU_DriveCmd
|
||||
ADCU_BrakeCmd
|
||||
ADCU_SteerCmd","ADCU_Drv_TgtPedpos
|
||||
ADCU_Str_TgtAngle
|
||||
ADCU_Brk_TgtPedpos","AD_Control_Accelerate
|
||||
AD_Control_Brake
|
||||
AD_Control_Steering","AD_Torque_Control
|
||||
AD_BrakePressure_Req
|
||||
AD_Steering_Angle_Cmd",
|
||||
3,挡位指令,int,/control/command/gear_cmd,autoware_vehicle_msgs::msg::GearCommand,ADCU_DriveCmd,ADCU_Drv_TgtGear,AD_Control_Accelerate,AD_Accelerate_Gear
|
||||
|
BIN
Binary file not shown.
BIN
Binary file not shown.
+57
@@ -0,0 +1,57 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(teemo_chassis_drive)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(can_msgs REQUIRED)
|
||||
find_package(teemo_chassis_msgs REQUIRED)
|
||||
find_package(autoware_vehicle_msgs REQUIRED)
|
||||
find_package(tier4_vehicle_msgs REQUIRED)
|
||||
|
||||
include_directories(include)
|
||||
|
||||
add_library(dbc_codec STATIC src/dbc_codec.cpp)
|
||||
target_include_directories(dbc_codec PUBLIC include)
|
||||
|
||||
add_executable(teemo_chassis_receiver src/receiver_node.cpp)
|
||||
target_link_libraries(teemo_chassis_receiver dbc_codec)
|
||||
ament_target_dependencies(
|
||||
teemo_chassis_receiver
|
||||
rclcpp
|
||||
can_msgs
|
||||
teemo_chassis_msgs
|
||||
autoware_vehicle_msgs
|
||||
tier4_vehicle_msgs
|
||||
)
|
||||
|
||||
add_executable(teemo_chassis_sender src/sender_node.cpp)
|
||||
target_link_libraries(teemo_chassis_sender dbc_codec)
|
||||
ament_target_dependencies(
|
||||
teemo_chassis_sender
|
||||
rclcpp
|
||||
can_msgs
|
||||
teemo_chassis_msgs
|
||||
autoware_vehicle_msgs
|
||||
tier4_vehicle_msgs
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
teemo_chassis_receiver
|
||||
teemo_chassis_sender
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(PROGRAMS
|
||||
scripts/autoware_test.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+109
@@ -0,0 +1,109 @@
|
||||
teemo_chassis_receiver:
|
||||
ros__parameters:
|
||||
input_topic: /kvaser/can_tx
|
||||
status_topic_prefix: status
|
||||
use_legacy_status_topic_names: false
|
||||
input_qos_reliability: best_effort
|
||||
input_qos_depth: 100
|
||||
log_runtime_stats: true
|
||||
log_runtime_stats_period_ms: 1000
|
||||
publish_autoware_status: true
|
||||
autoware_frame_id: base_link
|
||||
autoware_control_mode_topic: /vehicle/status/control_mode
|
||||
autoware_velocity_topic: /vehicle/status/velocity_status
|
||||
autoware_steering_topic: /vehicle/status/steering_status
|
||||
autoware_gear_topic: /vehicle/status/gear_status
|
||||
autoware_actuation_status_topic: /vehicle/status/actuation_status
|
||||
# TEEMO Drive_Mode_State value that means autonomous mode.
|
||||
autoware_autonomous_drive_mode_state: 1
|
||||
# 1.0 means Vehicle_Steering_Angle is already tire angle in degrees.
|
||||
# If the signal is confirmed as steering-wheel angle, set this to the steering ratio.
|
||||
steering_status_ratio: 1.0
|
||||
pedal_status_scale: 100.0
|
||||
|
||||
teemo_chassis_sender:
|
||||
ros__parameters:
|
||||
output_topic: /kvaser/can_rx
|
||||
command_topic: control/vehicle_command
|
||||
accelerate_command_topic: control/AD_Control_Accelerate
|
||||
brake_command_topic: control/AD_Control_Brake
|
||||
steering_command_topic: control/AD_Control_Steering
|
||||
command_timeout_ms: 500
|
||||
autoware_mode: true
|
||||
actuation_command_topic: /control/command/actuation_cmd
|
||||
gear_command_topic: /control/command/gear_cmd
|
||||
control_mode_service: /control/control_mode_request
|
||||
# 1.0 means AD_Steering_Angle_Cmd expects tire angle in degrees.
|
||||
# If the chassis expects steering-wheel angle, set this to the steering ratio.
|
||||
steering_command_ratio: 1.0
|
||||
autoware_accel_scale: 100.0
|
||||
autoware_brake_scale: 100.0
|
||||
|
||||
AD_OTAReq: 0
|
||||
|
||||
AD_Clean_Fan: false
|
||||
AD_MoveModeReq: 0
|
||||
ad_avh_active_cmd: false
|
||||
ad_fault_handling_status: 0
|
||||
ad_vehicle_weight: 0
|
||||
can_sign_tran_state: false
|
||||
AD_Release_R_Bumper: false
|
||||
AD_Release_L_Bumper: false
|
||||
AD_Disable_R_Bumper: false
|
||||
AD_Disable_L_Bumper: false
|
||||
AD_Disable_B_Bumper: false
|
||||
AD_Disable_F_Bumper: false
|
||||
AD_Clear_TRIP: false
|
||||
AD_Release_B_Bumper: false
|
||||
AD_Release_F_Bumper: false
|
||||
AD_Release_Emergency_Button: false
|
||||
|
||||
reserved_sys_power_control: 0
|
||||
Vehicle_Work_Mode_Control: 0
|
||||
radar_sys_power_control: 0
|
||||
business_sys_power_control: 0
|
||||
network_sys_power_control: 0
|
||||
ad_sys_power_control: 0
|
||||
|
||||
AD_PwrDownCountInit: 0
|
||||
AD_Vehicle_PwrReq: 0
|
||||
|
||||
AD_VehLightSwtEmergFreq: 0
|
||||
AD_Horn_2_Control: false
|
||||
AD_Fog_Light: false
|
||||
AD_Body_Valid: 0
|
||||
AD_Low_Beam: false
|
||||
AD_Reversing_Lights: false
|
||||
AD_Double_Flash_Light: false
|
||||
AD_Brake_Light: false
|
||||
AD_Horn_1_Control: false
|
||||
AD_High_Beam: false
|
||||
AD_Right_Turn_Light: false
|
||||
AD_Left_Turn_Light: false
|
||||
|
||||
# AD_Control_Accelerate (0x1D2)
|
||||
# Set to 1 to enable AD drive.
|
||||
AD_Accelerate_Valid: 1
|
||||
# 0:torque mode, 1: speed mode, 2: acceleration mode
|
||||
AD_Accelerate_Work_Mode: 0
|
||||
# Gear command 0:P档 1:D档 2:N档 3:R档
|
||||
AD_Accelerate_Gear: 1
|
||||
# Speed request when AD_Accelerate_Work_Mode=1 (km/h)
|
||||
AD_Speed_Req: 0.0
|
||||
# Acceleration request when AD_Accelerate_Work_Mode=2 (m/ss)
|
||||
AD_Accelerate_Req: 0.0
|
||||
AD_Torque_Pedal: 0 # 踏板开度
|
||||
AD_Energy_Recovery: false
|
||||
|
||||
# AD_Control_Brake (0x1D3)
|
||||
AD_DBS_WorkMode: false
|
||||
AD_AWSC_Flag: false
|
||||
# Brake command (0x100)
|
||||
AD_BrakePressure_Req: 0 # 制动开度
|
||||
AD_DBS_Valid: 1
|
||||
|
||||
# AD_Control_Steering (0x1D4)
|
||||
AD_Steering_Speed_Cmd: 5.0
|
||||
# Steering angle command range: [-90.0, 90.0]
|
||||
AD_Steering_Angle_Cmd: 0.0
|
||||
AD_Steering_Valid: 1
|
||||
+150
@@ -0,0 +1,150 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
namespace teemo_chassis_drive
|
||||
{
|
||||
|
||||
enum class ValueKind { Bool, Int, Float };
|
||||
|
||||
struct SignalDef
|
||||
{
|
||||
std::string field_name;
|
||||
int start_bit;
|
||||
int length;
|
||||
ValueKind kind;
|
||||
double factor{1.0};
|
||||
double offset{0.0};
|
||||
bool is_signed{false};
|
||||
std::optional<double> minimum{};
|
||||
std::optional<double> maximum{};
|
||||
};
|
||||
|
||||
using Payload = std::array<uint8_t, 8>;
|
||||
|
||||
// ── decode ────────────────────────────────────────────────────────────────────
|
||||
|
||||
inline Payload normalize(const std::vector<uint8_t> & data)
|
||||
{
|
||||
Payload p{};
|
||||
for (size_t i = 0; i < 8 && i < data.size(); ++i) p[i] = data[i];
|
||||
return p;
|
||||
}
|
||||
|
||||
inline double decode_signal(const Payload & p, const SignalDef & s)
|
||||
{
|
||||
uint64_t packed = 0;
|
||||
for (int i = 7; i >= 0; --i) packed = (packed << 8) | p[i]; // little-endian
|
||||
|
||||
uint64_t mask = (s.length == 64) ? UINT64_MAX : ((uint64_t(1) << s.length) - 1);
|
||||
uint64_t raw = (packed >> s.start_bit) & mask;
|
||||
|
||||
int64_t signed_raw = static_cast<int64_t>(raw);
|
||||
if (s.is_signed) {
|
||||
uint64_t sign_bit = uint64_t(1) << (s.length - 1);
|
||||
if (raw & sign_bit) signed_raw = static_cast<int64_t>(raw) - static_cast<int64_t>(sign_bit << 1);
|
||||
}
|
||||
|
||||
if (s.kind == ValueKind::Bool) return (signed_raw != 0) ? 1.0 : 0.0;
|
||||
return static_cast<double>(signed_raw) * s.factor + s.offset;
|
||||
}
|
||||
|
||||
// ── encode ────────────────────────────────────────────────────────────────────
|
||||
|
||||
inline int64_t physical_to_raw(double value, const SignalDef & s)
|
||||
{
|
||||
if (s.kind == ValueKind::Bool) return value != 0.0 ? 1 : 0;
|
||||
|
||||
if (s.minimum && value < *s.minimum) value = *s.minimum;
|
||||
if (s.maximum && value > *s.maximum) value = *s.maximum;
|
||||
|
||||
int64_t raw = static_cast<int64_t>(std::round((value - s.offset) / s.factor));
|
||||
|
||||
if (s.is_signed) {
|
||||
int64_t lo = -(int64_t(1) << (s.length - 1));
|
||||
int64_t hi = (int64_t(1) << (s.length - 1)) - 1;
|
||||
raw = std::max(lo, std::min(hi, raw));
|
||||
if (raw < 0) raw += (int64_t(1) << s.length);
|
||||
} else {
|
||||
int64_t hi = (int64_t(1) << s.length) - 1;
|
||||
raw = std::max(int64_t(0), std::min(hi, raw));
|
||||
}
|
||||
return raw;
|
||||
}
|
||||
|
||||
inline Payload encode_signal(Payload p, const SignalDef & s, double value)
|
||||
{
|
||||
uint64_t packed = 0;
|
||||
for (int i = 7; i >= 0; --i) packed = (packed << 8) | p[i];
|
||||
|
||||
uint64_t mask = (s.length == 64) ? UINT64_MAX : ((uint64_t(1) << s.length) - 1);
|
||||
uint64_t raw = static_cast<uint64_t>(physical_to_raw(value, s));
|
||||
packed = (packed & ~(mask << s.start_bit)) | ((raw & mask) << s.start_bit);
|
||||
|
||||
for (int i = 0; i < 8; ++i) { p[i] = packed & 0xFF; packed >>= 8; }
|
||||
return p;
|
||||
}
|
||||
|
||||
inline uint8_t compute_xor_checksum(const Payload & p)
|
||||
{
|
||||
uint8_t cs = 0;
|
||||
for (int i = 0; i < 7; ++i) cs ^= p[i];
|
||||
return cs;
|
||||
}
|
||||
|
||||
// ── frame definitions ─────────────────────────────────────────────────────────
|
||||
|
||||
struct RxFrameDef
|
||||
{
|
||||
uint32_t can_id;
|
||||
std::string name;
|
||||
std::string topic_suffix;
|
||||
std::string msg_type;
|
||||
std::vector<SignalDef> signals;
|
||||
std::vector<std::pair<std::string, std::string>> static_fields{}; // field -> value
|
||||
};
|
||||
|
||||
struct TxFrameDef
|
||||
{
|
||||
uint32_t can_id;
|
||||
std::string name;
|
||||
int cycle_ms;
|
||||
std::vector<SignalDef> signals;
|
||||
};
|
||||
|
||||
// Rolling counter and checksum signals (shared by all TX frames)
|
||||
static const SignalDef ROLLING_COUNTER_SIG{"ad_rolling_counter", 52, 4, ValueKind::Int, 1.0, 0.0, false, 0.0, 15.0};
|
||||
static const SignalDef CHECKSUM_SIG{"ad_check_sum", 56, 8, ValueKind::Int, 1.0, 0.0, false, 0.0, 255.0};
|
||||
|
||||
inline Payload encode_tx_frame(
|
||||
const TxFrameDef & def,
|
||||
const std::unordered_map<std::string, double> & values,
|
||||
int rolling_counter)
|
||||
{
|
||||
Payload p{};
|
||||
for (const auto & sig : def.signals) {
|
||||
auto it = values.find(sig.field_name);
|
||||
p = encode_signal(p, sig, it != values.end() ? it->second : 0.0);
|
||||
}
|
||||
p = encode_signal(p, ROLLING_COUNTER_SIG, static_cast<double>(rolling_counter & 0x0F));
|
||||
p = encode_signal(p, CHECKSUM_SIG, 0.0);
|
||||
p = encode_signal(p, CHECKSUM_SIG, static_cast<double>(compute_xor_checksum(p)));
|
||||
return p;
|
||||
}
|
||||
|
||||
// ── RX frame table ────────────────────────────────────────────────────────────
|
||||
|
||||
const std::unordered_map<uint32_t, RxFrameDef> & rx_frame_definitions();
|
||||
|
||||
// ── TX frame table ────────────────────────────────────────────────────────────
|
||||
|
||||
const std::vector<TxFrameDef> & tx_frame_definitions();
|
||||
|
||||
} // namespace teemo_chassis_drive
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.substitutions import PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
fastdds_builtin_transports = LaunchConfiguration("fastdds_builtin_transports")
|
||||
config_path = PathJoinSubstitution(
|
||||
[FindPackageShare("teemo_chassis_drive"), "config", "teemo_chassis.yaml"]
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument(
|
||||
"fastdds_builtin_transports",
|
||||
default_value="DEFAULT",
|
||||
description=(
|
||||
"Fast DDS builtin transports for this launch. "
|
||||
"Use DEFAULT for normal ROS 2 discovery; "
|
||||
"use SHM only when you explicitly want same-host shared-memory-only transport."
|
||||
),
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
"FASTDDS_BUILTIN_TRANSPORTS", fastdds_builtin_transports
|
||||
),
|
||||
Node(
|
||||
package="teemo_chassis_drive",
|
||||
executable="teemo_chassis_receiver",
|
||||
name="teemo_chassis_receiver",
|
||||
output="screen",
|
||||
parameters=[config_path],
|
||||
),
|
||||
Node(
|
||||
package="teemo_chassis_drive",
|
||||
executable="teemo_chassis_sender",
|
||||
name="teemo_chassis_sender",
|
||||
output="screen",
|
||||
parameters=[config_path],
|
||||
),
|
||||
]
|
||||
)
|
||||
+25
@@ -0,0 +1,25 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>teemo_chassis_drive</name>
|
||||
<version>0.1.0</version>
|
||||
<description>DBC-based ROS 2 CAN driver for the TEEMO chassis.</description>
|
||||
|
||||
<maintainer email="codex@example.com">Codex</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>can_msgs</depend>
|
||||
<depend>teemo_chassis_msgs</depend>
|
||||
<depend>autoware_vehicle_msgs</depend>
|
||||
<depend>tier4_vehicle_msgs</depend>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+159
@@ -0,0 +1,159 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Publish Autoware-style control commands for TEEMO chassis driver testing."""
|
||||
|
||||
import math
|
||||
|
||||
import rclpy
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
|
||||
from autoware_vehicle_msgs.msg import GearCommand
|
||||
from autoware_vehicle_msgs.srv import ControlModeCommand
|
||||
from tier4_vehicle_msgs.msg import ActuationCommandStamped
|
||||
|
||||
|
||||
class AutowareTestNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__("teemo_autoware_test")
|
||||
|
||||
self.actuation_topic = self.declare_parameter(
|
||||
"actuation_topic", "/control/command/actuation_cmd"
|
||||
).value
|
||||
self.gear_topic = self.declare_parameter(
|
||||
"gear_topic", "/control/command/gear_cmd"
|
||||
).value
|
||||
self.control_mode_service = self.declare_parameter(
|
||||
"control_mode_service", "/control/control_mode_request"
|
||||
).value
|
||||
self.frame_id = self.declare_parameter("frame_id", "base_link").value
|
||||
self.rate_hz = float(self.declare_parameter("rate_hz", 50.0).value)
|
||||
self.accel_cmd = float(self.declare_parameter("accel_cmd", 0.04).value)
|
||||
self.brake_cmd = float(self.declare_parameter("brake_cmd", 0.0).value)
|
||||
self.steer_cmd = float(self.declare_parameter("steer_cmd", 0.0).value)
|
||||
# "neutral":
|
||||
# "drive":
|
||||
# "reverse":
|
||||
# "park":
|
||||
# "parking"
|
||||
self.gear_name = str(self.declare_parameter("gear", "reverse").value).lower()
|
||||
self.autonomous_delay = float(
|
||||
self.declare_parameter("autonomous_delay_sec", 1.0).value
|
||||
)
|
||||
|
||||
if self.rate_hz <= 0.0:
|
||||
self.rate_hz = 50.0
|
||||
|
||||
self.pub_actuation = self.create_publisher(
|
||||
ActuationCommandStamped, self.actuation_topic, 10
|
||||
)
|
||||
self.pub_gear = self.create_publisher(GearCommand, self.gear_topic, 10)
|
||||
self.control_mode_client = self.create_client(
|
||||
ControlModeCommand, self.control_mode_service
|
||||
)
|
||||
|
||||
self.publish_timer = self.create_timer(1.0 / self.rate_hz, self.publish_commands)
|
||||
self.mode_timer = self.create_timer(self.autonomous_delay, self.request_autonomous)
|
||||
|
||||
self.tick_count = 0
|
||||
self.autonomous_requested = False
|
||||
|
||||
self.get_logger().info(
|
||||
"Publishing actuation to %s, gear to %s, control mode service %s"
|
||||
% (self.actuation_topic, self.gear_topic, self.control_mode_service)
|
||||
)
|
||||
self.get_logger().info(
|
||||
"Command: accel=%.3f brake=%.3f steer=%.3f rad gear=%s rate=%.1f Hz"
|
||||
% (self.accel_cmd, self.brake_cmd, self.steer_cmd, self.gear_name, self.rate_hz)
|
||||
)
|
||||
|
||||
def publish_commands(self):
|
||||
now = self.get_clock().now().to_msg()
|
||||
|
||||
actuation = ActuationCommandStamped()
|
||||
actuation.header.stamp = now
|
||||
actuation.header.frame_id = self.frame_id
|
||||
actuation.actuation.accel_cmd = self._finite_or_zero(self.accel_cmd)
|
||||
actuation.actuation.brake_cmd = self._finite_or_zero(self.brake_cmd)
|
||||
actuation.actuation.steer_cmd = self._finite_or_zero(self.steer_cmd)
|
||||
self.pub_actuation.publish(actuation)
|
||||
|
||||
gear = GearCommand()
|
||||
gear.stamp = now
|
||||
gear.command = self._gear_command(self.gear_name)
|
||||
self.pub_gear.publish(gear)
|
||||
|
||||
self.tick_count += 1
|
||||
if self.tick_count % int(self.rate_hz) == 0:
|
||||
self.get_logger().info(
|
||||
"sent %ds, autonomous_requested=%s"
|
||||
% (self.tick_count // int(self.rate_hz), self.autonomous_requested)
|
||||
)
|
||||
|
||||
def request_autonomous(self):
|
||||
if self.autonomous_requested:
|
||||
self.mode_timer.cancel()
|
||||
return
|
||||
|
||||
try:
|
||||
service_ready = self.control_mode_client.wait_for_service(timeout_sec=0.1)
|
||||
except Exception as exc:
|
||||
if rclpy.ok():
|
||||
self.get_logger().warn("service availability check failed: %s" % exc)
|
||||
return
|
||||
|
||||
if not service_ready:
|
||||
self.get_logger().warn("control_mode_request service is not ready")
|
||||
return
|
||||
|
||||
req = ControlModeCommand.Request()
|
||||
req.stamp = self.get_clock().now().to_msg()
|
||||
req.mode = ControlModeCommand.Request.AUTONOMOUS
|
||||
future = self.control_mode_client.call_async(req)
|
||||
future.add_done_callback(self.on_control_mode_response)
|
||||
self.autonomous_requested = True
|
||||
self.mode_timer.cancel()
|
||||
|
||||
def on_control_mode_response(self, future):
|
||||
try:
|
||||
response = future.result()
|
||||
except Exception as exc:
|
||||
self.get_logger().error("control_mode_request failed: %s" % exc)
|
||||
return
|
||||
|
||||
if response.success:
|
||||
self.get_logger().info("AUTONOMOUS control mode accepted")
|
||||
else:
|
||||
self.get_logger().error("AUTONOMOUS control mode rejected")
|
||||
|
||||
@staticmethod
|
||||
def _finite_or_zero(value):
|
||||
return value if math.isfinite(value) else 0.0
|
||||
|
||||
@staticmethod
|
||||
def _gear_command(name):
|
||||
mapping = {
|
||||
"none": GearCommand.NONE,
|
||||
"neutral": GearCommand.NEUTRAL,
|
||||
"drive": GearCommand.DRIVE,
|
||||
"reverse": GearCommand.REVERSE,
|
||||
"park": GearCommand.PARK,
|
||||
"parking": GearCommand.PARK,
|
||||
}
|
||||
return mapping.get(name, GearCommand.DRIVE)
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = AutowareTestNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+299
@@ -0,0 +1,299 @@
|
||||
#include "teemo_chassis_drive/dbc_codec.hpp"
|
||||
|
||||
namespace teemo_chassis_drive
|
||||
{
|
||||
|
||||
const std::unordered_map<uint32_t, RxFrameDef> & rx_frame_definitions()
|
||||
{
|
||||
static const std::unordered_map<uint32_t, RxFrameDef> table{
|
||||
{0x300, {0x300, "VCU_Vehicle_Status_3", "VCU_Vehicle_Status_3", "VehicleStatus3",
|
||||
{{"vehicle_steering_spd", 0, 16, ValueKind::Float, 0.01, 0.0, true}}}},
|
||||
|
||||
{0x303, {0x303, "VCU_Vehicle_Status_1", "VCU_Vehicle_Status_1", "VehicleStatus1",
|
||||
{{"vehicle_charge_sts", 56, 3, ValueKind::Int},
|
||||
{"vehicle_brk_config", 18, 2, ValueKind::Int},
|
||||
{"vehicle_range", 8,10, ValueKind::Int},
|
||||
{"drive_mode_state", 4, 4, ValueKind::Int},
|
||||
{"epb_status", 3, 1, ValueKind::Bool},
|
||||
{"accelerator_pedal_status", 40, 8, ValueKind::Int},
|
||||
{"brake_pedal_status", 32, 8, ValueKind::Int},
|
||||
{"vcu_303_rolling_counter", 60, 4, ValueKind::Int},
|
||||
{"vehicle_gear", 0, 2, ValueKind::Int}}}},
|
||||
|
||||
{0x304, {0x304, "VCU_Vehicle_Status_2", "VCU_Vehicle_Status_2", "VehicleStatus2",
|
||||
{{"whl_spd_sens_lvl_sts_rr", 59, 1, ValueKind::Bool},
|
||||
{"whl_spd_sens_lvl_sts_rl", 58, 1, ValueKind::Bool},
|
||||
{"whl_spd_sens_lvl_sts_fr", 57, 1, ValueKind::Bool},
|
||||
{"whl_spd_sens_lvl_sts_fl", 56, 1, ValueKind::Bool},
|
||||
{"vehicle_speed_vaild", 48, 1, ValueKind::Bool},
|
||||
{"vcu_304_rolling_counter", 60, 4, ValueKind::Int},
|
||||
{"vehicle_steering_angle", 32,16, ValueKind::Float, 0.1, -35.0},
|
||||
{"vehicle_brake_pressure", 16,16, ValueKind::Float, 0.01},
|
||||
{"vehicle_speed", 0,16, ValueKind::Float, 0.1, -80.0}}}},
|
||||
|
||||
{0x301, {0x301, "VCU_Vehicle_Diagnosis", "VCU_Vehicle_Diagnosis", "VehicleDiagnosis",
|
||||
{{"tire_bur_st", 59, 1, ValueKind::Bool},
|
||||
{"dropout_voltage", 58, 1, ValueKind::Bool},
|
||||
{"ad_remote_break", 57, 1, ValueKind::Bool},
|
||||
{"clean_fan_flag", 51, 1, ValueKind::Bool},
|
||||
{"slippery_slopes_flag", 50, 1, ValueKind::Bool},
|
||||
{"slip_flag", 49, 1, ValueKind::Bool},
|
||||
{"vcu_veh_cannot_pwr_off", 44, 2, ValueKind::Int},
|
||||
{"vcu_adota_flag", 39, 1, ValueKind::Bool},
|
||||
{"energy_recovery_flag", 36, 1, ValueKind::Bool},
|
||||
{"horn_2_state", 43, 1, ValueKind::Bool},
|
||||
{"vcu_veh_rdy", 38, 1, ValueKind::Bool},
|
||||
{"ads_light_state", 35, 1, ValueKind::Bool},
|
||||
{"kers_limited", 56, 1, ValueKind::Bool},
|
||||
{"oil_pot_state", 4, 1, ValueKind::Bool},
|
||||
{"business_relay_state", 31, 1, ValueKind::Bool},
|
||||
{"relay_4_g_state", 30, 1, ValueKind::Bool},
|
||||
{"radar_relay_state", 29, 1, ValueKind::Bool},
|
||||
{"orin_relay_state", 28, 1, ValueKind::Bool},
|
||||
{"motor_temp_state", 27, 1, ValueKind::Bool},
|
||||
{"fog_light_state", 11, 1, ValueKind::Bool},
|
||||
{"power_button_state", 2, 1, ValueKind::Bool},
|
||||
{"epb_button_state", 3, 1, ValueKind::Bool},
|
||||
{"motor_torque_limit_state", 8, 1, ValueKind::Bool},
|
||||
{"b_press_switch_collision_state", 26, 1, ValueKind::Bool},
|
||||
{"remo_touch_switch_disable_state", 24, 1, ValueKind::Bool},
|
||||
{"f_press_switch_collision_state", 25, 1, ValueKind::Bool},
|
||||
{"b_touch_switch_disable_state", 21, 1, ValueKind::Bool},
|
||||
{"l_touch_switch_disable_state", 22, 1, ValueKind::Bool},
|
||||
{"r_touch_switch_disable_state", 23, 1, ValueKind::Bool},
|
||||
{"f_touch_switch_disable_state", 20, 1, ValueKind::Bool},
|
||||
{"r_touch_switch_collision_state", 19, 1, ValueKind::Bool},
|
||||
{"l_touch_switch_collision_state", 18, 1, ValueKind::Bool},
|
||||
{"ad_fault_code", 52, 4, ValueKind::Int},
|
||||
{"epb_diagnosis", 14, 1, ValueKind::Bool},
|
||||
{"move_switch", 47, 1, ValueKind::Bool},
|
||||
{"low_beam_state", 33, 1, ValueKind::Bool},
|
||||
{"reversing_lights_state", 34, 1, ValueKind::Bool},
|
||||
{"tire_sensor_state", 41, 1, ValueKind::Bool},
|
||||
{"brake_light_state", 42, 1, ValueKind::Bool},
|
||||
{"vehicle_fault_grade", 16, 2, ValueKind::Int},
|
||||
{"eps_state", 10, 1, ValueKind::Bool},
|
||||
{"vcu_301_rolling_counter", 60, 4, ValueKind::Int},
|
||||
{"horn_1_state", 40, 1, ValueKind::Bool},
|
||||
{"high_beam_state", 15, 1, ValueKind::Bool},
|
||||
{"right_turn_light_state", 48, 1, ValueKind::Bool},
|
||||
{"left_turn_light_state", 32, 1, ValueKind::Bool},
|
||||
{"b_touch_switch_collision_state", 13, 1, ValueKind::Bool},
|
||||
{"f_touch_switch_collision_state", 12, 1, ValueKind::Bool},
|
||||
{"bms_state", 9, 1, ValueKind::Bool},
|
||||
{"emergency_button_state", 0, 1, ValueKind::Bool},
|
||||
{"dbs_state", 7, 1, ValueKind::Bool},
|
||||
{"ad_state", 6, 1, ValueKind::Bool},
|
||||
{"remote_state", 5, 1, ValueKind::Bool},
|
||||
{"motor_state", 1, 1, ValueKind::Bool}}}},
|
||||
|
||||
{0x306, {0x306, "VCU_FL_Wheel_Status", "VCU_FL_Wheel_Status", "WheelStatus",
|
||||
{{"wheel_speed_sensor_error", 56, 4, ValueKind::Int},
|
||||
{"valid_flag", 2, 1, ValueKind::Bool},
|
||||
{"slip_flag", 3, 1, ValueKind::Bool},
|
||||
{"sensor_attr", 0, 2, ValueKind::Int},
|
||||
{"tire_leak_state", 4, 1, ValueKind::Bool},
|
||||
{"pressure_warning", 5, 2, ValueKind::Int},
|
||||
{"tire_temperature", 48, 8, ValueKind::Float, 1.0, -55.0},
|
||||
{"tire_pressure", 16,16, ValueKind::Float, 0.1},
|
||||
{"sensor_state", 7, 1, ValueKind::Bool},
|
||||
{"wheel_speed", 32,16, ValueKind::Float, 0.01},
|
||||
{"wss_pul_cnt", 8, 8, ValueKind::Int},
|
||||
{"wheel_status_msg_cntr", 60, 4, ValueKind::Int}},
|
||||
{{"wheel", "fl"}}}},
|
||||
|
||||
{0x307, {0x307, "VCU_FR_Wheel_Status", "VCU_FR_Wheel_Status", "WheelStatus",
|
||||
{{"wheel_speed_sensor_error", 56, 4, ValueKind::Int},
|
||||
{"valid_flag", 2, 1, ValueKind::Bool},
|
||||
{"slip_flag", 3, 1, ValueKind::Bool},
|
||||
{"sensor_attr", 0, 2, ValueKind::Int},
|
||||
{"tire_leak_state", 4, 1, ValueKind::Bool},
|
||||
{"pressure_warning", 5, 2, ValueKind::Int},
|
||||
{"tire_temperature", 48, 8, ValueKind::Float, 1.0, -55.0},
|
||||
{"tire_pressure", 16,16, ValueKind::Float, 0.1},
|
||||
{"sensor_state", 7, 1, ValueKind::Bool},
|
||||
{"wheel_speed", 32,16, ValueKind::Float, 0.01},
|
||||
{"wss_pul_cnt", 8, 8, ValueKind::Int},
|
||||
{"wheel_status_msg_cntr", 60, 4, ValueKind::Int}},
|
||||
{{"wheel", "fr"}}}},
|
||||
|
||||
{0x308, {0x308, "VCU_RL_Wheel_Status", "VCU_RL_Wheel_Status", "WheelStatus",
|
||||
{{"wheel_speed_sensor_error", 56, 4, ValueKind::Int},
|
||||
{"valid_flag", 2, 1, ValueKind::Bool},
|
||||
{"slip_flag", 3, 1, ValueKind::Bool},
|
||||
{"sensor_attr", 0, 2, ValueKind::Int},
|
||||
{"tire_leak_state", 4, 1, ValueKind::Bool},
|
||||
{"pressure_warning", 5, 2, ValueKind::Int},
|
||||
{"tire_temperature", 48, 8, ValueKind::Float, 1.0, -55.0},
|
||||
{"tire_pressure", 16,16, ValueKind::Float, 0.1},
|
||||
{"sensor_state", 7, 1, ValueKind::Bool},
|
||||
{"wheel_speed", 32,16, ValueKind::Float, 0.01},
|
||||
{"wss_pul_cnt", 8, 8, ValueKind::Int},
|
||||
{"wheel_status_msg_cntr", 60, 4, ValueKind::Int}},
|
||||
{{"wheel", "rl"}}}},
|
||||
|
||||
{0x309, {0x309, "VCU_RR_Wheel_Status", "VCU_RR_Wheel_Status", "WheelStatus",
|
||||
{{"wheel_speed_sensor_error", 56, 4, ValueKind::Int},
|
||||
{"valid_flag", 2, 1, ValueKind::Bool},
|
||||
{"slip_flag", 3, 1, ValueKind::Bool},
|
||||
{"sensor_attr", 0, 2, ValueKind::Int},
|
||||
{"tire_leak_state", 4, 1, ValueKind::Bool},
|
||||
{"pressure_warning", 5, 2, ValueKind::Int},
|
||||
{"tire_temperature", 48, 8, ValueKind::Float, 1.0, -55.0},
|
||||
{"tire_pressure", 16,16, ValueKind::Float, 0.1},
|
||||
{"sensor_state", 7, 1, ValueKind::Bool},
|
||||
{"wheel_speed", 32,16, ValueKind::Float, 0.01},
|
||||
{"wss_pul_cnt", 8, 8, ValueKind::Int},
|
||||
{"wheel_status_msg_cntr", 60, 4, ValueKind::Int}},
|
||||
{{"wheel", "rr"}}}},
|
||||
|
||||
{0x30A, {0x30A, "VCU_Vehicle_HVBat_Status", "VCU_Vehicle_HVBat_Status", "VehicleHVBatStatus",
|
||||
{{"vehicle_poweroff_channel", 56, 4, ValueKind::Int},
|
||||
{"vehicle_poweroff_countdown_time", 2, 6, ValueKind::Int},
|
||||
{"battery_work_state", 0, 2, ValueKind::Int},
|
||||
{"vehicle_soc", 48, 8, ValueKind::Int},
|
||||
{"vehicle_hv_bat_msg_cntr", 60, 4, ValueKind::Int},
|
||||
{"high_voltage_battery_voltage", 24,16, ValueKind::Float, 0.1},
|
||||
{"high_voltage_battery_max_tem", 40, 8, ValueKind::Float, 1.0, -40.0},
|
||||
{"high_voltage_battery_current", 8,16, ValueKind::Float, 0.1, -1000.0}}}},
|
||||
|
||||
{0x30C, {0x30C, "VCU_Vehicle_PwrCtrl_Status", "VCU_Vehicle_PwrCtrl_Status", "VehiclePwrCtrlStatus",
|
||||
{{"vcu_state_flag", 28, 4, ValueKind::Int},
|
||||
{"vcu_power_st", 21, 3, ValueKind::Int},
|
||||
{"vcu_init", 32, 8, ValueKind::Int},
|
||||
{"power_button_state", 15, 1, ValueKind::Bool},
|
||||
{"vcu_veh_kl15_sts", 3, 1, ValueKind::Bool},
|
||||
{"vcu_mcukl15_sts", 5, 1, ValueKind::Bool},
|
||||
{"vcu_bmskl15_sts", 4, 1, ValueKind::Bool},
|
||||
{"vcu_bt_version2", 48, 8, ValueKind::Int},
|
||||
{"vcu_bt_version1", 40, 8, ValueKind::Int},
|
||||
{"vcu_ab_soft_st_old", 19, 1, ValueKind::Bool},
|
||||
{"vcu_ab_soft_st_new", 20, 1, ValueKind::Bool},
|
||||
{"vcu_cp_sts", 17, 2, ValueKind::Int},
|
||||
{"vcu_ota_flag", 16, 1, ValueKind::Bool},
|
||||
{"vcu_lv_mute_charge_st", 12, 3, ValueKind::Int},
|
||||
{"vcu_abn_pwr_off", 11, 1, ValueKind::Bool},
|
||||
{"vcu_pwr_down_st", 8, 3, ValueKind::Int},
|
||||
{"vcu_veh_rdy_diag_enable", 7, 1, ValueKind::Bool},
|
||||
{"vcu_low_batt_volt", 56, 8, ValueKind::Float, 0.1},
|
||||
{"vcu_weak_up_sig", 0, 3, ValueKind::Int}}}},
|
||||
|
||||
{0x30E, {0x30E, "VCU_Vehicle_Error_Status", "VCU_Vehicle_Error_Status", "VehicleErrorStatus",
|
||||
{{"flg_warning", 48, 1, ValueKind::Bool},
|
||||
{"vehicle_meter_soc", 40, 8, ValueKind::Int},
|
||||
{"residual_pressure", 24, 1, ValueKind::Bool},
|
||||
{"charge_abnormal", 25, 1, ValueKind::Bool},
|
||||
{"low_voltage", 16, 8, ValueKind::Float, 0.1},
|
||||
{"vcu_pwr_st", 14, 2, ValueKind::Int},
|
||||
{"vcu_can1_fault", 37, 3, ValueKind::Int},
|
||||
{"vcu_can0_fault", 29, 3, ValueKind::Int},
|
||||
{"vcu_epb_actin_when_ebsmf", 12, 1, ValueKind::Bool},
|
||||
{"vcu_eeprom_fault", 10, 1, ValueKind::Bool},
|
||||
{"vcu_ebs_actin_when_epbmf", 11, 1, ValueKind::Bool},
|
||||
{"vcu_error_code", 0,10, ValueKind::Int}}}},
|
||||
|
||||
{0x3A2, {0x3A2, "Vehicle_Odometer_Status", "Vehicle_Odometer_Status", "VehicleOdometerStatus",
|
||||
{{"vehicle_odometer_msg_cntr", 60, 4, ValueKind::Int},
|
||||
{"vehicle_ad_mileage", 42,18, ValueKind::Float, 0.8},
|
||||
{"vehicle_remote_mileage", 30,12, ValueKind::Float, 0.8},
|
||||
{"vehicle_trip", 18,12, ValueKind::Float, 0.8},
|
||||
{"vehicle_odo", 0,18, ValueKind::Float, 0.8}}}},
|
||||
|
||||
{0x3A3, {0x3A3, "Vehicle_Mileage1", "Vehicle_Mileage1", "VehicleMileage1",
|
||||
{{"vehicle_mileage1_msg_cntr", 48, 4, ValueKind::Int},
|
||||
{"vchicle_ad_mileage1", 24,24, ValueKind::Float, 0.1},
|
||||
{"vehicle_odo1", 0,24, ValueKind::Float, 0.1}}}},
|
||||
|
||||
{0x3A4, {0x3A4, "Vehicle_Mileage2", "Vehicle_Mileage2", "VehicleMileage2",
|
||||
{{"vehicle_mileage2_msg_cntr", 60, 4, ValueKind::Int},
|
||||
{"vehicle_trip1", 32,28, ValueKind::Int},
|
||||
{"vehicle_remote_mileage1", 0,20, ValueKind::Float, 0.1}}}},
|
||||
|
||||
{0x7F8, {0x7F8, "VCU_Version", "VCU_Version", "VehicleVersion",
|
||||
{{"car_model", 47, 5, ValueKind::Int},
|
||||
{"day", 56, 6, ValueKind::Int},
|
||||
{"month", 52, 4, ValueKind::Int},
|
||||
{"year", 40, 7, ValueKind::Int},
|
||||
{"byte5", 32, 8, ValueKind::Int},
|
||||
{"byte4", 24, 8, ValueKind::Int},
|
||||
{"byte3", 16, 8, ValueKind::Int},
|
||||
{"byte2", 8, 8, ValueKind::Int},
|
||||
{"byte1", 0, 8, ValueKind::Int}}}},
|
||||
};
|
||||
return table;
|
||||
}
|
||||
|
||||
const std::vector<TxFrameDef> & tx_frame_definitions()
|
||||
{
|
||||
static const std::vector<TxFrameDef> table{
|
||||
{0x1D6, "AD_OTAReq", 100,
|
||||
{{"ad_ota_req", 0, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0}}},
|
||||
|
||||
{0x1D7, "AD_Setup_Control", 20,
|
||||
{{"ad_clean_fan", 13, 1, ValueKind::Bool},
|
||||
{"ad_move_mode_req", 14, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0},
|
||||
{"ad_avh_active_cmd", 7, 1, ValueKind::Bool},
|
||||
{"ad_fault_handling_status", 8, 4, ValueKind::Int, 1.0, 0.0, false, 0.0, 15.0},
|
||||
{"ad_vehicle_weight", 20,13, ValueKind::Int, 1.0, 0.0, false, 0.0,8191.0},
|
||||
{"can_sign_tran_state", 5, 1, ValueKind::Bool},
|
||||
{"ad_release_r_bumper", 12, 1, ValueKind::Bool},
|
||||
{"ad_release_l_bumper", 19, 1, ValueKind::Bool},
|
||||
{"ad_disable_r_bumper", 4, 1, ValueKind::Bool},
|
||||
{"ad_disable_l_bumper", 3, 1, ValueKind::Bool},
|
||||
{"ad_disable_b_bumper", 2, 1, ValueKind::Bool},
|
||||
{"ad_disable_f_bumper", 1, 1, ValueKind::Bool},
|
||||
{"ad_clear_trip", 0, 1, ValueKind::Bool},
|
||||
{"ad_release_b_bumper", 18, 1, ValueKind::Bool},
|
||||
{"ad_release_f_bumper", 17, 1, ValueKind::Bool},
|
||||
{"ad_release_emergency_button",16, 1, ValueKind::Bool}}},
|
||||
|
||||
{0x1DB, "System_Power_Control", 20,
|
||||
{{"reserved_sys_power_control", 4, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0},
|
||||
{"vehicle_work_mode_control", 8, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0},
|
||||
{"radar_sys_power_control", 2, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0},
|
||||
{"business_sys_power_control", 10, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0},
|
||||
{"network_sys_power_control", 0, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0},
|
||||
{"ad_sys_power_control", 6, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0}}},
|
||||
|
||||
{0x1DA, "Power_on_CAN", 20,
|
||||
{{"ad_power_off_delay_time", 0, 6, ValueKind::Int, 1.0, 0.0, false, 0.0, 63.0},
|
||||
{"vehicle_power_req", 8, 8, ValueKind::Int, 1.0, 0.0, false, 0.0, 255.0}}},
|
||||
|
||||
{0x1DE, "AD_Control_Body", 20,
|
||||
{{"ad_veh_light_swt_emerg_freq", 0, 4, ValueKind::Int, 1.0, 0.0, false, 0.0, 15.0},
|
||||
{"ad_horn_control_1", 22, 1, ValueKind::Bool},
|
||||
{"ad_fof_light", 23, 1, ValueKind::Bool},
|
||||
{"ad_body_valid", 4, 4, ValueKind::Int, 1.0, 0.0, false, 0.0, 15.0},
|
||||
{"ad_low_beam", 15, 1, ValueKind::Bool},
|
||||
{"ad_reversing_lights", 14, 1, ValueKind::Bool},
|
||||
{"ad_double_flash_light", 13, 1, ValueKind::Bool},
|
||||
{"ad_brake_light", 12, 1, ValueKind::Bool},
|
||||
{"ad_horn_control", 9, 1, ValueKind::Bool},
|
||||
{"ad_high_beam", 8, 1, ValueKind::Bool},
|
||||
{"ad_right_turn_light", 10, 1, ValueKind::Bool},
|
||||
{"ad_left_turn_light", 11, 1, ValueKind::Bool}}},
|
||||
|
||||
{0x1D2, "AD_Control_Accelerate", 20,
|
||||
{{"ad_speed_req", 8,11, ValueKind::Float, 0.1, 0.0, false, 0.0, 40.0},
|
||||
{"ad_accelerate_req", 32, 8, ValueKind::Float, 0.1,-10.0, false, -10.0, 10.0},
|
||||
{"ad_torque_control", 24, 7, ValueKind::Int, 1.0, 0.0, false, 0.0, 100.0},
|
||||
{"ad_energy_recovery", 5, 1, ValueKind::Bool},
|
||||
{"ad_accelerate_gear", 3, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0},
|
||||
{"ad_accelerate_work_mode", 0, 3, ValueKind::Int, 1.0, 0.0, false, 0.0, 2.0},
|
||||
{"ad_accelerate_valid", 6, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 1.0}}},
|
||||
|
||||
{0x1D3, "AD_Control_Brake", 20,
|
||||
{{"ad_dbs_workmode", 1, 1, ValueKind::Bool},
|
||||
{"ad_awsc_flag", 0, 1, ValueKind::Bool},
|
||||
{"ad_brake_pressure_cmd", 8, 7, ValueKind::Int, 1.0, 0.0, false, 0.0, 100.0},
|
||||
{"ad_dbs_valid", 6, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0}}},
|
||||
|
||||
{0x1D4, "AD_Control_Steering", 20,
|
||||
{{"ad_steering_speed_cmd", 8,10, ValueKind::Float, 0.1, 5.0, false, 5.0, 107.3},
|
||||
{"ad_steering_angle_cmd", 18,11, ValueKind::Float, 0.1,-90.0, false, -90.0, 90.0},
|
||||
{"ad_steering_valid", 6, 2, ValueKind::Int, 1.0, 0.0, false, 0.0, 3.0}}},
|
||||
};
|
||||
return table;
|
||||
}
|
||||
|
||||
} // namespace teemo_chassis_drive
|
||||
+736
@@ -0,0 +1,736 @@
|
||||
#include <chrono>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "can_msgs/msg/frame.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/gear_report.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
|
||||
#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_diagnosis.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_error_status.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_hv_bat_status.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_mileage1.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_mileage2.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_odometer_status.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_pwr_ctrl_status.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_status1.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_status2.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_status3.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_version.hpp"
|
||||
#include "teemo_chassis_msgs/msg/wheel_status.hpp"
|
||||
#include "teemo_chassis_drive/dbc_codec.hpp"
|
||||
|
||||
using namespace teemo_chassis_drive;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
|
||||
double clamp_unit(double value)
|
||||
{
|
||||
return std::max(0.0, std::min(1.0, value));
|
||||
}
|
||||
|
||||
float deg_to_rad(double degrees)
|
||||
{
|
||||
return static_cast<float>(degrees * kPi / 180.0);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
class TeemoChassisReceiver : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
TeemoChassisReceiver() : Node("teemo_chassis_receiver")
|
||||
{
|
||||
input_topic_ = declare_parameter("input_topic", "/kvaser/can_tx");
|
||||
topic_prefix_ = declare_parameter("status_topic_prefix", "status");
|
||||
use_legacy_status_topic_names_ = declare_parameter("use_legacy_status_topic_names", false);
|
||||
input_qos_reliability_ = declare_parameter("input_qos_reliability", "best_effort");
|
||||
input_qos_depth_ = declare_parameter("input_qos_depth", 100);
|
||||
log_runtime_stats_ = declare_parameter("log_runtime_stats", true);
|
||||
stats_period_ms_ = declare_parameter("log_runtime_stats_period_ms", 1000);
|
||||
publish_autoware_status_ = declare_parameter("publish_autoware_status", true);
|
||||
autoware_frame_id_ = declare_parameter("autoware_frame_id", "base_link");
|
||||
autoware_control_mode_topic_ = declare_parameter(
|
||||
"autoware_control_mode_topic", "/vehicle/status/control_mode");
|
||||
autoware_velocity_topic_ = declare_parameter(
|
||||
"autoware_velocity_topic", "/vehicle/status/velocity_status");
|
||||
autoware_steering_topic_ = declare_parameter(
|
||||
"autoware_steering_topic", "/vehicle/status/steering_status");
|
||||
autoware_gear_topic_ = declare_parameter(
|
||||
"autoware_gear_topic", "/vehicle/status/gear_status");
|
||||
autoware_actuation_status_topic_ = declare_parameter(
|
||||
"autoware_actuation_status_topic", "/vehicle/status/actuation_status");
|
||||
autoware_autonomous_drive_mode_state_ = declare_parameter(
|
||||
"autoware_autonomous_drive_mode_state", 1);
|
||||
steering_status_ratio_ = declare_parameter("steering_status_ratio", 1.0);
|
||||
pedal_status_scale_ = declare_parameter("pedal_status_scale", 100.0);
|
||||
|
||||
if (!topic_prefix_.empty() && topic_prefix_.back() != '/') topic_prefix_ += '/';
|
||||
if (input_qos_depth_ <= 0) input_qos_depth_ = 100;
|
||||
if (steering_status_ratio_ == 0.0) steering_status_ratio_ = 1.0;
|
||||
if (pedal_status_scale_ <= 0.0) pedal_status_scale_ = 100.0;
|
||||
set_status_topic_suffixes();
|
||||
|
||||
using namespace teemo_chassis_msgs::msg;
|
||||
|
||||
output_topics_ = {
|
||||
make_topic_name(status_topic_suffixes_.vehicle_status_1),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_status_2),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_status_3),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_diagnosis),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_error_status),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_hv_bat_status),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_odometer_status),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_mileage1),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_mileage2),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_version),
|
||||
make_topic_name(status_topic_suffixes_.vehicle_pwr_ctrl_status),
|
||||
make_topic_name(status_topic_suffixes_.fl_wheel_status),
|
||||
make_topic_name(status_topic_suffixes_.fr_wheel_status),
|
||||
make_topic_name(status_topic_suffixes_.rl_wheel_status),
|
||||
make_topic_name(status_topic_suffixes_.rr_wheel_status),
|
||||
};
|
||||
if (publish_autoware_status_) {
|
||||
output_topics_.push_back(autoware_control_mode_topic_);
|
||||
output_topics_.push_back(autoware_velocity_topic_);
|
||||
output_topics_.push_back(autoware_steering_topic_);
|
||||
output_topics_.push_back(autoware_gear_topic_);
|
||||
output_topics_.push_back(autoware_actuation_status_topic_);
|
||||
}
|
||||
for (const auto & topic : output_topics_) {
|
||||
published_counts_[topic] = 0;
|
||||
}
|
||||
|
||||
pub_status1_ = create_publisher<VehicleStatus1>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_status_1), 10);
|
||||
pub_status2_ = create_publisher<VehicleStatus2>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_status_2), 10);
|
||||
pub_status3_ = create_publisher<VehicleStatus3>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_status_3), 10);
|
||||
pub_diagnosis_ = create_publisher<VehicleDiagnosis>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_diagnosis), 10);
|
||||
pub_error_ = create_publisher<VehicleErrorStatus>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_error_status), 10);
|
||||
pub_hvbat_ = create_publisher<VehicleHVBatStatus>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_hv_bat_status), 10);
|
||||
pub_odometer_ = create_publisher<VehicleOdometerStatus>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_odometer_status), 10);
|
||||
pub_mileage1_ = create_publisher<VehicleMileage1>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_mileage1), 10);
|
||||
pub_mileage2_ = create_publisher<VehicleMileage2>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_mileage2), 10);
|
||||
pub_version_ = create_publisher<VehicleVersion>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_version), 10);
|
||||
pub_pwr_ctrl_ = create_publisher<VehiclePwrCtrlStatus>(
|
||||
make_topic_name(status_topic_suffixes_.vehicle_pwr_ctrl_status), 10);
|
||||
pub_fl_wheel_ = create_publisher<WheelStatus>(
|
||||
make_topic_name(status_topic_suffixes_.fl_wheel_status), 10);
|
||||
pub_fr_wheel_ = create_publisher<WheelStatus>(
|
||||
make_topic_name(status_topic_suffixes_.fr_wheel_status), 10);
|
||||
pub_rl_wheel_ = create_publisher<WheelStatus>(
|
||||
make_topic_name(status_topic_suffixes_.rl_wheel_status), 10);
|
||||
pub_rr_wheel_ = create_publisher<WheelStatus>(
|
||||
make_topic_name(status_topic_suffixes_.rr_wheel_status), 10);
|
||||
|
||||
if (publish_autoware_status_) {
|
||||
pub_aw_control_mode_ =
|
||||
create_publisher<autoware_vehicle_msgs::msg::ControlModeReport>(
|
||||
autoware_control_mode_topic_, 10);
|
||||
pub_aw_velocity_ =
|
||||
create_publisher<autoware_vehicle_msgs::msg::VelocityReport>(
|
||||
autoware_velocity_topic_, 10);
|
||||
pub_aw_steering_ =
|
||||
create_publisher<autoware_vehicle_msgs::msg::SteeringReport>(
|
||||
autoware_steering_topic_, 10);
|
||||
pub_aw_gear_ =
|
||||
create_publisher<autoware_vehicle_msgs::msg::GearReport>(
|
||||
autoware_gear_topic_, 10);
|
||||
pub_aw_actuation_status_ =
|
||||
create_publisher<tier4_vehicle_msgs::msg::ActuationStatusStamped>(
|
||||
autoware_actuation_status_topic_, 10);
|
||||
}
|
||||
|
||||
auto input_qos = rclcpp::QoS(rclcpp::KeepLast(input_qos_depth_));
|
||||
if (input_qos_reliability_ == "reliable") {
|
||||
input_qos.reliable();
|
||||
} else {
|
||||
input_qos.best_effort();
|
||||
input_qos_reliability_ = "best_effort";
|
||||
}
|
||||
|
||||
sub_ = create_subscription<can_msgs::msg::Frame>(
|
||||
input_topic_, input_qos,
|
||||
std::bind(&TeemoChassisReceiver::on_frame, this, std::placeholders::_1));
|
||||
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Listening on '%s' with QoS depth=%d reliability=%s.",
|
||||
input_topic_.c_str(),
|
||||
input_qos_depth_,
|
||||
input_qos_reliability_.c_str());
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Status topic naming mode: %s",
|
||||
use_legacy_status_topic_names_ ? "legacy snake_case names" : "DBC frame names");
|
||||
RCLCPP_INFO(get_logger(), "Decoded ROS 2 output topics:");
|
||||
for (const auto & topic : output_topics_) {
|
||||
RCLCPP_INFO(get_logger(), " %s", topic.c_str());
|
||||
}
|
||||
|
||||
if (log_runtime_stats_) {
|
||||
if (stats_period_ms_ <= 0) stats_period_ms_ = 1000;
|
||||
stats_timer_ = create_wall_timer(
|
||||
std::chrono::milliseconds(stats_period_ms_),
|
||||
std::bind(&TeemoChassisReceiver::flush_runtime_stats, this));
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
struct StatusTopicSuffixes
|
||||
{
|
||||
std::string vehicle_status_1;
|
||||
std::string vehicle_status_2;
|
||||
std::string vehicle_status_3;
|
||||
std::string vehicle_diagnosis;
|
||||
std::string vehicle_error_status;
|
||||
std::string vehicle_hv_bat_status;
|
||||
std::string vehicle_odometer_status;
|
||||
std::string vehicle_mileage1;
|
||||
std::string vehicle_mileage2;
|
||||
std::string vehicle_version;
|
||||
std::string vehicle_pwr_ctrl_status;
|
||||
std::string fl_wheel_status;
|
||||
std::string fr_wheel_status;
|
||||
std::string rl_wheel_status;
|
||||
std::string rr_wheel_status;
|
||||
};
|
||||
|
||||
void set_status_topic_suffixes()
|
||||
{
|
||||
if (use_legacy_status_topic_names_) {
|
||||
status_topic_suffixes_ = {
|
||||
"vehicle_status_1",
|
||||
"vehicle_status_2",
|
||||
"vehicle_status_3",
|
||||
"vehicle_diagnosis",
|
||||
"vehicle_error_status",
|
||||
"vehicle_hv_bat_status",
|
||||
"vehicle_odometer_status",
|
||||
"vehicle_mileage1",
|
||||
"vehicle_mileage2",
|
||||
"vehicle_version",
|
||||
"vehicle_pwr_ctrl_status",
|
||||
"fl_wheel_status",
|
||||
"fr_wheel_status",
|
||||
"rl_wheel_status",
|
||||
"rr_wheel_status",
|
||||
};
|
||||
return;
|
||||
}
|
||||
|
||||
status_topic_suffixes_ = {
|
||||
"VCU_Vehicle_Status_1",
|
||||
"VCU_Vehicle_Status_2",
|
||||
"VCU_Vehicle_Status_3",
|
||||
"VCU_Vehicle_Diagnosis",
|
||||
"VCU_Vehicle_Error_Status",
|
||||
"VCU_Vehicle_HVBat_Status",
|
||||
"Vehicle_Odometer_Status",
|
||||
"Vehicle_Mileage1",
|
||||
"Vehicle_Mileage2",
|
||||
"VCU_Version",
|
||||
"VCU_Vehicle_PwrCtrl_Status",
|
||||
"VCU_FL_Wheel_Status",
|
||||
"VCU_FR_Wheel_Status",
|
||||
"VCU_RL_Wheel_Status",
|
||||
"VCU_RR_Wheel_Status",
|
||||
};
|
||||
}
|
||||
|
||||
std::string make_topic_name(const std::string & suffix) const
|
||||
{
|
||||
return topic_prefix_ + suffix;
|
||||
}
|
||||
|
||||
void track_published(const std::string & topic_suffix)
|
||||
{
|
||||
++interval_decoded_frames_;
|
||||
++published_counts_[make_topic_name(topic_suffix)];
|
||||
}
|
||||
|
||||
void track_published_topic(const std::string & topic)
|
||||
{
|
||||
++published_counts_[topic];
|
||||
}
|
||||
|
||||
void flush_runtime_stats()
|
||||
{
|
||||
if (!log_runtime_stats_) return;
|
||||
|
||||
const auto publisher_count = count_publishers(input_topic_);
|
||||
|
||||
if (interval_input_frames_ == 0) {
|
||||
if (!warned_no_input_) {
|
||||
if (publisher_count == 0) {
|
||||
RCLCPP_WARN(
|
||||
get_logger(),
|
||||
"No CAN frames have been received on '%s'. The topic is currently visible only from local subscriptions, or its upstream publisher is not running yet.",
|
||||
input_topic_.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(
|
||||
get_logger(),
|
||||
"Topic '%s' has %zu publisher(s), but this node still received 0 frames. This is usually caused by an inactive lifecycle publisher or a QoS mismatch. Current receiver QoS: depth=%d reliability=%s.",
|
||||
input_topic_.c_str(),
|
||||
publisher_count,
|
||||
input_qos_depth_,
|
||||
input_qos_reliability_.c_str());
|
||||
}
|
||||
warned_no_input_ = true;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
warned_no_input_ = false;
|
||||
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Runtime stats (%d ms): input=%zu decoded=%zu unknown_id=%zu publishers=%zu",
|
||||
stats_period_ms_,
|
||||
interval_input_frames_,
|
||||
interval_decoded_frames_,
|
||||
interval_unknown_frames_,
|
||||
publisher_count);
|
||||
|
||||
for (const auto & topic : output_topics_) {
|
||||
auto it = published_counts_.find(topic);
|
||||
if (it != published_counts_.end() && it->second > 0) {
|
||||
RCLCPP_INFO(get_logger(), " %s -> %zu msg", topic.c_str(), it->second);
|
||||
it->second = 0;
|
||||
}
|
||||
}
|
||||
|
||||
interval_input_frames_ = 0;
|
||||
interval_decoded_frames_ = 0;
|
||||
interval_unknown_frames_ = 0;
|
||||
}
|
||||
|
||||
void publish_autoware_status1(const teemo_chassis_msgs::msg::VehicleStatus1 & status)
|
||||
{
|
||||
if (!publish_autoware_status_) return;
|
||||
|
||||
autoware_vehicle_msgs::msg::ControlModeReport mode;
|
||||
mode.stamp = status.stamp;
|
||||
if (status.drive_mode_state == static_cast<uint8_t>(autoware_autonomous_drive_mode_state_)) {
|
||||
mode.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;
|
||||
} else {
|
||||
mode.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL;
|
||||
}
|
||||
pub_aw_control_mode_->publish(mode);
|
||||
track_published_topic(autoware_control_mode_topic_);
|
||||
|
||||
autoware_vehicle_msgs::msg::GearReport gear;
|
||||
gear.stamp = status.stamp;
|
||||
switch (status.vehicle_gear) {
|
||||
case 0:
|
||||
gear.report = autoware_vehicle_msgs::msg::GearReport::PARK;
|
||||
break;
|
||||
case 1:
|
||||
gear.report = autoware_vehicle_msgs::msg::GearReport::DRIVE;
|
||||
break;
|
||||
case 2:
|
||||
gear.report = autoware_vehicle_msgs::msg::GearReport::NEUTRAL;
|
||||
break;
|
||||
case 3:
|
||||
gear.report = autoware_vehicle_msgs::msg::GearReport::REVERSE;
|
||||
break;
|
||||
default:
|
||||
gear.report = autoware_vehicle_msgs::msg::GearReport::NONE;
|
||||
break;
|
||||
}
|
||||
pub_aw_gear_->publish(gear);
|
||||
track_published_topic(autoware_gear_topic_);
|
||||
|
||||
cached_accel_pedal_status_ =
|
||||
clamp_unit(static_cast<double>(status.accelerator_pedal_status) / pedal_status_scale_);
|
||||
cached_brake_pedal_status_ =
|
||||
clamp_unit(static_cast<double>(status.brake_pedal_status) / pedal_status_scale_);
|
||||
|
||||
tier4_vehicle_msgs::msg::ActuationStatusStamped actuation;
|
||||
actuation.header.stamp = status.stamp;
|
||||
actuation.header.frame_id = autoware_frame_id_;
|
||||
actuation.status.accel_status = cached_accel_pedal_status_;
|
||||
actuation.status.brake_status = cached_brake_pedal_status_;
|
||||
actuation.status.steer_status = cached_steering_tire_angle_rad_;
|
||||
pub_aw_actuation_status_->publish(actuation);
|
||||
track_published_topic(autoware_actuation_status_topic_);
|
||||
}
|
||||
|
||||
void publish_autoware_status2(const teemo_chassis_msgs::msg::VehicleStatus2 & status)
|
||||
{
|
||||
if (!publish_autoware_status_) return;
|
||||
|
||||
autoware_vehicle_msgs::msg::VelocityReport velocity;
|
||||
velocity.header.stamp = status.stamp;
|
||||
velocity.header.frame_id = autoware_frame_id_;
|
||||
velocity.longitudinal_velocity = static_cast<float>(status.vehicle_speed / 3.6);
|
||||
velocity.lateral_velocity = 0.0f;
|
||||
velocity.heading_rate = 0.0f;
|
||||
pub_aw_velocity_->publish(velocity);
|
||||
track_published_topic(autoware_velocity_topic_);
|
||||
|
||||
const double tire_angle_deg =
|
||||
static_cast<double>(status.vehicle_steering_angle) / steering_status_ratio_;
|
||||
cached_steering_tire_angle_rad_ = deg_to_rad(tire_angle_deg);
|
||||
|
||||
autoware_vehicle_msgs::msg::SteeringReport steering;
|
||||
steering.stamp = status.stamp;
|
||||
steering.steering_tire_angle = static_cast<float>(cached_steering_tire_angle_rad_);
|
||||
pub_aw_steering_->publish(steering);
|
||||
track_published_topic(autoware_steering_topic_);
|
||||
}
|
||||
|
||||
void on_frame(const can_msgs::msg::Frame::SharedPtr msg)
|
||||
{
|
||||
++interval_input_frames_;
|
||||
|
||||
const auto & table = rx_frame_definitions();
|
||||
auto it = table.find(msg->id);
|
||||
if (it == table.end()) {
|
||||
++interval_unknown_frames_;
|
||||
return;
|
||||
}
|
||||
|
||||
const auto & def = it->second;
|
||||
Payload p = normalize(std::vector<uint8_t>(msg->data.begin(), msg->data.end()));
|
||||
|
||||
// Build decoded values map
|
||||
std::unordered_map<std::string, double> vals;
|
||||
for (const auto & sig : def.signals)
|
||||
vals[sig.field_name] = decode_signal(p, sig);
|
||||
|
||||
auto stamp = msg->header.stamp;
|
||||
auto can_id = msg->id;
|
||||
uint8_t dlc = msg->dlc;
|
||||
std::array<uint8_t, 8> raw{};
|
||||
for (int i = 0; i < 8; ++i) raw[i] = p[i];
|
||||
|
||||
using namespace teemo_chassis_msgs::msg;
|
||||
|
||||
if (def.msg_type == "VehicleStatus1") {
|
||||
VehicleStatus1 out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.vehicle_charge_sts = static_cast<uint8_t>(vals["vehicle_charge_sts"]);
|
||||
out.vehicle_brk_config = static_cast<uint8_t>(vals["vehicle_brk_config"]);
|
||||
out.vehicle_range = static_cast<uint16_t>(vals["vehicle_range"]);
|
||||
out.drive_mode_state = static_cast<uint8_t>(vals["drive_mode_state"]);
|
||||
out.epb_status = vals["epb_status"] != 0.0;
|
||||
out.accelerator_pedal_status = static_cast<uint8_t>(vals["accelerator_pedal_status"]);
|
||||
out.brake_pedal_status = static_cast<uint8_t>(vals["brake_pedal_status"]);
|
||||
out.vcu_303_rolling_counter = static_cast<uint8_t>(vals["vcu_303_rolling_counter"]);
|
||||
out.vehicle_gear = static_cast<uint8_t>(vals["vehicle_gear"]);
|
||||
pub_status1_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_status_1);
|
||||
publish_autoware_status1(out);
|
||||
|
||||
} else if (def.msg_type == "VehicleStatus2") {
|
||||
VehicleStatus2 out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.whl_spd_sens_lvl_sts_rr = vals["whl_spd_sens_lvl_sts_rr"] != 0.0;
|
||||
out.whl_spd_sens_lvl_sts_rl = vals["whl_spd_sens_lvl_sts_rl"] != 0.0;
|
||||
out.whl_spd_sens_lvl_sts_fr = vals["whl_spd_sens_lvl_sts_fr"] != 0.0;
|
||||
out.whl_spd_sens_lvl_sts_fl = vals["whl_spd_sens_lvl_sts_fl"] != 0.0;
|
||||
out.vehicle_speed_vaild = vals["vehicle_speed_vaild"] != 0.0;
|
||||
out.vcu_304_rolling_counter = static_cast<uint8_t>(vals["vcu_304_rolling_counter"]);
|
||||
out.vehicle_steering_angle = static_cast<float>(vals["vehicle_steering_angle"]);
|
||||
out.vehicle_brake_pressure = static_cast<float>(vals["vehicle_brake_pressure"]);
|
||||
out.vehicle_speed = static_cast<float>(vals["vehicle_speed"]);
|
||||
pub_status2_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_status_2);
|
||||
publish_autoware_status2(out);
|
||||
|
||||
} else if (def.msg_type == "VehicleStatus3") {
|
||||
VehicleStatus3 out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.vehicle_steering_spd = static_cast<float>(vals["vehicle_steering_spd"]);
|
||||
pub_status3_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_status_3);
|
||||
|
||||
} else if (def.msg_type == "VehicleDiagnosis") {
|
||||
VehicleDiagnosis out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.tire_bur_st = vals["tire_bur_st"] != 0.0;
|
||||
out.dropout_voltage = vals["dropout_voltage"] != 0.0;
|
||||
out.ad_remote_break = vals["ad_remote_break"] != 0.0;
|
||||
out.clean_fan_flag = vals["clean_fan_flag"] != 0.0;
|
||||
out.slippery_slopes_flag = vals["slippery_slopes_flag"] != 0.0;
|
||||
out.slip_flag = vals["slip_flag"] != 0.0;
|
||||
out.vcu_veh_cannot_pwr_off = static_cast<uint8_t>(vals["vcu_veh_cannot_pwr_off"]);
|
||||
out.vcu_adota_flag = vals["vcu_adota_flag"] != 0.0;
|
||||
out.energy_recovery_flag = vals["energy_recovery_flag"] != 0.0;
|
||||
out.horn_2_state = vals["horn_2_state"] != 0.0;
|
||||
out.vcu_veh_rdy = vals["vcu_veh_rdy"] != 0.0;
|
||||
out.ads_light_state = vals["ads_light_state"] != 0.0;
|
||||
out.kers_limited = vals["kers_limited"] != 0.0;
|
||||
out.oil_pot_state = vals["oil_pot_state"] != 0.0;
|
||||
out.business_relay_state = vals["business_relay_state"] != 0.0;
|
||||
out.relay_4_g_state = vals["relay_4_g_state"] != 0.0;
|
||||
out.radar_relay_state = vals["radar_relay_state"] != 0.0;
|
||||
out.orin_relay_state = vals["orin_relay_state"] != 0.0;
|
||||
out.motor_temp_state = vals["motor_temp_state"] != 0.0;
|
||||
out.fog_light_state = vals["fog_light_state"] != 0.0;
|
||||
out.power_button_state = vals["power_button_state"] != 0.0;
|
||||
out.epb_button_state = vals["epb_button_state"] != 0.0;
|
||||
out.motor_torque_limit_state = vals["motor_torque_limit_state"] != 0.0;
|
||||
out.b_press_switch_collision_state = vals["b_press_switch_collision_state"] != 0.0;
|
||||
out.remo_touch_switch_disable_state = vals["remo_touch_switch_disable_state"] != 0.0;
|
||||
out.f_press_switch_collision_state = vals["f_press_switch_collision_state"] != 0.0;
|
||||
out.b_touch_switch_disable_state = vals["b_touch_switch_disable_state"] != 0.0;
|
||||
out.l_touch_switch_disable_state = vals["l_touch_switch_disable_state"] != 0.0;
|
||||
out.r_touch_switch_disable_state = vals["r_touch_switch_disable_state"] != 0.0;
|
||||
out.f_touch_switch_disable_state = vals["f_touch_switch_disable_state"] != 0.0;
|
||||
out.r_touch_switch_collision_state = vals["r_touch_switch_collision_state"] != 0.0;
|
||||
out.l_touch_switch_collision_state = vals["l_touch_switch_collision_state"] != 0.0;
|
||||
out.ad_fault_code = static_cast<uint8_t>(vals["ad_fault_code"]);
|
||||
out.epb_diagnosis = vals["epb_diagnosis"] != 0.0;
|
||||
out.move_switch = vals["move_switch"] != 0.0;
|
||||
out.low_beam_state = vals["low_beam_state"] != 0.0;
|
||||
out.reversing_lights_state = vals["reversing_lights_state"] != 0.0;
|
||||
out.tire_sensor_state = vals["tire_sensor_state"] != 0.0;
|
||||
out.brake_light_state = vals["brake_light_state"] != 0.0;
|
||||
out.vehicle_fault_grade = static_cast<uint8_t>(vals["vehicle_fault_grade"]);
|
||||
out.eps_state = vals["eps_state"] != 0.0;
|
||||
out.vcu_301_rolling_counter = static_cast<uint8_t>(vals["vcu_301_rolling_counter"]);
|
||||
out.horn_1_state = vals["horn_1_state"] != 0.0;
|
||||
out.high_beam_state = vals["high_beam_state"] != 0.0;
|
||||
out.right_turn_light_state = vals["right_turn_light_state"] != 0.0;
|
||||
out.left_turn_light_state = vals["left_turn_light_state"] != 0.0;
|
||||
out.b_touch_switch_collision_state = vals["b_touch_switch_collision_state"] != 0.0;
|
||||
out.f_touch_switch_collision_state = vals["f_touch_switch_collision_state"] != 0.0;
|
||||
out.bms_state = vals["bms_state"] != 0.0;
|
||||
out.emergency_button_state = vals["emergency_button_state"] != 0.0;
|
||||
out.dbs_state = vals["dbs_state"] != 0.0;
|
||||
out.ad_state = vals["ad_state"] != 0.0;
|
||||
out.remote_state = vals["remote_state"] != 0.0;
|
||||
out.motor_state = vals["motor_state"] != 0.0;
|
||||
pub_diagnosis_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_diagnosis);
|
||||
|
||||
} else if (def.msg_type == "VehicleErrorStatus") {
|
||||
VehicleErrorStatus out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.flg_warning = vals["flg_warning"] != 0.0;
|
||||
out.vehicle_meter_soc = static_cast<uint8_t>(vals["vehicle_meter_soc"]);
|
||||
out.residual_pressure = vals["residual_pressure"] != 0.0;
|
||||
out.charge_abnormal = vals["charge_abnormal"] != 0.0;
|
||||
out.low_voltage = static_cast<float>(vals["low_voltage"]);
|
||||
out.vcu_pwr_st = static_cast<uint8_t>(vals["vcu_pwr_st"]);
|
||||
out.vcu_can1_fault = static_cast<uint8_t>(vals["vcu_can1_fault"]);
|
||||
out.vcu_can0_fault = static_cast<uint8_t>(vals["vcu_can0_fault"]);
|
||||
out.vcu_epb_actin_when_ebsmf = vals["vcu_epb_actin_when_ebsmf"] != 0.0;
|
||||
out.vcu_eeprom_fault = vals["vcu_eeprom_fault"] != 0.0;
|
||||
out.vcu_ebs_actin_when_epbmf = vals["vcu_ebs_actin_when_epbmf"] != 0.0;
|
||||
out.vcu_error_code = static_cast<uint16_t>(vals["vcu_error_code"]);
|
||||
pub_error_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_error_status);
|
||||
|
||||
} else if (def.msg_type == "VehicleHVBatStatus") {
|
||||
VehicleHVBatStatus out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.vehicle_poweroff_channel = static_cast<uint8_t>(vals["vehicle_poweroff_channel"]);
|
||||
out.vehicle_poweroff_countdown_time = static_cast<uint8_t>(vals["vehicle_poweroff_countdown_time"]);
|
||||
out.battery_work_state = static_cast<uint8_t>(vals["battery_work_state"]);
|
||||
out.vehicle_soc = static_cast<uint8_t>(vals["vehicle_soc"]);
|
||||
out.vehicle_hv_bat_msg_cntr = static_cast<uint8_t>(vals["vehicle_hv_bat_msg_cntr"]);
|
||||
out.high_voltage_battery_voltage = static_cast<float>(vals["high_voltage_battery_voltage"]);
|
||||
out.high_voltage_battery_max_tem = static_cast<float>(vals["high_voltage_battery_max_tem"]);
|
||||
out.high_voltage_battery_current = static_cast<float>(vals["high_voltage_battery_current"]);
|
||||
pub_hvbat_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_hv_bat_status);
|
||||
|
||||
} else if (def.msg_type == "VehicleOdometerStatus") {
|
||||
VehicleOdometerStatus out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.vehicle_odometer_msg_cntr = static_cast<uint8_t>(vals["vehicle_odometer_msg_cntr"]);
|
||||
out.vehicle_ad_mileage = static_cast<float>(vals["vehicle_ad_mileage"]);
|
||||
out.vehicle_remote_mileage = static_cast<float>(vals["vehicle_remote_mileage"]);
|
||||
out.vehicle_trip = static_cast<float>(vals["vehicle_trip"]);
|
||||
out.vehicle_odo = static_cast<float>(vals["vehicle_odo"]);
|
||||
pub_odometer_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_odometer_status);
|
||||
|
||||
} else if (def.msg_type == "VehicleMileage1") {
|
||||
VehicleMileage1 out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.vehicle_mileage1_msg_cntr = static_cast<uint8_t>(vals["vehicle_mileage1_msg_cntr"]);
|
||||
out.vchicle_ad_mileage1 = static_cast<float>(vals["vchicle_ad_mileage1"]);
|
||||
out.vehicle_odo1 = static_cast<float>(vals["vehicle_odo1"]);
|
||||
pub_mileage1_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_mileage1);
|
||||
|
||||
} else if (def.msg_type == "VehicleMileage2") {
|
||||
VehicleMileage2 out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.vehicle_mileage2_msg_cntr = static_cast<uint8_t>(vals["vehicle_mileage2_msg_cntr"]);
|
||||
out.vehicle_trip1 = static_cast<uint32_t>(vals["vehicle_trip1"]);
|
||||
out.vehicle_remote_mileage1 = static_cast<float>(vals["vehicle_remote_mileage1"]);
|
||||
pub_mileage2_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_mileage2);
|
||||
|
||||
} else if (def.msg_type == "VehicleVersion") {
|
||||
VehicleVersion out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.car_model = static_cast<uint8_t>(vals["car_model"]);
|
||||
out.day = static_cast<uint8_t>(vals["day"]);
|
||||
out.month = static_cast<uint8_t>(vals["month"]);
|
||||
out.year = static_cast<uint8_t>(vals["year"]);
|
||||
out.byte5 = static_cast<uint8_t>(vals["byte5"]);
|
||||
out.byte4 = static_cast<uint8_t>(vals["byte4"]);
|
||||
out.byte3 = static_cast<uint8_t>(vals["byte3"]);
|
||||
out.byte2 = static_cast<uint8_t>(vals["byte2"]);
|
||||
out.byte1 = static_cast<uint8_t>(vals["byte1"]);
|
||||
pub_version_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_version);
|
||||
|
||||
} else if (def.msg_type == "VehiclePwrCtrlStatus") {
|
||||
VehiclePwrCtrlStatus out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
out.vcu_state_flag = static_cast<uint8_t>(vals["vcu_state_flag"]);
|
||||
out.vcu_power_st = static_cast<uint8_t>(vals["vcu_power_st"]);
|
||||
out.vcu_init = static_cast<uint8_t>(vals["vcu_init"]);
|
||||
out.power_button_state = vals["power_button_state"] != 0.0;
|
||||
out.vcu_veh_kl15_sts = vals["vcu_veh_kl15_sts"] != 0.0;
|
||||
out.vcu_mcukl15_sts = vals["vcu_mcukl15_sts"] != 0.0;
|
||||
out.vcu_bmskl15_sts = vals["vcu_bmskl15_sts"] != 0.0;
|
||||
out.vcu_bt_version2 = static_cast<uint8_t>(vals["vcu_bt_version2"]);
|
||||
out.vcu_bt_version1 = static_cast<uint8_t>(vals["vcu_bt_version1"]);
|
||||
out.vcu_ab_soft_st_old = vals["vcu_ab_soft_st_old"] != 0.0;
|
||||
out.vcu_ab_soft_st_new = vals["vcu_ab_soft_st_new"] != 0.0;
|
||||
out.vcu_cp_sts = static_cast<uint8_t>(vals["vcu_cp_sts"]);
|
||||
out.vcu_ota_flag = vals["vcu_ota_flag"] != 0.0;
|
||||
out.vcu_lv_mute_charge_st = static_cast<uint8_t>(vals["vcu_lv_mute_charge_st"]);
|
||||
out.vcu_abn_pwr_off = vals["vcu_abn_pwr_off"] != 0.0;
|
||||
out.vcu_pwr_down_st = static_cast<uint8_t>(vals["vcu_pwr_down_st"]);
|
||||
out.vcu_veh_rdy_diag_enable = vals["vcu_veh_rdy_diag_enable"] != 0.0;
|
||||
out.vcu_low_batt_volt = static_cast<float>(vals["vcu_low_batt_volt"]);
|
||||
out.vcu_weak_up_sig = static_cast<uint8_t>(vals["vcu_weak_up_sig"]);
|
||||
pub_pwr_ctrl_->publish(out);
|
||||
track_published(status_topic_suffixes_.vehicle_pwr_ctrl_status);
|
||||
|
||||
} else if (def.msg_type == "WheelStatus") {
|
||||
WheelStatus out;
|
||||
out.stamp = stamp; out.can_id = can_id; out.dlc = dlc; out.raw_data = raw;
|
||||
for (const auto & sf : def.static_fields)
|
||||
if (sf.first == "wheel") out.wheel = sf.second;
|
||||
out.wheel_speed_sensor_error = static_cast<uint8_t>(vals["wheel_speed_sensor_error"]);
|
||||
out.valid_flag = vals["valid_flag"] != 0.0;
|
||||
out.slip_flag = vals["slip_flag"] != 0.0;
|
||||
out.sensor_attr = static_cast<uint8_t>(vals["sensor_attr"]);
|
||||
out.tire_leak_state = vals["tire_leak_state"] != 0.0;
|
||||
out.pressure_warning = static_cast<uint8_t>(vals["pressure_warning"]);
|
||||
out.tire_temperature = static_cast<float>(vals["tire_temperature"]);
|
||||
out.tire_pressure = static_cast<float>(vals["tire_pressure"]);
|
||||
out.sensor_state = vals["sensor_state"] != 0.0;
|
||||
out.wheel_speed = static_cast<float>(vals["wheel_speed"]);
|
||||
out.wss_pul_cnt = static_cast<uint8_t>(vals["wss_pul_cnt"]);
|
||||
out.wheel_status_msg_cntr = static_cast<uint8_t>(vals["wheel_status_msg_cntr"]);
|
||||
|
||||
if (def.can_id == 0x306) {
|
||||
pub_fl_wheel_->publish(out);
|
||||
track_published(status_topic_suffixes_.fl_wheel_status);
|
||||
} else if (def.can_id == 0x307) {
|
||||
pub_fr_wheel_->publish(out);
|
||||
track_published(status_topic_suffixes_.fr_wheel_status);
|
||||
} else if (def.can_id == 0x308) {
|
||||
pub_rl_wheel_->publish(out);
|
||||
track_published(status_topic_suffixes_.rl_wheel_status);
|
||||
} else {
|
||||
pub_rr_wheel_->publish(out);
|
||||
track_published(status_topic_suffixes_.rr_wheel_status);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr sub_;
|
||||
rclcpp::TimerBase::SharedPtr stats_timer_;
|
||||
|
||||
std::string input_topic_;
|
||||
std::string topic_prefix_;
|
||||
bool use_legacy_status_topic_names_{false};
|
||||
std::string input_qos_reliability_{"best_effort"};
|
||||
int input_qos_depth_{100};
|
||||
bool log_runtime_stats_{true};
|
||||
int stats_period_ms_{1000};
|
||||
bool publish_autoware_status_{true};
|
||||
std::string autoware_frame_id_{"base_link"};
|
||||
std::string autoware_control_mode_topic_{"/vehicle/status/control_mode"};
|
||||
std::string autoware_velocity_topic_{"/vehicle/status/velocity_status"};
|
||||
std::string autoware_steering_topic_{"/vehicle/status/steering_status"};
|
||||
std::string autoware_gear_topic_{"/vehicle/status/gear_status"};
|
||||
std::string autoware_actuation_status_topic_{"/vehicle/status/actuation_status"};
|
||||
int autoware_autonomous_drive_mode_state_{1};
|
||||
double steering_status_ratio_{1.0};
|
||||
double pedal_status_scale_{100.0};
|
||||
bool warned_no_input_{false};
|
||||
size_t interval_input_frames_{0};
|
||||
size_t interval_decoded_frames_{0};
|
||||
size_t interval_unknown_frames_{0};
|
||||
StatusTopicSuffixes status_topic_suffixes_{};
|
||||
std::vector<std::string> output_topics_;
|
||||
std::unordered_map<std::string, size_t> published_counts_;
|
||||
|
||||
using VS1 = teemo_chassis_msgs::msg::VehicleStatus1;
|
||||
using VS2 = teemo_chassis_msgs::msg::VehicleStatus2;
|
||||
using VS3 = teemo_chassis_msgs::msg::VehicleStatus3;
|
||||
using VD = teemo_chassis_msgs::msg::VehicleDiagnosis;
|
||||
using VES = teemo_chassis_msgs::msg::VehicleErrorStatus;
|
||||
using VHVB = teemo_chassis_msgs::msg::VehicleHVBatStatus;
|
||||
using VOS = teemo_chassis_msgs::msg::VehicleOdometerStatus;
|
||||
using VM1 = teemo_chassis_msgs::msg::VehicleMileage1;
|
||||
using VM2 = teemo_chassis_msgs::msg::VehicleMileage2;
|
||||
using VV = teemo_chassis_msgs::msg::VehicleVersion;
|
||||
using VPCS = teemo_chassis_msgs::msg::VehiclePwrCtrlStatus;
|
||||
using WS = teemo_chassis_msgs::msg::WheelStatus;
|
||||
|
||||
rclcpp::Publisher<VS1>::SharedPtr pub_status1_;
|
||||
rclcpp::Publisher<VS2>::SharedPtr pub_status2_;
|
||||
rclcpp::Publisher<VS3>::SharedPtr pub_status3_;
|
||||
rclcpp::Publisher<VD>::SharedPtr pub_diagnosis_;
|
||||
rclcpp::Publisher<VES>::SharedPtr pub_error_;
|
||||
rclcpp::Publisher<VHVB>::SharedPtr pub_hvbat_;
|
||||
rclcpp::Publisher<VOS>::SharedPtr pub_odometer_;
|
||||
rclcpp::Publisher<VM1>::SharedPtr pub_mileage1_;
|
||||
rclcpp::Publisher<VM2>::SharedPtr pub_mileage2_;
|
||||
rclcpp::Publisher<VV>::SharedPtr pub_version_;
|
||||
rclcpp::Publisher<VPCS>::SharedPtr pub_pwr_ctrl_;
|
||||
rclcpp::Publisher<WS>::SharedPtr pub_fl_wheel_;
|
||||
rclcpp::Publisher<WS>::SharedPtr pub_fr_wheel_;
|
||||
rclcpp::Publisher<WS>::SharedPtr pub_rl_wheel_;
|
||||
rclcpp::Publisher<WS>::SharedPtr pub_rr_wheel_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr pub_aw_control_mode_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::VelocityReport>::SharedPtr pub_aw_velocity_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::SteeringReport>::SharedPtr pub_aw_steering_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::GearReport>::SharedPtr pub_aw_gear_;
|
||||
rclcpp::Publisher<tier4_vehicle_msgs::msg::ActuationStatusStamped>::SharedPtr
|
||||
pub_aw_actuation_status_;
|
||||
|
||||
double cached_accel_pedal_status_{0.0};
|
||||
double cached_brake_pedal_status_{0.0};
|
||||
double cached_steering_tire_angle_rad_{0.0};
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<TeemoChassisReceiver>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+674
@@ -0,0 +1,674 @@
|
||||
#include <chrono>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "can_msgs/msg/frame.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/gear_command.hpp"
|
||||
#include "autoware_vehicle_msgs/srv/control_mode_command.hpp"
|
||||
#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp"
|
||||
#include "teemo_chassis_msgs/msg/ad_control_accelerate.hpp"
|
||||
#include "teemo_chassis_msgs/msg/ad_control_brake.hpp"
|
||||
#include "teemo_chassis_msgs/msg/ad_control_steering.hpp"
|
||||
#include "teemo_chassis_msgs/msg/vehicle_control_command.hpp"
|
||||
#include "teemo_chassis_drive/dbc_codec.hpp"
|
||||
|
||||
using namespace teemo_chassis_drive;
|
||||
using VehicleControlCommand = teemo_chassis_msgs::msg::VehicleControlCommand;
|
||||
using ADControlAccelerate = teemo_chassis_msgs::msg::ADControlAccelerate;
|
||||
using ADControlBrake = teemo_chassis_msgs::msg::ADControlBrake;
|
||||
using ADControlSteering = teemo_chassis_msgs::msg::ADControlSteering;
|
||||
using ActuationCommandStamped = tier4_vehicle_msgs::msg::ActuationCommandStamped;
|
||||
using GearCommand = autoware_vehicle_msgs::msg::GearCommand;
|
||||
using ControlModeCommand = autoware_vehicle_msgs::srv::ControlModeCommand;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
|
||||
double clamp_value(double value, double low, double high)
|
||||
{
|
||||
return std::max(low, std::min(high, value));
|
||||
}
|
||||
|
||||
using TxSignalIndex = std::unordered_map<std::string, const SignalDef *>;
|
||||
|
||||
const TxSignalIndex & tx_signal_index()
|
||||
{
|
||||
static const TxSignalIndex index = []() {
|
||||
TxSignalIndex out;
|
||||
for (const auto & frame : tx_frame_definitions()) {
|
||||
for (const auto & signal : frame.signals) {
|
||||
out.emplace(signal.field_name, &signal);
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}();
|
||||
return index;
|
||||
}
|
||||
|
||||
const std::unordered_map<std::string, std::string> & dbc_parameter_names()
|
||||
{
|
||||
static const std::unordered_map<std::string, std::string> mapping{
|
||||
{"ad_ota_req", "AD_OTAReq"},
|
||||
{"ad_clean_fan", "AD_Clean_Fan"},
|
||||
{"ad_move_mode_req", "AD_MoveModeReq"},
|
||||
{"ad_release_r_bumper", "AD_Release_R_Bumper"},
|
||||
{"ad_release_l_bumper", "AD_Release_L_Bumper"},
|
||||
{"ad_disable_r_bumper", "AD_Disable_R_Bumper"},
|
||||
{"ad_disable_l_bumper", "AD_Disable_L_Bumper"},
|
||||
{"ad_disable_b_bumper", "AD_Disable_B_Bumper"},
|
||||
{"ad_disable_f_bumper", "AD_Disable_F_Bumper"},
|
||||
{"ad_clear_trip", "AD_Clear_TRIP"},
|
||||
{"ad_release_b_bumper", "AD_Release_B_Bumper"},
|
||||
{"ad_release_f_bumper", "AD_Release_F_Bumper"},
|
||||
{"ad_release_emergency_button", "AD_Release_Emergency_Button"},
|
||||
{"vehicle_work_mode_control", "Vehicle_Work_Mode_Control"},
|
||||
{"ad_power_off_delay_time", "AD_PwrDownCountInit"},
|
||||
{"vehicle_power_req", "AD_Vehicle_PwrReq"},
|
||||
{"ad_veh_light_swt_emerg_freq", "AD_VehLightSwtEmergFreq"},
|
||||
{"ad_horn_control_1", "AD_Horn_2_Control"},
|
||||
{"ad_fof_light", "AD_Fog_Light"},
|
||||
{"ad_body_valid", "AD_Body_Valid"},
|
||||
{"ad_low_beam", "AD_Low_Beam"},
|
||||
{"ad_reversing_lights", "AD_Reversing_Lights"},
|
||||
{"ad_double_flash_light", "AD_Double_Flash_Light"},
|
||||
{"ad_brake_light", "AD_Brake_Light"},
|
||||
{"ad_horn_control", "AD_Horn_1_Control"},
|
||||
{"ad_high_beam", "AD_High_Beam"},
|
||||
{"ad_right_turn_light", "AD_Right_Turn_Light"},
|
||||
{"ad_left_turn_light", "AD_Left_Turn_Light"},
|
||||
{"ad_speed_req", "AD_Speed_Req"},
|
||||
{"ad_accelerate_req", "AD_Accelerate_Req"},
|
||||
{"ad_torque_control", "AD_Torque_Pedal"},
|
||||
{"ad_energy_recovery", "AD_Energy_Recovery"},
|
||||
{"ad_accelerate_gear", "AD_Accelerate_Gear"},
|
||||
{"ad_accelerate_work_mode", "AD_Accelerate_Work_Mode"},
|
||||
{"ad_accelerate_valid", "AD_Accelerate_Valid"},
|
||||
{"ad_dbs_workmode", "AD_DBS_WorkMode"},
|
||||
{"ad_awsc_flag", "AD_AWSC_Flag"},
|
||||
{"ad_brake_pressure_cmd", "AD_BrakePressure_Req"},
|
||||
{"ad_dbs_valid", "AD_DBS_Valid"},
|
||||
{"ad_steering_speed_cmd", "AD_Steering_Speed_Cmd"},
|
||||
{"ad_steering_angle_cmd", "AD_Steering_Angle_Cmd"},
|
||||
{"ad_steering_valid", "AD_Steering_Valid"},
|
||||
};
|
||||
return mapping;
|
||||
}
|
||||
|
||||
std::string parameter_name_for_signal(const std::string & signal_name)
|
||||
{
|
||||
const auto & mapping = dbc_parameter_names();
|
||||
auto it = mapping.find(signal_name);
|
||||
return it == mapping.end() ? signal_name : it->second;
|
||||
}
|
||||
|
||||
void declare_tx_parameter(rclcpp::Node & node, const std::string & name, double default_value)
|
||||
{
|
||||
const auto parameter_name = parameter_name_for_signal(name);
|
||||
const auto & signals = tx_signal_index();
|
||||
auto it = signals.find(name);
|
||||
if (it == signals.end()) {
|
||||
node.declare_parameter(parameter_name, default_value);
|
||||
return;
|
||||
}
|
||||
|
||||
switch (it->second->kind) {
|
||||
case ValueKind::Bool:
|
||||
node.declare_parameter(parameter_name, default_value != 0.0);
|
||||
break;
|
||||
case ValueKind::Int:
|
||||
node.declare_parameter(parameter_name, static_cast<int64_t>(default_value));
|
||||
break;
|
||||
case ValueKind::Float:
|
||||
node.declare_parameter(parameter_name, default_value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
double parameter_to_double(const rclcpp::Parameter & parameter)
|
||||
{
|
||||
switch (parameter.get_type()) {
|
||||
case rclcpp::ParameterType::PARAMETER_BOOL:
|
||||
return parameter.as_bool() ? 1.0 : 0.0;
|
||||
case rclcpp::ParameterType::PARAMETER_INTEGER:
|
||||
return static_cast<double>(parameter.as_int());
|
||||
case rclcpp::ParameterType::PARAMETER_DOUBLE:
|
||||
return parameter.as_double();
|
||||
default:
|
||||
throw std::runtime_error(
|
||||
"Unsupported parameter type for '" + parameter.get_name() +
|
||||
"'. Expected bool, integer, or double.");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// Default TX values (safe / neutral state)
|
||||
static const std::unordered_map<std::string, double> DEFAULT_TX_VALUES{
|
||||
{"ad_ota_req", 0},
|
||||
{"ad_clean_fan", 0},
|
||||
{"ad_move_mode_req", 0},
|
||||
{"ad_avh_active_cmd", 0},
|
||||
{"ad_fault_handling_status", 0},
|
||||
{"ad_vehicle_weight", 0},
|
||||
{"can_sign_tran_state", 0},
|
||||
{"ad_release_r_bumper", 0},
|
||||
{"ad_release_l_bumper", 0},
|
||||
{"ad_disable_r_bumper", 0},
|
||||
{"ad_disable_l_bumper", 0},
|
||||
{"ad_disable_b_bumper", 0},
|
||||
{"ad_disable_f_bumper", 0},
|
||||
{"ad_clear_trip", 0},
|
||||
{"ad_release_b_bumper", 0},
|
||||
{"ad_release_f_bumper", 0},
|
||||
{"ad_release_emergency_button", 0},
|
||||
{"reserved_sys_power_control", 0},
|
||||
{"vehicle_work_mode_control", 0},
|
||||
{"radar_sys_power_control", 0},
|
||||
{"business_sys_power_control", 0},
|
||||
{"network_sys_power_control", 0},
|
||||
{"ad_sys_power_control", 0},
|
||||
{"ad_power_off_delay_time", 0},
|
||||
{"vehicle_power_req", 0},
|
||||
{"ad_veh_light_swt_emerg_freq", 0},
|
||||
{"ad_horn_control_1", 0},
|
||||
{"ad_fof_light", 0},
|
||||
{"ad_body_valid", 0},
|
||||
{"ad_low_beam", 0},
|
||||
{"ad_reversing_lights", 0},
|
||||
{"ad_double_flash_light", 0},
|
||||
{"ad_brake_light", 0},
|
||||
{"ad_horn_control", 0},
|
||||
{"ad_high_beam", 0},
|
||||
{"ad_right_turn_light", 0},
|
||||
{"ad_left_turn_light", 0},
|
||||
{"ad_speed_req", 0.0},
|
||||
{"ad_accelerate_req", 0.0},
|
||||
{"ad_energy_recovery", 0},
|
||||
{"ad_torque_control", 0},
|
||||
{"ad_accelerate_gear", 1},
|
||||
{"ad_accelerate_work_mode", 0},
|
||||
{"ad_accelerate_valid", 1},
|
||||
{"ad_dbs_workmode", 0},
|
||||
{"ad_awsc_flag", 0},
|
||||
{"ad_brake_pressure_cmd", 0},
|
||||
{"ad_dbs_valid", 0},
|
||||
{"ad_steering_speed_cmd", 2.68},
|
||||
{"ad_steering_angle_cmd", 0.0},
|
||||
{"ad_steering_valid", 1},
|
||||
};
|
||||
|
||||
// Fields that can be overridden by an incoming VehicleControlCommand
|
||||
static const std::vector<std::string> COMMAND_OVERRIDE_FIELDS{
|
||||
"ad_accelerate_valid", "ad_accelerate_work_mode", "ad_accelerate_gear",
|
||||
"ad_speedor_torque_control", "ad_torque_control",
|
||||
"ad_dbs_valid", "ad_brake_pressure_cmd", "ad_dbs_workmode", "ad_awsc_flag",
|
||||
"ad_steering_valid", "ad_steering_angle_cmd", "ad_steering_speed_cmd",
|
||||
"ad_body_valid", "ad_veh_light_swt_emerg_freq",
|
||||
"ad_low_beam", "ad_reversing_lights", "ad_double_flash_light",
|
||||
"ad_brake_light", "ad_horn_control", "ad_horn_control_1", "ad_fof_light",
|
||||
"ad_high_beam", "ad_right_turn_light", "ad_left_turn_light",
|
||||
};
|
||||
|
||||
class TeemoChassisSender : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
TeemoChassisSender() : Node("teemo_chassis_sender")
|
||||
{
|
||||
declare_parameter("output_topic", "/kvaser/can_rx");
|
||||
declare_parameter("command_topic", "control/vehicle_command");
|
||||
declare_parameter("accelerate_command_topic", "control/AD_Control_Accelerate");
|
||||
declare_parameter("brake_command_topic", "control/AD_Control_Brake");
|
||||
declare_parameter("steering_command_topic", "control/AD_Control_Steering");
|
||||
declare_parameter("command_timeout_ms", 500);
|
||||
declare_parameter("autoware_mode", true);
|
||||
declare_parameter("actuation_command_topic", "/control/command/actuation_cmd");
|
||||
declare_parameter("gear_command_topic", "/control/command/gear_cmd");
|
||||
declare_parameter("control_mode_service", "/control/control_mode_request");
|
||||
declare_parameter("steering_command_ratio", 1.0);
|
||||
declare_parameter("autoware_accel_scale", 100.0);
|
||||
declare_parameter("autoware_brake_scale", 100.0);
|
||||
declare_parameter("publish_converted_control_topics", true);
|
||||
|
||||
// Declare all default TX value parameters
|
||||
for (const auto & [k, v] : DEFAULT_TX_VALUES)
|
||||
declare_tx_parameter(*this, k, v);
|
||||
|
||||
output_topic_ = get_parameter("output_topic").as_string();
|
||||
command_topic_ = get_parameter("command_topic").as_string();
|
||||
accelerate_command_topic_ = get_parameter("accelerate_command_topic").as_string();
|
||||
brake_command_topic_ = get_parameter("brake_command_topic").as_string();
|
||||
steering_command_topic_ = get_parameter("steering_command_topic").as_string();
|
||||
command_timeout_ms_ = get_parameter("command_timeout_ms").as_int();
|
||||
autoware_mode_ = get_parameter("autoware_mode").as_bool();
|
||||
actuation_command_topic_ = get_parameter("actuation_command_topic").as_string();
|
||||
gear_command_topic_ = get_parameter("gear_command_topic").as_string();
|
||||
control_mode_service_ = get_parameter("control_mode_service").as_string();
|
||||
steering_command_ratio_ = get_parameter("steering_command_ratio").as_double();
|
||||
autoware_accel_scale_ = get_parameter("autoware_accel_scale").as_double();
|
||||
autoware_brake_scale_ = get_parameter("autoware_brake_scale").as_double();
|
||||
publish_converted_control_topics_ =
|
||||
get_parameter("publish_converted_control_topics").as_bool();
|
||||
|
||||
if (steering_command_ratio_ == 0.0) steering_command_ratio_ = 1.0;
|
||||
if (autoware_accel_scale_ <= 0.0) autoware_accel_scale_ = 100.0;
|
||||
if (autoware_brake_scale_ <= 0.0) autoware_brake_scale_ = 100.0;
|
||||
|
||||
pub_ = create_publisher<can_msgs::msg::Frame>(output_topic_, 100);
|
||||
|
||||
cmd_sub_ = create_subscription<VehicleControlCommand>(
|
||||
command_topic_, 10,
|
||||
std::bind(&TeemoChassisSender::on_command, this, std::placeholders::_1));
|
||||
|
||||
accelerate_cmd_sub_ = create_subscription<ADControlAccelerate>(
|
||||
accelerate_command_topic_, 10,
|
||||
std::bind(&TeemoChassisSender::on_accelerate_command, this, std::placeholders::_1));
|
||||
|
||||
brake_cmd_sub_ = create_subscription<ADControlBrake>(
|
||||
brake_command_topic_, 10,
|
||||
std::bind(&TeemoChassisSender::on_brake_command, this, std::placeholders::_1));
|
||||
|
||||
steering_cmd_sub_ = create_subscription<ADControlSteering>(
|
||||
steering_command_topic_, 10,
|
||||
std::bind(&TeemoChassisSender::on_steering_command, this, std::placeholders::_1));
|
||||
|
||||
if (publish_converted_control_topics_) {
|
||||
converted_accelerate_pub_ =
|
||||
create_publisher<ADControlAccelerate>(accelerate_command_topic_, 10);
|
||||
converted_brake_pub_ =
|
||||
create_publisher<ADControlBrake>(brake_command_topic_, 10);
|
||||
converted_steering_pub_ =
|
||||
create_publisher<ADControlSteering>(steering_command_topic_, 10);
|
||||
}
|
||||
|
||||
if (autoware_mode_) {
|
||||
actuation_cmd_sub_ = create_subscription<ActuationCommandStamped>(
|
||||
actuation_command_topic_, 10,
|
||||
std::bind(&TeemoChassisSender::on_autoware_actuation_command, this, std::placeholders::_1));
|
||||
|
||||
gear_cmd_sub_ = create_subscription<GearCommand>(
|
||||
gear_command_topic_, 10,
|
||||
std::bind(&TeemoChassisSender::on_autoware_gear_command, this, std::placeholders::_1));
|
||||
|
||||
control_mode_srv_ = create_service<ControlModeCommand>(
|
||||
control_mode_service_,
|
||||
std::bind(
|
||||
&TeemoChassisSender::on_control_mode_request, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
|
||||
// Create one timer per TX frame.
|
||||
// Per vehicle-side request, do not send AD power-control frames.
|
||||
for (const auto & def : tx_frame_definitions()) {
|
||||
if (def.name == "System_Power_Control" || def.name == "Power_on_CAN") {
|
||||
RCLCPP_WARN(
|
||||
get_logger(), "TX frame disabled by configuration: %s (0x%X)",
|
||||
def.name.c_str(), def.can_id);
|
||||
continue;
|
||||
}
|
||||
|
||||
rolling_counters_[def.can_id] = 0;
|
||||
auto period = std::chrono::milliseconds(def.cycle_ms);
|
||||
const auto * def_ptr = &def;
|
||||
timers_.push_back(create_wall_timer(period,
|
||||
[this, def_ptr]() { publish_frame(*def_ptr); }));
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(),
|
||||
"Publishing chassis commands to '%s'. Override topics: '%s', '%s', '%s', '%s'.",
|
||||
output_topic_.c_str(),
|
||||
command_topic_.c_str(),
|
||||
accelerate_command_topic_.c_str(),
|
||||
brake_command_topic_.c_str(),
|
||||
steering_command_topic_.c_str());
|
||||
if (autoware_mode_) {
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Autoware mode enabled. Subscribing '%s' and '%s'; serving '%s'.",
|
||||
actuation_command_topic_.c_str(),
|
||||
gear_command_topic_.c_str(),
|
||||
control_mode_service_.c_str());
|
||||
if (publish_converted_control_topics_) {
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Publishing converted TEEMO control topics: '%s', '%s', '%s'.",
|
||||
accelerate_command_topic_.c_str(),
|
||||
brake_command_topic_.c_str(),
|
||||
steering_command_topic_.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
static uint8_t to_u8(double value)
|
||||
{
|
||||
return static_cast<uint8_t>(std::round(clamp_value(value, 0.0, 255.0)));
|
||||
}
|
||||
|
||||
void update_command_field(const std::string & field, double value)
|
||||
{
|
||||
last_command_[field] = value;
|
||||
last_command_field_time_[field] = now();
|
||||
}
|
||||
|
||||
bool is_field_recent(const std::string & field) const
|
||||
{
|
||||
auto it = last_command_field_time_.find(field);
|
||||
if (it == last_command_field_time_.end()) {
|
||||
return false;
|
||||
}
|
||||
if (command_timeout_ms_ <= 0) {
|
||||
return true;
|
||||
}
|
||||
auto elapsed = (now() - it->second).nanoseconds();
|
||||
return elapsed <= static_cast<int64_t>(command_timeout_ms_) * 1'000'000LL;
|
||||
}
|
||||
|
||||
void on_command(const VehicleControlCommand::SharedPtr msg)
|
||||
{
|
||||
update_command_field("ad_accelerate_valid", msg->ad_accelerate_valid);
|
||||
update_command_field("ad_accelerate_work_mode", msg->ad_accelerate_work_mode);
|
||||
update_command_field("ad_accelerate_gear", msg->ad_accelerate_gear);
|
||||
update_command_field("ad_speedor_torque_control", msg->ad_speedor_torque_control);
|
||||
update_command_field("ad_torque_control", msg->ad_torque_control);
|
||||
update_command_field("ad_dbs_valid", msg->ad_dbs_valid);
|
||||
update_command_field("ad_brake_pressure_cmd", msg->ad_brake_pressure_cmd);
|
||||
update_command_field("ad_dbs_workmode", msg->ad_dbs_workmode ? 1.0 : 0.0);
|
||||
update_command_field("ad_awsc_flag", msg->ad_awsc_flag ? 1.0 : 0.0);
|
||||
update_command_field("ad_steering_valid", msg->ad_steering_valid);
|
||||
update_command_field("ad_steering_angle_cmd", msg->ad_steering_angle_cmd);
|
||||
update_command_field("ad_steering_speed_cmd", msg->ad_steering_speed_cmd);
|
||||
update_command_field("ad_body_valid", msg->ad_body_valid);
|
||||
update_command_field("ad_veh_light_swt_emerg_freq", msg->ad_veh_light_swt_emerg_freq);
|
||||
update_command_field("ad_low_beam", msg->ad_low_beam ? 1.0 : 0.0);
|
||||
update_command_field("ad_reversing_lights", msg->ad_reversing_lights ? 1.0 : 0.0);
|
||||
update_command_field("ad_double_flash_light", msg->ad_double_flash_light ? 1.0 : 0.0);
|
||||
update_command_field("ad_brake_light", msg->ad_brake_light ? 1.0 : 0.0);
|
||||
update_command_field("ad_horn_control", msg->ad_horn_control ? 1.0 : 0.0);
|
||||
update_command_field("ad_horn_control_1", msg->ad_horn_control_1 ? 1.0 : 0.0);
|
||||
update_command_field("ad_fof_light", msg->ad_fof_light ? 1.0 : 0.0);
|
||||
update_command_field("ad_high_beam", msg->ad_high_beam ? 1.0 : 0.0);
|
||||
update_command_field("ad_right_turn_light", msg->ad_right_turn_light ? 1.0 : 0.0);
|
||||
update_command_field("ad_left_turn_light", msg->ad_left_turn_light ? 1.0 : 0.0);
|
||||
}
|
||||
|
||||
void on_accelerate_command(const ADControlAccelerate::SharedPtr msg)
|
||||
{
|
||||
update_command_field("ad_accelerate_valid", msg->ad_accelerate_valid);
|
||||
update_command_field("ad_accelerate_work_mode", msg->ad_accelerate_work_mode);
|
||||
update_command_field("ad_accelerate_gear", msg->ad_accelerate_gear);
|
||||
update_command_field("ad_speedor_torque_control", msg->ad_speedor_torque_control);
|
||||
update_command_field("ad_torque_control", msg->ad_torque_control);
|
||||
}
|
||||
|
||||
void on_brake_command(const ADControlBrake::SharedPtr msg)
|
||||
{
|
||||
update_command_field("ad_dbs_valid", msg->ad_dbs_valid);
|
||||
update_command_field("ad_brake_pressure_cmd", msg->ad_brake_pressure_cmd);
|
||||
update_command_field("ad_dbs_workmode", msg->ad_dbs_workmode ? 1.0 : 0.0);
|
||||
update_command_field("ad_awsc_flag", msg->ad_awsc_flag ? 1.0 : 0.0);
|
||||
}
|
||||
|
||||
void on_steering_command(const ADControlSteering::SharedPtr msg)
|
||||
{
|
||||
update_command_field("ad_steering_valid", msg->ad_steering_valid);
|
||||
update_command_field("ad_steering_angle_cmd", msg->ad_steering_angle_cmd);
|
||||
update_command_field("ad_steering_speed_cmd", msg->ad_steering_speed_cmd);
|
||||
}
|
||||
|
||||
void on_autoware_actuation_command(const ActuationCommandStamped::SharedPtr msg)
|
||||
{
|
||||
const double accel_pct = std::isfinite(msg->actuation.accel_cmd)
|
||||
? clamp_value(msg->actuation.accel_cmd * autoware_accel_scale_, 0.0, 100.0)
|
||||
: 0.0;
|
||||
const double brake_pct = std::isfinite(msg->actuation.brake_cmd)
|
||||
? clamp_value(msg->actuation.brake_cmd * autoware_brake_scale_, 0.0, 100.0)
|
||||
: 0.0;
|
||||
const double steering_deg = std::isfinite(msg->actuation.steer_cmd)
|
||||
? clamp_value(msg->actuation.steer_cmd * 180.0 / kPi * steering_command_ratio_, -90.0, 90.0)
|
||||
: 0.0;
|
||||
|
||||
converted_accel_pct_ = accel_pct;
|
||||
converted_brake_pct_ = brake_pct;
|
||||
converted_steering_deg_ = steering_deg;
|
||||
|
||||
update_command_field("ad_accelerate_work_mode", 0.0);
|
||||
update_command_field("ad_torque_control", accel_pct);
|
||||
update_command_field("ad_brake_pressure_cmd", brake_pct);
|
||||
update_command_field("ad_steering_angle_cmd", steering_deg);
|
||||
|
||||
publish_converted_control_messages();
|
||||
}
|
||||
|
||||
void on_autoware_gear_command(const GearCommand::SharedPtr msg)
|
||||
{
|
||||
switch (msg->command) {
|
||||
case GearCommand::PARK:
|
||||
converted_gear_ = 0;
|
||||
update_command_field("ad_accelerate_gear", 0.0);
|
||||
break;
|
||||
case GearCommand::DRIVE:
|
||||
converted_gear_ = 1;
|
||||
update_command_field("ad_accelerate_gear", 1.0);
|
||||
break;
|
||||
case GearCommand::NEUTRAL:
|
||||
converted_gear_ = 2;
|
||||
update_command_field("ad_accelerate_gear", 2.0);
|
||||
break;
|
||||
case GearCommand::REVERSE:
|
||||
converted_gear_ = 3;
|
||||
update_command_field("ad_accelerate_gear", 3.0);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
publish_converted_accelerate_message();
|
||||
}
|
||||
|
||||
void on_control_mode_request(
|
||||
const ControlModeCommand::Request::SharedPtr request,
|
||||
ControlModeCommand::Response::SharedPtr response)
|
||||
{
|
||||
if (request->mode == ControlModeCommand::Request::AUTONOMOUS) {
|
||||
autonomous_requested_ = true;
|
||||
response->success = true;
|
||||
publish_converted_control_messages();
|
||||
RCLCPP_INFO(get_logger(), "ControlModeCommand: AUTONOMOUS requested.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (request->mode == ControlModeCommand::Request::MANUAL) {
|
||||
autonomous_requested_ = false;
|
||||
response->success = true;
|
||||
publish_converted_control_messages();
|
||||
RCLCPP_INFO(get_logger(), "ControlModeCommand: MANUAL requested.");
|
||||
return;
|
||||
}
|
||||
|
||||
response->success = false;
|
||||
}
|
||||
|
||||
void publish_converted_control_messages()
|
||||
{
|
||||
publish_converted_accelerate_message();
|
||||
publish_converted_brake_message();
|
||||
publish_converted_steering_message();
|
||||
}
|
||||
|
||||
void publish_converted_accelerate_message()
|
||||
{
|
||||
if (!publish_converted_control_topics_ || !converted_accelerate_pub_) {
|
||||
return;
|
||||
}
|
||||
|
||||
ADControlAccelerate msg;
|
||||
msg.stamp = now();
|
||||
msg.ad_accelerate_valid = autonomous_requested_ ? 1 : 0;
|
||||
msg.ad_accelerate_work_mode = 0;
|
||||
msg.ad_accelerate_gear = converted_gear_;
|
||||
msg.ad_speedor_torque_control = 0.0f;
|
||||
msg.ad_torque_control = to_u8(converted_accel_pct_);
|
||||
converted_accelerate_pub_->publish(msg);
|
||||
}
|
||||
|
||||
void publish_converted_brake_message()
|
||||
{
|
||||
if (!publish_converted_control_topics_ || !converted_brake_pub_) {
|
||||
return;
|
||||
}
|
||||
|
||||
ADControlBrake msg;
|
||||
msg.stamp = now();
|
||||
msg.ad_dbs_valid = autonomous_requested_ ? 1 : 0;
|
||||
msg.ad_brake_pressure_cmd = to_u8(converted_brake_pct_);
|
||||
msg.ad_dbs_workmode = false;
|
||||
msg.ad_awsc_flag = false;
|
||||
converted_brake_pub_->publish(msg);
|
||||
}
|
||||
|
||||
void publish_converted_steering_message()
|
||||
{
|
||||
if (!publish_converted_control_topics_ || !converted_steering_pub_) {
|
||||
return;
|
||||
}
|
||||
|
||||
ADControlSteering msg;
|
||||
msg.stamp = now();
|
||||
msg.ad_steering_valid = autonomous_requested_ ? 1 : 0;
|
||||
msg.ad_steering_angle_cmd = static_cast<float>(converted_steering_deg_);
|
||||
msg.ad_steering_speed_cmd =
|
||||
static_cast<float>(parameter_to_double(get_parameter(parameter_name_for_signal("ad_steering_speed_cmd"))));
|
||||
converted_steering_pub_->publish(msg);
|
||||
}
|
||||
|
||||
std::unordered_map<std::string, double> resolve_values()
|
||||
{
|
||||
// Start from YAML / parameter defaults
|
||||
std::unordered_map<std::string, double> vals;
|
||||
vals.reserve(DEFAULT_TX_VALUES.size());
|
||||
for (const auto & [k, _] : DEFAULT_TX_VALUES)
|
||||
vals[k] = parameter_to_double(get_parameter(parameter_name_for_signal(k)));
|
||||
|
||||
for (const auto & field : COMMAND_OVERRIDE_FIELDS) {
|
||||
if (!is_field_recent(field)) {
|
||||
continue;
|
||||
}
|
||||
auto it = last_command_.find(field);
|
||||
if (it != last_command_.end()) {
|
||||
vals[field] = it->second;
|
||||
}
|
||||
}
|
||||
|
||||
// Keep legacy command API while allowing direct DBC YAML fields.
|
||||
auto legacy_target_it = last_command_.find("ad_speedor_torque_control");
|
||||
if (legacy_target_it != last_command_.end() && is_field_recent("ad_speedor_torque_control")) {
|
||||
const double accelerate_target = legacy_target_it->second;
|
||||
const int work_mode = vals.count("ad_accelerate_work_mode")
|
||||
? static_cast<int>(vals["ad_accelerate_work_mode"])
|
||||
: 0;
|
||||
|
||||
if (work_mode == 1) {
|
||||
vals["ad_speed_req"] = accelerate_target;
|
||||
vals["ad_accelerate_req"] = 0.0;
|
||||
} else if (work_mode == 2) {
|
||||
vals["ad_speed_req"] = 0.0;
|
||||
vals["ad_accelerate_req"] = accelerate_target;
|
||||
} else {
|
||||
vals["ad_speed_req"] = 0.0;
|
||||
vals["ad_accelerate_req"] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
if (autoware_mode_) {
|
||||
const double valid = autonomous_requested_ ? 1.0 : 0.0;
|
||||
vals["ad_accelerate_valid"] = valid;
|
||||
vals["ad_dbs_valid"] = valid;
|
||||
vals["ad_steering_valid"] = valid;
|
||||
vals["ad_accelerate_work_mode"] = 0.0;
|
||||
|
||||
if (!is_field_recent("ad_torque_control")) {
|
||||
vals["ad_torque_control"] = 0.0;
|
||||
}
|
||||
if (!is_field_recent("ad_brake_pressure_cmd")) {
|
||||
vals["ad_brake_pressure_cmd"] = 0.0;
|
||||
}
|
||||
if (!is_field_recent("ad_steering_angle_cmd")) {
|
||||
vals["ad_steering_angle_cmd"] = 0.0;
|
||||
}
|
||||
} else {
|
||||
// Preserve the previous YAML-driven behavior outside Autoware mode.
|
||||
vals["ad_accelerate_valid"] = 1.0;
|
||||
}
|
||||
return vals;
|
||||
}
|
||||
|
||||
void publish_frame(const TxFrameDef & def)
|
||||
{
|
||||
auto vals = resolve_values();
|
||||
int & counter = rolling_counters_[def.can_id];
|
||||
Payload p = encode_tx_frame(def, vals, counter);
|
||||
counter = (counter + 1) & 0x0F;
|
||||
|
||||
can_msgs::msg::Frame frame;
|
||||
frame.header.stamp = now().operator builtin_interfaces::msg::Time();
|
||||
frame.header.frame_id = "";
|
||||
frame.id = def.can_id;
|
||||
frame.is_extended = false;
|
||||
frame.is_rtr = false;
|
||||
frame.is_error = false;
|
||||
frame.dlc = 8;
|
||||
for (int i = 0; i < 8; ++i) frame.data[i] = p[i];
|
||||
pub_->publish(frame);
|
||||
}
|
||||
|
||||
rclcpp::Publisher<can_msgs::msg::Frame>::SharedPtr pub_;
|
||||
rclcpp::Subscription<VehicleControlCommand>::SharedPtr cmd_sub_;
|
||||
rclcpp::Subscription<ADControlAccelerate>::SharedPtr accelerate_cmd_sub_;
|
||||
rclcpp::Subscription<ADControlBrake>::SharedPtr brake_cmd_sub_;
|
||||
rclcpp::Subscription<ADControlSteering>::SharedPtr steering_cmd_sub_;
|
||||
rclcpp::Publisher<ADControlAccelerate>::SharedPtr converted_accelerate_pub_;
|
||||
rclcpp::Publisher<ADControlBrake>::SharedPtr converted_brake_pub_;
|
||||
rclcpp::Publisher<ADControlSteering>::SharedPtr converted_steering_pub_;
|
||||
rclcpp::Subscription<ActuationCommandStamped>::SharedPtr actuation_cmd_sub_;
|
||||
rclcpp::Subscription<GearCommand>::SharedPtr gear_cmd_sub_;
|
||||
rclcpp::Service<ControlModeCommand>::SharedPtr control_mode_srv_;
|
||||
std::vector<rclcpp::TimerBase::SharedPtr> timers_;
|
||||
|
||||
std::string output_topic_;
|
||||
std::string command_topic_;
|
||||
std::string accelerate_command_topic_;
|
||||
std::string brake_command_topic_;
|
||||
std::string steering_command_topic_;
|
||||
int command_timeout_ms_;
|
||||
bool autoware_mode_{true};
|
||||
std::string actuation_command_topic_;
|
||||
std::string gear_command_topic_;
|
||||
std::string control_mode_service_;
|
||||
double steering_command_ratio_{1.0};
|
||||
double autoware_accel_scale_{100.0};
|
||||
double autoware_brake_scale_{100.0};
|
||||
bool publish_converted_control_topics_{true};
|
||||
bool autonomous_requested_{false};
|
||||
double converted_accel_pct_{0.0};
|
||||
double converted_brake_pct_{0.0};
|
||||
double converted_steering_deg_{0.0};
|
||||
uint8_t converted_gear_{1};
|
||||
|
||||
std::unordered_map<std::string, double> last_command_;
|
||||
std::unordered_map<std::string, rclcpp::Time> last_command_field_time_;
|
||||
std::unordered_map<uint32_t, int> rolling_counters_;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<TeemoChassisSender>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+37
@@ -0,0 +1,37 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(teemo_chassis_msgs)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
set(msg_files
|
||||
"msg/VehicleControlCommand.msg"
|
||||
"msg/ADControlAccelerate.msg"
|
||||
"msg/ADControlBrake.msg"
|
||||
"msg/ADControlSteering.msg"
|
||||
"msg/VehicleStatus1.msg"
|
||||
"msg/VehicleStatus2.msg"
|
||||
"msg/VehicleStatus3.msg"
|
||||
"msg/VehicleMileage1.msg"
|
||||
"msg/VehicleMileage2.msg"
|
||||
"msg/VehicleErrorStatus.msg"
|
||||
"msg/VehicleOdometerStatus.msg"
|
||||
"msg/VehicleHVBatStatus.msg"
|
||||
"msg/WheelStatus.msg"
|
||||
"msg/VehicleDiagnosis.msg"
|
||||
"msg/VehicleVersion.msg"
|
||||
"msg/VehiclePwrCtrlStatus.msg"
|
||||
)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
${msg_files}
|
||||
DEPENDENCIES builtin_interfaces
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
+8
@@ -0,0 +1,8 @@
|
||||
builtin_interfaces/Time stamp
|
||||
|
||||
# DBC frame: AD_Control_Accelerate (0x1D2)
|
||||
uint8 ad_accelerate_valid
|
||||
uint8 ad_accelerate_work_mode
|
||||
uint8 ad_accelerate_gear
|
||||
float32 ad_speedor_torque_control
|
||||
uint8 ad_torque_control
|
||||
+7
@@ -0,0 +1,7 @@
|
||||
builtin_interfaces/Time stamp
|
||||
|
||||
# DBC frame: AD_Control_Brake (0x1D3)
|
||||
uint8 ad_dbs_valid
|
||||
uint8 ad_brake_pressure_cmd
|
||||
bool ad_dbs_workmode
|
||||
bool ad_awsc_flag
|
||||
+6
@@ -0,0 +1,6 @@
|
||||
builtin_interfaces/Time stamp
|
||||
|
||||
# DBC frame: AD_Control_Steering (0x1D4)
|
||||
uint8 ad_steering_valid
|
||||
float32 ad_steering_angle_cmd
|
||||
float32 ad_steering_speed_cmd
|
||||
+33
@@ -0,0 +1,33 @@
|
||||
builtin_interfaces/Time stamp
|
||||
|
||||
# AD_Control_Accelerate (0x1D2)
|
||||
uint8 ad_accelerate_valid
|
||||
uint8 ad_accelerate_work_mode
|
||||
uint8 ad_accelerate_gear
|
||||
float32 ad_speedor_torque_control
|
||||
uint8 ad_torque_control
|
||||
|
||||
# AD_Control_Brake (0x1D3)
|
||||
uint8 ad_dbs_valid
|
||||
uint8 ad_brake_pressure_cmd
|
||||
bool ad_dbs_workmode
|
||||
bool ad_awsc_flag
|
||||
|
||||
# AD_Control_Steering (0x1D4)
|
||||
uint8 ad_steering_valid
|
||||
float32 ad_steering_angle_cmd
|
||||
float32 ad_steering_speed_cmd
|
||||
|
||||
# AD_Control_Body (0x1DE)
|
||||
uint8 ad_body_valid
|
||||
uint8 ad_veh_light_swt_emerg_freq
|
||||
bool ad_low_beam
|
||||
bool ad_reversing_lights
|
||||
bool ad_double_flash_light
|
||||
bool ad_brake_light
|
||||
bool ad_horn_control
|
||||
bool ad_horn_control_1
|
||||
bool ad_fof_light
|
||||
bool ad_high_beam
|
||||
bool ad_right_turn_light
|
||||
bool ad_left_turn_light
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
bool tire_bur_st
|
||||
bool dropout_voltage
|
||||
bool ad_remote_break
|
||||
bool clean_fan_flag
|
||||
bool slippery_slopes_flag
|
||||
bool slip_flag
|
||||
uint8 vcu_veh_cannot_pwr_off
|
||||
bool vcu_adota_flag
|
||||
bool energy_recovery_flag
|
||||
bool horn_2_state
|
||||
bool vcu_veh_rdy
|
||||
bool ads_light_state
|
||||
bool kers_limited
|
||||
bool oil_pot_state
|
||||
bool business_relay_state
|
||||
bool relay_4_g_state
|
||||
bool radar_relay_state
|
||||
bool orin_relay_state
|
||||
bool motor_temp_state
|
||||
bool fog_light_state
|
||||
bool power_button_state
|
||||
bool epb_button_state
|
||||
bool motor_torque_limit_state
|
||||
bool b_press_switch_collision_state
|
||||
bool remo_touch_switch_disable_state
|
||||
bool f_press_switch_collision_state
|
||||
bool b_touch_switch_disable_state
|
||||
bool l_touch_switch_disable_state
|
||||
bool r_touch_switch_disable_state
|
||||
bool f_touch_switch_disable_state
|
||||
bool r_touch_switch_collision_state
|
||||
bool l_touch_switch_collision_state
|
||||
uint8 ad_fault_code
|
||||
bool epb_diagnosis
|
||||
bool move_switch
|
||||
bool low_beam_state
|
||||
bool reversing_lights_state
|
||||
bool tire_sensor_state
|
||||
bool brake_light_state
|
||||
uint8 vehicle_fault_grade
|
||||
bool eps_state
|
||||
uint8 vcu_301_rolling_counter
|
||||
bool horn_1_state
|
||||
bool high_beam_state
|
||||
bool right_turn_light_state
|
||||
bool left_turn_light_state
|
||||
bool b_touch_switch_collision_state
|
||||
bool f_touch_switch_collision_state
|
||||
bool bms_state
|
||||
bool emergency_button_state
|
||||
bool dbs_state
|
||||
bool ad_state
|
||||
bool remote_state
|
||||
bool motor_state
|
||||
+20
@@ -0,0 +1,20 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
bool flg_warning
|
||||
uint8 vehicle_meter_soc
|
||||
bool residual_pressure
|
||||
bool charge_abnormal
|
||||
|
||||
# V
|
||||
float32 low_voltage
|
||||
|
||||
uint8 vcu_pwr_st
|
||||
uint8 vcu_can1_fault
|
||||
uint8 vcu_can0_fault
|
||||
bool vcu_epb_actin_when_ebsmf
|
||||
bool vcu_eeprom_fault
|
||||
bool vcu_ebs_actin_when_epbmf
|
||||
uint16 vcu_error_code
|
||||
+19
@@ -0,0 +1,19 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
uint8 vehicle_poweroff_channel
|
||||
uint8 vehicle_poweroff_countdown_time
|
||||
uint8 battery_work_state
|
||||
uint8 vehicle_soc
|
||||
uint8 vehicle_hv_bat_msg_cntr
|
||||
|
||||
# V
|
||||
float32 high_voltage_battery_voltage
|
||||
|
||||
# degC
|
||||
float32 high_voltage_battery_max_tem
|
||||
|
||||
# A
|
||||
float32 high_voltage_battery_current
|
||||
+12
@@ -0,0 +1,12 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
uint8 vehicle_mileage1_msg_cntr
|
||||
|
||||
# km
|
||||
float32 vchicle_ad_mileage1
|
||||
|
||||
# km
|
||||
float32 vehicle_odo1
|
||||
+12
@@ -0,0 +1,12 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
uint8 vehicle_mileage2_msg_cntr
|
||||
|
||||
# cm
|
||||
uint32 vehicle_trip1
|
||||
|
||||
# km
|
||||
float32 vehicle_remote_mileage1
|
||||
+18
@@ -0,0 +1,18 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
uint8 vehicle_odometer_msg_cntr
|
||||
|
||||
# km
|
||||
float32 vehicle_ad_mileage
|
||||
|
||||
# km
|
||||
float32 vehicle_remote_mileage
|
||||
|
||||
# km
|
||||
float32 vehicle_trip
|
||||
|
||||
# km
|
||||
float32 vehicle_odo
|
||||
+27
@@ -0,0 +1,27 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
uint8 vcu_state_flag
|
||||
uint8 vcu_power_st
|
||||
uint8 vcu_init
|
||||
bool power_button_state
|
||||
bool vcu_veh_kl15_sts
|
||||
bool vcu_mcukl15_sts
|
||||
bool vcu_bmskl15_sts
|
||||
uint8 vcu_bt_version2
|
||||
uint8 vcu_bt_version1
|
||||
bool vcu_ab_soft_st_old
|
||||
bool vcu_ab_soft_st_new
|
||||
uint8 vcu_cp_sts
|
||||
bool vcu_ota_flag
|
||||
uint8 vcu_lv_mute_charge_st
|
||||
bool vcu_abn_pwr_off
|
||||
uint8 vcu_pwr_down_st
|
||||
bool vcu_veh_rdy_diag_enable
|
||||
|
||||
# V
|
||||
float32 vcu_low_batt_volt
|
||||
|
||||
uint8 vcu_weak_up_sig
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
uint8 vehicle_charge_sts
|
||||
uint8 vehicle_brk_config
|
||||
|
||||
# km
|
||||
uint16 vehicle_range
|
||||
|
||||
uint8 drive_mode_state
|
||||
bool epb_status
|
||||
uint8 accelerator_pedal_status
|
||||
uint8 brake_pedal_status
|
||||
uint8 vcu_303_rolling_counter
|
||||
uint8 vehicle_gear
|
||||
+20
@@ -0,0 +1,20 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
bool whl_spd_sens_lvl_sts_rr
|
||||
bool whl_spd_sens_lvl_sts_rl
|
||||
bool whl_spd_sens_lvl_sts_fr
|
||||
bool whl_spd_sens_lvl_sts_fl
|
||||
bool vehicle_speed_vaild
|
||||
uint8 vcu_304_rolling_counter
|
||||
|
||||
# deg
|
||||
float32 vehicle_steering_angle
|
||||
|
||||
# MPa
|
||||
float32 vehicle_brake_pressure
|
||||
|
||||
# km/h
|
||||
float32 vehicle_speed
|
||||
+7
@@ -0,0 +1,7 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
# deg/s
|
||||
float32 vehicle_steering_spd
|
||||
+14
@@ -0,0 +1,14 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
uint8 car_model
|
||||
uint8 day
|
||||
uint8 month
|
||||
uint8 year
|
||||
uint8 byte5
|
||||
uint8 byte4
|
||||
uint8 byte3
|
||||
uint8 byte2
|
||||
uint8 byte1
|
||||
+19
@@ -0,0 +1,19 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
|
||||
string wheel
|
||||
|
||||
uint8 wheel_speed_sensor_error
|
||||
bool valid_flag
|
||||
bool slip_flag
|
||||
uint8 sensor_attr
|
||||
bool tire_leak_state
|
||||
uint8 pressure_warning
|
||||
float32 tire_temperature
|
||||
float32 tire_pressure
|
||||
bool sensor_state
|
||||
float32 wheel_speed
|
||||
uint8 wss_pul_cnt
|
||||
uint8 wheel_status_msg_cntr
|
||||
+22
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>teemo_chassis_msgs</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS 2 message definitions for the TEEMO chassis CAN driver.</description>
|
||||
|
||||
<maintainer email="codex@example.com">Codex</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
|
||||
<depend>builtin_interfaces</depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,100 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(ecar_can_driver)
|
||||
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(can_msgs REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(autoware_vehicle_msgs REQUIRED)
|
||||
find_package(tier4_vehicle_msgs REQUIRED)
|
||||
find_package(autoware_adapi_v1_msgs REQUIRED)
|
||||
find_package(autoware_control_msgs REQUIRED)
|
||||
|
||||
include_directories(include)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/SteerStatus.msg"
|
||||
"msg/BrakeStatus.msg"
|
||||
"msg/DriveStatus.msg"
|
||||
"msg/VehDyncState.msg"
|
||||
"msg/VehState.msg"
|
||||
"msg/SteerCmd.msg"
|
||||
"msg/BrakeCmd.msg"
|
||||
"msg/DriveCmd.msg"
|
||||
"msg/BodyCmd.msg"
|
||||
"msg/ParkCmd.msg"
|
||||
"msg/PowerCmd.msg"
|
||||
"msg/FaultReport.msg"
|
||||
DEPENDENCIES std_msgs
|
||||
)
|
||||
|
||||
# ========== CAN receiver node (parses /kvaser/can_tx → ROS topics) ==========
|
||||
add_executable(can_receiver_node
|
||||
src/can_receiver_node.cpp
|
||||
src/dbc_codec.cpp
|
||||
)
|
||||
ament_target_dependencies(can_receiver_node
|
||||
rclcpp
|
||||
std_msgs
|
||||
can_msgs
|
||||
builtin_interfaces
|
||||
autoware_vehicle_msgs
|
||||
tier4_vehicle_msgs
|
||||
)
|
||||
|
||||
# ========== CAN sender node (builds control frames → kvaser_output) ==========
|
||||
add_executable(can_sender_node
|
||||
src/can_sender_node.cpp
|
||||
src/dbc_codec.cpp
|
||||
)
|
||||
ament_target_dependencies(can_sender_node
|
||||
rclcpp
|
||||
std_msgs
|
||||
can_msgs
|
||||
autoware_vehicle_msgs
|
||||
tier4_vehicle_msgs
|
||||
autoware_adapi_v1_msgs
|
||||
autoware_control_msgs
|
||||
)
|
||||
|
||||
rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
target_link_libraries(can_receiver_node "${cpp_typesupport_target}")
|
||||
target_link_libraries(can_sender_node "${cpp_typesupport_target}")
|
||||
|
||||
install(TARGETS
|
||||
can_receiver_node
|
||||
can_sender_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,170 @@
|
||||
# ecar_can_driver
|
||||
|
||||
基于 `ECAR_UCAN_ComMatrix_V2.05.dbc` 的 ROS2 C++ CAN 驱动。
|
||||
CAN 硬件层使用 **Kvaser** (通过标准 ROS2 话题 `/kvaser/can_tx` / `kvaser_output` 与
|
||||
`kvaser_interface` / `ros2_socketcan` 等桥接节点对接)。
|
||||
|
||||
---
|
||||
|
||||
## 1. 话题 (Topics)
|
||||
|
||||
### 订阅
|
||||
|
||||
| 方向 | Topic | 类型 | 说明 |
|
||||
| :--: | :-- | :-- | :-- |
|
||||
| 输入 | `can_rx_topic` 参数指定, 默认 `/kvaser/can_tx` | `can_msgs/msg/Frame` | 从 Kvaser 收到的 CAN 总线原始帧 |
|
||||
|
||||
### 发布 —— 车辆状态聚合消息 (topic 名按 DBC 报文名对齐)
|
||||
|
||||
| Topic | 类型 | DBC 源 |
|
||||
| :-- | :-- | :-- |
|
||||
| `/vehicle/cdcu_steer_status` | `ecar_can_driver/msg/SteerStatus` | `CDCU_SteerStatus` |
|
||||
| `/vehicle/cdcu_brake_status` | `ecar_can_driver/msg/BrakeStatus` | `CDCU_BrakeStatus` |
|
||||
| `/vehicle/cdcu_drive_status` | `ecar_can_driver/msg/DriveStatus` | `CDCU_DriveStatus` |
|
||||
| `/vehicle/cdcu_veh_dync_state` | `ecar_can_driver/msg/VehDyncState` | `CDCU_VehDyncState` |
|
||||
| `/vehicle/cdcu_veh_state` | `ecar_can_driver/msg/VehState` | `CDCU_VehState` |
|
||||
|
||||
每条聚合消息都包含:
|
||||
|
||||
- `header`: 原始 CAN 帧时间戳
|
||||
- `can_id`: 原始 CAN ID
|
||||
- `dlc`: 原始 DLC
|
||||
- `raw_data`: 原始 8 字节数据
|
||||
- 对应 DBC 解析后的全部信号字段
|
||||
|
||||
字段命名规则:
|
||||
|
||||
- ROS2 topic 名按 DBC 报文名转成小写下划线, 例如 `CDCU_SteerStatus -> /vehicle/cdcu_steer_status`
|
||||
- 自定义消息字段名按 DBC 信号名转成小写下划线, 例如 `CDCU_EPS_StrWhlAngle -> cdcu_eps_str_whl_angle`
|
||||
|
||||
这样可以直接用 `ros2 topic echo /vehicle/cdcu_steer_status` 实时查看整帧解析结果。
|
||||
|
||||
### 发布 —— 单值状态话题 (topic 名按 DBC 信号名对齐)
|
||||
|
||||
| Topic | 类型 | DBC 源 |
|
||||
| :-- | :-- | :-- |
|
||||
| `/vehicle/cdcu_eps_str_whl_angle` | `std_msgs/msg/Float32` | CDCU_SteerStatus.CDCU_EPS_StrWhlAngle |
|
||||
| `/vehicle/cdcu_eps_str_trq` | `std_msgs/msg/Float32` | CDCU_SteerStatus.CDCU_EPS_StrTrq |
|
||||
| `/vehicle/cdcu_eps_whl_spd` | `std_msgs/msg/Float32` | CDCU_SteerStatus.CDCU_EPS_WhlSpd |
|
||||
| `/vehicle/cdcu_ehb_brk_presur` | `std_msgs/msg/Float32` | CDCU_BrakeStatus.CDCU_EHB_BrkPresur |
|
||||
| `/vehicle/cdcu_ehb_brk_pedpos` | `std_msgs/msg/Float32` | CDCU_BrakeStatus.CDCU_EHB_BrkPedpos |
|
||||
| `/vehicle/cdcu_mcu_throt_act` | `std_msgs/msg/Float32` | CDCU_DriveStatus.CDCU_MCU_ThrotAct |
|
||||
| `/vehicle/cdcu_mcu_mtr_curt` | `std_msgs/msg/Float32` | CDCU_DriveStatus.CDCU_MCU_MtrCurt |
|
||||
| `/vehicle/cdcu_mcu_mtr_spd` | `std_msgs/msg/Float32` | CDCU_DriveStatus.CDCU_MCU_MtrSpd |
|
||||
| `/vehicle/cdcu_mcu_gear_act` | `std_msgs/msg/UInt8` | CDCU_DriveStatus.CDCU_MCU_GearAct |
|
||||
| `/vehicle/cdcu_veh_longtdnal_spd` | `std_msgs/msg/Float32` | CDCU_VehDyncState.CDCU_Veh_LongtdnalSpd |
|
||||
| `/vehicle/cdcu_veh_longtdnal_acc_spd` | `std_msgs/msg/Float32` | CDCU_VehDyncState.CDCU_Veh_LongtdnalAccSpd |
|
||||
| `/vehicle/cdcu_veh_run_mode` | `std_msgs/msg/UInt8` | CDCU_VehState.CDCU_Veh_RunMode |
|
||||
|
||||
### 发布 —— 控制命令 (由 `can_sender_node` 按 DBC 编码后经 Kvaser 发到总线)
|
||||
|
||||
| 方向 | Topic | 类型 | 说明 |
|
||||
| :--: | :-- | :-- | :-- |
|
||||
| 输出 | `can_tx_topic` 参数指定, 默认 `kvaser_output` | `can_msgs/msg/Frame` | 待下发到 CAN 总线的帧 |
|
||||
|
||||
其中包含 6 个 ID:
|
||||
|
||||
| CAN ID | 报文 | 本驱动写死的内容 |
|
||||
| :---: | :---- | :----- |
|
||||
| `273 (0x111)` | `ADCU_BrakeCmd` | Activate 在首次发送后保持 `0` 持续 20ms, 随后跳到 `1`; CtrlMode=1, 踏板=100%, 目标压力=100bar, 目标减速度=-8m/s² |
|
||||
| `274 (0x112)` | `ADCU_ParkCmd` | Activate 在首次发送后保持 `0` 持续 20ms, 随后跳到 `1`; Enable=可配置 (默认 false) |
|
||||
| `275 (0x113)` | `ADCU_SteerCmd` | Activate 在首次发送后保持 `0` 持续 20ms, 随后跳到 `1`; CtrlMode=1 (角度模式), 目标角度=90° (默认), 角速度 30°/s |
|
||||
| `276 (0x114)` | `ADCU_DriveCmd` | Activate 在首次发送后保持 `0` 持续 20ms, 随后跳到 `1`; CtrlMode=1 (踏板模式), **TgtPedpos=0% (驱动扭矩为 0)**, 档位=1 |
|
||||
| `277 (0x115)` | `ADCU_BodyCmd` | 作为必发控制帧补发, 默认功能位为 0, Activate 按同样 20ms 策略, RollCnt 固定 0 |
|
||||
| `279 (0x117)` | `ADCU_PowerCmd` (`BatCmd`) | 作为必发控制帧补发, 默认功能位为 0, RollCnt 固定 0 |
|
||||
|
||||
---
|
||||
|
||||
## 2. 校验和 (Checksum)
|
||||
|
||||
```
|
||||
Byte7 = (Byte0 + Byte1 + Byte2 + Byte3 + Byte4 + Byte5 + Byte6) ^ 0xFF
|
||||
```
|
||||
|
||||
实现位于 `src/dbc_codec.cpp` 中的 `sum_checksum()` / `apply_checksum()`。
|
||||
每一帧控制命令在所有数据位填充完成后 (当前 `RollCnt` 固定为 0), 最后调用
|
||||
`apply_checksum()` 将结果写入 `Byte7`。
|
||||
|
||||
---
|
||||
|
||||
## 3. 编译
|
||||
|
||||
```bash
|
||||
# 依赖: ros-${ROS_DISTRO}-can-msgs
|
||||
sudo apt install ros-${ROS_DISTRO}-can-msgs
|
||||
|
||||
# 在本 workspace 根目录编译
|
||||
colcon build --packages-select ecar_can_driver
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
如果修改了 workspace 目录名, 请删除旧的 `build/`、`install/`、`log/` 后重新编译;
|
||||
这些目录会缓存编译时的绝对路径。
|
||||
|
||||
## 4. 运行
|
||||
|
||||
```bash
|
||||
# 方式1: 分别启动
|
||||
ros2 run ecar_can_driver can_receiver_node
|
||||
ros2 run ecar_can_driver can_sender_node
|
||||
|
||||
# 方式2: launch 启动 (默认读取 config/ecar_can_driver.params.yaml, 也可带参数覆盖)
|
||||
ros2 launch ecar_can_driver ecar_can_driver.launch.py \
|
||||
target_steer_angle_deg:=45.0 \
|
||||
publish_rate_hz:=100.0 \
|
||||
activate_low_duration_ms:=20.0
|
||||
```
|
||||
|
||||
也可以显式指定参数 YAML:
|
||||
|
||||
```bash
|
||||
ros2 launch ecar_can_driver ecar_can_driver.launch.py \
|
||||
params_file:=$(ros2 pkg prefix ecar_can_driver)/share/ecar_can_driver/config/ecar_can_driver.params.yaml
|
||||
```
|
||||
|
||||
YAML 示例:
|
||||
|
||||
```yaml
|
||||
ecar_can_receiver:
|
||||
ros__parameters:
|
||||
can_rx_topic: /kvaser/can_tx
|
||||
|
||||
ecar_can_sender:
|
||||
ros__parameters:
|
||||
can_tx_topic: kvaser_output
|
||||
```
|
||||
|
||||
也就是说，接收和发送的 CAN 话题名现在直接改这个 YAML 即可，launch 默认不会再把它们覆盖掉。
|
||||
|
||||
---
|
||||
|
||||
## 5. 文件结构
|
||||
|
||||
```
|
||||
ecar_can_driver/
|
||||
├── CMakeLists.txt
|
||||
├── package.xml
|
||||
├── README.md
|
||||
├── config/
|
||||
│ └── ecar_can_driver.params.yaml
|
||||
├── include/
|
||||
│ └── ecar_can_driver/
|
||||
│ └── dbc_codec.hpp # Motorola 大端编解码 + 信号表 + 校验
|
||||
├── src/
|
||||
│ ├── dbc_codec.cpp
|
||||
│ ├── can_receiver_node.cpp # /kvaser/can_tx → /vehicle/cdcu_*
|
||||
│ └── can_sender_node.cpp # 写死控制命令 → /kvaser_output
|
||||
└── launch/
|
||||
└── ecar_can_driver.launch.py
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 6. 扩展
|
||||
|
||||
- 如需解析更多 ID (例如 `CDCU_VehFtWhlSpd`、`CDCU_BatStatus` 等),
|
||||
在 `dbc_codec.hpp::sig` 中按 DBC 继续添加 `SignalSpec`, 然后在
|
||||
`can_receiver_node.cpp::on_can_frame()` 的 `switch` 中增加 case 即可。
|
||||
- 如需从上层动态下发控制量, 把 `CanSenderNode` 里对应的 `target_*`
|
||||
常量改为订阅 ROS 话题 (例如 `/control/steer_cmd`) 回调更新即可, 帧
|
||||
构造/校验流程完全不变。
|
||||
@@ -0,0 +1,20 @@
|
||||
ecar_can_receiver:
|
||||
ros__parameters:
|
||||
can_rx_topic: /socket_can/from_can_bus
|
||||
|
||||
ecar_can_sender:
|
||||
ros__parameters:
|
||||
autoware_mode: true
|
||||
can_tx_topic: /socket_can/to_can_bus
|
||||
command_timeout_ms: 100.0
|
||||
target_steer_angle_deg: 0.0
|
||||
target_brake_pedal_pct: 0.0
|
||||
target_brake_press_bar: 0.0
|
||||
target_brake_accel_mps2: 0.0
|
||||
target_drive_pedpos_pct: 0.0
|
||||
target_drive_gear: 1
|
||||
target_drive_ctrl_mode: 1 # 0: 油门模式, 1: 速度模式
|
||||
publish_rate_hz: 50.0
|
||||
body_power_rate_hz: 10.0
|
||||
park_enable: false
|
||||
activate_low_duration_ms: 20.0
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -0,0 +1 @@
|
||||
,zhq,zhq,23.04.2026 12:43,/home/zhq/.local/share/onlyoffice;
|
||||
Binary file not shown.
@@ -0,0 +1,29 @@
|
||||
序号,消息定义,数据类型,话题名称,对应autoware消息类型,易咖智车对应CAN报文,易咖对应CAN消息,尚元智行对应CAN报文,尚元智行对应CAN消息
|
||||
autoware需要底盘反馈的消息,,,,,,,,,
|
||||
1,控制模式反馈,int,/vehicle/status/control_mode,autoware_vehicle_msgs::msg::ControlModeReport,CDCU_VehState,CDCU_Veh_RunMode,VCU_Vehicle_Status_1,Drive_Mode_State,
|
||||
2,车辆纵向速度反馈,float,/vehicle/status/velocity_status,autoware_vehicle_msgs::msg::VelocityReport,CDCU_VehDyncState,CDCU_Veh_LongtdnalSpd,VCU_Vehicle_Status_2,Vehicle_Speed,
|
||||
3,车轮转角反馈,float,/vehicle/status/steering_status,autoware_vehicle_msgs::msg::SteeringReport,CDCU_SteerStatus,CDCU_EPS_StrWhlAngle,VCU_Vehicle_Status_2,Vehicle_Steering_Angle,
|
||||
4,实际挡位反馈,int,/vehicle/status/gear_status,autoware_vehicle_msgs::msg::GearReport,CDCU_DriveStatus,CDCU_MCU_GearAct,VCU_Vehicle_Status_1,Vehicle_Gear,
|
||||
5,踏板反馈（包括油门和刹车）,float,/vehicle/status/actuation_status,tier4_vehicle_msgs::msg::ActuationStatusStamped,"CDCU_DriveStatus
|
||||
CDCU_BrakeStatus","CDCU_MCU_ThrotAct
|
||||
CDCU_EHB_BrkPedpos",VCU_Vehicle_Status_1,"Accelerator_Pedal_Status
|
||||
Brake_Pedal_Status",
|
||||
,,,,,,,,,
|
||||
autoware给底盘发送的消息,,,,,,,,,
|
||||
1,控制模式,int,/control/control_mode_request,autoware_vehicle_msgs::srv::ControlModeCommand,"易咖智车需要ADCU_BrakeCmd、ADCU_ParkCmd、
|
||||
ADCU_SteerCmd、 ADCU_DriveCmd、ADCU_BodyCmd、ADCU_PowerCmd
|
||||
六帧自动驾驶指令报文必须按协议要求周期性发送，并且报文
|
||||
ADCU_BrakeCmd、ADCU_ParkCmd、ADCU_SteerCmd、ADCU_DriveCmd 中
|
||||
的激活信号 ADCU_Brk_Active、ADCU_Prk_Active、ADCU_Str_Active、
|
||||
ADCU_Drv_Active 必须在 500ms 时间窗内均完成上升沿触发，则底盘进入自动
|
||||
驾驶模式。",,AD_Control_Accelerate,AD_Accelerate_Valid置一则进入智驾模式,
|
||||
2,控制指令(包括油门/刹车/转向),float,/control/command/actuation_cmd,tier4_vehicle_msgs::msg::ActuationCommandStamped,"ADCU_DriveCmd
|
||||
ADCU_BrakeCmd
|
||||
ADCU_SteerCmd","ADCU_Drv_TgtPedpos
|
||||
ADCU_Str_TgtAngle
|
||||
ADCU_Brk_TgtPedpos","AD_Control_Accelerate
|
||||
AD_Control_Brake
|
||||
AD_Control_Steering","AD_Torque_Control
|
||||
AD_BrakePressure_Req
|
||||
AD_Steering_Angle_Cmd",
|
||||
3,挡位指令,int,/control/command/gear_cmd,autoware_vehicle_msgs::msg::GearCommand,ADCU_DriveCmd,ADCU_Drv_TgtGear,AD_Control_Accelerate,AD_Accelerate_Gear
|
||||
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,212 @@
|
||||
// ============================================================================
|
||||
// dbc_codec.hpp
|
||||
// ECAR_UCAN_ComMatrix DBC 编解码工具 (Motorola 大端 / 非多路复用)
|
||||
//
|
||||
// DBC 中信号的位定义 (格式: start|len@0+) 表示:
|
||||
// - start : 起始位 (Motorola 约定, byte-wise 大端, bit-wise 小端)
|
||||
// - len : 长度 bit
|
||||
// - @0 : Motorola (Big-Endian byte order)
|
||||
// - + : 无符号; '-' 表示有符号二进制补码
|
||||
//
|
||||
// 传统 Motorola 编码 (forward / sequential): 起始位为每个字节内的 MSB 位号
|
||||
// (bit7 = MSB, bit0 = LSB), 字节递增方向从 start/8 向后存放.
|
||||
//
|
||||
// 物理值 = raw * factor + offset
|
||||
// raw = (physical - offset) / factor
|
||||
// ============================================================================
|
||||
#ifndef ECAR_CAN_DRIVER__DBC_CODEC_HPP_
|
||||
#define ECAR_CAN_DRIVER__DBC_CODEC_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
|
||||
namespace ecar_can_driver
|
||||
{
|
||||
|
||||
using CanData = std::array<uint8_t, 8>;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// 底层位操作 —— Motorola Big-Endian
|
||||
// ---------------------------------------------------------------------------
|
||||
// 从 8 字节 CAN data 中读取 Motorola 信号
|
||||
uint64_t extract_motorola(const uint8_t * data, uint8_t start_bit, uint8_t length);
|
||||
|
||||
// 把 raw 值写入 8 字节 CAN data
|
||||
void insert_motorola(uint8_t * data, uint8_t start_bit, uint8_t length, uint64_t raw);
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// 符号扩展 (当 DBC 信号为带符号时)
|
||||
// ---------------------------------------------------------------------------
|
||||
int64_t sign_extend(uint64_t raw, uint8_t length);
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// 校验码: (Byte0 + Byte1 + ... + Byte6) ^ 0xFF 写入 Byte7
|
||||
// ---------------------------------------------------------------------------
|
||||
uint8_t sum_checksum(const uint8_t * data, size_t len = 7);
|
||||
|
||||
// 为兼容旧接口保留该别名 (当前实现已改为求和校验)
|
||||
inline uint8_t xor_checksum(const uint8_t * data, size_t len = 7)
|
||||
{
|
||||
return sum_checksum(data, len);
|
||||
}
|
||||
|
||||
// 把校验写入 Byte7
|
||||
void apply_checksum(uint8_t * data);
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// RollCnt (0..15) 自动递增工具
|
||||
// ---------------------------------------------------------------------------
|
||||
class RollCounter
|
||||
{
|
||||
public:
|
||||
uint8_t next() { value_ = (value_ + 1) & 0x0F; return value_; }
|
||||
uint8_t get() const { return value_; }
|
||||
private:
|
||||
uint8_t value_ = 0;
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// 高阶封装: 按物理值 set / get 信号
|
||||
// ---------------------------------------------------------------------------
|
||||
struct SignalSpec
|
||||
{
|
||||
uint8_t start_bit;
|
||||
uint8_t length;
|
||||
double factor;
|
||||
double offset;
|
||||
bool is_signed;
|
||||
};
|
||||
|
||||
double decode_physical(const uint8_t * data, const SignalSpec & s);
|
||||
void encode_physical(uint8_t * data, const SignalSpec & s, double physical);
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// CAN 消息 ID (来自 DBC)
|
||||
// —— 仅列出本驱动关心的 ID, 其余解析为原始字节
|
||||
// ---------------------------------------------------------------------------
|
||||
namespace msg_id
|
||||
{
|
||||
// ==== CDCU → ADCU (车辆状态报文) ====
|
||||
constexpr uint32_t CDCU_BrakeStatus = 529; // 0x211
|
||||
constexpr uint32_t CDCU_BrakeDiag = 530; // 0x212 诊断
|
||||
constexpr uint32_t CDCU_ParkStatus = 531; // 0x213
|
||||
constexpr uint32_t CDCU_ParkDiag = 532; // 0x214 诊断
|
||||
constexpr uint32_t CDCU_SteerStatus = 533; // 0x215
|
||||
constexpr uint32_t CDCU_SteerDiag = 534; // 0x216 诊断
|
||||
constexpr uint32_t CDCU_DriveStatus = 535; // 0x217
|
||||
constexpr uint32_t CDCU_DriveDiag = 536; // 0x218 诊断
|
||||
constexpr uint32_t CDCU_BodyStatus = 537; // 0x219
|
||||
constexpr uint32_t CDCU_BatStatus = 546; // 0x222
|
||||
constexpr uint32_t CDCU_BatDiag = 547; // 0x223 诊断
|
||||
constexpr uint32_t CDCU_VehState = 576; // 0x240
|
||||
constexpr uint32_t CDCU_VehDyncState = 592; // 0x250
|
||||
constexpr uint32_t CDCU_VehFtWhlSpd = 593; // 0x251
|
||||
constexpr uint32_t CDCU_VehRrWhlSpd = 594; // 0x252
|
||||
constexpr uint32_t CDCU_AxisAcc = 602; // 0x25A
|
||||
constexpr uint32_t CDCU_AxisAngularSpd = 603; // 0x25B
|
||||
constexpr uint32_t CDCU_PowerStatus = 608; // 0x260
|
||||
|
||||
// ==== ADCU → CDCU (控制命令报文) ====
|
||||
constexpr uint32_t ADCU_BrakeCmd = 273; // 0x111
|
||||
constexpr uint32_t ADCU_ParkCmd = 274; // 0x112
|
||||
constexpr uint32_t ADCU_SteerCmd = 275; // 0x113
|
||||
constexpr uint32_t ADCU_DriveCmd = 276; // 0x114
|
||||
constexpr uint32_t ADCU_BodyCmd = 277; // 0x115
|
||||
constexpr uint32_t ADCU_PowerCmd = 279; // 0x117 (常称 BatCmd)
|
||||
} // namespace msg_id
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// DBC 中部分关键信号规格 (编解码时直接使用)
|
||||
// ---------------------------------------------------------------------------
|
||||
namespace sig
|
||||
{
|
||||
// -------- CDCU_SteerStatus (ID 533) --------
|
||||
inline constexpr SignalSpec CDCU_EPS_StrWhlAngle {23, 16, 0.005, -90.0, false};
|
||||
inline constexpr SignalSpec CDCU_EPS_StrTrq {15, 8, 0.1, -12.8, false};
|
||||
inline constexpr SignalSpec CDCU_EPS_WhlSpd {39, 16, 0.01, -180.0, false};
|
||||
|
||||
// -------- CDCU_BrakeStatus (ID 529) --------
|
||||
inline constexpr SignalSpec CDCU_EHB_BrkPresur {15, 8, 1.0, 0.0, false}; // bar
|
||||
inline constexpr SignalSpec CDCU_EHB_BrkPedpos {23, 8, 0.4, 0.0, false}; // %
|
||||
|
||||
// -------- CDCU_DriveStatus (ID 535) --------
|
||||
inline constexpr SignalSpec CDCU_MCU_ThrotAct {15, 8, 0.4, 0.0, false}; // %
|
||||
inline constexpr SignalSpec CDCU_MCU_MtrCurt {23, 16, 0.1, 0.0, false}; // A
|
||||
inline constexpr SignalSpec CDCU_MCU_MtrSpd {39, 16, 1.0, 0.0, false}; // rpm
|
||||
inline constexpr SignalSpec CDCU_MCU_GearAct { 3, 2, 1.0, 0.0, false};
|
||||
|
||||
// -------- CDCU_VehDyncState (ID 592) --------
|
||||
inline constexpr SignalSpec CDCU_Veh_LongtdnalSpd {23, 16, 0.00390625, 0.0, false}; // Kmph
|
||||
inline constexpr SignalSpec CDCU_Veh_LongtdnalAccSpd{ 7, 16, 0.01, -40.0, false}; // mps2
|
||||
|
||||
// -------- CDCU_VehState (ID 576) --------
|
||||
inline constexpr SignalSpec CDCU_Veh_RunMode { 7, 4, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec CDCU_TurnLLamp_St {13, 1, 1.0, 0.0, false}; // 左转向灯
|
||||
inline constexpr SignalSpec CDCU_TurnRLamp_St {12, 1, 1.0, 0.0, false}; // 右转向灯
|
||||
inline constexpr SignalSpec CDCU_DblFlashLamp_St {14, 1, 1.0, 0.0, false}; // 双闪灯
|
||||
|
||||
// -------- CDCU_DriveDiag (ID 536) -------- 诊断报文
|
||||
inline constexpr SignalSpec CDCU_MCUCANCom_Err {16, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec CDCU_MCUOverTemp_Err {17, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec CDCU_MCUMtrOverTemp_Err {31, 1, 1.0, 0.0, false};
|
||||
|
||||
// -------- CDCU_BrakeDiag (ID 530) -------- 诊断报文
|
||||
inline constexpr SignalSpec CDCU_EHBMsgOffline_Err {12, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec CDCU_EHBOverTemp_Err {20, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec CDCU_EHBCAN_Err {29, 1, 1.0, 0.0, false};
|
||||
|
||||
// -------- CDCU_SteerDiag (ID 534) -------- 诊断报文
|
||||
inline constexpr SignalSpec CDCU_EPSMsgOffline_Err {12, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec EPS_CANCom_Err {17, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec EPS_MtrOverTemp_Err {21, 1, 1.0, 0.0, false};
|
||||
|
||||
// -------- CDCU_ParkDiag (ID 532) -------- 诊断报文
|
||||
inline constexpr SignalSpec CDCU_EPBMsgOffline_Err {12, 1, 1.0, 0.0, false};
|
||||
|
||||
// -------- CDCU_BatDiag (ID 547) -------- 诊断报文
|
||||
inline constexpr SignalSpec CDCU_BMSMsgOffline_Err { 4, 1, 1.0, 0.0, false};
|
||||
|
||||
// -------- ADCU_SteerCmd (ID 275) --------
|
||||
inline constexpr SignalSpec ADCU_Str_Active { 7, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Str_CtrlMode { 6, 2, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Str_TgtAngle {15, 16, 0.005, -90.0, false};
|
||||
inline constexpr SignalSpec ADCU_Str_TgtCurvature{31, 16, 0.0001, -3.0, false};
|
||||
inline constexpr SignalSpec ADCU_Str_TgtAngleSpd {47, 8, 0.2, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_StrCmd_RollCnt {51, 4, 1.0, 0.0, false};
|
||||
|
||||
// -------- ADCU_BrakeCmd (ID 273) --------
|
||||
inline constexpr SignalSpec ADCU_Brk_Active { 7, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Brk_CtrlMode { 6, 2, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Brk_TgtPedpos {15, 8, 0.4, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Brk_TgtPress {23, 16, 0.01, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Brk_TgtAccSpd {39, 16, 0.01, -20.0, false};
|
||||
inline constexpr SignalSpec ADCU_BrkCmd_RollCnt {51, 4, 1.0, 0.0, false};
|
||||
|
||||
// -------- ADCU_DriveCmd (ID 276) --------
|
||||
inline constexpr SignalSpec ADCU_Drv_Active { 7, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Drv_CtrlMode { 6, 2, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Drv_TgtGear { 1, 2, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Drv_TgtPedpos {15, 8, 1.0, -100.0, false}; // -100..155 (%)
|
||||
inline constexpr SignalSpec ADCU_Drv_TgtVehSpd0 {23, 16, 0.01,-100.0, false};
|
||||
inline constexpr SignalSpec ADCU_Drv_TgtVehAccSpd{39, 8, 0.1, -15.0, false};
|
||||
inline constexpr SignalSpec ADCU_Drv_VehSpdLimit {47, 8, 0.4, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_DrvCmd0_RollCnt {51, 4, 1.0, 0.0, false};
|
||||
|
||||
// -------- ADCU_BodyCmd (ID 277) --------
|
||||
inline constexpr SignalSpec ADCU_LampCmd_Active { 7, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_LampCmd_RollCnt {51, 4, 1.0, 0.0, false};
|
||||
|
||||
// -------- ADCU_PowerCmd (ID 279, BatCmd) --------
|
||||
inline constexpr SignalSpec ADCU_PwrCmd_RollCnt {51, 4, 1.0, 0.0, false};
|
||||
|
||||
// -------- ADCU_ParkCmd (ID 274) --------
|
||||
inline constexpr SignalSpec ADCU_Prk_Active { 7, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_Prk_Enable { 6, 1, 1.0, 0.0, false};
|
||||
inline constexpr SignalSpec ADCU_PrkCmd_RollCnt {51, 4, 1.0, 0.0, false};
|
||||
} // namespace sig
|
||||
|
||||
} // namespace ecar_can_driver
|
||||
|
||||
#endif // ECAR_CAN_DRIVER__DBC_CODEC_HPP_
|
||||
@@ -0,0 +1,97 @@
|
||||
#!/usr/bin/env python3
|
||||
# -----------------------------------------------------------------------------
|
||||
# ecar_can_driver.launch.py
|
||||
# 同时启动 CAN 接收 / CAN 发送 两个节点, 支持 launch 参数覆盖写死的控制命令.
|
||||
# -----------------------------------------------------------------------------
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def _parse_bool(text: str) -> bool:
|
||||
value = text.strip().lower()
|
||||
if value in ('1', 'true', 'yes', 'on'):
|
||||
return True
|
||||
if value in ('0', 'false', 'no', 'off'):
|
||||
return False
|
||||
raise RuntimeError(f'Invalid boolean launch argument value: {text}')
|
||||
|
||||
|
||||
def _build_nodes(context):
|
||||
params_file = LaunchConfiguration('params_file').perform(context)
|
||||
|
||||
sender_overrides = {}
|
||||
|
||||
def maybe_set(name, caster):
|
||||
raw = LaunchConfiguration(name).perform(context).strip()
|
||||
if raw == '':
|
||||
return
|
||||
sender_overrides[name] = caster(raw)
|
||||
|
||||
maybe_set('target_steer_angle_deg', float)
|
||||
maybe_set('target_brake_pedal_pct', float)
|
||||
maybe_set('target_brake_press_bar', float)
|
||||
maybe_set('target_brake_accel_mps2', float)
|
||||
maybe_set('target_drive_pedpos_pct', float)
|
||||
maybe_set('target_drive_gear', int)
|
||||
maybe_set('publish_rate_hz', float)
|
||||
maybe_set('body_power_rate_hz', float)
|
||||
maybe_set('park_enable', _parse_bool)
|
||||
maybe_set('activate_low_duration_ms', float)
|
||||
|
||||
sender_parameters = [params_file]
|
||||
if sender_overrides:
|
||||
sender_parameters.append(sender_overrides)
|
||||
|
||||
return [
|
||||
Node(
|
||||
package='ecar_can_driver',
|
||||
executable='can_receiver_node',
|
||||
name='ecar_can_receiver',
|
||||
output='screen',
|
||||
parameters=[params_file],
|
||||
),
|
||||
Node(
|
||||
package='ecar_can_driver',
|
||||
executable='can_sender_node',
|
||||
name='ecar_can_sender',
|
||||
output='screen',
|
||||
parameters=sender_parameters,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
default_params_file = PathJoinSubstitution([
|
||||
FindPackageShare('ecar_can_driver'),
|
||||
'config',
|
||||
'ecar_can_driver.params.yaml',
|
||||
])
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('params_file', default_value=default_params_file,
|
||||
description='Path to ROS2 parameter YAML file'),
|
||||
DeclareLaunchArgument('target_steer_angle_deg', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('target_brake_pedal_pct', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('target_brake_press_bar', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('target_brake_accel_mps2', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('target_drive_pedpos_pct', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('target_drive_gear', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('publish_rate_hz', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('body_power_rate_hz', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('park_enable', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
DeclareLaunchArgument('activate_low_duration_ms', default_value='',
|
||||
description='Optional override. Leave empty to use YAML value'),
|
||||
OpaqueFunction(function=_build_nodes),
|
||||
])
|
||||
@@ -0,0 +1,5 @@
|
||||
#
|
||||
# DBC message: ADCU_BodyCmd (ID 277)
|
||||
#
|
||||
std_msgs/Header header
|
||||
bool adcu_lamp_active
|
||||
@@ -0,0 +1,9 @@
|
||||
#
|
||||
# DBC message: ADCU_BrakeCmd (ID 273)
|
||||
#
|
||||
std_msgs/Header header
|
||||
bool adcu_brk_active
|
||||
uint8 adcu_brk_ctrl_mode
|
||||
float32 adcu_brk_tgt_pedpos
|
||||
float32 adcu_brk_tgt_press
|
||||
float32 adcu_brk_tgt_acc_spd
|
||||
@@ -0,0 +1,9 @@
|
||||
#
|
||||
# DBC message: CDCU_BrakeStatus
|
||||
#
|
||||
std_msgs/Header header
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
float32 cdcu_ehb_brk_presur
|
||||
float32 cdcu_ehb_brk_pedpos
|
||||
@@ -0,0 +1,11 @@
|
||||
#
|
||||
# DBC message: ADCU_DriveCmd (ID 276)
|
||||
#
|
||||
std_msgs/Header header
|
||||
bool adcu_drv_active
|
||||
uint8 adcu_drv_ctrl_mode
|
||||
uint8 adcu_drv_tgt_gear
|
||||
float32 adcu_drv_tgt_pedpos
|
||||
float32 adcu_drv_tgt_veh_spd
|
||||
float32 adcu_drv_tgt_veh_acc_spd
|
||||
float32 adcu_drv_veh_spd_limit
|
||||
@@ -0,0 +1,11 @@
|
||||
#
|
||||
# DBC message: CDCU_DriveStatus
|
||||
#
|
||||
std_msgs/Header header
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
float32 cdcu_mcu_throt_act
|
||||
float32 cdcu_mcu_mtr_curt
|
||||
float32 cdcu_mcu_mtr_spd
|
||||
uint8 cdcu_mcu_gear_act
|
||||
@@ -0,0 +1,9 @@
|
||||
# FaultReport.msg
|
||||
# 车辆故障诊断报告
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
string system # 系统名称: brake, steer, drive, park, battery
|
||||
bool msg_offline # 通信中断故障
|
||||
bool can_error # CAN通信故障
|
||||
bool over_temp # 过温故障
|
||||
@@ -0,0 +1,6 @@
|
||||
#
|
||||
# DBC message: ADCU_ParkCmd (ID 274)
|
||||
#
|
||||
std_msgs/Header header
|
||||
bool adcu_prk_active
|
||||
bool adcu_prk_enable
|
||||
@@ -0,0 +1,4 @@
|
||||
#
|
||||
# DBC message: ADCU_PowerCmd (ID 279)
|
||||
#
|
||||
std_msgs/Header header
|
||||
@@ -0,0 +1,9 @@
|
||||
#
|
||||
# DBC message: ADCU_SteerCmd (ID 275)
|
||||
#
|
||||
std_msgs/Header header
|
||||
bool adcu_str_active
|
||||
uint8 adcu_str_ctrl_mode
|
||||
float32 adcu_str_tgt_angle
|
||||
float32 adcu_str_tgt_curvature
|
||||
float32 adcu_str_tgt_angle_spd
|
||||
@@ -0,0 +1,10 @@
|
||||
#
|
||||
# DBC message: CDCU_SteerStatus
|
||||
#
|
||||
std_msgs/Header header
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
float32 cdcu_eps_str_whl_angle
|
||||
float32 cdcu_eps_str_trq
|
||||
float32 cdcu_eps_whl_spd
|
||||
@@ -0,0 +1,9 @@
|
||||
#
|
||||
# DBC message: CDCU_VehDyncState
|
||||
#
|
||||
std_msgs/Header header
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
float32 cdcu_veh_longtdnal_spd
|
||||
float32 cdcu_veh_longtdnal_acc_spd
|
||||
@@ -0,0 +1,8 @@
|
||||
#
|
||||
# DBC message: CDCU_VehState
|
||||
#
|
||||
std_msgs/Header header
|
||||
uint32 can_id
|
||||
uint8 dlc
|
||||
uint8[8] raw_data
|
||||
uint8 cdcu_veh_run_mode
|
||||
@@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>ecar_can_driver</name>
|
||||
<version>1.0.0</version>
|
||||
<description>ECAR DBC CAN driver for ROS2 (Kvaser). Parses CAN frames from /kvaser/can_tx into decoded vehicle status topics and custom frame messages, and sends control commands (steer/brake/drive/body/power) as CAN frames on kvaser_output with additive checksum.</description>
|
||||
|
||||
<maintainer email="dev@example.com">ecar</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>can_msgs</depend>
|
||||
<depend>builtin_interfaces</depend>
|
||||
<depend>autoware_vehicle_msgs</depend>
|
||||
<depend>tier4_vehicle_msgs</depend>
|
||||
<depend>autoware_adapi_v1_msgs</depend>
|
||||
<depend>autoware_control_msgs</depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,120 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Autoware → ecar_can_driver 适配测试脚本
|
||||
模拟 Autoware 发送控制指令，验证驱动节点是否正确接收并转换为 CAN 帧。
|
||||
|
||||
用法: ros2 run ecar_can_driver autoware_test.py
|
||||
或: python3 autoware_test.py
|
||||
"""
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from tier4_vehicle_msgs.msg import ActuationCommandStamped
|
||||
from autoware_control_msgs.msg import Control
|
||||
from autoware_vehicle_msgs.msg import GearCommand
|
||||
from autoware_vehicle_msgs.srv import ControlModeCommand
|
||||
|
||||
|
||||
class AutowareTestNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('autoware_test_node')
|
||||
|
||||
self.pub_actuation = self.create_publisher(
|
||||
ActuationCommandStamped, '/control/command/actuation_cmd', 10)
|
||||
self.pub_control = self.create_publisher(
|
||||
Control, 'ecar_can_sender/input/control_cmd', 10)
|
||||
self.pub_gear = self.create_publisher(
|
||||
GearCommand, '/control/command/gear_cmd', 10)
|
||||
self.cli_mode = self.create_client(
|
||||
ControlModeCommand, '/control/control_mode_request')
|
||||
|
||||
# 先以 50Hz 发送控制指令，1秒后再请求进入自动驾驶
|
||||
self.timer = self.create_timer(0.02, self.publish_commands)
|
||||
self.create_timer(1.0, self.request_autonomous_once)
|
||||
|
||||
self.autonomous_requested = False
|
||||
self.tick_count = 0
|
||||
|
||||
self.get_logger().info('=== Autoware 适配测试启动 ===')
|
||||
self.get_logger().info(
|
||||
'发送: control speed=1.0m/s, control steer=0.05rad, '
|
||||
'actuation accel=0.1(10%), gear=D')
|
||||
self.get_logger().info('1秒后请求 AUTONOMOUS 模式 (触发上升沿)')
|
||||
|
||||
def publish_commands(self):
|
||||
now = self.get_clock().now().to_msg()
|
||||
|
||||
act = ActuationCommandStamped()
|
||||
act.header.stamp = now
|
||||
act.header.frame_id = 'base_link'
|
||||
act.actuation.accel_cmd = 0.0 # 10% 油门
|
||||
act.actuation.brake_cmd = 0.00 # 0% 刹车
|
||||
act.actuation.steer_cmd = 0.0 # 0 rad
|
||||
self.pub_actuation.publish(act)
|
||||
|
||||
control = Control()
|
||||
control.stamp = now
|
||||
control.control_time = now
|
||||
control.longitudinal.stamp = now
|
||||
control.longitudinal.control_time = now
|
||||
control.longitudinal.velocity = 10.0/3.6 # m/s, can_sender 内部转成 km/h 下发
|
||||
control.longitudinal.acceleration = 0.0
|
||||
control.longitudinal.jerk = 0.0
|
||||
control.longitudinal.is_defined_acceleration = True
|
||||
control.longitudinal.is_defined_jerk = True
|
||||
control.lateral.stamp = now
|
||||
control.lateral.control_time = now
|
||||
control.lateral.steering_tire_angle = 0.0
|
||||
control.lateral.steering_tire_rotation_rate = 0.0
|
||||
control.lateral.is_defined_steering_tire_rotation_rate = True
|
||||
self.pub_control.publish(control)
|
||||
|
||||
gear = GearCommand()
|
||||
gear.stamp = now
|
||||
gear.command = GearCommand.DRIVE # R 挡
|
||||
self.pub_gear.publish(gear)
|
||||
|
||||
self.tick_count += 1
|
||||
if self.tick_count % 50 == 0:
|
||||
self.get_logger().info(
|
||||
f'[{self.tick_count // 50}s] 持续发送中... '
|
||||
f'autonomous={"YES" if self.autonomous_requested else "NO"}')
|
||||
|
||||
def request_autonomous_once(self):
|
||||
if self.autonomous_requested:
|
||||
return
|
||||
|
||||
if not self.cli_mode.wait_for_service(timeout_sec=0.5):
|
||||
self.get_logger().warn('control_mode_request 服务未就绪，下次重试...')
|
||||
return
|
||||
|
||||
req = ControlModeCommand.Request()
|
||||
req.mode = ControlModeCommand.Request.AUTONOMOUS
|
||||
future = self.cli_mode.call_async(req)
|
||||
future.add_done_callback(self.on_mode_response)
|
||||
self.autonomous_requested = True
|
||||
|
||||
def on_mode_response(self, future):
|
||||
try:
|
||||
resp = future.result()
|
||||
if resp.success:
|
||||
self.get_logger().info('AUTONOMOUS 模式请求成功，上升沿已触发')
|
||||
else:
|
||||
self.get_logger().error('AUTONOMOUS 模式请求被拒绝')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'服务调用异常: {e}')
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = AutowareTestNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
node.get_logger().info('测试结束')
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,617 @@
|
||||
// ============================================================================
|
||||
// can_receiver_node.cpp
|
||||
// 订阅可配置的 CAN RX topic (can_msgs::msg::Frame),
|
||||
// 根据 ECAR_UCAN_ComMatrix DBC 解码车辆状态信号, 并发布:
|
||||
// 1) 自定义按帧聚合 ROS2 消息, 便于实时查看整帧解析结果
|
||||
// 2) 兼容保留原有单值 /vehicle/* 话题
|
||||
// 3) Autoware 兼容话题 (ControlModeReport / VelocityReport / SteeringReport
|
||||
// / GearReport / ActuationStatusStamped)
|
||||
// 4) 新增: 故障诊断话题 /vehicle/status/fault_report
|
||||
// 5) 新增: 消息超时检测和发布机制
|
||||
// ============================================================================
|
||||
#include <memory>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "can_msgs/msg/frame.hpp"
|
||||
#include "std_msgs/msg/float32.hpp"
|
||||
#include "std_msgs/msg/u_int8.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "builtin_interfaces/msg/time.hpp"
|
||||
|
||||
#include "ecar_can_driver/msg/brake_status.hpp"
|
||||
#include "ecar_can_driver/msg/drive_status.hpp"
|
||||
#include "ecar_can_driver/msg/steer_status.hpp"
|
||||
#include "ecar_can_driver/msg/veh_dync_state.hpp"
|
||||
#include "ecar_can_driver/msg/veh_state.hpp"
|
||||
#include "ecar_can_driver/msg/fault_report.hpp"
|
||||
|
||||
#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/gear_report.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/turn_indicators_report.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/hazard_lights_report.hpp"
|
||||
#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp"
|
||||
#include "tier4_vehicle_msgs/msg/actuation_status.hpp"
|
||||
|
||||
#include "ecar_can_driver/dbc_codec.hpp"
|
||||
|
||||
using namespace ecar_can_driver;
|
||||
|
||||
class CanReceiverNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
CanReceiverNode()
|
||||
: Node("ecar_can_receiver")
|
||||
{
|
||||
can_rx_topic_ = declare_parameter<std::string>("can_rx_topic", "/kvaser/can_tx");
|
||||
report_timeout_ms_ = declare_parameter<double>("report_timeout_ms", 1000.0);
|
||||
loop_rate_ = declare_parameter<double>("loop_rate", 50.0);
|
||||
|
||||
// ---------- subscriber ----------
|
||||
sub_can_ = create_subscription<can_msgs::msg::Frame>(
|
||||
can_rx_topic_,
|
||||
rclcpp::QoS(100),
|
||||
std::bind(&CanReceiverNode::on_can_frame, this, std::placeholders::_1));
|
||||
|
||||
// ---------- publishers: 自定义按帧消息 ----------
|
||||
pub_steer_status_frame_ = create_publisher<ecar_can_driver::msg::SteerStatus>(
|
||||
"vehicle/cdcu_steer_status", 10);
|
||||
pub_brake_status_frame_ = create_publisher<ecar_can_driver::msg::BrakeStatus>(
|
||||
"vehicle/cdcu_brake_status", 10);
|
||||
pub_drive_status_frame_ = create_publisher<ecar_can_driver::msg::DriveStatus>(
|
||||
"vehicle/cdcu_drive_status", 10);
|
||||
pub_veh_dync_state_frame_ = create_publisher<ecar_can_driver::msg::VehDyncState>(
|
||||
"vehicle/cdcu_veh_dync_state", 10);
|
||||
pub_veh_state_frame_ = create_publisher<ecar_can_driver::msg::VehState>(
|
||||
"vehicle/cdcu_veh_state", 10);
|
||||
pub_fault_report_ = create_publisher<ecar_can_driver::msg::FaultReport>(
|
||||
"vehicle/status/fault_report", 10); // 新增
|
||||
|
||||
// ---------- publishers: Autoware 兼容话题 ----------
|
||||
pub_aw_control_mode_ = create_publisher<autoware_vehicle_msgs::msg::ControlModeReport>(
|
||||
"/vehicle/status/control_mode", 10);
|
||||
pub_aw_velocity_ = create_publisher<autoware_vehicle_msgs::msg::VelocityReport>(
|
||||
"/vehicle/status/velocity_status", 10);
|
||||
pub_aw_steering_ = create_publisher<autoware_vehicle_msgs::msg::SteeringReport>(
|
||||
"/vehicle/status/steering_status", 10);
|
||||
pub_aw_gear_ = create_publisher<autoware_vehicle_msgs::msg::GearReport>(
|
||||
"/vehicle/status/gear_status", 10);
|
||||
pub_aw_turn_indicators_ = create_publisher<autoware_vehicle_msgs::msg::TurnIndicatorsReport>(
|
||||
"/vehicle/status/turn_indicators_status", 10);
|
||||
pub_aw_hazard_lights_ = create_publisher<autoware_vehicle_msgs::msg::HazardLightsReport>(
|
||||
"/vehicle/status/hazard_lights_status", 10);
|
||||
pub_aw_actuation_status_ = create_publisher<tier4_vehicle_msgs::msg::ActuationStatusStamped>(
|
||||
"/vehicle/status/actuation_status", 10);
|
||||
|
||||
// ---------- publishers: 基础信号 ----------
|
||||
pub_steer_angle_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_eps_str_whl_angle", 10);
|
||||
pub_steer_torque_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_eps_str_trq", 10);
|
||||
pub_steer_wheel_spd_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_eps_whl_spd", 10);
|
||||
|
||||
pub_brake_pressure_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_ehb_brk_presur", 10);
|
||||
pub_brake_pedal_pos_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_ehb_brk_pedpos", 10);
|
||||
|
||||
pub_throttle_pct_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_mcu_throt_act", 10);
|
||||
pub_motor_current_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_mcu_mtr_curt", 10);
|
||||
pub_motor_speed_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_mcu_mtr_spd", 10);
|
||||
pub_gear_act_ = create_publisher<std_msgs::msg::UInt8>(
|
||||
"vehicle/cdcu_mcu_gear_act", 10); // 实际挡位反馈，用于换挡保护
|
||||
|
||||
pub_veh_speed_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_veh_longtdnal_spd", 10);
|
||||
pub_veh_accel_ = create_publisher<std_msgs::msg::Float32>(
|
||||
"vehicle/cdcu_veh_longtdnal_acc_spd", 10);
|
||||
|
||||
pub_run_mode_ = create_publisher<std_msgs::msg::UInt8>(
|
||||
"vehicle/cdcu_veh_run_mode", 10);
|
||||
|
||||
// ---------- timer: 周期性发布和超时检测 ----------
|
||||
timer_ = create_wall_timer(
|
||||
std::chrono::duration<double>(1.0 / loop_rate_),
|
||||
std::bind(&CanReceiverNode::timerCallback, this));
|
||||
|
||||
// 初始化接收时间
|
||||
auto now = this->now();
|
||||
steer_status_received_time_ = now;
|
||||
brake_status_received_time_ = now;
|
||||
drive_status_received_time_ = now;
|
||||
veh_dync_state_received_time_ = now;
|
||||
veh_state_received_time_ = now;
|
||||
|
||||
RCLCPP_INFO(get_logger(),
|
||||
"ECAR CAN receiver started. Subscribing %s, timeout=%.0fms, rate=%.1fHz",
|
||||
can_rx_topic_.c_str(), report_timeout_ms_, loop_rate_);
|
||||
}
|
||||
|
||||
private:
|
||||
template<typename MsgT>
|
||||
void fill_frame_metadata(const can_msgs::msg::Frame & frame, MsgT & msg) const
|
||||
{
|
||||
msg.header = frame.header;
|
||||
msg.can_id = frame.id;
|
||||
msg.dlc = frame.dlc;
|
||||
msg.raw_data = frame.data;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// 主回调: 根据 CAN ID 分发到对应解码函数
|
||||
// -------------------------------------------------------------------------
|
||||
void on_can_frame(const can_msgs::msg::Frame::SharedPtr msg)
|
||||
{
|
||||
if (msg->dlc < 8) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t id = msg->id;
|
||||
|
||||
switch (id) {
|
||||
case msg_id::CDCU_SteerStatus:
|
||||
decode_steer_status(*msg);
|
||||
break;
|
||||
case msg_id::CDCU_BrakeStatus:
|
||||
decode_brake_status(*msg);
|
||||
break;
|
||||
case msg_id::CDCU_DriveStatus:
|
||||
decode_drive_status(*msg);
|
||||
break;
|
||||
case msg_id::CDCU_VehDyncState:
|
||||
decode_veh_dync_state(*msg);
|
||||
break;
|
||||
case msg_id::CDCU_VehState:
|
||||
decode_veh_state(*msg);
|
||||
break;
|
||||
// 新增: 诊断报文解析
|
||||
case msg_id::CDCU_BrakeDiag:
|
||||
decode_brake_diag(*msg);
|
||||
break;
|
||||
case msg_id::CDCU_SteerDiag:
|
||||
decode_steer_diag(*msg);
|
||||
break;
|
||||
case msg_id::CDCU_DriveDiag:
|
||||
decode_drive_diag(*msg);
|
||||
break;
|
||||
case msg_id::CDCU_ParkDiag:
|
||||
decode_park_diag(*msg);
|
||||
break;
|
||||
case msg_id::CDCU_BatDiag:
|
||||
decode_bat_diag(*msg);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// 状态报文解码
|
||||
// -------------------------------------------------------------------------
|
||||
void decode_steer_status(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
steer_status_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::SteerStatus status_msg;
|
||||
fill_frame_metadata(frame, status_msg);
|
||||
status_msg.cdcu_eps_str_whl_angle =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_EPS_StrWhlAngle));
|
||||
status_msg.cdcu_eps_str_trq =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_EPS_StrTrq));
|
||||
status_msg.cdcu_eps_whl_spd =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_EPS_WhlSpd));
|
||||
steer_status_ptr_ = std::make_shared<ecar_can_driver::msg::SteerStatus>(status_msg);
|
||||
|
||||
std_msgs::msg::Float32 m;
|
||||
m.data = status_msg.cdcu_eps_str_whl_angle;
|
||||
pub_steer_angle_->publish(m);
|
||||
|
||||
m.data = status_msg.cdcu_eps_str_trq;
|
||||
pub_steer_torque_->publish(m);
|
||||
|
||||
m.data = status_msg.cdcu_eps_whl_spd;
|
||||
pub_steer_wheel_spd_->publish(m);
|
||||
|
||||
// Autoware SteeringReport
|
||||
autoware_vehicle_msgs::msg::SteeringReport aw_steer;
|
||||
aw_steer.stamp = frame.header.stamp;
|
||||
double tire_angle_rad = status_msg.cdcu_eps_str_whl_angle * M_PI / 180.0;
|
||||
aw_steer.steering_tire_angle = static_cast<float>(tire_angle_rad);
|
||||
pub_aw_steering_->publish(aw_steer);
|
||||
|
||||
cached_steer_status_ = tire_angle_rad;
|
||||
}
|
||||
|
||||
void decode_brake_status(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
brake_status_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::BrakeStatus status_msg;
|
||||
fill_frame_metadata(frame, status_msg);
|
||||
status_msg.cdcu_ehb_brk_presur =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_EHB_BrkPresur));
|
||||
status_msg.cdcu_ehb_brk_pedpos =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_EHB_BrkPedpos));
|
||||
brake_status_ptr_ = std::make_shared<ecar_can_driver::msg::BrakeStatus>(status_msg);
|
||||
|
||||
std_msgs::msg::Float32 m;
|
||||
m.data = status_msg.cdcu_ehb_brk_presur;
|
||||
pub_brake_pressure_->publish(m);
|
||||
|
||||
m.data = status_msg.cdcu_ehb_brk_pedpos;
|
||||
pub_brake_pedal_pos_->publish(m);
|
||||
|
||||
cached_brake_pedal_pct_ = status_msg.cdcu_ehb_brk_pedpos;
|
||||
cached_brake_stamp_ = frame.header.stamp;
|
||||
publish_actuation_status();
|
||||
}
|
||||
|
||||
void decode_drive_status(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
drive_status_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::DriveStatus status_msg;
|
||||
fill_frame_metadata(frame, status_msg);
|
||||
status_msg.cdcu_mcu_throt_act =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_MCU_ThrotAct));
|
||||
status_msg.cdcu_mcu_mtr_curt =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_MCU_MtrCurt));
|
||||
status_msg.cdcu_mcu_mtr_spd =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_MCU_MtrSpd));
|
||||
status_msg.cdcu_mcu_gear_act =
|
||||
static_cast<uint8_t>(decode_physical(d, sig::CDCU_MCU_GearAct));
|
||||
drive_status_ptr_ = std::make_shared<ecar_can_driver::msg::DriveStatus>(status_msg);
|
||||
|
||||
// 缓存实际挡位，用于换挡保护
|
||||
cached_gear_actual_ = status_msg.cdcu_mcu_gear_act;
|
||||
|
||||
std_msgs::msg::Float32 mf;
|
||||
std_msgs::msg::UInt8 mu;
|
||||
mf.data = status_msg.cdcu_mcu_throt_act;
|
||||
pub_throttle_pct_->publish(mf);
|
||||
|
||||
mf.data = status_msg.cdcu_mcu_mtr_curt;
|
||||
pub_motor_current_->publish(mf);
|
||||
|
||||
mf.data = status_msg.cdcu_mcu_mtr_spd;
|
||||
pub_motor_speed_->publish(mf);
|
||||
|
||||
mu.data = status_msg.cdcu_mcu_gear_act;
|
||||
pub_gear_act_->publish(mu);
|
||||
|
||||
// Autoware GearReport
|
||||
autoware_vehicle_msgs::msg::GearReport aw_gear;
|
||||
aw_gear.stamp = frame.header.stamp;
|
||||
switch (status_msg.cdcu_mcu_gear_act) {
|
||||
case 0: aw_gear.report = autoware_vehicle_msgs::msg::GearReport::NEUTRAL; break;
|
||||
case 1: aw_gear.report = autoware_vehicle_msgs::msg::GearReport::DRIVE; break;
|
||||
case 2: aw_gear.report = autoware_vehicle_msgs::msg::GearReport::REVERSE; break;
|
||||
case 3: aw_gear.report = autoware_vehicle_msgs::msg::GearReport::PARK; break;
|
||||
default: aw_gear.report = autoware_vehicle_msgs::msg::GearReport::NONE; break;
|
||||
}
|
||||
pub_aw_gear_->publish(aw_gear);
|
||||
|
||||
cached_accel_pedal_pct_ = status_msg.cdcu_mcu_throt_act;
|
||||
cached_accel_stamp_ = frame.header.stamp;
|
||||
publish_actuation_status();
|
||||
}
|
||||
|
||||
void decode_veh_dync_state(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
veh_dync_state_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::VehDyncState status_msg;
|
||||
fill_frame_metadata(frame, status_msg);
|
||||
status_msg.cdcu_veh_longtdnal_spd =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_Veh_LongtdnalSpd));
|
||||
status_msg.cdcu_veh_longtdnal_acc_spd =
|
||||
static_cast<float>(decode_physical(d, sig::CDCU_Veh_LongtdnalAccSpd));
|
||||
veh_dync_state_ptr_ = std::make_shared<ecar_can_driver::msg::VehDyncState>(status_msg);
|
||||
|
||||
std_msgs::msg::Float32 m;
|
||||
m.data = status_msg.cdcu_veh_longtdnal_spd;
|
||||
pub_veh_speed_->publish(m);
|
||||
|
||||
m.data = status_msg.cdcu_veh_longtdnal_acc_spd;
|
||||
pub_veh_accel_->publish(m);
|
||||
|
||||
// Autoware VelocityReport: km/h → m/s
|
||||
autoware_vehicle_msgs::msg::VelocityReport aw_vel;
|
||||
aw_vel.header = frame.header;
|
||||
aw_vel.header.frame_id = "base_link";
|
||||
|
||||
switch (cached_gear_actual_) {
|
||||
case 0: aw_vel.longitudinal_velocity = static_cast<float>(status_msg.cdcu_veh_longtdnal_spd / 3.6); break;
|
||||
case 1: aw_vel.longitudinal_velocity = static_cast<float>(status_msg.cdcu_veh_longtdnal_spd / 3.6); break;
|
||||
case 2: aw_vel.longitudinal_velocity = -static_cast<float>(status_msg.cdcu_veh_longtdnal_spd / 3.6); break;
|
||||
case 3: aw_vel.longitudinal_velocity = static_cast<float>(status_msg.cdcu_veh_longtdnal_spd / 3.6); break;
|
||||
default: aw_vel.longitudinal_velocity = static_cast<float>(status_msg.cdcu_veh_longtdnal_spd / 3.6); break;
|
||||
}
|
||||
|
||||
aw_vel.lateral_velocity = 0.0f;
|
||||
aw_vel.heading_rate = 0.0f;
|
||||
pub_aw_velocity_->publish(aw_vel);
|
||||
}
|
||||
|
||||
void decode_veh_state(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
veh_state_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
// 解析车辆运行模式
|
||||
uint8_t run_mode = static_cast<uint8_t>(decode_physical(d, sig::CDCU_Veh_RunMode));
|
||||
|
||||
// 解析转向灯和危险灯状态
|
||||
bool left_lamp = static_cast<bool>(decode_physical(d, sig::CDCU_TurnLLamp_St));
|
||||
bool right_lamp = static_cast<bool>(decode_physical(d, sig::CDCU_TurnRLamp_St));
|
||||
bool hazard_lamp = static_cast<bool>(decode_physical(d, sig::CDCU_DblFlashLamp_St));
|
||||
|
||||
ecar_can_driver::msg::VehState status_msg;
|
||||
fill_frame_metadata(frame, status_msg);
|
||||
status_msg.cdcu_veh_run_mode = run_mode;
|
||||
veh_state_ptr_ = std::make_shared<ecar_can_driver::msg::VehState>(status_msg);
|
||||
|
||||
std_msgs::msg::UInt8 m;
|
||||
m.data = status_msg.cdcu_veh_run_mode;
|
||||
pub_run_mode_->publish(m);
|
||||
|
||||
// Autoware ControlModeReport
|
||||
autoware_vehicle_msgs::msg::ControlModeReport aw_mode;
|
||||
aw_mode.stamp = frame.header.stamp;
|
||||
switch (run_mode) {
|
||||
case 3: // Automatic
|
||||
aw_mode.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;
|
||||
break;
|
||||
default:
|
||||
aw_mode.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL;
|
||||
break;
|
||||
}
|
||||
pub_aw_control_mode_->publish(aw_mode);
|
||||
|
||||
// Autoware TurnIndicatorsReport - 转向灯状态
|
||||
autoware_vehicle_msgs::msg::TurnIndicatorsReport turn_msg;
|
||||
turn_msg.stamp = frame.header.stamp;
|
||||
if (hazard_lamp) {
|
||||
// 危险灯模式下，按Autoware惯例报告为ENABLE_LEFT
|
||||
turn_msg.report = autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT;
|
||||
} else if (left_lamp) {
|
||||
turn_msg.report = autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT;
|
||||
} else if (right_lamp) {
|
||||
turn_msg.report = autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT;
|
||||
} else {
|
||||
turn_msg.report = autoware_vehicle_msgs::msg::TurnIndicatorsReport::DISABLE;
|
||||
}
|
||||
pub_aw_turn_indicators_->publish(turn_msg);
|
||||
|
||||
// Autoware HazardLightsReport - 危险报警灯状态
|
||||
autoware_vehicle_msgs::msg::HazardLightsReport hazard_msg;
|
||||
hazard_msg.stamp = frame.header.stamp;
|
||||
hazard_msg.report = hazard_lamp ?
|
||||
autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE :
|
||||
autoware_vehicle_msgs::msg::HazardLightsReport::DISABLE;
|
||||
pub_aw_hazard_lights_->publish(hazard_msg);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// 诊断报文解码（新增）
|
||||
// -------------------------------------------------------------------------
|
||||
void decode_brake_diag(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
brake_diag_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::FaultReport fault_msg;
|
||||
fault_msg.header = frame.header;
|
||||
fault_msg.system = "brake";
|
||||
fault_msg.msg_offline = static_cast<bool>(decode_physical(d, sig::CDCU_EHBMsgOffline_Err));
|
||||
fault_msg.over_temp = static_cast<bool>(decode_physical(d, sig::CDCU_EHBOverTemp_Err));
|
||||
fault_msg.can_error = static_cast<bool>(decode_physical(d, sig::CDCU_EHBCAN_Err));
|
||||
fault_report_ptr_ = std::make_shared<ecar_can_driver::msg::FaultReport>(fault_msg);
|
||||
}
|
||||
|
||||
void decode_steer_diag(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
steer_diag_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::FaultReport fault_msg;
|
||||
fault_msg.header = frame.header;
|
||||
fault_msg.system = "steer";
|
||||
fault_msg.msg_offline = static_cast<bool>(decode_physical(d, sig::CDCU_EPSMsgOffline_Err));
|
||||
fault_msg.can_error = static_cast<bool>(decode_physical(d, sig::EPS_CANCom_Err));
|
||||
fault_msg.over_temp = static_cast<bool>(decode_physical(d, sig::EPS_MtrOverTemp_Err));
|
||||
steer_fault_ptr_ = std::make_shared<ecar_can_driver::msg::FaultReport>(fault_msg);
|
||||
}
|
||||
|
||||
void decode_drive_diag(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
drive_diag_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::FaultReport fault_msg;
|
||||
fault_msg.header = frame.header;
|
||||
fault_msg.system = "drive";
|
||||
fault_msg.can_error = static_cast<bool>(decode_physical(d, sig::CDCU_MCUCANCom_Err));
|
||||
fault_msg.over_temp = static_cast<bool>(decode_physical(d, sig::CDCU_MCUOverTemp_Err));
|
||||
drive_fault_ptr_ = std::make_shared<ecar_can_driver::msg::FaultReport>(fault_msg);
|
||||
}
|
||||
|
||||
void decode_park_diag(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
park_diag_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::FaultReport fault_msg;
|
||||
fault_msg.header = frame.header;
|
||||
fault_msg.system = "park";
|
||||
fault_msg.msg_offline = static_cast<bool>(decode_physical(d, sig::CDCU_EPBMsgOffline_Err));
|
||||
park_fault_ptr_ = std::make_shared<ecar_can_driver::msg::FaultReport>(fault_msg);
|
||||
}
|
||||
|
||||
void decode_bat_diag(const can_msgs::msg::Frame & frame)
|
||||
{
|
||||
bat_diag_received_time_ = this->now();
|
||||
const uint8_t * d = frame.data.data();
|
||||
|
||||
ecar_can_driver::msg::FaultReport fault_msg;
|
||||
fault_msg.header = frame.header;
|
||||
fault_msg.system = "battery";
|
||||
fault_msg.msg_offline = static_cast<bool>(decode_physical(d, sig::CDCU_BMSMsgOffline_Err));
|
||||
bat_fault_ptr_ = std::make_shared<ecar_can_driver::msg::FaultReport>(fault_msg);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// 合成 ActuationStatusStamped
|
||||
// -------------------------------------------------------------------------
|
||||
void publish_actuation_status()
|
||||
{
|
||||
tier4_vehicle_msgs::msg::ActuationStatusStamped aw_act;
|
||||
aw_act.header.stamp = now();
|
||||
aw_act.header.frame_id = "base_link";
|
||||
aw_act.status.accel_status = cached_accel_pedal_pct_ / 100.0;
|
||||
aw_act.status.brake_status = cached_brake_pedal_pct_ / 100.0;
|
||||
aw_act.status.steer_status = cached_steer_status_;
|
||||
pub_aw_actuation_status_->publish(aw_act);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// 定时器回调: 周期性发布和超时检测（新增）
|
||||
// -------------------------------------------------------------------------
|
||||
void timerCallback()
|
||||
{
|
||||
const rclcpp::Time current_time = this->now();
|
||||
|
||||
// 检查各报文是否超时
|
||||
auto check_timeout = [this, ¤t_time](const rclcpp::Time & received_time,
|
||||
const std::string & msg_name) -> bool {
|
||||
double delta_ms = (current_time - received_time).seconds() * 1000.0;
|
||||
if (delta_ms > report_timeout_ms_) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
get_logger(), *get_clock(), 5000,
|
||||
"%s report timeout: %.1f ms", msg_name.c_str(), delta_ms);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
};
|
||||
|
||||
// 发布状态报文（带超时检查）
|
||||
if (steer_status_ptr_ && check_timeout(steer_status_received_time_, "steer_status")) {
|
||||
pub_steer_status_frame_->publish(*steer_status_ptr_);
|
||||
}
|
||||
if (brake_status_ptr_ && check_timeout(brake_status_received_time_, "brake_status")) {
|
||||
pub_brake_status_frame_->publish(*brake_status_ptr_);
|
||||
}
|
||||
if (drive_status_ptr_ && check_timeout(drive_status_received_time_, "drive_status")) {
|
||||
pub_drive_status_frame_->publish(*drive_status_ptr_);
|
||||
}
|
||||
if (veh_dync_state_ptr_ && check_timeout(veh_dync_state_received_time_, "veh_dync_state")) {
|
||||
pub_veh_dync_state_frame_->publish(*veh_dync_state_ptr_);
|
||||
}
|
||||
if (veh_state_ptr_ && check_timeout(veh_state_received_time_, "veh_state")) {
|
||||
pub_veh_state_frame_->publish(*veh_state_ptr_);
|
||||
}
|
||||
|
||||
// 发布故障报告（带超时检查）
|
||||
if (fault_report_ptr_ && check_timeout(brake_diag_received_time_, "brake_diag")) {
|
||||
pub_fault_report_->publish(*fault_report_ptr_);
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
std::string can_rx_topic_;
|
||||
double report_timeout_ms_;
|
||||
double loop_rate_;
|
||||
rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr sub_can_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
// 状态报文发布器
|
||||
rclcpp::Publisher<ecar_can_driver::msg::SteerStatus>::SharedPtr pub_steer_status_frame_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::BrakeStatus>::SharedPtr pub_brake_status_frame_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::DriveStatus>::SharedPtr pub_drive_status_frame_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::VehDyncState>::SharedPtr pub_veh_dync_state_frame_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::VehState>::SharedPtr pub_veh_state_frame_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::FaultReport>::SharedPtr pub_fault_report_;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_steer_angle_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_steer_torque_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_steer_wheel_spd_;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_brake_pressure_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_brake_pedal_pos_;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_throttle_pct_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_motor_current_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_motor_speed_;
|
||||
rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr pub_gear_act_;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_veh_speed_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_veh_accel_;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr pub_run_mode_;
|
||||
|
||||
// Autoware 兼容发布器
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr pub_aw_control_mode_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::VelocityReport>::SharedPtr pub_aw_velocity_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::SteeringReport>::SharedPtr pub_aw_steering_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::GearReport>::SharedPtr pub_aw_gear_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::TurnIndicatorsReport>::SharedPtr pub_aw_turn_indicators_;
|
||||
rclcpp::Publisher<autoware_vehicle_msgs::msg::HazardLightsReport>::SharedPtr pub_aw_hazard_lights_;
|
||||
rclcpp::Publisher<tier4_vehicle_msgs::msg::ActuationStatusStamped>::SharedPtr pub_aw_actuation_status_;
|
||||
|
||||
// 消息缓存指针
|
||||
std::shared_ptr<ecar_can_driver::msg::SteerStatus> steer_status_ptr_;
|
||||
std::shared_ptr<ecar_can_driver::msg::BrakeStatus> brake_status_ptr_;
|
||||
std::shared_ptr<ecar_can_driver::msg::DriveStatus> drive_status_ptr_;
|
||||
std::shared_ptr<ecar_can_driver::msg::VehDyncState> veh_dync_state_ptr_;
|
||||
std::shared_ptr<ecar_can_driver::msg::VehState> veh_state_ptr_;
|
||||
|
||||
// 故障报告缓存
|
||||
std::shared_ptr<ecar_can_driver::msg::FaultReport> fault_report_ptr_;
|
||||
std::shared_ptr<ecar_can_driver::msg::FaultReport> steer_fault_ptr_;
|
||||
std::shared_ptr<ecar_can_driver::msg::FaultReport> drive_fault_ptr_;
|
||||
std::shared_ptr<ecar_can_driver::msg::FaultReport> park_fault_ptr_;
|
||||
std::shared_ptr<ecar_can_driver::msg::FaultReport> bat_fault_ptr_;
|
||||
|
||||
// 接收时间戳
|
||||
rclcpp::Time steer_status_received_time_;
|
||||
rclcpp::Time brake_status_received_time_;
|
||||
rclcpp::Time drive_status_received_time_;
|
||||
rclcpp::Time veh_dync_state_received_time_;
|
||||
rclcpp::Time veh_state_received_time_;
|
||||
rclcpp::Time brake_diag_received_time_;
|
||||
rclcpp::Time steer_diag_received_time_;
|
||||
rclcpp::Time drive_diag_received_time_;
|
||||
rclcpp::Time park_diag_received_time_;
|
||||
rclcpp::Time bat_diag_received_time_;
|
||||
|
||||
// ActuationStatusStamped 合成缓存
|
||||
double cached_accel_pedal_pct_ = 0.0;
|
||||
double cached_brake_pedal_pct_ = 0.0;
|
||||
double cached_steer_status_ = 0.0;
|
||||
builtin_interfaces::msg::Time cached_accel_stamp_;
|
||||
builtin_interfaces::msg::Time cached_brake_stamp_;
|
||||
|
||||
// 实际挡位缓存（用于换挡保护）
|
||||
uint8_t cached_gear_actual_ = 0;
|
||||
};
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<CanReceiverNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,805 @@
|
||||
// ============================================================================
|
||||
// can_sender_node.cpp
|
||||
// 以 50Hz 周期性发送控制命令到可配置的 CAN TX topic (can_msgs::msg::Frame).
|
||||
//
|
||||
// 支持两种模式:
|
||||
// 1) 参数写死模式 (原有行为, autoware_mode=false)
|
||||
// 2) Autoware 模式 (autoware_mode=true):
|
||||
// - 订阅 /control/command/actuation_cmd 获取油门/刹车/转向
|
||||
// - 订阅 /control/command/control_cmd 获取目标车速/加速度/横向转角
|
||||
// - 订阅 /control/command/gear_cmd 获取挡位
|
||||
// - 订阅 /vehicle/cdcu_mcu_gear_act 获取实际挡位反馈（用于换挡保护）
|
||||
// - 订阅 /api/operation_mode/state 获取操作模式（用于STOP模式EPB）
|
||||
// - 提供 /control/control_mode_request 服务, 收到 AUTONOMOUS 请求时
|
||||
// 触发 current_activate_state() 上升沿进入自动驾驶模式
|
||||
//
|
||||
// 安全功能:
|
||||
// - 换挡制动保护: 目标挡位与实际挡位不一致时，强制施加20%制动
|
||||
// - STOP模式EPB: Autoware进入STOP模式时，自动拉起电子手刹
|
||||
// - 命令超时保护: 命令超时时进入安全状态
|
||||
//
|
||||
// 每帧 Byte7 校验: (B0 + B1 + B2 + B3 + B4 + B5 + B6) ^ 0xFF
|
||||
// RollCnt 固定发送 0.
|
||||
// ============================================================================
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <mutex>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "can_msgs/msg/frame.hpp"
|
||||
#include "rcl_interfaces/msg/set_parameters_result.hpp"
|
||||
#include "std_msgs/msg/u_int8.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
|
||||
#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp"
|
||||
#include "tier4_vehicle_msgs/msg/actuation_command.hpp"
|
||||
#include "autoware_vehicle_msgs/msg/gear_command.hpp"
|
||||
#include "autoware_vehicle_msgs/srv/control_mode_command.hpp"
|
||||
#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
|
||||
#include "autoware_control_msgs/msg/control.hpp"
|
||||
|
||||
#include "ecar_can_driver/msg/steer_cmd.hpp"
|
||||
#include "ecar_can_driver/msg/brake_cmd.hpp"
|
||||
#include "ecar_can_driver/msg/drive_cmd.hpp"
|
||||
#include "ecar_can_driver/msg/body_cmd.hpp"
|
||||
#include "ecar_can_driver/msg/park_cmd.hpp"
|
||||
#include "ecar_can_driver/msg/power_cmd.hpp"
|
||||
|
||||
#include "ecar_can_driver/dbc_codec.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace ecar_can_driver;
|
||||
|
||||
class CanSenderNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
CanSenderNode()
|
||||
: Node("ecar_can_sender")
|
||||
{
|
||||
// -------- 参数 --------
|
||||
autoware_mode_ = declare_parameter<bool> ("autoware_mode", true);
|
||||
target_steer_angle_deg_ = declare_parameter<double>("target_steer_angle_deg", 90.0);
|
||||
target_brake_pedal_pct_ = declare_parameter<double>("target_brake_pedal_pct", 100.0);
|
||||
target_brake_press_bar_ = declare_parameter<double>("target_brake_press_bar", 100.0);
|
||||
target_brake_accel_mps2_= declare_parameter<double>("target_brake_accel_mps2", -8.0);
|
||||
target_drive_pedpos_pct_= declare_parameter<double>("target_drive_pedpos_pct", 0.0);
|
||||
target_drive_gear_ = declare_parameter<int> ("target_drive_gear", 0);
|
||||
target_drive_ctrl_mode_ = declare_parameter<int> ("target_drive_ctrl_mode", 0);
|
||||
publish_rate_hz_ = declare_parameter<double>("publish_rate_hz", 50.0);
|
||||
body_power_rate_hz_ = declare_parameter<double>("body_power_rate_hz", 10.0);
|
||||
park_enable_ = declare_parameter<bool> ("park_enable", false);
|
||||
activate_low_duration_ms_ = declare_parameter<double>("activate_low_duration_ms", 20.0);
|
||||
can_tx_topic_ = declare_parameter<std::string>("can_tx_topic", "/kvaser/can_rx");
|
||||
command_timeout_ms_ = declare_parameter<double>("command_timeout_ms", 100.0);
|
||||
gear_switch_brake_pct_ = declare_parameter<double>("gear_switch_brake_pct", 20.0); // 换挡制动百分比
|
||||
enable_epb_on_stop_ = declare_parameter<bool> ("enable_epb_on_stop", true); // STOP模式自动EPB
|
||||
|
||||
if (!std::isfinite(publish_rate_hz_) || publish_rate_hz_ <= 0.0) {
|
||||
RCLCPP_WARN(get_logger(), "publish_rate_hz=%.3f invalid, clamping to 50.0", publish_rate_hz_);
|
||||
publish_rate_hz_ = 50.0;
|
||||
}
|
||||
|
||||
if (!std::isfinite(body_power_rate_hz_) || body_power_rate_hz_ <= 0.0) {
|
||||
RCLCPP_WARN(get_logger(), "body_power_rate_hz=%.3f invalid, clamping to 10.0", body_power_rate_hz_);
|
||||
body_power_rate_hz_ = 10.0;
|
||||
}
|
||||
body_power_period_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double>(1.0 / body_power_rate_hz_));
|
||||
|
||||
if (!std::isfinite(activate_low_duration_ms_) || activate_low_duration_ms_ < 0.0) {
|
||||
activate_low_duration_ms_ = 0.0;
|
||||
}
|
||||
activate_low_duration_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double, std::milli>(activate_low_duration_ms_));
|
||||
|
||||
if (!std::isfinite(command_timeout_ms_) || command_timeout_ms_ <= 0.0) {
|
||||
RCLCPP_WARN(get_logger(), "command_timeout_ms=%.3f invalid, using 100.0", command_timeout_ms_);
|
||||
command_timeout_ms_ = 100.0;
|
||||
}
|
||||
command_timeout_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double, std::milli>(command_timeout_ms_));
|
||||
|
||||
// -------- 发布器 --------
|
||||
pub_can_ = create_publisher<can_msgs::msg::Frame>(can_tx_topic_, rclcpp::QoS(100));
|
||||
|
||||
// -------- 发送命令物理值监控话题 --------
|
||||
pub_steer_cmd_ = create_publisher<ecar_can_driver::msg::SteerCmd>("command/adcu_steer_cmd", 10);
|
||||
pub_brake_cmd_ = create_publisher<ecar_can_driver::msg::BrakeCmd>("command/adcu_brake_cmd", 10);
|
||||
pub_drive_cmd_ = create_publisher<ecar_can_driver::msg::DriveCmd>("command/adcu_drive_cmd", 10);
|
||||
pub_body_cmd_ = create_publisher<ecar_can_driver::msg::BodyCmd>("command/adcu_body_cmd", 10);
|
||||
pub_park_cmd_ = create_publisher<ecar_can_driver::msg::ParkCmd>("command/adcu_park_cmd", 10);
|
||||
pub_power_cmd_ = create_publisher<ecar_can_driver::msg::PowerCmd>("command/adcu_power_cmd", 10);
|
||||
|
||||
// -------- Autoware 订阅 & 服务 --------
|
||||
if (autoware_mode_) {
|
||||
sub_actuation_cmd_ = create_subscription<tier4_vehicle_msgs::msg::ActuationCommandStamped>(
|
||||
"/control/command/actuation_cmd", rclcpp::QoS(1),
|
||||
std::bind(&CanSenderNode::on_actuation_cmd, this, std::placeholders::_1));
|
||||
|
||||
sub_gear_cmd_ = create_subscription<autoware_vehicle_msgs::msg::GearCommand>(
|
||||
"/control/command/gear_cmd", rclcpp::QoS(1),
|
||||
std::bind(&CanSenderNode::on_gear_cmd, this, std::placeholders::_1));
|
||||
|
||||
// 订阅实际挡位反馈（用于换挡保护）
|
||||
sub_gear_actual_ = create_subscription<std_msgs::msg::UInt8>(
|
||||
"vehicle/cdcu_mcu_gear_act", rclcpp::QoS(1),
|
||||
std::bind(&CanSenderNode::on_gear_actual, this, std::placeholders::_1));
|
||||
|
||||
// 订阅操作模式（用于STOP模式EPB）
|
||||
sub_operation_mode_ = create_subscription<autoware_adapi_v1_msgs::msg::OperationModeState>(
|
||||
"/api/operation_mode/state", rclcpp::QoS(1),
|
||||
std::bind(&CanSenderNode::on_operation_mode, this, std::placeholders::_1));
|
||||
|
||||
// 订阅 Autoware 的目标车速/加速度/横向转角指令
|
||||
sub_control_cmd_ = create_subscription<autoware_control_msgs::msg::Control>(
|
||||
"~/input/control_cmd", rclcpp::QoS(1),
|
||||
std::bind(&CanSenderNode::on_control_cmd, this, std::placeholders::_1));
|
||||
|
||||
srv_control_mode_ = create_service<autoware_vehicle_msgs::srv::ControlModeCommand>(
|
||||
"/control/control_mode_request",
|
||||
std::bind(&CanSenderNode::on_control_mode_request, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Autoware mode enabled. Safety features: gear_switch_brake=%.1f%%, epb_on_stop=%s",
|
||||
gear_switch_brake_pct_, enable_epb_on_stop_ ? "true" : "false");
|
||||
}
|
||||
|
||||
// -------- 周期定时器 --------
|
||||
auto period = std::chrono::duration<double>(1.0 / publish_rate_hz_);
|
||||
timer_ = create_wall_timer(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(period),
|
||||
std::bind(&CanSenderNode::on_tick, this));
|
||||
|
||||
// 动态参数回调
|
||||
param_callback_handle_ = add_on_set_parameters_callback(
|
||||
std::bind(&CanSenderNode::on_parameters_changed, this, std::placeholders::_1));
|
||||
|
||||
RCLCPP_INFO(get_logger(),
|
||||
"ECAR CAN sender started. tx_topic=%s, rate=%.1fHz, autoware_mode=%s",
|
||||
can_tx_topic_.c_str(), publish_rate_hz_, autoware_mode_ ? "true" : "false");
|
||||
}
|
||||
|
||||
private:
|
||||
// -------------------------------------------------------------------------
|
||||
// Autoware 回调
|
||||
// -------------------------------------------------------------------------
|
||||
static constexpr double kSteerDegMin = -30.3;
|
||||
static constexpr double kSteerDegMax = 30.3;
|
||||
static constexpr double kBrakePctMin = 0.0;
|
||||
static constexpr double kBrakePctMax = 100.0;
|
||||
static constexpr double kBrakeAccelMin = -20.0;
|
||||
static constexpr double kBrakeAccelMax = 635.35;
|
||||
static constexpr double kAccelPctMin = 0.0;
|
||||
static constexpr double kAccelPctMax = 100.0;
|
||||
static constexpr int kGearMin = 0;
|
||||
static constexpr int kGearMax = 3;
|
||||
|
||||
static double finite_or(double value, double fallback)
|
||||
{
|
||||
return std::isfinite(value) ? value : fallback;
|
||||
}
|
||||
|
||||
static double clamp_finite(double value, double min_value, double max_value, double fallback)
|
||||
{
|
||||
return std::clamp(finite_or(value, fallback), min_value, max_value);
|
||||
}
|
||||
|
||||
static int clamp_gear(int gear)
|
||||
{
|
||||
return std::clamp(gear, kGearMin, kGearMax);
|
||||
}
|
||||
|
||||
void on_actuation_cmd(const tier4_vehicle_msgs::msg::ActuationCommandStamped::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
const auto accel_cmd = msg->actuation.accel_cmd;
|
||||
const auto brake_cmd = msg->actuation.brake_cmd;
|
||||
const auto steer_cmd = msg->actuation.steer_cmd;
|
||||
|
||||
if (!std::isfinite(accel_cmd) || !std::isfinite(brake_cmd) || !std::isfinite(steer_cmd)) {
|
||||
aw_accel_pct_ = 0.0;
|
||||
aw_brake_pct_ = 0.0;
|
||||
aw_steer_deg_ = 0.0;
|
||||
actuation_cmd_received_ = false;
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
get_logger(), *get_clock(), 1000,
|
||||
"Received non-finite actuation command. Forcing inactive safe command.");
|
||||
return;
|
||||
}
|
||||
|
||||
// accel_cmd / brake_cmd: 0.0~1.0 归一化 → 百分比
|
||||
aw_accel_pct_ = clamp_finite(accel_cmd * 100.0, kAccelPctMin, kAccelPctMax, 0.0);
|
||||
aw_brake_pct_ = clamp_finite(brake_cmd * 100.0, kBrakePctMin, kBrakePctMax, 0.0);
|
||||
// steer_cmd: 前轮转角 (rad) -> 前轮转角 (deg)
|
||||
aw_steer_deg_ = clamp_finite(
|
||||
steer_cmd * 180.0 / M_PI, kSteerDegMin, kSteerDegMax, 0.0);
|
||||
actuation_cmd_received_ = true;
|
||||
last_actuation_cmd_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
void on_gear_cmd(const autoware_vehicle_msgs::msg::GearCommand::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
// Autoware 挡位枚举 → 易咖挡位: N=0, D=1, R=2, P=3
|
||||
switch (msg->command) {
|
||||
case autoware_vehicle_msgs::msg::GearCommand::NEUTRAL: aw_gear_ = 0; break;
|
||||
case autoware_vehicle_msgs::msg::GearCommand::DRIVE: aw_gear_ = 1; break;
|
||||
case autoware_vehicle_msgs::msg::GearCommand::REVERSE: aw_gear_ = 2; break;
|
||||
case autoware_vehicle_msgs::msg::GearCommand::PARK: aw_gear_ = 3; break;
|
||||
default: aw_gear_ = 0; break;
|
||||
}
|
||||
aw_gear_ = clamp_gear(aw_gear_);
|
||||
gear_cmd_received_ = true;
|
||||
last_gear_cmd_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
// 实际挡位反馈回调（用于换挡保护）
|
||||
void on_gear_actual(const std_msgs::msg::UInt8::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
actual_gear_ = clamp_gear(static_cast<int>(msg->data));
|
||||
gear_actual_received_ = true;
|
||||
last_gear_actual_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
// 操作模式回调（用于STOP模式EPB）
|
||||
void on_operation_mode(const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
operation_mode_is_stop_ = (msg->mode == autoware_adapi_v1_msgs::msg::OperationModeState::STOP);
|
||||
operation_mode_received_ = true;
|
||||
}
|
||||
|
||||
// 订阅 Autoware 的目标车速/横向控制指令回调
|
||||
void on_control_cmd(const autoware_control_msgs::msg::Control::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
|
||||
const double velocity_mps = msg->longitudinal.velocity;
|
||||
const double acceleration_mps2 = msg->longitudinal.acceleration;
|
||||
const double jerk_mps3 = msg->longitudinal.jerk;
|
||||
|
||||
const double steering_tire_angle_rad = msg->lateral.steering_tire_angle;
|
||||
const double steering_tire_rotation_rate_radps = msg->lateral.steering_tire_rotation_rate;
|
||||
|
||||
if (!std::isfinite(velocity_mps) ||
|
||||
!std::isfinite(acceleration_mps2) ||
|
||||
!std::isfinite(jerk_mps3) ||
|
||||
!std::isfinite(steering_tire_angle_rad) ||
|
||||
!std::isfinite(steering_tire_rotation_rate_radps)) {
|
||||
control_cmd_received_ = false;
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
get_logger(), *get_clock(), 1000,
|
||||
"Received non-finite control command. Ignoring control_cmd.");
|
||||
return;
|
||||
}
|
||||
|
||||
aw_control_velocity_kmph_ = velocity_mps * 3.6;
|
||||
aw_control_accel_mps2_ = acceleration_mps2;
|
||||
aw_control_jerk_mps3_ = jerk_mps3;
|
||||
aw_control_steer_deg_ = clamp_finite(steering_tire_angle_rad * 180.0 / M_PI, kSteerDegMin, kSteerDegMax, 0.0);
|
||||
aw_control_steer_rate_degps_ = steering_tire_rotation_rate_radps * 180.0 / M_PI;
|
||||
|
||||
control_cmd_received_ = true;
|
||||
last_control_cmd_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
|
||||
void on_control_mode_request(
|
||||
const autoware_vehicle_msgs::srv::ControlModeCommand::Request::SharedPtr request,
|
||||
autoware_vehicle_msgs::srv::ControlModeCommand::Response::SharedPtr response)
|
||||
{
|
||||
if (request->mode == autoware_vehicle_msgs::srv::ControlModeCommand::Request::AUTONOMOUS) {
|
||||
RCLCPP_INFO(get_logger(), "ControlModeCommand: AUTONOMOUS requested, triggering activate rising edge");
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
first_tx_seen_ = false;
|
||||
activate_edge_logged_ = false;
|
||||
autonomous_requested_ = true;
|
||||
response->success = true;
|
||||
} else if (request->mode == autoware_vehicle_msgs::srv::ControlModeCommand::Request::MANUAL) {
|
||||
RCLCPP_INFO(get_logger(), "ControlModeCommand: MANUAL requested, deactivating");
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
autonomous_requested_ = false;
|
||||
first_tx_seen_ = false;
|
||||
activate_edge_logged_ = false;
|
||||
response->success = true;
|
||||
} else {
|
||||
response->success = false;
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
bool current_activate_state() const
|
||||
{
|
||||
if (!autoware_mode_) {
|
||||
if (!first_tx_seen_) return false;
|
||||
return (std::chrono::steady_clock::now() - first_tx_time_) >= activate_low_duration_ns_;
|
||||
}
|
||||
// Autoware 模式: 只有收到 AUTONOMOUS 请求后才开始上升沿
|
||||
if (!autonomous_requested_ || !first_tx_seen_) return false;
|
||||
return (std::chrono::steady_clock::now() - first_tx_time_) >= activate_low_duration_ns_;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult on_parameters_changed(
|
||||
const std::vector<rclcpp::Parameter> & parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
result.successful = true;
|
||||
result.reason = "ok";
|
||||
|
||||
if (autoware_mode_) return result;
|
||||
|
||||
double new_target_steer_angle_deg = target_steer_angle_deg_;
|
||||
double new_target_brake_pedal_pct = target_brake_pedal_pct_;
|
||||
double new_target_drive_pedpos_pct = target_drive_pedpos_pct_;
|
||||
int new_target_drive_gear = target_drive_gear_;
|
||||
int new_target_drive_ctrl_mode = target_drive_ctrl_mode_;
|
||||
bool changed = false;
|
||||
|
||||
for (const auto & p : parameters) {
|
||||
const auto & name = p.get_name();
|
||||
if (name == "target_steer_angle_deg") {
|
||||
if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
|
||||
new_target_steer_angle_deg = p.as_double();
|
||||
else if (p.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
|
||||
new_target_steer_angle_deg = static_cast<double>(p.as_int());
|
||||
changed = true;
|
||||
} else if (name == "target_brake_pedal_pct") {
|
||||
if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
|
||||
new_target_brake_pedal_pct = p.as_double();
|
||||
else if (p.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
|
||||
new_target_brake_pedal_pct = static_cast<double>(p.as_int());
|
||||
changed = true;
|
||||
} else if (name == "target_drive_pedpos_pct") {
|
||||
if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
|
||||
new_target_drive_pedpos_pct = p.as_double();
|
||||
else if (p.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
|
||||
new_target_drive_pedpos_pct = static_cast<double>(p.as_int());
|
||||
changed = true;
|
||||
} else if (name == "target_drive_gear") {
|
||||
if (p.get_type() != rclcpp::ParameterType::PARAMETER_INTEGER) {
|
||||
result.successful = false;
|
||||
result.reason = "target_drive_gear must be integer";
|
||||
return result;
|
||||
}
|
||||
new_target_drive_gear = static_cast<int>(p.as_int());
|
||||
changed = true;
|
||||
} else if (name == "target_drive_ctrl_mode") {
|
||||
if (p.get_type() != rclcpp::ParameterType::PARAMETER_INTEGER) {
|
||||
result.successful = false;
|
||||
result.reason = "target_drive_ctrl_mode must be integer";
|
||||
return result;
|
||||
}
|
||||
new_target_drive_ctrl_mode = static_cast<int>(p.as_int());
|
||||
if (new_target_drive_ctrl_mode != 0 && new_target_drive_ctrl_mode != 1) {
|
||||
result.successful = false;
|
||||
result.reason = "target_drive_ctrl_mode must be 0 or 1";
|
||||
return result;
|
||||
}
|
||||
changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (changed) {
|
||||
target_steer_angle_deg_ = new_target_steer_angle_deg;
|
||||
target_brake_pedal_pct_ = new_target_brake_pedal_pct;
|
||||
target_drive_pedpos_pct_ = new_target_drive_pedpos_pct;
|
||||
target_drive_gear_ = new_target_drive_gear;
|
||||
target_drive_ctrl_mode_ = new_target_drive_ctrl_mode;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// 获取当前控制值 (Autoware 模式从订阅获取, 否则从参数获取)
|
||||
// 应用换挡制动保护和STOP模式EPB
|
||||
// -------------------------------------------------------------------------
|
||||
bool get_current_commands(double & steer_deg, double & accel_pct,
|
||||
double & brake_pct, int & gear,
|
||||
uint8_t & drive_ctrl_mode,
|
||||
double & target_velocity_kmph,
|
||||
bool & epb_enable)
|
||||
{
|
||||
epb_enable = false; // 默认不启用EPB
|
||||
drive_ctrl_mode = static_cast<uint8_t>(target_drive_ctrl_mode_); // 0油门模式 1速度模式
|
||||
target_velocity_kmph = 0.0;
|
||||
|
||||
if (autoware_mode_) {
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
const auto steady_now = std::chrono::steady_clock::now();
|
||||
const bool speed_mode = target_drive_ctrl_mode_ == 1;
|
||||
const bool actuation_fresh =
|
||||
actuation_cmd_received_ && (steady_now - last_actuation_cmd_time_) <= command_timeout_ns_;
|
||||
const bool control_fresh =
|
||||
control_cmd_received_ && (steady_now - last_control_cmd_time_) <= command_timeout_ns_;
|
||||
const bool gear_fresh =
|
||||
gear_cmd_received_ && (steady_now - last_gear_cmd_time_) <= command_timeout_ns_;
|
||||
const bool gear_actual_fresh =
|
||||
gear_actual_received_ && (steady_now - last_gear_actual_time_) <= command_timeout_ns_;
|
||||
|
||||
if ((speed_mode && !control_fresh) || (!speed_mode && !actuation_fresh) || !gear_fresh) {
|
||||
steer_deg = 0.0;
|
||||
accel_pct = 0.0;
|
||||
brake_pct = 0.0;
|
||||
gear = 0;
|
||||
if (autonomous_requested_ || actuation_cmd_received_ || control_cmd_received_ ||
|
||||
gear_cmd_received_) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
get_logger(), *get_clock(), 1000,
|
||||
"Autoware command timeout or missing command. Forcing inactive safe command.");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
gear = aw_gear_;
|
||||
|
||||
if (speed_mode) {
|
||||
steer_deg = aw_control_steer_deg_;
|
||||
accel_pct = 0.0;
|
||||
brake_pct = 0.0;
|
||||
target_velocity_kmph = aw_control_velocity_kmph_;
|
||||
} else {
|
||||
steer_deg = aw_steer_deg_;
|
||||
accel_pct = aw_accel_pct_;
|
||||
brake_pct = aw_brake_pct_;
|
||||
}
|
||||
|
||||
// ========== 安全功能 1: 换挡制动保护 ==========
|
||||
// 当目标挡位与实际挡位不一致时，强制施加制动
|
||||
if (gear_actual_fresh && actual_gear_ != gear) {
|
||||
brake_pct = std::max(brake_pct, gear_switch_brake_pct_);
|
||||
accel_pct = 0.0; // 换挡时切断油门
|
||||
target_velocity_kmph = 0.0;
|
||||
if (!gear_switch_logged_) {
|
||||
RCLCPP_INFO(get_logger(), "Gear switch protection: actual=%d, target=%d, brake=%.1f%%",
|
||||
actual_gear_, gear, brake_pct);
|
||||
gear_switch_logged_ = true;
|
||||
}
|
||||
} else {
|
||||
gear_switch_logged_ = false;
|
||||
}
|
||||
|
||||
// ========== 安全功能 2: STOP模式自动EPB ==========
|
||||
if (enable_epb_on_stop_ && operation_mode_received_ && operation_mode_is_stop_) {
|
||||
epb_enable = true;
|
||||
brake_pct = std::max(brake_pct, 30.0); // STOP模式至少30%制动
|
||||
accel_pct = 0.0;
|
||||
target_velocity_kmph = 0.0;
|
||||
if (!stop_epb_logged_) {
|
||||
RCLCPP_INFO(get_logger(), "STOP mode EPB activated");
|
||||
stop_epb_logged_ = true;
|
||||
}
|
||||
} else {
|
||||
stop_epb_logged_ = false;
|
||||
}
|
||||
|
||||
return true;
|
||||
} else {
|
||||
steer_deg = clamp_finite(target_steer_angle_deg_, kSteerDegMin, kSteerDegMax, 0.0);
|
||||
accel_pct = clamp_finite(target_drive_pedpos_pct_, kAccelPctMin, kAccelPctMax, 0.0);
|
||||
brake_pct = clamp_finite(target_brake_pedal_pct_, kBrakePctMin, kBrakePctMax, 0.0);
|
||||
gear = clamp_gear(target_drive_gear_);
|
||||
drive_ctrl_mode = static_cast<uint8_t>(target_drive_ctrl_mode_);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
can_msgs::msg::Frame make_frame(uint32_t id, const uint8_t * data)
|
||||
{
|
||||
can_msgs::msg::Frame frame;
|
||||
frame.header.stamp = now();
|
||||
frame.header.frame_id = "can";
|
||||
frame.id = id;
|
||||
frame.is_rtr = false;
|
||||
frame.is_extended = false;
|
||||
frame.is_error = false;
|
||||
frame.dlc = 8;
|
||||
for (size_t i = 0; i < 8; ++i) frame.data[i] = data[i];
|
||||
return frame;
|
||||
}
|
||||
|
||||
can_msgs::msg::Frame build_steer_cmd(bool active, double steer_deg)
|
||||
{
|
||||
uint8_t d[8] = {0};
|
||||
encode_physical(d, sig::ADCU_Str_Active, active ? 1 : 0);
|
||||
encode_physical(d, sig::ADCU_Str_CtrlMode, 0);
|
||||
encode_physical(d, sig::ADCU_Str_TgtAngle, steer_deg);
|
||||
encode_physical(d, sig::ADCU_Str_TgtCurvature, 0.0);
|
||||
encode_physical(d, sig::ADCU_Str_TgtAngleSpd, 50.0);
|
||||
encode_physical(d, sig::ADCU_StrCmd_RollCnt, 0);
|
||||
apply_checksum(d);
|
||||
return make_frame(msg_id::ADCU_SteerCmd, d);
|
||||
}
|
||||
|
||||
can_msgs::msg::Frame build_brake_cmd(bool active, double brake_pct)
|
||||
{
|
||||
uint8_t d[8] = {0};
|
||||
const auto brake_accel_mps2 =
|
||||
clamp_finite(target_brake_accel_mps2_, kBrakeAccelMin, kBrakeAccelMax, 0.0);
|
||||
encode_physical(d, sig::ADCU_Brk_Active, active ? 1 : 0);
|
||||
encode_physical(d, sig::ADCU_Brk_CtrlMode, 0);
|
||||
encode_physical(d, sig::ADCU_Brk_TgtPedpos, brake_pct);
|
||||
encode_physical(d, sig::ADCU_Brk_TgtPress, 0.0);
|
||||
encode_physical(d, sig::ADCU_Brk_TgtAccSpd, brake_accel_mps2);
|
||||
encode_physical(d, sig::ADCU_BrkCmd_RollCnt, 0);
|
||||
apply_checksum(d);
|
||||
return make_frame(msg_id::ADCU_BrakeCmd, d);
|
||||
}
|
||||
|
||||
can_msgs::msg::Frame build_drive_cmd(
|
||||
bool active, uint8_t drive_ctrl_mode, double accel_pct, int gear,
|
||||
double target_velocity_kmph)
|
||||
{
|
||||
uint8_t d[8] = {0};
|
||||
encode_physical(d, sig::ADCU_Drv_Active, active ? 1 : 0);
|
||||
encode_physical(d, sig::ADCU_Drv_CtrlMode, drive_ctrl_mode);
|
||||
encode_physical(d, sig::ADCU_Drv_TgtGear, gear);
|
||||
encode_physical(d, sig::ADCU_Drv_TgtPedpos, accel_pct);
|
||||
encode_physical(d, sig::ADCU_Drv_TgtVehSpd0, target_velocity_kmph);
|
||||
encode_physical(d, sig::ADCU_Drv_TgtVehAccSpd, 0.0);
|
||||
encode_physical(d, sig::ADCU_Drv_VehSpdLimit, 0.0);
|
||||
encode_physical(d, sig::ADCU_DrvCmd0_RollCnt, 0);
|
||||
apply_checksum(d);
|
||||
return make_frame(msg_id::ADCU_DriveCmd, d);
|
||||
}
|
||||
|
||||
can_msgs::msg::Frame build_body_cmd(bool active)
|
||||
{
|
||||
uint8_t d[8] = {0};
|
||||
encode_physical(d, sig::ADCU_LampCmd_Active, active ? 1 : 0);
|
||||
encode_physical(d, sig::ADCU_LampCmd_RollCnt, 0);
|
||||
apply_checksum(d);
|
||||
return make_frame(msg_id::ADCU_BodyCmd, d);
|
||||
}
|
||||
|
||||
can_msgs::msg::Frame build_bat_cmd()
|
||||
{
|
||||
uint8_t d[8] = {0};
|
||||
encode_physical(d, sig::ADCU_PwrCmd_RollCnt, 0);
|
||||
apply_checksum(d);
|
||||
return make_frame(msg_id::ADCU_PowerCmd, d);
|
||||
}
|
||||
|
||||
can_msgs::msg::Frame build_park_cmd(bool active, bool epb_enable)
|
||||
{
|
||||
uint8_t d[8] = {0};
|
||||
// EPB使能: 如果STOP模式或者显式请求，启用驻车
|
||||
bool park_enable = park_enable_ || epb_enable;
|
||||
encode_physical(d, sig::ADCU_Prk_Active, active ? 1 : 0);
|
||||
encode_physical(d, sig::ADCU_Prk_Enable, park_enable ? 1 : 0);
|
||||
encode_physical(d, sig::ADCU_PrkCmd_RollCnt, 0);
|
||||
apply_checksum(d);
|
||||
return make_frame(msg_id::ADCU_ParkCmd, d);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
void on_tick()
|
||||
{
|
||||
double steer_deg, accel_pct, brake_pct;
|
||||
double target_velocity_kmph;
|
||||
uint8_t drive_ctrl_mode;
|
||||
int gear;
|
||||
bool epb_enable;
|
||||
const bool commands_valid = get_current_commands(
|
||||
steer_deg, accel_pct, brake_pct, gear, drive_ctrl_mode, target_velocity_kmph, epb_enable);
|
||||
|
||||
if (autoware_mode_ && autonomous_requested_ && !commands_valid) {
|
||||
first_tx_seen_ = false;
|
||||
activate_edge_logged_ = false;
|
||||
}
|
||||
|
||||
if (autoware_mode_ && autonomous_requested_ && commands_valid && !first_tx_seen_) {
|
||||
first_tx_seen_ = true;
|
||||
first_tx_time_ = std::chrono::steady_clock::now();
|
||||
} else if (!autoware_mode_ && !first_tx_seen_) {
|
||||
first_tx_seen_ = true;
|
||||
first_tx_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
const auto tick_now = std::chrono::steady_clock::now();
|
||||
const bool active = commands_valid && current_activate_state();
|
||||
|
||||
if (!activate_edge_logged_ && active) {
|
||||
RCLCPP_INFO(get_logger(), "All cmd Activate signals switched from 0 to 1");
|
||||
activate_edge_logged_ = true;
|
||||
}
|
||||
|
||||
pub_can_->publish(build_brake_cmd(active, brake_pct));
|
||||
pub_can_->publish(build_steer_cmd(active, steer_deg));
|
||||
pub_can_->publish(build_drive_cmd(active, drive_ctrl_mode, accel_pct, gear, target_velocity_kmph));
|
||||
pub_can_->publish(build_park_cmd(active, epb_enable));
|
||||
|
||||
// 发布物理值监控话题
|
||||
auto stamp = now();
|
||||
|
||||
if (drive_ctrl_mode == 0) // 油门模式
|
||||
{
|
||||
ecar_can_driver::msg::SteerCmd steer_msg;
|
||||
steer_msg.header.stamp = stamp;
|
||||
steer_msg.adcu_str_active = active;
|
||||
steer_msg.adcu_str_ctrl_mode = 0;
|
||||
steer_msg.adcu_str_tgt_angle = static_cast<float>(steer_deg);
|
||||
steer_msg.adcu_str_tgt_curvature = 0.0f;
|
||||
steer_msg.adcu_str_tgt_angle_spd = 50.0f;
|
||||
pub_steer_cmd_->publish(steer_msg);
|
||||
|
||||
ecar_can_driver::msg::BrakeCmd brake_msg;
|
||||
brake_msg.header.stamp = stamp;
|
||||
brake_msg.adcu_brk_active = active;
|
||||
brake_msg.adcu_brk_ctrl_mode = 0;
|
||||
brake_msg.adcu_brk_tgt_pedpos = static_cast<float>(brake_pct);
|
||||
brake_msg.adcu_brk_tgt_press = 0.0f;
|
||||
brake_msg.adcu_brk_tgt_acc_spd = static_cast<float>(clamp_finite(target_brake_accel_mps2_, kBrakeAccelMin, kBrakeAccelMax, 0.0));
|
||||
pub_brake_cmd_->publish(brake_msg);
|
||||
|
||||
ecar_can_driver::msg::DriveCmd drive_msg;
|
||||
drive_msg.header.stamp = stamp;
|
||||
drive_msg.adcu_drv_active = active;
|
||||
drive_msg.adcu_drv_ctrl_mode = drive_ctrl_mode; // 0油门模式 1速度模式
|
||||
drive_msg.adcu_drv_tgt_gear = static_cast<uint8_t>(gear);
|
||||
drive_msg.adcu_drv_tgt_pedpos = static_cast<float>(accel_pct);
|
||||
drive_msg.adcu_drv_tgt_veh_spd = 0.0f;
|
||||
drive_msg.adcu_drv_tgt_veh_acc_spd = 0.0f;
|
||||
drive_msg.adcu_drv_veh_spd_limit = 0.0f;
|
||||
pub_drive_cmd_->publish(drive_msg);
|
||||
}
|
||||
else // 速度模式
|
||||
{
|
||||
ecar_can_driver::msg::SteerCmd steer_msg;
|
||||
steer_msg.header.stamp = stamp;
|
||||
steer_msg.adcu_str_active = active;
|
||||
steer_msg.adcu_str_ctrl_mode = 0;
|
||||
steer_msg.adcu_str_tgt_angle = static_cast<float>(aw_control_steer_deg_);
|
||||
steer_msg.adcu_str_tgt_curvature = 0.0f;
|
||||
steer_msg.adcu_str_tgt_angle_spd = 50.0f;
|
||||
pub_steer_cmd_->publish(steer_msg);
|
||||
|
||||
ecar_can_driver::msg::BrakeCmd brake_msg;
|
||||
brake_msg.header.stamp = stamp;
|
||||
brake_msg.adcu_brk_active = active;
|
||||
brake_msg.adcu_brk_ctrl_mode = 0;
|
||||
brake_msg.adcu_brk_tgt_pedpos = 0.0f;
|
||||
brake_msg.adcu_brk_tgt_press = 0.0f;
|
||||
brake_msg.adcu_brk_tgt_acc_spd = 0.0f;
|
||||
pub_brake_cmd_->publish(brake_msg);
|
||||
|
||||
ecar_can_driver::msg::DriveCmd drive_msg;
|
||||
drive_msg.header.stamp = stamp;
|
||||
drive_msg.adcu_drv_active = active;
|
||||
drive_msg.adcu_drv_ctrl_mode = drive_ctrl_mode; // 0油门模式 1速度模式
|
||||
drive_msg.adcu_drv_tgt_gear = static_cast<uint8_t>(gear);
|
||||
drive_msg.adcu_drv_tgt_pedpos = 0.0f;
|
||||
drive_msg.adcu_drv_tgt_veh_spd = static_cast<float>(aw_control_velocity_kmph_);
|
||||
drive_msg.adcu_drv_tgt_veh_acc_spd = 0.0f;
|
||||
drive_msg.adcu_drv_veh_spd_limit = 0.0f;
|
||||
pub_drive_cmd_->publish(drive_msg);
|
||||
}
|
||||
|
||||
ecar_can_driver::msg::ParkCmd park_msg;
|
||||
park_msg.header.stamp = stamp;
|
||||
park_msg.adcu_prk_active = active;
|
||||
park_msg.adcu_prk_enable = park_enable_ || epb_enable;
|
||||
pub_park_cmd_->publish(park_msg);
|
||||
|
||||
bool send_body_and_power = false;
|
||||
if (!body_power_sent_once_) {
|
||||
send_body_and_power = true;
|
||||
body_power_sent_once_ = true;
|
||||
last_body_power_tx_time_ = tick_now;
|
||||
} else if ((tick_now - last_body_power_tx_time_) >= body_power_period_ns_) {
|
||||
send_body_and_power = true;
|
||||
last_body_power_tx_time_ = tick_now;
|
||||
}
|
||||
|
||||
if (send_body_and_power) {
|
||||
pub_can_->publish(build_body_cmd(active));
|
||||
pub_can_->publish(build_bat_cmd());
|
||||
|
||||
ecar_can_driver::msg::BodyCmd body_msg;
|
||||
body_msg.header.stamp = stamp;
|
||||
body_msg.adcu_lamp_active = active;
|
||||
pub_body_cmd_->publish(body_msg);
|
||||
|
||||
ecar_can_driver::msg::PowerCmd power_msg;
|
||||
power_msg.header.stamp = stamp;
|
||||
pub_power_cmd_->publish(power_msg);
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
rclcpp::Publisher<can_msgs::msg::Frame>::SharedPtr pub_can_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
|
||||
|
||||
// 发送命令物理值监控发布器
|
||||
rclcpp::Publisher<ecar_can_driver::msg::SteerCmd>::SharedPtr pub_steer_cmd_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::BrakeCmd>::SharedPtr pub_brake_cmd_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::DriveCmd>::SharedPtr pub_drive_cmd_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::BodyCmd>::SharedPtr pub_body_cmd_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::ParkCmd>::SharedPtr pub_park_cmd_;
|
||||
rclcpp::Publisher<ecar_can_driver::msg::PowerCmd>::SharedPtr pub_power_cmd_;
|
||||
|
||||
// Autoware 订阅 & 服务
|
||||
rclcpp::Subscription<tier4_vehicle_msgs::msg::ActuationCommandStamped>::SharedPtr sub_actuation_cmd_;
|
||||
rclcpp::Subscription<autoware_vehicle_msgs::msg::GearCommand>::SharedPtr sub_gear_cmd_;
|
||||
rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr sub_gear_actual_; // 实际挡位反馈
|
||||
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr sub_operation_mode_; // 操作模式
|
||||
rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr sub_control_cmd_;
|
||||
rclcpp::Service<autoware_vehicle_msgs::srv::ControlModeCommand>::SharedPtr srv_control_mode_;
|
||||
|
||||
// 参数
|
||||
bool autoware_mode_;
|
||||
double target_steer_angle_deg_;
|
||||
double target_brake_pedal_pct_;
|
||||
double target_brake_press_bar_;
|
||||
double target_brake_accel_mps2_;
|
||||
double target_drive_pedpos_pct_;
|
||||
int target_drive_gear_;
|
||||
int target_drive_ctrl_mode_;
|
||||
double publish_rate_hz_;
|
||||
double body_power_rate_hz_;
|
||||
bool park_enable_;
|
||||
double activate_low_duration_ms_;
|
||||
double command_timeout_ms_;
|
||||
std::chrono::nanoseconds body_power_period_ns_{100ms};
|
||||
std::chrono::nanoseconds activate_low_duration_ns_{0};
|
||||
std::chrono::nanoseconds command_timeout_ns_{100ms};
|
||||
std::string can_tx_topic_;
|
||||
|
||||
// 安全功能参数
|
||||
double gear_switch_brake_pct_; // 换挡制动百分比
|
||||
bool enable_epb_on_stop_; // STOP模式自动EPB
|
||||
|
||||
// 状态
|
||||
bool first_tx_seen_ = false;
|
||||
std::chrono::steady_clock::time_point first_tx_time_{};
|
||||
bool activate_edge_logged_ = false;
|
||||
bool body_power_sent_once_ = false;
|
||||
std::chrono::steady_clock::time_point last_body_power_tx_time_{};
|
||||
|
||||
// Autoware 命令缓存
|
||||
std::mutex cmd_mutex_;
|
||||
double aw_steer_deg_ = 0.0;
|
||||
double aw_accel_pct_ = 0.0;
|
||||
double aw_brake_pct_ = 0.0;
|
||||
int aw_gear_ = 0;
|
||||
|
||||
// Autoware Control.msg 缓存
|
||||
double aw_control_velocity_kmph_ = 0.0;
|
||||
double aw_control_accel_mps2_ = 0.0;
|
||||
double aw_control_jerk_mps3_ = 0.0;
|
||||
double aw_control_steer_deg_ = 0.0;
|
||||
double aw_control_steer_rate_degps_ = 0.0;
|
||||
|
||||
bool autonomous_requested_ = false;
|
||||
bool actuation_cmd_received_ = false;
|
||||
bool control_cmd_received_ = false;
|
||||
bool gear_cmd_received_ = false;
|
||||
std::chrono::steady_clock::time_point last_actuation_cmd_time_{};
|
||||
std::chrono::steady_clock::time_point last_control_cmd_time_{};
|
||||
std::chrono::steady_clock::time_point last_gear_cmd_time_{};
|
||||
|
||||
// 实际挡位反馈（用于换挡保护）
|
||||
int actual_gear_ = 0;
|
||||
bool gear_actual_received_ = false;
|
||||
std::chrono::steady_clock::time_point last_gear_actual_time_{};
|
||||
bool gear_switch_logged_ = false; // 防止换挡保护日志刷屏
|
||||
|
||||
// 操作模式（用于STOP模式EPB）
|
||||
bool operation_mode_is_stop_ = false;
|
||||
bool operation_mode_received_ = false;
|
||||
bool stop_epb_logged_ = false; // 防止EPB日志刷屏
|
||||
};
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<CanSenderNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,116 @@
|
||||
// ============================================================================
|
||||
// dbc_codec.cpp — Motorola(Big-Endian) signal encode/decode for ECAR DBC
|
||||
// ============================================================================
|
||||
#include "ecar_can_driver/dbc_codec.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace ecar_can_driver
|
||||
{
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Motorola "backward/sequential" 位访问辅助函数
|
||||
// DBC 中 start_bit 采用 Motorola 约定, 以下按逐位遍历方式处理, 健壮且直观.
|
||||
//
|
||||
// 遍历约定:
|
||||
// - 初始位置 = start_bit (在其所在字节内的 bit 编号 0..7, bit7 为 MSB)
|
||||
// - 每读取 1 bit 后:
|
||||
// 如果当前 bit > 0 → 向低位走 (bit - 1, 同字节)
|
||||
// 如果当前 bit = 0 → 跨到下一字节的 bit 7 (byte + 1, bit = 7)
|
||||
// - bit 总位序为 MSB→LSB, 所以读取顺序就是结果整数从高位到低位
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
uint64_t extract_motorola(const uint8_t * data, uint8_t start_bit, uint8_t length)
|
||||
{
|
||||
uint64_t result = 0;
|
||||
int byte = start_bit / 8;
|
||||
int bit = start_bit % 8;
|
||||
|
||||
for (uint8_t i = 0; i < length; ++i) {
|
||||
uint64_t b = (data[byte] >> bit) & 0x01;
|
||||
result = (result << 1) | b;
|
||||
|
||||
if (bit == 0) {
|
||||
bit = 7;
|
||||
byte += 1;
|
||||
} else {
|
||||
bit -= 1;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void insert_motorola(uint8_t * data, uint8_t start_bit, uint8_t length, uint64_t raw)
|
||||
{
|
||||
int byte = start_bit / 8;
|
||||
int bit = start_bit % 8;
|
||||
|
||||
// 从 raw 的 MSB 向 LSB 依次写入
|
||||
for (int i = length - 1; i >= 0; --i) {
|
||||
uint8_t v = (raw >> i) & 0x01;
|
||||
// 清位
|
||||
data[byte] &= static_cast<uint8_t>(~(1u << bit));
|
||||
// 置位
|
||||
data[byte] |= static_cast<uint8_t>(v << bit);
|
||||
|
||||
if (bit == 0) {
|
||||
bit = 7;
|
||||
byte += 1;
|
||||
} else {
|
||||
bit -= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int64_t sign_extend(uint64_t raw, uint8_t length)
|
||||
{
|
||||
if (length == 0 || length >= 64) {
|
||||
return static_cast<int64_t>(raw);
|
||||
}
|
||||
const uint64_t sign_bit = 1ULL << (length - 1);
|
||||
if (raw & sign_bit) {
|
||||
const uint64_t mask = ~((1ULL << length) - 1ULL);
|
||||
return static_cast<int64_t>(raw | mask);
|
||||
}
|
||||
return static_cast<int64_t>(raw);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// 校验: (B0 + B1 + B2 + B3 + B4 + B5 + B6) ^ 0xFF
|
||||
// ---------------------------------------------------------------------------
|
||||
uint8_t sum_checksum(const uint8_t * data, size_t len)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
for (size_t i = 0; i < len; ++i) {
|
||||
sum = static_cast<uint16_t>(sum + data[i]);
|
||||
}
|
||||
return static_cast<uint8_t>((sum & 0xFFu) ^ 0xFFu);
|
||||
}
|
||||
|
||||
void apply_checksum(uint8_t * data)
|
||||
{
|
||||
data[7] = sum_checksum(data, 7);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// 高层物理值接口
|
||||
// ---------------------------------------------------------------------------
|
||||
double decode_physical(const uint8_t * data, const SignalSpec & s)
|
||||
{
|
||||
uint64_t raw = extract_motorola(data, s.start_bit, s.length);
|
||||
int64_t v = s.is_signed ? sign_extend(raw, s.length) : static_cast<int64_t>(raw);
|
||||
return static_cast<double>(v) * s.factor + s.offset;
|
||||
}
|
||||
|
||||
void encode_physical(uint8_t * data, const SignalSpec & s, double physical)
|
||||
{
|
||||
double scaled = (physical - s.offset) / s.factor;
|
||||
int64_t raw = static_cast<int64_t>(std::llround(scaled));
|
||||
|
||||
// 截断到 length 位
|
||||
const uint64_t mask = (s.length >= 64) ? ~0ULL : ((1ULL << s.length) - 1ULL);
|
||||
uint64_t raw_u = static_cast<uint64_t>(raw) & mask;
|
||||
insert_motorola(data, s.start_bit, s.length, raw_u);
|
||||
}
|
||||
|
||||
} // namespace ecar_can_driver
|
||||
@@ -0,0 +1,51 @@
|
||||
name: ROS2 CI
|
||||
|
||||
on:
|
||||
pull_request:
|
||||
branches:
|
||||
- 'develop'
|
||||
- 'main'
|
||||
|
||||
jobs:
|
||||
test_environment:
|
||||
runs-on: [ubuntu-latest]
|
||||
strategy:
|
||||
fail-fast: false
|
||||
matrix:
|
||||
ros_distribution:
|
||||
- humble
|
||||
- iron
|
||||
- jazzy
|
||||
- rolling
|
||||
include:
|
||||
# Humble Hawksbill (May 2022 - May 2027)
|
||||
- docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest
|
||||
ros_distribution: humble
|
||||
ros_version: 2
|
||||
# Iron Irwini (May 2023 - November 2024)
|
||||
- docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest
|
||||
ros_distribution: iron
|
||||
ros_version: 2
|
||||
# Jazzy Jalisco (May 2024 - May 2029)
|
||||
- docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-ros-base-latest
|
||||
ros_distribution: jazzy
|
||||
ros_version: 2
|
||||
# Rolling Ridley (June 2020 - Present)
|
||||
- docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest
|
||||
ros_distribution: rolling
|
||||
ros_version: 2
|
||||
container:
|
||||
image: ${{ matrix.docker_image }}
|
||||
steps:
|
||||
- name: setup directories
|
||||
run: mkdir -p ros_ws/src
|
||||
- name: checkout
|
||||
uses: actions/checkout@v2
|
||||
with:
|
||||
path: ros_ws/src
|
||||
- name: build and test
|
||||
uses: ros-tooling/action-ros-ci@master
|
||||
with:
|
||||
package-name: ros2_socketcan ros2_socketcan_msgs
|
||||
target-ros2-distro: ${{ matrix.ros_distribution }}
|
||||
vcs-repo-file-url: ""
|
||||
@@ -0,0 +1,4 @@
|
||||
*.swp
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
@@ -0,0 +1,76 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package ros2_socketcan
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
1.3.0 (2024-07-16)
|
||||
------------------
|
||||
* Jazzy release
|
||||
* fix: add missing header (`#42 <https://github.com/autowarefoundation/ros2_socketcan/issues/42>`_)
|
||||
* Allow remapping of the canbus topics (`#39 <https://github.com/autowarefoundation/ros2_socketcan/issues/39>`_)
|
||||
* Contributors: Joshua Whitley, Tim Clephas
|
||||
|
||||
1.2.0 (2023-03-03)
|
||||
------------------
|
||||
* Add CAN FD Support (`#28 <https://github.com/autowarefoundation/ros2_socketcan/issues/28>`_)
|
||||
* Add FD support to ROS2 interface.
|
||||
* Add FD send/receive.
|
||||
* Missed some fields in Frame msg.
|
||||
* Make standard and FD mutually exclusive.
|
||||
* Enable runtime checks for standard vs FD.
|
||||
* Try to minimize API changes.
|
||||
* Fix receive_id/fd_receive_id mix-up.
|
||||
* Make new message FD-specific.
|
||||
* Use FdFrame message.
|
||||
* Remove unused functions.
|
||||
* Always resize fd frame buffer to 64 before receive.
|
||||
* Add enable_can_fd to socket_can_bridge.launch.xml.
|
||||
---------
|
||||
* SocketCAN filters (`#25 <https://github.com/autowarefoundation/ros2_socketcan/issues/25>`_)
|
||||
* SocketCAN filters
|
||||
Filters can be set using launch parameter.
|
||||
A list of pairs (can id and mask) are fetched and applied to
|
||||
socket filter.
|
||||
Added unit test for filters application and fuctionality.
|
||||
* Full support of SocketCAN filters
|
||||
SocketCAN filters can be now
|
||||
with string description used
|
||||
by candump utility.
|
||||
Added support for error masks
|
||||
and joined CAN filters.
|
||||
Instead of list of integers, receiver
|
||||
node now uses string parameter
|
||||
in order to receive filters. Filters will
|
||||
now be parsed and setup during
|
||||
configuration.
|
||||
Added unit test for parsing and
|
||||
updated filters unit test.
|
||||
* Reference to man-pages docs of filters syntax
|
||||
Added links referencing man-pages docs for candump,
|
||||
containing more information about socketcan filters
|
||||
syntax used. Links were added to doxygen documentation
|
||||
of filters parsing method and to launch argument
|
||||
description.
|
||||
* Fix unit conversion bug in to_timeval() (`#24 <https://github.com/autowarefoundation/ros2_socketcan/issues/24>`_)
|
||||
* Reorganize folders for adding ros2_socketcan_msgs (`#23 <https://github.com/autowarefoundation/ros2_socketcan/issues/23>`_)
|
||||
Reorganize folders to permit adding a msgs package.
|
||||
* Contributors: Joshua Whitley, Marcel Dudek, ljuricic
|
||||
|
||||
1.1.0 (2022-02-03)
|
||||
------------------
|
||||
* Added bus time (`#12 <https://github.com/autowarefoundation/ros2_socketcan/issues/12>`_)
|
||||
* added the ability to get the bus time for the can packet, versus using ros time when received; packs bus time as part of the can id struct
|
||||
* cleanup; cast fix
|
||||
* chore: apply uncrustify
|
||||
* chore: fix include order for cpplint
|
||||
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
|
||||
* Merge pull request `#10 <https://github.com/autowarefoundation/ros2_socketcan/issues/10>`_ from wep21/ci-galactic
|
||||
Add galactic into action
|
||||
* Add galactic into action
|
||||
* Contributors: Andrew Saba, Daisuke Nishimatsu, Joshua Whitley
|
||||
|
||||
1.0.0 (2021-04-01)
|
||||
------------------
|
||||
* Initial release
|
||||
* Initial port from Autoware.Auto
|
||||
* Initial commit
|
||||
* Contributors: Joshua Whitley, Kenji Miyake, wep21
|
||||
@@ -0,0 +1,64 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(ros2_socketcan)
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake_auto REQUIRED)
|
||||
ament_auto_find_build_dependencies()
|
||||
|
||||
ament_auto_add_library(${PROJECT_NAME} SHARED
|
||||
src/socket_can_common.cpp
|
||||
src/socket_can_id.cpp
|
||||
src/socket_can_receiver.cpp
|
||||
src/socket_can_sender.cpp)
|
||||
|
||||
ament_auto_add_library(socket_can_receiver_node SHARED
|
||||
src/socket_can_receiver_node.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(socket_can_receiver_node
|
||||
${PROJECT_NAME}
|
||||
)
|
||||
|
||||
rclcpp_components_register_node(socket_can_receiver_node
|
||||
PLUGIN "drivers::socketcan::SocketCanReceiverNode"
|
||||
EXECUTABLE socket_can_receiver_node_exe
|
||||
)
|
||||
|
||||
ament_auto_add_library(socket_can_sender_node SHARED
|
||||
src/socket_can_sender_node.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(socket_can_sender_node
|
||||
${PROJECT_NAME}
|
||||
)
|
||||
|
||||
rclcpp_components_register_node(socket_can_sender_node
|
||||
PLUGIN "drivers::socketcan::SocketCanSenderNode"
|
||||
EXECUTABLE socket_can_sender_node_exe
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
|
||||
# TODO(c.ho) Make this into a pytest
|
||||
ament_add_gtest(${PROJECT_NAME}_test
|
||||
test/gtest_main.cpp
|
||||
test/receiver.cpp
|
||||
test/sanity_checks.cpp)
|
||||
target_include_directories(${PROJECT_NAME}_test PUBLIC include)
|
||||
target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME})
|
||||
endif()
|
||||
|
||||
ament_auto_package(INSTALL_TO_SHARE
|
||||
launch
|
||||
)
|
||||
@@ -0,0 +1,13 @@
|
||||
Any contribution that you make to this repository will
|
||||
be under the Apache 2 License, as dictated by that
|
||||
[license](http://www.apache.org/licenses/LICENSE-2.0.html):
|
||||
|
||||
~~~
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
~~~
|
||||
@@ -0,0 +1,201 @@
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
@@ -0,0 +1,103 @@
|
||||
socket_can {#socket_can}
|
||||
===============
|
||||
|
||||
# Purpose / Use cases
|
||||
<!-- Required -->
|
||||
<!-- Things to consider:
|
||||
- Why did we implement this feature? -->
|
||||
|
||||
CAN is the de-facto standard for communication between components on a vehicle.
|
||||
|
||||
As such, to send commands to the vehicle, a mechanism to send messages via CAN is required.
|
||||
|
||||
Similarly a mechanism to receive messages via CAN is required to receive data from the vehicle
|
||||
platform.
|
||||
|
||||
# Design
|
||||
<!-- Required -->
|
||||
<!-- Things to consider:
|
||||
- How does it work? -->
|
||||
|
||||
These classes are a thin wrapper around C functions to manage some extra book-keeping.
|
||||
|
||||
A typed interface for sending is also provided for compile-time checking of data sizes.
|
||||
|
||||
A helper class following the named parameter idiom is provided to wrap the CAN ID.
|
||||
|
||||
Sending and receiving are separate concerns and thus contained in separate classes.
|
||||
|
||||
Finally, care is taken to avoid exposing C/POSIX headers.
|
||||
|
||||
## Assumptions / Known limits
|
||||
<!-- Required -->
|
||||
|
||||
The concern for sender is only simple book-keeping and sending of data.
|
||||
|
||||
The same is true for the receiver.
|
||||
|
||||
Any complex error handling which would require receiving data is outside the concern of this class,
|
||||
and should be a part of a higher level class which contains an instance of this class.
|
||||
|
||||
# Inputs / Outputs / API
|
||||
<!-- Required -->
|
||||
<!-- Things to consider:
|
||||
- How do you use the package / API? -->
|
||||
|
||||
See the [sender API docs](@ref drivers::socketcan::SocketCanSender).
|
||||
and the [receiver API docs](@ref drivers::socketcan::SocketCanReceiver).
|
||||
|
||||
# Inner-workings / Algorithms
|
||||
<!-- If applicable -->
|
||||
|
||||
These classes have no substantive logic.
|
||||
|
||||
Unix's select() function was used to wait for resource availability. On any error, an exception is
|
||||
thrown.
|
||||
|
||||
# Error detection and handling
|
||||
<!-- Required -->
|
||||
|
||||
Both the receiver and the sender classes throw exceptions in the following cases:
|
||||
1. On construction if the specified interface is invalid or cannot be bound
|
||||
2. If the file descriptor is unavailable within the timeout period for sending
|
||||
3. Any other Unix error is raised during the sending process
|
||||
|
||||
Message-level error checking mechanisms a part of the CAN standard are outside the scope of this
|
||||
class.
|
||||
|
||||
# Security considerations
|
||||
<!-- Required -->
|
||||
<!-- Things to consider:
|
||||
- Spoofing (How do you check for and handle fake input?)
|
||||
- Tampering (How do you check for and handle tampered input?)
|
||||
- Repudiation (How are you affected by the actions of external actors?).
|
||||
- Information Disclosure (Can data leak?).
|
||||
- Denial of Service (How do you handle spamming?).
|
||||
- Elevation of Privilege (Do you need to change permission levels during execution?) -->
|
||||
|
||||
This component exposes any security concerns that CAN might have.
|
||||
|
||||
# References / External links
|
||||
<!-- Optional -->
|
||||
|
||||
API inspirations:
|
||||
1. [python-can](https://python-can.readthedocs.io/en/master/bus.html)
|
||||
2. [qtcanbus](https://doc.qt.io/qt-5.9/qcanbusdevice.html#writeFrame)
|
||||
3. [rust socketcan](https://docs.rs/socketcan/1.7.0/socketcan/struct.CANSocket.html)
|
||||
|
||||
Implementation-specific references:
|
||||
1. [SocketCAN reference](https://www.kernel.org/doc/Documentation/networking/can.txt)
|
||||
2. [socket](http://man7.org/linux/man-pages/man2/socket.2.html)
|
||||
3. [bind](http://man7.org/linux/man-pages/man2/bind.2.html)
|
||||
4. [send](http://man7.org/linux/man-pages/man2/send.2.html)
|
||||
5. [ioctl](http://man7.org/linux/man-pages/man2/ioctl.2.html)
|
||||
6. [close](http://man7.org/linux/man-pages/man2/close.2.html)
|
||||
|
||||
CAN-related references:
|
||||
1. [KVaser CAN Protocol Tour](https://www.kvaser.com/can-protocol-tutorial/)
|
||||
2. [Kvaser Higher Level Protocols](https://www.kvaser.com/about-can/higher-layer-protocols/)
|
||||
|
||||
# Future extensions / Unimplemented parts
|
||||
<!-- Optional -->
|
||||
|
||||
- AutoSAR/PCLint fixes around FDSET and select()
|
||||
+69
@@ -0,0 +1,69 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#ifndef ROS2_SOCKETCAN__SOCKET_CAN_COMMON_HPP_
|
||||
#define ROS2_SOCKETCAN__SOCKET_CAN_COMMON_HPP_
|
||||
|
||||
#include <sys/select.h>
|
||||
#include <sys/time.h>
|
||||
#include <linux/can.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
|
||||
/// Bind a non-blocking CAN_RAW socket to the given interface
|
||||
/// \param[in] interface The name of the interface to bind, must be smaller than IFNAMSIZ
|
||||
/// \param[in] enable_fd Whether this socket uses CAN FD or not
|
||||
/// \return The file descriptor bound to the given interface
|
||||
/// \throw std::runtime_error If one of socket(), fnctl(), ioctl(), bind() failed
|
||||
/// \throw std::domain_error If the provided interface name is too long
|
||||
int32_t bind_can_socket(const std::string & interface, bool enable_fd);
|
||||
|
||||
/// Set SocketCAN filters
|
||||
/// \param[in] fd File descriptor of the socket
|
||||
/// \param[in] f_list List of filters to be applied.
|
||||
/// \throw std::runtime_error If filters couldn't be applied
|
||||
void set_can_filter(int32_t fd, const std::vector<struct can_filter> & f_list);
|
||||
|
||||
/// Set SocketCAN error filter
|
||||
/// \param[in] fd File descriptor of the socket
|
||||
/// \param[in] err_mask Error mask to be applied as a filter
|
||||
void set_can_err_filter(int32_t fd, can_err_mask_t err_mask);
|
||||
|
||||
/// Set filters joining option for SocketCAN. If set, all filters
|
||||
/// must match for the frame to be passed.
|
||||
/// \param[in] fd File descriptor of the socket
|
||||
/// \param[in] join_filters Should the filters be joined?
|
||||
void set_can_filter_join(int32_t fd, bool join_filters);
|
||||
|
||||
/// Convert std::chrono duration to timeval (with microsecond resolution)
|
||||
struct timeval to_timeval(const std::chrono::nanoseconds timeout) noexcept;
|
||||
/// Convert timeval to time in microseconds
|
||||
uint64_t from_timeval(const struct timeval tv) noexcept;
|
||||
/// Create a fd_set for use with select() that only contains the specified file descriptor
|
||||
fd_set single_set(int32_t file_descriptor) noexcept;
|
||||
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
|
||||
#endif // ROS2_SOCKETCAN__SOCKET_CAN_COMMON_HPP_
|
||||
+116
@@ -0,0 +1,116 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
/// \file
|
||||
/// \brief This file defines a class a socket sender
|
||||
#ifndef ROS2_SOCKETCAN__SOCKET_CAN_ID_HPP_
|
||||
#define ROS2_SOCKETCAN__SOCKET_CAN_ID_HPP_
|
||||
|
||||
#include <cstdint>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "ros2_socketcan/visibility_control.hpp"
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
|
||||
constexpr std::size_t MAX_DATA_LENGTH = 8U;
|
||||
constexpr std::size_t MAX_FD_DATA_LENGTH = 64U;
|
||||
/// Special error for timeout
|
||||
class SOCKETCAN_PUBLIC SocketCanTimeout : public std::runtime_error
|
||||
{
|
||||
public:
|
||||
explicit SocketCanTimeout(const char * const what)
|
||||
: runtime_error{what} {}
|
||||
}; // class SocketCanTimeout
|
||||
|
||||
enum class FrameType : uint32_t
|
||||
{
|
||||
DATA,
|
||||
ERROR,
|
||||
REMOTE
|
||||
// SocketCan doesn't support Overload frame directly?
|
||||
}; // enum class FrameType
|
||||
|
||||
/// Tag for standard frame
|
||||
struct StandardFrame_ {};
|
||||
//lint -e{1502} NOLINT It's a tag
|
||||
constexpr StandardFrame_ StandardFrame;
|
||||
/// Tag for extended frame
|
||||
struct ExtendedFrame_ {};
|
||||
//lint -e{1502} NOLINT It's a tag
|
||||
constexpr ExtendedFrame_ ExtendedFrame;
|
||||
|
||||
/// A wrapper around can_id_t to make it a little more C++-y
|
||||
/// WARNING: I'm assuming the 0th bit is the MSB aka the leftmost bit
|
||||
class SOCKETCAN_PUBLIC CanId
|
||||
{
|
||||
public:
|
||||
using IdT = uint32_t;
|
||||
using LengthT = uint32_t;
|
||||
// Default constructor: standard data frame with id 0
|
||||
CanId() = default;
|
||||
/// Directly set id, blindly taking whatever bytes are given
|
||||
explicit CanId(const IdT raw_id, const uint64_t bus_time, const LengthT data_length = 0U);
|
||||
/// Sets ID
|
||||
/// \throw std::domain_error if id would get truncated
|
||||
CanId(const IdT id, const uint64_t bus_time, FrameType type, StandardFrame_);
|
||||
/// Sets ID
|
||||
/// \throw std::domain_error if id would get truncated
|
||||
CanId(const IdT id, const uint64_t bus_time, FrameType type, ExtendedFrame_);
|
||||
|
||||
/// Sets bit 31 to 0
|
||||
CanId & standard() noexcept;
|
||||
/// Sets bit 31 to 1
|
||||
CanId & extended() noexcept;
|
||||
/// Sets bit 29 to 1, and bit 30 to 0
|
||||
CanId & error_frame() noexcept;
|
||||
/// Sets bit 29 to 0, and bit 30 to 1
|
||||
CanId & remote_frame() noexcept;
|
||||
/// Clears bits 29 and 30 (sets to 0)
|
||||
CanId & data_frame() noexcept;
|
||||
/// Sets the type accordingly
|
||||
CanId & frame_type(const FrameType type);
|
||||
/// Sets leading bits
|
||||
/// \throw std::domain_error If id would get truncated, 11 bits for Standard, 29 bits for Extended
|
||||
CanId & identifier(const IdT id);
|
||||
|
||||
/// Get just the can_id bits
|
||||
IdT identifier() const noexcept;
|
||||
/// Get the whole id value
|
||||
IdT get() const noexcept;
|
||||
/// Check if frame is extended
|
||||
bool is_extended() const noexcept;
|
||||
/// Check frame type
|
||||
/// \throw std::domain_error If bits are in an inconsistent state
|
||||
FrameType frame_type() const;
|
||||
/// Get the length of the data; only nonzero on received data
|
||||
LengthT length() const noexcept;
|
||||
|
||||
uint64_t get_bus_time() {return bus_time;}
|
||||
|
||||
private:
|
||||
SOCKETCAN_LOCAL CanId(const IdT id, const uint64_t bus_time, FrameType type, bool is_extended);
|
||||
|
||||
IdT m_id{};
|
||||
LengthT m_data_length{};
|
||||
uint64_t bus_time;
|
||||
}; // class CanId
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
|
||||
#endif // ROS2_SOCKETCAN__SOCKET_CAN_ID_HPP_
|
||||
+171
@@ -0,0 +1,171 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
/// \file
|
||||
/// \brief This file defines a class a socket sender
|
||||
#ifndef ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_
|
||||
#define ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "ros2_socketcan/visibility_control.hpp"
|
||||
#include "ros2_socketcan/socket_can_id.hpp"
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
|
||||
/// Simple RAII wrapper around a raw CAN receiver
|
||||
class SOCKETCAN_PUBLIC SocketCanReceiver
|
||||
{
|
||||
public:
|
||||
/// Constructor
|
||||
explicit SocketCanReceiver(const std::string & interface = "can0", const bool enable_fd = false);
|
||||
/// Destructor
|
||||
~SocketCanReceiver() noexcept;
|
||||
|
||||
/// Structure containing possible CAN filter options.
|
||||
struct CanFilterList
|
||||
{
|
||||
std::vector<struct can_filter> filters;
|
||||
can_err_mask_t error_mask = 0;
|
||||
bool join_filters = false;
|
||||
|
||||
/// Default constructor
|
||||
CanFilterList() = default;
|
||||
|
||||
|
||||
/// \copydoc ParseFilters(const std::string & str)
|
||||
explicit CanFilterList(const char * str);
|
||||
|
||||
|
||||
/// \copydoc ParseFilters(const std::string & str)
|
||||
explicit CanFilterList(const std::string & str);
|
||||
|
||||
/// Parse CAN filters string:\n
|
||||
/// Filters:\n
|
||||
/// Comma separated filters can be specified for each given CAN interface.\n
|
||||
/// <can_id>:<can_mask>\n
|
||||
/// (matches when <received_can_id> & mask == can_id & mask)\n
|
||||
/// <can_id>~<can_mask>\n
|
||||
/// (matches when <received_can_id> & mask != can_id & mask)\n
|
||||
/// #<error_mask>\n
|
||||
/// (set error frame filter, see include/linux/can/error.h)\n
|
||||
/// [j|J]\n
|
||||
/// (join the given CAN filters - logical AND semantic)\n
|
||||
///
|
||||
/// CAN IDs, masks and data content are given and expected in hexadecimal values.
|
||||
/// When can_id and can_mask are both 8 digits, they are assumed to be 29 bit EFF.
|
||||
/// \see https://manpages.ubuntu.com/manpages/jammy/man1/candump.1.html
|
||||
/// \param[in] str Input to be parsed.
|
||||
/// \return Populated CanFilterList structure.
|
||||
/// \throw std::runtime_error if string couldn't be parsed.
|
||||
static CanFilterList ParseFilters(const std::string & str);
|
||||
};
|
||||
|
||||
/// Set SocketCAN filters
|
||||
/// \param[in] filters List of filters to be applied.
|
||||
/// \throw std::runtime_error If filters couldn't be applied
|
||||
void SetCanFilters(const CanFilterList & filters);
|
||||
|
||||
/// Receive CAN data
|
||||
/// \param[out] data A buffer to be written with data bytes. Must be at least 8 bytes in size
|
||||
/// \param[in] timeout Maximum duration to wait for data on the file descriptor. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \return The CanId for the received can_frame, with length appropriately populated
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
CanId receive(
|
||||
void * const data,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
|
||||
/// Receive typed CAN data. Slightly less efficient than untyped interface; has extra copy and
|
||||
/// branches
|
||||
/// \tparam Type of data to receive, must be 8 bytes or smaller
|
||||
/// \param[out] data A buffer to be written with data bytes. Must be at least 8 bytes in size
|
||||
/// \param[in] timeout Maximum duration to wait for data on the file descriptor. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \return The CanId for the received can_frame, with length appropriately populated
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error If received data would not fit into provided type
|
||||
/// \throw std::runtime_error on other errors
|
||||
template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
|
||||
CanId receive(
|
||||
T & data,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
|
||||
{
|
||||
static_assert(sizeof(data) <= MAX_DATA_LENGTH, "Data type too large for CAN");
|
||||
std::array<uint8_t, MAX_DATA_LENGTH> data_raw{};
|
||||
const auto ret = receive(&data_raw[0U], timeout);
|
||||
if (ret.length() != sizeof(data)) {
|
||||
throw std::runtime_error{"Received CAN data is of size incompatible with provided type!"};
|
||||
}
|
||||
(void)std::memcpy(&data, &data_raw[0U], ret.length());
|
||||
return ret;
|
||||
}
|
||||
|
||||
/// Receive CAN FD data
|
||||
/// \param[out] data A buffer to be written with data bytes. Must be at least 64 bytes in size
|
||||
/// \param[in] timeout Maximum duration to wait for data on the file descriptor. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \return The CanId for the received canfd_frame, with length appropriately populated
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
CanId receive_fd(
|
||||
void * const data,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
|
||||
/// Receive typed CAN FD data. Slightly less efficient than untyped interface; has extra copy and
|
||||
/// branches
|
||||
/// \tparam Type of data to receive, must be 64 bytes or smaller
|
||||
/// \param[out] data A buffer to be written with data bytes. Must be at least 64 bytes in size
|
||||
/// \param[in] timeout Maximum duration to wait for data on the file descriptor. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \return The CanId for the received canfd_frame, with length appropriately populated
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error If received data would not fit into provided type
|
||||
/// \throw std::runtime_error on other errors
|
||||
template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
|
||||
CanId receive_fd(
|
||||
T & data,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
|
||||
{
|
||||
static_assert(sizeof(data) <= MAX_FD_DATA_LENGTH, "Data type too large for CAN FD");
|
||||
std::array<uint8_t, MAX_FD_DATA_LENGTH> data_raw{};
|
||||
const auto ret = receive_fd(&data_raw[0U], timeout);
|
||||
if (ret.length() != sizeof(data)) {
|
||||
throw std::runtime_error{"Received CAN FD data is of size incompatible with provided type!"};
|
||||
}
|
||||
(void)std::memcpy(&data, &data_raw[0U], ret.length());
|
||||
return ret;
|
||||
}
|
||||
|
||||
private:
|
||||
// Wait for file descriptor to be available to send data via select()
|
||||
SOCKETCAN_LOCAL void wait(const std::chrono::nanoseconds timeout) const;
|
||||
|
||||
int32_t m_file_descriptor;
|
||||
bool m_enable_fd;
|
||||
}; // class SocketCanReceiver
|
||||
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
|
||||
#endif // ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_
|
||||
+88
@@ -0,0 +1,88 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#ifndef ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_NODE_HPP_
|
||||
#define ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "ros2_socketcan/visibility_control.hpp"
|
||||
#include "ros2_socketcan/socket_can_receiver.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "rosidl_runtime_cpp/message_initialization.hpp"
|
||||
#include "can_msgs/msg/frame.hpp"
|
||||
#include "ros2_socketcan_msgs/msg/fd_frame.hpp"
|
||||
#include "lifecycle_msgs/msg/state.hpp"
|
||||
|
||||
namespace lc = rclcpp_lifecycle;
|
||||
using LNI = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
/// \brief SocketCanReceiverNode class which can pass messages
|
||||
/// from CAN hardware or virtual channels
|
||||
class SOCKETCAN_PUBLIC SocketCanReceiverNode final
|
||||
: public lc::LifecycleNode
|
||||
{
|
||||
public:
|
||||
/// \brief Default constructor
|
||||
explicit SocketCanReceiverNode(rclcpp::NodeOptions options);
|
||||
|
||||
/// \brief Callback from transition to "configuring" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_configure(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback from transition to "activating" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_activate(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback from transition to "deactivating" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_deactivate(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback from transition to "unconfigured" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_cleanup(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback from transition to "shutdown" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_shutdown(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback for reading from hardware interface on timer tick.
|
||||
void receive();
|
||||
|
||||
private:
|
||||
std::string interface_;
|
||||
std::shared_ptr<lc::LifecyclePublisher<can_msgs::msg::Frame>> frames_pub_;
|
||||
std::shared_ptr<lc::LifecyclePublisher<ros2_socketcan_msgs::msg::FdFrame>> fd_frames_pub_;
|
||||
std::unique_ptr<SocketCanReceiver> receiver_;
|
||||
std::unique_ptr<std::thread> receiver_thread_;
|
||||
std::chrono::nanoseconds interval_ns_;
|
||||
bool enable_fd_;
|
||||
bool use_bus_time_;
|
||||
};
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
|
||||
#endif // ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_NODE_HPP_
|
||||
+196
@@ -0,0 +1,196 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
/// \file
|
||||
/// \brief This file defines a class a socket sender
|
||||
#ifndef ROS2_SOCKETCAN__SOCKET_CAN_SENDER_HPP_
|
||||
#define ROS2_SOCKETCAN__SOCKET_CAN_SENDER_HPP_
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
#include "ros2_socketcan/visibility_control.hpp"
|
||||
#include "ros2_socketcan/socket_can_id.hpp"
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
|
||||
/// Simple RAII wrapper around a raw CAN sender
|
||||
class SOCKETCAN_PUBLIC SocketCanSender
|
||||
{
|
||||
public:
|
||||
/// Constructor
|
||||
explicit SocketCanSender(
|
||||
const std::string & interface = "can0",
|
||||
const bool enable_fd = false,
|
||||
const CanId & default_id = CanId{});
|
||||
/// Destructor
|
||||
~SocketCanSender() noexcept;
|
||||
|
||||
/// Send raw data with the default id
|
||||
/// \param[in] data A pointer to the beginning of the data to send
|
||||
/// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \param[in] length The amount of data to send starting from the data pointer
|
||||
/// \throw std::domain_error If length is > 8
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
void send(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
|
||||
/// Send raw data with an explicit CAN id
|
||||
/// \param[in] data A pointer to the beginning of the data to send
|
||||
/// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \param[in] id The id field for the CAN frame
|
||||
/// \param[in] length The amount of data to send starting from the data pointer
|
||||
/// \throw std::domain_error If length is > 8
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
void send(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
|
||||
/// Send typed data with the default id
|
||||
/// \tparam Type of data to send, must be 8 bytes or smaller
|
||||
/// \param[in] data The data to send
|
||||
/// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
|
||||
void send(
|
||||
const T & data,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
|
||||
{
|
||||
send(data, m_default_id, timeout);
|
||||
}
|
||||
|
||||
/// Send typed data with an explicit CAN Id
|
||||
/// \tparam Type of data to send, must be 8 bytes or smaller
|
||||
/// \param[in] data The data to send
|
||||
/// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \param[in] id The id field for the CAN frame
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
|
||||
void send(
|
||||
const T & data,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
|
||||
{
|
||||
static_assert(sizeof(data) <= MAX_DATA_LENGTH, "Data type too large for CAN");
|
||||
//lint -e586 I have to use reinterpret cast because I'm operating on bytes, see below NOLINT
|
||||
send_impl(reinterpret_cast<const char *>(&data), sizeof(data), id, timeout);
|
||||
// reinterpret_cast to byte, or (unsigned) char is well defined;
|
||||
// all pointers can implicitly convert to void *
|
||||
}
|
||||
|
||||
/// Send raw data with the default id
|
||||
/// \param[in] data A pointer to the beginning of the data to send
|
||||
/// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \param[in] length The amount of data to send starting from the data pointer
|
||||
/// \throw std::domain_error If length is > 64
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
void send_fd(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
|
||||
/// Send raw data with an explicit CAN id
|
||||
/// \param[in] data A pointer to the beginning of the data to send
|
||||
/// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \param[in] id The id field for the CAN frame
|
||||
/// \param[in] length The amount of data to send starting from the data pointer
|
||||
/// \throw std::domain_error If length is > 64
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
void send_fd(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
|
||||
/// Send typed data with the default id
|
||||
/// \tparam Type of data to send, must be 8 bytes or smaller
|
||||
/// \param[in] data The data to send
|
||||
/// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
|
||||
void send_fd(
|
||||
const T & data,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
|
||||
{
|
||||
send_fd(data, m_default_id, timeout);
|
||||
}
|
||||
|
||||
/// Send typed data with an explicit CAN Id
|
||||
/// \tparam Type of data to send, must be 8 bytes or smaller
|
||||
/// \param[in] data The data to send
|
||||
/// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
|
||||
/// durations are treated the same as zero timeout
|
||||
/// \param[in] id The id field for the CAN frame
|
||||
/// \throw SocketCanTimeout On timeout
|
||||
/// \throw std::runtime_error on other errors
|
||||
template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
|
||||
void send_fd(
|
||||
const T & data,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
|
||||
{
|
||||
static_assert(sizeof(data) <= MAX_FD_DATA_LENGTH, "Data type too large for CAN FD");
|
||||
//lint -e586 I have to use reinterpret cast because I'm operating on bytes, see below NOLINT
|
||||
send_fd_impl(reinterpret_cast<const char *>(&data), sizeof(data), id, timeout);
|
||||
// reinterpret_cast to byte, or (unsigned) char is well defined;
|
||||
// all pointers can implicitly convert to void *
|
||||
}
|
||||
|
||||
/// Get the default CAN id
|
||||
CanId default_id() const noexcept;
|
||||
|
||||
private:
|
||||
// Underlying implementation of sending, data is assumed to be of an appropriate length
|
||||
void send_impl(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout) const;
|
||||
// Underlying implementation of FD sending, data is assumed to be of an appropriate length
|
||||
void send_fd_impl(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout) const;
|
||||
// Wait for file descriptor to be available to send data via select()
|
||||
SOCKETCAN_LOCAL void wait(const std::chrono::nanoseconds timeout) const;
|
||||
|
||||
bool m_enable_fd;
|
||||
int32_t m_file_descriptor{};
|
||||
CanId m_default_id;
|
||||
}; // class SocketCanSender
|
||||
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
|
||||
#endif // ROS2_SOCKETCAN__SOCKET_CAN_SENDER_HPP_
|
||||
+87
@@ -0,0 +1,87 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#ifndef ROS2_SOCKETCAN__SOCKET_CAN_SENDER_NODE_HPP_
|
||||
#define ROS2_SOCKETCAN__SOCKET_CAN_SENDER_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "ros2_socketcan/visibility_control.hpp"
|
||||
#include "ros2_socketcan/socket_can_sender.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "rosidl_runtime_cpp/message_initialization.hpp"
|
||||
#include "can_msgs/msg/frame.hpp"
|
||||
#include "ros2_socketcan_msgs/msg/fd_frame.hpp"
|
||||
#include "lifecycle_msgs/msg/state.hpp"
|
||||
|
||||
namespace lc = rclcpp_lifecycle;
|
||||
using LNI = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
/// \brief SocketCanSenderNode class which can pass messages
|
||||
/// from CAN hardware or virtual channels
|
||||
class SOCKETCAN_PUBLIC SocketCanSenderNode final
|
||||
: public lc::LifecycleNode
|
||||
{
|
||||
public:
|
||||
/// \brief Default constructor
|
||||
explicit SocketCanSenderNode(rclcpp::NodeOptions options);
|
||||
|
||||
/// \brief Callback from transition to "configuring" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_configure(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback from transition to "activating" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_activate(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback from transition to "deactivating" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_deactivate(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback from transition to "unconfigured" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_cleanup(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback from transition to "shutdown" state.
|
||||
/// \param[in] state The current state that the node is in.
|
||||
LNI::CallbackReturn on_shutdown(const lc::State & state) override;
|
||||
|
||||
/// \brief Callback for ros can frame.
|
||||
void on_frame(const can_msgs::msg::Frame::SharedPtr msg);
|
||||
|
||||
/// \brief Callback for ros can fd frame.
|
||||
void on_fd_frame(const ros2_socketcan_msgs::msg::FdFrame::SharedPtr msg);
|
||||
|
||||
private:
|
||||
std::string interface_;
|
||||
bool enable_fd_;
|
||||
rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr frames_sub_;
|
||||
rclcpp::Subscription<ros2_socketcan_msgs::msg::FdFrame>::SharedPtr fd_frames_sub_;
|
||||
std::unique_ptr<SocketCanSender> sender_;
|
||||
std::chrono::nanoseconds timeout_ns_;
|
||||
};
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
|
||||
#endif // ROS2_SOCKETCAN__SOCKET_CAN_SENDER_NODE_HPP_
|
||||
+50
@@ -0,0 +1,50 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
#ifndef ROS2_SOCKETCAN__VISIBILITY_CONTROL_HPP_
|
||||
#define ROS2_SOCKETCAN__VISIBILITY_CONTROL_HPP_
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define SOCKETCAN_EXPORT __attribute__ ((dllexport))
|
||||
#define SOCKETCAN_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define SOCKETCAN_EXPORT __declspec(dllexport)
|
||||
#define SOCKETCAN_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef SOCKETCAN_BUILDING_LIBRARY
|
||||
#define SOCKETCAN_PUBLIC SOCKETCAN_EXPORT
|
||||
#else
|
||||
#define SOCKETCAN_PUBLIC SOCKETCAN_IMPORT
|
||||
#endif
|
||||
#define SOCKETCAN_PUBLIC_TYPE SOCKETCAN_PUBLIC
|
||||
#define SOCKETCAN_LOCAL
|
||||
#else
|
||||
#define SOCKETCAN_EXPORT __attribute__ ((visibility("default")))
|
||||
#define SOCKETCAN_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define SOCKETCAN_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define SOCKETCAN_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define SOCKETCAN_PUBLIC
|
||||
#define SOCKETCAN_LOCAL
|
||||
#endif
|
||||
#define SOCKETCAN_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#endif // ROS2_SOCKETCAN__VISIBILITY_CONTROL_HPP_
|
||||
+27
@@ -0,0 +1,27 @@
|
||||
<launch>
|
||||
|
||||
<arg name="interface" default="can0" />
|
||||
<arg name="receiver_interval_sec" default="0.01" />
|
||||
<arg name="sender_timeout_sec" default="0.01" />
|
||||
<arg name="enable_can_fd" default="false" />
|
||||
<arg name="from_can_bus_topic" default="/socket_can/from_can_bus" />
|
||||
<arg name="to_can_bus_topic" default="/socket_can/to_can_bus" />
|
||||
<arg name="use_bus_time" default="true" />
|
||||
|
||||
<include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">
|
||||
<arg name="interface" value="$(var interface)" />
|
||||
<arg name="interval_sec" value="$(var receiver_interval_sec)" />
|
||||
<arg name="enable_can_fd" value="$(var enable_can_fd)" />
|
||||
<arg name="from_can_bus_topic" value="$(var from_can_bus_topic)" />
|
||||
<arg name="use_bus_time" value="$(var use_bus_time)" />
|
||||
</include>
|
||||
|
||||
<include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_sender.launch.py">
|
||||
<arg name="interface" value="$(var interface)" />
|
||||
<arg name="timeout_sec" value="$(var sender_timeout_sec)" />
|
||||
<arg name="enable_can_fd" value="$(var enable_can_fd)" />
|
||||
<arg name="to_can_bus_topic" value="$(var to_can_bus_topic)" />
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
# Copyright 2021 the Autoware Foundation
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
#
|
||||
# Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (DeclareLaunchArgument, EmitEvent,
|
||||
RegisterEventHandler)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.event_handlers import OnProcessStart
|
||||
from launch.events import matches_action
|
||||
from launch.substitutions import LaunchConfiguration, TextSubstitution
|
||||
from launch_ros.actions import LifecycleNode
|
||||
from launch_ros.event_handlers import OnStateTransition
|
||||
from launch_ros.events.lifecycle import ChangeState
|
||||
from lifecycle_msgs.msg import Transition
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
socket_can_receiver_node = LifecycleNode(
|
||||
package='ros2_socketcan',
|
||||
executable='socket_can_receiver_node_exe',
|
||||
name='socket_can_receiver',
|
||||
namespace=TextSubstitution(text=''),
|
||||
parameters=[{
|
||||
'interface': LaunchConfiguration('interface'),
|
||||
'enable_can_fd': LaunchConfiguration('enable_can_fd'),
|
||||
'interval_sec':
|
||||
LaunchConfiguration('interval_sec'),
|
||||
'filters': LaunchConfiguration('filters'),
|
||||
'use_bus_time': LaunchConfiguration('use_bus_time'),
|
||||
}],
|
||||
remappings=[('from_can_bus', LaunchConfiguration('from_can_bus_topic'))],
|
||||
output='screen')
|
||||
|
||||
socket_can_receiver_configure_event_handler = RegisterEventHandler(
|
||||
event_handler=OnProcessStart(
|
||||
target_action=socket_can_receiver_node,
|
||||
on_start=[
|
||||
EmitEvent(
|
||||
event=ChangeState(
|
||||
lifecycle_node_matcher=matches_action(socket_can_receiver_node),
|
||||
transition_id=Transition.TRANSITION_CONFIGURE,
|
||||
),
|
||||
),
|
||||
],
|
||||
),
|
||||
condition=IfCondition(LaunchConfiguration('auto_configure')),
|
||||
)
|
||||
|
||||
socket_can_receiver_activate_event_handler = RegisterEventHandler(
|
||||
event_handler=OnStateTransition(
|
||||
target_lifecycle_node=socket_can_receiver_node,
|
||||
start_state='configuring',
|
||||
goal_state='inactive',
|
||||
entities=[
|
||||
EmitEvent(
|
||||
event=ChangeState(
|
||||
lifecycle_node_matcher=matches_action(socket_can_receiver_node),
|
||||
transition_id=Transition.TRANSITION_ACTIVATE,
|
||||
),
|
||||
),
|
||||
],
|
||||
),
|
||||
condition=IfCondition(LaunchConfiguration('auto_activate')),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('interface', default_value='can0'),
|
||||
DeclareLaunchArgument('enable_can_fd', default_value='false'),
|
||||
DeclareLaunchArgument('interval_sec', default_value='0.01'),
|
||||
DeclareLaunchArgument('use_bus_time', default_value='false'),
|
||||
DeclareLaunchArgument('filters', default_value='0:0',
|
||||
description='Comma separated filters can be specified for each given'
|
||||
' CAN interface.\n'
|
||||
'\t<can_id>:<can_mask>\n'
|
||||
'\t\t(matches when <received_can_id> & mask == can_id & '
|
||||
'mask)\n'
|
||||
'\t<can_id>~<can_mask>\n'
|
||||
'\t\t(matches when <received_can_id> & mask != can_id & '
|
||||
'mask)\n'
|
||||
'\t#<error_mask>\n'
|
||||
'\t\t(set error frame filter, see include/linux/can/'
|
||||
'error.h)\n'
|
||||
'\t[j|J]\n'
|
||||
'\t\t(join the given CAN filters - logical AND '
|
||||
'semantic)\n\n'
|
||||
'\tCAN IDs, masks and data content are given and '
|
||||
'expected in hexadecimal values. When can_id and '
|
||||
'can_mask are both 8 digits, they are assumed to '
|
||||
"be 29 bit EFF. '0:0' default filter will accept "
|
||||
'all data frames.\n'
|
||||
'\tFor more information about syntax check: '
|
||||
'https://manpages.ubuntu.com/manpages/jammy/'
|
||||
'man1/candump.1.html'),
|
||||
DeclareLaunchArgument('auto_configure', default_value='true'),
|
||||
DeclareLaunchArgument('auto_activate', default_value='true'),
|
||||
DeclareLaunchArgument('from_can_bus_topic', default_value='from_can_bus'),
|
||||
socket_can_receiver_node,
|
||||
socket_can_receiver_configure_event_handler,
|
||||
socket_can_receiver_activate_event_handler,
|
||||
])
|
||||
+88
@@ -0,0 +1,88 @@
|
||||
# Copyright 2021 the Autoware Foundation
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
#
|
||||
# Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (DeclareLaunchArgument, EmitEvent,
|
||||
RegisterEventHandler)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.event_handlers import OnProcessStart
|
||||
from launch.events import matches_action
|
||||
from launch.substitutions import LaunchConfiguration, TextSubstitution
|
||||
from launch_ros.actions import LifecycleNode
|
||||
from launch_ros.event_handlers import OnStateTransition
|
||||
from launch_ros.events.lifecycle import ChangeState
|
||||
from lifecycle_msgs.msg import Transition
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
socket_can_sender_node = LifecycleNode(
|
||||
package='ros2_socketcan',
|
||||
executable='socket_can_sender_node_exe',
|
||||
name='socket_can_sender',
|
||||
namespace=TextSubstitution(text=''),
|
||||
parameters=[{
|
||||
'interface': LaunchConfiguration('interface'),
|
||||
'enable_can_fd': LaunchConfiguration('enable_can_fd'),
|
||||
'timeout_sec':
|
||||
LaunchConfiguration('timeout_sec'),
|
||||
}],
|
||||
remappings=[('to_can_bus', LaunchConfiguration('to_can_bus_topic'))],
|
||||
output='screen')
|
||||
|
||||
socket_can_sender_configure_event_handler = RegisterEventHandler(
|
||||
event_handler=OnProcessStart(
|
||||
target_action=socket_can_sender_node,
|
||||
on_start=[
|
||||
EmitEvent(
|
||||
event=ChangeState(
|
||||
lifecycle_node_matcher=matches_action(socket_can_sender_node),
|
||||
transition_id=Transition.TRANSITION_CONFIGURE,
|
||||
),
|
||||
),
|
||||
],
|
||||
),
|
||||
condition=IfCondition(LaunchConfiguration('auto_configure')),
|
||||
)
|
||||
|
||||
socket_can_sender_activate_event_handler = RegisterEventHandler(
|
||||
event_handler=OnStateTransition(
|
||||
target_lifecycle_node=socket_can_sender_node,
|
||||
start_state='configuring',
|
||||
goal_state='inactive',
|
||||
entities=[
|
||||
EmitEvent(
|
||||
event=ChangeState(
|
||||
lifecycle_node_matcher=matches_action(socket_can_sender_node),
|
||||
transition_id=Transition.TRANSITION_ACTIVATE,
|
||||
),
|
||||
),
|
||||
],
|
||||
),
|
||||
condition=IfCondition(LaunchConfiguration('auto_activate')),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('interface', default_value='can0'),
|
||||
DeclareLaunchArgument('enable_can_fd', default_value='false'),
|
||||
DeclareLaunchArgument('timeout_sec', default_value='0.01'),
|
||||
DeclareLaunchArgument('auto_configure', default_value='true'),
|
||||
DeclareLaunchArgument('auto_activate', default_value='true'),
|
||||
DeclareLaunchArgument('to_can_bus_topic', default_value='to_can_bus'),
|
||||
socket_can_sender_node,
|
||||
socket_can_sender_configure_event_handler,
|
||||
socket_can_sender_activate_event_handler,
|
||||
])
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>ros2_socketcan</name>
|
||||
<version>1.3.0</version>
|
||||
<description>Simple wrapper around SocketCAN</description>
|
||||
<maintainer email="whitleysoftwareservices@gmail.com">Josh Whitley</maintainer>
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake_auto</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
<depend>lifecycle_msgs</depend>
|
||||
<depend>can_msgs</depend>
|
||||
<depend>ros2_socketcan_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,161 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#include "ros2_socketcan/socket_can_common.hpp"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <net/if.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/can/raw.h>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <linux/can.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
int32_t bind_can_socket(const std::string & interface, bool enable_fd)
|
||||
{
|
||||
if (interface.length() >= static_cast<std::string::size_type>(IFNAMSIZ)) {
|
||||
throw std::domain_error{"CAN interface name too long"};
|
||||
}
|
||||
|
||||
// Create file descriptor
|
||||
const auto file_descriptor = socket(PF_CAN, static_cast<int32_t>(SOCK_RAW), CAN_RAW);
|
||||
if (0 > file_descriptor) {
|
||||
throw std::runtime_error{"Failed to open CAN socket"};
|
||||
}
|
||||
// Make it non-blocking so we can use timeouts
|
||||
//lint -e{9001} NOLINT I can't do anything about using this third party octal constant...
|
||||
if (0 != fcntl(file_descriptor, F_SETFL, O_NONBLOCK)) {
|
||||
throw std::runtime_error{"Failed to set CAN socket to nonblocking"};
|
||||
}
|
||||
|
||||
// Set up address/interface name
|
||||
struct ifreq ifr;
|
||||
// The destination struct is local; don't need address
|
||||
(void)strncpy(&ifr.ifr_name[0U], interface.c_str(), interface.length() + 1U);
|
||||
if (0 != ioctl(file_descriptor, static_cast<uint32_t>(SIOCGIFINDEX), &ifr)) {
|
||||
throw std::runtime_error{"Failed to set CAN socket name via ioctl()"};
|
||||
}
|
||||
|
||||
struct sockaddr_can addr;
|
||||
addr.can_family = static_cast<decltype(addr.can_family)>(AF_CAN);
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
// Bind address
|
||||
//lint -save -e586 NOLINT This (c-style casts actually) is the idiomatic way to use sockaddr
|
||||
if (0 > bind(file_descriptor, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr))) {
|
||||
throw std::runtime_error{"Failed to bind CAN socket"};
|
||||
}
|
||||
//lint -restore NOLINT
|
||||
|
||||
// Enable CAN FD support
|
||||
const int32_t enable_canfd = enable_fd ? 1 : 0;
|
||||
if (0 !=
|
||||
setsockopt(
|
||||
file_descriptor, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd,
|
||||
sizeof(enable_canfd)))
|
||||
{
|
||||
throw std::runtime_error{"Failed to enable CAN FD support"};
|
||||
}
|
||||
|
||||
return file_descriptor;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void set_can_filter(int32_t fd, const std::vector<struct can_filter> & f_list)
|
||||
{
|
||||
if (0 !=
|
||||
setsockopt(
|
||||
fd, SOL_CAN_RAW, CAN_RAW_FILTER, f_list.empty() ? NULL : f_list.data(),
|
||||
sizeof(can_filter) * f_list.size()))
|
||||
{
|
||||
throw std::runtime_error{"Failed to set up CAN filters: " + std::string{strerror(errno)}};
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void set_can_err_filter(int32_t fd, can_err_mask_t err_mask)
|
||||
{
|
||||
if (0 !=
|
||||
setsockopt(
|
||||
fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask,
|
||||
sizeof(err_mask)))
|
||||
{
|
||||
throw std::runtime_error{"Failed to set up CAN error filters: " +
|
||||
std::string{strerror(errno)}};
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void set_can_filter_join(int32_t fd, bool join_filters)
|
||||
{
|
||||
auto join = static_cast<int>(join_filters);
|
||||
if (0 !=
|
||||
setsockopt(
|
||||
fd, SOL_CAN_RAW, CAN_RAW_JOIN_FILTERS, &join,
|
||||
sizeof(join)))
|
||||
{
|
||||
throw std::runtime_error{"Failed to set up joined CAN filters: " +
|
||||
std::string{strerror(errno)}};
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
struct timeval to_timeval(const std::chrono::nanoseconds timeout) noexcept
|
||||
{
|
||||
const auto count = timeout.count();
|
||||
constexpr auto BILLION = 1'000'000'000LL;
|
||||
struct timeval c_timeout;
|
||||
c_timeout.tv_sec = static_cast<decltype(c_timeout.tv_sec)>(count / BILLION);
|
||||
c_timeout.tv_usec = static_cast<decltype(c_timeout.tv_usec)>((count % BILLION) / 1000LL);
|
||||
|
||||
return c_timeout;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
uint64_t from_timeval(const struct timeval tv) noexcept
|
||||
{
|
||||
return static_cast<uint64_t>(tv.tv_sec) * 1e6 + tv.tv_usec;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
fd_set single_set(int32_t file_descriptor) noexcept
|
||||
{
|
||||
fd_set descriptor_set;
|
||||
// TODO(c.ho) sort through all these MISRA errors...
|
||||
//lint -save -e9146 NOLINT
|
||||
//lint --e{9063, 9036, 9084, 9027, 9033, 550, 717, 9001, 9093, 953} NOLINT
|
||||
FD_ZERO(&descriptor_set);
|
||||
//lint --e{9063, 9036, 9084, 9027, 9033, 550, 9123, 9125, 9126, 1924, 9130} NOLINT
|
||||
FD_SET(file_descriptor, &descriptor_set);
|
||||
//lint -restore NOLINT
|
||||
|
||||
return descriptor_set;
|
||||
}
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
@@ -0,0 +1,194 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#include <linux/can.h> // for CAN typedef so I can static_assert it
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "ros2_socketcan/socket_can_id.hpp"
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
|
||||
//lint -e{9006} NOLINT false positive: this expression is compile time evaluated
|
||||
static_assert(
|
||||
MAX_DATA_LENGTH == sizeof(std::declval<struct can_frame>().data),
|
||||
"Unexpected CAN frame data size");
|
||||
static_assert(
|
||||
MAX_FD_DATA_LENGTH == sizeof(std::declval<struct canfd_frame>().data),
|
||||
"Unexpected CAN FD frame data size");
|
||||
static_assert(std::is_same<CanId::IdT, canid_t>::value, "Underlying type of CanId is incorrect");
|
||||
constexpr CanId::IdT EXTENDED_MASK = CAN_EFF_FLAG;
|
||||
constexpr CanId::IdT REMOTE_MASK = CAN_RTR_FLAG;
|
||||
constexpr CanId::IdT ERROR_MASK = CAN_ERR_FLAG;
|
||||
constexpr CanId::IdT EXTENDED_ID_MASK = CAN_EFF_MASK;
|
||||
constexpr CanId::IdT STANDARD_ID_MASK = CAN_SFF_MASK;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId::CanId(const IdT raw_id, const uint64_t bus_time, const LengthT data_length)
|
||||
: m_id{raw_id},
|
||||
m_data_length{data_length},
|
||||
bus_time(bus_time)
|
||||
{
|
||||
(void)frame_type(); // just to throw
|
||||
}
|
||||
CanId::CanId(const IdT id, const uint64_t bus_time, FrameType type, StandardFrame_)
|
||||
: CanId{id, bus_time, type, false} {}
|
||||
|
||||
CanId::CanId(const IdT id, const uint64_t bus_time, FrameType type, ExtendedFrame_)
|
||||
: CanId{id, bus_time, type, true} {}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId::CanId(const IdT id, const uint64_t bus_time, FrameType type, bool is_extended)
|
||||
: bus_time(bus_time)
|
||||
{
|
||||
// Set extended bit
|
||||
if (is_extended) {
|
||||
(void)extended();
|
||||
}
|
||||
(void)frame_type(type);
|
||||
(void)identifier(id);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId & CanId::standard() noexcept
|
||||
{
|
||||
//lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
|
||||
m_id = m_id & (~EXTENDED_MASK);
|
||||
return *this;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId & CanId::extended() noexcept
|
||||
{
|
||||
m_id = m_id | EXTENDED_MASK;
|
||||
return *this;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId & CanId::error_frame() noexcept
|
||||
{
|
||||
//lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
|
||||
m_id = m_id & (~REMOTE_MASK);
|
||||
m_id = m_id | ERROR_MASK;
|
||||
return *this;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId & CanId::remote_frame() noexcept
|
||||
{
|
||||
//lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
|
||||
m_id = m_id & (~ERROR_MASK);
|
||||
m_id = m_id | REMOTE_MASK;
|
||||
return *this;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId & CanId::data_frame() noexcept
|
||||
{
|
||||
//lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
|
||||
m_id = m_id & (~ERROR_MASK);
|
||||
//lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
|
||||
m_id = m_id & (~REMOTE_MASK);
|
||||
return *this;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId & CanId::frame_type(const FrameType type)
|
||||
{
|
||||
switch (type) {
|
||||
case FrameType::DATA:
|
||||
(void)data_frame();
|
||||
break;
|
||||
case FrameType::ERROR:
|
||||
(void)error_frame();
|
||||
break;
|
||||
case FrameType::REMOTE:
|
||||
(void)remote_frame();
|
||||
break;
|
||||
default:
|
||||
throw std::logic_error{"CanId: No such type"};
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId & CanId::identifier(const IdT id)
|
||||
{
|
||||
// Can specification: http://esd.cs.ucr.edu/webres/can20.pdf
|
||||
// says "The 7 most significant bits cannot all be recessive (value of 1)", pg 11
|
||||
constexpr auto MAX_EXTENDED = 0x1FBF'FFFFU;
|
||||
constexpr auto MAX_STANDARD = 0x07EFU;
|
||||
static_assert(MAX_EXTENDED <= EXTENDED_ID_MASK, "Max extended id value is wrong");
|
||||
static_assert(MAX_STANDARD <= STANDARD_ID_MASK, "Max extended id value is wrong");
|
||||
auto max_id = MAX_STANDARD;
|
||||
auto unmasked_id = id;
|
||||
if (is_extended()) {
|
||||
max_id = MAX_EXTENDED;
|
||||
unmasked_id = id & ~(EXTENDED_MASK);
|
||||
}
|
||||
if (max_id < unmasked_id) {
|
||||
throw std::domain_error{"CanId would be truncated!"};
|
||||
}
|
||||
// Clear and set
|
||||
//lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
|
||||
m_id = m_id & (~EXTENDED_ID_MASK); // clear ALL ID bits, not just standard bits
|
||||
m_id = m_id | id;
|
||||
return *this;
|
||||
}
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId::IdT CanId::get() const noexcept
|
||||
{
|
||||
return m_id;
|
||||
}
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
bool CanId::is_extended() const noexcept
|
||||
{
|
||||
return (m_id & EXTENDED_MASK) == EXTENDED_MASK;
|
||||
}
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId::IdT CanId::identifier() const noexcept
|
||||
{
|
||||
const auto mask = is_extended() ? EXTENDED_ID_MASK : STANDARD_ID_MASK;
|
||||
return m_id & mask;
|
||||
}
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId::LengthT CanId::length() const noexcept
|
||||
{
|
||||
return m_data_length;
|
||||
}
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
FrameType CanId::frame_type() const
|
||||
{
|
||||
const auto is_error = (m_id & ERROR_MASK) == ERROR_MASK;
|
||||
const auto is_remote = (m_id & REMOTE_MASK) == REMOTE_MASK;
|
||||
if (is_error && is_remote) {
|
||||
throw std::domain_error{"CanId has both bits 29 and 30 set! Inconsistent!"};
|
||||
}
|
||||
|
||||
if (is_error) {
|
||||
return FrameType::ERROR;
|
||||
}
|
||||
if (is_remote) {
|
||||
return FrameType::REMOTE;
|
||||
}
|
||||
return FrameType::DATA;
|
||||
}
|
||||
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
@@ -0,0 +1,207 @@
|
||||
// Copyright 2019 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#include "ros2_socketcan/socket_can_common.hpp"
|
||||
#include "ros2_socketcan/socket_can_receiver.hpp"
|
||||
|
||||
#include <unistd.h> // for close()
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
#include <linux/can.h>
|
||||
#include <linux/sockios.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
#include <cstdio>
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
SocketCanReceiver::SocketCanReceiver(const std::string & interface, const bool enable_fd)
|
||||
: m_file_descriptor{bind_can_socket(interface, enable_fd)},
|
||||
m_enable_fd(enable_fd)
|
||||
{
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
SocketCanReceiver::~SocketCanReceiver() noexcept
|
||||
{
|
||||
// Can't do anything on error; in fact generally shouldn't on close() error
|
||||
(void)close(m_file_descriptor);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
SocketCanReceiver::CanFilterList::CanFilterList(const char * str)
|
||||
{
|
||||
*this = ParseFilters(str);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
SocketCanReceiver::CanFilterList::CanFilterList(const std::string & str)
|
||||
{
|
||||
*this = ParseFilters(str);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
SocketCanReceiver::CanFilterList SocketCanReceiver::CanFilterList::ParseFilters(
|
||||
const std::string & str)
|
||||
{
|
||||
CanFilterList filter_list;
|
||||
filter_list.error_mask = 0;
|
||||
filter_list.join_filters = false;
|
||||
|
||||
std::istringstream input(str);
|
||||
std::string fstr;
|
||||
|
||||
while (getline(input, fstr, ',')) {
|
||||
// trim leading and trailing whitespaces
|
||||
fstr = fstr.substr(
|
||||
fstr.find_first_not_of(" \t"),
|
||||
fstr.find_last_not_of(" \t") - fstr.find_first_not_of(" \t") + 1);
|
||||
|
||||
struct can_filter filter;
|
||||
if (std::sscanf(fstr.c_str(), "%x:%x", &filter.can_id, &filter.can_mask) == 2) {
|
||||
filter.can_mask &= ~CAN_ERR_FLAG;
|
||||
if (fstr.size() > 8 && fstr[8] == ':') {
|
||||
filter.can_id |= CAN_EFF_FLAG;
|
||||
}
|
||||
filter_list.filters.push_back(filter);
|
||||
} else if (std::sscanf(fstr.c_str(), "%x~%x", &filter.can_id, &filter.can_mask) == 2) {
|
||||
filter.can_id |= CAN_INV_FILTER;
|
||||
filter.can_mask &= ~CAN_ERR_FLAG;
|
||||
if (fstr.size() > 8 && fstr[8] == '~') {
|
||||
filter.can_id |= CAN_EFF_FLAG;
|
||||
}
|
||||
filter_list.filters.push_back(filter);
|
||||
} else if (fstr == "j" || fstr == "J") {
|
||||
filter_list.join_filters = true;
|
||||
} else if (std::sscanf(fstr.c_str(), "#%x", &filter_list.error_mask) != 1) {
|
||||
throw std::runtime_error("Error during filter parsing: " + fstr);
|
||||
}
|
||||
}
|
||||
return filter_list;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanReceiver::SetCanFilters(const CanFilterList & filters)
|
||||
{
|
||||
set_can_filter(m_file_descriptor, filters.filters);
|
||||
set_can_err_filter(m_file_descriptor, filters.error_mask);
|
||||
set_can_filter_join(m_file_descriptor, filters.join_filters);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanReceiver::wait(const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
if (decltype(timeout)::zero() < timeout) {
|
||||
auto c_timeout = to_timeval(timeout);
|
||||
auto read_set = single_set(m_file_descriptor);
|
||||
// Wait
|
||||
if (0 == select(m_file_descriptor + 1, &read_set, NULL, NULL, &c_timeout)) {
|
||||
throw SocketCanTimeout{"CAN Receive Timeout"};
|
||||
}
|
||||
//lint --e{9130, 1924, 9123, 9125, 1924, 9126} NOLINT
|
||||
if (!FD_ISSET(m_file_descriptor, &read_set)) {
|
||||
throw SocketCanTimeout{"CAN Receive timeout"};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId SocketCanReceiver::receive(void * const data, const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
if (m_enable_fd) {
|
||||
throw std::runtime_error{"attempted to read standard frame from FD socket"};
|
||||
}
|
||||
|
||||
wait(timeout);
|
||||
// Read
|
||||
struct can_frame frame;
|
||||
const auto nbytes = read(m_file_descriptor, &frame, sizeof(frame));
|
||||
// Checks
|
||||
if (nbytes < 0) {
|
||||
throw std::runtime_error{strerror(errno)};
|
||||
}
|
||||
if (static_cast<std::size_t>(nbytes) < sizeof(frame)) {
|
||||
throw std::runtime_error{"read: incomplete CAN frame"};
|
||||
}
|
||||
if (static_cast<std::size_t>(nbytes) != sizeof(frame)) {
|
||||
throw std::logic_error{"Message was wrong size"};
|
||||
}
|
||||
// Write
|
||||
const auto data_length = static_cast<CanId::LengthT>(frame.can_dlc);
|
||||
(void)std::memcpy(data, static_cast<void *>(&frame.data[0U]), data_length);
|
||||
|
||||
// get bus timestamp
|
||||
struct timeval tv;
|
||||
ioctl(m_file_descriptor, SIOCGSTAMP, &tv);
|
||||
uint64_t bus_time = from_timeval(tv);
|
||||
|
||||
return CanId{frame.can_id, bus_time, data_length};
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId SocketCanReceiver::receive_fd(void * const data, const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
if (!m_enable_fd) {
|
||||
throw std::runtime_error{"attempted to read FD frame from standard socket"};
|
||||
}
|
||||
|
||||
wait(timeout);
|
||||
// Read
|
||||
struct canfd_frame frame;
|
||||
const auto nbytes = read(m_file_descriptor, &frame, sizeof(frame));
|
||||
|
||||
// Checks
|
||||
if (nbytes < 0) {
|
||||
throw std::runtime_error{strerror(errno)};
|
||||
}
|
||||
|
||||
if (static_cast<std::size_t>(nbytes) < sizeof(frame.can_id) + sizeof(frame.len)) {
|
||||
throw std::runtime_error{"read: corrupted CAN frame"};
|
||||
}
|
||||
|
||||
if (frame.len > CANFD_MAX_DLEN) {
|
||||
throw std::runtime_error{"read: frame length is larger than max allowed CAN FD payload length"};
|
||||
}
|
||||
|
||||
const auto data_length = static_cast<CanId::LengthT>(frame.len);
|
||||
// some CAN FD frames are shorter than 64 bytes
|
||||
const auto expected_length = sizeof(frame) - sizeof(frame.data) + data_length;
|
||||
|
||||
if (static_cast<std::size_t>(nbytes) < expected_length) {
|
||||
throw std::runtime_error{"read: incomplete CAN FD frame"};
|
||||
}
|
||||
// Write
|
||||
(void)std::memcpy(data, static_cast<void *>(&frame.data[0U]), data_length);
|
||||
|
||||
// get bus timestamp
|
||||
struct timeval tv;
|
||||
ioctl(m_file_descriptor, SIOCGSTAMP, &tv);
|
||||
uint64_t bus_time = from_timeval(tv);
|
||||
|
||||
return CanId{frame.can_id, bus_time, data_length};
|
||||
}
|
||||
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
+223
@@ -0,0 +1,223 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#include "ros2_socketcan/socket_can_receiver_node.hpp"
|
||||
#include "ros2_socketcan/socket_can_common.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace lc = rclcpp_lifecycle;
|
||||
using LNI = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
|
||||
using lifecycle_msgs::msg::State;
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
SocketCanReceiverNode::SocketCanReceiverNode(rclcpp::NodeOptions options)
|
||||
: lc::LifecycleNode("socket_can_receiver_node", options)
|
||||
{
|
||||
interface_ = this->declare_parameter("interface", "can0");
|
||||
use_bus_time_ = this->declare_parameter<bool>("use_bus_time", false);
|
||||
enable_fd_ = this->declare_parameter<bool>("enable_can_fd", false);
|
||||
double interval_sec = this->declare_parameter("interval_sec", 0.01);
|
||||
this->declare_parameter("filters", "0:0");
|
||||
interval_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double>(interval_sec));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "interface: %s", interface_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "use bus time: %d", use_bus_time_);
|
||||
RCLCPP_INFO(this->get_logger(), "can fd enabled: %s", enable_fd_ ? "true" : "false");
|
||||
RCLCPP_INFO(this->get_logger(), "interval(s): %f", interval_sec);
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanReceiverNode::on_configure(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
|
||||
try {
|
||||
receiver_ = std::make_unique<SocketCanReceiver>(interface_, enable_fd_);
|
||||
// apply CAN filters
|
||||
auto filters = get_parameter("filters").as_string();
|
||||
receiver_->SetCanFilters(SocketCanReceiver::CanFilterList(filters));
|
||||
RCLCPP_INFO(get_logger(), "applied filters: %s", filters.c_str());
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Error opening CAN receiver: %s - %s",
|
||||
interface_.c_str(), ex.what());
|
||||
return LNI::CallbackReturn::FAILURE;
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "Receiver successfully configured.");
|
||||
|
||||
if (!enable_fd_) {
|
||||
frames_pub_ = this->create_publisher<can_msgs::msg::Frame>("from_can_bus", 500);
|
||||
} else {
|
||||
fd_frames_pub_ =
|
||||
this->create_publisher<ros2_socketcan_msgs::msg::FdFrame>("from_can_bus_fd", 500);
|
||||
}
|
||||
|
||||
receiver_thread_ = std::make_unique<std::thread>(&SocketCanReceiverNode::receive, this);
|
||||
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanReceiverNode::on_activate(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
|
||||
// 检查当前状态，如果已经是 active，则忽略重复请求
|
||||
if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) {
|
||||
RCLCPP_DEBUG(this->get_logger(), "Receiver is already active, ignoring duplicate activate request.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
if (!enable_fd_) {
|
||||
frames_pub_->on_activate();
|
||||
} else {
|
||||
fd_frames_pub_->on_activate();
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "Receiver activated.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanReceiverNode::on_deactivate(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
|
||||
if (!enable_fd_) {
|
||||
frames_pub_->on_deactivate();
|
||||
} else {
|
||||
fd_frames_pub_->on_deactivate();
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "Receiver deactivated.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanReceiverNode::on_cleanup(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
|
||||
if (!enable_fd_) {
|
||||
frames_pub_.reset();
|
||||
} else {
|
||||
fd_frames_pub_.reset();
|
||||
}
|
||||
|
||||
if (receiver_thread_->joinable()) {
|
||||
receiver_thread_->join();
|
||||
}
|
||||
RCLCPP_DEBUG(this->get_logger(), "Receiver cleaned up.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanReceiverNode::on_shutdown(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
RCLCPP_DEBUG(this->get_logger(), "Receiver shutting down.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
void SocketCanReceiverNode::receive()
|
||||
{
|
||||
CanId receive_id{};
|
||||
|
||||
if (!enable_fd_) {
|
||||
can_msgs::msg::Frame frame_msg(rosidl_runtime_cpp::MessageInitialization::ZERO);
|
||||
frame_msg.header.frame_id = "can";
|
||||
|
||||
while (rclcpp::ok()) {
|
||||
if (this->get_current_state().id() != State::PRIMARY_STATE_ACTIVE) {
|
||||
std::this_thread::sleep_for(100ms);
|
||||
continue;
|
||||
}
|
||||
|
||||
try {
|
||||
receive_id = receiver_->receive(frame_msg.data.data(), interval_ns_);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
this->get_logger(), *this->get_clock(), 1000,
|
||||
"Error receiving CAN message: %s - %s",
|
||||
interface_.c_str(), ex.what());
|
||||
continue;
|
||||
}
|
||||
|
||||
if (use_bus_time_) {
|
||||
frame_msg.header.stamp =
|
||||
rclcpp::Time(static_cast<int64_t>(receive_id.get_bus_time() * 1000U));
|
||||
} else {
|
||||
frame_msg.header.stamp = this->now();
|
||||
}
|
||||
|
||||
frame_msg.id = receive_id.identifier();
|
||||
frame_msg.is_rtr = (receive_id.frame_type() == FrameType::REMOTE);
|
||||
frame_msg.is_extended = receive_id.is_extended();
|
||||
frame_msg.is_error = (receive_id.frame_type() == FrameType::ERROR);
|
||||
frame_msg.dlc = receive_id.length();
|
||||
frames_pub_->publish(std::move(frame_msg));
|
||||
}
|
||||
} else {
|
||||
ros2_socketcan_msgs::msg::FdFrame fd_frame_msg(rosidl_runtime_cpp::MessageInitialization::ZERO);
|
||||
fd_frame_msg.header.frame_id = "can";
|
||||
|
||||
while (rclcpp::ok()) {
|
||||
if (this->get_current_state().id() != State::PRIMARY_STATE_ACTIVE) {
|
||||
std::this_thread::sleep_for(100ms);
|
||||
continue;
|
||||
}
|
||||
|
||||
fd_frame_msg.data.resize(64);
|
||||
|
||||
try {
|
||||
receive_id = receiver_->receive_fd(fd_frame_msg.data.data<void>(), interval_ns_);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
this->get_logger(), *this->get_clock(), 1000,
|
||||
"Error receiving CAN FD message: %s - %s",
|
||||
interface_.c_str(), ex.what());
|
||||
continue;
|
||||
}
|
||||
|
||||
fd_frame_msg.data.resize(receive_id.length());
|
||||
|
||||
if (use_bus_time_) {
|
||||
fd_frame_msg.header.stamp =
|
||||
rclcpp::Time(static_cast<int64_t>(receive_id.get_bus_time() * 1000U));
|
||||
} else {
|
||||
fd_frame_msg.header.stamp = this->now();
|
||||
}
|
||||
|
||||
fd_frame_msg.id = receive_id.identifier();
|
||||
fd_frame_msg.is_extended = receive_id.is_extended();
|
||||
fd_frame_msg.is_error = (receive_id.frame_type() == FrameType::ERROR);
|
||||
fd_frame_msg.len = receive_id.length();
|
||||
fd_frames_pub_->publish(std::move(fd_frame_msg));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(drivers::socketcan::SocketCanReceiverNode)
|
||||
|
||||
@@ -0,0 +1,175 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#include "ros2_socketcan/socket_can_common.hpp"
|
||||
#include "ros2_socketcan/socket_can_sender.hpp"
|
||||
|
||||
#include <unistd.h> // for close()
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
#include <linux/can.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <chrono>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
SocketCanSender::SocketCanSender(
|
||||
const std::string & interface,
|
||||
const bool enable_fd,
|
||||
const CanId & default_id)
|
||||
: m_enable_fd(enable_fd),
|
||||
m_file_descriptor{bind_can_socket(interface, m_enable_fd)},
|
||||
m_default_id{default_id}
|
||||
{
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
SocketCanSender::~SocketCanSender() noexcept
|
||||
{
|
||||
(void)close(m_file_descriptor);
|
||||
// I'm destructing--there's not much else I can do on an error
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
CanId SocketCanSender::default_id() const noexcept
|
||||
{
|
||||
return m_default_id;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanSender::send(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
if (length > MAX_DATA_LENGTH) {
|
||||
throw std::domain_error{"Size is too large to send via CAN"};
|
||||
}
|
||||
send_impl(data, length, id, timeout);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanSender::send(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
send(data, length, m_default_id, timeout);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanSender::send_fd(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
if (length > MAX_FD_DATA_LENGTH) {
|
||||
throw std::domain_error{"Size is too large to send via CAN FD"};
|
||||
}
|
||||
send_fd_impl(data, length, id, timeout);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanSender::send_fd(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
send_fd(data, length, m_default_id, timeout);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanSender::wait(const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
if (decltype(timeout)::zero() < timeout) {
|
||||
auto c_timeout = to_timeval(timeout);
|
||||
auto write_set = single_set(m_file_descriptor);
|
||||
// Wait
|
||||
if (0 == select(m_file_descriptor + 1, NULL, &write_set, NULL, &c_timeout)) {
|
||||
throw SocketCanTimeout{"CAN Send Timeout"};
|
||||
}
|
||||
//lint --e{9130, 9123, 9125, 1924, 9126} NOLINT
|
||||
if (!FD_ISSET(m_file_descriptor, &write_set)) {
|
||||
throw SocketCanTimeout{"CAN Send timeout"};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanSender::send_impl(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
if (m_enable_fd) {
|
||||
throw std::runtime_error{"Tried to send standard frame from FD socket"};
|
||||
}
|
||||
|
||||
// Use select call on positive timeout
|
||||
wait(timeout);
|
||||
// Actually send the data
|
||||
constexpr int flags = 0; // TODO(c.ho) not implemented
|
||||
struct can_frame data_frame;
|
||||
data_frame.can_id = id.get();
|
||||
// User facing functions do check
|
||||
data_frame.can_dlc = static_cast<decltype(data_frame.can_dlc)>(length);
|
||||
//lint -e{586} NOLINT data_frame is a stack variable; guaranteed not to overlap
|
||||
(void)std::memcpy(static_cast<void *>(&data_frame.data[0U]), data, length);
|
||||
const auto bytes_sent = ::send(m_file_descriptor, &data_frame, sizeof(data_frame), flags);
|
||||
if (0 > bytes_sent) {
|
||||
throw std::runtime_error{strerror(errno)};
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
void SocketCanSender::send_fd_impl(
|
||||
const void * const data,
|
||||
const std::size_t length,
|
||||
const CanId id,
|
||||
const std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
if (!m_enable_fd) {
|
||||
throw std::runtime_error{"Tried to send FD frame from standard socket"};
|
||||
}
|
||||
|
||||
// Use select call on positive timeout
|
||||
wait(timeout);
|
||||
// Actually send the data
|
||||
constexpr int flags = 0; // TODO(c.ho) not implemented
|
||||
struct canfd_frame data_frame;
|
||||
data_frame.can_id = id.get();
|
||||
// User facing functions do check
|
||||
data_frame.len = static_cast<decltype(data_frame.len)>(length);
|
||||
//lint -e{586} NOLINT data_frame is a stack variable; guaranteed not to overlap
|
||||
(void)std::memcpy(static_cast<void *>(&data_frame.data[0U]), data, length);
|
||||
const auto bytes_sent = ::send(m_file_descriptor, &data_frame, sizeof(data_frame), flags);
|
||||
if (0 > bytes_sent) {
|
||||
throw std::runtime_error{strerror(errno)};
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
+163
@@ -0,0 +1,163 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#include "ros2_socketcan/socket_can_sender_node.hpp"
|
||||
#include "ros2_socketcan/socket_can_common.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
namespace lc = rclcpp_lifecycle;
|
||||
using LNI = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
|
||||
using lifecycle_msgs::msg::State;
|
||||
|
||||
namespace drivers
|
||||
{
|
||||
namespace socketcan
|
||||
{
|
||||
SocketCanSenderNode::SocketCanSenderNode(rclcpp::NodeOptions options)
|
||||
: lc::LifecycleNode("socket_can_sender_node", options)
|
||||
{
|
||||
interface_ = this->declare_parameter("interface", "can0");
|
||||
enable_fd_ = this->declare_parameter("enable_can_fd", false);
|
||||
double timeout_sec = this->declare_parameter("timeout_sec", 0.01);
|
||||
timeout_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double>(timeout_sec));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "interface: %s", interface_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "can fd enabled: %s", enable_fd_ ? "true" : "false");
|
||||
RCLCPP_INFO(this->get_logger(), "timeout(s): %f", timeout_sec);
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanSenderNode::on_configure(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
|
||||
try {
|
||||
sender_ = std::make_unique<SocketCanSender>(interface_, enable_fd_);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Error opening CAN sender: %s - %s",
|
||||
interface_.c_str(), ex.what());
|
||||
return LNI::CallbackReturn::FAILURE;
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "Sender successfully configured.");
|
||||
|
||||
if (!enable_fd_) {
|
||||
frames_sub_ = this->create_subscription<can_msgs::msg::Frame>(
|
||||
"to_can_bus", 500, std::bind(&SocketCanSenderNode::on_frame, this, std::placeholders::_1));
|
||||
} else {
|
||||
fd_frames_sub_ = this->create_subscription<ros2_socketcan_msgs::msg::FdFrame>(
|
||||
"to_can_bus_fd", 500, std::bind(
|
||||
&SocketCanSenderNode::on_fd_frame, this,
|
||||
std::placeholders::_1));
|
||||
}
|
||||
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanSenderNode::on_activate(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
RCLCPP_DEBUG(this->get_logger(), "Sender activated.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanSenderNode::on_deactivate(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
RCLCPP_DEBUG(this->get_logger(), "Sender deactivated.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanSenderNode::on_cleanup(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
|
||||
if (!enable_fd_) {
|
||||
frames_sub_.reset();
|
||||
} else {
|
||||
fd_frames_sub_.reset();
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(), "Sender cleaned up.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
LNI::CallbackReturn SocketCanSenderNode::on_shutdown(const lc::State & state)
|
||||
{
|
||||
(void)state;
|
||||
RCLCPP_DEBUG(this->get_logger(), "Sender shutting down.");
|
||||
return LNI::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
void SocketCanSenderNode::on_frame(const can_msgs::msg::Frame::SharedPtr msg)
|
||||
{
|
||||
if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) {
|
||||
FrameType type;
|
||||
if (msg->is_rtr) {
|
||||
type = FrameType::REMOTE;
|
||||
} else if (msg->is_error) {
|
||||
type = FrameType::ERROR;
|
||||
} else {
|
||||
type = FrameType::DATA;
|
||||
}
|
||||
|
||||
CanId send_id = msg->is_extended ? CanId(msg->id, 0, type, ExtendedFrame) :
|
||||
CanId(msg->id, 0, type, StandardFrame);
|
||||
try {
|
||||
sender_->send(msg->data.data(), msg->dlc, send_id, timeout_ns_);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
this->get_logger(), *this->get_clock(), 1000,
|
||||
"Error sending CAN message: %s - %s",
|
||||
interface_.c_str(), ex.what());
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SocketCanSenderNode::on_fd_frame(const ros2_socketcan_msgs::msg::FdFrame::SharedPtr msg)
|
||||
{
|
||||
if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) {
|
||||
FrameType type;
|
||||
if (msg->is_error) {
|
||||
type = FrameType::ERROR;
|
||||
} else {
|
||||
type = FrameType::DATA;
|
||||
}
|
||||
|
||||
CanId send_id = msg->is_extended ? CanId(msg->id, 0, type, ExtendedFrame) :
|
||||
CanId(msg->id, 0, type, StandardFrame);
|
||||
try {
|
||||
sender_->send_fd(msg->data.data<void>(), msg->len, send_id, timeout_ns_);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
this->get_logger(), *this->get_clock(), 1000,
|
||||
"Error sending CAN message: %s - %s",
|
||||
interface_.c_str(), ex.what());
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace socketcan
|
||||
} // namespace drivers
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(drivers::socketcan::SocketCanSenderNode)
|
||||
@@ -0,0 +1,22 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
int32_t main(int32_t argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
@@ -0,0 +1,387 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <linux/can/error.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "ros2_socketcan/socket_can_receiver.hpp"
|
||||
#include "ros2_socketcan/socket_can_sender.hpp"
|
||||
|
||||
using drivers::socketcan::SocketCanReceiver;
|
||||
using drivers::socketcan::SocketCanSender;
|
||||
using drivers::socketcan::CanId;
|
||||
using drivers::socketcan::StandardFrame;
|
||||
using drivers::socketcan::ExtendedFrame;
|
||||
using drivers::socketcan::FrameType;
|
||||
|
||||
// Requires elevated kernel permissions normal containers can't provide
|
||||
class DISABLED_receiver : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp()
|
||||
{
|
||||
constexpr auto test_interface = "vcan0";
|
||||
receiver_ = std::make_unique<SocketCanReceiver>(test_interface);
|
||||
sender_ = std::make_unique<SocketCanSender>(test_interface);
|
||||
}
|
||||
|
||||
std::unique_ptr<SocketCanReceiver> receiver_{};
|
||||
std::unique_ptr<SocketCanSender> sender_{};
|
||||
std::chrono::milliseconds send_timeout_{1LL};
|
||||
std::chrono::milliseconds receive_timeout_{10LL};
|
||||
}; // class receiver
|
||||
|
||||
TEST_F(DISABLED_receiver, basic_typed)
|
||||
{
|
||||
constexpr uint32_t send_msg = 0x5A'5A'5A'5AU;
|
||||
const CanId send_id{};
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
{
|
||||
uint32_t receive_msg{};
|
||||
CanId receive_id{};
|
||||
EXPECT_NO_THROW(receive_id = receiver_->receive(receive_msg, receive_timeout_));
|
||||
EXPECT_EQ(receive_msg, send_msg);
|
||||
EXPECT_EQ(receive_id.length(), sizeof(send_msg));
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DISABLED_receiver, ping_pong)
|
||||
{
|
||||
for (uint64_t idx = 0U; idx < 100U; ++idx) {
|
||||
CanId send_id{};
|
||||
{
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
// Switch between standard and extended
|
||||
if (idx % 2U == 0U) {
|
||||
(void)send_id.extended();
|
||||
} else {
|
||||
(void)send_id.standard();
|
||||
}
|
||||
// Switch between remote and data; error frame is not picked up by socketCan?
|
||||
if (idx % 3U == 0U) {
|
||||
(void)send_id.data_frame();
|
||||
} else {
|
||||
(void)send_id.remote_frame();
|
||||
}
|
||||
}
|
||||
EXPECT_NO_THROW(sender_->send(idx, send_id, send_timeout_)) << idx;
|
||||
{
|
||||
decltype(idx) receive_msg{};
|
||||
CanId receive_id{};
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(receive_msg, idx);
|
||||
EXPECT_EQ(receive_id.length(), sizeof(idx));
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DISABLED_receiver, can_filters_parser)
|
||||
{
|
||||
typedef SocketCanReceiver::CanFilterList CanFilterList;
|
||||
|
||||
auto filter_list = CanFilterList("101:7FF,333:ab,404:1,92345678:DFFFFFFF");
|
||||
ASSERT_EQ(filter_list.filters.size(), 4U);
|
||||
EXPECT_EQ(filter_list.filters[0].can_id, 0x101U);
|
||||
EXPECT_EQ(filter_list.filters[0].can_mask, 0x7FFU);
|
||||
EXPECT_EQ(filter_list.filters[1].can_id, 0x333U);
|
||||
EXPECT_EQ(filter_list.filters[1].can_mask, 0xABU);
|
||||
EXPECT_EQ(filter_list.filters[2].can_id, 0x404U);
|
||||
EXPECT_EQ(filter_list.filters[2].can_mask, 0x1U);
|
||||
EXPECT_EQ(filter_list.filters[3].can_id, 0x92345678U);
|
||||
EXPECT_EQ(filter_list.filters[3].can_mask, 0xDFFFFFFFU);
|
||||
EXPECT_EQ(filter_list.error_mask, 0x0U);
|
||||
EXPECT_FALSE(filter_list.join_filters);
|
||||
|
||||
filter_list = CanFilterList("");
|
||||
EXPECT_TRUE(filter_list.filters.empty());
|
||||
EXPECT_EQ(filter_list.error_mask, 0x0U);
|
||||
EXPECT_FALSE(filter_list.join_filters);
|
||||
filter_list = CanFilterList("#12345");
|
||||
EXPECT_TRUE(filter_list.filters.empty());
|
||||
EXPECT_EQ(filter_list.error_mask, 0x12345U);
|
||||
EXPECT_FALSE(filter_list.join_filters);
|
||||
|
||||
filter_list = CanFilterList("j");
|
||||
EXPECT_TRUE(filter_list.filters.empty());
|
||||
EXPECT_EQ(filter_list.error_mask, 0x0U);
|
||||
EXPECT_TRUE(filter_list.join_filters);
|
||||
|
||||
|
||||
filter_list = CanFilterList("0~0,#FFFFFFFF");
|
||||
ASSERT_EQ(filter_list.filters.size(), 1U);
|
||||
EXPECT_EQ(filter_list.filters[0].can_id, 0x0U | CAN_INV_FILTER);
|
||||
EXPECT_EQ(filter_list.filters[0].can_mask, 0x0U);
|
||||
EXPECT_EQ(filter_list.error_mask, 0xFFFFFFFFU);
|
||||
EXPECT_FALSE(filter_list.join_filters);
|
||||
|
||||
filter_list = CanFilterList("1:2,3~4,5:6,7~8,9:A,j");
|
||||
ASSERT_EQ(filter_list.filters.size(), 5U);
|
||||
EXPECT_EQ(filter_list.filters[0].can_id, 0x1U);
|
||||
EXPECT_EQ(filter_list.filters[0].can_mask, 0x2U);
|
||||
EXPECT_EQ(filter_list.filters[1].can_id, 0x3U | CAN_INV_FILTER);
|
||||
EXPECT_EQ(filter_list.filters[1].can_mask, 0x4U);
|
||||
EXPECT_EQ(filter_list.filters[2].can_id, 0x5U);
|
||||
EXPECT_EQ(filter_list.filters[2].can_mask, 0x6U);
|
||||
EXPECT_EQ(filter_list.filters[3].can_id, 0x7U | CAN_INV_FILTER);
|
||||
EXPECT_EQ(filter_list.filters[3].can_mask, 0x8U);
|
||||
EXPECT_EQ(filter_list.filters[4].can_id, 0x9U);
|
||||
EXPECT_EQ(filter_list.filters[4].can_mask, 0xAU);
|
||||
EXPECT_EQ(filter_list.error_mask, 0x0U);
|
||||
EXPECT_TRUE(filter_list.join_filters);
|
||||
|
||||
filter_list = CanFilterList("ABC:DEF,123:C00007FF,J,#5");
|
||||
ASSERT_EQ(filter_list.filters.size(), 2U);
|
||||
EXPECT_EQ(filter_list.filters[0].can_id, 0xABCU);
|
||||
EXPECT_EQ(filter_list.filters[0].can_mask, 0xDEFU);
|
||||
EXPECT_EQ(filter_list.filters[1].can_id, 0x123U);
|
||||
EXPECT_EQ(filter_list.filters[1].can_mask, 0xC00007FFU);
|
||||
EXPECT_EQ(filter_list.error_mask, 0x5U);
|
||||
EXPECT_TRUE(filter_list.join_filters);
|
||||
|
||||
// whitespace trimming test
|
||||
filter_list = CanFilterList(
|
||||
" ABC:DEF , 123:C00007FF , J , #5 ");
|
||||
ASSERT_EQ(filter_list.filters.size(), 2U);
|
||||
EXPECT_EQ(filter_list.filters[0].can_id, 0xABCU);
|
||||
EXPECT_EQ(filter_list.filters[0].can_mask, 0xDEFU);
|
||||
EXPECT_EQ(filter_list.filters[1].can_id, 0x123U);
|
||||
EXPECT_EQ(filter_list.filters[1].can_mask, 0xC00007FFU);
|
||||
EXPECT_EQ(filter_list.error_mask, 0x5U);
|
||||
EXPECT_TRUE(filter_list.join_filters);
|
||||
|
||||
// test incorrect input
|
||||
std::string str = " ABC:DEF , 123:C00007FF , J , #p5 ";
|
||||
EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
|
||||
str = "1:2,3~4,5:6,7~8,9:A,l";
|
||||
EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
|
||||
str = "1:2,3~4,5;6,7~8,9:A,j";
|
||||
EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
|
||||
str = "1:2,3 ~4,5:6,7~8,9:A,j";
|
||||
EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
|
||||
str = "not a correct string";
|
||||
EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
|
||||
}
|
||||
|
||||
TEST_F(DISABLED_receiver, can_filters)
|
||||
{
|
||||
constexpr uint32_t send_msg = 0x5A'5A'5A'5AU;
|
||||
SocketCanReceiver::CanFilterList filter_list;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// pass only ids: 0x100, 0x250, 0x555 of standard length
|
||||
filter_list.filters = {{0x100, 0xC00007FF}, {0x250, 0xC00007FF}, {0x555, 0xC00007FF}};
|
||||
receiver_->SetCanFilters(filter_list);
|
||||
CanId send_id{};
|
||||
|
||||
// error frame should be blocked
|
||||
send_id.error_frame();
|
||||
send_id.identifier(static_cast<CanId::IdT>(0x100U));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
|
||||
// RTR frame should be blocked
|
||||
send_id.remote_frame();
|
||||
send_id.standard();
|
||||
send_id.identifier(static_cast<CanId::IdT>(0x100U));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
|
||||
// extended data frame should be blocked
|
||||
send_id.data_frame();
|
||||
send_id.extended();
|
||||
send_id.identifier(static_cast<CanId::IdT>(0x100U));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
|
||||
// wrong ids - should be blocked
|
||||
send_id.data_frame();
|
||||
send_id.standard();
|
||||
for (uint32_t idx = 0x50U; idx < 0x100U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
}
|
||||
|
||||
// should pass
|
||||
send_id.identifier(static_cast<CanId::IdT>(0x100U));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
|
||||
// wrong ids - should be blocked
|
||||
for (uint32_t idx = 0x200U; idx < 0x250U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
}
|
||||
|
||||
// should pass
|
||||
send_id.identifier(static_cast<CanId::IdT>(0x250U));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
|
||||
// wrong ids - should be blocked
|
||||
for (uint32_t idx = 0x500U; idx < 0x550U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
}
|
||||
|
||||
// should pass
|
||||
send_id.identifier(static_cast<CanId::IdT>(0x555U));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
|
||||
uint32_t receive_msg{};
|
||||
CanId receive_id{};
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(0x100U, receive_id.get());
|
||||
EXPECT_FALSE(receive_id.is_extended());
|
||||
EXPECT_EQ(receive_id.frame_type(), FrameType::DATA);
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(0x250U, receive_id.get());
|
||||
EXPECT_FALSE(receive_id.is_extended());
|
||||
EXPECT_EQ(receive_id.frame_type(), FrameType::DATA);
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(0x555U, receive_id.get());
|
||||
EXPECT_FALSE(receive_id.is_extended());
|
||||
EXPECT_EQ(receive_id.frame_type(), FrameType::DATA);
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// pass only even ids
|
||||
filter_list.filters = {{0x0, 0x1}};
|
||||
receiver_->SetCanFilters(filter_list);
|
||||
send_id.extended();
|
||||
for (uint32_t idx = 0x1000U; idx < 0x1050U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
if (idx % 2 == 0) {
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
EXPECT_EQ(send_id.frame_type(), FrameType::DATA);
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// pass none ids
|
||||
filter_list.filters = {};
|
||||
receiver_->SetCanFilters(filter_list);
|
||||
send_id.standard();
|
||||
for (uint32_t idx = 0x300U; idx < 0x330U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
}
|
||||
EXPECT_THROW(receive_id = receiver_->receive(receive_msg, receive_timeout_), std::runtime_error);
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// pass all frames (including errors and remotes)
|
||||
filter_list.filters = {{0x0, 0x0}};
|
||||
filter_list.error_mask = 0xFFFFFFFFU;
|
||||
receiver_->SetCanFilters(filter_list);
|
||||
send_id.standard();
|
||||
for (uint32_t idx = 0x300U; idx < 0x330U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
EXPECT_EQ(send_id.frame_type(), FrameType::DATA);
|
||||
}
|
||||
send_id.error_frame();
|
||||
for (uint32_t idx = 0x200U; idx < 0x230U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
EXPECT_EQ(send_id.frame_type(), FrameType::ERROR);
|
||||
}
|
||||
send_id.remote_frame();
|
||||
for (uint32_t idx = 0x100U; idx < 0x130U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
EXPECT_EQ(send_id.frame_type(), FrameType::REMOTE);
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// JOIN FILTERS: pass only CAN_ERR_TX_TIMEOUT and CAN_ERR_BUSOFF error frames
|
||||
// filter_list.filters = {{0x0, 0x0 | CAN_INV_FILTER}};
|
||||
// filter_list.error_mask = (CAN_ERR_TX_TIMEOUT | CAN_ERR_BUSOFF);
|
||||
// receiver_->SetCanFilters(filter_list);
|
||||
receiver_->SetCanFilters(SocketCanReceiver::CanFilterList("0~0,#41")); // same as above comment
|
||||
|
||||
// should be blocked
|
||||
for (uint32_t idx = 0x300U; idx < 0x330U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
}
|
||||
|
||||
// should pass
|
||||
send_id.error_frame();
|
||||
send_id.identifier(static_cast<CanId::IdT>(CAN_ERR_TX_TIMEOUT));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
EXPECT_EQ(send_id.frame_type(), FrameType::ERROR);
|
||||
|
||||
// should be blocked
|
||||
send_id.identifier(static_cast<CanId::IdT>(CAN_ERR_ACK));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
|
||||
// should pass
|
||||
send_id.error_frame();
|
||||
send_id.identifier(static_cast<CanId::IdT>(CAN_ERR_BUSOFF));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
EXPECT_EQ(send_id.frame_type(), FrameType::ERROR);
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// JOIN FILTERS: pass only even id data and remote frames from 0x400 to 0x499
|
||||
filter_list.filters = {{0x0, 0x1}, {0x400, 0x700}};
|
||||
filter_list.error_mask = 0;
|
||||
filter_list.join_filters = true;
|
||||
receiver_->SetCanFilters(filter_list);
|
||||
|
||||
send_id.data_frame();
|
||||
send_id.standard();
|
||||
|
||||
// should be blocked
|
||||
for (uint32_t idx = 0x300U; idx < 0x330U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
}
|
||||
|
||||
// only even should pass
|
||||
for (uint32_t idx = 0x400U; idx < 0x500U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
if (idx % 2 == 0) {
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
EXPECT_EQ(send_id.frame_type(), FrameType::DATA);
|
||||
}
|
||||
}
|
||||
|
||||
// only even should pass
|
||||
send_id.remote_frame();
|
||||
for (uint32_t idx = 0x400U; idx < 0x500U; idx++) {
|
||||
send_id.identifier(static_cast<CanId::IdT>(idx));
|
||||
sender_->send(send_msg, send_id, send_timeout_);
|
||||
if (idx % 2 == 0) {
|
||||
receive_id = receiver_->receive(receive_msg, receive_timeout_);
|
||||
EXPECT_EQ(send_id.get(), receive_id.get());
|
||||
EXPECT_EQ(send_id.frame_type(), FrameType::REMOTE);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,238 @@
|
||||
// Copyright 2021 the Autoware Foundation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
//
|
||||
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <net/if.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <cstring>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "ros2_socketcan/socket_can_sender.hpp"
|
||||
#include "ros2_socketcan/socket_can_receiver.hpp"
|
||||
|
||||
using drivers::socketcan::SocketCanSender;
|
||||
using drivers::socketcan::SocketCanReceiver;
|
||||
using drivers::socketcan::CanId;
|
||||
using drivers::socketcan::StandardFrame;
|
||||
using drivers::socketcan::ExtendedFrame;
|
||||
using drivers::socketcan::FrameType;
|
||||
using drivers::socketcan::MAX_DATA_LENGTH;
|
||||
|
||||
|
||||
// Exercise the CanId stuff
|
||||
TEST(socket_can_basics, id_bad)
|
||||
{
|
||||
// Bad frame type
|
||||
// had to re-write to use lambda to compile properly
|
||||
const auto construct_bad_frame = []() -> auto {
|
||||
constexpr CanId::IdT truncated_id = 0x6000'0000U;
|
||||
return CanId{truncated_id, 0};
|
||||
};
|
||||
EXPECT_THROW(construct_bad_frame(), std::domain_error);
|
||||
|
||||
// Standard truncation
|
||||
const auto construct = [](const auto frame) -> auto {
|
||||
constexpr CanId::IdT truncated_id = 0xFFFF'FFFFU;
|
||||
return CanId{truncated_id, 0, FrameType::DATA, frame};
|
||||
};
|
||||
EXPECT_THROW(construct(StandardFrame), std::domain_error);
|
||||
EXPECT_THROW(construct(ExtendedFrame), std::domain_error);
|
||||
}
|
||||
|
||||
TEST(socket_can_basics, id)
|
||||
{
|
||||
// Default
|
||||
{
|
||||
CanId id{};
|
||||
EXPECT_EQ(id.get(), 0U);
|
||||
EXPECT_FALSE(id.is_extended());
|
||||
EXPECT_EQ(id.frame_type(), FrameType::DATA);
|
||||
// Set to extended
|
||||
id = id.extended();
|
||||
EXPECT_TRUE(id.is_extended());
|
||||
EXPECT_EQ(id.get(), 0x8000'0000U);
|
||||
// Change type to error
|
||||
id = id.error_frame();
|
||||
EXPECT_EQ(id.frame_type(), FrameType::ERROR);
|
||||
EXPECT_EQ(id.get(), 0xA000'0000U);
|
||||
// Change type to remote
|
||||
id = id.remote_frame();
|
||||
EXPECT_EQ(id.frame_type(), FrameType::REMOTE);
|
||||
EXPECT_EQ(id.get(), 0xC000'0000U);
|
||||
// Set to standard
|
||||
id = id.standard();
|
||||
EXPECT_FALSE(id.is_extended());
|
||||
EXPECT_EQ(id.get(), 0x4000'0000U);
|
||||
// Change type to data
|
||||
id = id.data_frame();
|
||||
EXPECT_EQ(id.frame_type(), FrameType::DATA);
|
||||
EXPECT_EQ(id.get(), 0U);
|
||||
}
|
||||
}
|
||||
|
||||
// Sanity checks on constructor
|
||||
TEST(socket_can_basics, bad_constructor)
|
||||
{
|
||||
{
|
||||
const std::string long_name{"abcdefghijklmnopqrs"};
|
||||
ASSERT_GE(long_name.size(), 14U);
|
||||
EXPECT_THROW(SocketCanSender{long_name}, std::domain_error);
|
||||
EXPECT_THROW(SocketCanReceiver{long_name}, std::domain_error);
|
||||
}
|
||||
{
|
||||
constexpr auto nonexistent_interface = "foo";
|
||||
EXPECT_THROW(SocketCanSender{nonexistent_interface}, std::runtime_error);
|
||||
EXPECT_THROW(SocketCanReceiver{nonexistent_interface}, std::runtime_error);
|
||||
}
|
||||
}
|
||||
|
||||
// Requires elevated kernel permissions normal containers can't provide
|
||||
class DISABLED_sender_test : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
using MsgT = uint64_t;
|
||||
static_assert(sizeof(MsgT) == 8U, "Data size is incorrect");
|
||||
|
||||
protected:
|
||||
void SetUp()
|
||||
{
|
||||
constexpr auto TEST_INTERFACE = "vcan0";
|
||||
sender_ = std::make_unique<SocketCanSender>(TEST_INTERFACE);
|
||||
// Set up file descriptor
|
||||
file_descriptor_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
||||
fcntl(file_descriptor_, F_SETFL, O_NONBLOCK);
|
||||
struct sockaddr_can addr;
|
||||
struct ifreq ifr;
|
||||
|
||||
strcpy(ifr.ifr_name, TEST_INTERFACE); // NOLINT literally just copying bytes
|
||||
ioctl(file_descriptor_, SIOCGIFINDEX, &ifr);
|
||||
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
bind(file_descriptor_, (struct sockaddr *)&addr, sizeof(addr));
|
||||
}
|
||||
|
||||
void TearDown()
|
||||
{
|
||||
close(file_descriptor_);
|
||||
}
|
||||
|
||||
uint32_t receive(
|
||||
MsgT & msg,
|
||||
const std::chrono::nanoseconds timeout = std::chrono::milliseconds{1LL})
|
||||
{
|
||||
if (timeout < decltype(timeout)::zero()) {
|
||||
throw std::domain_error{"Negative timeout"};
|
||||
}
|
||||
if (timeout >= std::chrono::seconds{1LL}) {
|
||||
throw std::domain_error{"Timeout >= 1s, not dealing with this"};
|
||||
}
|
||||
// Set up selector
|
||||
{
|
||||
struct timeval c_timeout;
|
||||
c_timeout.tv_sec = 0;
|
||||
c_timeout.tv_usec = std::chrono::duration_cast<std::chrono::milliseconds>(timeout).count();
|
||||
|
||||
fd_set read_set;
|
||||
FD_ZERO(&read_set);
|
||||
FD_SET(file_descriptor_, &read_set);
|
||||
// Wait
|
||||
if (0 == select(file_descriptor_ + 1, &read_set, nullptr, nullptr, &c_timeout)) {
|
||||
throw std::runtime_error{"Timeout"};
|
||||
}
|
||||
if (!FD_ISSET(file_descriptor_, &read_set)) {
|
||||
throw std::runtime_error{"What?"};
|
||||
}
|
||||
}
|
||||
// Read
|
||||
struct can_frame frame;
|
||||
const auto nbytes = read(file_descriptor_, &frame, sizeof(frame));
|
||||
// Checks
|
||||
if (nbytes < 0) {
|
||||
throw std::runtime_error{"CAN raw socket read"};
|
||||
perror("can raw socket read");
|
||||
}
|
||||
if (static_cast<std::size_t>(nbytes) < sizeof(frame)) {
|
||||
throw std::runtime_error{"read: incomplete CAN frame"};
|
||||
}
|
||||
if (static_cast<std::size_t>(nbytes) != sizeof(frame)) {
|
||||
throw std::logic_error{"Message was wrong size"};
|
||||
}
|
||||
// Write
|
||||
(void)std::memcpy(&msg, frame.data, sizeof(msg));
|
||||
return frame.can_id;
|
||||
}
|
||||
|
||||
std::unique_ptr<SocketCanSender> sender_;
|
||||
int file_descriptor_{};
|
||||
}; // class sender_test
|
||||
|
||||
// Minimal usage
|
||||
TEST_F(DISABLED_sender_test, basic_untyped)
|
||||
{
|
||||
constexpr MsgT data = 0xA5A5A5A5A5A5A5A5U;
|
||||
// Use untyped interface
|
||||
{
|
||||
EXPECT_THROW(
|
||||
sender_->send(&data, MAX_DATA_LENGTH + 1U, std::chrono::milliseconds{1LL}),
|
||||
std::domain_error
|
||||
);
|
||||
sender_->send(&data, 8U, std::chrono::milliseconds{1LL});
|
||||
MsgT msg{};
|
||||
ASSERT_NE(msg, data);
|
||||
const auto id = receive(msg);
|
||||
EXPECT_EQ(data, msg);
|
||||
EXPECT_EQ(id, sender_->default_id().get());
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DISABLED_sender_test, basic_typed)
|
||||
{
|
||||
constexpr MsgT data = 0xA5A5A5A5A5A5A5A5U;
|
||||
// Use typed interface
|
||||
{
|
||||
sender_->send(data, std::chrono::milliseconds{1LL});
|
||||
MsgT msg{};
|
||||
ASSERT_NE(msg, data);
|
||||
const auto id = receive(msg);
|
||||
EXPECT_EQ(data, msg);
|
||||
EXPECT_EQ(id, sender_->default_id().get());
|
||||
}
|
||||
}
|
||||
|
||||
// Ensure there's no funny stateful stuff happening
|
||||
TEST_F(DISABLED_sender_test, sequential)
|
||||
{
|
||||
for (MsgT idx = 1UL; idx < 100UL; ++idx) {
|
||||
sender_->send(idx, std::chrono::milliseconds{1LL});
|
||||
MsgT msg{};
|
||||
ASSERT_NE(msg, idx);
|
||||
const auto id = receive(msg);
|
||||
EXPECT_EQ(idx, msg);
|
||||
EXPECT_EQ(id, sender_->default_id().get());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,5 @@
|
||||
# If running in docker/ade need the following arguments:
|
||||
# --privileged --cap-add=ALL -v /lib/modules:/lib/modules
|
||||
sudo modprobe vcan
|
||||
sudo ip link add dev vcan0 type vcan
|
||||
sudo ip link set vcan0 up
|
||||
@@ -0,0 +1 @@
|
||||
sudo ip link del vcan0
|
||||
@@ -0,0 +1,49 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package ros2_socketcan_msgs
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
1.3.0 (2024-07-16)
|
||||
------------------
|
||||
* Jazzy release
|
||||
|
||||
1.2.0 (2023-03-03)
|
||||
------------------
|
||||
* Add CAN FD Support (`#28 <https://github.com/autowarefoundation/ros2_socketcan/issues/28>`_)
|
||||
* Add FD support to ROS2 interface.
|
||||
* Add FD send/receive.
|
||||
* Missed some fields in Frame msg.
|
||||
* Make standard and FD mutually exclusive.
|
||||
* Enable runtime checks for standard vs FD.
|
||||
* Try to minimize API changes.
|
||||
* Fix receive_id/fd_receive_id mix-up.
|
||||
* Make new message FD-specific.
|
||||
* Use FdFrame message.
|
||||
* Remove unused functions.
|
||||
* Always resize fd frame buffer to 64 before receive.
|
||||
* Add enable_can_fd to socket_can_bridge.launch.xml.
|
||||
---------
|
||||
* Adding ros2_socketcan_msgs (`#26 <https://github.com/autowarefoundation/ros2_socketcan/issues/26>`_)
|
||||
* Contributors: Joshua Whitley
|
||||
|
||||
* Add CAN FD Support (`#28 <https://github.com/autowarefoundation/ros2_socketcan/issues/28>`_)
|
||||
* Add FD support to ROS2 interface.
|
||||
* Add FD send/receive.
|
||||
* Missed some fields in Frame msg.
|
||||
* Make standard and FD mutually exclusive.
|
||||
* Enable runtime checks for standard vs FD.
|
||||
* Try to minimize API changes.
|
||||
* Fix receive_id/fd_receive_id mix-up.
|
||||
* Make new message FD-specific.
|
||||
* Use FdFrame message.
|
||||
* Remove unused functions.
|
||||
* Always resize fd frame buffer to 64 before receive.
|
||||
* Add enable_can_fd to socket_can_bridge.launch.xml.
|
||||
---------
|
||||
* Adding ros2_socketcan_msgs (`#26 <https://github.com/autowarefoundation/ros2_socketcan/issues/26>`_)
|
||||
* Contributors: Joshua Whitley
|
||||
|
||||
1.1.0 (2022-02-03)
|
||||
------------------
|
||||
|
||||
1.0.0 (2021-04-01)
|
||||
------------------
|
||||
@@ -0,0 +1,28 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(ros2_socketcan_msgs)
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake_auto REQUIRED)
|
||||
ament_auto_find_build_dependencies()
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/FdFrame.msg"
|
||||
DEPENDENCIES std_msgs
|
||||
ADD_LINTER_TESTS
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_auto_package()
|
||||
@@ -0,0 +1,13 @@
|
||||
Any contribution that you make to this repository will
|
||||
be under the Apache 2 License, as dictated by that
|
||||
[license](http://www.apache.org/licenses/LICENSE-2.0.html):
|
||||
|
||||
~~~
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
~~~
|
||||
@@ -0,0 +1,201 @@
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
@@ -0,0 +1,6 @@
|
||||
std_msgs/Header header
|
||||
uint32 id
|
||||
bool is_extended
|
||||
bool is_error
|
||||
uint8 len
|
||||
uint8[<=64] data
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>ros2_socketcan_msgs</name>
|
||||
<version>1.3.0</version>
|
||||
<description>Messages for SocketCAN</description>
|
||||
<maintainer email="josh@electrifiedautonomy.com">Josh Whitley</maintainer>
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake_auto</buildtool_depend>
|
||||
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Reference in New Issue
Block a user