上传文件至「catkin_truck 」
This commit is contained in:
173
catkin_truck /wirecontrol_sub.py
Normal file
173
catkin_truck /wirecontrol_sub.py
Normal file
@@ -0,0 +1,173 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user