m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat(agv_pro_autocharge): Added automatic recharge function code

Browse Source
This commit is contained in:
X-lanni
2025-09-19 13:45:59 +08:00
parent 79fa72a8b0
commit c6dbf7084e
10 changed files with 1003 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_pro_autocharge/README.md
Executable
+134
View File
@@ -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
Executable
+10
View File
@@ -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
Executable
+612
View File
@@ -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
Executable
+173
View File
@@ -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
Executable
+6
View File
@@ -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
Executable
+3
View File
@@ -0,0 +1,3 @@
# 导航参数配置
forward_distance: 1 # 距离充电桩前方1米
yaw_offset_deg: 10.0 # 顺时针旋转10度
agv_pro_autocharge/package.xml
Executable
+30
View File
@@ -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
Executable
+1
View File
@@ -0,0 +1 @@
agv_pro_autocharge
agv_pro_autocharge/setup.cfg
Executable
+4
View File
@@ -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
Executable
+30
View File
@@ -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',
],
},
)