diff --git a/agv_pro_autocharge/README.md b/agv_pro_autocharge/README.md
new file mode 100755
index 0000000..dbcb04e
--- /dev/null
+++ b/agv_pro_autocharge/README.md
@@ -0,0 +1,134 @@
+# AGV AutoCharge ROS2 Package
+
+这是一个为ROS2 Humble设计的AGV自动充电系统软件包。
+
+## 功能特性
+
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 键盘交互启动导航功能
+- 导航成功后启动串口控制
+- 支持充电状态检测和控制
+
+## 安装依赖
+
+确保您的系统已安装以下依赖：
+
+```bash
+# ROS2 Humble基础包
+sudo apt install ros-humble-rclpy
+sudo apt install ros-humble-geometry-msgs
+sudo apt install ros-humble-std-msgs
+sudo apt install ros-humble-nav-msgs
+sudo apt install ros-humble-visualization-msgs
+sudo apt install ros-humble-nav2-simple-commander
+
+# Python依赖
+pip3 install pyserial
+```
+
+## 编译安装
+
+```bash
+# 进入ROS2工作空间
+cd /path/to/your/ros2_ws/src
+
+# 复制软件包到工作空间
+cp -r agv_autocharge_ros2 .
+
+# 编译软件包
+cd ..
+colcon build --packages-select agv_autocharge_ros2
+
+# 加载环境变量
+source install/setup.bash
+```
+
+## 使用方法
+
+### 启动节点
+
+```bash
+# 启动自动充电控制器节点
+ros2 run agv_autocharge_ros2 combined_auto_recharger
+```
+
+### 节点功能
+
+- 监听 `/charger_position_update` 话题，接收充电桩位置更新
+- 发布 `/goal_marker` 话题，在RViz中显示充电桩位置标记
+- 发布 `/cmd_vel` 话题，控制机器人运动
+- 按键 `q` 启动导航到充电桩
+- 导航成功后自动启动串口控制
+
+### 配置文件
+
+充电桩位置配置文件位于：
+```
+config/charger_position.json
+```
+
+文件格式：
+```json
+{
+ "p_x": 1.329015495451769,
+ "p_y": 0.31961151635354823,
+ "orien_z": 0.4981823289472456,
+ "orien_w": 0.8670722963655905
+}
+```
+
+### 话题接口
+
+#### 订阅话题
+- `/charger_position_update` (geometry_msgs/PoseStamped): 充电桩位置更新
+
+#### 发布话题
+- `/goal_marker` (visualization_msgs/MarkerArray): 充电桩位置可视化标记
+- `/cmd_vel` (geometry_msgs/Twist): 机器人运动控制
+- `/chassis_security` (std_msgs/Int8): 底盘安全控制
+
+### 串口配置
+
+默认串口配置：
+- 端口: `/dev/ttyCH341USB0`
+- 波特率: 9600
+- 超时: 1秒
+
+可以根据需要修改代码中的串口参数。
+
+## 操作说明
+
+1. 启动节点后，系统会自动加载充电桩位置配置
+2. 系统会定期发布充电桩标记到RViz进行可视化
+3. 按下键盘上的 `q` 键启动导航到充电桩
+4. 导航成功后，系统会自动启动串口控制功能
+5. 串口控制会根据接收到的数据控制机器人运动
+6. 按 `Ctrl+C` 退出程序
+
+## 故障排除
+
+### 常见问题
+
+1. **串口无法打开**
+ - 检查串口设备是否连接
+ - 确认串口权限设置
+ - 验证串口设备名称
+
+2. **导航失败**
+ - 确认Nav2导航系统正常运行
+ - 检查充电桩位置配置是否正确
+ - 验证地图和定位系统状态
+
+3. **RViz中看不到标记**
+ - 确认RViz已订阅 `/goal_marker` 话题
+ - 检查MarkerArray显示设置
+ - 验证坐标系设置是否为'map'
+
+## 许可证
+
+MIT License
+
+## 维护者
+
+请联系维护者获取技术支持。
diff --git a/agv_pro_autocharge/agv_pro_autocharge/__init__.py b/agv_pro_autocharge/agv_pro_autocharge/__init__.py
new file mode 100755
index 0000000..caaf2a7
--- /dev/null
+++ b/agv_pro_autocharge/agv_pro_autocharge/__init__.py
@@ -0,0 +1,10 @@
+"""
+AGV AutoCharge ROS2 Package
+
+This package provides automatic charging functionality for AGV robots using ROS2 Humble.
+It includes position management, visualization, navigation, and serial control features.
+"""
+
+__version__ = '1.0.0'
+__author__ = 'Your Name'
+__email__ = 'your-email@example.com'
diff --git a/agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py b/agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py
new file mode 100755
index 0000000..81d8fe5
--- /dev/null
+++ b/agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py
@@ -0,0 +1,612 @@
+#!/usr/bin/env python3
+# coding=utf-8
+
+"""
+合并的自动充电控制器 - 结合位置管理、可视化和导航功能
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 按键'q'启动导航功能
+- 导航成功后启动串口控制
+"""
+
+# 引用ros库
+import rclpy
+from rclpy.node import Node
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+from rclpy.duration import Duration
+
+# 用到的变量定义
+from std_msgs.msg import Bool
+from std_msgs.msg import Int8
+from std_msgs.msg import UInt8
+from std_msgs.msg import Float32
+
+# 用于记录充电桩位置、发布导航点
+from geometry_msgs.msg import PoseStamped, Twist
+
+# rviz可视化相关
+from visualization_msgs.msg import Marker
+from visualization_msgs.msg import MarkerArray
+
+# 里程计话题相关
+from nav_msgs.msg import Odometry
+
+# 键盘控制相关
+import sys
+import select
+import termios
+import tty
+
+# 延迟相关
+import time
+import threading
+
+# 读写充电桩位置文件
+import json
+import math
+import yaml
+import os
+
+# 导入串口解析模块
+from .serial_can_parser import SerialCANParser
+
+# 存放充电桩位置的文件位置 - 参考原始auto_recharger.py的路径设置方式
+def find_config_files():
+ """查找配置文件路径"""
+ # 首先尝试几个可能的位置
+ possible_paths = [
+ # 开发环境路径
+ '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config',
+ # 你的工作空间路径
+ '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config',
+ # 当前包的相对路径
+ os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'config'),
+ # 安装路径
+ '/home/elephant/agv_pro_ros2/install/agv_pro_autocharge/share/agv_pro_autocharge/config'
+ ]
+
+ for config_dir in possible_paths:
+ yaml_path = os.path.join(config_dir, 'nav_goal_params.yaml')
+ json_path = os.path.join(config_dir, 'charger_position.json')
+
+ print(f"Checking config directory: {config_dir}")
+ if os.path.exists(yaml_path) and os.path.exists(json_path):
+ print(f"Found config files in: {config_dir}")
+ return yaml_path, json_path
+
+ # 如果都找不到，直接报错
+ print("ERROR: Could not find config files in any of the following locations:")
+ for path in possible_paths:
+ print(f" - {path}")
+ print("Please ensure the config files exist in one of these directories.")
+
+ # 返回第一个路径作为默认值，但文件可能不存在
+ return os.path.join(possible_paths[0], 'nav_goal_params.yaml'), os.path.join(possible_paths[0], 'charger_position.json')
+
+# 获取配置文件路径
+yaml_file, json_file = find_config_files()
+
+# print_and_fixRetract相关，用于打印带颜色的信息
+RESET = '\033[0m'
+RED = '\033[1;31m'
+GREEN = '\033[1;32m'
+YELLOW= '\033[1;33m'
+BLUE = '\033[1;34m'
+PURPLE= '\033[1;35m'
+CYAN = '\033[1;36m'
+
+# 圆周率
+PI = 3.1415926535897
+
+if os.name == 'nt':
+ import msvcrt
+else:
+ import termios
+ import tty
+
+settings = None
+if os.name != 'nt' and sys.stdin.isatty():
+ settings = list(termios.tcgetattr(sys.stdin))
+
+def get_key(settings):
+ if os.name == 'nt':
+ return msvcrt.getch().decode('utf-8')
+ else:
+ if sys.stdin.isatty():
+ tty.setraw(sys.stdin.fileno())
+ rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
+ if rlist:
+ key = sys.stdin.read(1)
+ else:
+ key = ''
+ if sys.stdin.isatty() and settings:
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+ return key
+
+def print_and_fixRetract(str):
+ global settings
+ '''键盘控制会导致回调函数内使用print()出现自动缩进的问题，此函数可以解决该现象'''
+ if sys.stdin.isatty() and settings:
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+ print(str)
+
+class CombinedAutoRecharger(Node):
+ def __init__(self):
+
+ # 创建节点
+ super().__init__("combined_auto_recharger")
+
+ print_and_fixRetract('Combined Auto Recharger Node Started!')
+
+ # 导航状态标记
+ self.navigation_active = False
+
+ # 串口控制相关
+ self.parser = None
+ self.serial_control_active = False
+ self.navigation_requested = False # 添加导航请求标志
+
+ # 创建导航器
+ self.navigator = BasicNavigator()
+
+ # 加载充电桩位置信息
+ self.load_charger_position()
+ # 加载导航参数
+ self.load_nav_goal_params()
+
+ # 创建发布者
+ self.robot_security_off_pub = self.create_publisher(Int8, '/chassis_security', 10)
+ self.Charger_marker_pub = self.create_publisher(MarkerArray, '/goal_marker', 10)
+ self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+
+ # 创建订阅者 - 只订阅充电桩位置更新
+ self.Charger_Position_Update_sub = self.create_subscription(
+ PoseStamped, "/charger_position_update",
+ self.Position_Update_callback, 10)
+
+ # 创建定时器，定期发布充电桩标记（每2秒发布一次）
+ self.marker_timer = self.create_timer(2.0, self.timer_callback)
+
+ # 创建导航检查定时器（每0.5秒检查一次导航请求）
+ self.navigation_timer = self.create_timer(0.5, self.check_navigation_request)
+
+ # 发布初始充电桩位置标记
+ self.update_charger_visualization()
+
+ print_and_fixRetract('Combined auto recharger node initialized successfully!')
+ print_and_fixRetract(f'{GREEN}Press "q" to start navigation, Ctrl+C to exit{RESET}')
+
+ def load_nav_goal_params(self):
+ """加载导航目标参数（前方距离和角度）"""
+ print_and_fixRetract(f"Attempting to load nav goal params from: {yaml_file}")
+ print_and_fixRetract(f"File exists? {os.path.exists(yaml_file)}")
+
+ try:
+ with open(yaml_file, 'r', encoding='utf-8') as f:
+ params = yaml.safe_load(f)
+
+ print_and_fixRetract(f"Raw params from file: {params}")
+
+ self.forward_distance = float(params.get('forward_distance', 1.0))
+ self.yaw_offset_deg = float(params.get('yaw_offset_deg', 0.0))
+
+ print_and_fixRetract(f"Successfully loaded nav goal params: forward_distance={self.forward_distance}, yaw_offset_deg={self.yaw_offset_deg}")
+
+ except FileNotFoundError:
+ print_and_fixRetract(f"{RED}Nav goal params file not found: {yaml_file}{RESET}")
+ print_and_fixRetract(f"{RED}Please create the configuration file with the required parameters{RESET}")
+ # 使用默认值
+ self.forward_distance = 1.0
+ self.yaw_offset_deg = 0.0
+ except Exception as e:
+ print_and_fixRetract(f"{RED}Failed to load nav goal params: {e}{RESET}")
+ self.forward_distance = 1.0
+ self.yaw_offset_deg = 0.0
+
+ def timer_callback(self):
+ '''定时器回调函数，定期发布充电桩标记'''
+ # 始终发布当前JSON文件中的位置信息
+ if hasattr(self, 'json_data') and self.json_data:
+ self.Pub_Charger_marker(
+ self.json_data['p_x'],
+ self.json_data['p_y'],
+ self.json_data['orien_z'],
+ self.json_data['orien_w']
+ )
+
+ def check_navigation_request(self):
+ '''检查是否有导航请求'''
+ if self.navigation_requested and not self.navigation_active:
+ self.navigation_requested = False
+ self.navigation_active = True
+ print_and_fixRetract(f"{BLUE}Processing navigation request...{RESET}")
+
+ # 在ROS2线程中执行导航
+ result = self.execute_navigation_internal()
+
+ if result:
+ print_and_fixRetract(f"{GREEN}Navigation successful! Starting serial control...{RESET}")
+ # 在ROS2线程中启动串口控制
+ self.start_serial_control_async()
+ else:
+ print_and_fixRetract(f"{RED}Navigation failed{RESET}")
+ self.navigation_active = False
+
+ def request_navigation(self):
+ '''请求开始导航'''
+ if not self.navigation_active:
+ self.navigation_requested = True
+ print_and_fixRetract(f"{BLUE}Navigation request queued...{RESET}")
+ else:
+ print_and_fixRetract(f"{YELLOW}Navigation already in progress{RESET}")
+
+ def load_charger_position(self):
+ '''加载充电桩位置信息'''
+ try:
+ with open(json_file, 'r', encoding='utf-8') as fp:
+ self.json_data = json.load(fp)
+ print_and_fixRetract(f"Loaded charger position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}")
+ except FileNotFoundError:
+ print_and_fixRetract(f"{RED}Charger position file {json_file} not found{RESET}")
+ print_and_fixRetract(f"{RED}Please create the configuration file with default charger position{RESET}")
+ # 使用默认位置
+ self.json_data = {
+ 'p_x': 0.0,
+ 'p_y': 0.0,
+ 'orien_z': 0.0,
+ 'orien_w': 1.0
+ }
+ except Exception as e:
+ print_and_fixRetract(f"Error loading charger position: {e}")
+ self.json_data = {
+ 'p_x': 0.0,
+ 'p_y': 0.0,
+ 'orien_z': 0.0,
+ 'orien_w': 1.0
+ }
+
+ def save_charger_position(self):
+ '''保存充电桩位置信息到JSON文件'''
+ try:
+ with open(json_file, 'w', encoding='utf-8') as fp:
+ json.dump(self.json_data, fp, ensure_ascii=False, indent=2)
+ print_and_fixRetract(f"{GREEN}Charger position saved to {json_file}{RESET}")
+ except Exception as e:
+ print_and_fixRetract(f"{RED}Error saving charger position: {e}{RESET}")
+
+ def Pub_Charger_Position(self):
+ '''更新充电桩位置信息并保存到JSON文件'''
+ # 发布充电桩位置的可视化
+ self.Pub_Charger_marker(
+ self.json_data['p_x'],
+ self.json_data['p_y'],
+ self.json_data['orien_z'],
+ self.json_data['orien_w'])
+
+ # 保存当前充电桩位置到JSON文件
+ position_data = {
+ 'p_x': self.json_data['p_x'],
+ 'p_y': self.json_data['p_y'],
+ 'orien_z': self.json_data['orien_z'],
+ 'orien_w': self.json_data['orien_w']
+ }
+
+ self.json_data = position_data
+ self.save_charger_position()
+ print_and_fixRetract(f"Position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}")
+
+ def Pub_Charger_marker(self, p_x, p_y, o_z, o_w):
+ '''发布目标点可视化话题'''
+
+ markerArray = MarkerArray()
+
+ # 获取当前时间戳
+ current_time = self.get_clock().now().to_msg()
+
+ marker_shape = Marker() # 创建marker对象
+ marker_shape.id = 0 # 必须赋值id
+ marker_shape.header.frame_id = 'map' # 以哪一个TF坐标为原点
+ marker_shape.header.stamp = current_time # 添加时间戳
+ marker_shape.type = Marker.ARROW # TEXT_VIEW_FACING #一直面向屏幕的字符格式
+ marker_shape.action = Marker.ADD # 添加marker
+ marker_shape.scale.x = 0.5 # marker大小
+ marker_shape.scale.y = 0.05 # marker大小
+ marker_shape.scale.z = 0.05 # marker大小，对于字符只有z起作用
+ marker_shape.pose.position.x = p_x # 字符位置
+ marker_shape.pose.position.y = p_y # 字符位置
+ marker_shape.pose.position.z = 0.1 # msg.position.z #字符位置
+ marker_shape.pose.orientation.z = o_z # 字符位置
+ marker_shape.pose.orientation.w = o_w # 字符位置
+ marker_shape.color.r = 1.0 # 字符颜色R(红色)通道
+ marker_shape.color.g = 0.0 # 字符颜色G(绿色)通道
+ marker_shape.color.b = 0.0 # 字符颜色B(蓝色)通道
+ marker_shape.color.a = 1.0 # 字符透明度
+ markerArray.markers.append(marker_shape) # 添加元素进数组
+
+ marker_string = Marker() # 创建marker对象
+ marker_string.id = 1 # 必须赋值id
+ marker_string.header.frame_id = 'map' # 以哪一个TF坐标为原点
+ marker_string.header.stamp = current_time # 添加时间戳
+ marker_string.type = Marker.TEXT_VIEW_FACING # 一直面向屏幕的字符格式
+ marker_string.action = Marker.ADD # 添加marker
+ marker_string.scale.x = 0.5 # marker大小
+ marker_string.scale.y = 0.5 # marker大小
+ marker_string.scale.z = 0.5 # marker大小，对于字符只有z起作用
+ marker_string.color.a = 1.0 # 字符透明度
+ marker_string.color.r = 1.0 # 字符颜色R(红色)通道
+ marker_string.color.g = 0.0 # 字符颜色G(绿色)通道
+ marker_string.color.b = 0.0 # 字符颜色B(蓝色)通道
+ marker_string.pose.position.x = p_x # 字符位置
+ marker_string.pose.position.y = p_y # 字符位置
+ marker_string.pose.position.z = 0.1 # msg.position.z #字符位置
+ marker_string.pose.orientation.z = o_z # 字符位置
+ marker_string.pose.orientation.w = o_w # 字符位置
+ marker_string.text = 'Charger' # 字符内容
+ markerArray.markers.append(marker_string) # 添加元素进数组
+ self.Charger_marker_pub.publish(markerArray) # 发布markerArray，rviz订阅并进行可视化
+
+ def Position_Update_callback(self, topic):
+ '''更新json文件中的充电桩位置'''
+ position_dic = {'p_x': 0, 'p_y': 0, 'orien_z': 0, 'orien_w': 0}
+ position_dic['p_x'] = topic.pose.position.x
+ position_dic['p_y'] = topic.pose.position.y
+ position_dic['orien_z'] = topic.pose.orientation.z
+ position_dic['orien_w'] = topic.pose.orientation.w
+
+ # 保存最新的充电桩位置到json文件
+ self.json_data = position_dic
+ self.save_charger_position()
+ print_and_fixRetract("New charging pile position saved.")
+
+ # 位置更新后立即发布一次新的标记，然后继续定时发布
+ self.update_charger_visualization()
+ print_and_fixRetract(f"{GREEN}Charger position updated and will be published continuously{RESET}")
+
+ def update_charger_visualization(self):
+ '''更新充电桩可视化标记'''
+ if hasattr(self, 'json_data'):
+ self.Pub_Charger_marker(
+ self.json_data['p_x'],
+ self.json_data['p_y'],
+ self.json_data['orien_z'],
+ self.json_data['orien_w']
+ )
+
+ def execute_navigation(self):
+ """外部调用的导航接口"""
+ self.request_navigation()
+ return True # 返回True表示请求已提交
+
+ def execute_navigation_internal(self):
+ """内部执行导航任务"""
+ print_and_fixRetract(f"{BLUE}Starting navigation...{RESET}")
+ # 从JSON文件读取充电桩位置
+ try:
+ with open(json_file, 'r', encoding='utf-8') as f:
+ charger_data = json.load(f)
+ px = charger_data['p_x']
+ py = charger_data['p_y']
+ # 充电桩姿态四元数转欧拉角
+ orien_z = charger_data['orien_z']
+ orien_w = charger_data['orien_w']
+ yaw = 2 * math.atan2(orien_z, orien_w) # 只考虑z/w分量
+ except Exception as e:
+ print_and_fixRetract(f"{RED}Failed to read charger position file: {e}{RESET}")
+ self.navigation_active = False
+ return False
+
+ # 计算目标点位置
+ x_offset = self.forward_distance * math.cos(yaw)
+ y_offset = self.forward_distance * math.sin(yaw)
+ goal_x = px + x_offset
+ goal_y = py + y_offset
+
+ # 计算目标点姿态（z轴顺时针yaw_offset_deg）
+ goal_yaw = yaw - math.radians(self.yaw_offset_deg)
+ goal_qz = math.sin(goal_yaw / 2)
+ goal_qw = math.cos(goal_yaw / 2)
+
+ print_and_fixRetract(f"Nav goal: x={goal_x:.3f}, y={goal_y:.3f}, yaw={math.degrees(goal_yaw):.1f}°")
+ goal_pose = self.create_pose(goal_x, goal_y, goal_qz, goal_qw)
+
+ # 执行导航
+ print_and_fixRetract(f"{BLUE}Executing navigation...{RESET}")
+ result1 = self.nav_through_pose([goal_pose], verbose=False)
+ print_and_fixRetract(f"Navigation result: {result1}")
+ self.navigation_active = False
+ return result1
+
+ def create_pose(self, x, y, z, w):
+ """创建单个目标点的位姿信息"""
+ pose = PoseStamped()
+ pose.header.frame_id = 'map'
+ pose.header.stamp = self.get_clock().now().to_msg()
+ pose.pose.position.x = x
+ pose.pose.position.y = y
+ pose.pose.orientation.z = z
+ pose.pose.orientation.w = w
+ return pose
+
+ def nav_through_pose(self, goal_poses, verbose: bool = False) -> bool:
+ """执行多点导航任务"""
+ # 开始执行多点导航任务
+ self.navigator.goThroughPoses(goal_poses)
+
+ # 等待导航任务完成，监控导航状态
+ while not self.navigator.isTaskComplete():
+ feedback = self.navigator.getFeedback()
+ if feedback and verbose:
+ remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
+ print_and_fixRetract(f"预计到达时间: {remaining:.0f} 秒")
+
+ # 根据导航结果返回相应状态
+ result = self.navigator.getResult()
+ if result == TaskResult.SUCCEEDED:
+ print_and_fixRetract(f'{GREEN}Navigation successful!{RESET}')
+ return True
+ elif result == TaskResult.CANCELED:
+ print_and_fixRetract(f'{YELLOW}Navigation canceled!{RESET}')
+ elif result == TaskResult.FAILED:
+ print_and_fixRetract(f'{RED}Navigation failed!{RESET}')
+ else:
+ print_and_fixRetract(f'{RED}Invalid navigation result!{RESET}')
+ return False
+
+ def start_serial_control_async(self):
+ """异步启动串口控制功能"""
+ def serial_control_thread():
+ self.start_serial_control()
+
+ # 在新线程中启动串口控制，避免阻塞ROS2主线程
+ serial_thread = threading.Thread(target=serial_control_thread, daemon=True)
+ serial_thread.start()
+
+ def start_serial_control(self):
+ """启动串口控制功能"""
+ print_and_fixRetract(f"{BLUE}Starting serial control...{RESET}")
+ try:
+ self.parser = SerialCANParser('/dev/agvpro_ec130', 9600, 1)
+ self.parser.open_serial() # 打开usb串口
+
+ # 发送AT 命令从透传模式进入AT指令模式
+ self.parser.send_at_commands(["AT+CG", "AT+AT"])
+
+ self.serial_control_active = True
+ print_and_fixRetract(f'{GREEN}Serial control started successfully{RESET}')
+
+ while self.serial_control_active:
+ # 开始读取数据
+ x_speed, z_speed, which_mode, infrared_bits = self.parser.read_serial_data()
+
+ if infrared_bits[7] == 0: # 无障碍物
+ if which_mode == 0x01: # 正常模式
+ # 直接发布ROS2 Twist消息
+ twist_msg = Twist()
+ twist_msg.linear.x = float(x_speed)
+ twist_msg.linear.y = 0.0
+ twist_msg.angular.z = float(z_speed)
+ self.cmd_vel_pub.publish(twist_msg)
+ print_and_fixRetract(f'Normal mode - Speed: x={x_speed}, z={z_speed}')
+
+ elif which_mode == 0xBB: # 测压区
+ # 停止运动
+ stop_msg = Twist()
+ self.cmd_vel_pub.publish(stop_msg)
+ print_and_fixRetract(f'{YELLOW}Pressure zone - Stop movement{RESET}')
+
+ elif which_mode == 0xAA: # 充电区
+ # 停止运动
+ stop_msg = Twist()
+ self.cmd_vel_pub.publish(stop_msg)
+ print_and_fixRetract(f'{GREEN}Charging zone - Stop movement{RESET}')
+ break # 充电完成后退出
+
+ elif which_mode == 0xCF: # 急停模式
+ emergency_stop_msg = Twist() # 所有速度都为0
+ self.cmd_vel_pub.publish(emergency_stop_msg)
+ print_and_fixRetract(f'{RED}Emergency stop mode - Immediate stop{RESET}')
+ break
+
+ else: # 检测到障碍物
+ obstacle_stop_msg = Twist() # 所有速度都为0
+ self.cmd_vel_pub.publish(obstacle_stop_msg)
+ print_and_fixRetract(f'{RED}Obstacle detected - Stop movement{RESET}')
+ break
+
+ except KeyboardInterrupt:
+ print_and_fixRetract("Serial control interrupted by user")
+ # 发布停止消息
+ emergency_stop = Twist()
+ self.cmd_vel_pub.publish(emergency_stop)
+ except Exception as e:
+ print_and_fixRetract(f"{RED}Serial control error: {e}{RESET}")
+ # 发布停止消息
+ emergency_stop = Twist()
+ self.cmd_vel_pub.publish(emergency_stop)
+ finally:
+ if self.parser:
+ self.parser.close_serial()
+ print_and_fixRetract("Serial control stopped")
+
+ def stop_serial_control(self):
+ """停止串口控制功能"""
+ self.serial_control_active = False
+
+ def get_charger_info(self):
+ '''获取充电桩位置信息'''
+ if hasattr(self, 'json_data'):
+ return {
+ 'position': {
+ 'x': self.json_data['p_x'],
+ 'y': self.json_data['p_y']
+ },
+ 'orientation': {
+ 'z': self.json_data['orien_z'],
+ 'w': self.json_data['orien_w']
+ }
+ }
+ return None
+
+
+def main(args=None):
+ '''主函数'''
+ rclpy.init(args=args)
+
+ combined_recharger = None
+ try:
+ combined_recharger = CombinedAutoRecharger()
+
+ print_and_fixRetract("Combined auto recharger node is running...")
+ print_and_fixRetract("Node functions:")
+ print_and_fixRetract("- Listening for charger position updates on /charger_position_update")
+ print_and_fixRetract("- Publishing visualization markers on /goal_marker every 2 seconds")
+ print_and_fixRetract("- Press 'q' to start navigation to charger position")
+ print_and_fixRetract("- Navigation success will trigger serial control")
+
+ # 启动ROS2事件循环线程
+ def ros2_spin():
+ try:
+ rclpy.spin(combined_recharger)
+ except Exception as spin_error:
+ print_and_fixRetract(f"ROS2 spin error: {spin_error}")
+
+ ros2_thread = threading.Thread(target=ros2_spin, daemon=True)
+ ros2_thread.start()
+
+ # 键盘监听循环
+ print_and_fixRetract(f"{GREEN}Press 'q' to start navigation, Ctrl+C to exit{RESET}")
+ print_and_fixRetract("Waiting for keyboard input...")
+
+ while True:
+ try:
+ key = get_key(settings)
+ if key:
+ print_and_fixRetract(f"Key pressed: {repr(key)}") # 调试信息
+ if key.lower() == 'q':
+ print_and_fixRetract(f"{BLUE}Navigation command received!{RESET}")
+
+ # 请求导航任务（异步执行）
+ combined_recharger.execute_navigation()
+
+ elif key == '\x03': # Ctrl+C
+ break
+
+ time.sleep(0.1) # 避免过度占用CPU
+ except KeyboardInterrupt:
+ break
+ except Exception as e:
+ print_and_fixRetract(f"Keyboard input error: {e}")
+ time.sleep(0.5)
+
+ except KeyboardInterrupt:
+ print_and_fixRetract("\nShutting down combined auto recharger node...")
+ finally:
+ if combined_recharger:
+ combined_recharger.stop_serial_control()
+ combined_recharger.destroy_node()
+ rclpy.shutdown()
+ print_and_fixRetract("Program exited safely")
+
+if __name__ == '__main__':
+ main()
+
diff --git a/agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py b/agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py
new file mode 100755
index 0000000..4277f58
--- /dev/null
+++ b/agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py
@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+#coding=UTF-8
+import serial
+import time
+
+class SerialCANParser:
+ def __init__(self, serial_port='/dev/ttyCH341USB0', baudrate=9600, timeout=1):
+ self.serial_port = serial_port # 串口名称
+ self.baudrate = baudrate # 波特率
+ self.timeout = timeout # 超时设置
+ self.ser = None # 串口对象
+ self.buffer = bytearray() # 存储当前读取的字节
+ self.max_retries = 3 # 最大重试次数
+ # 存储实时数据
+ self.x_speed = 0.0
+ self.z_speed = 0.0
+ self.infrared_bits = []
+
+ def open_serial(self):
+ """打开串口"""
+ try:
+ self.ser = serial.Serial(self.serial_port, self.baudrate, timeout=self.timeout)
+ print(f"串口 {self.serial_port} 已打开，波特率：{self.baudrate}")
+ except Exception as e:
+ print(f"打开串口失败: {e}")
+
+ def close_serial(self):
+ """关闭串口"""
+ if self.ser and self.ser.is_open:
+ self.ser.close()
+ print("串口已关闭。")
+ else:
+ print("串口未打开或已关闭。")
+
+ def can_id_check(self, date):
+ high_byte, low_byte = date[0:2]
+ # 高字节左移 3 位
+ can_id = (high_byte << 3)
+ # 低字节右移 5 位
+ can_id |= (low_byte >> 5)
+ return can_id
+
+ def parse_can_data(self, data):
+ """解析8字节CAN数据帧"""
+ if len(data) != 8:
+ print("数据帧长度不正确")
+ return None
+
+ # 解析 X、Y 和 Z 速度
+ x_speed_raw = ((data[0] << 8) | data[1]) # X速度的原始数据
+ z_speed_raw = ((data[4] << 8) | data[5]) # Z速度的原始数据
+
+ # 将原始数据转换为浮动数值，并考虑正负
+ if x_speed_raw & 0x8000: # 如果最高位为1，表示负数
+ x_speed_raw = -((65536 - x_speed_raw) & 0xFFFF) # 补码转换为负数
+ if z_speed_raw & 0x8000: # 如果最高位为1，表示负数
+ z_speed_raw = -((65536 - z_speed_raw) & 0xFFFF) # 补码转换为负数
+
+ # 转换单位为 m/s 和 rad/s
+ self.x_speed = x_speed_raw / 1000.0 # X速度单位为 m/s
+ self.y_speed = 0 # Y速度为0
+ self.z_speed = z_speed_raw / 1000.0 # Z速度单位为 rad/s
+ self.which_mode = data[2]
+ self.infrared = data[6] # 红外数据
+ self.raw_current = data[7] # 电流数据
+
+ if self.raw_current > 32767: # 无符号数大于 32767 表示负值（因为最大值是 65535）
+ # 转换为负数
+ self.actual_current = -(65536 - self.raw_current) * 30.0
+ else:
+ # 正数直接转换
+ self.actual_current = self.raw_current * 30.0
+
+ # 处理红外数据
+ self.infrared_bits = [(self.infrared >> (7 - i)) & 0x01 for i in range(8)]
+
+ # 打印或处理数据
+ print(f"X Speed: {self.x_speed:.3f}, Y Speed: {self.y_speed}, Z Speed: {self.z_speed:.3f}, "
+ f"Actual Current: {self.actual_current:.3f} mA, Infrared: {self.infrared}")
+
+ # 打印或处理红外位信息
+ print(f"L_A: {self.infrared_bits[2]}, L_B: {self.infrared_bits[3]}, R_B: {self.infrared_bits[4]}, "
+ f"R_A: {self.infrared_bits[5]}, infrared_flag : {self.infrared_bits[6]}, "
+ f"Charging flag: {self.infrared_bits[7]}")
+
+ def read_serial_data(self):
+ """读取串口数据并解析"""
+ while True: # 修改为简单的无限循环，由上层控制退出
+ if self.ser.in_waiting > 0:
+ byte = self.ser.read(1) # 读取一个字节
+ if len(self.buffer) < 2:
+ self.buffer.extend(byte)
+ if len(self.buffer) == 2:
+ # 如果帧头为 0x41 0x54，表示为AT帧头，则开始接收数据
+ if self.buffer[0] != 0x41 or self.buffer[1] != 0x54:
+ # 如果不是有效的帧头，则清空缓冲区并跳到下次循环
+ self.buffer.clear()
+ continue # 继续等待下一个字节
+ else:
+ self.buffer.extend(byte)
+ # print("缓冲区内容:", ' '.join(f'{b:02x}' for b in self.buffer)) # debug
+ # 如果缓冲区字节长度大于等于 17 字节（数据帧长度）
+ if len(self.buffer) >= 17:
+ # print("Received Frame (Hex):", ' '.join(f'{byte:02x}' for byte in self.buffer)) # debug
+ # 解析帧头、CAN帧ID、格式、类型和数据
+ # at_frame_header = self.buffer[0:2] # AT帧头
+ can_frame_id = self.can_id_check(self.buffer[2:4]) # CAN标准帧ID
+ # can_frame_format = self.buffer[4] # CAN帧格式（0,标准帧;1,扩展帧）
+ # can_frame_type = self.buffer[5] # CAN帧类型（0,数据帧;1,远程帧）
+ data_length = self.buffer[6] # 数据长度
+ data = self.buffer[7:15] # 数据帧
+ # print(f"帧ID: 0x{can_frame_id:X}") # debug
+ if can_frame_id == 0x182 and data_length == 0x08: # 根据can帧id进行判断
+ # 如果帧ID为0x182,校验通过,进行数据赋值
+ self.parse_can_data(data)
+ # 清空缓冲区，准备下一帧数据
+ self.buffer.clear()
+ return self.x_speed, self.z_speed, self.which_mode, self.infrared_bits # 返回解析后的数据
+ else:
+ # 清空缓冲区，准备下一帧数据
+ self.buffer.clear()
+
+ def read_serial_response(self):
+ """读取串口响应数据，直到接收到 '\r\n' 或超时"""
+ response = bytearray() # 使用 bytearray 来存储原始字节流
+ while True:
+ if self.ser.in_waiting > 0:
+ byte = self.ser.read(1)
+ response += byte
+ # 检查是否已接收到完整响应
+ if b'\r\n' in response:
+ break
+ # 超时机制，防止死循环
+ if len(response) > 100:
+ break
+ return bytes(response) # 返回原始字节流（bytes）
+
+ def send_at_commands(self, commands):
+ """发送 AT 命令并等待响应"""
+ for command in commands:
+ retries = 0
+ while retries < self.max_retries:
+ self.ser.write(command.encode() + b'\r\n')
+ print(f"发送命令: {command}")
+ # 等待响应并读取数据
+ response = self.read_serial_response()
+ # 检查响应是否包含 "OK"
+ if b"OK" in response:
+ print(f"收到响应: {response}")
+ break # 如果收到 OK，退出重试循环
+ else:
+ retries += 1
+ print(f"未收到预期的响应，收到: {response}")
+ if retries == self.max_retries:
+ print(f"重试 {self.max_retries} 次后仍未收到有效响应，请检查设备。")
+ break
+
+ def start(self):
+ """开始读取和处理数据"""
+ self.open_serial()
+ try:
+ # 发送AT 命令从透传模式进入AT指令模式
+ self.send_at_commands(["AT+CG", "AT+AT"])
+ # 开始读取数据
+ self.read_serial_data()
+ except KeyboardInterrupt:
+ print("手动中止程序。")
+ finally:
+ self.close_serial()
+
+if __name__ == '__main__':
+ parser = SerialCANParser('/dev/ttyCH341USB0', 9600, 1)
+ parser.start()
diff --git a/agv_pro_autocharge/config/charger_position.json b/agv_pro_autocharge/config/charger_position.json
new file mode 100755
index 0000000..7b738ef
--- /dev/null
+++ b/agv_pro_autocharge/config/charger_position.json
@@ -0,0 +1,6 @@
+{
+ "p_x": -0.04373347759246826,
+ "p_y": -4.024151802062988,
+ "orien_z": 0.09378381732290733,
+ "orien_w": 0.9955925851513477
+}
\ No newline at end of file
diff --git a/agv_pro_autocharge/config/nav_goal_params.yaml b/agv_pro_autocharge/config/nav_goal_params.yaml
new file mode 100755
index 0000000..e9339be
--- /dev/null
+++ b/agv_pro_autocharge/config/nav_goal_params.yaml
@@ -0,0 +1,3 @@
+# 导航参数配置
+forward_distance: 1 # 距离充电桩前方1米
+yaw_offset_deg: 10.0 # 顺时针旋转10度
diff --git a/agv_pro_autocharge/package.xml b/agv_pro_autocharge/package.xml
new file mode 100755
index 0000000..cc05bd0
--- /dev/null
+++ b/agv_pro_autocharge/package.xml
@@ -0,0 +1,30 @@
+
+
+
+ agv_pro_autocharge
+ 1.0.0
+ AGV automatic charging system for ROS2 Humble
+ Your Name
+ MIT
+
+
+ ament_python
+
+
+ rclpy
+ geometry_msgs
+ std_msgs
+ nav_msgs
+ visualization_msgs
+ nav2_simple_commander
+
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/agv_pro_autocharge/resource/agv_pro_autocharge b/agv_pro_autocharge/resource/agv_pro_autocharge
new file mode 100755
index 0000000..0138642
--- /dev/null
+++ b/agv_pro_autocharge/resource/agv_pro_autocharge
@@ -0,0 +1 @@
+agv_pro_autocharge
diff --git a/agv_pro_autocharge/setup.cfg b/agv_pro_autocharge/setup.cfg
new file mode 100755
index 0000000..9b4840c
--- /dev/null
+++ b/agv_pro_autocharge/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/agv_pro_autocharge
+[install]
+install_scripts=$base/lib/agv_pro_autocharge
diff --git a/agv_pro_autocharge/setup.py b/agv_pro_autocharge/setup.py
new file mode 100755
index 0000000..0dd0998
--- /dev/null
+++ b/agv_pro_autocharge/setup.py
@@ -0,0 +1,30 @@
+from setuptools import setup, find_packages
+import os
+from glob import glob
+
+package_name = 'agv_pro_autocharge'
+
+setup(
+ name=package_name,
+ version='1.0.0',
+ packages=find_packages(),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ # Include config files
+ (os.path.join('share', package_name, 'config'), glob('config/*')),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='Your Name',
+ maintainer_email='your-email@example.com',
+ description='AGV automatic charging system for ROS2 Humble',
+ license='MIT',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'combined_auto_recharger = agv_pro_autocharge.combined_auto_recharger:main',
+ ],
+ },
+)
diff --git a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h
index fa1512e..deb81d0 100644
--- a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h
+++ b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h
@@ -2,7 +2,8 @@
#define AGV_PRO_DRIVER_H
#include
-#include "serial_driver/serial_driver.hpp"
+#include
+#include
#include "rclcpp/rclcpp.hpp"
@@ -13,7 +14,9 @@
#include
#include
-#define RECEIVE_DATA_SIZE 14 //The length of the data sent by the esp32
+#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)
extern std::array odom_pose_covariance;
extern std::array odom_twist_covariance;
@@ -21,20 +24,136 @@ extern std::array 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();
- void print_hex(const std::string& label, const std::vector& data, std::optional override_size = std::nullopt);
+
+ /**
+ * @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& data,
+ std::optional 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& frame, bool debug);
- void is_power_on();
- void set_auto_report();
+
+ /**
+ * @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 build_serial_frame(uint8_t cmd_id, const std::vector& payload);
- std::vector read_serial_response(const std::vector& expected_header, size_t payload_size, double timeout_sec);
+
+ /**
+ * @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 read_serial_response(
+ const std::vector& 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);
+
+ boost::asio::io_service io_;
+ std::unique_ptr serial_port_;
std::string frame_id_of_odometry_;
std::string child_frame_id_of_odometry_;
@@ -54,6 +173,18 @@ private:
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;
@@ -63,6 +194,22 @@ private:
float battery_voltage = 0.0f;
+ std::array 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 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::SharedPtr pub_odom;
@@ -70,10 +217,8 @@ private:
rclcpp::Publisher::SharedPtr pub_voltage;
rclcpp::Subscription::SharedPtr cmd_sub;
+ sensor_msgs::msg::Imu imu_data;
std::unique_ptr odomBroadcaster;
- std::shared_ptr serial_driver_;
- std::shared_ptr io_context_;
-
};
#endif
\ No newline at end of file
diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp
index 829522a..2dfc219 100644
--- a/agv_pro_base/src/agv_pro_ros.cpp
+++ b/agv_pro_base/src/agv_pro_ros.cpp
@@ -1,22 +1,6 @@
#include "agv_pro_base/agv_pro_driver.h"
-std::array 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 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} };
-
-uint16_t crc16_ibm(const uint8_t* data, size_t length) {
+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(data[i]);
@@ -32,7 +16,7 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) {
std::vector AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector& payload)
{
- std::vector frame(RECEIVE_DATA_SIZE, 0x00);
+ std::vector frame(SEND_DATA_SIZE, 0x00);
frame[0] = 0xFE;
frame[1] = 0xFE;
frame[2] = 0x0B;
@@ -62,8 +46,7 @@ void AGV_PRO::print_hex(const std::string& label, const std::vector& da
void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug)
{
try {
- auto port = serial_driver_->port();
- size_t bytes_transmit_size = port->send(frame);
+ size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame));
if (debug) {
print_hex("Sent", frame, bytes_transmit_size);
}
@@ -72,9 +55,11 @@ void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug)
}
}
-std::vector AGV_PRO::read_serial_response(const std::vector& expected_header, size_t payload_size, double timeout_sec)
+std::vector AGV_PRO::read_serial_response(
+ const std::vector& expected_header,
+ size_t payload_size,
+ double timeout_sec)
{
- auto port = serial_driver_->port();
std::vector sliding_buf;
uint8_t byte = 0;
@@ -82,21 +67,24 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e
rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);
while ((this->now() - start_time) < timeout) {
- std::vector temp_buf(1);
- if (port->receive(temp_buf) == 1) {
- byte = temp_buf[0];
+ 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());
+ sliding_buf.erase(sliding_buf.begin());
}
-
if (sliding_buf == expected_header) {
- break;
+ break;
}
}
}
-
+
if (sliding_buf != expected_header) {
RCLCPP_WARN(this->get_logger(), "Timeout waiting for header");
return {};
@@ -104,7 +92,20 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e
size_t remain_len = payload_size + 2;
std::vector remain_buf(remain_len);
- if (port->receive(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 {};
}
@@ -115,22 +116,22 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e
return full_buf;
}
-void AGV_PRO::is_power_on(){
+bool AGV_PRO::is_power_on(){
auto power_query_frame = build_serial_frame(0x12, {});
send_serial_frame(power_query_frame,true);
const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x12};
- auto power_query_response = read_serial_response(expected_header, 8, 1.0);
+ 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;
+ 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;
+ return false;
}
int is_poweron_status = static_cast(power_query_response[4]);
@@ -146,13 +147,13 @@ void AGV_PRO::is_power_on(){
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;
+ 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;
+ return false;
}
int poweron_status = static_cast(status_query_response[4]);
@@ -162,37 +163,61 @@ void AGV_PRO::is_power_on(){
case 1:
status_msg = "Motor is operating normally.";
RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
- break;
+ 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());
- break;
+ 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());
- break;
+ return false;
case 4:
status_msg = "CAN initialization error.";
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
- break;
+ return false;
case 5:
status_msg = "Motor initialization error.";
RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
- break;
+ return false;
default:
RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status);
- break;
+ return false;
}
}
- else
+ else{
RCLCPP_INFO(this->get_logger(), "Motor is operating normally.");
+ return true;
+ }
}
-void AGV_PRO::set_auto_report(){
- auto frame = build_serial_frame(0x23, {0x01});
+void AGV_PRO::set_auto_report(bool enable){
+ auto frame = build_serial_frame(0x23, {static_cast(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);
@@ -220,11 +245,9 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
std::vector data_vec(buf, buf + sizeof(buf));
- auto port = serial_driver_->port();
-
try
{
- port->send(data_vec);
+ boost::asio::write(*serial_port_,boost::asio::buffer(data_vec));
// print_hex("Sent", data_vec);//debug
}
catch(const std::exception &ex)
@@ -235,34 +258,46 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
bool AGV_PRO::readData()
{
- std::vector buf_header(1);
std::vector buf_length(1);
- std::vector data_buf(RECEIVE_DATA_SIZE-3);
+ std::vector data_buf(RECEIVE_PAYLOAD_SIZE);
- auto port = serial_driver_->port();
+ uint8_t byte = 0;
+ boost::system::error_code ec;
while (true)
{
- size_t ret = port->receive(buf_header);
- if (ret != 1 || buf_header[0] != 0xfe) {
+ 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 = port->receive(buf_header);
- if (ret == 1 && buf_header[0] == 0xfe) {
+ 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 = port->receive(buf_length);
+ 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] != 0x0b) {
- RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
+ if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) {
+ //RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
return false;
}
- ret = port->receive(data_buf);
- if (ret != data_buf.size())
+ 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;
@@ -271,18 +306,18 @@ bool AGV_PRO::readData()
std::vector recv_buf;
recv_buf.push_back(0xFE);
recv_buf.push_back(0xFE);
- recv_buf.push_back(0x0B);
+ recv_buf.push_back(0x1C);
recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
- // print_hex("recv_buf", recv_buf); //debug
+ //print_hex("recv_buf", recv_buf); //debug
if (recv_buf[3] != 0x25) {
- // RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]);
+ //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug
return false;
}
- uint16_t received_crc = recv_buf[13] | (recv_buf[12] << 8);
- uint16_t computed_crc = crc16_ibm(recv_buf.data(), 12);
+ 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);
@@ -298,6 +333,18 @@ bool AGV_PRO::readData()
battery_voltage = static_cast(recv_buf[9]) / 10.0f;
enable_status = recv_buf[10];
+ imu_data.linear_acceleration.x = static_cast((static_cast(recv_buf[11]) << 8) | recv_buf[12]) * 0.01;
+ imu_data.linear_acceleration.y = static_cast((static_cast(recv_buf[13]) << 8) | recv_buf[14]) * 0.01;
+ imu_data.linear_acceleration.z = static_cast((static_cast(recv_buf[15]) << 8) | recv_buf[16]) * 0.01;
+
+ imu_data.angular_velocity.x = static_cast((static_cast(recv_buf[17]) << 8) | recv_buf[18]) * 0.01;
+ imu_data.angular_velocity.y = static_cast((static_cast(recv_buf[19]) << 8) | recv_buf[20]) * 0.01;
+ imu_data.angular_velocity.z = static_cast((static_cast(recv_buf[21]) << 8) | recv_buf[22]) * 0.01;
+
+ roll = static_cast((static_cast(recv_buf[23]) << 8) | recv_buf[24]) * 0.01;
+ pitch = static_cast((static_cast(recv_buf[25]) << 8) | recv_buf[26]) * 0.01;
+ yaw = static_cast((static_cast(recv_buf[27]) << 8) | recv_buf[28]) * 0.01;
+
return true;
}
@@ -308,6 +355,40 @@ void AGV_PRO::publisherVoltage()
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();
@@ -345,12 +426,12 @@ void AGV_PRO::publisherOdom(double dt)
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
- odom.pose.covariance = odom_pose_covariance;
+ 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 = odom_twist_covariance;
+ odom.twist.covariance = this->odom_twist_covariance;
pub_odom->publish(odom);
}
@@ -369,6 +450,7 @@ void AGV_PRO::Control()
publisherOdom(dt);
// RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
publisherVoltage();
+ publisherImuSensor();
}
}
@@ -401,64 +483,54 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
lastTime = this->get_clock()->now();
- drivers::serial_driver::SerialPortConfig config(
- 1000000,
- drivers::serial_driver::FlowControl::NONE,
- drivers::serial_driver::Parity::NONE,
- drivers::serial_driver::StopBits::ONE
- );
-
try{
- io_context_ = std::make_shared(1);
- serial_driver_ = std::make_shared(*io_context_);
- serial_driver_->init_port(device_name_, config);
- serial_driver_->port()->open();
-
- RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
- RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
- RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
+ serial_port_ = std::make_unique(io_);
- AGV_PRO::is_power_on();
- AGV_PRO::set_auto_report();
+ 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;
}
-
- control_timer_ = this->create_wall_timer(
- std::chrono::milliseconds(20),
- std::bind(&AGV_PRO::Control, this)
- );
- RCLCPP_INFO(this->get_logger(), "Control timer started");
+ 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()
-{
- std::array buf = {
- 0xFE, 0xFE, 0x0b, 0x22,
- 0x01, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00
- };
-
- uint16_t crc = crc16_ibm(buf.data(), 12);
- buf[12] = (crc >> 8) & 0xff;
- buf[13] = crc & 0xff;
-
- std::vector data_vec(buf.begin(), buf.end());
-
- auto port = serial_driver_->port();
-
- try
- {
- port->send(data_vec);
- }
- catch(const std::exception &ex)
- {
- RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
- }
-
- serial_driver_->port()->close();
- RCLCPP_INFO(this->get_logger(),"Shutting down");
+{
+ if (serial_port_ && serial_port_->is_open()) {
+ this->set_auto_report(0);
+ serial_port_->cancel();
+ serial_port_->close();
+ }
}
\ No newline at end of file
diff --git a/agv_pro_description/urdf/agv_pro.urdf b/agv_pro_description/urdf/agv_pro.urdf
index b91f3e0..9824f10 100755
--- a/agv_pro_description/urdf/agv_pro.urdf
+++ b/agv_pro_description/urdf/agv_pro.urdf
@@ -221,4 +221,12 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/navigation2/nav2_rviz_plugins/CMakeLists.txt b/navigation2/nav2_rviz_plugins/CMakeLists.txt
index 39bdbab..44ac329 100644
--- a/navigation2/nav2_rviz_plugins/CMakeLists.txt
+++ b/navigation2/nav2_rviz_plugins/CMakeLists.txt
@@ -36,6 +36,7 @@ set(nav2_rviz_plugins_headers_to_moc
include/nav2_rviz_plugins/goal_pose_updater.hpp
include/nav2_rviz_plugins/goal_common.hpp
include/nav2_rviz_plugins/goal_tool.hpp
+ include/nav2_rviz_plugins/charger_tool.hpp
include/nav2_rviz_plugins/nav2_panel.hpp
include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp
include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp
@@ -49,6 +50,7 @@ set(library_name ${PROJECT_NAME})
add_library(${library_name} SHARED
src/goal_tool.cpp
+ src/charger_tool.cpp
src/nav2_panel.cpp
src/particle_cloud_display/flat_weighted_arrows_array.cpp
src/particle_cloud_display/particle_cloud_display.cpp
diff --git a/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp b/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp
new file mode 100644
index 0000000..673bf3e
--- /dev/null
+++ b/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp
@@ -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 NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
+#define NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
+
+#include
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/qos.hpp"
+#include
+
+#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 nav2_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::SharedPtr publisher_;
+ rclcpp::Clock::SharedPtr clock_;
+ rviz_common::properties::StringProperty * topic_property_;
+ rviz_common::properties::QosProfileProperty * qos_profile_property_;
+
+ rclcpp::QoS qos_profile_;
+};
+
+} // namespace nav2_rviz_plugins
+
+#endif // NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
diff --git a/navigation2/nav2_rviz_plugins/plugins_description.xml b/navigation2/nav2_rviz_plugins/plugins_description.xml
index 197a9a7..0eb7522 100644
--- a/navigation2/nav2_rviz_plugins/plugins_description.xml
+++ b/navigation2/nav2_rviz_plugins/plugins_description.xml
@@ -6,6 +6,12 @@
A tool used to specify the navigation goal pose.
+
+ A tool used to specify the charging stations goal pose.
+
+
diff --git a/navigation2/nav2_rviz_plugins/src/charger_tool.cpp b/navigation2/nav2_rviz_plugins/src/charger_tool.cpp
new file mode 100644
index 0000000..e2e685d
--- /dev/null
+++ b/navigation2/nav2_rviz_plugins/src/charger_tool.cpp
@@ -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 "nav2_rviz_plugins/charger_tool.hpp"
+
+#include
+#include
+
+#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 nav2_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(
+ 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 nav2_rviz_plugins
+
+#include // NOLINT
+PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ChargerTool, rviz_common::Tool)
diff --git a/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd.rviz b/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd.rviz
new file mode 100755
index 0000000..5cf819a
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd.rviz
@@ -0,0 +1,756 @@
+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: 418
+ - 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
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: LaserScan
+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:
+ 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
+ camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ imu_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ 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: false
+ base_footprint:
+ Value: true
+ base_link:
+ Value: true
+ camera_accel_frame:
+ Value: true
+ camera_accel_optical_frame:
+ Value: true
+ camera_color_frame:
+ Value: true
+ camera_color_optical_frame:
+ Value: true
+ camera_depth_frame:
+ Value: true
+ camera_depth_optical_frame:
+ Value: true
+ camera_gyro_frame:
+ Value: true
+ camera_gyro_optical_frame:
+ Value: true
+ camera_ir_frame:
+ Value: true
+ camera_ir_optical_frame:
+ Value: true
+ camera_link:
+ Value: true
+ imu_link:
+ Value: true
+ laser_link:
+ Value: true
+ left_front_wheel_link:
+ Value: true
+ left_rear_wheel_link:
+ Value: true
+ map:
+ Value: true
+ odom:
+ 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:
+ map:
+ odom:
+ base_footprint:
+ base_link:
+ camera_link:
+ camera_depth_frame:
+ camera_accel_frame:
+ camera_accel_optical_frame:
+ {}
+ camera_color_frame:
+ camera_color_optical_frame:
+ {}
+ camera_depth_optical_frame:
+ {}
+ camera_gyro_frame:
+ camera_gyro_optical_frame:
+ {}
+ camera_ir_frame:
+ camera_ir_optical_frame:
+ {}
+ imu_link:
+ {}
+ laser_link:
+ {}
+ left_front_wheel_link:
+ {}
+ left_rear_wheel_link:
+ {}
+ right_front_wheel_link:
+ {}
+ right_rear_wheel_link:
+ {}
+ 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: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 227
+ Min Color: 0; 0; 0
+ Min Intensity: 17
+ Name: LaserScan
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ 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: false
+ - 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
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 0.036771878600120544
+ Min Value: -0.163384810090065
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: AxisColor
+ 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: PointCloud2
+ 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: /camera/ground
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 0.3959618806838989
+ Min Value: 0.05118126422166824
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: AxisColor
+ 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: PointCloud2
+ 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: /camera/obstacles
+ Use Fixed Frame: true
+ Use rainbow: true
+ 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:
+ Class: rviz_default_plugins/Orbit
+ Distance: 5.277319431304932
+ 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: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.7853981852531433
+ Target Frame:
+ Value: Orbit (rviz_default_plugins)
+ Yaw: 0.7853981852531433
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1016
+ Hide Left Dock: false
+ Hide Right Dock: true
+ Navigation 2:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001a20000035afc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000760000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001df000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320100000222000001750000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff00000001000002440000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005920000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ RealsenseCamera:
+ collapsed: false
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1850
+ X: 70
+ Y: 27
diff --git a/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd_scan.rviz b/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd_scan.rviz
new file mode 100755
index 0000000..bfe4e7f
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd_scan.rviz
@@ -0,0 +1,819 @@
+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: 417
+ - 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
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: LaserScan
+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:
+ 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
+ camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ imu_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ 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: false
+ base_footprint:
+ Value: true
+ base_link:
+ Value: true
+ camera_accel_frame:
+ Value: true
+ camera_accel_optical_frame:
+ Value: true
+ camera_color_frame:
+ Value: true
+ camera_color_optical_frame:
+ Value: true
+ camera_depth_frame:
+ Value: true
+ camera_depth_optical_frame:
+ Value: true
+ camera_gyro_frame:
+ Value: true
+ camera_gyro_optical_frame:
+ Value: true
+ camera_ir_frame:
+ Value: true
+ camera_ir_optical_frame:
+ Value: true
+ camera_link:
+ Value: true
+ imu_link:
+ Value: true
+ laser_link:
+ Value: true
+ left_front_wheel_link:
+ Value: true
+ left_rear_wheel_link:
+ Value: true
+ map:
+ Value: true
+ odom:
+ 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:
+ map:
+ odom:
+ base_footprint:
+ base_link:
+ camera_link:
+ camera_depth_frame:
+ camera_accel_frame:
+ camera_accel_optical_frame:
+ {}
+ camera_color_frame:
+ camera_color_optical_frame:
+ {}
+ camera_depth_optical_frame:
+ {}
+ camera_gyro_frame:
+ camera_gyro_optical_frame:
+ {}
+ camera_ir_frame:
+ camera_ir_optical_frame:
+ {}
+ imu_link:
+ {}
+ laser_link:
+ {}
+ left_front_wheel_link:
+ {}
+ left_rear_wheel_link:
+ {}
+ right_front_wheel_link:
+ {}
+ right_rear_wheel_link:
+ {}
+ 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: 228
+ Min Color: 0; 0; 0
+ Min Intensity: 7
+ Name: LaserScan
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ 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: false
+ 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
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 0.048584431409835815
+ Min Value: -0.123008593916893
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: AxisColor
+ 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: PointCloud2
+ 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: /camera/ground
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 0.39776936173439026
+ Min Value: 0.05130460113286972
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: AxisColor
+ 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: PointCloud2
+ 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: /camera/obstacles
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Class: rtabmap_rviz_plugins/MapGraph
+ Enabled: true
+ Global loop closure: 255; 0; 0
+ Landmark: 0; 128; 0
+ Local loop closure: 255; 255; 0
+ Merged neighbor: 255; 170; 0
+ Name: MapGraph
+ Neighbor: 0; 0; 255
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /mapGraph
+ User: 255; 0; 0
+ Value: true
+ Virtual: 255; 0; 255
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rtabmap_rviz_plugins/MapCloud
+ Cloud decimation: 4
+ Cloud from scan: false
+ Cloud max depth (m): 4
+ Cloud min depth (m): 0
+ Cloud voxel size (m): 0.009999999776482582
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ Download graph: false
+ Download map: false
+ Download namespace: rtabmap
+ Enabled: true
+ Filter ceiling (m): 0
+ Filter floor (m): 0
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: MapCloud
+ Node filtering angle (degrees): 30
+ Node filtering radius (m): 0
+ Position Transformer: XYZ
+ 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: /mapData
+ Use Fixed Frame: true
+ Use rainbow: true
+ 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:
+ Class: rviz_default_plugins/Orbit
+ Distance: 4.3704118728637695
+ 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: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.7853981852531433
+ Target Frame:
+ Value: Orbit (rviz_default_plugins)
+ Yaw: 0.7853981852531433
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1016
+ Hide Left Dock: false
+ Hide Right Dock: true
+ Navigation 2:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001a20000035afc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000760000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320100000221000001760000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff00000001000002fb0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005920000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ RealsenseCamera:
+ collapsed: false
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1850
+ X: 70
+ Y: 27
diff --git a/rtabmap_ros/rtabmap_demos/config/rtabmap_scan.rviz b/rtabmap_ros/rtabmap_demos/config/rtabmap_scan.rviz
new file mode 100755
index 0000000..6fb706d
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/config/rtabmap_scan.rviz
@@ -0,0 +1,683 @@
+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: 370
+ - 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
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: LaserScan
+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:
+ 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
+ camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ imu_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ 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: false
+ base_footprint:
+ Value: true
+ base_link:
+ Value: true
+ camera_accel_frame:
+ Value: true
+ camera_accel_optical_frame:
+ Value: true
+ camera_color_frame:
+ Value: true
+ camera_color_optical_frame:
+ Value: true
+ camera_depth_frame:
+ Value: true
+ camera_depth_optical_frame:
+ Value: true
+ camera_gyro_frame:
+ Value: true
+ camera_gyro_optical_frame:
+ Value: true
+ camera_ir_frame:
+ Value: true
+ camera_ir_optical_frame:
+ Value: true
+ camera_link:
+ Value: true
+ imu_link:
+ Value: true
+ laser_link:
+ Value: true
+ left_front_wheel_link:
+ Value: true
+ left_rear_wheel_link:
+ Value: true
+ map:
+ Value: true
+ odom:
+ 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:
+ map:
+ odom:
+ base_footprint:
+ base_link:
+ camera_link:
+ camera_depth_frame:
+ camera_accel_frame:
+ camera_accel_optical_frame:
+ {}
+ camera_color_frame:
+ camera_color_optical_frame:
+ {}
+ camera_depth_optical_frame:
+ {}
+ camera_gyro_frame:
+ camera_gyro_optical_frame:
+ {}
+ camera_ir_frame:
+ camera_ir_optical_frame:
+ {}
+ imu_link:
+ {}
+ laser_link:
+ {}
+ left_front_wheel_link:
+ {}
+ left_rear_wheel_link:
+ {}
+ right_front_wheel_link:
+ {}
+ right_rear_wheel_link:
+ {}
+ 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: 230
+ Min Color: 0; 0; 0
+ Min Intensity: 8
+ Name: LaserScan
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ 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.5707999467849731
+ 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: 171.56951904296875
+ Target Frame:
+ Value: TopDownOrtho (rviz_default_plugins)
+ X: 0
+ Y: 0
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 932
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Navigation 2:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001a200000306fc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000760000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000001f2000001510000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff00000001000002fb0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006090000003efc0100000002fb0000000800540069006d0065010000000000000609000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004610000030600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ RealsenseCamera:
+ collapsed: false
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1545
+ X: 208
+ Y: 43
diff --git a/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_mapping.launch.py b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_mapping.launch.py
new file mode 100644
index 0000000..a493ab0
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_mapping.launch.py
@@ -0,0 +1,109 @@
+# Requirements:
+# Compile agv_pro_base and agv_pro_bringup packages
+#
+# Example:
+# $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
+# $ ros2 launch orbbec_camera gemini2.launch.py
+# $ ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true
+# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.actions import SetParameter
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+ localization = LaunchConfiguration('localization')
+
+ parameters={
+ 'frame_id':'base_footprint',
+ 'odom_frame_id':'odom',
+ 'odom_tf_linear_variance':0.001,
+ 'odom_tf_angular_variance':0.001,
+ 'subscribe_rgbd':True,
+ 'subscribe_scan':True,
+ 'approx_sync':True,
+ 'sync_queue_size': 10,
+ # RTAB-Map's internal parameters should be strings
+ 'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans
+ 'RGBD/ProximityBySpace': 'true', # Local loop closure detection (using estimated position) with locations in WM
+ 'RGBD/ProximityByTime': 'false', # Local loop closure detection with locations in STM
+ 'RGBD/ProximityPathMaxNeighbors': '10', # Do also proximity detection by space by merging close scans together.
+ 'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP
+ 'Vis/MinInliers': '12', # 3D visual words minimum inliers to accept loop closure
+ 'RGBD/OptimizeFromGraphEnd': 'false', # Optimize graph from initial node so /map -> /odom transform will be generated
+ 'RGBD/OptimizeMaxError': '4', # Reject any loop closure causing large errors (>3x link's covariance) in the map
+ 'Reg/Force3DoF': 'true', # 2D SLAM
+ 'Grid/FromDepth': 'false', # Create 2D occupancy grid from laser scan
+ 'Mem/STMSize': '30', # increased to 30 to avoid adding too many loop closures on just seen locations
+ 'RGBD/LocalRadius': '5', # limit length of proximity detections
+ 'Icp/CorrespondenceRatio': '0.2', # minimum scan overlap to accept loop closure
+ 'Icp/PM': 'false',
+ 'Icp/PointToPlane': 'false',
+ 'Icp/MaxCorrespondenceDistance': '0.15',
+ 'Icp/VoxelSize': '0.05'
+ }
+
+ remappings=[
+ ('rgb/image', '/camera/color/image_raw'),
+ ('depth/image', '/camera/depth/image_raw'),
+ ('rgb/camera_info', '/camera/color/camera_info'),
+ ('scan', '/scan')]
+
+ config_rviz = os.path.join(
+ get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
+ )
+
+ return LaunchDescription([
+
+ # Launch arguments
+ DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'),
+ DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'),
+ DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
+ DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
+
+ SetParameter(name='use_sim_time', value=False),
+
+ # Nodes to launch
+ Node(
+ package='rtabmap_sync', executable='rgbd_sync', output='screen',
+ parameters=[parameters,
+ {
+ # 'rgb_image_transport':'compressed',
+ # 'depth_image_transport':'compressedDepth',
+ 'approx_sync_max_interval': 0.1}],
+ remappings=remappings),
+
+ # SLAM mode:
+ Node(
+ condition=UnlessCondition(localization),
+ package='rtabmap_slam', executable='rtabmap', output='screen',
+ parameters=[parameters],
+ remappings=remappings,
+ arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
+
+ # Localization mode:
+ Node(
+ condition=IfCondition(localization),
+ package='rtabmap_slam', executable='rtabmap', output='screen',
+ parameters=[parameters,
+ {'Mem/IncrementalMemory':'False',
+ 'Mem/InitWMWithAllNodes':'True'}],
+ remappings=remappings),
+
+ # Visualization:
+ Node(
+ package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+ condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
+ parameters=[parameters],
+ remappings=remappings),
+ Node(
+ package='rviz2', executable='rviz2', name="rviz2", output='screen',
+ condition=IfCondition(LaunchConfiguration("rviz")),
+ arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
+ ])
diff --git a/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd.launch.py b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd.launch.py
new file mode 100644
index 0000000..805265f
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd.launch.py
@@ -0,0 +1,132 @@
+# Requirements:
+# Compile agv_pro_base and agv_pro_bringup packages
+#
+# Example:
+# Bringup agvpro:
+# $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
+#
+# Bringup orbbec gemini2 camera:
+# $ ros2 launch orbbec_camera gemini2.launch.py
+#
+# SLAM:
+# $ ros2 launch rtabmap_demos agvpro_rgbd_demo.launch.py
+#
+# Teleop:
+# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription, OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+
+ use_sim_time = LaunchConfiguration('use_sim_time')
+ localization = LaunchConfiguration('localization')
+
+ parameters={
+ 'frame_id':'base_footprint',
+ 'use_sim_time':use_sim_time,
+ 'subscribe_depth':True,
+ 'use_action_for_goal':True,
+ 'Reg/Force3DoF':'true',
+ 'Grid/RayTracing':'true', # Fill empty space
+ 'Grid/3D':'false', # Use 2D occupancy
+ 'Grid/RangeMax':'3',
+ 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
+ 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
+ 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored
+ 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
+ }
+
+ remappings=[
+ ('rgb/image', '/camera/color/image_raw'),
+ ('rgb/camera_info', '/camera/color/camera_info'),
+ ('depth/image', '/camera/depth/image_raw')]
+
+ # Directories
+ pkg_nav2_bringup = get_package_share_directory(
+ 'nav2_bringup')
+
+ nav2_params_file = PathJoinSubstitution(
+ [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_rgbd_nav2_params.yaml']
+ )
+
+ # Paths
+ nav2_launch = PathJoinSubstitution(
+ [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+ rviz_launch = PathJoinSubstitution(
+ [pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
+
+ return LaunchDescription([
+
+ # Launch arguments
+ DeclareLaunchArgument(
+ 'use_sim_time', default_value='false',
+ description='Use simulation (Gazebo) clock if true'),
+
+ DeclareLaunchArgument(
+ 'localization', default_value='false',
+ description='Launch in localization mode.'),
+
+ # Nodes to launch
+
+ # Navigation2
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([nav2_launch]),
+ launch_arguments=[
+ ('use_sim_time', 'false'),
+ ('params_file', nav2_params_file)
+ ]
+ ),
+
+ # RViz
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([rviz_launch])
+ ),
+
+ # SLAM mode:
+ Node(
+ condition=UnlessCondition(localization),
+ package='rtabmap_slam', executable='rtabmap', output='screen',
+ parameters=[parameters],
+ remappings=remappings,
+ arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
+
+ # Localization mode:
+ Node(
+ condition=IfCondition(localization),
+ package='rtabmap_slam', executable='rtabmap', output='screen',
+ parameters=[parameters,
+ {'Mem/IncrementalMemory':'False',
+ 'Mem/InitWMWithAllNodes':'True'}],
+ remappings=remappings),
+
+ Node(
+ package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+ parameters=[parameters],
+ remappings=remappings),
+
+ # Obstacle detection with the camera for nav2 local costmap.
+ # First, we need to convert depth image to a point cloud.
+ # Second, we segment the floor from the obstacles.
+ Node(
+ package='rtabmap_util', executable='point_cloud_xyz', output='screen',
+ parameters=[{'decimation': 2,
+ 'max_depth': 3.0,
+ 'voxel_size': 0.02}],
+ remappings=[('depth/image', '/camera/depth/image_raw'),
+ ('depth/camera_info', '/camera/depth/camera_info'),
+ ('cloud', '/camera/depth_registered/points')]),
+ Node(
+ package='rtabmap_util', executable='obstacles_detection', output='screen',
+ parameters=[parameters],
+ remappings=[('cloud', '/camera/depth_registered/points'),
+ ('obstacles', '/camera/obstacles'),
+ ('ground', '/camera/ground')]),
+ ])
diff --git a/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd_scan.launch.py b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd_scan.launch.py
new file mode 100644
index 0000000..63e8719
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd_scan.launch.py
@@ -0,0 +1,161 @@
+# Requirements:
+# Compile agv_pro_base and agv_pro_bringup packages
+#
+# Example:
+# Bringup agvpro:
+# $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
+#
+# Bringup orbbec gemini2 camera:
+# $ ros2 launch orbbec_camera gemini2.launch.py
+#
+# SLAM:
+# $ ros2 launch rtabmap_demos agvpro_rgbd_scan.launch.py
+#
+# Teleop:
+# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
+
+import os
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+
+ use_sim_time = LaunchConfiguration('use_sim_time')
+ localization = LaunchConfiguration('localization')
+
+ parameters={
+ 'frame_id':'base_footprint',
+ 'use_sim_time':use_sim_time,
+ 'subscribe_rgbd':True,
+ 'subscribe_scan':True,
+ 'use_action_for_goal':True,
+ 'approx_sync':True,
+ 'sync_queue_size': 10,
+ # RTAB-Map's parameters should be strings:
+ 'Reg/Strategy':'1',
+ 'Reg/Force3DoF':'true',
+ 'RGBD/NeighborLinkRefining':'True',
+ 'Grid/RayTracing':'true', # Fill empty space
+ 'Grid/3D':'false', # Use 2D occupancy
+ 'Grid/RangeMax':'3',
+ 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
+ 'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map
+ 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
+ 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored
+ 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
+ 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
+ }
+
+ remappings=[
+ ('rgb/image', '/camera/color/image_raw'),
+ ('depth/image', '/camera/depth/image_raw'),
+ ('rgb/camera_info', '/camera/color/camera_info'),
+ ('scan', '/scan')]
+
+ # Directories
+ pkg_nav2_bringup = get_package_share_directory(
+ 'nav2_bringup')
+
+ nav2_params_file = PathJoinSubstitution(
+ [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_rgbd_scan_nav2_params.yaml']
+ )
+
+ # Paths
+ nav2_launch = PathJoinSubstitution(
+ [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+
+ return LaunchDescription([
+
+ # Launch arguments
+ DeclareLaunchArgument(
+ 'use_sim_time', default_value='false',
+ description='Use simulation (Gazebo) clock if true'),
+
+ DeclareLaunchArgument(
+ 'localization', default_value='false',
+ description='Launch in localization mode.'),
+
+ DeclareLaunchArgument(
+ 'rtabmap_viz', default_value='false',
+ description='Launch RTAB-Map UI (optional).'),
+
+ DeclareLaunchArgument(
+ 'rviz', default_value='true',
+ description='Launch RVIZ (optional).'),
+
+ DeclareLaunchArgument(
+ 'rviz_cfg', default_value=os.path.join(
+ get_package_share_directory('rtabmap_demos'), 'config', 'rtabmap_rgbd_scan.rviz'),
+ description='Configuration path of rviz2.'),
+
+ # Nodes to launch
+ # Navigation2
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([nav2_launch]),
+ launch_arguments=[
+ ('use_sim_time', 'false'),
+ ('params_file', nav2_params_file)
+ ]
+ ),
+
+ Node(
+ package='rtabmap_sync', executable='rgbd_sync', output='screen',
+ parameters=[parameters,
+ {
+ 'use_sim_time':use_sim_time,
+ 'approx_sync_max_interval': 0.1}],
+ remappings=remappings),
+
+ # SLAM Mode:
+ Node(
+ condition=UnlessCondition(localization),
+ package='rtabmap_slam', executable='rtabmap', output='screen',
+ parameters=[parameters],
+ remappings=remappings,
+ arguments=['-d']),
+
+ # Localization mode:
+ Node(
+ condition=IfCondition(localization),
+ package='rtabmap_slam', executable='rtabmap', output='screen',
+ parameters=[parameters,
+ {'Mem/IncrementalMemory':'False',
+ 'Mem/InitWMWithAllNodes':'True'}],
+ remappings=remappings),
+
+ # Visualization:
+ Node(
+ package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+ condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
+ parameters=[parameters],
+ remappings=remappings),
+ Node(
+ package='rviz2', executable='rviz2', name="rviz2", output='screen',
+ condition=IfCondition(LaunchConfiguration("rviz")),
+ arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
+
+ # Obstacle detection with the camera for nav2 local costmap.
+ # First, we need to convert depth image to a point cloud.
+ # Second, we segment the floor from the obstacles.
+ Node(
+ package='rtabmap_util', executable='point_cloud_xyz', output='screen',
+ parameters=[{'decimation': 2,
+ 'max_depth': 3.0,
+ 'voxel_size': 0.02}],
+ remappings=[('depth/image', '/camera/depth/image_raw'),
+ ('depth/camera_info', '/camera/depth/camera_info'),
+ ('cloud', '/camera/depth_registered/points')]),
+ Node(
+ package='rtabmap_util', executable='obstacles_detection', output='screen',
+ parameters=[parameters],
+ remappings=[('cloud', '/camera/depth_registered/points'),
+ ('obstacles', '/camera/obstacles'),
+ ('ground', '/camera/ground')]),
+ ])
diff --git a/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_scan.launch.py b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_scan.launch.py
new file mode 100644
index 0000000..599c991
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_scan.launch.py
@@ -0,0 +1,133 @@
+# Requirements:
+# Compile agv_pro_base and agv_pro_bringup packages
+#
+# Example:
+# Bringup agvpro:
+# $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
+#
+# SLAM:
+# $ ros2 launch rtabmap_demos agvpro_scan_demo.launch.py
+#
+# Teleop:
+# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction,IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def launch_setup(context, *args, **kwargs):
+ use_sim_time = LaunchConfiguration('use_sim_time')
+ localization = LaunchConfiguration('localization').perform(context)
+ localization = localization == 'True' or localization == 'true'
+ icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
+ icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
+
+ parameters={
+ 'frame_id':'base_footprint',
+ 'use_sim_time':use_sim_time,
+ 'subscribe_depth':False,
+ 'subscribe_rgb':False,
+ 'subscribe_scan':True,
+ 'approx_sync':True,
+ 'use_action_for_goal':True,
+ 'Reg/Strategy':'1',
+ 'Reg/Force3DoF':'true',
+ 'RGBD/NeighborLinkRefining':'True',
+ 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
+ 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
+ }
+ arguments = []
+ if localization:
+ parameters['Mem/IncrementalMemory'] = 'False'
+ parameters['Mem/InitWMWithAllNodes'] = 'True'
+ else:
+ arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
+
+ remappings=[
+ ('scan', '/scan')]
+ if icp_odometry:
+ remappings.append(('odom', 'icp_odom'))
+ # modified nav2 params to use icp_odom instead odom frame
+ nav2_params_file = PathJoinSubstitution(
+ [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_scan_nav2_params.yaml']
+ )
+ else:
+ # original nav2 params
+ nav2_params_file = PathJoinSubstitution(
+ [FindPackageShare('agv_pro_navigation2'), 'param', 'agvpro.yaml']
+ )
+
+ # Directories
+ pkg_nav2_bringup = get_package_share_directory(
+ 'nav2_bringup')
+
+ # Paths
+ nav2_launch = PathJoinSubstitution(
+ [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+ rviz_launch = PathJoinSubstitution(
+ [pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
+
+ return [
+ # Nodes to launch
+
+ # Navigation2
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([nav2_launch]),
+ launch_arguments=[
+ ('use_sim_time', 'false'),
+ ('params_file', nav2_params_file)
+ ]
+ ),
+
+ # RViz
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([rviz_launch])
+ ),
+
+ # ICP odometry (optional)
+ Node(
+ condition=IfCondition(LaunchConfiguration('icp_odometry')),
+ package='rtabmap_odom', executable='icp_odometry', output='screen',
+ parameters=[parameters,
+ {'odom_frame_id':'icp_odom',
+ 'guess_frame_id':'odom'}],
+ remappings=remappings),
+
+ # SLAM:
+ Node(
+ package='rtabmap_slam', executable='rtabmap', output='screen',
+ parameters=[parameters],
+ remappings=remappings,
+ arguments=arguments),
+
+ # Visualization
+ Node(
+ package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+ parameters=[parameters],
+ remappings=remappings),
+ ]
+
+def generate_launch_description():
+ return LaunchDescription([
+
+ # Launch arguments
+ DeclareLaunchArgument(
+ 'use_sim_time', default_value='false',
+ description='Use simulation (Gazebo) clock if true'),
+
+ DeclareLaunchArgument(
+ 'localization', default_value='false',
+ description='Launch in localization mode.'),
+
+ DeclareLaunchArgument(
+ 'icp_odometry', default_value='false',
+ description='Launch ICP odometry on top of wheel odometry.'),
+
+ OpaqueFunction(function=launch_setup)
+ ])
diff --git a/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_nav2_params.yaml b/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_nav2_params.yaml
new file mode 100644
index 0000000..5a07ca9
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_nav2_params.yaml
@@ -0,0 +1,301 @@
+# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap.
+bt_navigator:
+ ros__parameters:
+ use_sim_time: True
+ global_frame: map
+ robot_base_frame: base_footprint
+ odom_topic: /odom
+ bt_loop_duration: 10
+ default_server_timeout: 20
+ wait_for_service_timeout: 1000
+ # '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_smooth_path_action_bt_node
+ - nav2_follow_path_action_bt_node
+ - nav2_spin_action_bt_node
+ - nav2_wait_action_bt_node
+ - nav2_assisted_teleop_action_bt_node
+ - nav2_back_up_action_bt_node
+ - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node
+ - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition
+ - 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
+ - nav2_controller_cancel_bt_node
+ - nav2_path_longer_on_approach_bt_node
+ - nav2_wait_cancel_bt_node
+ - nav2_spin_cancel_bt_node
+ - nav2_back_up_cancel_bt_node
+ - nav2_assisted_teleop_cancel_bt_node
+ - nav2_drive_on_heading_cancel_bt_node
+ - nav2_is_battery_charging_condition_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+controller_server:
+ ros__parameters:
+ use_sim_time: True
+ 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.5
+ movement_time_allowance: 10.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: 1.0
+ 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: 20
+ sim_time: 1.7
+ linear_granularity: 0.05
+ angular_granularity: 0.025
+ transform_tolerance: 0.2
+ xy_goal_tolerance: 0.25
+ trans_stopped_velocity: 0.25
+ short_circuit_trajectory_evaluation: True
+ stateful: True
+ critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+ BaseObstacle.scale: 0.02
+ PathAlign.scale: 32.0
+ PathAlign.forward_point_distance: 0.1
+ GoalAlign.scale: 24.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
+
+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: True
+ 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: 3.0
+ inflation_radius: 0.55
+ 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 ground obstacles
+ 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
+ ground:
+ topic: /camera/ground
+ max_obstacle_height: 0.4
+ clearing: True
+ marking: False
+ data_type: "PointCloud2"
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
+ obstacles:
+ topic: /camera/obstacles
+ max_obstacle_height: 0.4
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
+ 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
+ always_send_full_costmap: True
+
+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: True
+ 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", "inflation_layer"]
+ static_layer:
+ plugin: "nav2_costmap_2d::StaticLayer"
+ map_subscribe_transient_local: True
+ inflation_layer:
+ plugin: "nav2_costmap_2d::InflationLayer"
+ cost_scaling_factor: 3.0
+ inflation_radius: 0.55
+ always_send_full_costmap: True
+
+planner_server:
+ ros__parameters:
+ expected_planner_frequency: 20.0
+ use_sim_time: True
+ planner_plugins: ["GridBased"]
+ GridBased:
+ plugin: "nav2_navfn_planner/NavfnPlanner"
+ tolerance: 0.5
+ use_astar: false
+ allow_unknown: true
+
+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
+
+behavior_server:
+ ros__parameters:
+ costmap_topic: local_costmap/costmap_raw
+ footprint_topic: local_costmap/published_footprint
+ cycle_frequency: 10.0
+ behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+ spin:
+ plugin: "nav2_behaviors/Spin"
+ backup:
+ plugin: "nav2_behaviors/BackUp"
+ drive_on_heading:
+ plugin: "nav2_behaviors/DriveOnHeading"
+ wait:
+ plugin: "nav2_behaviors/Wait"
+ assisted_teleop:
+ plugin: "nav2_behaviors/AssistedTeleop"
+ global_frame: odom
+ robot_base_frame: base_footprint
+ transform_tolerance: 0.1
+ use_sim_time: true
+ 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: True
+
+waypoint_follower:
+ ros__parameters:
+ use_sim_time: True
+ 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, 1.0]
+ min_velocity: [-0.26, 0.0, -1.0]
+ 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
diff --git a/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_scan_nav2_params.yaml b/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_scan_nav2_params.yaml
new file mode 100644
index 0000000..7efa6b5
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_scan_nav2_params.yaml
@@ -0,0 +1,301 @@
+# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap.
+bt_navigator:
+ ros__parameters:
+ use_sim_time: True
+ global_frame: map
+ robot_base_frame: base_footprint
+ odom_topic: /odom
+ bt_loop_duration: 10
+ default_server_timeout: 20
+ wait_for_service_timeout: 1000
+ # '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_smooth_path_action_bt_node
+ - nav2_follow_path_action_bt_node
+ - nav2_spin_action_bt_node
+ - nav2_wait_action_bt_node
+ - nav2_assisted_teleop_action_bt_node
+ - nav2_back_up_action_bt_node
+ - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node
+ - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition
+ - 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
+ - nav2_controller_cancel_bt_node
+ - nav2_path_longer_on_approach_bt_node
+ - nav2_wait_cancel_bt_node
+ - nav2_spin_cancel_bt_node
+ - nav2_back_up_cancel_bt_node
+ - nav2_assisted_teleop_cancel_bt_node
+ - nav2_drive_on_heading_cancel_bt_node
+ - nav2_is_battery_charging_condition_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+controller_server:
+ ros__parameters:
+ use_sim_time: True
+ 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: 1.0
+ 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: 20
+ sim_time: 1.7
+ linear_granularity: 0.05
+ angular_granularity: 0.025
+ transform_tolerance: 0.2
+ xy_goal_tolerance: 0.25
+ trans_stopped_velocity: 0.25
+ short_circuit_trajectory_evaluation: True
+ stateful: True
+ critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+ BaseObstacle.scale: 0.02
+ PathAlign.scale: 32.0
+ PathAlign.forward_point_distance: 0.1
+ GoalAlign.scale: 24.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
+
+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: True
+ 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: 3.0
+ inflation_radius: 0.55
+ 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 ground obstacles
+ 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
+ ground:
+ topic: /camera/ground
+ max_obstacle_height: 0.4
+ clearing: True
+ marking: False
+ data_type: "PointCloud2"
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
+ obstacles:
+ topic: /camera/obstacles
+ max_obstacle_height: 0.4
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
+ 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
+ always_send_full_costmap: True
+
+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: True
+ 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", "inflation_layer"]
+ static_layer:
+ plugin: "nav2_costmap_2d::StaticLayer"
+ map_subscribe_transient_local: True
+ inflation_layer:
+ plugin: "nav2_costmap_2d::InflationLayer"
+ cost_scaling_factor: 3.0
+ inflation_radius: 0.55
+ always_send_full_costmap: True
+
+planner_server:
+ ros__parameters:
+ expected_planner_frequency: 20.0
+ use_sim_time: True
+ planner_plugins: ["GridBased"]
+ GridBased:
+ plugin: "nav2_navfn_planner/NavfnPlanner"
+ tolerance: 0.5
+ use_astar: false
+ allow_unknown: true
+
+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
+
+behavior_server:
+ ros__parameters:
+ costmap_topic: local_costmap/costmap_raw
+ footprint_topic: local_costmap/published_footprint
+ cycle_frequency: 10.0
+ behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+ spin:
+ plugin: "nav2_behaviors/Spin"
+ backup:
+ plugin: "nav2_behaviors/BackUp"
+ drive_on_heading:
+ plugin: "nav2_behaviors/DriveOnHeading"
+ wait:
+ plugin: "nav2_behaviors/Wait"
+ assisted_teleop:
+ plugin: "nav2_behaviors/AssistedTeleop"
+ global_frame: odom
+ robot_base_frame: base_footprint
+ transform_tolerance: 0.1
+ use_sim_time: true
+ 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: True
+
+waypoint_follower:
+ ros__parameters:
+ use_sim_time: True
+ 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, 1.0]
+ min_velocity: [-0.26, 0.0, -1.0]
+ 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
diff --git a/rtabmap_ros/rtabmap_demos/params/agvpro_scan_nav2_params.yaml b/rtabmap_ros/rtabmap_demos/params/agvpro_scan_nav2_params.yaml
new file mode 100644
index 0000000..10b4d00
--- /dev/null
+++ b/rtabmap_ros/rtabmap_demos/params/agvpro_scan_nav2_params.yaml
@@ -0,0 +1,295 @@
+# Modified to use icp_odom frame
+bt_navigator:
+ ros__parameters:
+ use_sim_time: True
+ global_frame: map
+ robot_base_frame: base_footprint
+ odom_topic: /odom
+ bt_loop_duration: 10
+ default_server_timeout: 20
+ wait_for_service_timeout: 1000
+ # '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_smooth_path_action_bt_node
+ - nav2_follow_path_action_bt_node
+ - nav2_spin_action_bt_node
+ - nav2_wait_action_bt_node
+ - nav2_assisted_teleop_action_bt_node
+ - nav2_back_up_action_bt_node
+ - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node
+ - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition
+ - 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
+ - nav2_controller_cancel_bt_node
+ - nav2_path_longer_on_approach_bt_node
+ - nav2_wait_cancel_bt_node
+ - nav2_spin_cancel_bt_node
+ - nav2_back_up_cancel_bt_node
+ - nav2_assisted_teleop_cancel_bt_node
+ - nav2_drive_on_heading_cancel_bt_node
+ - nav2_is_battery_charging_condition_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+controller_server:
+ ros__parameters:
+ use_sim_time: True
+ 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: 2.25
+ decel_lim_x: -2.5
+ decel_lim_y: 0.0
+ decel_lim_theta: -2.25
+ vx_samples: 20
+ vy_samples: 5
+ vtheta_samples: 40
+ sim_time: 1.7
+ linear_granularity: 0.05
+ angular_granularity: 0.025
+ transform_tolerance: 0.2
+ xy_goal_tolerance: 0.25
+ trans_stopped_velocity: 0.25
+ 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
+
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ update_frequency: 5.0
+ publish_frequency: 2.0
+ global_frame: icp_odom
+ robot_base_frame: base_footprint
+ use_sim_time: True
+ 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: 3.0
+ inflation_radius: 0.55
+ 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:
+ plugin: "nav2_costmap_2d::StaticLayer"
+ map_subscribe_transient_local: True
+ always_send_full_costmap: True
+
+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: True
+ 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: 3.0
+ inflation_radius: 0.55
+ always_send_full_costmap: True
+
+planner_server:
+ ros__parameters:
+ expected_planner_frequency: 20.0
+ use_sim_time: True
+ planner_plugins: ["GridBased"]
+ GridBased:
+ plugin: "nav2_navfn_planner/NavfnPlanner"
+ tolerance: 0.5
+ use_astar: false
+ allow_unknown: true
+
+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
+
+behavior_server:
+ ros__parameters:
+ costmap_topic: local_costmap/costmap_raw
+ footprint_topic: local_costmap/published_footprint
+ cycle_frequency: 10.0
+ behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+ spin:
+ plugin: "nav2_behaviors/Spin"
+ backup:
+ plugin: "nav2_behaviors/BackUp"
+ drive_on_heading:
+ plugin: "nav2_behaviors/DriveOnHeading"
+ wait:
+ plugin: "nav2_behaviors/Wait"
+ assisted_teleop:
+ plugin: "nav2_behaviors/AssistedTeleop"
+ global_frame: icp_odom
+ robot_base_frame: base_footprint
+ transform_tolerance: 0.1
+ use_sim_time: true
+ 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: True
+
+waypoint_follower:
+ ros__parameters:
+ use_sim_time: True
+ 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, 1.0]
+ min_velocity: [-0.26, 0.0, -1.0]
+ 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