190 lines
7.1 KiB
Python
190 lines
7.1 KiB
Python
#!/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}")
|
|
|