feat(agv_pro_autocharge): Added automatic recharge function code

agv_pro_autocharge/README.md

@@ -0,0 +1,134 @@
+# AGV AutoCharge ROS2 Package
+
+这是一个为ROS2 Humble设计的AGV自动充电系统软件包。
+
+## 功能特性
+
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 键盘交互启动导航功能
+- 导航成功后启动串口控制
+- 支持充电状态检测和控制
+
+## 安装依赖
+
+确保您的系统已安装以下依赖：
+
+```bash
+# ROS2 Humble基础包
+sudo apt install ros-humble-rclpy
+sudo apt install ros-humble-geometry-msgs
+sudo apt install ros-humble-std-msgs
+sudo apt install ros-humble-nav-msgs
+sudo apt install ros-humble-visualization-msgs
+sudo apt install ros-humble-nav2-simple-commander
+
+# Python依赖
+pip3 install pyserial
+```
+
+## 编译安装
+
+```bash
+# 进入ROS2工作空间
+cd /path/to/your/ros2_ws/src
+
+# 复制软件包到工作空间
+cp -r agv_autocharge_ros2 .
+
+# 编译软件包
+cd ..
+colcon build --packages-select agv_autocharge_ros2
+
+# 加载环境变量
+source install/setup.bash
+```
+
+## 使用方法
+
+### 启动节点
+
+```bash
+# 启动自动充电控制器节点
+ros2 run agv_autocharge_ros2 combined_auto_recharger
+```
+
+### 节点功能
+
+- 监听 `/charger_position_update` 话题，接收充电桩位置更新
+- 发布 `/goal_marker` 话题，在RViz中显示充电桩位置标记
+- 发布 `/cmd_vel` 话题，控制机器人运动
+- 按键 `q` 启动导航到充电桩
+- 导航成功后自动启动串口控制
+
+### 配置文件
+
+充电桩位置配置文件位于：
+```
+config/charger_position.json
+```
+
+文件格式：
+```json
+{
+  "p_x": 1.329015495451769,
+  "p_y": 0.31961151635354823,
+  "orien_z": 0.4981823289472456,
+  "orien_w": 0.8670722963655905
+}
+```
+
+### 话题接口
+
+#### 订阅话题
+- `/charger_position_update` (geometry_msgs/PoseStamped): 充电桩位置更新
+
+#### 发布话题
+- `/goal_marker` (visualization_msgs/MarkerArray): 充电桩位置可视化标记
+- `/cmd_vel` (geometry_msgs/Twist): 机器人运动控制
+- `/chassis_security` (std_msgs/Int8): 底盘安全控制
+
+### 串口配置
+
+默认串口配置：
+- 端口: `/dev/ttyCH341USB0`
+- 波特率: 9600
+- 超时: 1秒
+
+可以根据需要修改代码中的串口参数。
+
+## 操作说明
+
+1. 启动节点后，系统会自动加载充电桩位置配置
+2. 系统会定期发布充电桩标记到RViz进行可视化
+3. 按下键盘上的 `q` 键启动导航到充电桩
+4. 导航成功后，系统会自动启动串口控制功能
+5. 串口控制会根据接收到的数据控制机器人运动
+6. 按 `Ctrl+C` 退出程序
+
+## 故障排除
+
+### 常见问题
+
+1. **串口无法打开**
+   - 检查串口设备是否连接
+   - 确认串口权限设置
+   - 验证串口设备名称
+
+2. **导航失败**
+   - 确认Nav2导航系统正常运行
+   - 检查充电桩位置配置是否正确
+   - 验证地图和定位系统状态
+
+3. **RViz中看不到标记**
+   - 确认RViz已订阅 `/goal_marker` 话题
+   - 检查MarkerArray显示设置
+   - 验证坐标系设置是否为'map'
+
+## 许可证
+
+MIT License
+
+## 维护者
+
+请联系维护者获取技术支持。

agv_pro_autocharge/agv_pro_autocharge/__init__.py

@@ -0,0 +1,10 @@
+"""
+AGV AutoCharge ROS2 Package
+
+This package provides automatic charging functionality for AGV robots using ROS2 Humble.
+It includes position management, visualization, navigation, and serial control features.
+"""
+
+__version__ = '1.0.0'
+__author__ = 'Your Name'
+__email__ = 'your-email@example.com'

agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py

@@ -0,0 +1,612 @@
+#!/usr/bin/env python3 
+# coding=utf-8
+
+"""
+合并的自动充电控制器 - 结合位置管理、可视化和导航功能
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 按键'q'启动导航功能
+- 导航成功后启动串口控制
+"""
+
+# 引用ros库
+import rclpy
+from rclpy.node import Node
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+from rclpy.duration import Duration
+
+# 用到的变量定义
+from std_msgs.msg import Bool 
+from std_msgs.msg import Int8 
+from std_msgs.msg import UInt8
+from std_msgs.msg import Float32
+
+# 用于记录充电桩位置、发布导航点
+from geometry_msgs.msg import PoseStamped, Twist
+
+# rviz可视化相关
+from visualization_msgs.msg import Marker
+from visualization_msgs.msg import MarkerArray
+
+# 里程计话题相关
+from nav_msgs.msg import Odometry
+
+# 键盘控制相关
+import sys
+import select
+import termios
+import tty
+
+# 延迟相关
+import time
+import threading
+
+# 读写充电桩位置文件
+import json
+import math
+import yaml
+import os
+
+# 导入串口解析模块
+from .serial_can_parser import SerialCANParser
+
+# 存放充电桩位置的文件位置 - 参考原始auto_recharger.py的路径设置方式
+def find_config_files():
+    """查找配置文件路径"""
+    # 首先尝试几个可能的位置
+    possible_paths = [
+        # 开发环境路径
+        '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config',
+        # 你的工作空间路径
+        '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config',
+        # 当前包的相对路径
+        os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'config'),
+        # 安装路径
+        '/home/elephant/agv_pro_ros2/install/agv_pro_autocharge/share/agv_pro_autocharge/config'
+    ]
+    
+    for config_dir in possible_paths:
+        yaml_path = os.path.join(config_dir, 'nav_goal_params.yaml')
+        json_path = os.path.join(config_dir, 'charger_position.json')
+        
+        print(f"Checking config directory: {config_dir}")
+        if os.path.exists(yaml_path) and os.path.exists(json_path):
+            print(f"Found config files in: {config_dir}")
+            return yaml_path, json_path
+    
+    # 如果都找不到，直接报错
+    print("ERROR: Could not find config files in any of the following locations:")
+    for path in possible_paths:
+        print(f"  - {path}")
+    print("Please ensure the config files exist in one of these directories.")
+    
+    # 返回第一个路径作为默认值，但文件可能不存在
+    return os.path.join(possible_paths[0], 'nav_goal_params.yaml'), os.path.join(possible_paths[0], 'charger_position.json')
+
+# 获取配置文件路径
+yaml_file, json_file = find_config_files()
+
+# print_and_fixRetract相关，用于打印带颜色的信息
+RESET = '\033[0m'
+RED   = '\033[1;31m'
+GREEN = '\033[1;32m'
+YELLOW= '\033[1;33m'
+BLUE  = '\033[1;34m'
+PURPLE= '\033[1;35m'
+CYAN  = '\033[1;36m'
+
+# 圆周率
+PI = 3.1415926535897
+
+if os.name == 'nt':
+    import msvcrt
+else:
+    import termios
+    import tty
+	
+settings = None
+if os.name != 'nt' and sys.stdin.isatty():
+    settings = list(termios.tcgetattr(sys.stdin))
+
+def get_key(settings):
+    if os.name == 'nt':
+        return msvcrt.getch().decode('utf-8')
+    else:
+        if sys.stdin.isatty():
+            tty.setraw(sys.stdin.fileno())
+        rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
+        if rlist:
+            key = sys.stdin.read(1)
+        else:
+            key = ''
+        if sys.stdin.isatty() and settings:
+            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+        return key
+
+def print_and_fixRetract(str):
+    global settings
+    '''键盘控制会导致回调函数内使用print()出现自动缩进的问题，此函数可以解决该现象'''
+    if sys.stdin.isatty() and settings:
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    print(str)
+
+class CombinedAutoRecharger(Node):
+    def __init__(self):
+        
+        # 创建节点
+        super().__init__("combined_auto_recharger")
+
+        print_and_fixRetract('Combined Auto Recharger Node Started!')
+
+        # 导航状态标记
+        self.navigation_active = False
+        
+        # 串口控制相关
+        self.parser = None
+        self.serial_control_active = False
+        self.navigation_requested = False  # 添加导航请求标志
+        
+        # 创建导航器
+        self.navigator = BasicNavigator()
+
+        # 加载充电桩位置信息
+        self.load_charger_position()
+        # 加载导航参数
+        self.load_nav_goal_params()
+
+        # 创建发布者
+        self.robot_security_off_pub = self.create_publisher(Int8, '/chassis_security', 10) 
+        self.Charger_marker_pub = self.create_publisher(MarkerArray, '/goal_marker', 10)
+        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+
+        # 创建订阅者 - 只订阅充电桩位置更新
+        self.Charger_Position_Update_sub = self.create_subscription(
+            PoseStamped, "/charger_position_update", 
+            self.Position_Update_callback, 10)
+
+        # 创建定时器，定期发布充电桩标记（每2秒发布一次）
+        self.marker_timer = self.create_timer(2.0, self.timer_callback)
+        
+        # 创建导航检查定时器（每0.5秒检查一次导航请求）
+        self.navigation_timer = self.create_timer(0.5, self.check_navigation_request)
+
+        # 发布初始充电桩位置标记
+        self.update_charger_visualization()
+
+        print_and_fixRetract('Combined auto recharger node initialized successfully!')
+        print_and_fixRetract(f'{GREEN}Press "q" to start navigation, Ctrl+C to exit{RESET}')
+
+    def load_nav_goal_params(self):
+        """加载导航目标参数（前方距离和角度）"""
+        print_and_fixRetract(f"Attempting to load nav goal params from: {yaml_file}")
+        print_and_fixRetract(f"File exists? {os.path.exists(yaml_file)}")
+        
+        try:
+            with open(yaml_file, 'r', encoding='utf-8') as f:
+                params = yaml.safe_load(f)
+            
+            print_and_fixRetract(f"Raw params from file: {params}")
+            
+            self.forward_distance = float(params.get('forward_distance', 1.0))
+            self.yaw_offset_deg = float(params.get('yaw_offset_deg', 0.0))
+            
+            print_and_fixRetract(f"Successfully loaded nav goal params: forward_distance={self.forward_distance}, yaw_offset_deg={self.yaw_offset_deg}")
+            
+        except FileNotFoundError:
+            print_and_fixRetract(f"{RED}Nav goal params file not found: {yaml_file}{RESET}")
+            print_and_fixRetract(f"{RED}Please create the configuration file with the required parameters{RESET}")
+            # 使用默认值
+            self.forward_distance = 1.0
+            self.yaw_offset_deg = 0.0
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Failed to load nav goal params: {e}{RESET}")
+            self.forward_distance = 1.0
+            self.yaw_offset_deg = 0.0
+
+    def timer_callback(self):
+        '''定时器回调函数，定期发布充电桩标记'''
+        # 始终发布当前JSON文件中的位置信息
+        if hasattr(self, 'json_data') and self.json_data:
+            self.Pub_Charger_marker(
+                self.json_data['p_x'],
+                self.json_data['p_y'], 
+                self.json_data['orien_z'],
+                self.json_data['orien_w']
+            )
+
+    def check_navigation_request(self):
+        '''检查是否有导航请求'''
+        if self.navigation_requested and not self.navigation_active:
+            self.navigation_requested = False
+            self.navigation_active = True
+            print_and_fixRetract(f"{BLUE}Processing navigation request...{RESET}")
+            
+            # 在ROS2线程中执行导航
+            result = self.execute_navigation_internal()
+            
+            if result:
+                print_and_fixRetract(f"{GREEN}Navigation successful! Starting serial control...{RESET}")
+                # 在ROS2线程中启动串口控制
+                self.start_serial_control_async()
+            else:
+                print_and_fixRetract(f"{RED}Navigation failed{RESET}")
+                self.navigation_active = False
+
+    def request_navigation(self):
+        '''请求开始导航'''
+        if not self.navigation_active:
+            self.navigation_requested = True
+            print_and_fixRetract(f"{BLUE}Navigation request queued...{RESET}")
+        else:
+            print_and_fixRetract(f"{YELLOW}Navigation already in progress{RESET}")
+
+    def load_charger_position(self):
+        '''加载充电桩位置信息'''
+        try:
+            with open(json_file, 'r', encoding='utf-8') as fp:
+                self.json_data = json.load(fp)
+            print_and_fixRetract(f"Loaded charger position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}")
+        except FileNotFoundError:
+            print_and_fixRetract(f"{RED}Charger position file {json_file} not found{RESET}")
+            print_and_fixRetract(f"{RED}Please create the configuration file with default charger position{RESET}")
+            # 使用默认位置
+            self.json_data = {
+                'p_x': 0.0,
+                'p_y': 0.0, 
+                'orien_z': 0.0,
+                'orien_w': 1.0
+            }
+        except Exception as e:
+            print_and_fixRetract(f"Error loading charger position: {e}")
+            self.json_data = {
+                'p_x': 0.0,
+                'p_y': 0.0,
+                'orien_z': 0.0, 
+                'orien_w': 1.0
+            }
+
+    def save_charger_position(self):
+        '''保存充电桩位置信息到JSON文件'''
+        try:
+            with open(json_file, 'w', encoding='utf-8') as fp:
+                json.dump(self.json_data, fp, ensure_ascii=False, indent=2)
+            print_and_fixRetract(f"{GREEN}Charger position saved to {json_file}{RESET}")
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Error saving charger position: {e}{RESET}")
+
+    def Pub_Charger_Position(self):
+        '''更新充电桩位置信息并保存到JSON文件'''
+        # 发布充电桩位置的可视化
+        self.Pub_Charger_marker(
+            self.json_data['p_x'], 
+            self.json_data['p_y'], 
+            self.json_data['orien_z'], 
+            self.json_data['orien_w'])
+        
+        # 保存当前充电桩位置到JSON文件
+        position_data = {
+            'p_x': self.json_data['p_x'],
+            'p_y': self.json_data['p_y'],
+            'orien_z': self.json_data['orien_z'],
+            'orien_w': self.json_data['orien_w']
+        }
+        
+        self.json_data = position_data
+        self.save_charger_position()
+        print_and_fixRetract(f"Position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}")
+
+    def Pub_Charger_marker(self, p_x, p_y, o_z, o_w):
+        '''发布目标点可视化话题'''
+        
+        markerArray = MarkerArray()
+        
+        # 获取当前时间戳
+        current_time = self.get_clock().now().to_msg()
+
+        marker_shape = Marker()  # 创建marker对象
+        marker_shape.id = 0  # 必须赋值id
+        marker_shape.header.frame_id = 'map'  # 以哪一个TF坐标为原点
+        marker_shape.header.stamp = current_time  # 添加时间戳
+        marker_shape.type = Marker.ARROW  # TEXT_VIEW_FACING #一直面向屏幕的字符格式
+        marker_shape.action = Marker.ADD  # 添加marker
+        marker_shape.scale.x = 0.5  # marker大小
+        marker_shape.scale.y = 0.05  # marker大小
+        marker_shape.scale.z = 0.05  # marker大小，对于字符只有z起作用
+        marker_shape.pose.position.x = p_x  # 字符位置
+        marker_shape.pose.position.y = p_y  # 字符位置
+        marker_shape.pose.position.z = 0.1  # msg.position.z #字符位置
+        marker_shape.pose.orientation.z = o_z  # 字符位置
+        marker_shape.pose.orientation.w = o_w  # 字符位置
+        marker_shape.color.r = 1.0  # 字符颜色R(红色)通道
+        marker_shape.color.g = 0.0  # 字符颜色G(绿色)通道
+        marker_shape.color.b = 0.0  # 字符颜色B(蓝色)通道
+        marker_shape.color.a = 1.0  # 字符透明度
+        markerArray.markers.append(marker_shape)  # 添加元素进数组
+        
+        marker_string = Marker()  # 创建marker对象
+        marker_string.id = 1  # 必须赋值id
+        marker_string.header.frame_id = 'map'  # 以哪一个TF坐标为原点
+        marker_string.header.stamp = current_time  # 添加时间戳
+        marker_string.type = Marker.TEXT_VIEW_FACING  # 一直面向屏幕的字符格式
+        marker_string.action = Marker.ADD  # 添加marker
+        marker_string.scale.x = 0.5  # marker大小
+        marker_string.scale.y = 0.5  # marker大小
+        marker_string.scale.z = 0.5  # marker大小，对于字符只有z起作用
+        marker_string.color.a = 1.0  # 字符透明度
+        marker_string.color.r = 1.0  # 字符颜色R(红色)通道
+        marker_string.color.g = 0.0  # 字符颜色G(绿色)通道
+        marker_string.color.b = 0.0  # 字符颜色B(蓝色)通道
+        marker_string.pose.position.x = p_x  # 字符位置
+        marker_string.pose.position.y = p_y  # 字符位置
+        marker_string.pose.position.z = 0.1  # msg.position.z #字符位置
+        marker_string.pose.orientation.z = o_z  # 字符位置
+        marker_string.pose.orientation.w = o_w  # 字符位置
+        marker_string.text = 'Charger'  # 字符内容
+        markerArray.markers.append(marker_string)  # 添加元素进数组
+        self.Charger_marker_pub.publish(markerArray)  # 发布markerArray，rviz订阅并进行可视化
+
+    def Position_Update_callback(self, topic):
+        '''更新json文件中的充电桩位置'''
+        position_dic = {'p_x': 0, 'p_y': 0, 'orien_z': 0, 'orien_w': 0}
+        position_dic['p_x'] = topic.pose.position.x
+        position_dic['p_y'] = topic.pose.position.y
+        position_dic['orien_z'] = topic.pose.orientation.z
+        position_dic['orien_w'] = topic.pose.orientation.w
+
+        # 保存最新的充电桩位置到json文件
+        self.json_data = position_dic
+        self.save_charger_position()
+        print_and_fixRetract("New charging pile position saved.")
+        
+        # 位置更新后立即发布一次新的标记，然后继续定时发布
+        self.update_charger_visualization()
+        print_and_fixRetract(f"{GREEN}Charger position updated and will be published continuously{RESET}")
+
+    def update_charger_visualization(self):
+        '''更新充电桩可视化标记'''
+        if hasattr(self, 'json_data'):
+            self.Pub_Charger_marker(
+                self.json_data['p_x'],
+                self.json_data['p_y'], 
+                self.json_data['orien_z'],
+                self.json_data['orien_w']
+            )
+
+    def execute_navigation(self):
+        """外部调用的导航接口"""
+        self.request_navigation()
+        return True  # 返回True表示请求已提交
+
+    def execute_navigation_internal(self):
+        """内部执行导航任务"""
+        print_and_fixRetract(f"{BLUE}Starting navigation...{RESET}")
+        # 从JSON文件读取充电桩位置
+        try:
+            with open(json_file, 'r', encoding='utf-8') as f:
+                charger_data = json.load(f)
+            px = charger_data['p_x']
+            py = charger_data['p_y']
+            # 充电桩姿态四元数转欧拉角
+            orien_z = charger_data['orien_z']
+            orien_w = charger_data['orien_w']
+            yaw = 2 * math.atan2(orien_z, orien_w)  # 只考虑z/w分量
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Failed to read charger position file: {e}{RESET}")
+            self.navigation_active = False
+            return False
+
+        # 计算目标点位置
+        x_offset = self.forward_distance * math.cos(yaw)
+        y_offset = self.forward_distance * math.sin(yaw)
+        goal_x = px + x_offset
+        goal_y = py + y_offset
+
+        # 计算目标点姿态（z轴顺时针yaw_offset_deg）
+        goal_yaw = yaw - math.radians(self.yaw_offset_deg)
+        goal_qz = math.sin(goal_yaw / 2)
+        goal_qw = math.cos(goal_yaw / 2)
+
+        print_and_fixRetract(f"Nav goal: x={goal_x:.3f}, y={goal_y:.3f}, yaw={math.degrees(goal_yaw):.1f}°")
+        goal_pose = self.create_pose(goal_x, goal_y, goal_qz, goal_qw)
+
+        # 执行导航
+        print_and_fixRetract(f"{BLUE}Executing navigation...{RESET}")
+        result1 = self.nav_through_pose([goal_pose], verbose=False)
+        print_and_fixRetract(f"Navigation result: {result1}")
+        self.navigation_active = False
+        return result1
+
+    def create_pose(self, x, y, z, w):
+        """创建单个目标点的位姿信息"""
+        pose = PoseStamped()
+        pose.header.frame_id = 'map'
+        pose.header.stamp = self.get_clock().now().to_msg()
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+        pose.pose.orientation.z = z
+        pose.pose.orientation.w = w
+        return pose
+
+    def nav_through_pose(self, goal_poses, verbose: bool = False) -> bool:
+        """执行多点导航任务"""
+        # 开始执行多点导航任务
+        self.navigator.goThroughPoses(goal_poses)
+
+        # 等待导航任务完成，监控导航状态
+        while not self.navigator.isTaskComplete():
+            feedback = self.navigator.getFeedback()
+            if feedback and verbose:
+                remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
+                print_and_fixRetract(f"预计到达时间: {remaining:.0f} 秒")
+
+        # 根据导航结果返回相应状态
+        result = self.navigator.getResult()
+        if result == TaskResult.SUCCEEDED:
+            print_and_fixRetract(f'{GREEN}Navigation successful!{RESET}')
+            return True
+        elif result == TaskResult.CANCELED:
+            print_and_fixRetract(f'{YELLOW}Navigation canceled!{RESET}')
+        elif result == TaskResult.FAILED:
+            print_and_fixRetract(f'{RED}Navigation failed!{RESET}')
+        else:
+            print_and_fixRetract(f'{RED}Invalid navigation result!{RESET}')
+        return False
+
+    def start_serial_control_async(self):
+        """异步启动串口控制功能"""
+        def serial_control_thread():
+            self.start_serial_control()
+        
+        # 在新线程中启动串口控制，避免阻塞ROS2主线程
+        serial_thread = threading.Thread(target=serial_control_thread, daemon=True)
+        serial_thread.start()
+
+    def start_serial_control(self):
+        """启动串口控制功能"""
+        print_and_fixRetract(f"{BLUE}Starting serial control...{RESET}")
+        try:
+            self.parser = SerialCANParser('/dev/agvpro_ec130', 9600, 1)
+            self.parser.open_serial()  # 打开usb串口
+            
+            # 发送AT 命令从透传模式进入AT指令模式
+            self.parser.send_at_commands(["AT+CG", "AT+AT"])
+            
+            self.serial_control_active = True
+            print_and_fixRetract(f'{GREEN}Serial control started successfully{RESET}')
+            
+            while self.serial_control_active:
+                # 开始读取数据
+                x_speed, z_speed, which_mode, infrared_bits = self.parser.read_serial_data()
+                
+                if infrared_bits[7] == 0:  # 无障碍物
+                    if which_mode == 0x01:  # 正常模式
+                        # 直接发布ROS2 Twist消息
+                        twist_msg = Twist()
+                        twist_msg.linear.x = float(x_speed)
+                        twist_msg.linear.y = 0.0
+                        twist_msg.angular.z = float(z_speed)
+                        self.cmd_vel_pub.publish(twist_msg)
+                        print_and_fixRetract(f'Normal mode - Speed: x={x_speed}, z={z_speed}')
+                        
+                    elif which_mode == 0xBB:  # 测压区
+                        # 停止运动
+                        stop_msg = Twist()
+                        self.cmd_vel_pub.publish(stop_msg)
+                        print_and_fixRetract(f'{YELLOW}Pressure zone - Stop movement{RESET}')
+                        
+                    elif which_mode == 0xAA:  # 充电区
+                        # 停止运动
+                        stop_msg = Twist()
+                        self.cmd_vel_pub.publish(stop_msg)
+                        print_and_fixRetract(f'{GREEN}Charging zone - Stop movement{RESET}')
+                        break  # 充电完成后退出
+                        
+                    elif which_mode == 0xCF:  # 急停模式
+                        emergency_stop_msg = Twist()  # 所有速度都为0
+                        self.cmd_vel_pub.publish(emergency_stop_msg)
+                        print_and_fixRetract(f'{RED}Emergency stop mode - Immediate stop{RESET}')
+                        break
+                        
+                else:  # 检测到障碍物
+                    obstacle_stop_msg = Twist()  # 所有速度都为0
+                    self.cmd_vel_pub.publish(obstacle_stop_msg)
+                    print_and_fixRetract(f'{RED}Obstacle detected - Stop movement{RESET}')
+                    break
+                    
+        except KeyboardInterrupt:
+            print_and_fixRetract("Serial control interrupted by user")
+            # 发布停止消息
+            emergency_stop = Twist()
+            self.cmd_vel_pub.publish(emergency_stop)
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Serial control error: {e}{RESET}")
+            # 发布停止消息
+            emergency_stop = Twist()
+            self.cmd_vel_pub.publish(emergency_stop)
+        finally:
+            if self.parser:
+                self.parser.close_serial()
+            print_and_fixRetract("Serial control stopped")
+
+    def stop_serial_control(self):
+        """停止串口控制功能"""
+        self.serial_control_active = False
+
+    def get_charger_info(self):
+        '''获取充电桩位置信息'''
+        if hasattr(self, 'json_data'):
+            return {
+                'position': {
+                    'x': self.json_data['p_x'],
+                    'y': self.json_data['p_y']
+                },
+                'orientation': {
+                    'z': self.json_data['orien_z'],
+                    'w': self.json_data['orien_w']
+                }
+            }
+        return None
+
+
+def main(args=None):
+    '''主函数'''
+    rclpy.init(args=args)
+    
+    combined_recharger = None
+    try:
+        combined_recharger = CombinedAutoRecharger()
+        
+        print_and_fixRetract("Combined auto recharger node is running...")
+        print_and_fixRetract("Node functions:")
+        print_and_fixRetract("- Listening for charger position updates on /charger_position_update")
+        print_and_fixRetract("- Publishing visualization markers on /goal_marker every 2 seconds")
+        print_and_fixRetract("- Press 'q' to start navigation to charger position")
+        print_and_fixRetract("- Navigation success will trigger serial control")
+        
+        # 启动ROS2事件循环线程
+        def ros2_spin():
+            try:
+                rclpy.spin(combined_recharger)
+            except Exception as spin_error:
+                print_and_fixRetract(f"ROS2 spin error: {spin_error}")
+
+        ros2_thread = threading.Thread(target=ros2_spin, daemon=True)
+        ros2_thread.start()
+        
+        # 键盘监听循环
+        print_and_fixRetract(f"{GREEN}Press 'q' to start navigation, Ctrl+C to exit{RESET}")
+        print_and_fixRetract("Waiting for keyboard input...")
+        
+        while True:
+            try:
+                key = get_key(settings)
+                if key:
+                    print_and_fixRetract(f"Key pressed: {repr(key)}")  # 调试信息
+                    if key.lower() == 'q':
+                        print_and_fixRetract(f"{BLUE}Navigation command received!{RESET}")
+                        
+                        # 请求导航任务（异步执行）
+                        combined_recharger.execute_navigation()
+                    
+                    elif key == '\x03':  # Ctrl+C
+                        break
+                
+                time.sleep(0.1)  # 避免过度占用CPU
+            except KeyboardInterrupt:
+                break
+            except Exception as e:
+                print_and_fixRetract(f"Keyboard input error: {e}")
+                time.sleep(0.5)
+            
+    except KeyboardInterrupt:
+        print_and_fixRetract("\nShutting down combined auto recharger node...")
+    finally:
+        if combined_recharger:
+            combined_recharger.stop_serial_control()
+            combined_recharger.destroy_node()
+        rclpy.shutdown()
+        print_and_fixRetract("Program exited safely")
+
+if __name__ == '__main__':
+    main()
+

agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py

@@ -0,0 +1,173 @@
+#!/usr/bin/env python3                                                                                                           
+#coding=UTF-8
+import serial
+import time
+
+class SerialCANParser:
+    def __init__(self, serial_port='/dev/ttyCH341USB0', baudrate=9600, timeout=1):
+        self.serial_port = serial_port  # 串口名称
+        self.baudrate = baudrate  # 波特率
+        self.timeout = timeout  # 超时设置
+        self.ser = None  # 串口对象
+        self.buffer = bytearray()  # 存储当前读取的字节
+        self.max_retries = 3  # 最大重试次数
+        # 存储实时数据
+        self.x_speed = 0.0
+        self.z_speed = 0.0
+        self.infrared_bits = []
+        
+    def open_serial(self):
+        """打开串口"""
+        try:
+            self.ser = serial.Serial(self.serial_port, self.baudrate, timeout=self.timeout)
+            print(f"串口 {self.serial_port} 已打开，波特率：{self.baudrate}")
+        except Exception as e:
+            print(f"打开串口失败: {e}")
+            
+    def close_serial(self):
+        """关闭串口"""
+        if self.ser and self.ser.is_open:
+            self.ser.close()
+            print("串口已关闭。")
+        else:
+            print("串口未打开或已关闭。")
+            
+    def can_id_check(self, date):
+        high_byte, low_byte = date[0:2]
+        # 高字节左移 3 位
+        can_id = (high_byte << 3)
+        # 低字节右移 5 位
+        can_id |= (low_byte >> 5)
+        return can_id
+        
+    def parse_can_data(self, data):
+        """解析8字节CAN数据帧"""
+        if len(data) != 8:
+            print("数据帧长度不正确")
+            return None
+            
+        # 解析 X、Y 和 Z 速度
+        x_speed_raw = ((data[0] << 8) | data[1])  # X速度的原始数据
+        z_speed_raw = ((data[4] << 8) | data[5])  # Z速度的原始数据
+        
+        # 将原始数据转换为浮动数值，并考虑正负
+        if x_speed_raw & 0x8000:  # 如果最高位为1，表示负数
+            x_speed_raw = -((65536 - x_speed_raw) & 0xFFFF)  # 补码转换为负数
+        if z_speed_raw & 0x8000:  # 如果最高位为1，表示负数
+            z_speed_raw = -((65536 - z_speed_raw) & 0xFFFF)  # 补码转换为负数
+            
+        # 转换单位为 m/s 和 rad/s
+        self.x_speed = x_speed_raw / 1000.0  # X速度单位为 m/s
+        self.y_speed = 0  # Y速度为0
+        self.z_speed = z_speed_raw / 1000.0  # Z速度单位为 rad/s
+        self.which_mode = data[2]
+        self.infrared = data[6]  # 红外数据
+        self.raw_current = data[7]  # 电流数据
+        
+        if self.raw_current > 32767:  # 无符号数大于 32767 表示负值（因为最大值是 65535）
+            # 转换为负数
+            self.actual_current = -(65536 - self.raw_current) * 30.0
+        else:
+            # 正数直接转换
+            self.actual_current = self.raw_current * 30.0
+            
+        # 处理红外数据
+        self.infrared_bits = [(self.infrared >> (7 - i)) & 0x01 for i in range(8)]
+        
+        # 打印或处理数据
+        print(f"X Speed: {self.x_speed:.3f}, Y Speed: {self.y_speed}, Z Speed: {self.z_speed:.3f}, "
+              f"Actual Current: {self.actual_current:.3f} mA, Infrared: {self.infrared}")
+              
+        # 打印或处理红外位信息
+        print(f"L_A: {self.infrared_bits[2]}, L_B: {self.infrared_bits[3]}, R_B: {self.infrared_bits[4]}, "
+              f"R_A: {self.infrared_bits[5]}, infrared_flag : {self.infrared_bits[6]}, "
+              f"Charging flag: {self.infrared_bits[7]}")
+              
+    def read_serial_data(self):
+        """读取串口数据并解析"""
+        while True:  # 修改为简单的无限循环，由上层控制退出
+            if self.ser.in_waiting > 0:
+                byte = self.ser.read(1)   # 读取一个字节
+                if len(self.buffer) < 2:
+                    self.buffer.extend(byte)  
+                    if len(self.buffer) == 2:
+                        # 如果帧头为 0x41 0x54，表示为AT帧头，则开始接收数据
+                        if self.buffer[0] != 0x41 or self.buffer[1] != 0x54:
+                            # 如果不是有效的帧头，则清空缓冲区并跳到下次循环
+                            self.buffer.clear()
+                            continue  # 继续等待下一个字节                 
+                else:
+                    self.buffer.extend(byte)
+                    # print("缓冲区内容:", ' '.join(f'{b:02x}' for b in self.buffer)) # debug
+                    # 如果缓冲区字节长度大于等于 17 字节（数据帧长度）
+                    if len(self.buffer) >= 17:
+                        # print("Received Frame (Hex):", ' '.join(f'{byte:02x}' for byte in self.buffer)) # debug
+                        # 解析帧头、CAN帧ID、格式、类型和数据
+                        # at_frame_header = self.buffer[0:2]  # AT帧头
+                        can_frame_id = self.can_id_check(self.buffer[2:4])  # CAN标准帧ID
+                        # can_frame_format = self.buffer[4]  # CAN帧格式（0,标准帧;1,扩展帧）
+                        # can_frame_type = self.buffer[5]  # CAN帧类型（0,数据帧;1,远程帧）
+                        data_length = self.buffer[6]  # 数据长度
+                        data = self.buffer[7:15]  # 数据帧
+                        # print(f"帧ID: 0x{can_frame_id:X}")    # debug
+                        if can_frame_id == 0x182 and data_length == 0x08: # 根据can帧id进行判断
+                            # 如果帧ID为0x182,校验通过,进行数据赋值
+                            self.parse_can_data(data)
+                            # 清空缓冲区，准备下一帧数据
+                            self.buffer.clear()
+                            return self.x_speed, self.z_speed, self.which_mode, self.infrared_bits   # 返回解析后的数据
+                        else:
+                            # 清空缓冲区，准备下一帧数据
+                            self.buffer.clear()
+                            
+    def read_serial_response(self):
+        """读取串口响应数据，直到接收到 '\r\n' 或超时"""
+        response = bytearray()  # 使用 bytearray 来存储原始字节流
+        while True:
+            if self.ser.in_waiting > 0:
+                byte = self.ser.read(1)
+                response += byte
+            # 检查是否已接收到完整响应
+            if b'\r\n' in response:
+                break
+            # 超时机制，防止死循环
+            if len(response) > 100:
+                break
+        return bytes(response)  # 返回原始字节流（bytes）
+        
+    def send_at_commands(self, commands):
+        """发送 AT 命令并等待响应"""
+        for command in commands:
+            retries = 0
+            while retries < self.max_retries:
+                self.ser.write(command.encode() + b'\r\n')
+                print(f"发送命令: {command}")
+                # 等待响应并读取数据
+                response = self.read_serial_response()
+                # 检查响应是否包含 "OK"
+                if b"OK" in response:
+                    print(f"收到响应: {response}")
+                    break  # 如果收到 OK，退出重试循环
+                else:
+                    retries += 1
+                    print(f"未收到预期的响应，收到: {response}")
+            if retries == self.max_retries:
+                print(f"重试 {self.max_retries} 次后仍未收到有效响应，请检查设备。")
+                break
+                
+    def start(self):
+        """开始读取和处理数据"""
+        self.open_serial()
+        try:
+            # 发送AT 命令从透传模式进入AT指令模式
+            self.send_at_commands(["AT+CG", "AT+AT"])
+            # 开始读取数据
+            self.read_serial_data()
+        except KeyboardInterrupt:
+            print("手动中止程序。")
+        finally:
+            self.close_serial()
+
+if __name__ == '__main__':
+    parser = SerialCANParser('/dev/ttyCH341USB0', 9600, 1)
+    parser.start()

agv_pro_autocharge/config/charger_position.json

@@ -0,0 +1,6 @@
+{
+  "p_x": -0.04373347759246826,
+  "p_y": -4.024151802062988,
+  "orien_z": 0.09378381732290733,
+  "orien_w": 0.9955925851513477
+}

agv_pro_autocharge/config/nav_goal_params.yaml

@@ -0,0 +1,3 @@
+# 导航参数配置
+forward_distance: 1  # 距离充电桩前方1米
+yaw_offset_deg: 10.0   # 顺时针旋转10度

agv_pro_autocharge/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agv_pro_autocharge</name>
+  <version>1.0.0</version>
+  <description>AGV automatic charging system for ROS2 Humble</description>
+  <maintainer email="your-email@example.com">Your Name</maintainer>
+  <license>MIT</license>
+
+  <!-- Build dependencies -->
+  <buildtool_depend>ament_python</buildtool_depend>
+  
+  <!-- Runtime dependencies -->
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>nav2_simple_commander</depend>
+  
+  <!-- Test dependencies -->
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

agv_pro_autocharge/resource/agv_pro_autocharge

@@ -0,0 +1 @@
+agv_pro_autocharge

agv_pro_autocharge/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/agv_pro_autocharge
+[install]
+install_scripts=$base/lib/agv_pro_autocharge

agv_pro_autocharge/setup.py

@@ -0,0 +1,30 @@
+from setuptools import setup, find_packages
+import os
+from glob import glob
+
+package_name = 'agv_pro_autocharge'
+
+setup(
+    name=package_name,
+    version='1.0.0',
+    packages=find_packages(),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        # Include config files
+        (os.path.join('share', package_name, 'config'), glob('config/*')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Your Name',
+    maintainer_email='your-email@example.com',
+    description='AGV automatic charging system for ROS2 Humble',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'combined_auto_recharger = agv_pro_autocharge.combined_auto_recharger:main',
+        ],
+    },
+)