173 lines
6.8 KiB
Python
173 lines
6.8 KiB
Python
#!/usr/bin/python3
|
||
# -*- coding: utf-8 -*-
|
||
|
||
# 作者： 郭帆、张贺达
|
||
|
||
# 功能描述：
|
||
# 通过can通道接受线控反馈数据
|
||
# 根据can协议（dbc文件）进行数据解析
|
||
# 解析后的数据按照wirecontrol_feed_back_msg数据格式进行发布
|
||
|
||
#最后修改日期:2023-08-23
|
||
import time
|
||
import can
|
||
import cantools
|
||
import rospy
|
||
import rosparam
|
||
import signal
|
||
import os
|
||
import yaml
|
||
|
||
from msg_gen.msg import ToControl
|
||
from msg_gen.msg import Power
|
||
from msg_gen.msg import State
|
||
from msg_gen.msg import Break
|
||
from msg_gen.msg import Engine
|
||
from msg_gen.msg import Driving
|
||
from msg_gen.msg import MileMeter
|
||
|
||
|
||
Running_Flag = True
|
||
|
||
def signal_handler(signal, frame):
|
||
global Running_Flag
|
||
Running_Flag = False
|
||
|
||
def main():
|
||
wirecontrol_can_channel = None
|
||
global Running_Flag
|
||
|
||
signal.signal(signal.SIGINT, signal_handler)
|
||
rospy.init_node("wirecontrol_sub")
|
||
# 按照 GPS 接收脚本的方式读取 AllPathList.yaml
|
||
script_dir = os.path.dirname(os.path.abspath(__file__))
|
||
project_root = os.path.abspath(os.path.join(script_dir, '../../..'))
|
||
all_path_list_yaml = os.path.join(project_root, 'ConfigFile', 'yaml', 'AllPathList.yaml')
|
||
|
||
with open(all_path_list_yaml, 'r', encoding='utf-8') as f:
|
||
config = yaml.safe_load(f)
|
||
print("找到AllPathList.yaml文件")
|
||
|
||
feedback_dbc_file = config.get('NewZt_zhd_1005_dbc', '')
|
||
wirecontrol_yaml_path = config.get('wirecontrol_yaml', '')
|
||
|
||
if feedback_dbc_file and os.path.exists(feedback_dbc_file):
|
||
print(f"成功读取DBC文件绝对路径: {feedback_dbc_file}")
|
||
else:
|
||
print(f"配置中未找到有效路径或文件不存在: {feedback_dbc_file}")
|
||
return
|
||
|
||
if wirecontrol_yaml_path and os.path.exists(wirecontrol_yaml_path):
|
||
print(f"成功读取YAML文件绝对路径: {wirecontrol_yaml_path}")
|
||
try:
|
||
with open(wirecontrol_yaml_path, 'r', encoding='utf-8') as f:
|
||
wc = yaml.safe_load(f)
|
||
if wc is not None and 'wirecontrol_can_channel' in wc:
|
||
wirecontrol_can_channel = wc['wirecontrol_can_channel']
|
||
print(f"从配置文件读取的CAN通道: {wirecontrol_can_channel}")
|
||
else:
|
||
print("配置文件中未找到wirecontrol_can_channel参数")
|
||
return
|
||
except Exception as e:
|
||
print(f"读取配置文件时发生错误: {str(e)}")
|
||
return
|
||
else:
|
||
print(f"配置中未找到有效路径或文件不存在: {wirecontrol_yaml_path}")
|
||
return
|
||
|
||
pub_tocontrol = rospy.Publisher('/ToControl_msg', ToControl, queue_size=10)
|
||
pub_v2n_1 = rospy.Publisher('/Power_msg', Power, queue_size=10)
|
||
pub_v2n_2 = rospy.Publisher('/State_msg', State, queue_size=10)
|
||
pub_v2n_3 = rospy.Publisher('/Break_msg', Break, queue_size=10)
|
||
pub_v2n_4 = rospy.Publisher('/Engine_msg', Engine, queue_size=10)
|
||
pub_v2n_5 = rospy.Publisher('/Driving_msg', Driving, queue_size=10)
|
||
pub_v2n_6 = rospy.Publisher('/MileMeter_msg', MileMeter, queue_size=10)
|
||
|
||
ToControlMsg = ToControl()
|
||
power0 = Power()
|
||
state0 = State()
|
||
Break0 = Break()
|
||
Engine0 = Engine()
|
||
Drive0 = Driving()
|
||
MileMeter0 = MileMeter()
|
||
|
||
# 验证DBC文件是否存在（上方已验证路径有效，这里直接加载）
|
||
db = cantools.db.load_file(feedback_dbc_file)
|
||
|
||
# 与 GPS 接收脚本一致，使用 bustype 参数
|
||
try:
|
||
bus = can.interface.Bus(channel=wirecontrol_can_channel, bustype='socketcan')
|
||
connect_flag = True
|
||
except:
|
||
connect_flag = False
|
||
|
||
# 打印DBC文件中已知的消息ID，用于调试
|
||
known_ids = list(db._frame_id_to_message.keys())
|
||
print(f"DBC文件中已知的消息ID: {[hex(id) for id in known_ids]}")
|
||
|
||
# 我们关心的特定ID
|
||
target_ids = [0x0C01A5A7, 0x0C02A5A7]
|
||
print(f"我们关心的目标ID: {[hex(id) for id in target_ids]}")
|
||
|
||
try:
|
||
while Running_Flag and connect_flag and not rospy.is_shutdown():
|
||
message = bus.recv()
|
||
|
||
# 跳过无效消息
|
||
if message is None:
|
||
continue
|
||
|
||
if message.arbitration_id not in target_ids:
|
||
# 静默跳过非目标ID
|
||
# print("esc")
|
||
continue
|
||
try:
|
||
if message.arbitration_id == 0x0C01A5A7:
|
||
decoded_message = db.decode_message(message.arbitration_id, message.data)
|
||
# print(decoded_message)
|
||
ToControlMsg.wheel_angle = decoded_message["steer_angle_fb"]
|
||
# print("getin")
|
||
|
||
ToControlMsg.LinkState = True
|
||
# print(f"解码 0x0C01A5A7: 方向盘角度 = {ToControlMsg.wheel_angle}")
|
||
|
||
elif message.arbitration_id == 0x0C02A5A7:
|
||
decoded_message = db.decode_message(message.arbitration_id, message.data)
|
||
ToControlMsg.Car_Control_Mode = decoded_message["Veh_running_state"]
|
||
ToControlMsg.EPB_Parking_State = decoded_message["Parking_Status"]
|
||
if (decoded_message["Gear_signal"] == 2):
|
||
decoded_message["Gear_signal"] = -1 #todo
|
||
ToControlMsg.current_Gear = decoded_message["Gear_signal"]
|
||
ToControlMsg.Gas_pedal_percentage = decoded_message["ACC_pedal"]
|
||
ToControlMsg.Break_pedal_percentage = decoded_message["Break_padel"]
|
||
ToControlMsg.Speed_byGear = decoded_message["V_speed"]
|
||
# print(f"解码 0x0C02A5A7: 速度 = {ToControlMsg.Speed_byGear}, 档位 = {ToControlMsg.current_Gear}")
|
||
|
||
# 发布消息
|
||
pub_tocontrol.publish(ToControlMsg)
|
||
pub_v2n_1.publish(power0)
|
||
pub_v2n_2.publish(state0)
|
||
pub_v2n_3.publish(Break0)
|
||
pub_v2n_4.publish(Engine0)
|
||
pub_v2n_5.publish(Drive0)
|
||
pub_v2n_6.publish(MileMeter0)
|
||
|
||
except KeyError as e:
|
||
print(f"KeyError: 消息ID 0x{message.arbitration_id:X} 在DBC中未找到对应的定义")
|
||
print(f"错误详情: {e}")
|
||
continue
|
||
except Exception as e:
|
||
print(f"解码消息时出错 (ID: 0x{message.arbitration_id:X}): {e}")
|
||
continue
|
||
|
||
except KeyboardInterrupt:
|
||
print("程序被用户中断")
|
||
except Exception as e:
|
||
print(f"程序运行出错: {e}")
|
||
finally:
|
||
if 'bus' in locals():
|
||
bus.shutdown()
|
||
print("程序退出")
|
||
|
||
if __name__ == '__main__':
|
||
main() |