二维建图、三维建图加导航

src/laserscan_to_point_pulisher/README.md

@@ -0,0 +1,91 @@
+# ROS2 节点 README 文档
+
+## 节点名称
+**robot_pose_publisher**
+
+## 功能描述
+该节点的主要功能是将激光雷达的扫描数据（`LaserScan`）转换为点云数据（`Path`消息中的`poses`字段），并发布到`/scan_points`话题，供其他节点使用。通过订阅激光雷达的扫描数据，计算每个扫描点的坐标，并将这些坐标封装到`Path`消息中进行发布，以便在机器人导航、环境建模等应用中使用。
+
+## 订阅接口
+| 话题名称    | 消息类型                  | 回调函数          | 描述                     |
+|:------------|:--------------------------|:------------------|:-------------------------|
+| `/scan`     | `sensor_msgs/msg/LaserScan` | `laserscan_callback` | 订阅激光雷达的扫描数据 |
+
+## 发布接口
+| 话题名称      | 消息类型                  | 发布内容描述                     |
+|:--------------|:--------------------------|:-------------------------------|
+| `/scan_points`| `nav_msgs/msg/Path`       | 发布由激光扫描数据转换而来的点云数据 |
+
+## 其他信息
+
+### 代码中的问题
+1. **节点名称不准确**：
+   - 在代码中，节点名称被设置为`robot_pose_publisher`，但根据文件名和类名`laserscanToPointPublish`，这个名称可能不够准确，容易引起误解。
+
+2. **变量名拼写错误**：
+   - 在`main`函数中，销毁节点时使用了`robot_pose_publisher.destroy_node()`，但变量名实际上是`robot_laser_scan_publisher`，这会导致程序运行时出现错误。
+
+3. **消息类型和话题名称的注释不清晰**：
+   - 在代码中，部分注释和变量名的拼写存在错误，例如`sacn_point_publisher`（应为`scan_point_publisher`），这可能会影响代码的可读性和维护性。
+
+4. **缺少错误处理和日志输出**：
+   - 在数据处理和消息发布过程中，缺少对异常情况的处理和日志输出，这在调试和运行时可能会导致问题难以发现。
+
+### 修改建议
+1. **修正节点名称**：
+   - 将节点名称修改为更符合实际功能的名字，例如`laserscan_to_point_publisher`，以提高代码的可读性和准确性。
+   - 修改代码：
+     ```python
+     class laserscanToPointPublish(Node):
+         def __init__(self):
+             super().__init__('laserscan_to_point_publisher')
+             # 其他初始化代码...
+     ```
+
+2. **修正变量名拼写错误**：
+   - 在`main`函数中，确保变量名一致，避免拼写错误导致的运行时错误。
+   - 修改代码：
+     ```python
+     def main(args=None):
+         rclpy.init(args=args)
+         robot_laser_scan_publisher = laserscanToPointPublish()
+         rclpy.spin(robot_laser_scan_publisher)
+         robot_laser_scan_publisher.destroy_node()
+         rclpy.shutdown()
+     ```
+
+3. **添加日志输出和错误处理**：
+   - 在数据处理和消息发布过程中，添加日志输出，以便在运行时能够监控节点的状态和数据流动。
+   - 示例修改：
+     ```python
+     def laserscan_callback(self, msg):
+         self.get_logger().info('Received laser scan data')
+         try:
+             # 数据处理逻辑
+             laser_points = self.laserscan_to_points(msg.ranges, msg.angle_min, msg.angle_increment)
+             self.scan_point_publisher.publish(laser_points)
+             self.get_logger().info('Published scan points')
+         except Exception as e:
+             self.get_logger().error(f'Error processing laser scan data: {e}')
+     ```
+
+4. **优化代码结构和注释**：
+   - 修正变量名和注释中的拼写错误，提高代码的可读性。
+   - 添加详细的注释，解释每个函数和关键步骤的作用，方便后续维护和扩展。
+
+### 使用说明
+1. **运行节点**：
+   - 确保ROS2环境已正确配置，并安装了必要的依赖项。
+   - 使用以下命令运行节点：
+     ```bash
+     ros2 run <package_name> laserscan_to_point_publisher
+     ```
+
+2. **验证节点功能**：
+   - 使用`ros2 topic echo /scan_points`命令查看发布的点云数据。
+   - 确保激光雷达的扫描数据能够正确订阅，并且转换后的点云数据能够正常发布。
+
+3. **参数调整**：
+   - 根据实际应用场景，可以调整节点中的参数，如订阅的话题名称、发布的消息类型等。
+
+通过以上修改和优化，可以提高节点的稳定性和可维护性，使其更好地适应实际应用需求。

src/laserscan_to_point_pulisher/laserscan_to_point_pulisher/laserscan_to_point_publish.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped, TransformStamped
+from nav_msgs.msg import Path
+from sensor_msgs.msg import LaserScan
+import tf2_ros
+import math
+
+class laserscanToPointPublish(Node):
+
+    def __init__(self):
+        super().__init__('robot_pose_publisher')
+        self.subscription = self.create_subscription(
+            LaserScan,
+            '/scan',
+            self.laserscan_callback,
+            10)
+        self.sacn_point_publisher = self.create_publisher(
+            Path,
+            '/scan_points',
+            10)
+        
+    def laserscan_callback(self, msg):
+#            print(msg)
+        angle_min  = msg.angle_min
+        angle_increment = msg.angle_increment
+        laserscan = msg.ranges
+#            print(laserscan)
+        laser_points = self.laserscan_to_points(laserscan, angle_increment, angle_increment) 
+        self.sacn_point_publisher.publish(laser_points)
+        
+            
+    def laserscan_to_points(self, laserscan, angle_min, angle_increment):
+        points = []
+        angle = angle_min
+        laser_points = Path()
+
+        for distance in laserscan:
+            x = distance * math.cos(angle)
+            y = distance * math.sin(angle)
+            pose = PoseStamped()
+            pose.pose.position.x = x
+            pose.pose.position.y = y
+            points.append(pose)
+            angle += angle_increment
+        laser_points.poses = points
+ 
+        return laser_points
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    robot_laser_scan_publisher = laserscanToPointPublish()
+
+    rclpy.spin(robot_laser_scan_publisher)
+
+    robot_pose_publisher.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+

src/laserscan_to_point_pulisher/package.xml

@@ -0,0 +1,18 @@
+<?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>laserscan_to_point_pulisher</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="parallels@todo.todo">parallels</maintainer>
+  <license>TODO: License declaration</license>
+
+  <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>

src/laserscan_to_point_pulisher/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/laserscan_to_point_pulisher
+[install]
+install-scripts=$base/lib/laserscan_to_point_pulisher

src/laserscan_to_point_pulisher/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'laserscan_to_point_pulisher'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='parallels',
+    maintainer_email='parallels@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        "laserscan_to_point_pulisher = laserscan_to_point_pulisher.laserscan_to_point_publish:main"
+        ],
+    },
+)

src/laserscan_to_point_pulisher/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/laserscan_to_point_pulisher/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/laserscan_to_point_pulisher/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/yahboom_app_save_map/README.md

@@ -0,0 +1,119 @@
+# ROS2 节点 README 文档
+
+## 节点名称
+### 服务端节点
+**minimal_service**
+
+### 客户端节点
+**minimal_client_async**
+
+## 功能描述
+### 服务端节点
+该服务端节点的主要功能是提供一个服务，用于保存地图数据。它接收客户端发送的地图名称，生成唯一的地图ID，将地图信息存储到SQLite数据库中，并使用`map_saver_cli`工具将地图数据保存到指定路径。
+
+### 客户端节点
+该客户端节点的主要功能是向服务端节点发送地图保存请求。它通过服务调用将指定的地图名称发送给服务端，并接收服务端的响应。
+
+## 订阅的接口
+### 服务端节点
+- **服务名称**：`yahboomAppSaveMap`
+- **服务类型**：`yahboom_web_savmap_interfaces/srv/WebSaveMap`
+- **回调函数**：`add_three_ints_callback`
+
+### 客户端节点
+- **服务名称**：`add_three_ints`
+- **服务类型**：`yahboom_web_savmap_interfaces/srv/WebSaveMap`
+- **请求发送函数**：`send_request`
+
+## 发布的接口
+### 服务端节点
+- **服务响应**：通过服务响应返回处理结果。
+
+### 客户端节点
+- **服务请求**：通过服务请求发送地图名称。
+
+## 其他信息
+
+### 服务端节点的代码中的问题
+1. **服务名称不匹配**：
+   - 在服务端节点中，服务名称被设置为`yahboomAppSaveMap`，但在客户端节点中，服务名称被设置为`add_three_ints`，这会导致服务调用失败。
+
+2. **异常处理不够完善**：
+   - 在数据库操作和外部命令执行过程中，异常处理不够全面，可能导致在出现错误时无法正确记录问题。
+
+3. **日志输出不够详细**：
+   - 在服务回调函数中，日志输出不够详细，无法方便地监控服务的处理过程。
+
+4. **硬编码路径和参数**：
+   - 地图保存路径和数据库路径被硬编码在代码中，这在不同环境下可能需要修改代码，降低了可移植性。
+
+### 客户端节点的代码中的问题
+1. **服务名称不匹配**：
+   - 客户端节点中服务名称被设置为`add_three_ints`，与服务端节点的服务名称不一致，这会导致服务调用失败。
+
+2. **请求参数硬编码**：
+   - 地图名称被硬编码为`"mapssss"`，这在实际使用中可能需要根据用户输入动态设置。
+
+3. **缺少错误处理**：
+   - 在服务调用过程中，缺少对可能发生的错误的处理，可能导致程序在出现异常时崩溃。
+
+### 修改建议
+#### 服务端节点
+1. **修正服务名称**：
+   - 将服务名称改为一致，例如`yahboomAppSaveMap`，以确保客户端能够正确调用服务。
+
+2. **完善异常处理**：
+   - 在数据库操作和外部命令执行时，添加更详细的异常处理和日志记录，方便调试和问题定位。
+
+3. **优化日志输出**：
+   - 在服务回调函数中添加更多的日志信息，记录每个关键步骤的执行情况。
+
+4. **参数化路径和配置**：
+   - 将地图保存路径和数据库路径参数化，通过参数服务器或配置文件进行设置，提高代码的可移植性。
+
+#### 客户端节点
+1. **修正服务名称**：
+   - 将服务名称改为与服务端一致，例如`yahboomAppSaveMap`，以确保能够正确调用服务。
+
+2. **动态设置请求参数**：
+   - 通过命令行参数或参数服务器动态设置地图名称，而不是硬编码。
+
+3. **添加错误处理**：
+   - 在服务调用过程中添加错误处理逻辑，确保在出现异常时能够正确处理并记录错误信息。
+
+### 使用说明
+#### 运行服务端节点
+1. **确保ROS2环境已正确配置**，并安装了必要的依赖项。
+2. **运行节点**：
+   ```bash
+   ros2 run <package_name> yahboom_app_save_map.py
+   ```
+
+#### 运行客户端节点
+1. **确保ROS2环境已正确配置**，并安装了必要的依赖项。
+2. **运行节点**：
+   ```bash
+   ros2 run <package_name> yahboom_app_save_map_client.py
+   ```
+
+#### 验证节点功能
+1. **服务端节点**：
+   - 使用`ros2 service list`命令查看服务是否已正确注册。
+   - 使用`ros2 service type <service_name>`命令查看服务类型。
+
+2. **客户端节点**：
+   - 运行客户端节点后，检查服务端节点的日志输出，确保请求被正确处理。
+   - 使用`ros2 service call <service_name> <service_type> '<request_data>'`命令手动调用服务，验证服务功能。
+
+### 示例
+#### 服务端节点示例
+```bash
+ros2 run <package_name> yahboom_app_save_map.py
+```
+
+#### 客户端节点示例
+```bash
+ros2 run <package_name> yahboom_app_save_map_client.py
+```
+
+通过以上修改和优化，可以提高节点的稳定性和可维护性，使其更好地适应实际应用需求。

src/yahboom_app_save_map/launch/yahboom_app_save_map.launch.py

@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+#from launch.actions
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    
+    yahboom_app_save_map_node = Node(
+        package='yahboom_app_save_map',
+        executable="server",
+        output="screen"
+    )
+
+    return LaunchDescription(
+        [
+            yahboom_app_save_map_node,
+        ]
+    )

src/yahboom_app_save_map/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>yahboom_app_save_map</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yahboom@todo.todo">yahboom</maintainer>
+  <license>TODO: License declaration</license>
+
+  <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>
+    <exec_depend>yahboom_web_savmap_interfaces</exec_depend>
+  </export>
+</package>

src/yahboom_app_save_map/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/yahboom_app_save_map
+[install]
+install-scripts=$base/lib/yahboom_app_save_map

src/yahboom_app_save_map/setup.py

@@ -0,0 +1,28 @@
+from setuptools import setup
+
+package_name = 'yahboom_app_save_map'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name, ['launch/yahboom_app_save_map.launch.py',]),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='yahboom',
+    maintainer_email='yahboom@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        "server=yahboom_app_save_map.yahboom_app_save_map:main",
+        "client=yahboom_app_save_map.yahboom_app_save_map_client:main"
+        ],
+    },
+)

src/yahboom_app_save_map/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/yahboom_app_save_map/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/yahboom_app_save_map/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/yahboom_app_save_map/yahboom_app_save_map/yahboom_app_save_map.py

@@ -0,0 +1,57 @@
+from yahboom_web_savmap_interfaces.srv import WebSaveMap                                                            
+
+import rclpy
+from rclpy.node import Node
+import sqlite3
+import uuid
+import subprocess
+import datetime
+import hashlib
+
+class MinimalService(Node):
+
+    def __init__(self):
+        super().__init__('minimal_service')
+        self.srv = self.create_service(WebSaveMap, 'yahboomAppSaveMap', self.add_three_ints_callback)       # CHANGE
+        
+    def run_shellcommand(self, *args):
+        '''run the provided command and return its stdout'''
+        args = sum([(arg if type(arg) == list else [arg]) for arg in args], [])
+        print(args)
+        possess = subprocess.Popen(args, stdout=subprocess.PIPE)
+        return possess
+
+    def add_three_ints_callback(self, request, response):
+        map_name = request.mapname
+        map_path = "/home/yahboom/cartoros2/data/maps/" + map_name
+        now = datetime.datetime.now()
+        str_time = now.strftime("%Y-%m-%d %H:%M:%S.%f")
+        map_namestr = str_time + map_name
+        map_id = hashlib.md5(map_namestr.encode()).hexdigest()
+        response.response = request.mapname
+        try:
+            conn = sqlite3.connect("/home/yahboom/cartoros2/data/xgo.db")
+            c = conn.cursor()
+            c.execute("INSERT INTO xgo_map (map_name, map_id, map_path) VALUES (?, ?, ?)",(map_name, map_id, map_path))
+            self.run_shellcommand('ros2', 'run', 'nav2_map_server', 'map_saver_cli','-f', map_path, '--ros-args', '-p', 'save_map_timeout:=10000')
+        except sqlite3.IntegrityError as e:
+            re_data = {"msg": str(e)}
+            response.response = re_data
+            self.get_logger().info('Incoming request\nmapname: %s' % (re_data))  # CHANGE
+            return response     
+                                                        # CHANGE
+        self.get_logger().info('Incoming request\nmapname: %s' % (request.mapname))  # CHANGE
+
+        return response
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_service = MinimalService()
+
+    rclpy.spin(minimal_service)
+
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/yahboom_app_save_map/yahboom_app_save_map/yahboom_app_save_map_client.py

@@ -0,0 +1,47 @@
+from yahboom_web_savmap_interfaces.srv import WebSaveMap                            # CHANGE
+import sys
+import rclpy
+from rclpy.node import Node
+
+
+class MinimalClientAsync(Node):
+
+    def __init__(self):
+        super().__init__('minimal_client_async')
+        self.cli = self.create_client(WebSaveMap, 'add_three_ints')       # CHANGE
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('service not available, waiting again...')
+        self.req = WebSaveMap.Request()                                   # CHANGE
+
+    def send_request(self):
+        print(sys.argv[0])
+        self.req.mapname = "mapssss"                                      # CHANGE
+        self.future = self.cli.call_async(self.req)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_client = MinimalClientAsync()
+    minimal_client.send_request()
+
+    while rclpy.ok():
+        rclpy.spin_once(minimal_client)
+        if minimal_client.future.done():
+            try:
+                response = minimal_client.future.result()
+            except Exception as e:
+                minimal_client.get_logger().info(
+                    'Service call failed %r' % (e,))
+            else:
+                minimal_client.get_logger().info(
+                    'Result of add_three_ints: %s' %                                # CHANGE
+                    (response.response))  # CHANGE
+            break
+
+    minimal_client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/yahboomcar_ctrl/launch/keyboart_ctrl_launch-Copy1.py

@@ -0,0 +1,10 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+	return LaunchDescription([
+	Node(
+		package='yahboomcar_ctrl',
+		executable='yahboom_keyboard',
+	),	
+	])

src/yahboomcar_ctrl/launch/yahboomcar_joy_launch.py

@@ -0,0 +1,10 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    node1 = Node(
+        package='joy',
+        executable='joy_node',
+    )
+    launch_description = LaunchDescription([node1])
+    return launch_description

src/yahboomcar_ctrl/package.xml

@@ -0,0 +1,18 @@
+<?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>yahboomcar_ctrl</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nx-ros2@todo.todo">nx-ros2</maintainer>
+  <license>TODO: License declaration</license>
+
+  <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>

src/yahboomcar_ctrl/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/yahboomcar_ctrl
+[install]
+install-scripts=$base/lib/yahboomcar_ctrl

src/yahboomcar_ctrl/setup.py

@@ -0,0 +1,30 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'yahboomcar_ctrl'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']), (os.path.join('share',package_name,'launch'),glob(os.path.join('launch','*launch.py'))),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='nx-ros2',
+    maintainer_email='nx-ros2@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        'yahboom_joy_X3	= yahboomcar_ctrl.yahboom_joy_X3:main',
+        'yahboom_keyboard	= yahboomcar_ctrl.yahboom_keyboard:main',
+        'yahboom_joy_R2	= yahboomcar_ctrl.yahboom_joy_R2:main',
+        ],
+    },
+)

src/yahboomcar_ctrl/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/yahboomcar_ctrl/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/yahboomcar_ctrl/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/yahboomcar_ctrl/yahboomcar_ctrl/yahboom_joy_R2.py

@@ -0,0 +1,161 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+#public lib
+import os
+import time
+import getpass
+import threading
+from time import sleep
+
+#ros lib
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Joy
+from actionlib_msgs.msg import GoalID
+from std_msgs.msg import Int32, Bool
+
+class JoyTeleop(Node):
+	def __init__(self,name):
+		super().__init__(name)
+		self.Joy_active = False
+		self.Buzzer_active = False
+		self.RGBLight_index = 0
+		self.cancel_time = time.time()
+		self.user_name = getpass.getuser()
+		self.linear_Gear = 1
+		self.angular_Gear = 1
+		
+		#create pub
+		self.pub_goal = self.create_publisher(GoalID,"move_base/cancel",10)
+		self.pub_cmdVel = self.create_publisher(Twist,'cmd_vel',  10)
+		self.pub_Buzzer = self.create_publisher(Bool,"Buzzer",  1)
+		self.pub_JoyState = self.create_publisher(Bool,"JoyState",  10)
+		self.pub_RGBLight = self.create_publisher(Int32,"RGBLight" , 10)
+		
+		#create sub
+		self.sub_Joy = self.create_subscription(Joy,'joy', self.buttonCallback,1)
+		
+		#declare parameter and get the value
+		self.declare_parameter('xspeed_limit',1.0)
+		self.declare_parameter('yspeed_limit',5.0)
+		self.declare_parameter('angular_speed_limit',5.0)
+		self.xspeed_limit = self.get_parameter('xspeed_limit').get_parameter_value().double_value
+		self.yspeed_limit = self.get_parameter('yspeed_limit').get_parameter_value().double_value
+		self.angular_speed_limit = self.get_parameter('angular_speed_limit').get_parameter_value().double_value
+		
+		
+	def buttonCallback(self,joy_data):
+		if not isinstance(joy_data, Joy): return
+		if self.user_name == "root": self.user_jetson(joy_data)
+		else: self.user_pc(joy_data)
+		
+	def user_jetson(self, joy_data):
+		#cancel nav
+		if joy_data.buttons[9] == 1: self.cancel_nav()
+		#RGBLight
+		if joy_data.buttons[7] == 1:
+			RGBLight_ctrl = Int32()
+			if self.RGBLight_index < 6:
+				for i in range(3): 
+					RGBLight_ctrl.data = self.RGBLight_index
+					self.pub_RGBLight.publish(RGBLight_ctrl)
+			else: self.RGBLight_index = 0
+			self.RGBLight_index += 1
+		#Buzzer
+		if joy_data.buttons[11] == 1:
+			Buzzer_ctrl = Bool() 
+			self.Buzzer_active=not self.Buzzer_active
+			Buzzer_ctrl.data =self.Buzzer_active
+			for i in range(3): self.pub_Buzzer.publish(Buzzer_ctrl)
+        #linear Gear control
+		if joy_data.buttons[13] == 1:
+			if self.linear_Gear == 1.0: self.linear_Gear = 1.0 / 3
+			elif self.linear_Gear == 1.0 / 3: self.linear_Gear = 2.0 / 3
+			elif self.linear_Gear == 2.0 / 3: self.linear_Gear = 1
+        # angular Gear control
+		if joy_data.buttons[14] == 1:
+			if self.angular_Gear == 1.0: self.angular_Gear = 1.0 / 4
+			elif self.angular_Gear == 1.0 / 4: self.angular_Gear = 1.0 / 2
+			elif self.angular_Gear == 1.0 / 2: self.angular_Gear = 3.0 / 4
+			elif self.angular_Gear == 3.0 / 4: self.angular_Gear = 1.0
+		xlinear_speed = self.filter_data(joy_data.axes[1]) * self.xspeed_limit * self.linear_Gear
+        #ylinear_speed = self.filter_data(joy_data.axes[2]) * self.yspeed_limit * self.linear_Gear
+		ylinear_speed = self.filter_data(joy_data.axes[2]) * self.yspeed_limit * self.linear_Gear
+		angular_speed = self.filter_data(joy_data.axes[2]) * self.angular_speed_limit * self.angular_Gear
+		if xlinear_speed > self.xspeed_limit: xlinear_speed = self.xspeed_limit
+		elif xlinear_speed < -self.xspeed_limit: xlinear_speed = -self.xspeed_limit
+		if ylinear_speed > self.yspeed_limit: ylinear_speed = self.yspeed_limit
+		elif ylinear_speed < -self.yspeed_limit: ylinear_speed = -self.yspeed_limit
+		if angular_speed > self.angular_speed_limit: angular_speed = self.angular_speed_limit
+		elif angular_speed < -self.angular_speed_limit: angular_speed = -self.angular_speed_limit
+		twist = Twist()
+		twist.linear.x = xlinear_speed
+		twist.linear.y = ylinear_speed
+		#twist.angular.z = angular_speed
+		if self.Joy_active == True:
+			 for i in range(3): self.pub_cmdVel.publish(twist)
+        
+        
+	def user_pc(self, joy_data):
+
+        # 取消 Cancel
+		if joy_data.axes[5] == -1: self.cancel_nav()
+		if joy_data.buttons[5] == 1:
+			if self.RGBLight_index < 6:
+				self.pub_RGBLight.publish(self.RGBLight_index)
+                # print ("pub RGBLight success")
+			else: self.RGBLight_index = 0
+			self.RGBLight_index += 1
+		if joy_data.buttons[7] == 1:
+			self.Buzzer_active=not self.Buzzer_active
+            # print "self.Buzzer_active: ", self.Buzzer_active
+			self.pub_Buzzer.publish(self.Buzzer_active)
+        # 档位控制 Gear control
+		if joy_data.buttons[9] == 1:
+			if self.linear_Gear == 1.0: self.linear_Gear = 1.0 / 3
+			elif self.linear_Gear == 1.0 / 3: self.linear_Gear = 2.0 / 3
+			elif self.linear_Gear == 2.0 / 3: self.linear_Gear = 1
+		if joy_data.buttons[10] == 1:
+			if self.angular_Gear == 1.0: self.angular_Gear = 1.0 / 4
+			elif self.angular_Gear == 1.0 / 4: self.angular_Gear = 1.0 / 2
+			elif self.angular_Gear == 1.0 / 2: self.angular_Gear = 3.0 / 4
+			elif self.angular_Gear == 3.0 / 4: self.angular_Gear = 1.0
+		xlinear_speed = self.filter_data(joy_data.axes[1]) * self.xspeed_limit * self.linear_Gear
+		ylinear_speed = self.filter_data(joy_data.axes[0]) * self.yspeed_limit * self.linear_Gear
+		angular_speed = self.filter_data(joy_data.axes[3]) * self.angular_speed_limit * self.angular_Gear
+		if xlinear_speed > self.xspeed_limit: xlinear_speed = self.xspeed_limit
+		elif xlinear_speed < -self.xspeed_limit: xlinear_speed = -self.xspeed_limit
+		if ylinear_speed > self.yspeed_limit: ylinear_speed = self.yspeed_limit
+		elif ylinear_speed < -self.yspeed_limit: ylinear_speed = -self.yspeed_limit
+		if angular_speed > self.angular_speed_limit: angular_speed = self.angular_speed_limit
+		elif angular_speed < -self.angular_speed_limit: angular_speed = -self.angular_speed_limit
+		twist = Twist()
+		twist.linear.x = xlinear_speed
+		twist.linear.y = ylinear_speed
+		twist.angular.z = angular_speed
+		for i in range(3): self.pub_cmdVel.publish(twist)
+        
+	def filter_data(self, value):
+		if abs(value) < 0.2: value = 0
+		return value
+		
+	def cancel_nav(self):
+		now_time = time.time()
+		print("Joy control now")
+		if now_time - self.cancel_time > 1:
+			Joy_ctrl = Bool()
+			self.Joy_active = not self.Joy_active
+			Joy_ctrl.data = self.Joy_active
+			for i in range(3):
+				self.pub_JoyState.publish(Joy_ctrl)
+				#self.pub_goal.publish(GoalID())
+				self.pub_cmdVel.publish(Twist())
+			self.cancel_time = now_time
+			
+def main():
+	rclpy.init()
+	joy_ctrl = JoyTeleop('joy_ctrl')
+	rclpy.spin(joy_ctrl)		
+			

src/yahboomcar_ctrl/yahboomcar_ctrl/yahboom_joy_X3.py

@@ -0,0 +1,158 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+#public lib
+import os
+import time
+import getpass
+import threading
+from time import sleep
+
+#ros lib
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Joy
+from actionlib_msgs.msg import GoalID
+from std_msgs.msg import Int32, Bool
+
+class JoyTeleop(Node):
+	def __init__(self,name):
+		super().__init__(name)
+		self.Joy_active = False
+		self.Buzzer_active = False
+		self.RGBLight_index = 0
+		self.cancel_time = time.time()
+		self.user_name = getpass.getuser()
+		self.linear_Gear = 1
+		self.angular_Gear = 1
+		
+		#create pub
+		self.pub_goal = self.create_publisher(GoalID,"move_base/cancel",10)
+		self.pub_cmdVel = self.create_publisher(Twist,'cmd_vel',  10)
+		self.pub_Buzzer = self.create_publisher(Bool,"Buzzer",  1)
+		self.pub_JoyState = self.create_publisher(Bool,"JoyState",  10)
+		self.pub_RGBLight = self.create_publisher(Int32,"RGBLight" , 10)
+		
+		#create sub
+		self.sub_Joy = self.create_subscription(Joy,'joy', self.buttonCallback,10)
+		
+		#declare parameter and get the value
+		self.declare_parameter('xspeed_limit',1.0)
+		self.declare_parameter('yspeed_limit',1.0)
+		self.declare_parameter('angular_speed_limit',5.0)
+		self.xspeed_limit = self.get_parameter('xspeed_limit').get_parameter_value().double_value
+		self.yspeed_limit = self.get_parameter('yspeed_limit').get_parameter_value().double_value
+		self.angular_speed_limit = self.get_parameter('angular_speed_limit').get_parameter_value().double_value
+		
+		
+	def buttonCallback(self,joy_data):
+		if not isinstance(joy_data, Joy): return
+		if self.user_name == "root": self.user_jetson(joy_data)
+		else: self.user_pc(joy_data)
+		
+	def user_jetson(self, joy_data):
+		#cancel nav
+		if joy_data.axes[9] == 1: self.cancel_nav()
+		#RGBLight
+		if joy_data.buttons[7] == 1:
+                    RGBLight_ctrl = Int32()
+                    if self.RGBLight_index < 6:
+                        RGBLight_ctrl.data = self.RGBLight_index
+                        for i in range(3): self.pub_RGBLight.publish(RGBLight_ctrl)
+                    else: self.RGBLight_index = 0
+                    self.RGBLight_index += 1
+		#Buzzer
+		if joy_data.buttons[11] == 1:
+			Buzzer_ctrl = Bool() 
+			self.Buzzer_active=not self.Buzzer_active
+			Buzzer_ctrl.data =self.Buzzer_active
+			for i in range(3): self.pub_Buzzer.publish(Buzzer_ctrl)
+        #linear Gear control
+		if joy_data.buttons[13] == 1:
+			if self.linear_Gear == 1.0: self.linear_Gear = 1.0 / 3
+			elif self.linear_Gear == 1.0 / 3: self.linear_Gear = 2.0 / 3
+			elif self.linear_Gear == 2.0 / 3: self.linear_Gear = 1
+        # angular Gear control
+		if joy_data.buttons[14] == 1:
+			if self.angular_Gear == 1.0: self.angular_Gear = 1.0 / 4
+			elif self.angular_Gear == 1.0 / 4: self.angular_Gear = 1.0 / 2
+			elif self.angular_Gear == 1.0 / 2: self.angular_Gear = 3.0 / 4
+			elif self.angular_Gear == 3.0 / 4: self.angular_Gear = 1.0
+		xlinear_speed = self.filter_data(joy_data.axes[1]) * self.xspeed_limit * self.linear_Gear
+        #ylinear_speed = self.filter_data(joy_data.axes[2]) * self.yspeed_limit * self.linear_Gear
+		ylinear_speed = self.filter_data(joy_data.axes[0]) * self.yspeed_limit * self.linear_Gear
+		angular_speed = self.filter_data(joy_data.axes[2]) * self.angular_speed_limit * self.angular_Gear
+		if xlinear_speed > self.xspeed_limit: xlinear_speed = self.xspeed_limit
+		elif xlinear_speed < -self.xspeed_limit: xlinear_speed = -self.xspeed_limit
+		if ylinear_speed > self.yspeed_limit: ylinear_speed = self.yspeed_limit
+		elif ylinear_speed < -self.yspeed_limit: ylinear_speed = -self.yspeed_limit
+		if angular_speed > self.angular_speed_limit: angular_speed = self.angular_speed_limit
+		elif angular_speed < -self.angular_speed_limit: angular_speed = -self.angular_speed_limit
+		twist = Twist()
+		twist.linear.x = xlinear_speed
+		twist.linear.y = ylinear_speed
+		twist.angular.z = angular_speed
+		if self.Joy_active == True:
+			print("joy control now")
+			for i in range(3): self.pub_cmdVel.publish(twist)
+        
+	def user_pc(self, joy_data):
+
+        # 取消 Cancel
+		if joy_data.axes[5] == -1: self.cancel_nav()
+		if joy_data.buttons[5] == 1:
+			if self.RGBLight_index < 6:
+				self.pub_RGBLight.publish(self.RGBLight_index)
+                # print ("pub RGBLight success")
+			else: self.RGBLight_index = 0
+			self.RGBLight_index += 1
+		if joy_data.buttons[7] == 1:
+			self.Buzzer_active=not self.Buzzer_active
+            # print "self.Buzzer_active: ", self.Buzzer_active
+			self.pub_Buzzer.publish(self.Buzzer_active)
+        # 档位控制 Gear control
+		if joy_data.buttons[9] == 1:
+			if self.linear_Gear == 1.0: self.linear_Gear = 1.0 / 3
+			elif self.linear_Gear == 1.0 / 3: self.linear_Gear = 2.0 / 3
+			elif self.linear_Gear == 2.0 / 3: self.linear_Gear = 1
+		if joy_data.buttons[10] == 1:
+			if self.angular_Gear == 1.0: self.angular_Gear = 1.0 / 4
+			elif self.angular_Gear == 1.0 / 4: self.angular_Gear = 1.0 / 2
+			elif self.angular_Gear == 1.0 / 2: self.angular_Gear = 3.0 / 4
+			elif self.angular_Gear == 3.0 / 4: self.angular_Gear = 1.0
+		xlinear_speed = self.filter_data(joy_data.axes[1]) * self.xspeed_limit * self.linear_Gear
+		ylinear_speed = self.filter_data(joy_data.axes[0]) * self.yspeed_limit * self.linear_Gear
+		angular_speed = self.filter_data(joy_data.axes[2]) * self.angular_speed_limit * self.angular_Gear
+		if xlinear_speed > self.xspeed_limit: xlinear_speed = self.xspeed_limit
+		elif xlinear_speed < -self.xspeed_limit: xlinear_speed = -self.xspeed_limit
+		if ylinear_speed > self.yspeed_limit: ylinear_speed = self.yspeed_limit
+		elif ylinear_speed < -self.yspeed_limit: ylinear_speed = -self.yspeed_limit
+		if angular_speed > self.angular_speed_limit: angular_speed = self.angular_speed_limit
+		elif angular_speed < -self.angular_speed_limit: angular_speed = -self.angular_speed_limit
+		twist = Twist()
+		twist.linear.x = xlinear_speed
+		twist.linear.y = ylinear_speed
+		twist.angular.z = angular_speed
+		for i in range(3): self.pub_cmdVel.publish(twist)
+        
+	def filter_data(self, value):
+		if abs(value) < 0.2: value = 0
+		return value
+		
+	def cancel_nav(self):
+		now_time = time.time()
+		if now_time - self.cancel_time > 1:
+			Joy_ctrl = Bool()
+			self.Joy_active = not self.Joy_active
+			Joy_ctrl.data = self.Joy_active
+			for i in range(3):
+				self.pub_JoyState.publish(Joy_ctrl)
+				#self.pub_goal.publish(GoalID())
+				self.pub_cmdVel.publish(Twist())
+			self.cancel_time = now_time
+			
+def main():
+	rclpy.init()
+	joy_ctrl = JoyTeleop('joy_ctrl')
+	rclpy.spin(joy_ctrl)		

src/yahboomcar_ctrl/yahboomcar_ctrl/yahboom_keyboard.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python
+# encoding: utf-8
+#import public lib
+from geometry_msgs.msg import Twist
+import sys, select, termios, tty
+
+#import ros lib
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+
+msg = """
+Control Your SLAM-Bot!
+---------------------------
+Moving around:
+   u    i    o
+   j    k    l
+   m    ,    .
+
+q/z : increase/decrease max speeds by 10%
+w/x : increase/decrease only linear speed by 10%
+e/c : increase/decrease only angular speed by 10%
+t/T : x and y speed switch
+s/S : stop keyboard control
+space key, k : force stop
+anything else : stop smoothly
+
+CTRL-C to quit
+"""
+
+moveBindings = {
+    'i': (1, 0),
+    'o': (1, -1),
+    'j': (0, 1),
+    'l': (0, -1),
+    'u': (1, 1),
+    ',': (-1, 0),
+    '.': (-1, 1),
+    'm': (-1, -1),
+    'I': (1, 0),
+    'O': (1, -1),
+    'J': (0, 1),
+    'L': (0, -1),
+    'U': (1, 1),
+    'M': (-1, -1),
+}
+
+speedBindings = {
+    'Q': (1.1, 1.1),
+    'Z': (.9, .9),
+    'W': (1.1, 1),
+    'X': (.9, 1),
+    'E': (1, 1.1),
+    'C': (1, .9),
+    'q': (1.1, 1.1),
+    'z': (.9, .9),
+    'w': (1.1, 1),
+    'x': (.9, 1),
+    'e': (1, 1.1),
+    'c': (1, .9),
+}
+
+
+
+class Yahboom_Keybord(Node):
+	def __init__(self,name):
+		super().__init__(name)
+		self.pub = self.create_publisher(Twist,'cmd_vel',1)
+		self.declare_parameter("linear_speed_limit",1.0)
+		self.declare_parameter("angular_speed_limit",5.0)
+		self.linenar_speed_limit = self.get_parameter("linear_speed_limit").get_parameter_value().double_value
+		self.angular_speed_limit = self.get_parameter("angular_speed_limit").get_parameter_value().double_value
+		self.settings = termios.tcgetattr(sys.stdin)
+	def getKey(self):
+		tty.setraw(sys.stdin.fileno())
+		rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
+		if rlist: key = sys.stdin.read(1)
+		else: key = ''
+		termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
+		return key
+	def vels(self, speed, turn):
+		return "currently:\tspeed %s\tturn %s " % (speed,turn)		
+	
+def main():
+	rclpy.init()
+	yahboom_keyboard = Yahboom_Keybord("yahboom_keyboard_ctrl")
+	xspeed_switch = True
+	(speed, turn) = (0.2, 1.0)
+	(x, th) = (0, 0)
+	status = 0
+	stop = False
+	count = 0
+	twist = Twist()
+	try:
+		print(msg)
+		print(yahboom_keyboard.vels(speed, turn))
+		while (1):
+			key = yahboom_keyboard.getKey()
+			if key=="t" or key == "T": xspeed_switch = not xspeed_switch
+			elif key == "s" or key == "S":
+				print ("stop keyboard control: {}".format(not stop))
+				stop = not stop
+			if key in moveBindings.keys():
+				x = moveBindings[key][0]
+				th = moveBindings[key][1]
+				count = 0	
+			elif key in speedBindings.keys():
+				speed = speed * speedBindings[key][0]
+				turn = turn * speedBindings[key][1]
+				count = 0
+				if speed > yahboom_keyboard.linenar_speed_limit: 
+					speed = yahboom_keyboard.linenar_speed_limit
+					print("Linear speed limit reached!")
+				if turn > yahboom_keyboard.angular_speed_limit: 
+					turn = yahboom_keyboard.angular_speed_limit
+					print("Angular speed limit reached!")
+				print(yahboom_keyboard.vels(speed, turn))
+				if (status == 14): print(msg)
+				status = (status + 1) % 15
+			elif key == ' ': (x, th) = (0, 0)
+			else:
+				count = count + 1
+				if count > 4: (x, th) = (0, 0)
+				if (key == '\x03'): break
+			if xspeed_switch: twist.linear.x = speed * x
+			else: twist.linear.y = speed * x
+			twist.angular.z = turn * th
+			if not stop: yahboom_keyboard.pub.publish(twist)
+			if stop:yahboom_keyboard.pub.publish(Twist())
+	except Exception as e: print(e)
+	finally: yahboom_keyboard.pub.publish(Twist())
+	termios.tcsetattr(sys.stdin, termios.TCSADRAIN, yahboom_keyboard.settings)
+	yahboom_keyboard.destroy_node()
+	rclpy.shutdown()

src/yahboomcar_nav/launch/cartographer_launch.py

@@ -0,0 +1,62 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import ThisLaunchFileDir
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    package_path = get_package_share_directory('yahboomcar_nav')
+    configuration_directory = LaunchConfiguration('configuration_directory', default=os.path.join(
+                                                  package_path, 'params'))
+    configuration_basename = LaunchConfiguration('configuration_basename', default='lds_2d.lua')
+
+    resolution = LaunchConfiguration('resolution', default='0.05')
+    publish_period_sec = LaunchConfiguration(
+        'publish_period_sec', default='1.0')
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'configuration_directory',
+            default_value=configuration_directory,
+            description='Full path to config file to load'),
+        DeclareLaunchArgument(
+            'configuration_basename',
+            default_value=configuration_basename,
+            description='Name of lua file for cartographer'),
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+
+        Node(
+            package='cartographer_ros',
+            executable='cartographer_node',
+            name='cartographer_node',
+            output='screen',
+            parameters=[{'use_sim_time': use_sim_time}],
+            arguments=['-configuration_directory', configuration_directory,
+                       '-configuration_basename', configuration_basename]),
+
+        DeclareLaunchArgument(
+            'resolution',
+            default_value=resolution,
+            description='Resolution of a grid cell in the published occupancy grid'),
+
+        DeclareLaunchArgument(
+            'publish_period_sec',
+            default_value=publish_period_sec,
+            description='OccupancyGrid publishing period'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [ThisLaunchFileDir(), '/occupancy_grid_launch.py']),
+            launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution,
+                              'publish_period_sec': publish_period_sec}.items(),
+        ),
+    ])

src/yahboomcar_nav/launch/display_map_launch.py

@@ -0,0 +1,28 @@
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+def generate_launch_description():
+    package_path = get_package_share_path('yahboomcar_nav')
+    default_rviz_config_path = package_path / 'rviz/map.rviz'
+    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
+                                     description='Absolute path to rviz config file')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+    )
+
+    return LaunchDescription([
+        rviz_arg,
+        rviz_node
+    ])

src/yahboomcar_nav/launch/display_nav_launch.py

@@ -0,0 +1,28 @@
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+def generate_launch_description():
+    package_path = get_package_share_path('yahboomcar_nav')
+    default_rviz_config_path = package_path / 'rviz/nav.rviz'
+    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
+                                     description='Absolute path to rviz config file')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+    )
+
+    return LaunchDescription([
+        rviz_arg,
+        rviz_node
+    ])

src/yahboomcar_nav/launch/display_rtabmap_map_launch.py

@@ -0,0 +1,28 @@
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+def generate_launch_description():
+    package_path = get_package_share_path('yahboomcar_nav')
+    default_rviz_config_path = package_path / 'rviz/rtabmap_map.rviz'
+    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
+                                     description='Absolute path to rviz config file')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+    )
+
+    return LaunchDescription([
+        rviz_arg,
+        rviz_node
+    ])

src/yahboomcar_nav/launch/display_rtabmap_nav_launch.py

@@ -0,0 +1,28 @@
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+def generate_launch_description():
+    package_path = get_package_share_path('yahboomcar_nav')
+    default_rviz_config_path = package_path / 'rviz/rtabmap_nav.rviz'
+    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
+                                     description='Absolute path to rviz config file')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+    )
+
+    return LaunchDescription([
+        rviz_arg,
+        rviz_node
+    ])

src/yahboomcar_nav/launch/laser_bringup_launch.py

@@ -0,0 +1,71 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.conditions import LaunchConfigurationEquals
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import DeclareLaunchArgument
+import os
+
+
+def generate_launch_description():
+    ROBOT_TYPE = os.getenv('ROBOT_TYPE')
+    RPLIDAR_TYPE = os.getenv('RPLIDAR_TYPE')
+    print("\n-------- robot_type = {}, rplidar_type = {} --------\n".format(ROBOT_TYPE, RPLIDAR_TYPE))
+    # CAMERA_TYPE = os.getenv('CAMERA_TYPE')
+    robot_type_arg = DeclareLaunchArgument(name='robot_type', default_value=ROBOT_TYPE, 
+                                              choices=['x1','x3','r2'],
+                                              description='The type of robot')
+    rplidar_type_arg = DeclareLaunchArgument(name='rplidar_type', default_value=RPLIDAR_TYPE, 
+                                              choices=['a1','s2','4ROS'],
+                                              description='The type of robot')
+
+    bringup_x1_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_bringup'), 'launch'),
+        '/yahboomcar_bringup_X1_launch.py']),
+         condition=LaunchConfigurationEquals('robot_type', 'x1')
+    )
+    bringup_x3_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_bringup'), 'launch'),
+        '/yahboomcar_bringup_X3_launch.py']),
+         condition=LaunchConfigurationEquals('robot_type', 'x3')
+    )
+    bringup_r2_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_bringup'), 'launch'),
+        '/yahboomcar_bringup_R2_launch.py']),
+         condition=LaunchConfigurationEquals('robot_type', 'r2')
+    )
+    lidar_a1_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('sllidar_ros2'), 'launch'),
+        '/sllidar_launch.py']),
+        condition=LaunchConfigurationEquals('rplidar_type', 'a1')
+    )
+    lidar_s2_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('sllidar_ros2'), 'launch'),
+        '/sllidar_s2_launch.py']),
+        condition=LaunchConfigurationEquals('rplidar_type', 's2')
+    )
+    lidar_4ROS_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('ydlidar_ros2_driver'), 'launch'),
+        '/ydlidar_raw_launch.py']),
+        condition=LaunchConfigurationEquals('rplidar_type', '4ROS')
+    )
+
+    tf_base_link_to_laser = Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            arguments = ['0.0435', '5.258E-05', '0.11', '3.14', '0', '0', 'base_link', 'laser']
+    )
+    
+    return LaunchDescription([
+        robot_type_arg,
+        bringup_x1_launch,
+        bringup_x3_launch,
+        bringup_r2_launch,
+        rplidar_type_arg,
+        lidar_a1_launch,
+        lidar_s2_launch,
+        lidar_4ROS_launch,
+        tf_base_link_to_laser
+    ])

src/yahboomcar_nav/launch/map_cartographer_launch.py

@@ -0,0 +1,19 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    package_launch_path =os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch')
+
+    laser_bringup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [package_launch_path,'/laser_bringup_launch.py'])
+    )
+
+    cartographer_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [package_launch_path, '/cartographer_launch.py'])
+    )
+
+    return LaunchDescription([laser_bringup_launch, cartographer_launch])

src/yahboomcar_nav/launch/map_gmapping_4ros_s2_launch.py

@@ -0,0 +1,32 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    laser_bringup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch'),
+        '/laser_bringup_launch.py'])
+    )
+    # 4ROS和s2的雷达单帧激光点数大于1440，gmapping只适用于单帧二维激光点数小于1440的点，所以做过滤
+    scan_filter_node = Node(
+        package='yahboomcar_nav',
+        executable='scan_filter',
+    )
+
+    slam_gmapping_node = Node(
+            package='slam_gmapping', 
+            executable='slam_gmapping', 
+            output='screen', 
+            parameters=[os.path.join(get_package_share_directory("slam_gmapping"), "params", "slam_gmapping.yaml")],
+            remappings = [("/scan","/downsampled_scan")],
+    )
+
+    return LaunchDescription([
+        laser_bringup_launch,
+        scan_filter_node, 
+        slam_gmapping_node
+    ])

src/yahboomcar_nav/launch/map_gmapping_a1_launch.py

@@ -0,0 +1,21 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+
+    laser_bringup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch'),
+         '/laser_bringup_launch.py'])
+    )
+
+    slam_gmapping_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('slam_gmapping'), 'launch'),
+         '/slam_gmapping.launch.py'])
+    )
+
+    return LaunchDescription([laser_bringup_launch, slam_gmapping_launch])

src/yahboomcar_nav/launch/map_gmapping_launch.py

@@ -0,0 +1,38 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+from launch.conditions import LaunchConfigurationEquals
+from launch.actions import DeclareLaunchArgument
+
+
+def generate_launch_description():
+    RPLIDAR_TYPE = os.getenv('RPLIDAR_TYPE')
+    rplidar_type_arg = DeclareLaunchArgument(name='rplidar_type', default_value=RPLIDAR_TYPE, 
+                                              choices=['a1','s2','4ROS'],
+                                              description='The type of robot')
+
+    gmapping_4ros_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch'),
+        '/map_gmapping_4ros_s2_launch.py']),
+        condition=LaunchConfigurationEquals('rplidar_type', '4ROS')
+    )
+    gmapping_s2_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch'),
+        '/map_gmapping_4ros_s2_launch.py']),
+        condition=LaunchConfigurationEquals('rplidar_type', 's2')
+    )
+    gmapping_a1_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch'),
+        '/map_gmapping_a1_launch.py']),
+        condition=LaunchConfigurationEquals('rplidar_type', 'a1')
+    )
+
+    return LaunchDescription([
+        rplidar_type_arg,
+        gmapping_4ros_launch, 
+        gmapping_s2_launch, 
+        gmapping_a1_launch
+    ])

src/yahboomcar_nav/launch/map_rtabmap_launch.py

@@ -0,0 +1,19 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    package_launch_path =os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch')
+
+    laser_bringup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [package_launch_path,'/laser_bringup_launch.py'])
+    )
+
+    rtabmap_sync_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [package_launch_path, '/rtabmap_sync_launch.py'])
+    )
+
+    return LaunchDescription([laser_bringup_launch, rtabmap_sync_launch])

src/yahboomcar_nav/launch/navigation_dwa_launch.py

@@ -0,0 +1,36 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    package_path = get_package_share_directory('yahboomcar_nav')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    map_yaml_path = LaunchConfiguration(
+        'map', default=os.path.join(package_path, 'maps', 'yahboomcar.yaml'))
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        package_path, 'params', 'dwa_nav_params.yaml'))
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
+                              description='Use simulation (Gazebo) clock if true'),
+        DeclareLaunchArgument('map', default_value=map_yaml_path,
+                              description='Full path to map file to load'),
+        DeclareLaunchArgument('params_file', default_value=nav2_param_path,
+                              description='Full path to param file to load'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
+            launch_arguments={
+                'map': map_yaml_path,
+                'use_sim_time': use_sim_time,
+                'params_file': nav2_param_path}.items(),
+        ),
+    ])

src/yahboomcar_nav/launch/navigation_rtabmap_launch.py

@@ -0,0 +1,27 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    package_launch_path =os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch')
+
+    laser_bringup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [package_launch_path,'/laser_bringup_launch.py'])
+    )
+
+    rtabmap_localization_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [package_launch_path, '/rtabmap_localization_launch.py'])
+    )
+
+    rtabmap_nav_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [package_launch_path, '/rtabmap_nav_launch.py'])
+    )
+
+    return LaunchDescription([
+        laser_bringup_launch, 
+        rtabmap_localization_launch,
+        rtabmap_nav_launch
+    ])

src/yahboomcar_nav/launch/navigation_teb_launch.py

@@ -0,0 +1,36 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    package_path = get_package_share_directory('yahboomcar_nav')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    map_yaml_path = LaunchConfiguration(
+        'map', default=os.path.join(package_path, 'maps', 'yahboomcar.yaml'))
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        package_path, 'params', 'teb_nav_params.yaml'))
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
+                              description='Use simulation (Gazebo) clock if true'),
+        DeclareLaunchArgument('map', default_value=map_yaml_path,
+                              description='Full path to map file to load'),
+        DeclareLaunchArgument('params_file', default_value=nav2_param_path,
+                              description='Full path to param file to load'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
+            launch_arguments={
+                'map': map_yaml_path,
+                'use_sim_time': use_sim_time,
+                'params_file': nav2_param_path}.items(),
+        ),
+    ])

src/yahboomcar_nav/launch/occupancy_grid_launch.py

@@ -0,0 +1,35 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    resolution = LaunchConfiguration('resolution', default='0.05')
+    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'resolution',
+            default_value=resolution,
+            description='Resolution of a grid cell in the published occupancy grid'),
+
+        DeclareLaunchArgument(
+            'publish_period_sec',
+            default_value=publish_period_sec,
+            description='OccupancyGrid publishing period'),
+
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+
+        Node(
+            package='cartographer_ros',
+            executable='occupancy_grid_node',
+            name='occupancy_grid_node',
+            output='screen',
+            parameters=[{'use_sim_time': use_sim_time}],
+            arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]),
+    ])

src/yahboomcar_nav/launch/rtabmap_localization_launch.py

@@ -0,0 +1,86 @@
+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
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    qos = LaunchConfiguration('qos')
+    localization = LaunchConfiguration('localization')
+    config_rviz = os.path.join(
+        get_package_share_directory('rtabmap_launch'), 'launch', 'config', 'rgbd.rviz'
+    )
+
+    parameters={
+          'frame_id':'base_footprint',
+          'use_sim_time':use_sim_time,
+          'subscribe_rgbd':True,
+          'subscribe_scan':True,
+          'use_action_for_goal':True,
+          'qos_scan':qos,
+          'qos_image':qos,
+          'qos_imu':qos,
+          # RTAB-Map's parameters should be strings:
+          '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)
+    }
+
+    remappings=[
+          ('rgb/image', '/camera/color/image_raw'),
+          ('rgb/camera_info', '/camera/color/camera_info'),
+          ('depth/image', '/camera/depth/image_raw'),
+          ('odom', '/odom')
+        ]
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'qos', default_value='2',
+            description='QoS used for input sensor topics'),
+            
+        DeclareLaunchArgument(
+            'localization', default_value='true',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, 
+                              description='Configuration path of rviz2.'),
+        
+        DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'),
+
+
+        # Nodes to launch
+        Node(
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=[{'approx_sync':True, 'approx_sync_max_interval':0.01, 'use_sim_time':use_sim_time, 'qos':qos}],
+            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', 
+            parameters=[parameters,
+              {'Mem/IncrementalMemory':'False',
+               'Mem/InitWMWithAllNodes':'True'}],
+            remappings=remappings),
+
+    ])

src/yahboomcar_nav/launch/rtabmap_nav_launch.py

@@ -0,0 +1,36 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    package_path = get_package_share_directory('yahboomcar_nav')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    map_yaml_path = LaunchConfiguration(
+        'map', default=os.path.join(package_path, 'maps', 'yahboomcar.yaml'))
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        package_path, 'params', 'rtabmap_nav_params.yaml'))
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
+                              description='Use simulation (Gazebo) clock if true'),
+        DeclareLaunchArgument('map', default_value=map_yaml_path,
+                              description='Full path to map file to load'),
+        DeclareLaunchArgument('params_file', default_value=nav2_param_path,
+                              description='Full path to param file to load'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
+            launch_arguments={      
+                'map': map_yaml_path,
+                'use_sim_time': use_sim_time,
+                'params_file': nav2_param_path}.items(),
+        ),
+    ])

src/yahboomcar_nav/launch/rtabmap_sync_launch.py

@@ -0,0 +1,95 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    qos = LaunchConfiguration('qos')
+    localization = LaunchConfiguration('localization')
+    config_rviz = os.path.join(
+        get_package_share_directory('rtabmap_launch'), 'launch', 'config', 'rgbd.rviz'
+    )
+
+    parameters={
+          'frame_id':'base_footprint',
+          'use_sim_time':use_sim_time,
+          'subscribe_rgbd':True,
+          'subscribe_scan':True,
+          'use_action_for_goal':True,
+          'qos_scan':qos,
+          'qos_image':qos,
+          'qos_imu':qos,
+          # RTAB-Map's parameters should be strings:
+          '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)
+    }
+
+    remappings=[
+          ('rgb/image', '/camera/color/image_raw'),
+          ('rgb/camera_info', '/camera/color/camera_info'),
+          ('depth/image', '/camera/depth/image_raw'),
+          ('odom', '/odom')
+        ]
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'qos', default_value='2',
+            description='QoS used for input sensor topics'),
+            
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, 
+                              description='Configuration path of rviz2.'),
+        
+        DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'),
+
+
+        # Nodes to launch
+        Node(
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=[{'approx_sync':True, 'approx_sync_max_interval':0.01, 'use_sim_time':use_sim_time, 'qos':qos}],
+            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),
+
+        # Node(
+        #     package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+        #     parameters=[parameters],
+        #     remappings=remappings),
+        
+        # Node(
+        #     package='rviz2', executable='rviz2', output='screen',
+        #     condition=IfCondition(LaunchConfiguration("rviz")),
+        #     arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
+    ])

src/yahboomcar_nav/launch/rtabmap_viz_launch.py

@@ -0,0 +1,51 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    qos = LaunchConfiguration('qos')
+
+    parameters={
+          'frame_id':'base_footprint',
+          'use_sim_time':use_sim_time,
+          'subscribe_rgbd':True,
+          'subscribe_scan':True,
+          'use_action_for_goal':True,
+          'qos_scan':qos,
+          'qos_image':qos,
+          'qos_imu':qos,
+          # RTAB-Map's parameters should be strings:
+          '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)
+    }
+
+    remappings=[
+          ('rgb/image', '/camera/color/image_raw'),
+          ('rgb/camera_info', '/camera/color/camera_info'),
+          ('depth/image', '/camera/depth/image_raw'),
+          ('odom', '/odom')
+        ]
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'qos', default_value='2',
+            description='QoS used for input sensor topics'),
+            
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=[parameters],
+            remappings=remappings),
+        
+    ])

src/yahboomcar_nav/launch/save_map_launch.py

@@ -0,0 +1,34 @@
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+import os
+
+
+def generate_launch_description():
+    package_share_path = str(get_package_share_path('yahboomcar_nav'))
+    # 获取yahboomcar_nav目录的路径
+    package_path = os.path.abspath(os.path.join(
+        package_share_path, "../../../../src/yahboomcar_nav"))
+    map_name = "yahboomcar"
+    default_map_path = os.path.join(package_path, 'maps', map_name)
+
+    map_arg = DeclareLaunchArgument(name='map_path', default_value=str(default_map_path),
+                                    description='The path of the map')
+
+    map_saver_node = Node(
+        package='nav2_map_server',
+        executable='map_saver_cli',
+        arguments=[
+            '-f', LaunchConfiguration('map_path'), '--ros-args', '-p', 'save_map_timeout:=10000'],
+    )
+
+    return LaunchDescription([
+        map_arg,
+        map_saver_node
+    ])

src/yahboomcar_nav/maps/yahboomcar.yaml

@@ -0,0 +1,7 @@
+image: /root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.pgm
+mode: trinary
+resolution: 0.05
+origin: [-2.3, -8.95, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/yahboomcar_nav/maps/yahboomcar2.yaml

@@ -0,0 +1,7 @@
+image: /root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.pgm
+mode: trinary
+resolution: 0.05
+origin: [-10, -10, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/yahboomcar_nav/package.xml

@@ -0,0 +1,22 @@
+<?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>yahboomcar_nav</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1461190907@qq.com">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <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>

src/yahboomcar_nav/params/dwa_nav_params.yaml

@@ -0,0 +1,273 @@
+amcl:
+  ros__parameters:
+    use_sim_time: False
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "differential"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    # scan_topic: scan
+    # map_topic: map
+    # set_initial_pose: true
+    # # always_reset_initial_pose: false
+    # initial_pose:
+    #   x: 0.0
+    #   y: 0.0
+    #   z: 0.0
+    #   yaw: 0.0
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: False
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: odom
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 10.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    controller_plugins: ["FollowPath"]
+
+    # 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: 0.5
+      decel_lim_x: -0.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -0.5
+      vx_samples: 20
+      vy_samples: 0
+      vtheta_samples: 40
+      sim_time: 1.5
+      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
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.1
+      plugins: ["obstacle_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        inflation_radius: 0.1
+        cost_scaling_factor: 5.0
+      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"
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      robot_radius: 0.1
+      resolution: 0.05
+      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"
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 5.0
+        inflation_radius: 0.1
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    yaml_filename: "yahboomcar.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5000
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: False
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_footprint
+    transform_timeout: 0.1
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False

src/yahboomcar_nav/params/lds_2d.lua

@@ -0,0 +1,45 @@
+include "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "base_footprint",
+  published_frame = "odom",
+  odom_frame = "odom",
+  provide_odom_frame = false,
+  publish_frame_projected_to_2d = false,
+  use_odometry = true, 
+  use_nav_sat = false,
+  use_landmarks = false,
+  num_laser_scans = 1,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+TRAJECTORY_BUILDER_2D.min_range = 0.1
+TRAJECTORY_BUILDER_2D.max_range = 8
+TRAJECTORY_BUILDER_2D.missing_data_ray_length = 0.5
+TRAJECTORY_BUILDER_2D.use_imu_data = false 
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 
+TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
+
+POSE_GRAPH.constraint_builder.min_score = 0.7
+POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
+
+--POSE_GRAPH.optimize_every_n_nodes = 30
+
+return options

src/yahboomcar_nav/params/rtabmap_nav_params.yaml

@@ -0,0 +1,204 @@
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: odom
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 10.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    controller_plugins: ["FollowPath"]
+
+    # 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: 0.5
+      decel_lim_x: -0.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -0.5
+      vx_samples: 20
+      vy_samples: 0
+      vtheta_samples: 40
+      sim_time: 1.5
+      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
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.1
+      plugins: ["obstacle_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        inflation_radius: 0.1
+        cost_scaling_factor: 5.0
+      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"
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      robot_radius: 0.1
+      resolution: 0.05
+      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"
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 5.0
+        inflation_radius: 0.1
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: False
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_footprint
+    transform_timeout: 0.1
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False

src/yahboomcar_nav/params/teb_nav_params.yaml

@@ -0,0 +1,328 @@
+amcl:
+  ros__parameters:
+    use_sim_time: False
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "differential"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: False
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    ontroller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugin: "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
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+      stateful: True
+    # TEB parameters
+    FollowPath:
+      plugin: "teb_local_planner::TebLocalPlannerROS"
+      footprint_model.type: circular
+      footprint_model.radius: 0.1
+      min_obstacle_dist: 0.1
+      inflation_dist: 0.1
+      costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
+      costmap_converter_spin_thread: True
+      costmap_converter_rate: 15
+      enable_homotopy_class_planning: True
+      enable_multithreading: True
+      optimization_verbose: False
+      teb_autoresize: True
+      min_samples: 3
+      max_samples: 20
+      max_global_plan_lookahead_dist: 1.0
+      visualize_hc_graph: True
+      max_vel_x: 0.26
+      max_vel_theta: 1.0
+      acc_lim_x: 2.5
+      acc_lim_theta: 3.2
+
+costmap_converter:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: False
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_footprint
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.1
+      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 5.0
+        inflation_radius: 0.1
+      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"
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.2
+        z_voxels: 10
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: pointcloud
+        pointcloud:
+          topic: /intel_realsense_r200_depth/points
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+local_costmap_client:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: False
+      footprint_padding: 0.03
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint
+      robot_radius: 0.1 # radius set and used, so no footprint points
+      resolution: 0.05
+      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        footprint_clearing_enabled: true
+        max_obstacle_height: 2.0
+        combination_method: 1
+        scan:
+          topic: /scan
+          obstacle_range: 2.5
+          raytrace_range: 3.0
+          max_obstacle_height: 2.0
+          min_obstacle_height: 0.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          inf_is_valid: false
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        footprint_clearing_enabled: true
+        max_obstacle_height: 2.0
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        unknown_threshold: 15
+        mark_threshold: 0
+        observation_sources: pointcloud
+        combination_method: 1
+        pointcloud:  # no frame set, uses frame from message
+          topic: /intel_realsense_r200_depth/points
+          max_obstacle_height: 2.0
+          min_obstacle_height: 0.0
+          obstacle_range: 2.5
+          raytrace_range: 3.0
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"  
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+        nabled: true
+        subscribe_to_updates: true
+        transform_tolerance: 0.1
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        enabled: true
+        inflation_radius: 0.1
+        cost_scaling_factor: 5.0
+        inflate_unknown: false
+        inflate_around_unknown: true  
+      always_send_full_costmap: True
+
+global_costmap_client:
+  ros__parameters:
+    use_sim_time: False
+  
+global_costmap_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    yaml_filename: "yahboomcar.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: False
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_footprint
+    transform_timeout: 0.1
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

src/yahboomcar_nav/rviz/map.rviz

@@ -0,0 +1,340 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /LaserScan1
+        - /TF1/Frames1
+        - /TF1/Tree1
+        - /TF1/Tree1/map1
+        - /TF1/Tree1/map1/odom1
+        - /TF1/Tree1/map1/odom1/base_footprint1
+        - /TF1/Tree1/map1/odom1/base_footprint1/base_link1
+      Splitter Ratio: 0.7029411792755127
+    Tree Height: 617
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+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
+        back_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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
+          Value: true
+        front_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom
+      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: 47
+      Min Color: 0; 0; 0
+      Min Intensity: 47
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.029999999329447746
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        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
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        back_left_wheel:
+          Value: true
+        back_right_wheel:
+          Value: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        camera_link:
+          Value: true
+        front_left_wheel:
+          Value: true
+        front_right_wheel:
+          Value: true
+        imu_link:
+          Value: true
+        laser:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                back_left_wheel:
+                  {}
+                back_right_wheel:
+                  {}
+                camera_link:
+                  {}
+                front_left_wheel:
+                  {}
+                front_right_wheel:
+                  {}
+                imu_link:
+                  {}
+                laser:
+                  {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.1803981065750122
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.0672152042388916
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 166
+  Y: 43

src/yahboomcar_nav/rviz/nav.rviz

@@ -0,0 +1,562 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1/Frames1
+        - /PoseArray1/Topic1
+        - /Odometry1
+        - /Odometry1/Shape1
+        - /Odometry1/Covariance1
+        - /Odometry1/Covariance1/Position1
+        - /Odometry1/Covariance1/Orientation1
+        - /MarkerArray1/Topic1
+      Splitter Ratio: 0.5184426307678223
+    Tree Height: 629
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: nav2_rviz_plugins/Navigation 2
+    Name: Navigation 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      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: 47
+      Min Color: 0; 0; 0
+      Min Intensity: 47
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.029999999329447746
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      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
+        back_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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
+          Value: true
+        front_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        back_left_wheel:
+          Value: false
+        back_right_wheel:
+          Value: false
+        base_footprint:
+          Value: false
+        base_link:
+          Value: false
+        camera_link:
+          Value: false
+        front_left_wheel:
+          Value: false
+        front_right_wheel:
+          Value: false
+        imu_link:
+          Value: false
+        laser:
+          Value: false
+        laser_link:
+          Value: false
+        map:
+          Value: true
+        odom:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                back_left_wheel:
+                  {}
+                back_right_wheel:
+                  {}
+                camera_link:
+                  {}
+                front_left_wheel:
+                  {}
+                front_right_wheel:
+                  {}
+                imu_link:
+                  {}
+                laser:
+                  {}
+                laser_link:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: global_costmap
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        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.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        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
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: global_path
+      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
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /plan
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.019999999552965164
+      Axes Length: 0.30000001192092896
+      Axes Radius: 0.009999999776482582
+      Class: rviz_default_plugins/PoseArray
+      Color: 0; 255; 0
+      Enabled: true
+      Head Length: 0.07000000029802322
+      Head Radius: 0.029999999329447746
+      Name: PoseArray
+      Shaft Length: 0.23000000417232513
+      Shaft Radius: 0.009999999776482582
+      Shape: Arrow (Flat)
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /particlecloud
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: local_costmap
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        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
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 3
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom
+      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/PointCloud
+      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: PointCloud
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cost_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 255
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: local_path
+      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
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /local_plan
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/PoseWithCovariance
+      Color: 200; 25; 255
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Head Length: 0.10000000149011612
+      Head Radius: 0.10000000149011612
+      Name: PoseWithCovariance
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /amcl_pose
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+      Value: true
+    - 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/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+    - Class: nav2_rviz_plugins/GoalTool
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 9.005563735961914
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.1836086511611938
+        Y: 0.37973466515541077
+        Z: 0.04900933429598808
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5647963285446167
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.4672164916992188
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 858
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Navigation 2:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001b700000300fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000300000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320000000286000000b7000000b700ffffff000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003800000030000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1341
+  X: 79
+  Y: 27

src/yahboomcar_nav/rviz/rtabmap_map.rviz

@@ -0,0 +1,430 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Info1
+        - /TF1/Tree1
+        - /Map1
+      Splitter Ratio: 0.5147058963775635
+    Tree Height: 833
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+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
+    - Class: rtabmap_rviz_plugins/Info
+      Enabled: true
+      Name: Info
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /info
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 0.7155374884605408
+        Min Value: -0.12338679283857346
+        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: 47
+      Min Color: 0; 0; 0
+      Min Intensity: 47
+      Name: MapCloud
+      Node filtering angle (degrees): 30
+      Node filtering radius (m): 0
+      Position Transformer: XYZ
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /mapData
+      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
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /mapGraph
+      User: 255; 0; 0
+      Value: true
+      Virtual: 255; 0; 255
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        back_left_wheel:
+          Value: true
+        back_right_wheel:
+          Value: true
+        base_footprint:
+          Value: true
+        base_link:
+          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_link:
+          Value: true
+        front_left_wheel:
+          Value: true
+        front_right_wheel:
+          Value: true
+        imu_link:
+          Value: true
+        laser:
+          Value: true
+        laser_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                back_left_wheel:
+                  {}
+                back_right_wheel:
+                  {}
+                camera_link:
+                  camera_color_frame:
+                    camera_color_optical_frame:
+                      {}
+                  camera_depth_frame:
+                    camera_depth_optical_frame:
+                      {}
+                front_left_wheel:
+                  {}
+                front_right_wheel:
+                  {}
+                imu_link:
+                  {}
+                laser:
+                  {}
+                laser_link:
+                  {}
+      Update Interval: 0
+      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: ""
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: ""
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        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
+      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: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.019999999552965164
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_map
+      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/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 47
+      Min Color: 0; 0; 0
+      Min Intensity: 47
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.029999999329447746
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/color/image_raw
+      Value: false
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 1
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom_raw
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 9.297886848449707
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.8725785613059998
+        Y: -0.40878167748451233
+        Z: 1.779373049736023
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: map
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 1.8003987073898315
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001bd000003cafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003b000003ca000000c700fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000005a00fffffffb000000100044006900730070006c0061007900730100000000000001bd0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000284fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000284000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000057c000003ca00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 24

src/yahboomcar_nav/rviz/rtabmap_nav.rviz

@@ -0,0 +1,616 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1/Tree1
+        - /PoseArray1/Topic1
+        - /Odometry1/Shape1
+        - /Odometry1/Covariance1
+        - /Odometry1/Covariance1/Position1
+        - /Odometry1/Covariance1/Orientation1
+        - /MarkerArray1/Topic1
+      Splitter Ratio: 0.5184426307678223
+    Tree Height: 633
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: nav2_rviz_plugins/Navigation 2
+    Name: Navigation 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 0.1914999932050705
+        Min Value: 0.1914999932050705
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 2
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.03999999910593033
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      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
+        back_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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
+        front_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        back_left_wheel:
+          Value: true
+        back_right_wheel:
+          Value: true
+        base_footprint:
+          Value: false
+        base_link:
+          Value: false
+        camera_color_frame:
+          Value: true
+        camera_color_optical_frame:
+          Value: true
+        camera_depth_frame:
+          Value: true
+        camera_depth_optical_frame:
+          Value: true
+        camera_link:
+          Value: false
+        front_left_wheel:
+          Value: true
+        front_right_wheel:
+          Value: true
+        imu_link:
+          Value: false
+        laser:
+          Value: false
+        laser_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                back_left_wheel:
+                  {}
+                back_right_wheel:
+                  {}
+                camera_link:
+                  camera_color_frame:
+                    camera_color_optical_frame:
+                      {}
+                  camera_depth_frame:
+                    camera_depth_optical_frame:
+                      {}
+                front_left_wheel:
+                  {}
+                front_right_wheel:
+                  {}
+                imu_link:
+                  {}
+                laser:
+                  {}
+                laser_link:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: global_costmap
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        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.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        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
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: global_path
+      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
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /plan
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.019999999552965164
+      Axes Length: 0.30000001192092896
+      Axes Radius: 0.009999999776482582
+      Class: rviz_default_plugins/PoseArray
+      Color: 0; 255; 0
+      Enabled: true
+      Head Length: 0.07000000029802322
+      Head Radius: 0.029999999329447746
+      Name: PoseArray
+      Shaft Length: 0.23000000417232513
+      Shaft Radius: 0.009999999776482582
+      Shape: Arrow (Flat)
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /particlecloud
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: local_costmap
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        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
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 1
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom
+      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/PointCloud
+      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: PointCloud
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cost_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 255
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: local_path
+      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
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /local_plan
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/PoseWithCovariance
+      Color: 200; 25; 255
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Head Length: 0.10000000149011612
+      Head Radius: 0.10000000149011612
+      Name: PoseWithCovariance
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /amcl_pose
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+      Value: true
+    - 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: 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
+        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/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+    - Class: nav2_rviz_plugins/GoalTool
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 7.478187084197998
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.7786166667938232
+        Y: -1.399194359779358
+        Z: 0.13125765323638916
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.1772146224975586
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 862
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Navigation 2:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ed00000304fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000304000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032000000028a000000b7000000b700ffffff000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005430000030400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1846
+  X: 72
+  Y: 27

src/yahboomcar_nav/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/yahboomcar_nav
+[install]
+install-scripts=$base/lib/yahboomcar_nav

src/yahboomcar_nav/setup.py

@@ -0,0 +1,32 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'yahboomcar_nav'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share',package_name,'launch'),glob(os.path.join('launch','*launch.py'))),
+        (os.path.join('share',package_name,'rviz'),glob(os.path.join('rviz','*.rviz*'))),
+        (os.path.join('share', package_name, 'params'), glob(os.path.join('params', '*.*'))),
+        (os.path.join('share', package_name, 'maps'), glob(os.path.join('maps', '*.*'))),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='root',
+    maintainer_email='1461190907@qq.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'scan_filter = yahboomcar_nav.scan_filter:main'
+        ],
+    },
+)

src/yahboomcar_nav/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/yahboomcar_nav/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/yahboomcar_nav/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/yahboomcar_nav/yahboomcar_nav/scan_filter.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+# coding:utf-8
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import LaserScan
+import numpy as np
+from rclpy.clock import Clock
+
+
+class scan_compression(Node):
+    def __init__(self,name):
+        super().__init__(name)
+        self.multiple = 2
+        self.pub = self.create_publisher(LaserScan, "/downsampled_scan", 1000)
+        self.laserSub = self.create_subscription(LaserScan,"/scan", self.laserCallback, 1000)
+
+    def laserCallback(self, data):
+        # self.get_logger().info("laserCallback")
+        if not isinstance(data, LaserScan): return
+        laser_scan = LaserScan()
+        laser_scan.header.stamp = Clock().now().to_msg()
+        laser_scan.header.frame_id = data.header.frame_id
+        laser_scan.angle_increment = data.angle_increment * self.multiple
+        laser_scan.time_increment = data.time_increment
+        laser_scan.intensities = data.intensities
+        laser_scan.scan_time = data.scan_time
+        laser_scan.angle_min = data.angle_min
+        laser_scan.angle_max = data.angle_max
+        laser_scan.range_min = data.range_min
+        laser_scan.range_max = data.range_max
+        # self.get_logger().info("len(np.array(data.ranges)) = {}".format(len(np.array(data.ranges))))
+        for i in range(len(np.array(data.ranges))):
+            if i % self.multiple == 0: laser_scan.ranges.append(data.ranges[i])
+        self.pub.publish(laser_scan)
+
+def main():
+    rclpy.init()
+    scan_cp = scan_compression("scan_dilute")
+    rclpy.spin(scan_cp)
+    scan_cp.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/yahboomcar_slam/CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.5)
+project(yahboomcar_slam)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+find_package(PCL REQUIRED COMPONENTS common io visualization)
+
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+
+add_executable(pointcloud_mapping src/point_cloud_main.cpp src/point_cloud.cpp)
+target_include_directories(pointcloud_mapping PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(
+  pointcloud_mapping
+  "rclcpp"
+  "std_msgs"
+  "pcl_conversions"
+  "pcl_ros"
+  "cv_bridge"
+  "sensor_msgs"
+  "image_transport"
+  "tf2_ros"
+  "message_filters"
+  "geometry_msgs"
+)
+
+
+install(TARGETS 
+  pointcloud_mapping
+  DESTINATION lib/${PROJECT_NAME})
+
+include_directories(include/yahboomcar_slam)
+install(DIRECTORY launch params pcl rviz DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+target_link_libraries(pointcloud_mapping ${PCL_LIBRARIES})
+ament_package()

src/yahboomcar_slam/include/yahboomcar_slam/point_cloud.h

@@ -0,0 +1,133 @@
+#ifndef POINTCLOUDMAPPING_H
+#define POINTCLOUDMAPPING_H
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <cmath>
+#include <mutex>
+#include <thread>
+#include <chrono>
+#include <condition_variable>
+#include <ctime>
+#include <climits>
+#include <Eigen/Core>
+#include <Eigen/Geometry> // Eigen 几何模块
+
+#include <pcl/io/ply_io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/icp_nl.h>
+#include <pcl/registration/transforms.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/compression/compression_profiles.h>
+#include <pcl/compression/octree_pointcloud_compression.h>
+#include <pcl/common/transforms.h>
+#include <pcl/filters/filter.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "message_filters/subscriber.h"
+#include "message_filters/synchronizer.h"
+#include "message_filters/sync_policies/approximate_time.h"
+
+using namespace std;
+
+class PointCloudMapper: public rclcpp::Node
+{
+public:
+    typedef pcl::PointXYZRGBA PointT;
+    typedef pcl::PointCloud<PointT> PointCloud;
+    bool mbuseExact, mbuseCompressed, use_viewer;
+    std::string local_frame_id, global_frame_id;
+    size_t queueSize;
+
+    PointCloudMapper();
+
+    ~PointCloudMapper();
+
+    Eigen::Isometry3d convert2Eigen(geometry_msgs::msg::PoseStamped &Tcw);
+
+    // 插入一个keyframe，会更新一次地图
+    void insertKeyFrame(cv::Mat &color, cv::Mat &depth, geometry_msgs::msg::PoseStamped &Tcw);
+
+    void viewer();
+
+    void getGlobalCloudMap(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &outputMap);
+
+    void reset();
+
+    void shutdown();
+
+    void callback(const sensor_msgs::msg::Image::SharedPtr msgRGB,
+                    const sensor_msgs::msg::Image::SharedPtr msgD,  
+                    const geometry_msgs::msg::PoseStamped::SharedPtr tcw);
+protected:
+    unsigned int index = 0;
+    float mresolution = 0.04;
+    float mDepthMapFactor = 1000.0;
+    float mcx = 0, mcy = 0, mfx = 0, mfy = 0;
+    std::string mNodePath;
+
+    pcl::VoxelGrid<PointT> voxel; // 点云显示精度
+    int lastKeyframeSize = 0;       //
+    size_t mGlobalPointCloudID = 0; // 点云ID
+    size_t mLastGlobalPointCloudID = 0;
+
+    // data to generate point clouds
+    vector<cv::Mat> colorImgs, depthImgs; // image buffer
+    cv::Mat depthImg, colorImg, mpose;
+    vector<PointCloud> mvGlobalPointClouds; // 存储关键帧对应的点云序列
+    vector<geometry_msgs::msg::PoseStamped> mvGlobalPointCloudsPose;
+
+    PointCloud::Ptr globalMap, localMap;
+
+    bool shutDownFlag = false; // 程序退出标志位
+    mutex shutDownMutex;
+
+    bool mbKeyFrameUpdate = false; // 有新的关键帧插入
+    mutex keyframeMutex;
+    mutex keyFrameUpdateMutex;
+    mutex deletekeyframeMutex;
+
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_global_pointcloud;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_local_pointcloud;
+
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, geometry_msgs::msg::PoseStamped> approximate_sync_policy;
+
+    std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> rgb_sub;
+    std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> depth_sub;
+    std::shared_ptr<message_filters::Subscriber<geometry_msgs::msg::PoseStamped>> tcw_sub;
+
+    std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy>> syncApproximate;
+
+    PointCloud::Ptr generatePointCloud(cv::Mat &color, cv::Mat &depth, Eigen::Isometry3d &T);
+
+    void compressPointCloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud, std::stringstream &compressedData);
+
+    void depressPointCloud(std::stringstream &compressedData, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloudOut);
+
+    Eigen::Matrix4f cvMat2Eigen(const cv::Mat &cvT);
+
+    void dispDepth(const cv::Mat &in, cv::Mat &out, const float maxValue);
+};
+#endif // POINTCLOUDMAPPING_H

src/yahboomcar_slam/launch/camera_octomap_launch.py

@@ -0,0 +1,32 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+
+    bringup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_bringup'), 'launch'),
+        '/yahboomcar_bringup_x1_launch.py'])
+    )
+
+    octomap_mapping_node = Node(
+        package='octomap_server',
+        executable='octomap_server_node',
+        name="octomap_server",
+        remappings=[('/cloud_in','/camera/depth/points')],
+        parameters=[
+            {"resolution": 0.05},
+            {"frame_id": "odom"},
+            {"colored_map": False},
+            {"sensor_model.max_range": 5.0}
+        ]
+    )
+
+
+    return LaunchDescription([
+        bringup_launch,
+        octomap_mapping_node
+    ])

src/yahboomcar_slam/launch/display_octomap_launch.py

@@ -0,0 +1,28 @@
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+def generate_launch_description():
+    package_path = get_package_share_path('yahboomcar_slam')
+    default_rviz_config_path = package_path / 'rviz/cotomap.rviz'
+    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
+                                     description='Absolute path to rviz config file')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+    )
+
+    return LaunchDescription([
+        rviz_arg,
+        rviz_node
+    ])

src/yahboomcar_slam/launch/display_pcl_launch.py

@@ -0,0 +1,28 @@
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+def generate_launch_description():
+    package_path = get_package_share_path('yahboomcar_slam')
+    default_rviz_config_path = package_path / 'rviz/orbslam_pcl.rviz'
+    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
+                                     description='Absolute path to rviz config file')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+    )
+
+    return LaunchDescription([
+        rviz_arg,
+        rviz_node
+    ])

src/yahboomcar_slam/launch/octomap_server_launch.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    octomap_mapping_node = Node(
+        package='octomap_server',
+        executable='octomap_server_node',
+        name="octomap_server",
+        remappings=[('/cloud_in','/Local/PointCloudOutput')],
+        parameters=[
+            {"resolution": 0.05},
+            {"frame_id": "odom"},
+            {"colored_map": False},
+            {"sensor_model.max_range": 5.0}
+        ]
+    )
+
+    return LaunchDescription([
+        octomap_mapping_node
+    ])

src/yahboomcar_slam/launch/orbslam_base_launch.py

@@ -0,0 +1,36 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+# 参数声明与获取
+# from launch.actions import DeclareLaunchArgument
+# from launch.substitutions import LaunchConfiguration
+# 文件包含相关
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    package_path = get_package_share_directory('yahboomcar_slam')
+
+    bringup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(get_package_share_directory('yahboomcar_bringup'), 'launch'),
+        '/yahboomcar_bringup_x1_launch.py'])
+    )
+
+    orbslam_pose_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(package_path, 'launch'),
+        '/orbslam_pose_launch.py'])
+    )
+
+    # camera_link是相机本身的tf, camera是orbslam pose发布的
+    tf_camera_link_to_camera = Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            arguments = ['0', '0', '0', '1.57', '3.14', '1.57', 'camera_link', 'camera']
+    )
+    
+    return LaunchDescription([
+        bringup_launch,
+        orbslam_pose_launch,
+        tf_camera_link_to_camera
+    ])

src/yahboomcar_slam/launch/orbslam_pcl_map_launch.py

@@ -0,0 +1,34 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    package_path = get_package_share_directory('yahboomcar_slam')
+
+    tf_odom_to_world = Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            arguments = ['0', '0', '0', '1.57', '3.14', '1.57', 'odom', 'world']
+    )
+
+    pointcloud_mapping_config = os.path.join(              
+        package_path,
+        'params',
+        'pointcloud_map.yaml'
+    )
+
+    pointcloud_mapping_node = Node(
+        package='yahboomcar_slam',
+        executable='pointcloud_mapping',
+        name='pointcloud_mapping_node',
+        output="screen",
+        parameters=[pointcloud_mapping_config]
+    )
+    
+    return LaunchDescription([
+        tf_odom_to_world,
+        pointcloud_mapping_node
+    ])

src/yahboomcar_slam/launch/orbslam_pcl_octomap_launch.py

@@ -0,0 +1,45 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+# 参数声明与获取
+# from launch.actions import DeclareLaunchArgument
+# from launch.substitutions import LaunchConfiguration
+# 文件包含相关
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    package_path = get_package_share_directory('yahboomcar_slam')
+    
+    # orbslam_pcl_launch中全局坐标系是world, 所以需要提供world的tf
+    tf_odom_to_world = Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            arguments = ['0', '0', '0', '1.57', '3.14', '1.57', 'odom', 'world']
+    )
+
+    pointcloud_mapping_config = os.path.join(              
+        package_path,
+        'params',
+        'pointcloud_octomap.yaml'
+    )
+
+    pointcloud_mapping_node = Node(
+        package='yahboomcar_slam',
+        executable='pointcloud_mapping',
+        name='pointcloud_mapping_node',
+        output="screen",
+        parameters=[pointcloud_mapping_config]
+    )
+
+    octomap_server_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+        [os.path.join(package_path, 'launch'),
+         '/octomap_server_launch.py'])
+    )
+    
+    return LaunchDescription([
+        tf_odom_to_world,
+        pointcloud_mapping_node,
+        octomap_server_launch
+    ])

src/yahboomcar_slam/launch/orbslam_pose_launch.py

@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    package_path = get_package_share_directory('yahboomcar_slam')
+    default_orbvoc_path = os.path.join(package_path, 'params', 'ORBvoc.txt')
+    default_config_path = os.path.join(package_path, 'params', 'rgbd.yaml')
+    
+    orbslam_rgbd_pose = Node(
+        package='ros2_orbslam',
+        executable='rgbd_pose',
+        arguments=[default_orbvoc_path, default_config_path],
+    )
+
+    return LaunchDescription([
+        orbslam_rgbd_pose
+    ])

src/yahboomcar_slam/launch/orbslam_ros_launch.py

@@ -0,0 +1,47 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch.conditions import LaunchConfigurationEquals
+from launch.actions import DeclareLaunchArgument
+
+def generate_launch_description():
+    package_path = get_package_share_directory('yahboomcar_slam')
+    orbvoc_path = os.path.join(package_path, 'params', 'ORBvoc.txt')
+    mono_yaml_path = os.path.join(package_path, 'params', 'mono.yaml')
+    rgbd_yaml_path = os.path.join(package_path, 'params', 'rgbd.yaml')
+
+    orb_slam_type_arg = DeclareLaunchArgument(name='orb_slam_type', default_value='mono', 
+                                              choices=['mono','stereo','rgbd'],
+                                              description='The type of orbslam2')
+
+    # 单目相机
+    orbslam_mono = Node(
+        package='ros2_orbslam',
+        executable='mono',
+        arguments=[orbvoc_path, mono_yaml_path],
+        condition=LaunchConfigurationEquals('orb_slam_type', 'mono')
+    )
+
+    # 双目相机
+    orbslam_stereo = Node(
+        package='ros2_orbslam',
+        executable='stereo',
+        arguments=[orbvoc_path, mono_yaml_path],
+        condition=LaunchConfigurationEquals('orb_slam_type', 'stereo')
+    )
+    
+    # RGBD相机
+    orbslam_rgbd = Node(
+        package='ros2_orbslam',
+        executable='rgbd',
+        arguments=[orbvoc_path, rgbd_yaml_path],
+        condition=LaunchConfigurationEquals('orb_slam_type', 'rgbd')
+    )
+
+    return LaunchDescription([
+        orb_slam_type_arg,
+        orbslam_mono,
+        orbslam_stereo,
+        orbslam_rgbd
+    ])

src/yahboomcar_slam/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>yahboomcar_slam</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1461190907@qq.com">jetson</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>cv_bridge</depend>
+  <depend>sensor_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>tf2_ros</depend>
+  <depend>message_filters</depend>
+  <depend>geometry_msgs</depend>
+  
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/yahboomcar_slam/params/mono.yaml

@@ -0,0 +1,57 @@
+%YAML:1.0
+
+#--------------------------------------------------------------------------------------------
+# Camera Parameters. Adjust them!
+#--------------------------------------------------------------------------------------------
+
+# Camera calibration and distortion parameters (OpenCV) 
+Camera.fx: 517.306408
+Camera.fy: 516.469215
+Camera.cx: 318.643040
+Camera.cy: 255.313989
+
+Camera.k1: 0.262383
+Camera.k2: -0.953104
+Camera.p1: -0.005358
+Camera.p2: 0.002628
+Camera.k3: 1.163314
+
+# Camera frames per second 
+Camera.fps: 30.0
+
+# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
+Camera.RGB: 1
+
+#--------------------------------------------------------------------------------------------
+# ORB Parameters
+#--------------------------------------------------------------------------------------------
+
+# ORB Extractor: Number of features per image
+ORBextractor.nFeatures: 1000
+
+# ORB Extractor: Scale factor between levels in the scale pyramid 	
+ORBextractor.scaleFactor: 1.2
+
+# ORB Extractor: Number of levels in the scale pyramid	
+ORBextractor.nLevels: 8
+
+# ORB Extractor: Fast threshold
+# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
+# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
+# You can lower these values if your images have low contrast			
+ORBextractor.iniThFAST: 20
+ORBextractor.minThFAST: 7
+
+#--------------------------------------------------------------------------------------------
+# Viewer Parameters
+#--------------------------------------------------------------------------------------------
+Viewer.KeyFrameSize: 0.04
+Viewer.KeyFrameLineWidth: 1
+Viewer.GraphLineWidth: 0.9
+Viewer.PointSize: 0.025
+Viewer.CameraSize: 0.08
+Viewer.CameraLineWidth: 3
+Viewer.ViewpointX: 0
+Viewer.ViewpointY: -0.7
+Viewer.ViewpointZ: -1.8
+Viewer.ViewpointF: 500

src/yahboomcar_slam/params/pointcloud_map.yaml

@@ -0,0 +1,17 @@
+pointcloud_mapping_node:
+    ros__parameters:
+        topicColor: "/RGBD/RGB/Image"  
+        topicDepth: "/RGBD/Depth/Image"
+        topicTcw: "/RGBD/CameraPose"
+        local_frame_id: "camera"
+        global_frame_id: "world"
+        fx: 615.50506
+        fy: 623.69024
+        cx: 365.84388
+        cy: 238.778
+        resolution: 0.01
+        depthfactor: 1000.0
+        queueSize: 10
+        buseExact: false
+        use_viewer: true
+

src/yahboomcar_slam/params/pointcloud_octomap.yaml

@@ -0,0 +1,17 @@
+pointcloud_mapping_node:
+    ros__parameters:
+        topicColor: "/RGBD/RGB/Image"
+        topicDepth: "/RGBD/Depth/Image"
+        topicTcw: "/RGBD/CameraPose"
+        local_frame_id: "camera"
+        global_frame_id: "world"
+        fx: 615.50506
+        fy: 623.69024
+        cx: 365.84388
+        cy: 238.778
+        resolution: 0.01
+        depthfactor: 1000.0
+        queueSize: 10
+        buseExact: false
+        use_viewer: false
+

src/yahboomcar_slam/params/rgbd.yaml

@@ -0,0 +1,70 @@
+%YAML:1.0
+
+#--------------------------------------------------------------------------------------------
+# Camera Parameters. Adjust them!
+#--------------------------------------------------------------------------------------------
+
+# Camera calibration and distortion parameters (OpenCV) 
+Camera.fx: 517.306408
+Camera.fy: 516.469215
+Camera.cx: 318.643040
+Camera.cy: 255.313989
+
+Camera.k1: 0.262383
+Camera.k2: -0.953104
+Camera.p1: -0.005358
+Camera.p2: 0.002628
+Camera.k3: 1.163314
+
+Camera.width: 640
+Camera.height: 480
+
+# Camera frames per second 
+Camera.fps: 30.0
+
+# IR projector baseline times fx (aprox.)
+Camera.bf: 40.0
+
+# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
+Camera.RGB: 1
+
+# Close/Far threshold. Baseline times.
+ThDepth: 40.0
+
+# Deptmap values factor 
+DepthMapFactor: 5000.0
+
+#--------------------------------------------------------------------------------------------
+# ORB Parameters
+#--------------------------------------------------------------------------------------------
+
+# ORB Extractor: Number of features per image
+ORBextractor.nFeatures: 1000
+
+# ORB Extractor: Scale factor between levels in the scale pyramid 	
+ORBextractor.scaleFactor: 1.2
+
+# ORB Extractor: Number of levels in the scale pyramid	
+ORBextractor.nLevels: 8
+
+# ORB Extractor: Fast threshold
+# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
+# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
+# You can lower these values if your images have low contrast			
+ORBextractor.iniThFAST: 20
+ORBextractor.minThFAST: 7
+
+#--------------------------------------------------------------------------------------------
+# Viewer Parameters
+#--------------------------------------------------------------------------------------------
+Viewer.KeyFrameSize: 0.05
+Viewer.KeyFrameLineWidth: 1
+Viewer.GraphLineWidth: 0.9
+Viewer.PointSize:2
+Viewer.CameraSize: 0.08
+Viewer.CameraLineWidth: 3
+Viewer.ViewpointX: 0
+Viewer.ViewpointY: -0.7
+Viewer.ViewpointZ: -1.8
+Viewer.ViewpointF: 500
+

src/yahboomcar_slam/rviz/cotomap.rviz

@@ -0,0 +1,294 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 633
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+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
+    - Class: octomap_rviz_plugins/OccupancyGrid
+      Enabled: true
+      Max. Height Display: 3.4028234663852886e+38
+      Max. Octree Depth: 16
+      Min. Height Display: -3.4028234663852886e+38
+      Name: OccupancyGrid
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /octomap_full
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Alpha: 0.699999988079071
+      Class: octomap_rviz_plugins/OccupancyMap
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Max. Octree Depth: 16
+      Name: OccupancyMap
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /octomap_full
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /octomap_full_updates
+      Use Timestamp: false
+      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
+        back_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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
+          Value: true
+        front_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: 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: false
+      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
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /Local/PointCloudOutput
+      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: RGB8
+      Decay Time: 0
+      Enabled: false
+      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
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /Global/PointCloudOutput
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 12.658510208129883
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7103976607322693
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.3854002952575684
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 862
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001fd00000304fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000304000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000304fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000304000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005330000030400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1846
+  X: 72
+  Y: 27

src/yahboomcar_slam/rviz/orbslam_pcl.rviz

@@ -0,0 +1,321 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 633
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+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
+    - Class: octomap_rviz_plugins/OccupancyGrid
+      Enabled: false
+      Max. Height Display: 3.4028234663852886e+38
+      Max. Octree Depth: 16
+      Min. Height Display: -3.4028234663852886e+38
+      Name: OccupancyGrid
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /octomap_full
+      Value: false
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Alpha: 0.699999988079071
+      Class: octomap_rviz_plugins/OccupancyMap
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Max. Octree Depth: 16
+      Name: OccupancyMap
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /octomap_full
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /octomap_full_updates
+      Use Timestamp: false
+      Value: false
+    - 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
+        back_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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
+          Value: true
+        front_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: 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: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /Local/PointCloudOutput
+      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: 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: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /Global/PointCloudOutput
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      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: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /RGBD/Path
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 5.106155872344971
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.1078801155090332
+        Y: -0.7274764776229858
+        Z: 1.5379326343536377
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5603976845741272
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.005398750305176
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 862
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001fd00000304fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000304000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000304fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000304000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005330000030400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1846
+  X: 72
+  Y: 27

src/yahboomcar_slam/src/point_cloud.cpp

@@ -0,0 +1,383 @@
+#include "point_cloud.h"
+
+using namespace std;
+
+PointCloudMapper::PointCloudMapper() : Node("pointcloud_mapping")
+{
+    float fx_, fy_, cx_, cy_, resolution_, depthfactor_;
+    int queueSize_;
+    bool mbuseExact_;
+
+    mbuseCompressed = false;
+    lastKeyframeSize = 0;
+    mGlobalPointCloudID = 0; // 点云IDls
+    mLastGlobalPointCloudID = 0;
+    queueSize = 10;
+
+    std::string topicColor, topicDepth, topicTcw;
+
+    this->declare_parameter<std::string>("topicColor", "/RGBD/RGB/Image");
+    this->declare_parameter<std::string>("topicDepth", "/RGBD/Depth/Image");
+    this->declare_parameter<std::string>("topicTcw", "/RGBD/CameraPose");
+    this->declare_parameter<std::string>("local_frame_id", "camera");
+    this->declare_parameter<std::string>("global_frame_id", "camera");
+    this->declare_parameter<std::string>("node_path", "/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_slam/pcl/");
+    this->declare_parameter<bool>("use_viewer", false);
+    this->declare_parameter<float>("fx", 517.306408);
+    this->declare_parameter<float>("fy", 516.469215);
+    this->declare_parameter<float>("cx", 318.643040);
+    this->declare_parameter<float>("cy", 255.313989);
+    this->declare_parameter<float>("resolution", 0.05);
+    this->declare_parameter<float>("depthfactor", 1000.0);
+    this->declare_parameter<int>("queueSize", 10);
+    this->declare_parameter<bool>("buseExact", false);
+
+    this->get_parameter<std::string>("topicColor", topicColor);
+    this->get_parameter<std::string>("topicDepth", topicDepth);
+    this->get_parameter<std::string>("topicTcw", topicTcw);
+    this->get_parameter<std::string>("local_frame_id", local_frame_id);
+    this->get_parameter<std::string>("global_frame_id", global_frame_id);
+    this->get_parameter<std::string>("node_path", mNodePath);
+    this->get_parameter<bool>("use_viewer", use_viewer);
+    this->get_parameter<float>("fx", fx_);
+    this->get_parameter<float>("fy", fy_);
+    this->get_parameter<float>("cx", cx_);
+    this->get_parameter<float>("cy", cy_);
+    this->get_parameter<float>("resolution", resolution_);
+    this->get_parameter<float>("depthfactor", depthfactor_);
+    this->get_parameter<int>("queueSize", queueSize_);
+    this->get_parameter<bool>("buseExact", mbuseExact_);
+
+    mbuseExact = mbuseExact_;
+    queueSize = queueSize_;
+    mcx = cx_;
+    mcy = cy_;
+    mfx = fx_;
+    mfy = fy_;
+    mresolution = resolution_;
+    mDepthMapFactor = depthfactor_;
+
+    rgb_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(shared_ptr<rclcpp::Node>(this), topicColor);
+    depth_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(shared_ptr<rclcpp::Node>(this), topicDepth);
+    tcw_sub = std::make_shared<message_filters::Subscriber<geometry_msgs::msg::PoseStamped>>(shared_ptr<rclcpp::Node>(this), topicTcw);
+
+    syncApproximate = std::make_shared<message_filters::Synchronizer<approximate_sync_policy>>(approximate_sync_policy(10), *rgb_sub, *depth_sub, *tcw_sub);
+    syncApproximate->registerCallback(&PointCloudMapper::callback, this);
+
+    voxel.setLeafSize(mresolution, mresolution, mresolution);
+    globalMap = PointCloud::Ptr(new PointCloud());
+    localMap = PointCloud::Ptr(new PointCloud());
+
+    pub_global_pointcloud = this->create_publisher<sensor_msgs::msg::PointCloud2>("/Global/PointCloudOutput", 1);
+    pub_local_pointcloud = this->create_publisher<sensor_msgs::msg::PointCloud2>("/Local/PointCloudOutput", 10);
+}
+
+void PointCloudMapper::callback(const sensor_msgs::msg::Image::SharedPtr msgRGB,
+                                const sensor_msgs::msg::Image::SharedPtr msgD,
+                                const geometry_msgs::msg::PoseStamped::SharedPtr tcw)
+{
+    // RCLCPP_INFO(this->get_logger(), "callback start");
+    // RCLCPP_INFO(this->get_logger(), "tcw->pose.position.x = %f",tcw->pose.position.x);
+    // RCLCPP_INFO(this->get_logger(), "tcw->pose.position.y = %f",tcw->pose.position.y);
+    // RCLCPP_INFO(this->get_logger(), "tcw->pose.position.z = %f",tcw->pose.position.z);
+
+    cv::Mat color, depth, depthDisp;
+    geometry_msgs::msg::PoseStamped Tcw = *tcw;
+    cv_bridge::CvImageConstPtr pCvImage;
+
+    pCvImage = cv_bridge::toCvShare(msgRGB, "bgr8");
+    pCvImage->image.copyTo(color);
+    pCvImage = cv_bridge::toCvShare(msgD, msgD->encoding); // imageDepth->encoding
+    pCvImage->image.copyTo(depth);
+    // IR image input
+    if (color.type() == CV_16U)
+    {
+        cv::Mat tmp;
+        color.convertTo(tmp, CV_8U, 0.02);
+        cv::cvtColor(tmp, color, CV_GRAY2BGR);
+    }
+    if (depth.type() != CV_32F)
+        depth.convertTo(depth, CV_32F);
+
+    insertKeyFrame(color, depth, Tcw);
+}
+
+PointCloudMapper::~PointCloudMapper()
+{
+    shutdown();
+}
+
+// 由外部函数调用，每生成一个关键帧调用一次该函数
+void PointCloudMapper::insertKeyFrame(cv::Mat &color, cv::Mat &depth, geometry_msgs::msg::PoseStamped &T)
+{
+    unique_lock<mutex> lck(keyframeMutex);
+    // 已测试接受到的数据没有问题
+    // cout<< BLUE<<"--------------------------------T:\n"<<T.matrix()<<WHITE<<endl;
+    mvGlobalPointCloudsPose.push_back(T);
+
+    colorImgs.push_back(color.clone());
+    depthImgs.push_back(depth.clone());
+
+    // mLastGlobalPointCloudID=mGlobalPointCloudID;
+    mGlobalPointCloudID++;
+    mbKeyFrameUpdate = true;
+    RCLCPP_INFO(this->get_logger(), "receive a keyframe, id = %ld", mGlobalPointCloudID);
+    // cout << GREEN << "--------receive a keyframe, id = " << mGlobalPointCloudID << WHITE<< endl;
+}
+
+/**
+ * @function 更加关键帧生成点云、并对点云进行滤波处理
+ * 备注：点云生成函数在　台式机上调用时间在0.1ｓ 左右
+ */
+pcl::PointCloud<PointCloudMapper::PointT>::Ptr PointCloudMapper::generatePointCloud(cv::Mat &color, cv::Mat &depth, Eigen::Isometry3d &T)
+{
+    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
+    PointCloud::Ptr tmp(new PointCloud()); // point cloud is null ptr
+    for (int m = 0; m < depth.rows; m += 3)
+    {
+        for (int n = 0; n < depth.cols; n += 3)
+        {
+            float d = depth.ptr<float>(m)[n] / mDepthMapFactor;
+            if (d < 0.01 || d > 10)
+            {
+                continue;
+            }
+            // cout << "d = "<< d  <<endl;
+            PointT p;
+            p.z = d;
+            p.x = (n - mcx) * p.z / mfx;
+            p.y = (m - mcy) * p.z / mfy;
+
+            p.b = color.ptr<uchar>(m)[n * 3];
+            p.g = color.ptr<uchar>(m)[n * 3 + 1];
+            p.r = color.ptr<uchar>(m)[n * 3 + 2];
+
+            tmp->points.push_back(p);
+        }
+    }
+
+    PointCloud::Ptr cloud_voxel_tem(new PointCloud);
+    tmp->is_dense = false;
+    voxel.setInputCloud(tmp);
+    voxel.setLeafSize(mresolution, mresolution, mresolution);
+    voxel.filter(*cloud_voxel_tem);
+
+    PointCloud::Ptr cloud1(new PointCloud);
+
+    // 这里对点云进行变换
+    pcl::transformPointCloud(*cloud_voxel_tem, *cloud1, T.matrix());
+
+    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
+    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
+
+    cout << "generate point cloud from  kf-ID:" << mLastGlobalPointCloudID << ", size="
+         << cloud1->points.size() << " cost time: " << time_used.count() * 1000 << " ms ." << endl;
+    mLastGlobalPointCloudID++;
+    return cloud1;
+}
+
+// 由于随着尺寸的增加以后,显示函数会异常退出
+void PointCloudMapper::viewer()
+{
+    RCLCPP_INFO(this->get_logger(), "PointCloudMapper::viewer()");
+    int N = 0, i = 0;
+    bool KFUpdate = false;
+    if (use_viewer) {
+        pcl::visualization::CloudViewer pcl_viewer("viewer");
+        while (rclcpp::ok())
+        {
+            {
+                unique_lock<mutex> lck_shutdown(shutDownMutex);
+                if (shutDownFlag)
+                {
+                    break;
+                }
+            }
+            // keyframe is updated
+            KFUpdate = false;
+            {
+                unique_lock<mutex> lck(keyframeMutex);
+                N = (int)mvGlobalPointCloudsPose.size();
+                KFUpdate = mbKeyFrameUpdate;
+                mbKeyFrameUpdate = false;
+            }
+            if (KFUpdate)
+            {
+                std::cout << "----KFUpdate N:" << N << std::endl;
+                for (i = lastKeyframeSize; i < N && i < (lastKeyframeSize + 5); i++)
+                {
+                    if ((mvGlobalPointCloudsPose.size() != colorImgs.size()) ||
+                        (mvGlobalPointCloudsPose.size() != depthImgs.size()) ||
+                        (depthImgs.size() != colorImgs.size()))
+                    {
+                        cout << " depthImgs.size != colorImgs.size()  " << endl;
+                        continue;
+                    }
+                    PointCloud::Ptr tem_cloud1(new PointCloud());
+                    cout << "i: " << i << "  mvPosePointClouds.size(): " << mvGlobalPointCloudsPose.size() << endl;
+                    // 生成一幅点云大约在0.1s左右 点云数据
+                    Eigen::Isometry3d transform = convert2Eigen(mvGlobalPointCloudsPose[i]);
+                    // std::cout << "transform.matrix() = "<<transform.matrix() << std::endl;
+                    // cv::imshow("colorImgs"+i, colorImgs[i]);  //在窗口显示图像
+                    // cv::waitKey(0);  //暂停，保持图像显示，等待按键结束
+                    tem_cloud1 = generatePointCloud(colorImgs[i], depthImgs[i], transform);
+
+                    if (tem_cloud1->empty())
+                        continue;
+                    
+                    RCLCPP_INFO(this->get_logger(), "tem_cloud1 is not empty()");
+                    *globalMap += *tem_cloud1;
+
+                    sensor_msgs::msg::PointCloud2 local;
+                    pcl::toROSMsg(*tem_cloud1, local); // 转换成ROS下的数据类型 最终通过topic发布
+                    local.header.stamp = this->get_clock()->now();
+                    local.header.frame_id = local_frame_id;
+                    pub_local_pointcloud->publish(local);
+                    RCLCPP_INFO(this->get_logger(), "pub_local_pointcloud->publish(local)");
+                }
+                lastKeyframeSize = i;
+                sensor_msgs::msg::PointCloud2 output;
+                pcl::toROSMsg(*globalMap, output);
+                output.header.stamp = this->get_clock()->now();
+                output.header.frame_id = global_frame_id;
+                pub_global_pointcloud->publish(output);
+                cout << "show global map, size=" << globalMap->points.size() << endl;
+                pcl_viewer.showCloud(globalMap);
+            }
+        }
+    } else {
+        while (rclcpp::ok())
+        {
+            {
+                unique_lock<mutex> lck_shutdown(shutDownMutex);
+                if (shutDownFlag)
+                {
+                    break;
+                }
+            }
+            // keyframe is updated
+            KFUpdate = false;
+            {
+                unique_lock<mutex> lck(keyframeMutex);
+                N = (int)mvGlobalPointCloudsPose.size();
+                KFUpdate = mbKeyFrameUpdate;
+                mbKeyFrameUpdate = false;
+            }
+            if (KFUpdate)
+            {
+                std::cout << "----KFUpdate N:" << N << std::endl;
+                for (i = lastKeyframeSize; i < N && i < (lastKeyframeSize + 5); i++)
+                {
+                    if ((mvGlobalPointCloudsPose.size() != colorImgs.size()) ||
+                        (mvGlobalPointCloudsPose.size() != depthImgs.size()) ||
+                        (depthImgs.size() != colorImgs.size()))
+                    {
+                        cout << " depthImgs.size != colorImgs.size()  " << endl;
+                        continue;
+                    }
+                    PointCloud::Ptr tem_cloud1(new PointCloud());
+                    cout << "i: " << i << "  mvPosePointClouds.size(): " << mvGlobalPointCloudsPose.size() << endl;
+                    // 生成一幅点云大约在0.1s左右 点云数据
+                    Eigen::Isometry3d transform = convert2Eigen(mvGlobalPointCloudsPose[i]);
+                    // std::cout << "transform.matrix() = "<<transform.matrix() << std::endl;
+                    // cv::imshow("colorImgs"+i, colorImgs[i]);  //在窗口显示图像
+                    // cv::waitKey(0);  //暂停，保持图像显示，等待按键结束
+                    tem_cloud1 = generatePointCloud(colorImgs[i], depthImgs[i], transform);
+
+                    if (tem_cloud1->empty())
+                        continue;
+
+                    RCLCPP_INFO(this->get_logger(), "tem_cloud1 is not empty()");   
+                    *globalMap += *tem_cloud1;
+
+                    sensor_msgs::msg::PointCloud2 local;
+                    pcl::toROSMsg(*tem_cloud1, local); // 转换成ROS下的数据类型 最终通过topic发布
+                    local.header.stamp = this->get_clock()->now();
+                    local.header.frame_id = local_frame_id;
+                    pub_local_pointcloud->publish(local);
+                    RCLCPP_INFO(this->get_logger(), "pub_local_pointcloud->publish(local)");
+                }
+                lastKeyframeSize = i;
+                sensor_msgs::msg::PointCloud2 output;
+                pcl::toROSMsg(*globalMap, output);
+                output.header.stamp = this->get_clock()->now();
+                output.header.frame_id = global_frame_id;
+                pub_global_pointcloud->publish(output);
+                cout << "show global map, size=" << globalMap->points.size() << endl;
+            }
+        }
+    }
+}
+
+Eigen::Matrix4f PointCloudMapper::cvMat2Eigen(const cv::Mat &cvT)
+{
+    Eigen::Matrix<float, 4, 4> T;
+    T << cvT.at<float>(0, 0), cvT.at<float>(0, 1), cvT.at<float>(0, 2), cvT.at<float>(0, 3),
+        cvT.at<float>(1, 0), cvT.at<float>(1, 1), cvT.at<float>(1, 2), cvT.at<float>(1, 3),
+        cvT.at<float>(2, 0), cvT.at<float>(2, 1), cvT.at<float>(2, 2), cvT.at<float>(2, 3),
+        0, 0, 0, 1;
+
+    return T;
+}
+/**
+ * @ 将深度图像映射成彩色图
+ *
+ */
+void PointCloudMapper::dispDepth(const cv::Mat &in, cv::Mat &out, const float maxValue)
+{
+    cv::Mat tmp = cv::Mat(in.rows, in.cols, CV_8U);
+    const uint32_t maxInt = 255;
+
+    for (int r = 0; r < in.rows; ++r)
+    {
+        const uint16_t *itI = in.ptr<uint16_t>(r);
+        uint8_t *itO = tmp.ptr<uint8_t>(r);
+
+        for (int c = 0; c < in.cols; ++c, ++itI, ++itO)
+        {
+            *itO = (uint8_t)std::min((*itI * maxInt / maxValue), 255.0f);
+        }
+    }
+
+    cv::applyColorMap(tmp, out, cv::COLORMAP_JET);
+}
+
+Eigen::Isometry3d PointCloudMapper::convert2Eigen(geometry_msgs::msg::PoseStamped &Tcw)
+{
+    Eigen::Quaterniond q = Eigen::Quaterniond(Tcw.pose.orientation.w, Tcw.pose.orientation.x,
+                                              Tcw.pose.orientation.y, Tcw.pose.orientation.z);
+    Eigen::AngleAxisd V6(q);
+    //  V6.fromRotationMatrix<double,3,3>(q.toRotationMatrix());
+    Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // 三维变换矩阵
+    T.rotate(V6);                                        // 旋转部分赋值
+    T(0, 3) = Tcw.pose.position.x;
+    T(1, 3) = Tcw.pose.position.y;
+    T(2, 3) = Tcw.pose.position.z;
+    return T;
+}
+
+void PointCloudMapper::getGlobalCloudMap(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &outputMap)
+{
+    unique_lock<mutex> lck_keyframeUpdated(keyFrameUpdateMutex);
+    outputMap = globalMap;
+}
+
+// 复位点云显示模块
+void PointCloudMapper::reset()
+{
+    mvGlobalPointCloudsPose.clear();
+    mvGlobalPointClouds.clear();
+    mGlobalPointCloudID = 0;
+    mLastGlobalPointCloudID = 0;
+}
+
+void PointCloudMapper::shutdown()
+{
+    {
+        unique_lock<mutex> lck(shutDownMutex);
+        shutDownFlag = true;
+    }
+    string save_path = mNodePath + "resultPointCloudFile.pcd";
+    pcl::io::savePCDFile(save_path, *globalMap, true);
+    cout << "save pcd files to :  " << save_path << endl;
+}

src/yahboomcar_slam/src/point_cloud_main.cpp

@@ -0,0 +1,15 @@
+#include "point_cloud.h"
+#include <thread> 
+
+using namespace std;
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<PointCloudMapper>();
+	thread t(&PointCloudMapper::viewer, node);  // 第二个参数必须填对象的this指针，否则会拷贝对象。
+    rclcpp::spin(node);
+    node->shutdown();
+    rclcpp::shutdown();
+    return 0;
+}

src/占位.txt

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