feat: add lms-nav2-gazebo-slam module

.gitignore

@@ -0,0 +1,5 @@
+build/
+install/
+log/
+.colcon/
+*.log

build.sh

@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}"
+exec bash "$RUNTIME_ROOT/docker/docker_build_nav2.sh" "$(dirname "$MODULE_PATH")" "$(basename "$MODULE_PATH")" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}"

run.sh

@@ -0,0 +1,8 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+RUNTIME_ROOT="${AUTONOMY_STUDIO_RUNTIME_ROOT:-$HOME/autonomy-studio/runtime/nav2}"
+FOLDER_NAME="$(basename "$MODULE_PATH")"
+CONTAINER_NAME="${FOLDER_NAME#\[}"; CONTAINER_NAME="${CONTAINER_NAME/\]/-}"
+exec bash "$RUNTIME_ROOT/docker/docker_run_nav2.sh" "$CONTAINER_NAME" "$(dirname "$MODULE_PATH")" "$FOLDER_NAME" "${NAV2_DOCKER_IMAGE:-future-nav2-gazebo:humble}" "${ROS_DOMAIN_ID:-0}" "$@"

src/slam2d_pkg/config/slam_f1tenth.yaml

@@ -0,0 +1,39 @@
+# File: slam_f1tenth.yaml
+# Description: SLAM (slam_toolbox) parameters for F1TENTH using Hokuyo 2D LiDAR
+#
+# slam_mkmini 미러. F1TENTH 조정: max_laser_range(Hokuyo 사거리에 맞춤),
+# minimum_travel(소형차). scan_topic /scan (urg_node native). use_sim_time:True 유지(pub_clock).
+# ★ max_laser_range 는 실제 Hokuyo 모델(UST-10LX≈10m / UTM-30LX≈30m)에 맞춰 조정.
+
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: True
+    do_loop_closing: True
+    publish_map_to_odom_transform: True
+    map_frame: map
+    odom_frame: odom
+    base_frame: base_link
+    scan_topic: /scan
+    qos_overrides./scan.subscriber.reliability: best_effort
+    transform_timeout: 0.2
+    tf_buffer_duration: 60.0
+    min_laser_range: 0.10
+    max_laser_range: 10.0
+
+    # Scan processing cadence (소형차)
+    minimum_travel_distance: 0.2
+    minimum_travel_heading: 0.4
+
+    # Loop closure quality
+    loop_search_maximum_distance: 8.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+
+    # Continuous scan matching precision
+    correlation_search_space_resolution: 0.005
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_smear_deviation: 0.05
+
+    # Reject low-quality scan matches
+    link_match_minimum_response_fine: 0.2
+    link_scan_maximum_distance: 1.5

src/slam2d_pkg/config/slam_fr09.yaml

@@ -0,0 +1,35 @@
+# File: slam.yaml
+# Description: SLAM module parameters for FR-09 using Ouster-OS1-U
+
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: True
+    do_loop_closing: True
+    publish_map_to_odom_transform: True
+    map_frame: map
+    odom_frame: odom
+    base_frame: base_link
+    scan_topic: /scan
+    qos_overrides./scan.subscriber.reliability: best_effort
+    transform_timeout: 0.2
+    tf_buffer_duration: 60.0
+    min_laser_range: 0.2
+    max_laser_range: 12.0
+
+    # Scan processing cadence
+    minimum_travel_distance: 0.25
+    minimum_travel_heading: 0.40
+
+    # Loop closure quality
+    loop_search_maximum_distance: 8.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+
+    # Continuous scan matching precision
+    correlation_search_space_resolution: 0.005
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_smear_deviation: 0.05
+
+    # Reject low-quality scan matches
+    link_match_minimum_response_fine: 0.2
+    link_scan_maximum_distance: 1.5

src/slam2d_pkg/config/slam_mkmini.yaml

@@ -0,0 +1,35 @@
+# File: slam.yaml
+# Description: SLAM module parameters for FR-09 using Ouster-OS1-U
+
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: True
+    do_loop_closing: True
+    publish_map_to_odom_transform: True
+    map_frame: map
+    odom_frame: odom
+    base_frame: base_link
+    scan_topic: /scan
+    qos_overrides./scan.subscriber.reliability: best_effort
+    transform_timeout: 0.2
+    tf_buffer_duration: 60.0
+    min_laser_range: 0.20
+    max_laser_range: 12.0
+
+    # Scan processing cadence
+    minimum_travel_distance: 0.28
+    minimum_travel_heading: 0.45
+
+    # Loop closure quality
+    loop_search_maximum_distance: 8.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+
+    # Continuous scan matching precision
+    correlation_search_space_resolution: 0.005
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_smear_deviation: 0.05
+
+    # Reject low-quality scan matches
+    link_match_minimum_response_fine: 0.2
+    link_scan_maximum_distance: 1.5

src/slam2d_pkg/config/slam_turtlebot3.yaml

@@ -0,0 +1,24 @@
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: true
+    do_loop_closing: true
+    publish_map_to_odom_transform: true
+    map_frame: map
+    odom_frame: odom
+    base_frame: base_footprint
+    scan_topic: /scan
+    qos_overrides./scan.subscriber.reliability: best_effort
+    transform_timeout: 0.2
+    tf_buffer_duration: 60.0
+    min_laser_range: 0.12
+    max_laser_range: 12.0
+    minimum_travel_distance: 0.12
+    minimum_travel_heading: 0.18
+    loop_search_maximum_distance: 8.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+    correlation_search_space_resolution: 0.005
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_smear_deviation: 0.05
+    link_match_minimum_response_fine: 0.2
+    link_scan_maximum_distance: 1.5

src/slam2d_pkg/launch/slam.launch.py

@@ -0,0 +1,60 @@
+# File: slam.launch.py
+# Description:
+#     순수 SLAM 모듈
+#     Inputs : /scan, /odom_custom, TF(odom→base_link, base_link→os_lidar/os_imu)
+#     Outputs: /map, TF(map→odom)
+#
+#     Note:
+#       - perception 모듈이 먼저 실행되어 /scan, /odom_custom, TF를 발행 중이어야 함
+#       - sensor_bridge는 시뮬레이션에서만 perception 앞단에 필요
+#       - gui는 별도 실행
+#       - 차량 및 센서 모델 추가 시 ARGUMENTS의 choices 목록에 추가
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.descriptions import ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+ARGUMENTS = [
+    DeclareLaunchArgument('sim_mode', default_value='true'),
+    DeclareLaunchArgument('robot', default_value='turtlebot3', choices=['turtlebot3', 'fr09', 'mkmini', 'f1tenth']),
+]
+
+
+def launch_setup(context, *args, **kwargs):
+    sim_mode = LaunchConfiguration('sim_mode').perform(context).lower()
+    robot    = LaunchConfiguration('robot').perform(context)
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=PathJoinSubstitution([
+                FindPackageShare('slam2d_pkg'), 'config', f'slam_{robot}.yaml'
+            ]),
+            param_rewrites={'use_sim_time': sim_mode},
+            convert_types=True,
+        ),
+        allow_substs=True,
+    )
+
+    actions = [
+        Node(
+            package='slam_toolbox',
+            executable='async_slam_toolbox_node',
+            name='slam_toolbox',
+            output='screen',
+            parameters=[configured_params],
+            remappings=[('scan', '/scan')],
+        ),
+    ]
+
+    return actions
+
+
+def generate_launch_description():
+    ld = LaunchDescription(ARGUMENTS)
+    ld.add_action(OpaqueFunction(function=launch_setup))
+    return ld

src/slam2d_pkg/package.xml

@@ -0,0 +1,21 @@
+<?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>slam2d_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jang1future@gmail.com">dgist</maintainer>
+  <license>TODO: License declaration</license>
+
+  <!-- 런타임 의존 (rosdep 자동설치용). slam.launch.py 가 async_slam_toolbox_node 사용 -->
+  <exec_depend>slam_toolbox</exec_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/slam2d_pkg/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/slam2d_pkg
+[install]
+install_scripts=$base/lib/slam2d_pkg

src/slam2d_pkg/setup.py

@@ -0,0 +1,36 @@
+
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'slam2d_pkg'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),  
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='dgist',
+    maintainer_email='jang1future@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'test = slam2d_pkg.test:main',
+            'teleop_key = slam2d_pkg.teleop_key:main'
+        ],
+    },
+)

src/slam2d_pkg/slam2d_pkg/teleop_key.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+import time, math, pygame, carla
+HOST, PORT = "127.0.0.1", 2000
+#HOST, PORT = "192.168.219.71", 2000
+
+# speed limit < 20km/h
+SPEED_LIMIT_KMH = 6.0
+SPEED_LIMIT_MS  = SPEED_LIMIT_KMH / 3.6
+GOV_MARGIN_MS   = 1.2   
+GOV_TH_MIN      = 0.05  
+GOV_TH_CUT      = 0.35  
+GOV_KP_BRAKE    = 0.18  
+GOV_BIAS_BRAKE  = 0.04  
+
+# control tuning
+THR_SLEW_UP     = 1.2  
+THR_SLEW_DOWN   = 1.5  
+BRK_SLEW_UP     = 3.5   
+BRK_SLEW_DOWN   = 3.0  
+
+STEER_RATE      = 2.8  
+STEER_RETURN    = 4.0  
+
+SOFTSTART_THR   = 0.28 
+SOFTSTART_V_MAX = 4.0 
+
+NONLIN_WEIGHT   = 0.55  
+
+
+def find_vehicle(world, timeout=10.0, interval=0.2):
+    deadline = time.time() + timeout
+    last_cnt = -1
+    while time.time() < deadline:
+        try:
+            world.wait_for_tick(timeout=1.0)
+        except Exception:
+            pass
+        actors = world.get_actors().filter("vehicle.*")
+        if last_cnt != len(actors):
+
+            print(f"[carla] vehicles detected: {len(actors)}")
+            last_cnt = len(actors)
+        for v in actors:
+            if v.attributes.get("role_name", "") == "hero":
+                return v
+        time.sleep(interval)
+    return None
+
+def main():
+    client = carla.Client(HOST, PORT); client.set_timeout(5.0)
+    world  = client.get_world()
+
+    veh = find_vehicle(world, timeout=15.0)
+    if not veh:
+        print("No vehicle found. Make sure ros2_native.py spawned the hero.")
+        return
+
+    #veh.set_autopilot(False)
+
+    ctrl = carla.VehicleControl(throttle=0.0, brake=0.0, steer=0.0,
+                                hand_brake=False, reverse=False)
+    pygame.init()
+    pygame.display.set_caption("CARLA Teleop (WASD)")
+    screen = pygame.display.set_mode((400, 120))
+    clock = pygame.time.Clock()
+    
+    th_cmd = 0.0
+    br_cmd = 0.0
+    st_cmd = 0.0
+
+    def _speed_ms(v):  # m/s
+        return math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z)
+
+    def _slew(curr, target, up, down, dt):
+        rate = up if target > curr else down
+        step = rate * dt
+        if abs(target - curr) <= step:
+            return target
+        return curr + step * (1 if target > curr else -1)
+
+    try:
+        while True:
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT: 
+                    return
+                if event.type == pygame.KEYDOWN:
+                    if event.key == pygame.K_ESCAPE: 
+                        return
+                    if event.key == pygame.K_q: ctrl.reverse = True
+                    if event.key == pygame.K_e: ctrl.reverse = False
+                    if event.key == pygame.K_r: st_cmd = 0.0
+
+            keys = pygame.key.get_pressed()
+
+            dt = max(1e-3, clock.tick(60) / 1000.0)
+
+            accel_pressed  = keys[pygame.K_w] or keys[pygame.K_UP]
+            brake_pressed  = keys[pygame.K_s] or keys[pygame.K_DOWN]
+            left_pressed   = keys[pygame.K_a] or keys[pygame.K_LEFT]
+            right_pressed  = keys[pygame.K_d] or keys[pygame.K_RIGHT]
+            ctrl.hand_brake = keys[pygame.K_SPACE]
+
+            try:
+                v_ms = _speed_ms(veh.get_velocity())
+            except Exception:
+                v_ms = 0.0
+
+            want_thr = 0.0
+            if accel_pressed and not brake_pressed:
+                want_thr = 1.0
+                if v_ms < SOFTSTART_V_MAX:
+                    k = 1.0 - (v_ms / SOFTSTART_V_MAX)
+                    want_thr = max(want_thr * (1.0 - 0.25*k), SOFTSTART_THR)
+
+            if brake_pressed:
+                want_brk = 1.0
+            elif not accel_pressed and v_ms > 0.1:
+                want_brk = 0.15
+            else:
+                want_brk = 0.0
+
+            th_cmd = _slew(th_cmd, want_thr, THR_SLEW_UP, THR_SLEW_DOWN, dt)
+            br_cmd = _slew(br_cmd, want_brk, BRK_SLEW_UP, BRK_SLEW_DOWN, dt)
+
+            if br_cmd > 0.05:
+                th_cmd = max(0.0, th_cmd - 3.0*dt)
+
+            if left_pressed and not right_pressed:
+                target_st = -1.0
+            elif right_pressed and not left_pressed:
+                target_st = 1.0
+            else:
+                if abs(st_cmd) < STEER_RETURN*dt:
+                    target_st = 0.0
+                else:
+                    target_st = st_cmd - STEER_RETURN*dt * (1 if st_cmd > 0 else -1)
+
+            st_cmd = _slew(st_cmd, target_st, STEER_RATE, STEER_RATE, dt)
+
+            if v_ms < 15.0:
+                st_limit = 1.0 - 0.65*(v_ms/15.0)  
+            elif v_ms < 25.0:
+                st_limit = 0.35 - 0.10*((v_ms-15.0)/10.0) 
+            else:
+                st_limit = 0.25
+            st_cmd = max(-st_limit, min(st_limit, st_cmd))
+
+            steer_out = (1.0 - NONLIN_WEIGHT)*st_cmd + NONLIN_WEIGHT*(st_cmd**3)
+
+            if v_ms >= (SPEED_LIMIT_MS - GOV_MARGIN_MS):
+                approach = min(1.0, max(0.0, (v_ms - (SPEED_LIMIT_MS - GOV_MARGIN_MS)) / GOV_MARGIN_MS))
+                th_cmd = max(GOV_TH_MIN, th_cmd * (GOV_TH_CUT + (1.0 - GOV_TH_CUT) * (1.0 - approach)))
+
+            if v_ms > SPEED_LIMIT_MS:
+                overshoot = v_ms - SPEED_LIMIT_MS
+                th_cmd = 0.0
+                br_cmd = max(br_cmd, min(1.0, GOV_KP_BRAKE * overshoot + GOV_BIAS_BRAKE))
+
+            ctrl.throttle = max(0.0, min(1.0, th_cmd))
+            ctrl.brake    = max(0.0, min(1.0, br_cmd))
+            ctrl.steer    = max(-1.0, min(1.0, steer_out))
+            veh.apply_control(ctrl)
+
+            screen.fill((20,20,20))
+            txt = f"v:{v_ms:4.1f} T:{ctrl.throttle:.2f} B:{ctrl.brake:.2f} S:{ctrl.steer:.2f} Rev:{int(ctrl.reverse)} HB:{int(ctrl.hand_brake)}"
+            pygame.display.set_caption(txt)
+            pygame.display.flip()
+
+            clock.tick(30)
+    finally:
+        pygame.quit()
+
+if __name__ == "__main__":
+    main()

src/slam2d_pkg/slam2d_pkg/test.py

@@ -0,0 +1,6 @@
+def main():
+    print('Hi from slam2d_pkg.')
+
+
+if __name__ == '__main__':
+    main()

src/slam2d_pkg/test/test_copyright.py

@@ -0,0 +1,25 @@
+# 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
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/slam2d_pkg/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/slam2d_pkg/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/slam3d_pkg/config/rtabmap_fr09-real.yaml

@@ -0,0 +1,107 @@
+icp_odometry:
+  ros__parameters:
+    use_sim_time: true
+
+    odom_frame_id: icp_odom
+    guess_frame_id: odom
+    OdomF2M/ScanSubtractRadius: '0.0'
+    OdomF2M/ScanMaxSize: '10000'
+
+
+    frame_id: base_link
+    wait_for_transform: 5.0
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+    Mem/NotLinkedNodesKept: "false"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.5"                    # 한 번의 odometry 업데이트 최대 회전 제한. 크면 오정합 발생. 작으면 계산량 증가. default: 0.78rad
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+    Icp/GuessFromTf: "true"
+
+    Odom/GuessMotion: "false"
+
+rtabmap:
+  ros__parameters:
+    use_sim_time: true
+
+    subscribe_rgbd: false
+    subscribe_rgb: false
+    subscribe_depth: false
+    subscribe_scan_cloud: true
+    use_action_for_goal: true
+    odom_sensor_sync: true
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Mem/NotLinkedNodesKept: "false"
+    Grid/RangeMin: "0.0"
+    Grid/NormalsSegmentation: "true"
+    Grid/MaxGroundAngle: "8"
+    Grid/NormalK: "10"
+
+    Grid/MaxGroundHeight: "-0.8"
+    Grid/MinGroundHeight: "-1.3"
+    Grid/MaxObstacleHeight: "0.1"
+
+    Grid/RayTracing: "false"
+    Grid/3D: "false"
+    Grid/Sensor: "0"
+    RGBD/OptimizeMaxError: "0.5"
+
+    RGBD/LinearUpdate: "0.05"
+    RGBD/AngularUpdate: "0.05"
+    RGBD/ProximityPathMaxNeighbors: "1"
+    Grid/RangeMax: "25.0"
+    Mem/LaserScanVoxelSize: "0.01"
+    Odom/FilteringStrategy: "1"
+    Odom/ScanKeyFrameThr: "0.9"
+    Odom/GuessMotion: "false"
+
+    Grid/CellSize: "0.2"
+    Grid/ClusterRadius: "0.2"
+    Grid/MinClusterSize: "2"
+    Grid/FlatObstacleDetected: "false"
+
+    RGBD/StartAtOrigin: "true"
+    RGBD/LocalizationPriorError: "2.0"
+    RGBD/LocalizationSmoothing: "false"
+    Icp/GuessFromTf: "true"
+
+
+    frame_id: base_link
+    wait_for_transform: 5.0
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.2"
+
+    Odom/GuessMotion: "false"

src/slam3d_pkg/config/rtabmap_fr09-sim.yaml

@@ -0,0 +1,102 @@
+icp_odometry:
+  ros__parameters:
+    use_sim_time: true
+
+    odom_frame_id: icp_odom
+    guess_frame_id: odom
+    OdomF2M/ScanSubtractRadius: '0.0'
+    OdomF2M/ScanMaxSize: '10000'
+
+
+    frame_id: base_link
+    wait_for_transform: 2.0
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+    Mem/NotLinkedNodesKept: "false"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.5"                    # 한 번의 odometry 업데이트 최대 회전 제한. 크면 오정합 발생. 작으면 계산량 증가. default: 0.78rad
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+    Icp/GuessFromTf: "true"
+
+    Odom/GuessMotion: "false"
+
+rtabmap:
+  ros__parameters:
+    use_sim_time: true
+
+    subscribe_rgbd: false
+    subscribe_rgb: false
+    subscribe_depth: false
+    subscribe_scan_cloud: true
+    use_action_for_goal: true
+    odom_sensor_sync: true
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Mem/NotLinkedNodesKept: "false"
+    Grid/RangeMin: "0.0"
+    Grid/NormalsSegmentation: "false"
+    Grid/MaxGroundHeight: "0.1"
+    Grid/MinGroundHeight: "-0.2"
+    Grid/MaxObstacleHeight: "1.0"
+    Grid/RayTracing: "false"
+    Grid/3D: "false"
+    Grid/Sensor: "0"
+    RGBD/OptimizeMaxError: "0.5"
+
+    RGBD/LinearUpdate: "0.05"
+    RGBD/AngularUpdate: "0.05"
+    RGBD/ProximityPathMaxNeighbors: "1"
+    Grid/RangeMax: "30.0"
+    Mem/LaserScanVoxelSize: "0.01"
+    Odom/FilteringStrategy: "1"
+    Odom/ScanKeyFrameThr: "0.9"
+    Odom/GuessMotion: "false"
+
+    Grid/CellSize: "0.2"
+    Grid/MinClusterSize: "10"
+    Grid/FlatObstacleDetected: "true"
+
+    RGBD/StartAtOrigin: "true"
+    RGBD/LocalizationPriorError: "5.0"
+    RGBD/LocalizationSmoothing: "false"
+    Icp/GuessFromTf: "true"
+
+
+    frame_id: base_link
+    wait_for_transform: 2.0
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+
+    Odom/GuessMotion: "false"

src/slam3d_pkg/config/rtabmap_fr09.yaml

@@ -0,0 +1,102 @@
+icp_odometry:
+  ros__parameters:
+    odom_frame_id: icp_odom
+    guess_frame_id: odom
+    OdomF2M/ScanSubtractRadius: '0.0'
+    OdomF2M/ScanMaxSize: '10000'
+
+
+    frame_id: base_link
+    wait_for_transform: 5.0
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+    Mem/NotLinkedNodesKept: "false"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.5"                    # 한 번의 odometry 업데이트 최대 회전 제한. 크면 오정합 발생. 작으면 계산량 증가. default: 0.78rad
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+    Icp/GuessFromTf: "true"
+
+    Odom/GuessMotion: "false"
+
+rtabmap:
+  ros__parameters:
+    subscribe_rgb: false
+    subscribe_depth: false
+    subscribe_scan_cloud: true
+    use_action_for_goal: true
+    odom_sensor_sync: true
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Mem/NotLinkedNodesKept: "false"
+    Grid/RangeMin: "0.0"
+    Grid/NormalsSegmentation: "true"
+    Grid/MaxGroundAngle: "8"
+    Grid/NormalK: "10"
+
+    Grid/MaxGroundHeight: "-0.8"
+    Grid/MinGroundHeight: "-1.3"
+    Grid/MaxObstacleHeight: "0.1"
+
+    Grid/RayTracing: "false"
+    Grid/3D: "false"
+    Grid/Sensor: "0"
+    RGBD/OptimizeMaxError: "0.5"
+
+    RGBD/LinearUpdate: "0.05"
+    RGBD/AngularUpdate: "0.05"
+    RGBD/ProximityPathMaxNeighbors: "1"
+    Grid/RangeMax: "25.0"
+    Mem/LaserScanVoxelSize: "0.01"
+    Odom/FilteringStrategy: "1"
+    Odom/ScanKeyFrameThr: "0.9"
+    Odom/GuessMotion: "false"
+
+    Grid/CellSize: "0.2"
+    Grid/ClusterRadius: "0.2"
+    Grid/MinClusterSize: "2"
+    Grid/FlatObstacleDetected: "false"
+
+    RGBD/StartAtOrigin: "true"
+    RGBD/LocalizationPriorError: "2.0"
+    RGBD/LocalizationSmoothing: "false"
+    Icp/GuessFromTf: "true"
+
+
+    frame_id: base_link
+    wait_for_transform: 5.0
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.2"
+
+    Odom/GuessMotion: "false"

src/slam3d_pkg/config/rtabmap_mkmini-real.yaml

@@ -0,0 +1,96 @@
+icp_odometry:
+  ros__parameters:
+    odom_frame_id: icp_odom
+    guess_frame_id: odom
+    OdomF2M/ScanSubtractRadius: '0.0'
+    OdomF2M/ScanMaxSize: '10000'
+
+
+    frame_id: base_link
+    wait_for_transform: 2.0
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+    Mem/NotLinkedNodesKept: "false"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+    Icp/GuessFromTf: "false"
+
+    Odom/GuessMotion: "false"
+
+rtabmap:
+  ros__parameters:
+    subscribe_rgb: false
+    subscribe_depth: false
+    subscribe_scan_cloud: true
+    use_action_for_goal: true
+    odom_sensor_sync: true
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Mem/NotLinkedNodesKept: "false"
+    Grid/RangeMin: "0.0"
+    Grid/NormalsSegmentation: "false"
+    Grid/MaxGroundHeight: "0.1"
+    Grid/MinGroundHeight: "-0.2"
+    Grid/MaxObstacleHeight: "1.0"
+    Grid/RayTracing: "false"
+    Grid/3D: "false"
+    RGBD/OptimizeMaxError: "0.5"
+
+    RGBD/LinearUpdate: "0.05"
+    RGBD/AngularUpdate: "0.05"
+    RGBD/ProximityPathMaxNeighbors: "1"
+    Grid/RangeMax: "10.0"
+    Mem/LaserScanVoxelSize: "0.01"
+    Odom/FilteringStrategy: "1"
+    Odom/ScanKeyFrameThr: "0.9"
+    Odom/GuessMotion: "false"
+
+    Grid/CellSize: "0.2"
+    Grid/MinClusterSize: "10"
+    Grid/FlatObstacleDetected: "true"
+
+    RGBD/StartAtOrigin: "true"
+    RGBD/LocalizationPriorError: "5.0"
+    RGBD/LocalizationSmoothing: "false"
+    Icp/GuessFromTf: "false"
+
+
+    frame_id: base_link
+    wait_for_transform: 2.0
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+
+    Odom/GuessMotion: "false"

src/slam3d_pkg/config/rtabmap_mkmini-sim.yaml

@@ -0,0 +1,96 @@
+icp_odometry:
+  ros__parameters:
+    odom_frame_id: icp_odom
+    guess_frame_id: odom
+    OdomF2M/ScanSubtractRadius: '0.0'
+    OdomF2M/ScanMaxSize: '10000'
+
+
+    frame_id: base_link
+    wait_for_transform: 2.0
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+    Mem/NotLinkedNodesKept: "false"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+    Icp/GuessFromTf: "false"
+
+    Odom/GuessMotion: "false"
+
+rtabmap:
+  ros__parameters:
+    subscribe_rgb: false
+    subscribe_depth: false
+    subscribe_scan_cloud: true
+    use_action_for_goal: true
+    odom_sensor_sync: true
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Mem/NotLinkedNodesKept: "false"
+    Grid/RangeMin: "0.0"
+    Grid/NormalsSegmentation: "false"
+    Grid/MaxGroundHeight: "0.1"
+    Grid/MinGroundHeight: "-0.2"
+    Grid/MaxObstacleHeight: "1.0"
+    Grid/RayTracing: "false"
+    Grid/3D: "false"
+    RGBD/OptimizeMaxError: "0.5"
+
+    RGBD/LinearUpdate: "0.05"
+    RGBD/AngularUpdate: "0.05"
+    RGBD/ProximityPathMaxNeighbors: "1"
+    Grid/RangeMax: "10.0"
+    Mem/LaserScanVoxelSize: "0.01"
+    Odom/FilteringStrategy: "1"
+    Odom/ScanKeyFrameThr: "0.9"
+    Odom/GuessMotion: "false"
+
+    Grid/CellSize: "0.2"
+    Grid/MinClusterSize: "10"
+    Grid/FlatObstacleDetected: "true"
+
+    RGBD/StartAtOrigin: "true"
+    RGBD/LocalizationPriorError: "5.0"
+    RGBD/LocalizationSmoothing: "false"
+    Icp/GuessFromTf: "false"
+
+
+    frame_id: base_link
+    wait_for_transform: 2.0
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+
+    Odom/GuessMotion: "false"

src/slam3d_pkg/config/rtabmap_mkmini.yaml

@@ -0,0 +1,96 @@
+icp_odometry:
+  ros__parameters:
+    odom_frame_id: icp_odom
+    guess_frame_id: odom
+    OdomF2M/ScanSubtractRadius: '0.0'
+    OdomF2M/ScanMaxSize: '10000'
+
+
+    frame_id: base_link
+    wait_for_transform: 2.0
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+    Mem/NotLinkedNodesKept: "false"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+    Icp/GuessFromTf: "false"
+
+    Odom/GuessMotion: "false"
+
+rtabmap:
+  ros__parameters:
+    subscribe_rgb: false
+    subscribe_depth: false
+    subscribe_scan_cloud: true
+    use_action_for_goal: true
+    odom_sensor_sync: true
+
+    approx_sync: true
+    topic_queue_size: 30
+    sync_queue_size: 30
+
+    Mem/NotLinkedNodesKept: "false"
+    Grid/RangeMin: "0.0"
+    Grid/NormalsSegmentation: "false"
+    Grid/MaxGroundHeight: "0.1"
+    Grid/MinGroundHeight: "-0.2"
+    Grid/MaxObstacleHeight: "1.0"
+    Grid/RayTracing: "false"
+    Grid/3D: "false"
+    RGBD/OptimizeMaxError: "0.5"
+
+    RGBD/LinearUpdate: "0.05"
+    RGBD/AngularUpdate: "0.05"
+    RGBD/ProximityPathMaxNeighbors: "1"
+    Grid/RangeMax: "10.0"
+    Mem/LaserScanVoxelSize: "0.01"
+    Odom/FilteringStrategy: "1"
+    Odom/ScanKeyFrameThr: "0.9"
+    Odom/GuessMotion: "false"
+
+    Grid/CellSize: "0.2"
+    Grid/MinClusterSize: "10"
+    Grid/FlatObstacleDetected: "true"
+
+    RGBD/StartAtOrigin: "true"
+    RGBD/LocalizationPriorError: "5.0"
+    RGBD/LocalizationSmoothing: "false"
+    Icp/GuessFromTf: "false"
+
+
+    frame_id: base_link
+    wait_for_transform: 2.0
+
+    Reg/Strategy: "1"
+    Reg/Force3DoF: "true"
+
+    Icp/VoxelSize: "0.03"
+    Icp/MaxCorrespondenceDistance: "0.2"
+    Icp/PointToPlaneGroundNormalsUp: "0.9"
+    Icp/RangeMin: "0.0"
+    Icp/MaxTranslation: "1.0"
+    Icp/MaxRotation: "0.78"
+
+    Icp/PointToPlaneMinComplexity: "0.03"
+    Icp/PointToPlaneLowComplexityStrategy: "1"
+    Icp/OutlierRatio: "0.6"
+    Icp/Force4DoF: "true"
+    Icp/CorrespondenceRatio: "0.02"
+
+    Odom/GuessMotion: "false"

src/slam3d_pkg/launch/slam.launch.py

@@ -0,0 +1,74 @@
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.actions import GroupAction
+from launch_ros.actions import PushRosNamespace
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.descriptions import ParameterFile
+from launch.substitutions import PythonExpression
+from launch.conditions import IfCondition, UnlessCondition
+from nav2_common.launch import RewrittenYaml
+
+
+ARGUMENTS = [
+    DeclareLaunchArgument('sim_mode', default_value='true'),
+    DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09', 'f1tenth']),
+]
+
+def launch_setup(context, *args, **kwargs):
+    sim_mode = LaunchConfiguration('sim_mode').perform(context).lower()
+    robot = LaunchConfiguration('robot').perform(context)
+
+    remappings=[
+          ('/tf', 'tf'),
+          ('/tf_static', 'tf_static'),
+          ('odom', 'icp_odom'),
+        #   ('odom', '/vehicle_odom'),
+          ('scan_cloud', '/lidar_filtered')]
+    
+    # Directories
+    pkg_slam = get_package_share_directory(
+        'slam3d_pkg')
+    
+    if sim_mode == 'true':
+        rtabmap_params_file = PathJoinSubstitution([
+            pkg_slam,
+            'config',
+            PythonExpression(["'rtabmap_' + '", robot, "' + '-sim.yaml'"])])
+    else:
+        rtabmap_params_file = PathJoinSubstitution([
+            pkg_slam,
+            'config',
+            PythonExpression(["'rtabmap_' + '", robot, "' + '-real.yaml'"])])        
+    
+    load_nodes = GroupAction(
+        actions=[
+            Node(
+                package='rtabmap_odom',
+                executable='icp_odometry',
+                name='icp_odometry',
+                output='screen',
+                parameters=[rtabmap_params_file],
+                remappings=remappings,
+            ),
+            Node(
+                package='rtabmap_slam',
+                executable='rtabmap',
+                name='rtabmap',
+                output='screen',
+                parameters=[rtabmap_params_file],
+                remappings=remappings,
+                arguments=['-d']
+            ),
+        ],
+    )
+    return [load_nodes]
+
+def generate_launch_description():
+    ld = LaunchDescription(ARGUMENTS)
+    ld.add_action(OpaqueFunction(function=launch_setup))
+    return ld
+

src/slam3d_pkg/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>slam3d_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jang1future@gmail.com">dgist</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/slam3d_pkg/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/slam3d_pkg
+[install]
+install_scripts=$base/lib/slam3d_pkg

src/slam3d_pkg/setup.py

@@ -0,0 +1,36 @@
+
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'slam3d_pkg'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),  
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='dgist',
+    maintainer_email='jang1future@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'test = slam3d_pkg.test:main',
+            'teleop_key = slam3d_pkg.teleop_key:main'
+        ],
+    },
+)

src/slam3d_pkg/slam3d_pkg/teleop_key.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+import time, math, pygame, carla
+HOST, PORT = "127.0.0.1", 2000
+#HOST, PORT = "192.168.219.71", 2000
+
+# speed limit < 20km/h
+SPEED_LIMIT_KMH = 6.0
+SPEED_LIMIT_MS  = SPEED_LIMIT_KMH / 3.6
+GOV_MARGIN_MS   = 1.2   
+GOV_TH_MIN      = 0.05  
+GOV_TH_CUT      = 0.35  
+GOV_KP_BRAKE    = 0.18  
+GOV_BIAS_BRAKE  = 0.04  
+
+# control tuning
+THR_SLEW_UP     = 1.2  
+THR_SLEW_DOWN   = 1.5  
+BRK_SLEW_UP     = 3.5   
+BRK_SLEW_DOWN   = 3.0  
+
+STEER_RATE      = 2.8  
+STEER_RETURN    = 4.0  
+
+SOFTSTART_THR   = 0.28 
+SOFTSTART_V_MAX = 4.0 
+
+NONLIN_WEIGHT   = 0.55  
+
+
+def find_vehicle(world, timeout=10.0, interval=0.2):
+    deadline = time.time() + timeout
+    last_cnt = -1
+    while time.time() < deadline:
+        try:
+            world.wait_for_tick(timeout=1.0)
+        except Exception:
+            pass
+        actors = world.get_actors().filter("vehicle.*")
+        if last_cnt != len(actors):
+
+            print(f"[carla] vehicles detected: {len(actors)}")
+            last_cnt = len(actors)
+        for v in actors:
+            if v.attributes.get("role_name", "") == "hero":
+                return v
+        time.sleep(interval)
+    return None
+
+def main():
+    client = carla.Client(HOST, PORT); client.set_timeout(5.0)
+    world  = client.get_world()
+
+    veh = find_vehicle(world, timeout=15.0)
+    if not veh:
+        print("No vehicle found. Make sure ros2_native.py spawned the hero.")
+        return
+
+    #veh.set_autopilot(False)
+
+    ctrl = carla.VehicleControl(throttle=0.0, brake=0.0, steer=0.0,
+                                hand_brake=False, reverse=False)
+    pygame.init()
+    pygame.display.set_caption("CARLA Teleop (WASD)")
+    screen = pygame.display.set_mode((400, 120))
+    clock = pygame.time.Clock()
+    
+    th_cmd = 0.0
+    br_cmd = 0.0
+    st_cmd = 0.0
+
+    def _speed_ms(v):  # m/s
+        return math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z)
+
+    def _slew(curr, target, up, down, dt):
+        rate = up if target > curr else down
+        step = rate * dt
+        if abs(target - curr) <= step:
+            return target
+        return curr + step * (1 if target > curr else -1)
+
+    try:
+        while True:
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT: 
+                    return
+                if event.type == pygame.KEYDOWN:
+                    if event.key == pygame.K_ESCAPE: 
+                        return
+                    if event.key == pygame.K_q: ctrl.reverse = True
+                    if event.key == pygame.K_e: ctrl.reverse = False
+                    if event.key == pygame.K_r: st_cmd = 0.0
+
+            keys = pygame.key.get_pressed()
+
+            dt = max(1e-3, clock.tick(60) / 1000.0)
+
+            accel_pressed  = keys[pygame.K_w] or keys[pygame.K_UP]
+            brake_pressed  = keys[pygame.K_s] or keys[pygame.K_DOWN]
+            left_pressed   = keys[pygame.K_a] or keys[pygame.K_LEFT]
+            right_pressed  = keys[pygame.K_d] or keys[pygame.K_RIGHT]
+            ctrl.hand_brake = keys[pygame.K_SPACE]
+
+            try:
+                v_ms = _speed_ms(veh.get_velocity())
+            except Exception:
+                v_ms = 0.0
+
+            want_thr = 0.0
+            if accel_pressed and not brake_pressed:
+                want_thr = 1.0
+                if v_ms < SOFTSTART_V_MAX:
+                    k = 1.0 - (v_ms / SOFTSTART_V_MAX)
+                    want_thr = max(want_thr * (1.0 - 0.25*k), SOFTSTART_THR)
+
+            if brake_pressed:
+                want_brk = 1.0
+            elif not accel_pressed and v_ms > 0.1:
+                want_brk = 0.15
+            else:
+                want_brk = 0.0
+
+            th_cmd = _slew(th_cmd, want_thr, THR_SLEW_UP, THR_SLEW_DOWN, dt)
+            br_cmd = _slew(br_cmd, want_brk, BRK_SLEW_UP, BRK_SLEW_DOWN, dt)
+
+            if br_cmd > 0.05:
+                th_cmd = max(0.0, th_cmd - 3.0*dt)
+
+            if left_pressed and not right_pressed:
+                target_st = -1.0
+            elif right_pressed and not left_pressed:
+                target_st = 1.0
+            else:
+                if abs(st_cmd) < STEER_RETURN*dt:
+                    target_st = 0.0
+                else:
+                    target_st = st_cmd - STEER_RETURN*dt * (1 if st_cmd > 0 else -1)
+
+            st_cmd = _slew(st_cmd, target_st, STEER_RATE, STEER_RATE, dt)
+
+            if v_ms < 15.0:
+                st_limit = 1.0 - 0.65*(v_ms/15.0)  
+            elif v_ms < 25.0:
+                st_limit = 0.35 - 0.10*((v_ms-15.0)/10.0) 
+            else:
+                st_limit = 0.25
+            st_cmd = max(-st_limit, min(st_limit, st_cmd))
+
+            steer_out = (1.0 - NONLIN_WEIGHT)*st_cmd + NONLIN_WEIGHT*(st_cmd**3)
+
+            if v_ms >= (SPEED_LIMIT_MS - GOV_MARGIN_MS):
+                approach = min(1.0, max(0.0, (v_ms - (SPEED_LIMIT_MS - GOV_MARGIN_MS)) / GOV_MARGIN_MS))
+                th_cmd = max(GOV_TH_MIN, th_cmd * (GOV_TH_CUT + (1.0 - GOV_TH_CUT) * (1.0 - approach)))
+
+            if v_ms > SPEED_LIMIT_MS:
+                overshoot = v_ms - SPEED_LIMIT_MS
+                th_cmd = 0.0
+                br_cmd = max(br_cmd, min(1.0, GOV_KP_BRAKE * overshoot + GOV_BIAS_BRAKE))
+
+            ctrl.throttle = max(0.0, min(1.0, th_cmd))
+            ctrl.brake    = max(0.0, min(1.0, br_cmd))
+            ctrl.steer    = max(-1.0, min(1.0, steer_out))
+            veh.apply_control(ctrl)
+
+            screen.fill((20,20,20))
+            txt = f"v:{v_ms:4.1f} T:{ctrl.throttle:.2f} B:{ctrl.brake:.2f} S:{ctrl.steer:.2f} Rev:{int(ctrl.reverse)} HB:{int(ctrl.hand_brake)}"
+            pygame.display.set_caption(txt)
+            pygame.display.flip()
+
+            clock.tick(30)
+    finally:
+        pygame.quit()
+
+if __name__ == "__main__":
+    main()

src/slam3d_pkg/slam3d_pkg/test.py

@@ -0,0 +1,6 @@
+def main():
+    print('Hi from slam3d_pkg.')
+
+
+if __name__ == '__main__':
+    main()

src/slam3d_pkg/test/test_copyright.py

@@ -0,0 +1,25 @@
+# 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
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/slam3d_pkg/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/slam3d_pkg/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/slam_pkg/launch/slam.launch.py

@@ -0,0 +1,15 @@
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    node = Node(
+        package='slam_pkg',
+        executable='slam_manager',
+        name='slam_manager',
+        output='screen',
+    )
+    
+    ld = LaunchDescription([])
+    ld.add_action(node)
+    return ld

src/slam_pkg/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>slam_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jang1future@gmail.com">dgist</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/slam_pkg/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/slam_pkg
+[install]
+install_scripts=$base/lib/slam_pkg

src/slam_pkg/setup.py

@@ -0,0 +1,36 @@
+
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'slam_pkg'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),        
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='dgist',
+    maintainer_email='jang1future@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'test = slam_pkg.test:main',
+            'slam_manager = slam_pkg.slam_manager:main',
+        ],
+    },
+)

src/slam_pkg/slam_pkg/slam_manager.py

@@ -0,0 +1,56 @@
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy
+from std_msgs.msg import String
+
+import subprocess
+import json
+
+class SLAMManager(Node):
+    def __init__(self):
+        super().__init__('slam_manager')
+        self.get_logger().info('SLAM Manager node has been started.')
+
+        qos = QoSProfile(
+            depth=1,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=ReliabilityPolicy.RELIABLE
+        )
+        self.slam_process = None
+
+        self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos)
+
+    def handle_config(self, msg:String):
+        self.get_logger().info(f"SLAM Received config: {msg.data}")
+        config_data = json.loads(msg.data)
+        
+        self.slam_process = subprocess.Popen(['ros2', 'launch', 
+                                    f'slam{config_data.get("level")}_pkg', 
+                                    'slam.launch.py', 
+                                    f'sim_mode:={config_data.get("sim_mode")}', 
+                                    f'robot:={config_data.get("robot")}'])
+        pass
+
+    def destroy_node(self):
+        if self.slam_process:
+            self.slam_process.terminate()
+            self.slam_process.wait()
+        
+        return super().destroy_node()
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SLAMManager()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
+    

src/slam_pkg/slam_pkg/teleop_key.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+import time, math, pygame, carla
+HOST, PORT = "127.0.0.1", 2000
+#HOST, PORT = "192.168.219.71", 2000
+
+# speed limit < 20km/h
+SPEED_LIMIT_KMH = 6.0
+SPEED_LIMIT_MS  = SPEED_LIMIT_KMH / 3.6
+GOV_MARGIN_MS   = 1.2   
+GOV_TH_MIN      = 0.05  
+GOV_TH_CUT      = 0.35  
+GOV_KP_BRAKE    = 0.18  
+GOV_BIAS_BRAKE  = 0.04  
+
+# control tuning
+THR_SLEW_UP     = 1.2  
+THR_SLEW_DOWN   = 1.5  
+BRK_SLEW_UP     = 3.5   
+BRK_SLEW_DOWN   = 3.0  
+
+STEER_RATE      = 2.8  
+STEER_RETURN    = 4.0  
+
+SOFTSTART_THR   = 0.28 
+SOFTSTART_V_MAX = 4.0 
+
+NONLIN_WEIGHT   = 0.55  
+
+
+def find_vehicle(world, timeout=10.0, interval=0.2):
+    deadline = time.time() + timeout
+    last_cnt = -1
+    while time.time() < deadline:
+        try:
+            world.wait_for_tick(timeout=1.0)
+        except Exception:
+            pass
+        actors = world.get_actors().filter("vehicle.*")
+        if last_cnt != len(actors):
+
+            print(f"[carla] vehicles detected: {len(actors)}")
+            last_cnt = len(actors)
+        for v in actors:
+            if v.attributes.get("role_name", "") == "hero":
+                return v
+        time.sleep(interval)
+    return None
+
+def main():
+    client = carla.Client(HOST, PORT); client.set_timeout(5.0)
+    world  = client.get_world()
+
+    veh = find_vehicle(world, timeout=15.0)
+    if not veh:
+        print("No vehicle found. Make sure ros2_native.py spawned the hero.")
+        return
+
+    #veh.set_autopilot(False)
+
+    ctrl = carla.VehicleControl(throttle=0.0, brake=0.0, steer=0.0,
+                                hand_brake=False, reverse=False)
+    pygame.init()
+    pygame.display.set_caption("CARLA Teleop (WASD)")
+    screen = pygame.display.set_mode((400, 120))
+    clock = pygame.time.Clock()
+    
+    th_cmd = 0.0
+    br_cmd = 0.0
+    st_cmd = 0.0
+
+    def _speed_ms(v):  # m/s
+        return math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z)
+
+    def _slew(curr, target, up, down, dt):
+        rate = up if target > curr else down
+        step = rate * dt
+        if abs(target - curr) <= step:
+            return target
+        return curr + step * (1 if target > curr else -1)
+
+    try:
+        while True:
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT: 
+                    return
+                if event.type == pygame.KEYDOWN:
+                    if event.key == pygame.K_ESCAPE: 
+                        return
+                    if event.key == pygame.K_q: ctrl.reverse = True
+                    if event.key == pygame.K_e: ctrl.reverse = False
+                    if event.key == pygame.K_r: st_cmd = 0.0
+
+            keys = pygame.key.get_pressed()
+
+            dt = max(1e-3, clock.tick(60) / 1000.0)
+
+            accel_pressed  = keys[pygame.K_w] or keys[pygame.K_UP]
+            brake_pressed  = keys[pygame.K_s] or keys[pygame.K_DOWN]
+            left_pressed   = keys[pygame.K_a] or keys[pygame.K_LEFT]
+            right_pressed  = keys[pygame.K_d] or keys[pygame.K_RIGHT]
+            ctrl.hand_brake = keys[pygame.K_SPACE]
+
+            try:
+                v_ms = _speed_ms(veh.get_velocity())
+            except Exception:
+                v_ms = 0.0
+
+            want_thr = 0.0
+            if accel_pressed and not brake_pressed:
+                want_thr = 1.0
+                if v_ms < SOFTSTART_V_MAX:
+                    k = 1.0 - (v_ms / SOFTSTART_V_MAX)
+                    want_thr = max(want_thr * (1.0 - 0.25*k), SOFTSTART_THR)
+
+            if brake_pressed:
+                want_brk = 1.0
+            elif not accel_pressed and v_ms > 0.1:
+                want_brk = 0.15
+            else:
+                want_brk = 0.0
+
+            th_cmd = _slew(th_cmd, want_thr, THR_SLEW_UP, THR_SLEW_DOWN, dt)
+            br_cmd = _slew(br_cmd, want_brk, BRK_SLEW_UP, BRK_SLEW_DOWN, dt)
+
+            if br_cmd > 0.05:
+                th_cmd = max(0.0, th_cmd - 3.0*dt)
+
+            if left_pressed and not right_pressed:
+                target_st = -1.0
+            elif right_pressed and not left_pressed:
+                target_st = 1.0
+            else:
+                if abs(st_cmd) < STEER_RETURN*dt:
+                    target_st = 0.0
+                else:
+                    target_st = st_cmd - STEER_RETURN*dt * (1 if st_cmd > 0 else -1)
+
+            st_cmd = _slew(st_cmd, target_st, STEER_RATE, STEER_RATE, dt)
+
+            if v_ms < 15.0:
+                st_limit = 1.0 - 0.65*(v_ms/15.0)  
+            elif v_ms < 25.0:
+                st_limit = 0.35 - 0.10*((v_ms-15.0)/10.0) 
+            else:
+                st_limit = 0.25
+            st_cmd = max(-st_limit, min(st_limit, st_cmd))
+
+            steer_out = (1.0 - NONLIN_WEIGHT)*st_cmd + NONLIN_WEIGHT*(st_cmd**3)
+
+            if v_ms >= (SPEED_LIMIT_MS - GOV_MARGIN_MS):
+                approach = min(1.0, max(0.0, (v_ms - (SPEED_LIMIT_MS - GOV_MARGIN_MS)) / GOV_MARGIN_MS))
+                th_cmd = max(GOV_TH_MIN, th_cmd * (GOV_TH_CUT + (1.0 - GOV_TH_CUT) * (1.0 - approach)))
+
+            if v_ms > SPEED_LIMIT_MS:
+                overshoot = v_ms - SPEED_LIMIT_MS
+                th_cmd = 0.0
+                br_cmd = max(br_cmd, min(1.0, GOV_KP_BRAKE * overshoot + GOV_BIAS_BRAKE))
+
+            ctrl.throttle = max(0.0, min(1.0, th_cmd))
+            ctrl.brake    = max(0.0, min(1.0, br_cmd))
+            ctrl.steer    = max(-1.0, min(1.0, steer_out))
+            veh.apply_control(ctrl)
+
+            screen.fill((20,20,20))
+            txt = f"v:{v_ms:4.1f} T:{ctrl.throttle:.2f} B:{ctrl.brake:.2f} S:{ctrl.steer:.2f} Rev:{int(ctrl.reverse)} HB:{int(ctrl.hand_brake)}"
+            pygame.display.set_caption(txt)
+            pygame.display.flip()
+
+            clock.tick(30)
+    finally:
+        pygame.quit()
+
+if __name__ == "__main__":
+    main()

src/slam_pkg/slam_pkg/test.py

@@ -0,0 +1,6 @@
+def main():
+    print('Hi from slam_pkg.')
+
+
+if __name__ == '__main__':
+    main()

src/slam_pkg/test/test_copyright.py

@@ -0,0 +1,25 @@
+# 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
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/slam_pkg/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/slam_pkg/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'