Initial commit

2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
@@ -0,0 +1,189 @@
+#!/usr/bin/env python3
+import rospy
+import serial
+import binascii
+import time
+from std_msgs.msg import String
+from sensor_msgs.msg import BatteryState
+from rc_receiver.msg import rc
+from geometry_msgs.msg import Twist
+
+class WirelessChargingNode:
+    def __init__(self):
+        rospy.init_node('wireless_charging_yichuang', anonymous=False)
+        
+        # 获取参数
+        self.port = rospy.get_param('~port', '/dev/ttyS1')
+        self.baudrate = rospy.get_param('~baudrate', 115200)
+        self.polling_rate = rospy.get_param('~polling_rate', 10)
+        self.timeout = rospy.get_param('~timeout', 0.5)
+        
+        # 初始化串口
+        self.ser = None
+        self.init_serial()
+        
+        # 初始化订阅者
+        self.attach_result_sub = rospy.Subscriber(
+            '/attach/result', 
+            String, 
+            self.attach_result_callback
+        )
+        self.battery_state_sub = rospy.Subscriber(
+            '/sensor/battery_state', 
+            BatteryState, 
+            self.battery_state_callback
+        )
+        self.rc_contrl_sub = rospy.Subscriber(
+            '/remote_ctrl', 
+            rc, 
+            self.rc_ctrl_callback
+        )
+        self.auto_contrl_sub = rospy.Subscriber(
+            '/cmd_vel', 
+            Twist, 
+            self.auto_ctrl_callback
+        )
+        
+        # 充电状态标志（仅用于日志显示）
+        self.is_charging = False
+        self.charging_status = False
+        self.full_charge_count = 0
+        # 用于记录上次处理的消息ID，确保每次新消息都能被处理
+        self.last_attach_msg_id = None
+        self.last_battery_msg_id = None
+
+        self.car_ctrl_mode = 0
+        
+
+        self.rate = rospy.Rate(self.polling_rate)
+        rospy.loginfo("Wireless charging node initialized")
+
+    def init_serial(self):
+        """初始化串口连接"""
+        try:
+            self.ser = serial.Serial(
+                port=self.port,
+                baudrate=self.baudrate,
+                timeout=self.timeout,
+                parity='N',
+                stopbits=1,
+                bytesize=8
+            )
+            if self.ser.is_open:
+                rospy.loginfo(f"Serial port {self.port} opened successfully")
+        except serial.SerialException as e:
+            rospy.logerr(f"Failed to open serial port: {e}")
+            self.ser = None
+
+    def send_start_charging(self):
+        """发送开始充电指令"""
+        if not self.ser or not self.ser.is_open:
+            rospy.logwarn("Serial port not open, cannot send start charging command")
+            self.init_serial()  # 尝试重新初始化串口
+            return
+            
+        try:
+            # 开始充电指令
+            start_cmd = b'\x04\x06\x30\x01\x00\xB1\x17\x2B'   
+            self.ser.write(start_cmd)
+            rospy.loginfo("Sent start charging command")
+            self.is_charging = True
+        except Exception as e:
+            rospy.logerr(f"Failed to send start charging command: {e}")
+
+    def send_stop_charging(self):
+        """发送停止充电指令"""
+        if not self.ser or not self.ser.is_open:
+            rospy.logwarn("Serial port not open, cannot send stop charging command")
+            self.init_serial()  # 尝试重新初始化串口
+            return
+            
+        try:
+            # 停止充电指令
+            stop_cmd = b'\x04\x06\x30\x01\x00\xB2\x57\x2A'  
+            self.ser.write(stop_cmd)
+            rospy.loginfo("Sent stop charging command")
+            self.is_charging = False
+        except Exception as e:
+            rospy.logerr(f"Failed to send stop charging command: {e}")
+
+    def attach_result_callback(self, msg):
+        """处理/attach/result话题回调 - 每次收到"Finish"都触发"""
+        rospy.logdebug(f"Received attach result: {msg.data}")
+        
+        # 生成消息唯一标识（使用时间戳和消息内容的组合）
+        current_msg_id = f"{rospy.get_time()}-{msg.data}"
+        
+        # 检查是否为新消息且内容为"Finish"
+        if msg.data == "Finish" and current_msg_id != self.last_attach_msg_id:
+            rospy.loginfo("Received 'Finish' signal, sending start charging command")
+            self.send_start_charging()
+            time.sleep(0.1)
+            self.send_start_charging()
+            time.sleep(0.1)
+            self.send_start_charging()
+            # 更新最后处理的消息ID
+            self.last_attach_msg_id = current_msg_id
+
+
+    def rc_ctrl_callback(self, msg):
+        """/remote_ctrl接收到yundong指令发送停充指令"""
+        self.car_ctrl_mode = msg.control_mode
+        if (msg.linear_x <= -0.01 or msg.linear_x >= 0.01) and self.charging_status == True and self.car_ctrl_mode == 1:
+                rospy.loginfo("Received 'back' command, sending stop charging command")
+                self.send_stop_charging()
+
+    def auto_ctrl_callback(self, msg):
+        """/remote_ctrl接收到yundong指令发送停充指令"""
+        if (msg.linear.x <= -0.01 or msg.linear.x >= 0.01) and self.charging_status == True and self.car_ctrl_mode == 0:
+                rospy.loginfo("Received 'back' command, sending stop charging command")
+                self.send_stop_charging()
+
+
+    def battery_state_callback(self, msg):
+        """处理/sensor/battery_state话题回调 - 满电一段时间发送停充指令"""
+        rospy.logdebug(f"Battery percentage: {msg.percentage}")
+        
+        # 生成消息唯一标识
+        current_msg_id = f"{rospy.get_time()}-{msg.percentage}"
+        
+        # 检查是否为新消息且电量达到100%
+        if msg.percentage == 100.0 and current_msg_id != self.last_battery_msg_id:
+            self.full_charge_count += 1
+            if self.full_charge_count >= 25:
+            	rospy.loginfo(f"Battery fully charged ({msg.percentage}%), sending stop charging command")
+            	self.send_stop_charging()
+            	self.full_charge_count = 0
+            # 更新最后处理的消息ID
+            self.last_battery_msg_id = current_msg_id
+        if msg.percentage < 100.0:
+            self.full_charge_count = 0
+        if msg.power_supply_status == 1:
+            self.charging_status = True
+        else:
+            self.charging_status = False
+
+    def run(self):
+        while not rospy.is_shutdown():
+            try:
+                if self.ser and self.ser.is_open:
+                    self.ser.flushInput()
+            except Exception as e:
+                rospy.logerr(f"Error in main loop: {e}")
+            
+            self.rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        node = WirelessChargingNode()
+        node.run()
+    except rospy.ROSInterruptException:
+        pass
+    finally:
+        try:
+            if 'node' in locals() and hasattr(node, 'ser') and node.ser and node.ser.is_open:
+                node.ser.close()
+                rospy.loginfo("WirelessCharging serial port closed")
+        except Exception as e:
+            rospy.logerr(f"Error closing serial port: {e}")
+