Merge pull request #1 from elephantrobotics/humble-imu

Add IMU support, auto recharge, RViz tool, and rtabmap demos
2025-09-29 19:09:08 +08:00
parent 6e8320c438 cf17bb899b
commit 0088e02b9f
27 changed files with 5208 additions and 126 deletions
@@ -0,0 +1,134 @@
+# AGV AutoCharge ROS2 Package
+
+这是一个为ROS2 Humble设计的AGV自动充电系统软件包。
+
+## 功能特性
+
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 键盘交互启动导航功能
+- 导航成功后启动串口控制
+- 支持充电状态检测和控制
+
+## 安装依赖
+
+确保您的系统已安装以下依赖：
+
+```bash
+# ROS2 Humble基础包
+sudo apt install ros-humble-rclpy
+sudo apt install ros-humble-geometry-msgs
+sudo apt install ros-humble-std-msgs
+sudo apt install ros-humble-nav-msgs
+sudo apt install ros-humble-visualization-msgs
+sudo apt install ros-humble-nav2-simple-commander
+
+# Python依赖
+pip3 install pyserial
+```
+
+## 编译安装
+
+```bash
+# 进入ROS2工作空间
+cd /path/to/your/ros2_ws/src
+
+# 复制软件包到工作空间
+cp -r agv_autocharge_ros2 .
+
+# 编译软件包
+cd ..
+colcon build --packages-select agv_autocharge_ros2
+
+# 加载环境变量
+source install/setup.bash
+```
+
+## 使用方法
+
+### 启动节点
+
+```bash
+# 启动自动充电控制器节点
+ros2 run agv_autocharge_ros2 combined_auto_recharger
+```
+
+### 节点功能
+
+- 监听 `/charger_position_update` 话题，接收充电桩位置更新
+- 发布 `/goal_marker` 话题，在RViz中显示充电桩位置标记
+- 发布 `/cmd_vel` 话题，控制机器人运动
+- 按键 `q` 启动导航到充电桩
+- 导航成功后自动启动串口控制
+
+### 配置文件
+
+充电桩位置配置文件位于：
+```
+config/charger_position.json
+```
+
+文件格式：
+```json
+{
+  "p_x": 1.329015495451769,
+  "p_y": 0.31961151635354823,
+  "orien_z": 0.4981823289472456,
+  "orien_w": 0.8670722963655905
+}
+```
+
+### 话题接口
+
+#### 订阅话题
+- `/charger_position_update` (geometry_msgs/PoseStamped): 充电桩位置更新
+
+#### 发布话题
+- `/goal_marker` (visualization_msgs/MarkerArray): 充电桩位置可视化标记
+- `/cmd_vel` (geometry_msgs/Twist): 机器人运动控制
+- `/chassis_security` (std_msgs/Int8): 底盘安全控制
+
+### 串口配置
+
+默认串口配置：
+- 端口: `/dev/ttyCH341USB0`
+- 波特率: 9600
+- 超时: 1秒
+
+可以根据需要修改代码中的串口参数。
+
+## 操作说明
+
+1. 启动节点后，系统会自动加载充电桩位置配置
+2. 系统会定期发布充电桩标记到RViz进行可视化
+3. 按下键盘上的 `q` 键启动导航到充电桩
+4. 导航成功后，系统会自动启动串口控制功能
+5. 串口控制会根据接收到的数据控制机器人运动
+6. 按 `Ctrl+C` 退出程序
+
+## 故障排除
+
+### 常见问题
+
+1. **串口无法打开**
+   - 检查串口设备是否连接
+   - 确认串口权限设置
+   - 验证串口设备名称
+
+2. **导航失败**
+   - 确认Nav2导航系统正常运行
+   - 检查充电桩位置配置是否正确
+   - 验证地图和定位系统状态
+
+3. **RViz中看不到标记**
+   - 确认RViz已订阅 `/goal_marker` 话题
+   - 检查MarkerArray显示设置
+   - 验证坐标系设置是否为'map'
+
+## 许可证
+
+MIT License
+
+## 维护者
+
+请联系维护者获取技术支持。
@@ -0,0 +1,10 @@
+"""
+AGV AutoCharge ROS2 Package
+
+This package provides automatic charging functionality for AGV robots using ROS2 Humble.
+It includes position management, visualization, navigation, and serial control features.
+"""
+
+__version__ = '1.0.0'
+__author__ = 'Your Name'
+__email__ = 'your-email@example.com'
@@ -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()
+
@@ -0,0 +1,173 @@
+#!/usr/bin/env python3                                                                                                           
+#coding=UTF-8
+import serial
+import time
+
+class SerialCANParser:
+    def __init__(self, serial_port='/dev/ttyCH341USB0', baudrate=9600, timeout=1):
+        self.serial_port = serial_port  # 串口名称
+        self.baudrate = baudrate  # 波特率
+        self.timeout = timeout  # 超时设置
+        self.ser = None  # 串口对象
+        self.buffer = bytearray()  # 存储当前读取的字节
+        self.max_retries = 3  # 最大重试次数
+        # 存储实时数据
+        self.x_speed = 0.0
+        self.z_speed = 0.0
+        self.infrared_bits = []
+        
+    def open_serial(self):
+        """打开串口"""
+        try:
+            self.ser = serial.Serial(self.serial_port, self.baudrate, timeout=self.timeout)
+            print(f"串口 {self.serial_port} 已打开，波特率：{self.baudrate}")
+        except Exception as e:
+            print(f"打开串口失败: {e}")
+            
+    def close_serial(self):
+        """关闭串口"""
+        if self.ser and self.ser.is_open:
+            self.ser.close()
+            print("串口已关闭。")
+        else:
+            print("串口未打开或已关闭。")
+            
+    def can_id_check(self, date):
+        high_byte, low_byte = date[0:2]
+        # 高字节左移 3 位
+        can_id = (high_byte << 3)
+        # 低字节右移 5 位
+        can_id |= (low_byte >> 5)
+        return can_id
+        
+    def parse_can_data(self, data):
+        """解析8字节CAN数据帧"""
+        if len(data) != 8:
+            print("数据帧长度不正确")
+            return None
+            
+        # 解析 X、Y 和 Z 速度
+        x_speed_raw = ((data[0] << 8) | data[1])  # X速度的原始数据
+        z_speed_raw = ((data[4] << 8) | data[5])  # Z速度的原始数据
+        
+        # 将原始数据转换为浮动数值，并考虑正负
+        if x_speed_raw & 0x8000:  # 如果最高位为1，表示负数
+            x_speed_raw = -((65536 - x_speed_raw) & 0xFFFF)  # 补码转换为负数
+        if z_speed_raw & 0x8000:  # 如果最高位为1，表示负数
+            z_speed_raw = -((65536 - z_speed_raw) & 0xFFFF)  # 补码转换为负数
+            
+        # 转换单位为 m/s 和 rad/s
+        self.x_speed = x_speed_raw / 1000.0  # X速度单位为 m/s
+        self.y_speed = 0  # Y速度为0
+        self.z_speed = z_speed_raw / 1000.0  # Z速度单位为 rad/s
+        self.which_mode = data[2]
+        self.infrared = data[6]  # 红外数据
+        self.raw_current = data[7]  # 电流数据
+        
+        if self.raw_current > 32767:  # 无符号数大于 32767 表示负值（因为最大值是 65535）
+            # 转换为负数
+            self.actual_current = -(65536 - self.raw_current) * 30.0
+        else:
+            # 正数直接转换
+            self.actual_current = self.raw_current * 30.0
+            
+        # 处理红外数据
+        self.infrared_bits = [(self.infrared >> (7 - i)) & 0x01 for i in range(8)]
+        
+        # 打印或处理数据
+        print(f"X Speed: {self.x_speed:.3f}, Y Speed: {self.y_speed}, Z Speed: {self.z_speed:.3f}, "
+              f"Actual Current: {self.actual_current:.3f} mA, Infrared: {self.infrared}")
+              
+        # 打印或处理红外位信息
+        print(f"L_A: {self.infrared_bits[2]}, L_B: {self.infrared_bits[3]}, R_B: {self.infrared_bits[4]}, "
+              f"R_A: {self.infrared_bits[5]}, infrared_flag : {self.infrared_bits[6]}, "
+              f"Charging flag: {self.infrared_bits[7]}")
+              
+    def read_serial_data(self):
+        """读取串口数据并解析"""
+        while True:  # 修改为简单的无限循环，由上层控制退出
+            if self.ser.in_waiting > 0:
+                byte = self.ser.read(1)   # 读取一个字节
+                if len(self.buffer) < 2:
+                    self.buffer.extend(byte)  
+                    if len(self.buffer) == 2:
+                        # 如果帧头为 0x41 0x54，表示为AT帧头，则开始接收数据
+                        if self.buffer[0] != 0x41 or self.buffer[1] != 0x54:
+                            # 如果不是有效的帧头，则清空缓冲区并跳到下次循环
+                            self.buffer.clear()
+                            continue  # 继续等待下一个字节                 
+                else:
+                    self.buffer.extend(byte)
+                    # print("缓冲区内容:", ' '.join(f'{b:02x}' for b in self.buffer)) # debug
+                    # 如果缓冲区字节长度大于等于 17 字节（数据帧长度）
+                    if len(self.buffer) >= 17:
+                        # print("Received Frame (Hex):", ' '.join(f'{byte:02x}' for byte in self.buffer)) # debug
+                        # 解析帧头、CAN帧ID、格式、类型和数据
+                        # at_frame_header = self.buffer[0:2]  # AT帧头
+                        can_frame_id = self.can_id_check(self.buffer[2:4])  # CAN标准帧ID
+                        # can_frame_format = self.buffer[4]  # CAN帧格式（0,标准帧;1,扩展帧）
+                        # can_frame_type = self.buffer[5]  # CAN帧类型（0,数据帧;1,远程帧）
+                        data_length = self.buffer[6]  # 数据长度
+                        data = self.buffer[7:15]  # 数据帧
+                        # print(f"帧ID: 0x{can_frame_id:X}")    # debug
+                        if can_frame_id == 0x182 and data_length == 0x08: # 根据can帧id进行判断
+                            # 如果帧ID为0x182,校验通过,进行数据赋值
+                            self.parse_can_data(data)
+                            # 清空缓冲区，准备下一帧数据
+                            self.buffer.clear()
+                            return self.x_speed, self.z_speed, self.which_mode, self.infrared_bits   # 返回解析后的数据
+                        else:
+                            # 清空缓冲区，准备下一帧数据
+                            self.buffer.clear()
+                            
+    def read_serial_response(self):
+        """读取串口响应数据，直到接收到 '\r\n' 或超时"""
+        response = bytearray()  # 使用 bytearray 来存储原始字节流
+        while True:
+            if self.ser.in_waiting > 0:
+                byte = self.ser.read(1)
+                response += byte
+            # 检查是否已接收到完整响应
+            if b'\r\n' in response:
+                break
+            # 超时机制，防止死循环
+            if len(response) > 100:
+                break
+        return bytes(response)  # 返回原始字节流（bytes）
+        
+    def send_at_commands(self, commands):
+        """发送 AT 命令并等待响应"""
+        for command in commands:
+            retries = 0
+            while retries < self.max_retries:
+                self.ser.write(command.encode() + b'\r\n')
+                print(f"发送命令: {command}")
+                # 等待响应并读取数据
+                response = self.read_serial_response()
+                # 检查响应是否包含 "OK"
+                if b"OK" in response:
+                    print(f"收到响应: {response}")
+                    break  # 如果收到 OK，退出重试循环
+                else:
+                    retries += 1
+                    print(f"未收到预期的响应，收到: {response}")
+            if retries == self.max_retries:
+                print(f"重试 {self.max_retries} 次后仍未收到有效响应，请检查设备。")
+                break
+                
+    def start(self):
+        """开始读取和处理数据"""
+        self.open_serial()
+        try:
+            # 发送AT 命令从透传模式进入AT指令模式
+            self.send_at_commands(["AT+CG", "AT+AT"])
+            # 开始读取数据
+            self.read_serial_data()
+        except KeyboardInterrupt:
+            print("手动中止程序。")
+        finally:
+            self.close_serial()
+
+if __name__ == '__main__':
+    parser = SerialCANParser('/dev/ttyCH341USB0', 9600, 1)
+    parser.start()
@@ -0,0 +1,6 @@
+{
+  "p_x": -0.04373347759246826,
+  "p_y": -4.024151802062988,
+  "orien_z": 0.09378381732290733,
+  "orien_w": 0.9955925851513477
+}
@@ -0,0 +1,3 @@
+# 导航参数配置
+forward_distance: 1  # 距离充电桩前方1米
+yaw_offset_deg: 10.0   # 顺时针旋转10度
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agv_pro_autocharge</name>
+  <version>1.0.0</version>
+  <description>AGV automatic charging system for ROS2 Humble</description>
+  <maintainer email="your-email@example.com">Your Name</maintainer>
+  <license>MIT</license>
+
+  <!-- Build dependencies -->
+  <buildtool_depend>ament_python</buildtool_depend>
+  
+  <!-- Runtime dependencies -->
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>nav2_simple_commander</depend>
+  
+  <!-- Test dependencies -->
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
@@ -0,0 +1 @@
+agv_pro_autocharge
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/agv_pro_autocharge
+[install]
+install_scripts=$base/lib/agv_pro_autocharge
@@ -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',
+        ],
+    },
+)
@@ -2,7 +2,8 @@
 #define AGV_PRO_DRIVER_H

 #include <algorithm> 
-#include "serial_driver/serial_driver.hpp"
+#include <iostream>
+#include <boost/asio.hpp>

 #include "rclcpp/rclcpp.hpp"

@@ -13,7 +14,9 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

-#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<double, 36> odom_pose_covariance;
 extern std::array<double, 36> odom_twist_covariance;
@@ -21,20 +24,136 @@ extern std::array<double, 36> odom_twist_covariance;
 class AGV_PRO : public rclcpp::Node
 {
 public:
+  /**
+   * @brief Constructor
+   */
  AGV_PRO(std::string node_name);
+
+  /**
+   * @brief Destructor
+   */
  ~AGV_PRO();
+
 private:
+  /**
+   * @brief Main control loop for the AGV.
+   */
  void Control();
-  void print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> 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<uint8_t>& data,
+    std::optional<size_t> override_size = std::nullopt);
+
+  /**
+   * @brief Send a serial frame to the AGV and optionally print it in hex.
+   *
+   * @param[in] frame The byte vector representing the serial frame to send.
+   * @param[in] debug If true, prints the transmitted frame using print_hex().
+   */
  void send_serial_frame(const std::vector<uint8_t>& frame, bool debug);
-  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<uint8_t> build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload);
-  std::vector<uint8_t> read_serial_response(const std::vector<uint8_t>& 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<uint8_t> read_serial_response(
+    const std::vector<uint8_t>& expected_header,
+    size_t payload_size,
+    double timeout_sec);
+
+  /**
+   * @brief Compute the CRC-16-IBM checksum for a byte array.
+   *
+   * This function calculates the CRC using the standard IBM polynomial 0xA001.
+   *
+   * @param[in] data Pointer to the byte array.
+   * @param[in] length Number of bytes to include in the CRC calculation.
+   * @return The computed 16-bit CRC value.
+   */
+  uint16_t crc16_ibm(const uint8_t* data, size_t length);
+
+  boost::asio::io_service io_;
+  std::unique_ptr<boost::asio::serial_port> serial_port_;

  std::string frame_id_of_odometry_;
  std::string child_frame_id_of_odometry_;
@@ -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<double, 36> odom_pose_covariance = {
+    {1e-9, 0, 0, 0, 0, 0,
+    0, 1e-3, 1e-9, 0, 0, 0,
+    0, 0, 1e6, 0, 0, 0,
+    0, 0, 0, 1e6, 0, 0,
+    0, 0, 0, 0, 1e6, 0,
+    0, 0, 0, 0, 0, 1e-9} };
+
+  std::array<double, 36> odom_twist_covariance = {
+    {1e-9, 0, 0, 0, 0, 0,
+    0, 1e-3, 1e-9, 0, 0, 0,
+    0, 0, 1e6, 0, 0, 0,
+    0, 0, 0, 1e6, 0, 0,
+    0, 0, 0, 0, 1e6, 0,
+    0, 0, 0, 0, 0, 1e-9} };
+
  rclcpp::Time currentTime, lastTime;
  rclcpp::TimerBase::SharedPtr control_timer_;
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
@@ -70,10 +217,8 @@ private:
  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_voltage;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;

+  sensor_msgs::msg::Imu imu_data;
  std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;
-  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
-  std::shared_ptr<drivers::common::IoContext> io_context_;
-
 };

 #endif
@@ -1,22 +1,6 @@
 #include "agv_pro_base/agv_pro_driver.h"

-std::array<double, 36> odom_pose_covariance = {
-  {1e-9, 0, 0, 0, 0, 0,
-  0, 1e-3, 1e-9, 0, 0, 0,
-  0, 0, 1e6, 0, 0, 0,
-  0, 0, 0, 1e6, 0, 0,
-  0, 0, 0, 0, 1e6, 0,
-  0, 0, 0, 0, 0, 1e-9} };
-
-std::array<double, 36> odom_twist_covariance = {
-  {1e-9, 0, 0, 0, 0, 0,
-  0, 1e-3, 1e-9, 0, 0, 0,
-  0, 0, 1e6, 0, 0, 0,
-  0, 0, 0, 1e6, 0, 0,
-  0, 0, 0, 0, 1e6, 0,
-  0, 0, 0, 0, 0, 1e-9} };
-
-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<uint16_t>(data[i]);
@@ -32,7 +16,7 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) {

 std::vector<uint8_t> AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload)
 {
-  std::vector<uint8_t> frame(RECEIVE_DATA_SIZE, 0x00);
+  std::vector<uint8_t> 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<uint8_t>& da
 void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& 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<uint8_t>& frame, bool debug)
  }
 }

-std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& expected_header, size_t payload_size, double timeout_sec)
+std::vector<uint8_t> AGV_PRO::read_serial_response(
+  const std::vector<uint8_t>& expected_header,
+  size_t payload_size,
+  double timeout_sec)
 {
-  auto port = serial_driver_->port();
  std::vector<uint8_t> sliding_buf;
  uint8_t byte = 0;

@@ -82,17 +67,20 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
  rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);

  while ((this->now() - start_time) < timeout) {
-    std::vector<uint8_t> 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;
      }
    }
  }
@@ -104,7 +92,20 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e

  size_t remain_len = payload_size + 2;
  std::vector<uint8_t> 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<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& 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<uint8_t> 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<int8_t>(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<int8_t>(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<uint8_t>(enable)});
  send_serial_frame(frame,true);
 }

+void AGV_PRO::clearSerialBuffer(int fd) {
+  if (tcflush(fd, TCIOFLUSH) < 0) {
+    RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Serial buffer flushed.");
+  }
+}
+
+void AGV_PRO::disableDTR_RTS(int fd) {
+  int status;
+  if (::ioctl(fd, TIOCMGET, &status) == 0) {
+    status &= ~(TIOCM_DTR | TIOCM_RTS);
+    if (::ioctl(fd, TIOCMSET, &status) != 0) {
+      RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno));
+    } else {
+      RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully.");
+    }
+  } else {
+    RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno));
+  }
+}
+
 void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
  linearX = std::clamp(msg->linear.x, -1.5, 1.5);
@@ -220,11 +245,9 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)

  std::vector<uint8_t> 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<uint8_t> buf_header(1);
  std::vector<uint8_t> buf_length(1);
-  std::vector<uint8_t> data_buf(RECEIVE_DATA_SIZE-3);
+  std::vector<uint8_t> 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<uint8_t> 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<float>(recv_buf[9]) / 10.0f;
  enable_status = recv_buf[10];

+  imu_data.linear_acceleration.x = static_cast<double>((static_cast<int16_t>(recv_buf[11]) << 8) | recv_buf[12]) * 0.01;
+  imu_data.linear_acceleration.y = static_cast<double>((static_cast<int16_t>(recv_buf[13]) << 8) | recv_buf[14]) * 0.01;
+  imu_data.linear_acceleration.z = static_cast<double>((static_cast<int16_t>(recv_buf[15]) << 8) | recv_buf[16]) * 0.01;
+
+  imu_data.angular_velocity.x = static_cast<double>((static_cast<int16_t>(recv_buf[17]) << 8) | recv_buf[18]) * 0.01;
+  imu_data.angular_velocity.y = static_cast<double>((static_cast<int16_t>(recv_buf[19]) << 8) | recv_buf[20]) * 0.01;
+  imu_data.angular_velocity.z = static_cast<double>((static_cast<int16_t>(recv_buf[21]) << 8) | recv_buf[22]) * 0.01;
+
+  roll  = static_cast<double>((static_cast<int16_t>(recv_buf[23]) << 8) | recv_buf[24]) * 0.01;
+  pitch = static_cast<double>((static_cast<int16_t>(recv_buf[25]) << 8) | recv_buf[26]) * 0.01;
+  yaw   = static_cast<double>((static_cast<int16_t>(recv_buf[27]) << 8) | recv_buf[28]) * 0.01;
+
  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<drivers::common::IoContext>(1);
-    serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
-    serial_driver_->init_port(device_name_, config);
-    serial_driver_->port()->open();
+    serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
+
+    serial_port_->open(device_name_);
+    serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
+    serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
+    serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
+    serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
+    serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
+
+    int fd = serial_port_->native_handle();
+    this->clearSerialBuffer(fd);
+    this->disableDTR_RTS(fd);
+
+    rclcpp::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time

    RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
-    RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
-    RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
+    RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str());

-    AGV_PRO::is_power_on();
-    AGV_PRO::set_auto_report();
+    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<uint8_t, 14> 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<uint8_t> data_vec(buf.begin(), buf.end());
-
-  auto port = serial_driver_->port();
-
-  try
-  {
-    port->send(data_vec);
+  if (serial_port_ && serial_port_->is_open()) {
+    this->set_auto_report(0);
+    serial_port_->cancel();
+    serial_port_->close();
  } 
-  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");
 }
@@ -221,4 +221,12 @@
    <child link="${namespace}camera_link" />
  </joint>

+  <link name="${namespace}imu_link"/>
+
+  <joint name="${namespace}imu_joint" type="fixed">
+    <origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
+    <parent link="${namespace}base_link" />
+    <child link="${namespace}imu_link" />
+  </joint>
+
 </robot>
@@ -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
@@ -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 <QObject>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/qos.hpp"
+#include <memory>
+
+#include "rviz_default_plugins/tools/pose/pose_tool.hpp"
+#include "rviz_default_plugins/visibility_control.hpp"
+
+namespace rviz_common
+{
+
+class DisplayContext;
+
+namespace properties
+{
+class StringProperty;
+class QosProfileProperty;
+}  // namespace properties
+}  // namespace rviz_common
+
+namespace 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<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_;
+  rclcpp::Clock::SharedPtr clock_;
+  rviz_common::properties::StringProperty * topic_property_;
+  rviz_common::properties::QosProfileProperty * qos_profile_property_;
+
+  rclcpp::QoS qos_profile_;
+};
+
+}  // namespace nav2_rviz_plugins
+
+#endif  // NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
@@ -6,6 +6,12 @@
    <description>A tool used to specify the navigation goal pose.</description>
  </class>

+  <class name="nav2_rviz_plugins/ChargerTool"
+         type="nav2_rviz_plugins::GoalTool"
+         base_class_type="rviz_common::Tool">
+    <description>A tool used to specify the charging stations goal pose.</description>
+  </class>
+
  <class name="nav2_rviz_plugins/Navigation 2"
         type="nav2_rviz_plugins::Nav2Panel"
         base_class_type="rviz_common::Panel">
@@ -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 <memory>
+#include <string>
+
+#include "rviz_common/display_context.hpp"
+#include "rviz_common/load_resource.hpp"
+#include "rviz_common/properties/string_property.hpp"
+#include "rviz_common/properties/qos_profile_property.hpp"
+
+namespace 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<geometry_msgs::msg::PoseStamped>(
+    topic_property_->getStdString(), qos_profile_);
+  clock_ = raw_node->get_clock();
+}
+
+void
+ChargerTool::onPoseSet(double x, double y, double theta)
+{
+  std::string fixed_frame = context_->getFixedFrame().toStdString();
+
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = clock_->now();
+  goal.header.frame_id = fixed_frame;
+
+  goal.pose.position.x = x;
+  goal.pose.position.y = y;
+  goal.pose.position.z = 0.0;
+
+  goal.pose.orientation = orientationAroundZAxis(theta);
+
+  logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame);
+
+  publisher_->publish(goal);
+}
+
+}  // namespace nav2_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ChargerTool, rviz_common::Tool)
@@ -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: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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: <Fixed 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
@@ -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: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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: <Fixed 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
@@ -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: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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: <Fixed 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
@@ -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")]]),
+    ])
@@ -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')]),
+    ])
@@ -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')]),
+    ])
@@ -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)
+    ])
@@ -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
@@ -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
@@ -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