Start of refactor - remove upstream code and just contain deltas
This commit is contained in:
Matt Spencer
+134
@@ -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
|
||||
|
||||
## 维护者
|
||||
|
||||
请联系维护者获取技术支持。
|
||||
@@ -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'
|
||||
+612
@@ -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()
|
||||
|
||||
+173
@@ -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()
|
||||
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"p_x": -0.04373347759246826,
|
||||
"p_y": -4.024151802062988,
|
||||
"orien_z": 0.09378381732290733,
|
||||
"orien_w": 0.9955925851513477
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
# 导航参数配置
|
||||
forward_distance: 1 # 距离充电桩前方1米
|
||||
yaw_offset_deg: 10.0 # 顺时针旋转10度
|
||||
+30
@@ -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.8</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>
|
||||
@@ -0,0 +1 @@
|
||||
agv_pro_autocharge
|
||||
+4
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/agv_pro_autocharge
|
||||
[install]
|
||||
install_scripts=$base/lib/agv_pro_autocharge
|
||||
+30
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,64 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(agv_pro_base)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(serial_driver REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(agv_pro_msgs REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
add_executable(agv_pro_node
|
||||
src/agv_pro_node.cpp
|
||||
src/agv_pro_ros.cpp
|
||||
)
|
||||
|
||||
target_include_directories(agv_pro_node PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
||||
target_compile_features(agv_pro_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
|
||||
ament_target_dependencies(agv_pro_node
|
||||
rclcpp
|
||||
tf2
|
||||
tf2_ros
|
||||
tf2_geometry_msgs
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
serial_driver
|
||||
nav_msgs
|
||||
agv_pro_msgs
|
||||
)
|
||||
|
||||
install(TARGETS agv_pro_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,284 @@
|
||||
#ifndef AGV_PRO_DRIVER_H
|
||||
#define AGV_PRO_DRIVER_H
|
||||
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <boost/asio.hpp>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <std_msgs/msg/float32.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#include <agv_pro_msgs/srv/set_digital_output.hpp>
|
||||
#include <agv_pro_msgs/srv/get_digital_input.hpp>
|
||||
#include <agv_pro_msgs/srv/set_led_color.hpp>
|
||||
#include <agv_pro_msgs/srv/set_led_mode.hpp>
|
||||
|
||||
#define SEND_DATA_SIZE 14 // Total bytes in a command frame to ESP32(version>=V1.0.8)
|
||||
#define RECEIVE_FRAME_SIZE 31 // Total bytes in a frame from ESP32(version>=V1.0.8)
|
||||
#define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3) // Payload length (excluding header)
|
||||
|
||||
#define POWER_ON 0x10
|
||||
#define GET_POWER_STATE 0x12
|
||||
#define SET_AUTO_REPORT_STATE 0x23
|
||||
#define SET_LED_COLOR 0x34
|
||||
#define SET_LED_MODE 0x3A
|
||||
#define SET_OUTPUT_IO 0x40
|
||||
#define GET_INPUT_IO 0x41
|
||||
|
||||
extern std::array<double, 36> odom_pose_covariance;
|
||||
extern std::array<double, 36> odom_twist_covariance;
|
||||
|
||||
class AGV_PRO : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor
|
||||
*/
|
||||
AGV_PRO(std::string node_name);
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
~AGV_PRO();
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Main control loop for the AGV.
|
||||
*/
|
||||
void Control();
|
||||
|
||||
/**
|
||||
* @brief Print a vector of bytes in hexadecimal format to the ROS logger.
|
||||
*
|
||||
* @param[in] label A label to prepend to the printed data.
|
||||
* @param[in] data The byte vector to print.
|
||||
* @param[in] override_size Optional size to display instead of the full data length.
|
||||
*/
|
||||
void print_hex(const std::string& label,
|
||||
const std::vector<uint8_t>& data,
|
||||
std::optional<size_t> override_size = std::nullopt);
|
||||
|
||||
/**
|
||||
* @brief Send a serial frame to the AGV and optionally print it in hex.
|
||||
*
|
||||
* @param[in] frame The byte vector representing the serial frame to send.
|
||||
* @param[in] debug If true, prints the transmitted frame using print_hex().
|
||||
*/
|
||||
void send_serial_frame(const std::vector<uint8_t>& frame, bool debug);
|
||||
|
||||
/**
|
||||
* @brief Query and print the current power status of the AGV.
|
||||
* @return true if AGV is successfully power on; false otherwise.
|
||||
*/
|
||||
bool is_power_on();
|
||||
|
||||
/**
|
||||
* @brief Enable or disable AGV auto-reporting
|
||||
* @param[in] enable 0 = disable, 1 = enable
|
||||
*/
|
||||
void set_auto_report(bool enable);
|
||||
|
||||
/**
|
||||
* @brief Clear the serial port input and output buffers
|
||||
* @param[in] fd File descriptor of the serial port
|
||||
*/
|
||||
void clearSerialBuffer(int fd);
|
||||
|
||||
/**
|
||||
* @brief Disable the DTR (Data Terminal Ready) and RTS (Request To Send) lines of the serial port
|
||||
* @param[in] fd File descriptor of the serial port
|
||||
*/
|
||||
void disableDTR_RTS(int fd);
|
||||
|
||||
/**
|
||||
* @brief Read sensor and motor data from the AGV via serial port.
|
||||
* @return true if data is successfully read and verified; false otherwise.
|
||||
*/
|
||||
bool readData();
|
||||
|
||||
/**
|
||||
* @brief Odometry publisher
|
||||
* @param[in] dt Time difference (in seconds) since the last odometry update.
|
||||
*/
|
||||
void publisherOdom(double dt);
|
||||
|
||||
/**
|
||||
* @brief Voltage publisher
|
||||
*/
|
||||
void publisherVoltage();
|
||||
|
||||
/**
|
||||
* @brief ImuSensor publisher
|
||||
*/
|
||||
void publisherImuSensor();
|
||||
|
||||
/**
|
||||
* @brief Callback for velocity command updates
|
||||
* @param[in] msg The Twist message containing desired linear and angular velocities
|
||||
*/
|
||||
void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief Build a standard AGV serial frame with header, payload, and CRC.
|
||||
*
|
||||
* The frame has a fixed size of RECEIVE_DATA_SIZE, starts with 0xFE 0xFE 0x0B,
|
||||
* includes a command ID and up to 8 payload bytes, and ends with a 16-bit CRC.
|
||||
*
|
||||
* @param[in] cmd_id The command ID for the serial frame.
|
||||
* @param[in] payload The payload bytes to include (up to 8 bytes).
|
||||
* @return A vector containing the complete serial frame ready to transmit.
|
||||
*/
|
||||
std::vector<uint8_t> build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload);
|
||||
|
||||
/**
|
||||
* @brief Read a serial response from the AGV device, waiting for a specific header.
|
||||
*
|
||||
* This function reads bytes from the serial port until the expected header
|
||||
* sequence is detected or the timeout expires. After detecting the header,
|
||||
* it reads the remaining payload bytes along with a 2-byte CRC.
|
||||
*
|
||||
* @param[in] expected_header The byte sequence to identify the start of a valid frame.
|
||||
* @param[in] payload_size The expected number of payload bytes following the header.
|
||||
* @param[in] timeout_sec Maximum time (in seconds) to wait for the header.
|
||||
* @return A vector containing the complete frame (header + payload + CRC).
|
||||
* Returns an empty vector if a timeout occurs or the full payload is not received.
|
||||
*/
|
||||
std::vector<uint8_t> read_serial_response(
|
||||
const std::vector<uint8_t>& expected_header,
|
||||
size_t payload_size,
|
||||
double timeout_sec);
|
||||
|
||||
/**
|
||||
* @brief Compute the CRC-16-IBM checksum for a byte array.
|
||||
*
|
||||
* This function calculates the CRC using the standard IBM polynomial 0xA001.
|
||||
*
|
||||
* @param[in] data Pointer to the byte array.
|
||||
* @param[in] length Number of bytes to include in the CRC calculation.
|
||||
* @return The computed 16-bit CRC value.
|
||||
*/
|
||||
uint16_t crc16_ibm(const uint8_t* data, size_t length);
|
||||
|
||||
/**
|
||||
* @brief Handle the SetDigitalOutput service request.
|
||||
*
|
||||
* This service sets the state (HIGH/LOW) of a specific digital output pin on the AGV device.
|
||||
* The request contains the pin number and desired state, which are sent to the hardware
|
||||
* via the serial interface. The response reports whether the operation succeeded.
|
||||
*
|
||||
* @param[in] request The service request, containing:
|
||||
* - pin: The digital output pin number.
|
||||
* - state: Desired output state (true = HIGH, false = LOW).
|
||||
* @param[out] response The service response, containing:
|
||||
* - success: True if the operation succeeded.
|
||||
* - message: Optional status or error description.
|
||||
*/
|
||||
void handleSetDigitalOutput(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response);
|
||||
|
||||
/**
|
||||
* @brief Handle the GetDigitalInput service request.
|
||||
*
|
||||
* This service reads the state (HIGH/LOW) of a specific digital input pin on the AGV device.
|
||||
* The request specifies the pin number, and the node queries the hardware via the serial
|
||||
* interface to retrieve its current state.
|
||||
*
|
||||
* @param[in] request The service request, containing:
|
||||
* - pin: The digital input pin number to read.
|
||||
* @param[out] response The service response, containing:
|
||||
* - state: Current pin state (true = HIGH, false = LOW).
|
||||
* - success: True if the read operation succeeded.
|
||||
* - message: Optional status or error description.
|
||||
*/
|
||||
void handleGetDigitalInput(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
|
||||
|
||||
void handleSetLedColor(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response);
|
||||
|
||||
void handleSetLedMode(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response);
|
||||
|
||||
boost::asio::io_service io_;
|
||||
std::unique_ptr<boost::asio::serial_port> serial_port_;
|
||||
|
||||
std::string frame_id_of_odometry_;
|
||||
std::string child_frame_id_of_odometry_;
|
||||
std::string frame_id_of_imu_;
|
||||
std::string name_space_;
|
||||
std::string device_name_;
|
||||
|
||||
double x= 0.0;
|
||||
double y= 0.0;
|
||||
double theta= 0.0;
|
||||
|
||||
double vx= 0.0;
|
||||
double vy= 0.0;
|
||||
double vtheta= 0.0;
|
||||
|
||||
double linearX = 0.0;
|
||||
double linearY = 0.0;
|
||||
double angularZ = 0.0;
|
||||
|
||||
double ax= 0.0;
|
||||
double ay= 0.0;
|
||||
double az= 0.0;
|
||||
|
||||
double wx= 0.0;
|
||||
double wy= 0.0;
|
||||
double wz= 0.0;
|
||||
|
||||
double roll = 0.0;
|
||||
double pitch = 0.0;
|
||||
double yaw = 0.0;
|
||||
|
||||
int is_poweron_status = 0;
|
||||
int poweron_status = 0;
|
||||
|
||||
uint8_t motor_status = 0;
|
||||
uint8_t motor_error = 0;
|
||||
uint8_t enable_status = 0;
|
||||
|
||||
float battery_voltage = 0.0f;
|
||||
|
||||
std::array<double, 36> odom_pose_covariance = {
|
||||
{1e-9, 0, 0, 0, 0, 0,
|
||||
0, 1e-3, 1e-9, 0, 0, 0,
|
||||
0, 0, 1e6, 0, 0, 0,
|
||||
0, 0, 0, 1e6, 0, 0,
|
||||
0, 0, 0, 0, 1e6, 0,
|
||||
0, 0, 0, 0, 0, 1e-9} };
|
||||
|
||||
std::array<double, 36> odom_twist_covariance = {
|
||||
{1e-9, 0, 0, 0, 0, 0,
|
||||
0, 1e-3, 1e-9, 0, 0, 0,
|
||||
0, 0, 1e6, 0, 0, 0,
|
||||
0, 0, 0, 1e6, 0, 0,
|
||||
0, 0, 0, 0, 1e6, 0,
|
||||
0, 0, 0, 0, 0, 1e-9} };
|
||||
|
||||
rclcpp::Time currentTime, lastTime;
|
||||
rclcpp::TimerBase::SharedPtr control_timer_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_voltage;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
|
||||
rclcpp::Service<agv_pro_msgs::srv::SetDigitalOutput>::SharedPtr set_output_service;
|
||||
rclcpp::Service<agv_pro_msgs::srv::GetDigitalInput>::SharedPtr get_input_service;
|
||||
rclcpp::Service<agv_pro_msgs::srv::SetLedColor>::SharedPtr set_led_service;
|
||||
rclcpp::Service<agv_pro_msgs::srv::SetLedMode>::SharedPtr set_led_mode_service;
|
||||
|
||||
sensor_msgs::msg::Imu imu_data;
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,27 @@
|
||||
<?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_base</name>
|
||||
<version>1.0.8</version>
|
||||
<description>Control Nodes for AGV Pro</description>
|
||||
<maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
|
||||
<license>BSD-3-Clause license</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>serial_driver</depend>
|
||||
<depend>asio_cmake_module</depend>
|
||||
<depend>io_context</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<depend>agv_pro_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,12 @@
|
||||
#include "agv_pro_base/agv_pro_driver.h"
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<AGV_PRO>("agv_pro_base_node");
|
||||
|
||||
rclcpp::spin(node);
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,717 @@
|
||||
#include "agv_pro_base/agv_pro_driver.h"
|
||||
|
||||
uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) {
|
||||
uint16_t crc = 0xFFFF;
|
||||
for (size_t i = 0; i < length; ++i) {
|
||||
crc ^= static_cast<uint16_t>(data[i]);
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
if (crc & 0x0001)
|
||||
crc = (crc >> 1) ^ 0xA001;
|
||||
else
|
||||
crc = crc >> 1;
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload)
|
||||
{
|
||||
std::vector<uint8_t> frame(SEND_DATA_SIZE, 0x00);
|
||||
frame[0] = 0xFE;
|
||||
frame[1] = 0xFE;
|
||||
frame[2] = 0x0B;
|
||||
frame[3] = cmd_id;
|
||||
|
||||
for (size_t i = 0; i < payload.size() && i < 8; ++i) {
|
||||
frame[4 + i] = payload[i];
|
||||
}
|
||||
|
||||
uint16_t crc = crc16_ibm(frame.data(), 12);
|
||||
frame[12] = (crc >> 8) & 0xff;
|
||||
frame[13] = crc & 0xff;
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
void AGV_PRO::print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> override_size) {
|
||||
std::stringstream ss;
|
||||
for (auto b : data) {
|
||||
ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2)
|
||||
<< static_cast<int>(b) << " ";
|
||||
}
|
||||
size_t len = override_size.value_or(data.size());
|
||||
RCLCPP_INFO(this->get_logger(), "%s (%zu bytes): [%s]", label.c_str(), len, ss.str().c_str());
|
||||
}
|
||||
|
||||
void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
|
||||
{
|
||||
try {
|
||||
size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame));
|
||||
if (debug) {
|
||||
print_hex("Sent", frame, bytes_transmit_size);
|
||||
}
|
||||
} catch (const std::exception &ex) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port: %s", ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<uint8_t> AGV_PRO::read_serial_response(
|
||||
const std::vector<uint8_t>& expected_header,
|
||||
size_t payload_size,
|
||||
double timeout_sec)
|
||||
{
|
||||
std::vector<uint8_t> sliding_buf;
|
||||
uint8_t byte = 0;
|
||||
|
||||
rclcpp::Time start_time = this->now();
|
||||
rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);
|
||||
|
||||
while ((this->now() - start_time) < timeout) {
|
||||
boost::asio::mutable_buffers_1 buf(&byte, 1);
|
||||
boost::system::error_code ec;
|
||||
size_t n = serial_port_->read_some(buf, ec);
|
||||
if (ec) {
|
||||
RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return {};
|
||||
}
|
||||
if (n == 1) {
|
||||
sliding_buf.push_back(byte);
|
||||
if (sliding_buf.size() > expected_header.size()) {
|
||||
sliding_buf.erase(sliding_buf.begin());
|
||||
}
|
||||
if (sliding_buf == expected_header) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (sliding_buf != expected_header) {
|
||||
RCLCPP_WARN(this->get_logger(), "Timeout waiting for header");
|
||||
return {};
|
||||
}
|
||||
|
||||
size_t remain_len = payload_size + 2;
|
||||
std::vector<uint8_t> remain_buf(remain_len);
|
||||
size_t total_read = 0;
|
||||
|
||||
while (total_read < remain_len && (this->now() - start_time) < timeout) {
|
||||
boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read);
|
||||
boost::system::error_code ec;
|
||||
size_t n = serial_port_->read_some(buf, ec);
|
||||
if (ec) {
|
||||
RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return {};
|
||||
}
|
||||
total_read += n;
|
||||
}
|
||||
|
||||
if (total_read != remain_len) {
|
||||
RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload");
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<uint8_t> full_buf = expected_header;
|
||||
full_buf.insert(full_buf.end(), remain_buf.begin(), remain_buf.end());
|
||||
|
||||
return full_buf;
|
||||
}
|
||||
|
||||
bool AGV_PRO::is_power_on(){
|
||||
auto power_query_frame = build_serial_frame(GET_POWER_STATE, {});
|
||||
send_serial_frame(power_query_frame,true);
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x12};
|
||||
auto power_query_response = read_serial_response(expected_header, 8, 12.0);
|
||||
|
||||
print_hex("recv_buf", power_query_response);
|
||||
|
||||
if (power_query_response.size() != 14) return false;
|
||||
|
||||
uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13];
|
||||
uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12);
|
||||
if (received_crc != computed_crc) {
|
||||
RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
|
||||
return false;
|
||||
}
|
||||
|
||||
int is_poweron_status = static_cast<int8_t>(power_query_response[4]);
|
||||
RCLCPP_INFO(this->get_logger(), "is_poweron_status: %d", is_poweron_status);
|
||||
|
||||
if (is_poweron_status == 0){
|
||||
auto status_query_frame = build_serial_frame(POWER_ON, {});
|
||||
send_serial_frame(status_query_frame,true);
|
||||
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x10};
|
||||
auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds
|
||||
print_hex("recv_buf", status_query_response);
|
||||
|
||||
if (status_query_response.size() != 14) return false;
|
||||
|
||||
uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13];
|
||||
uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12);
|
||||
if (received_crc != computed_crc) {
|
||||
RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
|
||||
return false;
|
||||
}
|
||||
|
||||
int poweron_status = static_cast<int8_t>(status_query_response[4]);
|
||||
std::string status_msg;
|
||||
|
||||
switch (poweron_status) {
|
||||
case 1:
|
||||
status_msg = "Motor is operating normally.";
|
||||
RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return true;
|
||||
case 2:
|
||||
status_msg = "Emergency stop button is not released.";
|
||||
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return false;
|
||||
case 3:
|
||||
status_msg = "Battery voltage is below 19.5V.";
|
||||
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return false;
|
||||
case 4:
|
||||
status_msg = "CAN initialization error.";
|
||||
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return false;
|
||||
case 5:
|
||||
status_msg = "Motor initialization error.";
|
||||
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
|
||||
return false;
|
||||
default:
|
||||
RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else{
|
||||
RCLCPP_INFO(this->get_logger(), "Motor is operating normally.");
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::set_auto_report(bool enable){
|
||||
auto frame = build_serial_frame(0x23, {static_cast<uint8_t>(enable)});
|
||||
send_serial_frame(frame,true);
|
||||
}
|
||||
|
||||
void AGV_PRO::clearSerialBuffer(int fd) {
|
||||
if (tcflush(fd, TCIOFLUSH) < 0) {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "Serial buffer flushed.");
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::disableDTR_RTS(int fd) {
|
||||
int status;
|
||||
if (::ioctl(fd, TIOCMGET, &status) == 0) {
|
||||
status &= ~(TIOCM_DTR | TIOCM_RTS);
|
||||
if (::ioctl(fd, TIOCMSET, &status) != 0) {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno));
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully.");
|
||||
}
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
linearX = std::clamp(msg->linear.x, -1.5, 1.5);
|
||||
linearY = std::clamp(msg->linear.y, -1.0, 1.0);
|
||||
angularZ = std::clamp(msg->angular.z, -1.0, 1.0);
|
||||
|
||||
int16_t x_send = static_cast<int16_t>(linearX * 100);
|
||||
int16_t y_send = static_cast<int16_t>(linearY * 100);
|
||||
int16_t rot_send = static_cast<int16_t>(angularZ * 100);
|
||||
|
||||
uint8_t buf[14] = { 0xfe,0xfe,0x0b,0x21 };
|
||||
|
||||
buf[4] = (x_send >> 8) & 0xff;
|
||||
buf[5] = x_send & 0xff;
|
||||
buf[6] = (y_send >> 8) & 0xff;
|
||||
buf[7] = y_send & 0xff;
|
||||
buf[8] = (rot_send >> 8) & 0xff;
|
||||
buf[9] = rot_send & 0xff;
|
||||
buf[10] = 0x00;
|
||||
buf[11] = 0x00;
|
||||
|
||||
uint16_t crc = crc16_ibm(buf, 12);
|
||||
buf[12] = (crc >> 8) & 0xff;
|
||||
buf[13] = crc & 0xff;
|
||||
|
||||
std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
|
||||
|
||||
try
|
||||
{
|
||||
boost::asio::write(*serial_port_,boost::asio::buffer(data_vec));
|
||||
// print_hex("Sent", data_vec);//debug
|
||||
}
|
||||
catch(const std::exception &ex)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::handleSetDigitalOutput(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response)
|
||||
{
|
||||
uint8_t output_number = request->pin;
|
||||
uint8_t output_state = request->state;
|
||||
|
||||
if (output_number < 1 || output_number > 6){
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid output pin number: %u", output_number);
|
||||
response->success = false;
|
||||
response->message = "Invalid output pin number";
|
||||
return;
|
||||
}
|
||||
|
||||
auto frame = build_serial_frame(SET_OUTPUT_IO, {output_number, output_state});
|
||||
send_serial_frame(frame, true);
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_OUTPUT_IO};
|
||||
auto response_frame = read_serial_response(expected_header, 8, 5.0);
|
||||
|
||||
// print_hex("recv_buf", response_frame); //debug
|
||||
|
||||
uint8_t status = response_frame[4];
|
||||
if (status == 0x01) {
|
||||
RCLCPP_DEBUG(this->get_logger(), "SetDigitalOutput succeeded");
|
||||
response->success = true;
|
||||
response->message = "Success";
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "SetDigitalOutput failed with status: 0x%02X", status);
|
||||
response->success = false;
|
||||
response->message = "Failed with status code";
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::handleGetDigitalInput(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response)
|
||||
{
|
||||
uint8_t input_number = request->pin;
|
||||
|
||||
if (input_number < 1 || input_number > 6){
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid input pin number: %u", input_number);
|
||||
response->success = false;
|
||||
response->message = "Invalid input pin number";
|
||||
return;
|
||||
}
|
||||
|
||||
auto frame = build_serial_frame(GET_INPUT_IO, {input_number});
|
||||
send_serial_frame(frame, true);
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, GET_INPUT_IO};
|
||||
auto response_frame = read_serial_response(expected_header, 8, 5.0);
|
||||
// print_hex("recv_buf", response_frame); //debug
|
||||
|
||||
uint8_t status = response_frame[5];
|
||||
if (status == 0xff) {
|
||||
RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status);
|
||||
response->success = false;
|
||||
} else {
|
||||
RCLCPP_DEBUG(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
|
||||
response->state = static_cast<int32_t>(status);
|
||||
response->success = true;
|
||||
response->message = "Success";
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::handleSetLedColor(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response)
|
||||
{
|
||||
if (request->position < 0 || request->position > 1) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid LED position: %d", request->position);
|
||||
response->success = false;
|
||||
response->message = "Invalid LED position";
|
||||
return;
|
||||
}
|
||||
|
||||
if (request->brightness < 0 || request->brightness > 255) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid brightness: %d", request->brightness);
|
||||
response->success = false;
|
||||
response->message = "Invalid brightness";
|
||||
return;
|
||||
}
|
||||
|
||||
if (request->r < 0 || request->r > 255 ||
|
||||
request->g < 0 || request->g > 255 ||
|
||||
request->b < 0 || request->b > 255) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(),
|
||||
"Invalid RGB value: r=%d g=%d b=%d",
|
||||
request->r, request->g, request->b
|
||||
);
|
||||
response->success = false;
|
||||
response->message = "Invalid RGB value";
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t position = static_cast<uint8_t>(request->position);
|
||||
uint8_t brightness = static_cast<uint8_t>(request->brightness);
|
||||
uint8_t r = static_cast<uint8_t>(request->r);
|
||||
uint8_t g = static_cast<uint8_t>(request->g);
|
||||
uint8_t b = static_cast<uint8_t>(request->b);
|
||||
|
||||
auto frame = build_serial_frame(SET_LED_COLOR, {position, brightness, r, g, b});
|
||||
send_serial_frame(frame, true);
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_COLOR};
|
||||
auto response_frame = read_serial_response(expected_header, 8, 5.0);
|
||||
|
||||
// print_hex("recv_buf", response_frame); //debug
|
||||
|
||||
uint8_t status = response_frame[4];
|
||||
if (status == 0x01) {
|
||||
RCLCPP_DEBUG(this->get_logger(), "SetLedColor succeeded");
|
||||
response->success = true;
|
||||
response->message = "Success";
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "SetLedColor failed with status: 0x%02X", status);
|
||||
response->success = false;
|
||||
response->message = "Failed with status code";
|
||||
}
|
||||
}
|
||||
|
||||
void AGV_PRO::handleSetLedMode(
|
||||
const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
|
||||
std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response)
|
||||
{
|
||||
uint8_t mode = request->mode ? 0x01 : 0x00;
|
||||
|
||||
auto frame = build_serial_frame(SET_LED_MODE, {mode});
|
||||
send_serial_frame(frame, true);
|
||||
|
||||
const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_MODE};
|
||||
auto response_frame = read_serial_response(expected_header, 8, 5.0);
|
||||
|
||||
// print_hex("recv_buf", response_frame); //debug
|
||||
|
||||
uint8_t status = response_frame[4];
|
||||
if (status == 0x01) {
|
||||
RCLCPP_DEBUG(this->get_logger(), "SetLedMode succeeded");
|
||||
response->success = true;
|
||||
response->message = "Success";
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "SetLedMode failed with status: 0x%02X", status);
|
||||
response->success = false;
|
||||
response->message = "Failed with status code";
|
||||
}
|
||||
}
|
||||
|
||||
bool AGV_PRO::readData()
|
||||
{
|
||||
std::vector<uint8_t> buf_length(1);
|
||||
std::vector<uint8_t> data_buf(RECEIVE_PAYLOAD_SIZE);
|
||||
|
||||
uint8_t byte = 0;
|
||||
boost::system::error_code ec;
|
||||
|
||||
while (true)
|
||||
{
|
||||
size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
|
||||
if (ec) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return false;
|
||||
}
|
||||
if (ret != 1 || byte != 0xfe) {
|
||||
continue;
|
||||
}
|
||||
|
||||
ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
|
||||
if (ec) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return false;
|
||||
}
|
||||
if (ret == 1 && byte == 0xfe) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec);
|
||||
if (ec) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) {
|
||||
//RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
|
||||
return false;
|
||||
}
|
||||
|
||||
ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec);
|
||||
if (ec || ret != data_buf.size())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload");
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> recv_buf;
|
||||
recv_buf.push_back(0xFE);
|
||||
recv_buf.push_back(0xFE);
|
||||
recv_buf.push_back(0x1C);
|
||||
recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
|
||||
|
||||
// print_hex("recv_buf", recv_buf); //debug
|
||||
|
||||
if (recv_buf[3] != 0x25) {
|
||||
//RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8);
|
||||
uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2);
|
||||
|
||||
if (received_crc != computed_crc) {
|
||||
RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc);
|
||||
return false;
|
||||
}
|
||||
|
||||
vx = static_cast<double>(static_cast<int8_t>(recv_buf[4])) * 0.01;
|
||||
vy = static_cast<double>(static_cast<int8_t>(recv_buf[5])) * 0.01;
|
||||
vtheta = static_cast<double>(static_cast<int8_t>(recv_buf[6])) * 0.01;
|
||||
|
||||
motor_status = recv_buf[7];
|
||||
motor_error = recv_buf[8];
|
||||
battery_voltage = static_cast<float>(recv_buf[9]) / 10.0f;
|
||||
enable_status = recv_buf[10];
|
||||
|
||||
imu_data.linear_acceleration.x = static_cast<double>(static_cast<int16_t>((recv_buf[11] << 8) | recv_buf[12])) * 0.01;
|
||||
imu_data.linear_acceleration.y = static_cast<double>(static_cast<int16_t>((recv_buf[13] << 8) | recv_buf[14])) * 0.01;
|
||||
imu_data.linear_acceleration.z = static_cast<double>(static_cast<int16_t>((recv_buf[15] << 8) | recv_buf[16])) * 0.01;
|
||||
|
||||
imu_data.angular_velocity.x = static_cast<double>(static_cast<int16_t>((recv_buf[17] << 8) | recv_buf[18])) * 0.01;
|
||||
imu_data.angular_velocity.y = static_cast<double>(static_cast<int16_t>((recv_buf[19] << 8) | recv_buf[20])) * 0.01;
|
||||
imu_data.angular_velocity.z = static_cast<double>(static_cast<int16_t>((recv_buf[21] << 8) | recv_buf[22])) * 0.01;
|
||||
|
||||
roll = static_cast<double>(static_cast<int16_t>((recv_buf[23] << 8) | recv_buf[24])) * 0.01;
|
||||
pitch = static_cast<double>(static_cast<int16_t>((recv_buf[25] << 8) | recv_buf[26])) * 0.01;
|
||||
yaw = static_cast<double>(static_cast<int16_t>((recv_buf[27] << 8) | recv_buf[28])) * 0.01;
|
||||
|
||||
// RCLCPP_INFO(this->get_logger(),
|
||||
// "IMU Data - Accel[x: %.2f, y: %.2f, z: %.2f], "
|
||||
// "Gyro[x: %.2f, y: %.2f, z: %.2f], "
|
||||
// "RPY[roll: %.2f, pitch: %.2f, yaw: %.2f]",
|
||||
// imu_data.linear_acceleration.x,
|
||||
// imu_data.linear_acceleration.y,
|
||||
// imu_data.linear_acceleration.z,
|
||||
// imu_data.angular_velocity.x,
|
||||
// imu_data.angular_velocity.y,
|
||||
// imu_data.angular_velocity.z,
|
||||
// roll, pitch, yaw);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AGV_PRO::publisherVoltage()
|
||||
{
|
||||
std_msgs::msg::Float32 voltage_msg,voltage_backup_msg;
|
||||
voltage_msg.data = battery_voltage;
|
||||
pub_voltage->publish(voltage_msg);
|
||||
}
|
||||
|
||||
void AGV_PRO::publisherImuSensor()
|
||||
{
|
||||
sensor_msgs::msg::Imu ImuSensor;
|
||||
|
||||
ImuSensor.header.stamp = this->get_clock()->now();
|
||||
ImuSensor.header.frame_id = "imu_link";
|
||||
|
||||
tf2::Quaternion qua;
|
||||
qua.setRPY(0, 0, yaw * M_PI / 180.0);
|
||||
|
||||
ImuSensor.orientation.x = qua[0];
|
||||
ImuSensor.orientation.y = qua[1];
|
||||
ImuSensor.orientation.z = qua[2];
|
||||
ImuSensor.orientation.w = qua[3];
|
||||
|
||||
ImuSensor.angular_velocity.x = imu_data.angular_velocity.x;
|
||||
ImuSensor.angular_velocity.y = imu_data.angular_velocity.y;
|
||||
ImuSensor.angular_velocity.z = imu_data.angular_velocity.z;
|
||||
|
||||
ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x;
|
||||
ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y;
|
||||
ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z;
|
||||
|
||||
ImuSensor.orientation_covariance[0] = 1e6;
|
||||
ImuSensor.orientation_covariance[4] = 1e6;
|
||||
ImuSensor.orientation_covariance[8] = 1e-6;
|
||||
|
||||
ImuSensor.angular_velocity_covariance[0] = 1e6;
|
||||
ImuSensor.angular_velocity_covariance[4] = 1e6;
|
||||
ImuSensor.angular_velocity_covariance[8] = 1e-6;
|
||||
|
||||
pub_imu->publish(ImuSensor);
|
||||
}
|
||||
|
||||
void AGV_PRO::publisherOdom(double dt)
|
||||
{
|
||||
currentTime = this->get_clock()->now();
|
||||
|
||||
double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt;
|
||||
double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt;
|
||||
double delta_th = vtheta * dt;
|
||||
|
||||
x += delta_x;
|
||||
y += delta_y;
|
||||
theta += delta_th;
|
||||
|
||||
geometry_msgs::msg::TransformStamped odom_trans;
|
||||
odom_trans.header.stamp = currentTime;
|
||||
odom_trans.header.frame_id = frame_id_of_odometry_;
|
||||
odom_trans.child_frame_id = child_frame_id_of_odometry_;
|
||||
|
||||
tf2::Quaternion quat;
|
||||
quat.setRPY(0.0, 0.0, theta);
|
||||
geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat);
|
||||
|
||||
odom_trans.transform.translation.x = x;
|
||||
odom_trans.transform.translation.y = y;
|
||||
odom_trans.transform.translation.z = 0.0;
|
||||
odom_trans.transform.rotation = odom_quat;
|
||||
|
||||
odomBroadcaster->sendTransform(odom_trans);
|
||||
|
||||
nav_msgs::msg::Odometry odom;
|
||||
odom.header.stamp = currentTime;
|
||||
odom.header.frame_id = frame_id_of_odometry_;
|
||||
odom.child_frame_id = child_frame_id_of_odometry_;
|
||||
|
||||
odom.pose.pose.position.x = x;
|
||||
odom.pose.pose.position.y = y;
|
||||
odom.pose.pose.position.z = 0.0;
|
||||
odom.pose.pose.orientation = odom_quat;
|
||||
odom.pose.covariance = this->odom_pose_covariance;
|
||||
|
||||
odom.twist.twist.linear.x = vx;
|
||||
odom.twist.twist.linear.y = vy;
|
||||
odom.twist.twist.angular.z = vtheta;
|
||||
odom.twist.covariance = this->odom_twist_covariance;
|
||||
|
||||
pub_odom->publish(odom);
|
||||
}
|
||||
|
||||
void AGV_PRO::Control()
|
||||
{
|
||||
if (true == readData())
|
||||
{
|
||||
currentTime = this->get_clock()->now();
|
||||
double dt = 0.0;
|
||||
if (lastTime.nanoseconds() != 0) {
|
||||
dt = (currentTime - lastTime).seconds();
|
||||
}
|
||||
|
||||
lastTime = currentTime;
|
||||
publisherOdom(dt);
|
||||
// RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
|
||||
publisherVoltage();
|
||||
publisherImuSensor();
|
||||
}
|
||||
}
|
||||
|
||||
AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
|
||||
{
|
||||
this->declare_parameter<std::string>("port_name","/dev/agvpro_controller");
|
||||
this->declare_parameter<std::string>("odometry.frame_id", "odom");
|
||||
this->declare_parameter<std::string>("odometry.child_frame_id", "base_footprint");
|
||||
this->declare_parameter<std::string>("imu.frame_id", "imu_link");
|
||||
this->declare_parameter<std::string>("namespace", "");
|
||||
|
||||
this->get_parameter_or<std::string>("port_name",device_name_,std::string("/dev/agvpro_controller"));
|
||||
this->get_parameter_or<std::string>("odometry.frame_id",frame_id_of_odometry_,std::string("odom"));
|
||||
this->get_parameter_or<std::string>("odometry.child_frame_id",child_frame_id_of_odometry_,std::string("base_footprint"));
|
||||
this->get_parameter_or<std::string>("imu.frame_id",frame_id_of_imu_,std::string("imu_link"));
|
||||
this->get_parameter_or<std::string>("namespace",name_space_,std::string(""));
|
||||
|
||||
if (name_space_ != "") {
|
||||
frame_id_of_odometry_ = name_space_ + "/" + frame_id_of_odometry_;
|
||||
child_frame_id_of_odometry_ = name_space_ + "/" + child_frame_id_of_odometry_;
|
||||
frame_id_of_imu_ = name_space_ + "/" + frame_id_of_imu_;
|
||||
}
|
||||
|
||||
odomBroadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
|
||||
pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("imu", 20);
|
||||
pub_odom = this->create_publisher<nav_msgs::msg::Odometry>("odom", 50);
|
||||
pub_voltage = create_publisher<std_msgs::msg::Float32>("voltage", 10);
|
||||
cmd_sub = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1));
|
||||
|
||||
set_output_service = this->create_service<agv_pro_msgs::srv::SetDigitalOutput>(
|
||||
"set_digital_output",
|
||||
std::bind(&AGV_PRO::handleSetDigitalOutput, this, std::placeholders::_1, std::placeholders::_2)
|
||||
);
|
||||
|
||||
get_input_service = this->create_service<agv_pro_msgs::srv::GetDigitalInput>(
|
||||
"get_digital_input",
|
||||
std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
|
||||
);
|
||||
|
||||
set_led_service = this->create_service<agv_pro_msgs::srv::SetLedColor>(
|
||||
"set_led_color",
|
||||
std::bind(&AGV_PRO::handleSetLedColor, this, std::placeholders::_1, std::placeholders::_2)
|
||||
);
|
||||
|
||||
set_led_mode_service = this->create_service<agv_pro_msgs::srv::SetLedMode>(
|
||||
"set_led_mode",
|
||||
std::bind(&AGV_PRO::handleSetLedMode, this, std::placeholders::_1, std::placeholders::_2)
|
||||
);
|
||||
|
||||
lastTime = this->get_clock()->now();
|
||||
|
||||
try{
|
||||
serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
|
||||
|
||||
serial_port_->open(device_name_);
|
||||
serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
|
||||
serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
|
||||
serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
|
||||
serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
|
||||
serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
|
||||
|
||||
int fd = serial_port_->native_handle();
|
||||
this->clearSerialBuffer(fd);
|
||||
this->disableDTR_RTS(fd);
|
||||
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
|
||||
RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str());
|
||||
|
||||
boost::asio::serial_port_base::baud_rate baud_option;
|
||||
serial_port_->get_option(baud_option);
|
||||
unsigned int current_baud = baud_option.value();
|
||||
RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud);
|
||||
}
|
||||
catch (const std::exception &ex){
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
|
||||
return;
|
||||
}
|
||||
|
||||
if (this->is_power_on()) {
|
||||
this->set_auto_report(1);
|
||||
|
||||
control_timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(20),
|
||||
std::bind(&AGV_PRO::Control, this)
|
||||
);
|
||||
RCLCPP_INFO(this->get_logger(), "Control timer started");
|
||||
}
|
||||
else {
|
||||
RCLCPP_WARN(this->get_logger(), "Control timer not started.");
|
||||
}
|
||||
}
|
||||
|
||||
AGV_PRO::~AGV_PRO()
|
||||
{
|
||||
if (serial_port_ && serial_port_->is_open()) {
|
||||
this->set_auto_report(0);
|
||||
serial_port_->cancel();
|
||||
serial_port_->close();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(agv_pro_bringup)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY
|
||||
launch
|
||||
config
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,50 @@
|
||||
-- Copyright 2016 The Cartographer Authors
|
||||
--
|
||||
-- Licensed under the Apache License, Version 2.0 (the "License");
|
||||
-- you may not use this file except in compliance with the License.
|
||||
-- You may obtain a copy of the License at
|
||||
--
|
||||
-- http://www.apache.org/licenses/LICENSE-2.0
|
||||
--
|
||||
-- Unless required by applicable law or agreed to in writing, software
|
||||
-- distributed under the License is distributed on an "AS IS" BASIS,
|
||||
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
-- See the License for the specific language governing permissions and
|
||||
-- limitations under the License.
|
||||
|
||||
include "map_builder.lua"
|
||||
include "trajectory_builder.lua"
|
||||
|
||||
options = {
|
||||
map_builder = MAP_BUILDER,
|
||||
trajectory_builder = TRAJECTORY_BUILDER,
|
||||
map_frame = "map",
|
||||
tracking_frame = "base_footprint",
|
||||
published_frame = "base_footprint",
|
||||
odom_frame = "odom",
|
||||
provide_odom_frame = true,
|
||||
publish_frame_projected_to_2d = true,
|
||||
use_pose_extrapolator = true,
|
||||
use_odometry = true,
|
||||
use_nav_sat = false,
|
||||
use_landmarks = false,
|
||||
num_laser_scans = 1,
|
||||
num_multi_echo_laser_scans = 0,
|
||||
num_subdivisions_per_laser_scan = 1,
|
||||
num_point_clouds = 0,
|
||||
lookup_transform_timeout_sec = 0.2,
|
||||
submap_publish_period_sec = 0.3,
|
||||
pose_publish_period_sec = 5e-3,
|
||||
trajectory_publish_period_sec = 30e-3,
|
||||
rangefinder_sampling_ratio = 1.,
|
||||
odometry_sampling_ratio = 1.,
|
||||
fixed_frame_pose_sampling_ratio = 1.,
|
||||
imu_sampling_ratio = 1.,
|
||||
landmarks_sampling_ratio = 1.,
|
||||
}
|
||||
|
||||
MAP_BUILDER.use_trajectory_builder_2d = true
|
||||
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
|
||||
TRAJECTORY_BUILDER_2D.use_imu_data = false
|
||||
|
||||
return options
|
||||
@@ -0,0 +1,118 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node,PushRosNamespace
|
||||
from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
|
||||
from launch.substitutions import Command,LaunchConfiguration,PythonExpression
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def include_lidar(pkg_name, launch_file, enable_lidar, lidar_type, expected_type):
|
||||
|
||||
return IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(
|
||||
get_package_share_directory(pkg_name),
|
||||
'launch',
|
||||
launch_file
|
||||
)
|
||||
),
|
||||
condition=IfCondition(
|
||||
PythonExpression([
|
||||
"'", enable_lidar, "' == 'true' and '",
|
||||
lidar_type, "' == '", expected_type, "'"
|
||||
])
|
||||
)
|
||||
)
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
port_name_arg = LaunchConfiguration('port_name')
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
lidar_type = LaunchConfiguration('lidar_type')
|
||||
enable_lidar = LaunchConfiguration('enable_lidar')
|
||||
|
||||
urdf_file = os.path.join(
|
||||
get_package_share_directory('agv_pro_description'),
|
||||
'urdf',
|
||||
'agv_pro.urdf'
|
||||
)
|
||||
|
||||
robot_description_content = Command([
|
||||
'xacro ',
|
||||
urdf_file,
|
||||
' namespace:=',
|
||||
PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']),
|
||||
])
|
||||
|
||||
declare_port_name_arg = DeclareLaunchArgument(
|
||||
'port_name',
|
||||
default_value='/dev/agvpro_controller',
|
||||
description='port name, e.g. /dev/ttyACM0'
|
||||
)
|
||||
|
||||
declare_namespace_arg = DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='',
|
||||
description='Namespace for nodes'
|
||||
)
|
||||
|
||||
declare_enable_lidar_arg = DeclareLaunchArgument(
|
||||
'enable_lidar',
|
||||
default_value='true',
|
||||
description='Whether to launch lidar drivers'
|
||||
)
|
||||
|
||||
declare_lidar_type_arg = DeclareLaunchArgument(
|
||||
'lidar_type',
|
||||
default_value='n10p',
|
||||
description='Lidar type: n10p | mid360 | l2'
|
||||
)
|
||||
|
||||
ns_action = PushRosNamespace(namespace)
|
||||
|
||||
agv_pro_node = Node(
|
||||
package='agv_pro_base',
|
||||
executable='agv_pro_node',
|
||||
name='agv_pro_node',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'port_name': port_name_arg,
|
||||
'namespace': namespace,
|
||||
}],
|
||||
remappings=[('cmd_vel', '/cmd_vel')]
|
||||
)
|
||||
|
||||
joint_state_pub = Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher'
|
||||
)
|
||||
|
||||
robot_state_pub = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
parameters=[{'robot_description': robot_description_content}],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
lidar_launchs = [
|
||||
include_lidar('lslidar_driver', 'lsn10p_launch.py', enable_lidar, lidar_type, 'n10p'),
|
||||
include_lidar('livox_ros_driver2', 'MID360_launch.py',enable_lidar, lidar_type, 'mid360'),
|
||||
include_lidar('unitree_lidar_ros2', 'launch.py', enable_lidar, lidar_type, 'l2'),
|
||||
]
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
declare_port_name_arg,
|
||||
declare_namespace_arg,
|
||||
declare_enable_lidar_arg,
|
||||
declare_lidar_type_arg,
|
||||
ns_action,
|
||||
agv_pro_node,
|
||||
joint_state_pub,
|
||||
robot_state_pub,
|
||||
*lidar_launchs,
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,71 @@
|
||||
"""
|
||||
Copyright 2018 The Cartographer Authors
|
||||
Copyright 2022 Wyca Robotics (for the ros2 conversion)
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
"""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.actions import Shutdown
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
## ***** Launch arguments *****
|
||||
use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')
|
||||
|
||||
## ***** File paths ******
|
||||
# AGV-specific cartographer configuration lives in this package so the
|
||||
# upstream cartographer_ros install can stay in its vanilla state.
|
||||
configuration_directory = FindPackageShare('agv_pro_bringup').find('agv_pro_bringup') + '/config'
|
||||
# The RViz config is provided by the (vanilla) upstream cartographer_ros package.
|
||||
rviz_config = FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'
|
||||
|
||||
cartographer_node = Node(
|
||||
package = 'cartographer_ros',
|
||||
executable = 'cartographer_node',
|
||||
parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}],
|
||||
arguments = [
|
||||
'-configuration_directory', configuration_directory,
|
||||
'-configuration_basename', 'agvpro_backpack_2d.lua'],
|
||||
remappings = [
|
||||
('echoes', 'horizontal_laser_2d')],
|
||||
output = 'screen'
|
||||
)
|
||||
|
||||
cartographer_occupancy_grid_node = Node(
|
||||
package = 'cartographer_ros',
|
||||
executable = 'cartographer_occupancy_grid_node',
|
||||
parameters = [
|
||||
{'use_sim_time': False},
|
||||
{'resolution': 0.05}],
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package = 'rviz2',
|
||||
executable = 'rviz2',
|
||||
on_exit = Shutdown(),
|
||||
arguments = ['-d', rviz_config],
|
||||
parameters = [{'use_sim_time': False}],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
use_sim_time_arg,
|
||||
# Nodes
|
||||
rviz_node,
|
||||
cartographer_node,
|
||||
cartographer_occupancy_grid_node,
|
||||
])
|
||||
@@ -0,0 +1,24 @@
|
||||
<?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_bringup</name>
|
||||
<version>1.0.8</version>
|
||||
<description>ROS 2 launch scripts for starting the AGV Pro</description>
|
||||
<maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
|
||||
<license>BSD-3-Clause license</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>joint_state_publisher</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
<exec_depend>agv_pro_description</exec_depend>
|
||||
<exec_depend>agv_pro_base</exec_depend>
|
||||
<exec_depend>cartographer_ros</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,21 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(agv_pro_description)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
install(DIRECTORY meshes urdf launch rviz
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,63 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node,PushRosNamespace
|
||||
from launch.conditions import IfCondition
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import Command,LaunchConfiguration,PythonExpression
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
namespace = LaunchConfiguration('namespace', default='')
|
||||
|
||||
use_rviz = LaunchConfiguration('use_rviz', default='true')
|
||||
|
||||
rviz_config_dir = os.path.join(
|
||||
get_package_share_directory('agv_pro_description'),
|
||||
'rviz',
|
||||
'agvpro_display.rviz')
|
||||
|
||||
urdf_file = os.path.join(
|
||||
get_package_share_directory('agv_pro_description'),
|
||||
'urdf',
|
||||
'agv_pro.urdf'
|
||||
)
|
||||
|
||||
robot_description_content = Command([
|
||||
'xacro ',
|
||||
urdf_file,
|
||||
' namespace:=',
|
||||
PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']),
|
||||
])
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='',
|
||||
description='Namespace for nodes'),
|
||||
|
||||
PushRosNamespace(namespace),
|
||||
|
||||
Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher'
|
||||
),
|
||||
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
parameters=[{'robot_description': robot_description_content}]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
arguments=['-d', rviz_config_dir],
|
||||
condition=IfCondition(use_rviz),
|
||||
output='screen')
|
||||
|
||||
])
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+20
@@ -0,0 +1,20 @@
|
||||
<?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_description</name>
|
||||
<version>1.0.8</version>
|
||||
<description>
|
||||
<p>URDF Description package for AGV pro</p>
|
||||
</description>
|
||||
<maintainer email="weijun.xie@elephantrobotics.com">weijun.xie</maintainer>
|
||||
<license>BSD-3-Clause license</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,234 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /RobotModel1
|
||||
- /TF1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 549
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz_common/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_footprint:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_front_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_rear_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_front_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_rear_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Mass Properties:
|
||||
Inertia: false
|
||||
Mass: false
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
laser_link:
|
||||
Value: true
|
||||
left_front_wheel_link:
|
||||
Value: true
|
||||
left_rear_wheel_link:
|
||||
Value: true
|
||||
right_front_wheel_link:
|
||||
Value: true
|
||||
right_rear_wheel_link:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
base_footprint:
|
||||
base_link:
|
||||
laser_link:
|
||||
{}
|
||||
left_front_wheel_link:
|
||||
{}
|
||||
left_rear_wheel_link:
|
||||
{}
|
||||
right_front_wheel_link:
|
||||
{}
|
||||
right_rear_wheel_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base_footprint
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 1.6701574325561523
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.785398006439209
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.785398006439209
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1200
|
||||
X: 720
|
||||
Y: 343
|
||||
+232
@@ -0,0 +1,232 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot name="AGV pro" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:arg name="namespace" default=""/>
|
||||
<xacro:property name="namespace" value="$(arg namespace)"/>
|
||||
|
||||
<link name="${namespace}base_footprint"/>
|
||||
|
||||
<joint name="${namespace}base_joint" type="fixed">
|
||||
<parent link="${namespace}base_footprint"/>
|
||||
<child link="${namespace}base_link" />
|
||||
<origin xyz="0 0 0.020" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}base_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
|
||||
<mass value="19.236" />
|
||||
<inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458"
|
||||
iyy="0.24191" iyz="0.0044629"
|
||||
izz="0.33755" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/base_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/base_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="${namespace}right_rear_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.21659" />
|
||||
<inertia ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
|
||||
iyy="0.00097519" iyz="-2.3635E-07"
|
||||
izz="0.00051178" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_rb_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_rb_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}right_rear_wheel_joint" type="continuous">
|
||||
<origin xyz="-0.171806101587598 -0.179900399999999 0.0518836514526621" rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}right_rear_wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}right_front_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="6.6563E-05 -0.019725 8.3836E-05" rpy="0 0 0" />
|
||||
<mass value="0.21659122149244" />
|
||||
<inertia ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
|
||||
iyy="0.00097519" iyz="-2.3635E-07"
|
||||
izz="0.00051178" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_rf_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_rf_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}right_front_wheel_joint" type="continuous">
|
||||
<origin xyz="-0.17181 0.1799 0.051884"
|
||||
rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}right_front_wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}left_front_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="1.4671E-06 -0.019803 4.3218E-06" rpy="0 0 0" />
|
||||
<mass value="0.3015" />
|
||||
<inertia ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07"
|
||||
iyy="0.00099948" iyz="7.5332E-08"
|
||||
izz="0.00052362" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_lf_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_lf_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}left_front_wheel_joint" type="continuous">
|
||||
<origin xyz="0.17128 0.1799 0.052"
|
||||
rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}left_front_wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}left_rear_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="-2.4454E-06 0.019725 -4.3121E-06" rpy="0 0 0" />
|
||||
<mass value="0.29613" />
|
||||
<inertia ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07"
|
||||
iyy="0.0009752" iyz="1.173E-07"
|
||||
izz="0.00051131" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_lb_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_lb_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}left_rear_wheel_joint" type="continuous">
|
||||
<origin xyz="0.17128 -0.1799 0.052" rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}left_rear_wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}laser_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
|
||||
<mass value="0.049095" />
|
||||
<inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07" iyy="2.0483E-05"
|
||||
iyz="-4.2154E-09"
|
||||
izz="3.4612E-05" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}lidar_joint" type="fixed">
|
||||
<origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}laser_link" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}camera_link"/>
|
||||
|
||||
<joint name="${namespace}camera_joint" type="fixed">
|
||||
<origin xyz="0.23191 0 0.14928" rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}camera_link" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}imu_link"/>
|
||||
|
||||
<joint name="${namespace}imu_joint" type="fixed">
|
||||
<origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}imu_link" />
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,21 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(agv_pro_gazebo)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
install(DIRECTORY meshes urdf launch rviz config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,80 @@
|
||||
# AGV_Pro
|
||||
ROS2 packages for AGV_Pro
|
||||
|
||||
> Software environment for Jetson Orin Nano
|
||||
|
||||
```
|
||||
ubuntu 22.04
|
||||
ros2 humble
|
||||
gazebo 11
|
||||
```
|
||||
|
||||
# Installation
|
||||
|
||||
Create workspace and clone the repository.
|
||||
|
||||
```
|
||||
git clone https://github.com/elephantrobotics/agv_pro_ros2.git agv_pro_ros2/src
|
||||
```
|
||||
|
||||
Install dependencies
|
||||
|
||||
```
|
||||
cd ~/agv_pro_ros2
|
||||
|
||||
rosdep install --from-paths src --ignore-src -r -y
|
||||
```
|
||||
|
||||
Build workspace
|
||||
|
||||
```
|
||||
cd ~/agv_pro_ros2
|
||||
|
||||
colcon build
|
||||
```
|
||||
|
||||
Setup the workspace
|
||||
|
||||
```
|
||||
source ~/agv_pro_ros2/install/local_setup.bash
|
||||
```
|
||||
|
||||
```
|
||||
apt install ros-$ROS_DISTRO-gazebo-ros-pkgs
|
||||
|
||||
sudo apt install ros-$ROS_DISTRO-ros2-controllers
|
||||
|
||||
sudo apt install ros-humble-teleop-twist-keyboard
|
||||
```
|
||||
|
||||
# Update to new version
|
||||
|
||||
```
|
||||
cd ~/myagv_ros2/src
|
||||
|
||||
git pull
|
||||
|
||||
cd ..
|
||||
|
||||
colcon build
|
||||
```
|
||||
|
||||
# Start
|
||||
|
||||
```
|
||||
ros2 launch agv_pro_gazebo agv_pro_gazebo.launch.py
|
||||
```
|
||||
|
||||
# keyboard Control
|
||||
|
||||
```
|
||||
ros2 run teleop_twist_keyboard teleop_twist_keyboard
|
||||
```
|
||||
|
||||
# Synchronous motion
|
||||
|
||||
```
|
||||
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
|
||||
ros2 launch agv_pro_gazebo agv_pro_gazebo.launch.py
|
||||
ros2 run teleop_twist_keyboard teleop_twist_keyboard
|
||||
```
|
||||
@@ -0,0 +1,23 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
diff_drive_controller:
|
||||
type: diff_drive_controller/DiffDriveController
|
||||
left_wheel_names: ["left_front_wheel_joint", "left_rear_wheel_joint"]
|
||||
right_wheel_names: ["right_front_wheel_joint", "right_rear_wheel_joint"]
|
||||
|
||||
wheel_separation: 0.36
|
||||
wheel_radius: 0.05
|
||||
|
||||
base_frame_id: base_link
|
||||
use_stamped_vel: false
|
||||
publish_rate: 50
|
||||
|
||||
enable_odom_tf: true
|
||||
odom_frame_id: odom
|
||||
pose_covariance_diagonal: [0.001, 0.001, 99999.0, 99999.0, 99999.0, 0.03]
|
||||
twist_covariance_diagonal: [0.001, 0.001, 99999.0, 99999.0, 99999.0, 0.03]
|
||||
@@ -0,0 +1,28 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100 # 控制器更新频率 (Hz)
|
||||
use_sim_time: true # 使用仿真时间
|
||||
|
||||
# 定义关节状态广播器
|
||||
fishbot_joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
use_sim_time: true
|
||||
|
||||
# 定义全向驱动控制器
|
||||
fishbot_omni_drive_controller:
|
||||
type: omni_drive_controller/OmniDriveController
|
||||
|
||||
# 四轮全向控制器配置
|
||||
fishbot_omni_drive_controller:
|
||||
ros__parameters:
|
||||
front_left_wheel_joint: front_left_wheel_joint
|
||||
front_right_wheel_joint: front_right_wheel_joint
|
||||
rear_left_wheel_joint: rear_left_wheel_joint
|
||||
rear_right_wheel_joint: rear_right_wheel_joint
|
||||
wheel_separation: 0.36 # 轮距
|
||||
wheel_diameter: 0.1 # 轮子直径
|
||||
publish_rate: 50.0 # 发布频率
|
||||
odom_frame_id: odom
|
||||
base_frame_id: base_link
|
||||
pose_covariance_diagonal: [0.001, 0.001, 99999.0, 99999.0, 99999.0, 0.03]
|
||||
twist_covariance_diagonal: [0.001, 0.001, 99999.0, 99999.0, 99999.0, 0.03]
|
||||
@@ -0,0 +1,40 @@
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch_ros.actions import Node
|
||||
|
||||
import xacro
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Check if we're told to use sim time
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
# Process the URDF file
|
||||
pkg_path = os.path.join(get_package_share_directory('agv_pro_gazebo'))
|
||||
xacro_file = os.path.join(pkg_path,'urdf','agv_pro.xacro')
|
||||
robot_description_config = xacro.process_file(xacro_file)
|
||||
|
||||
# Create a robot_state_publisher node
|
||||
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
|
||||
node_robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[params]
|
||||
)
|
||||
|
||||
# Launch!
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='Use sim time if true'),
|
||||
|
||||
node_robot_state_publisher
|
||||
])
|
||||
@@ -0,0 +1,48 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_rviz = LaunchConfiguration('use_rviz', default='true')
|
||||
rviz_config_dir = os.path.join(
|
||||
get_package_share_directory('agv_pro_gazebo'),
|
||||
'rviz',
|
||||
'agvpro_display.rviz')
|
||||
|
||||
urdf_file = os.path.join(
|
||||
get_package_share_directory('agv_pro_gazebo'),
|
||||
'urdf',
|
||||
'agv_pro.urdf'
|
||||
)
|
||||
|
||||
with open(urdf_file, 'r') as file:
|
||||
robot_description_content = file.read()
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher'
|
||||
),
|
||||
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
parameters=[{'robot_description': robot_description_content}]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
arguments=['-d', rviz_config_dir],
|
||||
condition=IfCondition(use_rviz),
|
||||
output='screen')
|
||||
|
||||
])
|
||||
@@ -0,0 +1,65 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import Command
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_name = 'agv_pro_gazebo'
|
||||
pkg_dir = get_package_share_directory(pkg_name)
|
||||
xacro_file = os.path.join(pkg_dir, 'urdf', 'agv_pro.xacro')
|
||||
world_file = os.path.join(pkg_dir, 'worlds', 'empty.world')
|
||||
rviz_config = os.path.join(pkg_dir, 'rviz', 'agvpro_display.rviz')
|
||||
|
||||
robot_description_content = ParameterValue(
|
||||
Command(['xacro ', xacro_file]),
|
||||
value_type=str
|
||||
)
|
||||
robot_description = {'robot_description': robot_description_content}
|
||||
|
||||
return LaunchDescription([
|
||||
# Launch Gazebo
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
|
||||
),
|
||||
launch_arguments={'world': world_file}.items()
|
||||
),
|
||||
|
||||
# Spawn robot into Gazebo
|
||||
Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-topic', 'robot_description',
|
||||
'-entity', 'agv_pro'],
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# State publisher
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[robot_description]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher',
|
||||
output='screen',
|
||||
),
|
||||
|
||||
# Optional: RViz
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
arguments=['-d', rviz_config],
|
||||
),
|
||||
])
|
||||
@@ -0,0 +1,71 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, ExecuteProcess
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_name = 'agv_pro_description'
|
||||
|
||||
# Paths
|
||||
pkg_dir = get_package_share_directory(pkg_name)
|
||||
xacro_file = os.path.join(pkg_dir, 'urdf', 'agv_pro.xacro')
|
||||
world_file = os.path.join(pkg_dir, 'worlds', 'empty.world') # 创建一个空 world 即可
|
||||
rviz_config = os.path.join(pkg_dir, 'rviz', 'agvpro_display.rviz')
|
||||
|
||||
robot_description_content = Command(['xacro ', xacro_file])
|
||||
robot_description = {'robot_description': robot_description_content}
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Start Gazebo with empty world
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]
|
||||
),
|
||||
launch_arguments={'world': world_file}.items()
|
||||
),
|
||||
|
||||
# Spawn robot into Gazebo
|
||||
Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-topic', 'robot_description',
|
||||
'-entity', 'agv_pro'],
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# Robot state publisher
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[robot_description]
|
||||
),
|
||||
|
||||
# Optionally publish joint states if not using controllers
|
||||
Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher',
|
||||
output='screen',
|
||||
),
|
||||
Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['joint_state_broadcaster'],
|
||||
output='screen',
|
||||
),
|
||||
|
||||
# RViz (optional, visualize TF & model)
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
arguments=['-d', rviz_config],
|
||||
),
|
||||
])
|
||||
@@ -0,0 +1,17 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='teleop_twist_keyboard',
|
||||
executable='teleop_twist_keyboard',
|
||||
name='teleop_keyboard',
|
||||
output='screen',
|
||||
prefix='xterm -e', # 或 'gnome-terminal --' 替换为你的终端命令
|
||||
remappings=[
|
||||
('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped')
|
||||
]
|
||||
)
|
||||
])
|
||||
@@ -0,0 +1,35 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch.substitutions import Command
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_dir = get_package_share_directory('agv_pro_description')
|
||||
xacro_file = os.path.join(pkg_dir, 'urdf', 'minimal_robot.xacro')
|
||||
world_file = os.path.join(pkg_dir, 'worlds', 'empty.world')
|
||||
|
||||
robot_description = {'robot_description': Command(['xacro ', xacro_file])}
|
||||
|
||||
return LaunchDescription([
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
|
||||
),
|
||||
launch_arguments={'world': world_file}.items()
|
||||
),
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
parameters=[robot_description],
|
||||
output='screen'
|
||||
),
|
||||
Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-topic', 'robot_description', '-entity', 'minimal_bot'],
|
||||
output='screen'
|
||||
)
|
||||
])
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,36 @@
|
||||
<?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_gazebo</name>
|
||||
<version>1.0.0</version>
|
||||
<description>
|
||||
<p>URDF Description package for AGV pro</p>
|
||||
</description>
|
||||
<maintainer email="weijun.xie@elephantrobotics.com">weijun.xie</maintainer>
|
||||
<license>BSD-3-Clause license</license>
|
||||
<!-- Build tool -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>xacro</buildtool_depend>
|
||||
|
||||
<!-- Build dependencies -->
|
||||
<depend>gazebo_ros_pkgs</depend>
|
||||
<depend>robot_state_publisher</depend>
|
||||
<depend>joint_state_publisher</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>ros2_control</depend>
|
||||
<depend>controller_manager</depend>
|
||||
<depend>joint_state_broadcaster</depend>
|
||||
<depend>diff_drive_controller</depend>
|
||||
<depend>teleop_twist_keyboard</depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,234 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /RobotModel1
|
||||
- /TF1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 549
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz_common/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_footprint:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_front_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_rear_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_front_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_rear_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Mass Properties:
|
||||
Inertia: false
|
||||
Mass: false
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
laser_link:
|
||||
Value: true
|
||||
left_front_wheel_link:
|
||||
Value: true
|
||||
left_rear_wheel_link:
|
||||
Value: true
|
||||
right_front_wheel_link:
|
||||
Value: true
|
||||
right_rear_wheel_link:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
base_footprint:
|
||||
base_link:
|
||||
laser_link:
|
||||
{}
|
||||
left_front_wheel_link:
|
||||
{}
|
||||
left_rear_wheel_link:
|
||||
{}
|
||||
right_front_wheel_link:
|
||||
{}
|
||||
right_rear_wheel_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base_footprint
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 1.6701574325561523
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.785398006439209
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.785398006439209
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1200
|
||||
X: 720
|
||||
Y: 343
|
||||
@@ -0,0 +1,123 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="agv_pro">
|
||||
|
||||
<!-- Gazebo-specific properties -->
|
||||
<xacro:property name="wheel_damping" value="0.1"/>
|
||||
<xacro:property name="wheel_axis" value="0 1 0"/>
|
||||
|
||||
<!-- Base footprint -->
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 0.020" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
|
||||
<mass value="19.236" />
|
||||
<inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458" iyy="0.24191" iyz="0.0044629" izz="0.33755" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="file://$(find agv_pro_description)/meshes/base_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="file://$(find agv_pro_description)/meshes/base_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Gazebo plugin for control -->
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so"/>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/White</material>
|
||||
<mu1>1.0</mu1>
|
||||
<mu2>1.0</mu2>
|
||||
<kp>100000.0</kp>
|
||||
<kd>1.0</kd>
|
||||
</gazebo>
|
||||
|
||||
<!-- Include wheel macros -->
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro"/>
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/parts/gazebo_control_plugin.xacro"/>
|
||||
<!-- Add all four wheels using macro -->
|
||||
<xacro:wheel name="right_rear_wheel" mesh="file://$(find agv_pro_description)/meshes/wheel_rb_link.stl" origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
|
||||
|
||||
<xacro:wheel name="right_front_wheel" mesh="file://$(find agv_pro_description)/meshes/wheel_rf_link.stl" origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
|
||||
|
||||
<xacro:wheel name="left_front_wheel" mesh="file://$(find agv_pro_description)/meshes/wheel_lf_link.stl" origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0" mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07" iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>
|
||||
|
||||
<xacro:wheel name="left_rear_wheel" mesh="file://$(find agv_pro_description)/meshes/wheel_lb_link.stl" origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0" mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07" iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>
|
||||
|
||||
<!-- Lidar -->
|
||||
<link name="laser_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
|
||||
<mass value="0.049095" />
|
||||
<inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07" iyy="2.0483E-05" iyz="-4.2154E-09" izz="3.4612E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="file://$(find agv_pro_description)/meshes/laser_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="file://$(find agv_pro_description)/meshes/laser_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="laser_link" />
|
||||
</joint>
|
||||
|
||||
<!-- ros2_control tag -->
|
||||
<ros2_control name="AGVHardware" type="system">
|
||||
<hardware>
|
||||
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
||||
</hardware>
|
||||
|
||||
<joint name="right_rear_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_front_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_front_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_rear_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,92 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="agv_pro">
|
||||
|
||||
<!-- Define vehicle dimensions -->
|
||||
<xacro:property name="vehicle_width" value="0.36"/> <!-- 车辆宽度 -->
|
||||
<xacro:property name="wheel_radius" value="0.05"/> <!-- 轮子半径 -->
|
||||
|
||||
<!-- Gazebo-specific properties -->
|
||||
<xacro:property name="wheel_damping" value="0.1"/>
|
||||
<xacro:property name="wheel_axis" value="0 1 0"/>
|
||||
|
||||
<!-- Base footprint -->
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 0.020" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
|
||||
<mass value="19.236" />
|
||||
<inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458" iyy="0.24191" iyz="0.0044629" izz="0.33755" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="file://$(find agv_pro_gazebo)/meshes/base_link.stl" />
|
||||
</geometry>
|
||||
<material name=""/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="file://$(find agv_pro_gazebo)/meshes/base_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Include wheel macros & controller definitions -->
|
||||
<xacro:include filename="$(find agv_pro_gazebo)/urdf/parts/wheel_macro.xacro"/>
|
||||
|
||||
<!-- Wheels definition -->
|
||||
<!-- Right Rear Wheel -->
|
||||
<xacro:wheel name="right_rear_wheel" mesh="file://$(find agv_pro_gazebo)/meshes/wheel_rb_link.stl" origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
|
||||
|
||||
<!-- Right Front Wheel -->
|
||||
<xacro:wheel name="right_front_wheel" mesh="file://$(find agv_pro_gazebo)/meshes/wheel_rf_link.stl" origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
|
||||
|
||||
<!-- Left Front Wheel -->
|
||||
<xacro:wheel name="left_front_wheel" mesh="file://$(find agv_pro_gazebo)/meshes/wheel_lf_link.stl" origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0" mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07" iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>
|
||||
|
||||
<!-- Left Rear Wheel -->
|
||||
<xacro:wheel name="left_rear_wheel" mesh="file://$(find agv_pro_gazebo)/meshes/wheel_lb_link.stl" origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0" mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07" iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>
|
||||
|
||||
|
||||
<!-- Lidar definition -->
|
||||
<link name="laser_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
|
||||
<mass value="0.049095" />
|
||||
<inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07" iyy="2.0483E-05" iyz="-4.2154E-09" izz="3.4612E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="file://$(find agv_pro_gazebo)/meshes/laser_link.stl" />
|
||||
</geometry>
|
||||
<material name=""/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="file://$(find agv_pro_gazebo)/meshes/laser_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="laser_link" />
|
||||
</joint>
|
||||
|
||||
<!-- Include ros2_controller.xacro to define controllers -->
|
||||
<xacro:include filename="$(find agv_pro_gazebo)/urdf/parts/gazebo_control_plugin.xacro"/>
|
||||
<xacro:include filename="$(find agv_pro_gazebo)/urdf/ros2_controller.xacro"/>
|
||||
<xacro:ros2_controller/>
|
||||
<xacro:gazebo_control_plugin/>
|
||||
</robot>
|
||||
@@ -0,0 +1,144 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="agv_pro">
|
||||
|
||||
<xacro:property name="wheel_axis" value="0 1 0"/>
|
||||
<xacro:property name="wheel_damping" value="0.1"/>
|
||||
|
||||
<!-- Macro: wheel with Gazebo plugin -->
|
||||
<xacro:macro name="wheel" params="name mesh origin_xyz origin_rpy mass ixx ixy ixz iyy iyz izz">
|
||||
<link name="${name}_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
<material name=""><color rgba="1 1 1 1"/></material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="${name}_joint" type="continuous">
|
||||
<origin xyz="${origin_xyz}" rpy="${origin_rpy}"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="${name}_link"/>
|
||||
<axis xyz="${wheel_axis}"/>
|
||||
<dynamics damping="${wheel_damping}"/>
|
||||
</joint>
|
||||
|
||||
<transmission name="${name}_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<actuator name="${name}_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
<joint name="${name}_joint">
|
||||
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_link">
|
||||
<mu1>0.8</mu1>
|
||||
<mu2>0.8</mu2>
|
||||
<kp>100000.0</kp>
|
||||
<kd>1.0</kd>
|
||||
<material>Gazebo/Grey</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- Base links -->
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link"/>
|
||||
<origin xyz="0 0 0.020" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
|
||||
<mass value="19.236" />
|
||||
<inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458"
|
||||
iyy="0.24191" iyz="0.0044629" izz="0.33755"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/base_link.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Gazebo plugin for ros2_control -->
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so"/>
|
||||
</gazebo>
|
||||
|
||||
<!-- Wheels -->
|
||||
<xacro:wheel name="right_rear_wheel"
|
||||
mesh="package://agv_pro_description/meshes/wheel_rb_link.stl"
|
||||
origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0"
|
||||
mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
|
||||
iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
|
||||
|
||||
<xacro:wheel name="right_front_wheel"
|
||||
mesh="package://agv_pro_description/meshes/wheel_rf_link.stl"
|
||||
origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0"
|
||||
mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
|
||||
iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
|
||||
|
||||
<xacro:wheel name="left_front_wheel"
|
||||
mesh="package://agv_pro_description/meshes/wheel_lf_link.stl"
|
||||
origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0"
|
||||
mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07"
|
||||
iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>
|
||||
|
||||
<xacro:wheel name="left_rear_wheel"
|
||||
mesh="package://agv_pro_description/meshes/wheel_lb_link.stl"
|
||||
origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0"
|
||||
mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07"
|
||||
iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>
|
||||
|
||||
<!-- Lidar -->
|
||||
<link name="laser_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0"/>
|
||||
<mass value="0.049095"/>
|
||||
<inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07"
|
||||
iyy="2.0483E-05" iyz="-4.2154E-09" izz="3.4612E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/laser_link.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/laser_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<origin xyz="0.17891 0 0.20928" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="laser_link"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,91 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="agv_pro">
|
||||
|
||||
<!-- Gazebo-specific properties -->
|
||||
<xacro:property name="wheel_damping" value="0.1"/>
|
||||
<xacro:property name="wheel_axis" value="0 1 0"/>
|
||||
|
||||
<!-- Base footprint -->
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 0.020" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
|
||||
<mass value="19.236" />
|
||||
<inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458" iyy="0.24191" iyz="0.0044629" izz="0.33755" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="model://agv_pro_description/meshes/base_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="model://agv_pro_description/meshes/base_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/White</material>
|
||||
<mu1>1.0</mu1>
|
||||
<mu2>1.0</mu2>
|
||||
<kp>100000.0</kp>
|
||||
<kd>1.0</kd>
|
||||
</gazebo>
|
||||
|
||||
<!-- Include wheel macros & plugin -->
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro"/>
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/parts/gazebo_control_plugin.xacro"/>
|
||||
|
||||
<!-- All four wheels with corrected mesh paths -->
|
||||
<xacro:wheel name="right_rear_wheel" mesh="model://agv_pro_description/meshes/wheel_rb_link.stl" origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
|
||||
<xacro:wheel name="right_front_wheel" mesh="model://agv_pro_description/meshes/wheel_rf_link.stl" origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
|
||||
<xacro:wheel name="left_front_wheel" mesh="model://agv_pro_description/meshes/wheel_lf_link.stl" origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0" mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07" iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>
|
||||
<xacro:wheel name="left_rear_wheel" mesh="model://agv_pro_description/meshes/wheel_lb_link.stl" origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0" mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07" iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>
|
||||
|
||||
<!-- Lidar -->
|
||||
<link name="laser_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
|
||||
<mass value="0.049095" />
|
||||
<inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07" iyy="2.0483E-05" iyz="-4.2154E-09" izz="3.4612E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="model://agv_pro_description/meshes/laser_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="model://agv_pro_description/meshes/laser_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="laser_link" />
|
||||
</joint>
|
||||
|
||||
<!-- 插件调用 -->
|
||||
<xacro:gazebo_control_plugin/>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="gazebo_control_plugin">
|
||||
<gazebo>
|
||||
<!-- 使用全向控制插件 -->
|
||||
<plugin filename="libgazebo_ros_planar_move.so" name="mecanum_drive_controller">
|
||||
<ros>
|
||||
<remapping>cmd_vel:=/cmd_vel</remapping>
|
||||
<remapping>odom:=/odom</remapping>
|
||||
</ros>
|
||||
|
||||
<!-- 配置全向控制 -->
|
||||
<frontLeftJoint>front_left_wheel_joint</frontLeftJoint> <!-- 前左轮 -->
|
||||
<frontRightJoint>front_right_wheel_joint</frontRightJoint> <!-- 前右轮 -->
|
||||
<rearLeftJoint>rear_left_wheel_joint</rearLeftJoint> <!-- 后左轮 -->
|
||||
<rearRightJoint>rear_right_wheel_joint</rearRightJoint> <!-- 后右轮 -->
|
||||
<wheelDiameter>0.1</wheelDiameter> <!-- 轮子直径 -->
|
||||
<wheelSeparation>0.36</wheelSeparation> <!-- 轮距（车辆宽度） -->
|
||||
|
||||
<torque>20</torque> <!-- 轮子扭矩 -->
|
||||
<topicName>cmd_vel</topicName> <!-- 控制命令话题 -->
|
||||
<odometryFrame>odom</odometryFrame> <!-- 里程计坐标系 -->
|
||||
<odometryTopic>odom</odometryTopic> <!-- 里程计话题 -->
|
||||
<robotBaseFrame>base_footprint</robotBaseFrame> <!-- 机器人基础坐标系 -->
|
||||
<publishOdomTF>true</publishOdomTF> <!-- 发布里程计变换 -->
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,58 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="wheel" params="name mesh origin_xyz origin_rpy mass ixx ixy ixz iyy iyz izz">
|
||||
|
||||
<link name="${name}_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
<material name="gray">
|
||||
<color rgba="0.3 0.3 0.3 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_joint" type="continuous">
|
||||
<origin xyz="${origin_xyz}" rpy="${origin_rpy}" />
|
||||
<parent link="base_link" />
|
||||
<child link="${name}_link" />
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1"/>
|
||||
</joint>
|
||||
|
||||
<!-- Correct transmission for ROS2 -->
|
||||
<transmission name="${name}_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="${name}_joint">
|
||||
<hardwareInterface>hardware_interface/velocity</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="${name}_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>hardware_interface/velocity</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_link">
|
||||
<mu1>0.8</mu1>
|
||||
<mu2>0.8</mu2>
|
||||
<kp>100000.0</kp>
|
||||
<kd>1.0</kd>
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,56 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="ros2_controller">
|
||||
<ros2_control name="FishBotGazeboSystem" type="system">
|
||||
<hardware>
|
||||
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
||||
</hardware>
|
||||
|
||||
<!-- 配置所有轮子的控制接口 -->
|
||||
<joint name="front_left_wheel_joint">
|
||||
<command_interface name="position" />
|
||||
<command_interface name="velocity" />
|
||||
<command_interface name="effort" />
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
<state_interface name="effort" />
|
||||
</joint>
|
||||
|
||||
<joint name="front_right_wheel_joint">
|
||||
<command_interface name="position" />
|
||||
<command_interface name="velocity" />
|
||||
<command_interface name="effort" />
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
<state_interface name="effort" />
|
||||
</joint>
|
||||
|
||||
<joint name="rear_left_wheel_joint">
|
||||
<command_interface name="position" />
|
||||
<command_interface name="velocity" />
|
||||
<command_interface name="effort" />
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
<state_interface name="effort" />
|
||||
</joint>
|
||||
|
||||
<joint name="rear_right_wheel_joint">
|
||||
<command_interface name="position" />
|
||||
<command_interface name="velocity" />
|
||||
<command_interface name="effort" />
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
<state_interface name="effort" />
|
||||
</joint>
|
||||
</ros2_control>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
|
||||
<parameters>$(find agv_pro_gazebo)/config/agv_control.yaml</parameters>
|
||||
<ros>
|
||||
<remapping>/omni_drive_controller/cmd_vel:=/cmd_vel</remapping>
|
||||
<remapping>/omni_drive_controller/odom:=/odom</remapping>
|
||||
</ros>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,11 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.6">
|
||||
<world name="empty_world">
|
||||
<include>
|
||||
<uri>model://ground_plane</uri>
|
||||
</include>
|
||||
<include>
|
||||
<uri>model://sun</uri>
|
||||
</include>
|
||||
</world>
|
||||
</sdf>
|
||||
@@ -0,0 +1,37 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(agv_pro_msgs)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/AGVProStatus.msg"
|
||||
"srv/SetDigitalOutput.srv"
|
||||
"srv/GetDigitalInput.srv"
|
||||
"srv/SetLedColor.srv"
|
||||
"srv/SetLedMode.srv"
|
||||
DEPENDENCIES std_msgs
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,6 @@
|
||||
std_msgs/Header header
|
||||
|
||||
uint8 motor_status
|
||||
uint8 motor_error
|
||||
float64 battery_voltage
|
||||
uint8 enable_status
|
||||
@@ -0,0 +1,23 @@
|
||||
<?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_msgs</name>
|
||||
<version>1.0.8</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>std_msgs</depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,133 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy
|
||||
import time
|
||||
from rclpy.node import Node
|
||||
|
||||
from agv_pro_msgs.srv import (
|
||||
SetDigitalOutput,
|
||||
GetDigitalInput,
|
||||
SetLedColor,
|
||||
SetLedMode
|
||||
)
|
||||
|
||||
class AGVIOClient(Node):
|
||||
def __init__(self):
|
||||
super().__init__('agv_io_client')
|
||||
|
||||
# Create service client
|
||||
self.cli_set_io = self.create_client(SetDigitalOutput, 'set_digital_output')
|
||||
self.cli_get_io = self.create_client(GetDigitalInput, 'get_digital_input')
|
||||
self.cli_led_output = self.create_client(SetLedColor, 'set_led_color')
|
||||
self.cli_led_mode = self.create_client(SetLedMode, 'set_led_mode')
|
||||
|
||||
# Wait until all services are available
|
||||
self._wait_for_services()
|
||||
|
||||
def _wait_for_services(self):
|
||||
"""Wait for all services to become available."""
|
||||
clients = [
|
||||
self.cli_set_io,
|
||||
self.cli_get_io,
|
||||
self.cli_led_output,
|
||||
self.cli_led_mode
|
||||
]
|
||||
|
||||
for cli in clients:
|
||||
while not cli.wait_for_service(timeout_sec=1.0):
|
||||
pass
|
||||
|
||||
def _call_service(self, client, request):
|
||||
future = client.call_async(request)
|
||||
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
|
||||
|
||||
if future.result() is not None:
|
||||
return future.result()
|
||||
else:
|
||||
self.get_logger().error(f'Service call failed: {future.exception()}')
|
||||
return None
|
||||
|
||||
# ------------------------------
|
||||
# Set digital output
|
||||
# ------------------------------
|
||||
def set_digital_output(self, pin: int, state: int) -> bool:
|
||||
"""Set digital output pin state."""
|
||||
req = SetDigitalOutput.Request()
|
||||
req.pin = pin
|
||||
req.state = state
|
||||
|
||||
res = self._call_service(self.cli_set_io, req)
|
||||
return res.success if res else False
|
||||
|
||||
# ------------------------------
|
||||
# Get digital input
|
||||
# ------------------------------
|
||||
def get_digital_input(self, pin: int):
|
||||
"""Read digital input pin state."""
|
||||
req = GetDigitalInput.Request()
|
||||
req.pin = pin
|
||||
|
||||
res = self._call_service(self.cli_get_io, req)
|
||||
return res.state if (res and res.success) else None
|
||||
|
||||
# ------------------------------
|
||||
# Set LED color
|
||||
# ------------------------------
|
||||
def set_led_color(self,
|
||||
position: int,
|
||||
brightness: int,
|
||||
r: int,
|
||||
g: int,
|
||||
b: int) -> bool:
|
||||
"""Set LED RGB color and brightness."""
|
||||
req = SetLedColor.Request()
|
||||
req.position = position
|
||||
req.brightness = brightness
|
||||
req.r = r
|
||||
req.g = g
|
||||
req.b = b
|
||||
|
||||
res = self._call_service(self.cli_led_output, req)
|
||||
return res.success if res else False
|
||||
|
||||
# ------------------------------
|
||||
# Set LED mode
|
||||
# ------------------------------
|
||||
def set_led_mode(self, mode: bool) -> bool:
|
||||
"""Set LED mode (True/False)."""
|
||||
req = SetLedMode.Request()
|
||||
req.mode = mode
|
||||
|
||||
res = self._call_service(self.cli_led_mode, req)
|
||||
return res.success if res else False
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
client = AGVIOClient()
|
||||
|
||||
################
|
||||
# Example usage:
|
||||
################
|
||||
|
||||
# client.set_digital_output(pin=1, state=1)
|
||||
# client.get_digital_input(pin=2)
|
||||
|
||||
# client.set_led_mode(True)
|
||||
# for i in range(10):
|
||||
# client.set_led_color(0, 100, 255, 255, 0)
|
||||
# client.set_led_color(1, 100, 255, 255, 0)
|
||||
# time.sleep(0.5)
|
||||
# client.set_led_color(0, 0, 0, 0, 0)
|
||||
# client.set_led_color(1, 0, 0, 0, 0)
|
||||
# time.sleep(0.5)
|
||||
|
||||
# client.set_led_color(0, 100, 255, 255, 0)
|
||||
# client.set_led_color(1, 100, 255, 255, 0)
|
||||
# client.set_led_mode(True)
|
||||
|
||||
client.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,5 @@
|
||||
int32 pin
|
||||
---
|
||||
bool success
|
||||
int32 state
|
||||
string message
|
||||
@@ -0,0 +1,5 @@
|
||||
int32 pin
|
||||
int32 state
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,8 @@
|
||||
int32 position
|
||||
int32 brightness
|
||||
int32 r
|
||||
int32 g
|
||||
int32 b
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,4 @@
|
||||
bool mode
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,31 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(agv_pro_navigation2)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
install(
|
||||
DIRECTORY launch map param rviz scripts
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,63 @@
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
use_rviz = LaunchConfiguration('use_rviz', default='true')
|
||||
map_dir = LaunchConfiguration(
|
||||
'map',
|
||||
default=os.path.join(
|
||||
get_package_share_directory('agv_pro_navigation2'),
|
||||
'map',
|
||||
'map.yaml'))
|
||||
|
||||
param_file_name = 'agvpro.yaml'
|
||||
param_dir = LaunchConfiguration(
|
||||
'params_file',
|
||||
default=os.path.join(
|
||||
get_package_share_directory('agv_pro_navigation2'),
|
||||
'param',
|
||||
param_file_name))
|
||||
|
||||
nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
|
||||
|
||||
rviz_config_dir = os.path.join(
|
||||
get_package_share_directory('agv_pro_navigation2'),
|
||||
'rviz',
|
||||
'agvpro_navigation2.rviz')
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'map',
|
||||
default_value=map_dir,
|
||||
description='Full path to map file to load'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=param_dir,
|
||||
description='Full path to param file to load'),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
|
||||
launch_arguments={
|
||||
'map': map_dir,
|
||||
'params_file': param_dir}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
arguments=['-d', rviz_config_dir],
|
||||
parameters=[{'use_sim_time': use_sim_time}],
|
||||
condition=IfCondition(use_rviz),
|
||||
output='screen'),
|
||||
])
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,7 @@
|
||||
image: map.pgm
|
||||
mode: trinary
|
||||
resolution: 0.05
|
||||
origin: [-10, -24.4, 0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.25
|
||||
@@ -0,0 +1,19 @@
|
||||
<?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_navigation2</name>
|
||||
<version>1.0.8</version>
|
||||
<description>ROS2 launch scripts for navigation2</description>
|
||||
<maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
|
||||
<license>BSD-3-Clause license</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<exec_depend>nav2_bringup</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,361 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
alpha1: 0.4
|
||||
alpha2: 0.3
|
||||
alpha3: 0.1
|
||||
alpha4: 0.1
|
||||
alpha5: 0.04
|
||||
base_frame_id: "base_footprint"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 2
|
||||
robot_model_type: "nav2_amcl::OmniMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.02
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 0.3
|
||||
update_min_a: 0.06
|
||||
update_min_d: 0.025
|
||||
z_hit: 0.7
|
||||
z_max: 0.001
|
||||
z_rand: 0.059
|
||||
z_short: 0.24
|
||||
|
||||
# Initial Pose
|
||||
set_initial_pose: True
|
||||
initial_pose.x: 0.0
|
||||
initial_pose.y: 0.0
|
||||
initial_pose.z: 0.0
|
||||
initial_pose.yaw: 0.0
|
||||
|
||||
amcl_map_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
amcl_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
enable_groot_monitoring: True
|
||||
groot_zmq_publisher_port: 1666
|
||||
groot_zmq_server_port: 1667
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.15
|
||||
movement_time_allowance: 8.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 0.5
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 40
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.1
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.1
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 23.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 18.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]"
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 4.0
|
||||
inflation_radius: 0.30
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
use_sim_time: False
|
||||
footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 4.0
|
||||
inflation_radius: 0.30
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
# Overridden in launch by the "map" launch configuration or provided default value.
|
||||
# To use in yaml, remove the default "map" value in the navigation2_active.launch.py file & provide full path to map below.
|
||||
yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 1.0
|
||||
use_sim_time: False
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 2.0
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
recoveries_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
recovery_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_recoveries/Spin"
|
||||
backup:
|
||||
plugin: "nav2_recoveries/BackUp"
|
||||
wait:
|
||||
plugin: "nav2_recoveries/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
transform_timeout: 0.1
|
||||
use_sim_time: False
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 0.5]
|
||||
min_velocity: [-0.26, 0.0, -0.5]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,592 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /TF1/Frames1
|
||||
- /TF1/Tree1
|
||||
Splitter Ratio: 0.5833333134651184
|
||||
Tree Height: 462
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: nav2_rviz_plugins/Navigation 2
|
||||
Name: Navigation 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_footprint:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: false
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
camera_link:
|
||||
Value: true
|
||||
imu_link:
|
||||
Value: true
|
||||
laser_frame:
|
||||
Value: true
|
||||
map:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
map:
|
||||
odom:
|
||||
base_footprint:
|
||||
base_link:
|
||||
{}
|
||||
camera_link:
|
||||
{}
|
||||
imu_link:
|
||||
{}
|
||||
laser_frame:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/LaserScan
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 1016
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 1008
|
||||
Name: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Best Effort
|
||||
Value: /scan
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: ""
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: Bumper Hit
|
||||
Position Transformer: ""
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.07999999821186066
|
||||
Style: Spheres
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Best Effort
|
||||
Value: /mobile_base/sensors/bumper_pointcloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: true
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic:
|
||||
Depth: 1
|
||||
Durability Policy: Transient Local
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /map
|
||||
Update Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /map_updates
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: nav2_rviz_plugins/ParticleCloud
|
||||
Color: 0; 180; 0
|
||||
Enabled: true
|
||||
Max Arrow Length: 0.30000001192092896
|
||||
Min Arrow Length: 0.019999999552965164
|
||||
Name: Amcl Particle Swarm
|
||||
Shape: Arrow (Flat)
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Best Effort
|
||||
Value: /particle_cloud
|
||||
Value: true
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Alpha: 0.30000001192092896
|
||||
Class: rviz_default_plugins/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Global Costmap
|
||||
Topic:
|
||||
Depth: 1
|
||||
Durability Policy: Transient Local
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /global_costmap/costmap
|
||||
Update Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /global_costmap/costmap_updates
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
- Alpha: 0.30000001192092896
|
||||
Class: rviz_default_plugins/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Downsampled Costmap
|
||||
Topic:
|
||||
Depth: 1
|
||||
Durability Policy: Transient Local
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /downsampled_costmap
|
||||
Update Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /downsampled_costmap_updates
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz_default_plugins/Path
|
||||
Color: 255; 0; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.019999999552965164
|
||||
Head Length: 0.019999999552965164
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: Arrows
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.004999999888241291
|
||||
Shaft Length: 0.019999999552965164
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /plan
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 125; 125; 125
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: VoxelGrid
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.05000000074505806
|
||||
Style: Boxes
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /global_costmap/voxel_marked_cloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/Polygon
|
||||
Color: 25; 255; 0
|
||||
Enabled: false
|
||||
Name: Polygon
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /global_costmap/published_footprint
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: Global Planner
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Alpha: 0.699999988079071
|
||||
Class: rviz_default_plugins/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Local Costmap
|
||||
Topic:
|
||||
Depth: 1
|
||||
Durability Policy: Transient Local
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /local_costmap/costmap
|
||||
Update Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /local_costmap/costmap_updates
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz_default_plugins/Path
|
||||
Color: 0; 12; 255
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Local Plan
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /local_plan
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: false
|
||||
Name: Trajectories
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /marker
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/Polygon
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Name: Polygon
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /local_costmap/published_footprint
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: VoxelGrid
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /local_costmap/voxel_marked_cloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: Controller
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: RealsenseCamera
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /intel_realsense_r200_depth/image_raw
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: RealsenseDepthImage
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /intel_realsense_r200_depth/points
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: false
|
||||
Name: Realsense
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: true
|
||||
Name: MarkerArray
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /waypoints
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: map
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
- Class: nav2_rviz_plugins/GoalTool
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Angle: -1.6150002479553223
|
||||
Class: rviz_default_plugins/TopDownOrtho
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Scale: 133.21060180664062
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: TopDownOrtho (rviz_default_plugins)
|
||||
X: 1.462070345878601
|
||||
Y: 0.5454937219619751
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 932
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
Navigation 2:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000020b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032010000024e000001390000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
RealsenseCamera:
|
||||
collapsed: false
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1545
|
||||
X: 291
|
||||
Y: 68
|
||||
@@ -0,0 +1,101 @@
|
||||
#! /usr/bin/env python3
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
|
||||
"""
|
||||
Basic navigation demo to go to poses.
|
||||
"""
|
||||
|
||||
def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
|
||||
"""
|
||||
Set the initial pose of the robot for AMCL localization.
|
||||
|
||||
Args:
|
||||
navigator (BasicNavigator): The navigator instance controlling the robot.
|
||||
x (float): Initial X position in the map frame.
|
||||
y (float): Initial Y position in the map frame.
|
||||
oz (float): Orientation Z component (quaternion).
|
||||
ow (float): Orientation W component (quaternion).
|
||||
"""
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = x
|
||||
initial_pose.pose.position.y = y
|
||||
initial_pose.pose.orientation.z = oz
|
||||
initial_pose.pose.orientation.w = ow
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
def create_pose(navigator: BasicNavigator, x, y, z, w):
|
||||
pose = PoseStamped()
|
||||
pose.header.frame_id = 'map'
|
||||
pose.header.stamp = navigator.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(navigator: BasicNavigator, goal_poses, verbose: bool = False) -> bool:
|
||||
|
||||
nav_start = navigator.get_clock().now()
|
||||
navigator.goThroughPoses(goal_poses)
|
||||
|
||||
while not navigator.isTaskComplete():
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and verbose:
|
||||
remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
|
||||
print(f"Estimated time of arrival: {remaining:.0f} seconds")
|
||||
|
||||
# Do something depending on the return code
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Goal succeeded!')
|
||||
return True
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Goal was canceled!')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Goal failed!')
|
||||
else:
|
||||
print('Goal has an invalid return status!')
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set robot initial pose
|
||||
# set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)\
|
||||
|
||||
# Wait for navigation to fully activate, since autostarting nav2
|
||||
# navigator.waitUntilNav2Active()
|
||||
|
||||
way1_goals = [
|
||||
[0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
|
||||
[1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
|
||||
[1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
|
||||
[-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
|
||||
]
|
||||
way2_goals = [
|
||||
[0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
|
||||
[1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
|
||||
[1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
|
||||
[-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
|
||||
]
|
||||
|
||||
goal_poses_1 = [create_pose(navigator, *g) for g in way1_goals]
|
||||
goal_poses_2 = [create_pose(navigator, *g) for g in way2_goals]
|
||||
|
||||
result1 = nav_through_pose(navigator, goal_poses_1, verbose=True)
|
||||
print(f"First segment navigation result: {result1}")
|
||||
|
||||
if result1 ==True:
|
||||
result2 = nav_through_pose(navigator, goal_poses_2, verbose=True)
|
||||
print(f"Second segment navigation result: {result2}")
|
||||
|
||||
rclpy.shutdown()
|
||||
@@ -0,0 +1,97 @@
|
||||
#! /usr/bin/env python3
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
"""
|
||||
Basic navigation demo to go to pose.
|
||||
"""
|
||||
|
||||
def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
|
||||
"""
|
||||
Set the initial pose of the robot for AMCL localization.
|
||||
|
||||
Args:
|
||||
navigator (BasicNavigator): The navigator instance controlling the robot.
|
||||
x (float): Initial X position in the map frame.
|
||||
y (float): Initial Y position in the map frame.
|
||||
oz (float): Orientation Z component (quaternion).
|
||||
ow (float): Orientation W component (quaternion).
|
||||
"""
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = x
|
||||
initial_pose.pose.position.y = y
|
||||
initial_pose.pose.orientation.z = oz
|
||||
initial_pose.pose.orientation.w = ow
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
|
||||
def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float, verbose: bool = False) -> bool:
|
||||
"""
|
||||
Navigate the robot to a target goal pose.
|
||||
|
||||
Args:
|
||||
navigator (BasicNavigator): The navigator instance controlling the robot.
|
||||
x (float): Goal X position in the map frame.
|
||||
y (float): Goal Y position in the map frame.
|
||||
oz (float): Orientation Z component (quaternion).
|
||||
ow (float): Orientation W component (quaternion).
|
||||
verbose (bool, optional): If True, prints navigation feedback such as estimated arrival time. Default is False.
|
||||
|
||||
Returns:
|
||||
bool: True if navigation succeeded, False otherwise.
|
||||
"""
|
||||
goal_pose = PoseStamped()
|
||||
goal_pose.header.frame_id = 'map'
|
||||
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose.pose.position.x = x
|
||||
goal_pose.pose.position.y = y
|
||||
goal_pose.pose.orientation.z = oz
|
||||
goal_pose.pose.orientation.w = ow
|
||||
|
||||
navigator.goToPose(goal_pose)
|
||||
|
||||
while not navigator.isTaskComplete():
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and verbose:
|
||||
remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
|
||||
print(f"Estimated time of arrival: {remaining:.0f} seconds")
|
||||
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Goal succeeded!')
|
||||
return True
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Goal was canceled!')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Goal failed!')
|
||||
else:
|
||||
print('Goal has an invalid return status!')
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
rclpy.init()
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set robot initial pose
|
||||
# set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)
|
||||
|
||||
# Wait for navigation to fully activate, since autostarting nav2
|
||||
# navigator.waitUntilNav2Active()
|
||||
|
||||
goal_A = [1.6766083240509033,0.37930558800697327,-0.03491306994337919, 0.9993903529387947]
|
||||
goal_B = [-0.5062443017959595,1.559376835823059,0.6869307039904945,0.7267229237578264]
|
||||
|
||||
x_goal, y_goal, orientation_z, orientation_w = goal_A
|
||||
success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
|
||||
print("Navigation result:", success)
|
||||
|
||||
x_goal, y_goal, orientation_z, orientation_w = goal_B
|
||||
success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
|
||||
print("Navigation result:", success)
|
||||
|
||||
rclpy.shutdown()
|
||||
@@ -0,0 +1,108 @@
|
||||
#! /usr/bin/env python3
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
"""
|
||||
Basic navigation demo to go to poses.
|
||||
"""
|
||||
|
||||
def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
|
||||
"""
|
||||
Set the initial pose of the robot for AMCL localization.
|
||||
|
||||
Args:
|
||||
navigator (BasicNavigator): The navigator instance controlling the robot.
|
||||
x (float): Initial X position in the map frame.
|
||||
y (float): Initial Y position in the map frame.
|
||||
oz (float): Orientation Z component (quaternion).
|
||||
ow (float): Orientation W component (quaternion).
|
||||
"""
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = x
|
||||
initial_pose.pose.position.y = y
|
||||
initial_pose.pose.orientation.z = oz
|
||||
initial_pose.pose.orientation.w = ow
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
def create_pose(navigator: BasicNavigator, x, y, z, w):
|
||||
pose = PoseStamped()
|
||||
pose.header.frame_id = 'map'
|
||||
pose.header.stamp = navigator.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_waypoint_follower(navigator: BasicNavigator, goal_poses, verbose: bool = False) -> bool:
|
||||
|
||||
nav_start = navigator.get_clock().now()
|
||||
navigator.followWaypoints(goal_poses)
|
||||
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
# Do something with the feedback
|
||||
i = i + 1
|
||||
feedback = navigator.getFeedback()
|
||||
if (feedback and i % 5) and verbose == 0:
|
||||
print('Executing current waypoint: ' +
|
||||
str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses)))
|
||||
now = navigator.get_clock().now()
|
||||
|
||||
# Some navigation timeout to demo cancellation
|
||||
if now - nav_start > Duration(seconds=600.0):
|
||||
navigator.cancelTask()
|
||||
|
||||
# Do something depending on the return code
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Goal succeeded!')
|
||||
return True
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Goal was canceled!')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Goal failed!')
|
||||
else:
|
||||
print('Goal has an invalid return status!')
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set robot initial pose
|
||||
# set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)\
|
||||
|
||||
# Wait for navigation to fully activate, since autostarting nav2
|
||||
# navigator.waitUntilNav2Active()
|
||||
|
||||
way1_goals = [
|
||||
[0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
|
||||
[1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
|
||||
[1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
|
||||
[-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
|
||||
]
|
||||
way2_goals = [
|
||||
[0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
|
||||
[1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
|
||||
[1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
|
||||
[-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
|
||||
]
|
||||
|
||||
goal_poses_1 = [create_pose(navigator, *g) for g in way1_goals]
|
||||
goal_poses_2 = [create_pose(navigator, *g) for g in way2_goals]
|
||||
|
||||
result1 = nav_waypoint_follower(navigator, goal_poses_1, verbose=False)
|
||||
print(f"First segment navigation result: {result1}")
|
||||
|
||||
if result1 ==True:
|
||||
result2 = nav_waypoint_follower(navigator, goal_poses_2, verbose=False)
|
||||
print(f"Second segment navigation result: {result2}")
|
||||
|
||||
rclpy.shutdown()
|
||||
@@ -0,0 +1,103 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(agv_pro_rviz_plugins)
|
||||
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html
|
||||
set(CMAKE_INCLUDE_CURRENT_DIR ON)
|
||||
set(CMAKE_AUTOMOC ON)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rviz_common REQUIRED)
|
||||
find_package(rviz_default_plugins REQUIRED)
|
||||
find_package(rviz_ogre_vendor REQUIRED)
|
||||
find_package(rviz_rendering REQUIRED)
|
||||
|
||||
set(agv_pro_rviz_plugins_headers_to_moc
|
||||
include/agv_pro_rviz_plugins/charger_tool.hpp
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(library_name ${PROJECT_NAME})
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/charger_tool.cpp
|
||||
${agv_pro_rviz_plugins_headers_to_moc}
|
||||
)
|
||||
|
||||
set(dependencies
|
||||
geometry_msgs
|
||||
pluginlib
|
||||
Qt5
|
||||
rclcpp
|
||||
rviz_common
|
||||
rviz_default_plugins
|
||||
rviz_ogre_vendor
|
||||
rviz_rendering
|
||||
)
|
||||
|
||||
ament_target_dependencies(${library_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_include_directories(${library_name} PUBLIC
|
||||
${Qt5Widgets_INCLUDE_DIRS}
|
||||
${OGRE_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
target_link_libraries(${library_name}
|
||||
rviz_common::rviz_common
|
||||
)
|
||||
|
||||
# Causes the visibility macros to use dllexport rather than dllimport,
|
||||
# which is appropriate when building the dll but not consuming it.
|
||||
target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
|
||||
|
||||
pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
|
||||
|
||||
install(
|
||||
TARGETS ${library_name}
|
||||
EXPORT ${library_name}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
INCLUDES DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_targets(${library_name} HAS_LIBRARY_TARGET)
|
||||
ament_export_dependencies(
|
||||
Qt5
|
||||
geometry_msgs
|
||||
pluginlib
|
||||
rclcpp
|
||||
rviz_common
|
||||
rviz_default_plugins
|
||||
rviz_ogre_vendor
|
||||
rviz_rendering
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+69
@@ -0,0 +1,69 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef AGV_PRO_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
|
||||
#define AGV_PRO_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
|
||||
|
||||
#include <QObject>
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "rclcpp/node.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include <memory>
|
||||
|
||||
#include "rviz_default_plugins/tools/pose/pose_tool.hpp"
|
||||
#include "rviz_default_plugins/visibility_control.hpp"
|
||||
|
||||
namespace rviz_common
|
||||
{
|
||||
|
||||
class DisplayContext;
|
||||
|
||||
namespace properties
|
||||
{
|
||||
class StringProperty;
|
||||
class QosProfileProperty;
|
||||
} // namespace properties
|
||||
} // namespace rviz_common
|
||||
|
||||
namespace agv_pro_rviz_plugins
|
||||
{
|
||||
|
||||
class RVIZ_DEFAULT_PLUGINS_PUBLIC ChargerTool : public rviz_default_plugins::tools::PoseTool
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
ChargerTool();
|
||||
~ChargerTool() override;
|
||||
|
||||
void onInitialize() override;
|
||||
|
||||
protected:
|
||||
void onPoseSet(double x, double y, double theta) override;
|
||||
|
||||
private Q_SLOTS:
|
||||
void updateTopic();
|
||||
|
||||
private:
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_;
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
rviz_common::properties::StringProperty * topic_property_;
|
||||
rviz_common::properties::QosProfileProperty * qos_profile_property_;
|
||||
|
||||
rclcpp::QoS qos_profile_;
|
||||
};
|
||||
|
||||
} // namespace agv_pro_rviz_plugins
|
||||
|
||||
#endif // AGV_PRO_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
|
||||
@@ -0,0 +1,33 @@
|
||||
<?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_rviz_plugins</name>
|
||||
<version>1.0.0</version>
|
||||
<description>AGV Pro RViz plugins (charger tool), extracted to keep navigation2 upstream unmodified</description>
|
||||
<maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>qtbase5-dev</build_depend>
|
||||
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>resource_retriever</depend>
|
||||
<depend>rviz_common</depend>
|
||||
<depend>rviz_default_plugins</depend>
|
||||
<depend>rviz_ogre_vendor</depend>
|
||||
<depend>rviz_rendering</depend>
|
||||
|
||||
<exec_depend>libqt5-core</exec_depend>
|
||||
<exec_depend>libqt5-gui</exec_depend>
|
||||
<exec_depend>libqt5-opengl</exec_depend>
|
||||
<exec_depend>libqt5-widgets</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,9 @@
|
||||
<library path="agv_pro_rviz_plugins">
|
||||
|
||||
<class name="agv_pro_rviz_plugins/ChargerTool"
|
||||
type="agv_pro_rviz_plugins::ChargerTool"
|
||||
base_class_type="rviz_common::Tool">
|
||||
<description>A tool used to specify the charging stations goal pose.</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
@@ -0,0 +1,87 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "agv_pro_rviz_plugins/charger_tool.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rviz_common/display_context.hpp"
|
||||
#include "rviz_common/load_resource.hpp"
|
||||
#include "rviz_common/properties/string_property.hpp"
|
||||
#include "rviz_common/properties/qos_profile_property.hpp"
|
||||
|
||||
namespace agv_pro_rviz_plugins
|
||||
{
|
||||
|
||||
ChargerTool::ChargerTool()
|
||||
: rviz_default_plugins::tools::PoseTool(), qos_profile_(5)
|
||||
{
|
||||
shortcut_key_ = 'c';
|
||||
topic_property_ = new rviz_common::properties::StringProperty(
|
||||
"Topic", "charger_position_update",
|
||||
"The topic on which to publish goals.",
|
||||
getPropertyContainer(), SLOT(updateTopic()), this);
|
||||
|
||||
qos_profile_property_ = new rviz_common::properties::QosProfileProperty(
|
||||
topic_property_, qos_profile_);
|
||||
}
|
||||
|
||||
ChargerTool::~ChargerTool()
|
||||
{
|
||||
}
|
||||
|
||||
void ChargerTool::onInitialize()
|
||||
{
|
||||
PoseTool::onInitialize();
|
||||
setName("Charger Update");
|
||||
setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/SetGoal.png"));
|
||||
updateTopic();
|
||||
}
|
||||
|
||||
void ChargerTool::updateTopic()
|
||||
{
|
||||
rclcpp::Node::SharedPtr raw_node =
|
||||
context_->getRosNodeAbstraction().lock()->get_raw_node();
|
||||
// TODO(anhosi, wjwwood): replace with abstraction for publishers once available
|
||||
publisher_ = raw_node->
|
||||
template create_publisher<geometry_msgs::msg::PoseStamped>(
|
||||
topic_property_->getStdString(), qos_profile_);
|
||||
clock_ = raw_node->get_clock();
|
||||
}
|
||||
|
||||
void
|
||||
ChargerTool::onPoseSet(double x, double y, double theta)
|
||||
{
|
||||
std::string fixed_frame = context_->getFixedFrame().toStdString();
|
||||
|
||||
geometry_msgs::msg::PoseStamped goal;
|
||||
goal.header.stamp = clock_->now();
|
||||
goal.header.frame_id = fixed_frame;
|
||||
|
||||
goal.pose.position.x = x;
|
||||
goal.pose.position.y = y;
|
||||
goal.pose.position.z = 0.0;
|
||||
|
||||
goal.pose.orientation = orientationAroundZAxis(theta);
|
||||
|
||||
logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame);
|
||||
|
||||
publisher_->publish(goal);
|
||||
}
|
||||
|
||||
} // namespace agv_pro_rviz_plugins
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp> // NOLINT
|
||||
PLUGINLIB_EXPORT_CLASS(agv_pro_rviz_plugins::ChargerTool, rviz_common::Tool)
|
||||
Reference in New Issue
Block a user