#!/usr/bin/env python3 import rospy import serial import binascii import time from std_msgs.msg import String from sensor_msgs.msg import BatteryState class BMSDriverNode: def __init__(self): rospy.init_node('bms_driver_mingnuo', anonymous=False) self.port = rospy.get_param('~port', '/dev/ttyUSB0') self.baudrate = rospy.get_param('~baudrate', 9600) self.timeout = 0.5 self.battery_cmd = b'\x01\x03\x00\x00\x00\x1E\xC5\xC2' # 电量查询指令 self.alarm_cmd = b'\x01\x01\x00\x00\x00\x34\x3D\xDD' # 报警查询指令 self.polling_rate = 1.0 try: self.ser = serial.Serial( port=self.port, baudrate=self.baudrate, timeout=self.timeout, parity='N', stopbits=1, bytesize=8 ) rospy.loginfo(f"Successfully connected to BMS: {self.port} @ {self.baudrate}bps") except serial.SerialException as e: rospy.logerr(f"Failed to connect to BMS: {e}") raise # 初始化全局消息对象 self.global_msg = BatteryState() self.battery_pub = rospy.Publisher('/sensor/battery_state', BatteryState, queue_size=10) self.rate = rospy.Rate(self.polling_rate) def update_battery_data(self, hex_data): """更新全局消息中的电池数据""" try: # if len(hex_data) < 65: # rospy.logwarn(f"Battery response frame too short: {hex_data.hex()}") # return False # if hex_data[0] != 0x01 or hex_data[1] != 0x03 or hex_data[2] != 0x3C: # rospy.logwarn(f"Battery response frame format error: {hex_data.hex()}") # return False # 更新电池数据 self.global_msg.header.stamp = rospy.Time.now() self.global_msg.voltage = ((hex_data[3] << 8) | hex_data[4]) / 100.0 self.global_msg.percentage = ((hex_data[7] << 8) | hex_data[8]) unsigned_value = (hex_data[13] << 8) | hex_data[14] # 转换为16位有符号整数 if unsigned_value & 0x8000: signed_value = unsigned_value - 0x10000 else: signed_value = unsigned_value # 正数直接使用 self.global_msg.current = signed_value / 100.0 self.global_msg.power_supply_status = 1 if self.global_msg.current > 0.5 else 0 self.global_msg.present = True self.global_msg.temperature = (hex_data[61] << 8) | hex_data[62] return True except Exception as e: rospy.loginfo(f"Error updating battery data: {e}") return False def update_alarm_data(self, hex_data): """更新全局消息中的报警数据""" try: # if len(hex_data) < 12: # rospy.logwarn(f"Alarm response frame too short: {hex_data.hex()}") # return False # if hex_data[0] != 0x01 or hex_data[1] != 0x01 or hex_data[2] != 0x07: # rospy.logwarn(f"Alarm response frame format error: {hex_data.hex()}") # return False # 更新报警数据 self.global_msg.header.stamp = rospy.Time.now() alarm_code1 = hex_data[3] self.global_msg.power_supply_health = alarm_code1 if ((hex_data[7] == 0) and (hex_data[8] == 0) and (hex_data[9] == 0)): alarm_code_over_discharge_voltage = 0 else: alarm_code_over_discharge_voltage = 1 if ((hex_data[6] == 0) and (hex_data[5] == 0) and (hex_data[4] < 16)): alarm_code_over_charge_voltage = 0 else: alarm_code_over_charge_voltage = 1 self.global_msg.power_supply_technology = (hex_data[4] & 0xF) | (alarm_code_over_charge_voltage << 4) | (alarm_code_over_discharge_voltage << 5) return True except Exception as e: rospy.loginfo(f"Error updating alarm data: {e}") return False def receive_and_update(self): """接收数据并更新全局消息""" data = self.ser.read_all() if not data: return False, "No data" # 判断是否为电量响应 if data[0] == 0x01 and data[1] == 0x03 and data[2] == 0x3C: success = self.update_battery_data(data) return success, "Battery data updated" # 判断是否为报警响应 elif data[0] == 0x01 and data[1] == 0x01 and data[2] == 0x07: success = self.update_alarm_data(data) return success, "Alarm data updated" # 不符合任何类型 else: rospy.loginfo(f"Unknown response frame: {data.hex()}") return False, "Unknown frame" def run(self): while not rospy.is_shutdown(): try: # 1. 发送电量指令并更新 self.ser.flushInput() self.ser.write(self.battery_cmd) rospy.logdebug(f"Sent battery command: {binascii.hexlify(self.battery_cmd).decode().upper()}") time.sleep(0.19) battery_success, battery_msg = self.receive_and_update() if battery_success: self.battery_pub.publish(self.global_msg) # rospy.loginfo(f"Battery state updated: Voltage={self.global_msg.voltage:.2f}V, SOC={self.global_msg.percentage:.1f}%, Current={self.global_msg.current:.2f}A") else: rospy.loginfo(f"Battery update failed: {battery_msg}") time.sleep(0.2) # 2. 发送报警指令并更新 self.ser.flushInput() self.ser.write(self.alarm_cmd) rospy.logdebug(f"Sent alarm command: {binascii.hexlify(self.alarm_cmd).decode().upper()}") time.sleep(0.08) alarm_success, alarm_msg = self.receive_and_update() if alarm_success: self.battery_pub.publish(self.global_msg) # rospy.loginfo(f"Alarm state updated: Health={self.global_msg.power_supply_health}") else: rospy.loginfo(f"Alarm update failed: {alarm_msg}") except Exception as e: rospy.logerr(f"Communication error: {e}") self.rate.sleep() if __name__ == '__main__': try: node = BMSDriverNode() node.run() except rospy.ROSInterruptException: pass finally: if 'node' in locals() and hasattr(node, 'ser') and node.ser.is_open: node.ser.close() rospy.loginfo("BMS serial port closed")