增加电池SOC计算

soc_pkg/battery_logic.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import numpy as np
+
+# 已知总电压与电量百分比（SoC）的对应关系
+total_voltages = [25.2, 24.6, 24.0, 23.4, 22.8, 22.2, 21.6, 21.0, 20.4, 19.8, 18.0]
+socs = [100, 90, 80, 70, 60, 50, 40, 30, 20, 10, 0]
+
+def get_soc_from_total_voltage(total_voltage):
+    """
+    根据总电压计算电量百分比（SoC）
+    :param total_voltage: 总电压值
+    :return: 电量百分比（0% 到 100%）
+    """
+    soc = np.interp(total_voltage, total_voltages[::-1], socs[::-1])
+    return max(0, min(100, soc))
+
+def get_battery_color(soc):
+    """
+    根据电量百分比返回电池颜色
+    :param soc: 电量百分比
+    :return: QColor 对象表示的颜色
+    """
+    if soc > 80:
+        return (0, 255, 0)  # 绿色
+    elif soc > 50:
+        return (255, 255, 0)  # 黄色
+    elif soc > 20:
+        return (255, 165, 0)  # 橙色
+    else:
+        return (255, 0, 0)  # 红色

soc_pkg/main_gui.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import sys
+import rospy
+from std_msgs.msg import Float32  # 假设电压数据是 Float32 类型
+from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout
+from PyQt5.QtGui import QPainter, QColor, QBrush, QFont
+from PyQt5.QtCore import Qt, QTimer
+
+# 导入电池逻辑模块
+from battery_logic import get_soc_from_total_voltage, get_battery_color
+
+class BatteryWidget(QWidget):
+    def __init__(self, total_voltage=24.4):
+        super().__init__()
+        self.total_voltage = total_voltage
+        self.soc = get_soc_from_total_voltage(self.total_voltage)
+
+    def paintEvent(self, event):
+        # 自定义绘制电池图标和百分比文字
+        painter = QPainter(self)
+        painter.setRenderHint(QPainter.Antialiasing)  # 开启抗锯齿
+
+        # 绘制电池轮廓（小尺寸以适应固定窗口）
+        width, height = 80, 40  # 电池图标的宽度和高度
+        x, y = (self.width() - width) // 2, 20  # 图标垂直居中
+        painter.setPen(Qt.black)  # 设置边框颜色为黑色
+        painter.setBrush(QColor(200, 200, 200))  # 设置背景颜色为灰色
+        painter.drawRoundedRect(x, y, width, height, 3, 3)  # 绘制圆角矩形
+
+        # 绘制电池正极（小尺寸终端）
+        painter.setBrush(QColor(0, 0, 0))  # 设置正极颜色为黑色
+        painter.drawRect(x + width, y + height // 4, width // 10, height // 2)
+
+        # 根据电量百分比填充电池
+        fill_height = int(height * 0.8)  # 留出上下边距
+        fill_width = int((width - 4) * (self.soc / 100))  # 根据 SoC 缩放填充宽度
+        fill_color = get_battery_color(self.soc)  # 获取电量对应的颜色
+        painter.setBrush(QBrush(QColor(*fill_color)))
+        painter.drawRect(x + 2, y + (height - fill_height) // 2, fill_width, fill_height)
+
+        # 绘制百分比文字在电池图标下方
+        font = QFont("Arial", 12)  # 设置字体大小
+        painter.setFont(font)
+        text = f"{self.soc:.1f}%"  # 百分比格式化为一位小数
+        text_width = painter.fontMetrics().width(text)  # 获取文字宽度
+        text_height = painter.fontMetrics().height()  # 获取文字高度
+        painter.setPen(Qt.black)  # 设置文字颜色为黑色
+        painter.drawText(
+            (self.width() - text_width) // 2,  # 水平居中
+            y + height + text_height + 5,      # 放置在电池图标下方
+            text
+        )
+
+    def update_voltage(self, total_voltage):
+        # 更新电池电压并重新绘制
+        self.total_voltage = total_voltage
+        self.soc = get_soc_from_total_voltage(self.total_voltage)
+        self.update()  # 触发重新绘制
+
+class MainWindow(QWidget):
+    def __init__(self):
+        super().__init__()
+        # 初始化默认电压
+        self.battery_voltage = 24.4  # 默认初始电压
+        self.init_ui()
+        self.init_ros()
+
+    def init_ui(self):
+        self.setWindowTitle("电池状态")  # 设置窗口标题
+        self.setFixedSize(300, 120)  # 设置固定窗口大小
+
+        # 创建电池组件
+        self.battery_widget = BatteryWidget(total_voltage=self.battery_voltage)  # 初始电压
+        layout = QVBoxLayout()
+        layout.addWidget(self.battery_widget)
+        self.setLayout(layout)
+
+    def init_ros(self):
+        # 初始化 ROS 节点
+        rospy.init_node("battery_status_gui", anonymous=True)
+        rospy.Subscriber("/PowerVoltage", Float32, self.voltage_callback)
+        self.timer = QTimer(self)
+        self.timer.timeout.connect(self.ros_spin_once)
+        self.timer.start(100)  # 每 100ms 调用一次 ROS spin
+
+    def voltage_callback(self, msg):
+        # 订阅到电压数据时更新电池电压
+        self.battery_voltage = msg.data
+        self.battery_widget.update_voltage(self.battery_voltage)
+
+    def ros_spin_once(self):
+        # 手动调用 ROS spinOnce
+        rospy.sleep(0.1)
+
+
+if __name__ == "__main__":
+    app = QApplication(sys.argv)
+    window = MainWindow()
+    window.show()
+
+    # 运行 PyQt 主循环
+    try:
+        sys.exit(app.exec_())
+    except rospy.ROSInterruptException:
+        pass

soc_pkg/ros_soc.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+# *- coding: utf-8 --
+
+import sys
+import numpy as np
+import rospy
+from std_msgs.msg import Float32  # 假设电压数据是 Float32 类型
+from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout
+from PyQt5.QtGui import QPainter, QColor, QBrush, QFont
+from PyQt5.QtCore import Qt, QTimer
+
+# 已知总电压与电量百分比（SoC）的对应关系
+total_voltages = [25.2, 24.6, 24.0, 23.4, 22.8, 22.2, 21.6, 21.0, 20.4, 19.8, 18.0]
+socs = [100, 90, 80, 70, 60, 50, 40, 30, 20, 10, 0]
+
+def get_soc_from_total_voltage(total_voltage):
+    # 使用线性插值计算电量百分比（SoC）
+    soc = np.interp(total_voltage, total_voltages[::-1], socs[::-1])
+    return max(0, min(100, soc))  # 确保 SoC 在 0% 到 100% 之间
+
+class BatteryWidget(QWidget):
+    def __init__(self, total_voltage=24.4):
+        super().__init__()
+        self.total_voltage = total_voltage
+        self.soc = get_soc_from_total_voltage(self.total_voltage)
+
+    def paintEvent(self, event):
+        # 自定义绘制电池图标和百分比文字
+        painter = QPainter(self)
+        painter.setRenderHint(QPainter.Antialiasing)  # 开启抗锯齿
+
+        # 绘制电池轮廓（小尺寸以适应固定窗口）
+        width, height = 80, 40  # 电池图标的宽度和高度
+        x, y = (self.width() - width) // 2, 20  # 图标垂直居中
+        painter.setPen(Qt.black)  # 设置边框颜色为黑色
+        painter.setBrush(QColor(200, 200, 200))  # 设置背景颜色为灰色
+        painter.drawRoundedRect(x, y, width, height, 3, 3)  # 绘制圆角矩形
+
+        # 绘制电池正极（小尺寸终端）
+        painter.setBrush(QColor(0, 0, 0))  # 设置正极颜色为黑色
+        painter.drawRect(x + width, y + height // 4, width // 10, height // 2)
+
+        # 根据电量百分比填充电池
+        fill_height = int(height * 0.8)  # 留出上下边距
+        fill_width = int((width - 4) * (self.soc / 100))  # 根据 SoC 缩放填充宽度
+        fill_color = self.get_battery_color(self.soc)  # 获取电量对应的颜色
+        painter.setBrush(QBrush(fill_color))
+        painter.drawRect(x + 2, y + (height - fill_height) // 2, fill_width, fill_height)
+
+        # 绘制百分比文字在电池图标下方
+        font = QFont("Arial", 12)  # 设置字体大小
+        painter.setFont(font)
+        text = f"{self.soc:.1f}%"  # 百分比格式化为一位小数
+        text_width = painter.fontMetrics().width(text)  # 获取文字宽度
+        text_height = painter.fontMetrics().height()  # 获取文字高度
+        painter.setPen(Qt.black)  # 设置文字颜色为黑色
+        painter.drawText(
+            (self.width() - text_width) // 2,  # 水平居中
+            y + height + text_height + 5,      # 放置在电池图标下方
+            text
+        )
+
+    def get_battery_color(self, soc):
+        # 根据电量百分比设置电池颜色
+        if soc > 80:
+            return QColor(0, 255, 0)  # 绿色
+        elif soc > 50:
+            return QColor(255, 255, 0)  # 黄色
+        elif soc > 20:
+            return QColor(255, 165, 0)  # 橙色
+        else:
+            return QColor(255, 0, 0)  # 红色
+
+    def update_voltage(self, total_voltage):
+        # 更新电池电压并重新绘制
+        self.total_voltage = total_voltage
+        self.soc = get_soc_from_total_voltage(self.total_voltage)
+        self.update()  # 触发重新绘制
+
+class MainWindow(QWidget):
+    def __init__(self):
+        super().__init__()
+        # 初始化默认电压
+        self.battery_voltage = 24.4  # 默认初始电压
+        self.init_ui()
+        self.init_ros()
+
+    def init_ui(self):
+        self.setWindowTitle("电池状态")  # 设置窗口标题
+        self.setFixedSize(300, 120)  # 设置固定窗口大小
+
+        # 创建电池组件
+        self.battery_widget = BatteryWidget(total_voltage=self.battery_voltage)  # 初始电压
+        layout = QVBoxLayout()
+        layout.addWidget(self.battery_widget)
+        self.setLayout(layout)
+
+    def init_ros(self):
+        # 初始化 ROS 节点
+        rospy.init_node("battery_status_gui", anonymous=True)
+        rospy.Subscriber("/PowerVoltage", Float32, self.voltage_callback)
+        self.timer = QTimer(self)
+        self.timer.timeout.connect(self.ros_spin_once)
+        self.timer.start(100)  # 每 100ms 调用一次 ROS spin
+
+    def voltage_callback(self, msg):
+        # 订阅到电压数据时更新电池电压
+        self.battery_voltage = msg.data
+        self.battery_widget.update_voltage(self.battery_voltage)
+
+    def ros_spin_once(self):
+        # 手动调用 ROS spinOnce
+        rospy.sleep(0.1)
+
+
+if __name__ == "__main__":
+    app = QApplication(sys.argv)
+    window = MainWindow()
+    window.show()
+
+    # 运行 PyQt 主循环
+    try:
+        sys.exit(app.exec_())
+    except rospy.ROSInterruptException:
+        pass

soc_pkg/soc.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+# *- coding: utf-8 --
+
+import numpy as np
+
+# Known total voltage vs. SoC data points
+total_voltages = [25.2, 24.6, 24.0, 23.4, 22.8, 22.2, 21.6, 21.0, 20.4, 19.8, 18.0]
+socs = [100, 90, 80, 70, 60, 50, 40, 30, 20, 10, 0]
+
+def get_soc_from_total_voltage(total_voltage):
+    soc = np.interp(total_voltage, total_voltages[::-1], socs[::-1])
+    return max(0, min(100, soc))  
+
+# Example usage
+V_total = 24.8  
+soc = get_soc_from_total_voltage(V_total)
+print(f"State of Charge: {soc:.1f}%") 
+
+
+
+# # Example: Total voltage measurement
+# V_total = 23.5  # Measured total voltage
+
+# # Number of cells in series
+# N = 6
+
+# # Calculate average voltage per cell
+# V_cell_avg = V_total / N
+# print(f"Average voltage per cell: {V_cell_avg}V")

soc_pkg/soc_qt.py

@@ -0,0 +1,102 @@
+import sys
+import numpy as np
+from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout
+from PyQt5.QtGui import QPainter, QColor, QBrush, QFont
+from PyQt5.QtCore import Qt
+
+# 已知总电压与电量百分比（SoC）的对应关系
+total_voltages = [25.2, 24.6, 24.0, 23.4, 22.8, 22.2, 21.6, 21.0, 20.4, 19.8, 18.0]
+socs = [100, 90, 80, 70, 60, 50, 40, 30, 20, 10, 0]
+
+def get_soc_from_total_voltage(total_voltage):
+    # 使用线性插值计算电量百分比（SoC）
+    soc = np.interp(total_voltage, total_voltages[::-1], socs[::-1])
+    return max(0, min(100, soc))  # 确保 SoC 在 0% 到 100% 之间
+
+class BatteryWidget(QWidget):
+    def __init__(self, total_voltage=24.4):
+        super().__init__()
+        self.total_voltage = total_voltage
+        self.soc = get_soc_from_total_voltage(self.total_voltage)
+
+    def paintEvent(self, event):
+        # 自定义绘制电池图标和百分比文字
+        painter = QPainter(self)
+        painter.setRenderHint(QPainter.Antialiasing)  # 开启抗锯齿
+
+        # 绘制电池轮廓（小尺寸以适应固定窗口）
+        width, height = 80, 40  # 电池图标的宽度和高度
+        x, y = (self.width() - width) // 2, 20  # 图标垂直居中
+        painter.setPen(Qt.black)  # 设置边框颜色为黑色
+        painter.setBrush(QColor(200, 200, 200))  # 设置背景颜色为灰色
+        painter.drawRoundedRect(x, y, width, height, 3, 3)  # 绘制圆角矩形
+
+        # 绘制电池正极（小尺寸终端）
+        painter.setBrush(QColor(0, 0, 0))  # 设置正极颜色为黑色
+        painter.drawRect(x + width, y + height // 4, width // 10, height // 2)
+
+        # 根据电量百分比填充电池
+        fill_height = int(height * 0.8)  # 留出上下边距
+        fill_width = int((width - 4) * (self.soc / 100))  # 根据 SoC 缩放填充宽度
+        fill_color = self.get_battery_color(self.soc)  # 获取电量对应的颜色
+        painter.setBrush(QBrush(fill_color))
+        painter.drawRect(x + 2, y + (height - fill_height) // 2, fill_width, fill_height)
+
+        # 绘制百分比文字在电池图标下方
+        font = QFont("Arial", 12)  # 设置字体大小
+        painter.setFont(font)
+        text = f"{self.soc:.1f}%"  # 百分比格式化为一位小数
+        text_width = painter.fontMetrics().width(text)  # 获取文字宽度
+        text_height = painter.fontMetrics().height()  # 获取文字高度
+        painter.setPen(Qt.black)  # 设置文字颜色为黑色
+        painter.drawText(
+            (self.width() - text_width) // 2,  # 水平居中
+            y + height + text_height + 5,      # 放置在电池图标下方
+            text
+        )
+
+    def get_battery_color(self, soc):
+        # 根据电量百分比设置电池颜色
+        if soc > 80:
+            return QColor(0, 255, 0)  # 绿色
+        elif soc > 50:
+            return QColor(255, 255, 0)  # 黄色
+        elif soc > 20:
+            return QColor(255, 165, 0)  # 橙色
+        else:
+            return QColor(255, 0, 0)  # 红色
+
+    def update_voltage(self, total_voltage):
+        # 更新电池电压并重新绘制
+        self.total_voltage = total_voltage
+        self.soc = get_soc_from_total_voltage(self.total_voltage)
+        self.update()  # 触发重新绘制
+
+class MainWindow(QWidget):
+    def __init__(self):
+        super().__init__()
+        self.init_ui()
+
+    def init_ui(self):
+        self.setWindowTitle("电池状态")  # 设置窗口标题
+        self.setFixedSize(300, 120)  # 设置固定窗口大小
+
+        # 创建电池组件
+        self.battery_widget = BatteryWidget(total_voltage=24.4)  # 初始电压
+        layout = QVBoxLayout()
+        layout.addWidget(self.battery_widget)
+        self.setLayout(layout)
+
+    def update_battery_voltage(self, total_voltage):
+        """
+        公共接口：更新电池电压
+        :param total_voltage: 新的总电压值
+        """
+        self.battery_widget.update_voltage(total_voltage)
+
+
+if __name__ == "__main__":
+    app = QApplication(sys.argv)
+    window = MainWindow()
+    window.show()
+    sys.exit(app.exec_())

soc_pkg/代码分离/battery_gui.py

@@ -0,0 +1,89 @@
+from PyQt5.QtWidgets import QWidget, QVBoxLayout
+from PyQt5.QtGui import QPainter, QColor, QBrush, QFont
+from PyQt5.QtCore import Qt
+
+
+class BatteryWidget(QWidget):
+    def __init__(self, total_voltage=24.4):
+        super().__init__()
+        self.total_voltage = total_voltage
+        self.soc = self.get_soc_from_total_voltage(self.total_voltage)
+
+    def paintEvent(self, event):
+        painter = QPainter(self)
+        painter.setRenderHint(QPainter.Antialiasing)  # 开启抗锯齿
+
+        # 绘制电池轮廓
+        width, height = 80, 40  # 电池图标的宽度和高度
+        x, y = (self.width() - width) // 2, 20  # 图标垂直居中
+        painter.setPen(Qt.black)  # 设置边框颜色为黑色
+        painter.setBrush(QColor(200, 200, 200))  # 设置背景颜色为灰色
+        painter.drawRoundedRect(x, y, width, height, 3, 3)  # 绘制圆角矩形
+
+        # 绘制电池正极
+        painter.setBrush(QColor(0, 0, 0))  # 设置正极颜色为黑色
+        painter.drawRect(x + width, y + height // 4, width // 10, height // 2)
+
+        # 根据电量百分比填充电池
+        fill_height = int(height * 0.8)  # 留出上下边距
+        fill_width = int((width - 4) * (self.soc / 100))  # 根据 SoC 缩放填充宽度
+        fill_color = self.get_battery_color(self.soc)  # 获取电量对应的颜色
+        painter.setBrush(QBrush(QColor(*fill_color)))
+        painter.drawRect(x + 2, y + (height - fill_height) // 2, fill_width, fill_height)
+
+        # 绘制百分比文字
+        font = QFont("Arial", 14)  # 设置字体大小
+        painter.setFont(font)
+        text = f"{self.soc:.1f}%"  # 百分比格式化为一位小数
+        text_width = painter.fontMetrics().width(text)  # 获取文字宽度
+        text_height = painter.fontMetrics().height()  # 获取文字高度
+        painter.setPen(Qt.black)  # 设置文字颜色为黑色
+        painter.drawText(
+            (self.width() - text_width) // 2,  # 水平居中
+            y + height + text_height + 5,      # 放置在电池图标下方
+            text
+        )
+
+    def get_soc_from_total_voltage(self, total_voltage):
+        """
+        调用外部逻辑计算电量百分比
+        """
+        from battery_logic import get_soc_from_total_voltage
+        return get_soc_from_total_voltage(total_voltage)
+
+    def get_battery_color(self, soc):
+        """
+        调用外部逻辑获取电池颜色
+        """
+        from battery_logic import get_battery_color
+        return get_battery_color(soc)
+
+    def update_voltage(self, total_voltage):
+        # 更新电池电压并重新绘制
+        self.total_voltage = total_voltage
+        self.soc = self.get_soc_from_total_voltage(self.total_voltage)
+        self.update()  # 触发重新绘制
+
+
+class MainWindow(QWidget):
+    def __init__(self):
+        super().__init__()
+        self.battery_voltage = 24.4  # 默认初始电压
+        self.init_ui()
+
+    def init_ui(self):
+        self.setWindowTitle("电池状态")  # 设置窗口标题
+        self.setFixedSize(300, 120)  # 设置固定窗口大小
+
+        # 创建电池组件
+        self.battery_widget = BatteryWidget(total_voltage=self.battery_voltage)
+        layout = QVBoxLayout()
+        layout.addWidget(self.battery_widget)
+        self.setLayout(layout)
+
+    def update_battery_voltage(self, total_voltage):
+        """
+        公共接口：更新电池电压
+        :param total_voltage: 新的总电压值
+        """
+        self.battery_widget.update_voltage(total_voltage)

soc_pkg/代码分离/battery_logic.py

@@ -0,0 +1,29 @@
+import numpy as np
+
+# 已知总电压与电量百分比（SoC）的对应关系
+total_voltages = [12.6, 12.3, 12.0, 11.7, 11.4, 11.1, 10.8, 10.5, 10.2, 9.9, 9.0]
+socs = [100, 90, 80, 70, 60, 50, 40, 30, 20, 10, 0]
+
+def get_soc_from_total_voltage(total_voltage):
+    """
+    根据总电压计算电量百分比（SoC）
+    :param total_voltage: 总电压值
+    :return: 电量百分比（0% 到 100%）
+    """
+    soc = np.interp(total_voltage, total_voltages[::-1], socs[::-1])
+    return max(0, min(100, soc))  # 确保 SoC 在 0% 到 100% 之间
+
+def get_battery_color(soc):
+    """
+    根据电量百分比返回电池颜色
+    :param soc: 电量百分比
+    :return: QColor 对象表示的颜色
+    """
+    if soc > 80:
+        return (0, 255, 0)  # 绿色
+    elif soc > 50:
+        return (255, 255, 0)  # 黄色
+    elif soc > 20:
+        return (255, 165, 0)  # 橙色
+    else:
+        return (255, 0, 0)  # 红色

soc_pkg/代码分离/main_ros.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import sys
+import signal
+import rospy
+from std_msgs.msg import Float32
+from PyQt5.QtWidgets import QApplication
+from battery_gui import MainWindow
+
+class RosHandler:
+    def __init__(self, voltage_callback):
+        """
+        初始化 ROS 节点和订阅器
+        :param voltage_callback: 回调函数，用于处理接收到的电压数据
+        """
+        rospy.init_node("battery_status_gui")
+        rospy.loginfo("Battery Status GUI initialized.")
+        self.voltage_subscriber = rospy.Subscriber("/robot/PowerValtage", Float32, voltage_callback)
+
+    def spin(self):
+        """
+        手动调用 ROS spinOnce
+        """
+        rospy.spin()
+
+
+if __name__ == "__main__":
+    # 创建 PyQt 应用程序
+    app = QApplication(sys.argv)
+    window = MainWindow()
+    window.show()
+
+    # 定义电压回调函数
+    def voltage_callback(msg):
+        """
+        处理从 ROS 接收到的电压数据
+        :param msg: ROS 消息对象
+        """
+        voltage = msg.data
+        window.update_battery_voltage(voltage)
+
+    # 初始化 ROS 处理器
+    ros_handler = RosHandler(voltage_callback)
+
+    # 定义信号处理函数，确保按 Ctrl+C 可以正常退出
+    def signal_handler(sig, frame):
+        print("\nStopping program...")
+        rospy.signal_shutdown("User requested shutdown")  # 关闭 ROS 节点
+        app.quit()  # 退出 PyQt 主循环
+        sys.exit(0)  # 退出程序
+
+    # 注册信号处理函数
+    signal.signal(signal.SIGINT, signal_handler)
+
+    try:
+        # 启动 PyQt 主循环
+        sys.exit(app.exec_())
+    except rospy.ROSInterruptException:
+        pass