commit 0040af6283cbce7690a1c13328f2b138cdcaa0e4 Author: 유창훈 Date: Tue Jul 28 10:35:38 2026 +0900 feat: add lms-nav2-gazebo-slam module diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..76b3365 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +build/ +install/ +log/ +.colcon/ +*.log diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..0c1c5bc --- /dev/null +++ b/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}" diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..656b11d --- /dev/null +++ b/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}" "$@" diff --git a/src/slam2d_pkg/config/slam_f1tenth.yaml b/src/slam2d_pkg/config/slam_f1tenth.yaml new file mode 100755 index 0000000..2392a42 --- /dev/null +++ b/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 diff --git a/src/slam2d_pkg/config/slam_fr09.yaml b/src/slam2d_pkg/config/slam_fr09.yaml new file mode 100755 index 0000000..81479bc --- /dev/null +++ b/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 diff --git a/src/slam2d_pkg/config/slam_mkmini.yaml b/src/slam2d_pkg/config/slam_mkmini.yaml new file mode 100755 index 0000000..dca28f3 --- /dev/null +++ b/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 diff --git a/src/slam2d_pkg/config/slam_turtlebot3.yaml b/src/slam2d_pkg/config/slam_turtlebot3.yaml new file mode 100644 index 0000000..8b0320a --- /dev/null +++ b/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 diff --git a/src/slam2d_pkg/launch/slam.launch.py b/src/slam2d_pkg/launch/slam.launch.py new file mode 100755 index 0000000..5e067e8 --- /dev/null +++ b/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 diff --git a/src/slam2d_pkg/package.xml b/src/slam2d_pkg/package.xml new file mode 100755 index 0000000..d3f57bb --- /dev/null +++ b/src/slam2d_pkg/package.xml @@ -0,0 +1,21 @@ + + + + slam2d_pkg + 0.0.0 + TODO: Package description + dgist + TODO: License declaration + + + slam_toolbox + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/slam2d_pkg/resource/slam2d_pkg b/src/slam2d_pkg/resource/slam2d_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/slam2d_pkg/setup.cfg b/src/slam2d_pkg/setup.cfg new file mode 100755 index 0000000..7f59bd5 --- /dev/null +++ b/src/slam2d_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/slam2d_pkg +[install] +install_scripts=$base/lib/slam2d_pkg diff --git a/src/slam2d_pkg/setup.py b/src/slam2d_pkg/setup.py new file mode 100755 index 0000000..feeab45 --- /dev/null +++ b/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' + ], + }, +) diff --git a/src/slam2d_pkg/slam2d_pkg/__init__.py b/src/slam2d_pkg/slam2d_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/slam2d_pkg/slam2d_pkg/teleop_key.py b/src/slam2d_pkg/slam2d_pkg/teleop_key.py new file mode 100755 index 0000000..92d86b7 --- /dev/null +++ b/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() diff --git a/src/slam2d_pkg/slam2d_pkg/test.py b/src/slam2d_pkg/slam2d_pkg/test.py new file mode 100755 index 0000000..0584936 --- /dev/null +++ b/src/slam2d_pkg/slam2d_pkg/test.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from slam2d_pkg.') + + +if __name__ == '__main__': + main() diff --git a/src/slam2d_pkg/test/test_copyright.py b/src/slam2d_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/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' diff --git a/src/slam2d_pkg/test/test_flake8.py b/src/slam2d_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/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) diff --git a/src/slam2d_pkg/test/test_pep257.py b/src/slam2d_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/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' diff --git a/src/slam3d_pkg/config/rtabmap_fr09-real.yaml b/src/slam3d_pkg/config/rtabmap_fr09-real.yaml new file mode 100755 index 0000000..6f07a26 --- /dev/null +++ b/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" \ No newline at end of file diff --git a/src/slam3d_pkg/config/rtabmap_fr09-sim.yaml b/src/slam3d_pkg/config/rtabmap_fr09-sim.yaml new file mode 100755 index 0000000..3356894 --- /dev/null +++ b/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" \ No newline at end of file diff --git a/src/slam3d_pkg/config/rtabmap_fr09.yaml b/src/slam3d_pkg/config/rtabmap_fr09.yaml new file mode 100755 index 0000000..5a5db29 --- /dev/null +++ b/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" \ No newline at end of file diff --git a/src/slam3d_pkg/config/rtabmap_mkmini-real.yaml b/src/slam3d_pkg/config/rtabmap_mkmini-real.yaml new file mode 100755 index 0000000..910b31b --- /dev/null +++ b/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" \ No newline at end of file diff --git a/src/slam3d_pkg/config/rtabmap_mkmini-sim.yaml b/src/slam3d_pkg/config/rtabmap_mkmini-sim.yaml new file mode 100755 index 0000000..910b31b --- /dev/null +++ b/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" \ No newline at end of file diff --git a/src/slam3d_pkg/config/rtabmap_mkmini.yaml b/src/slam3d_pkg/config/rtabmap_mkmini.yaml new file mode 100755 index 0000000..910b31b --- /dev/null +++ b/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" \ No newline at end of file diff --git a/src/slam3d_pkg/launch/slam.launch.py b/src/slam3d_pkg/launch/slam.launch.py new file mode 100755 index 0000000..22d870d --- /dev/null +++ b/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 + diff --git a/src/slam3d_pkg/package.xml b/src/slam3d_pkg/package.xml new file mode 100755 index 0000000..52f6de1 --- /dev/null +++ b/src/slam3d_pkg/package.xml @@ -0,0 +1,18 @@ + + + + slam3d_pkg + 0.0.0 + TODO: Package description + dgist + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/slam3d_pkg/resource/slam3d_pkg b/src/slam3d_pkg/resource/slam3d_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/slam3d_pkg/setup.cfg b/src/slam3d_pkg/setup.cfg new file mode 100755 index 0000000..58d4600 --- /dev/null +++ b/src/slam3d_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/slam3d_pkg +[install] +install_scripts=$base/lib/slam3d_pkg diff --git a/src/slam3d_pkg/setup.py b/src/slam3d_pkg/setup.py new file mode 100755 index 0000000..43097eb --- /dev/null +++ b/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' + ], + }, +) diff --git a/src/slam3d_pkg/slam3d_pkg/__init__.py b/src/slam3d_pkg/slam3d_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/slam3d_pkg/slam3d_pkg/teleop_key.py b/src/slam3d_pkg/slam3d_pkg/teleop_key.py new file mode 100755 index 0000000..92d86b7 --- /dev/null +++ b/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() diff --git a/src/slam3d_pkg/slam3d_pkg/test.py b/src/slam3d_pkg/slam3d_pkg/test.py new file mode 100755 index 0000000..218f606 --- /dev/null +++ b/src/slam3d_pkg/slam3d_pkg/test.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from slam3d_pkg.') + + +if __name__ == '__main__': + main() diff --git a/src/slam3d_pkg/test/test_copyright.py b/src/slam3d_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/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' diff --git a/src/slam3d_pkg/test/test_flake8.py b/src/slam3d_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/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) diff --git a/src/slam3d_pkg/test/test_pep257.py b/src/slam3d_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/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' diff --git a/src/slam_pkg/launch/slam.launch.py b/src/slam_pkg/launch/slam.launch.py new file mode 100755 index 0000000..11e876c --- /dev/null +++ b/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 diff --git a/src/slam_pkg/package.xml b/src/slam_pkg/package.xml new file mode 100755 index 0000000..d0c2609 --- /dev/null +++ b/src/slam_pkg/package.xml @@ -0,0 +1,18 @@ + + + + slam_pkg + 0.0.0 + TODO: Package description + dgist + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/slam_pkg/resource/slam_pkg b/src/slam_pkg/resource/slam_pkg new file mode 100755 index 0000000..e69de29 diff --git a/src/slam_pkg/setup.cfg b/src/slam_pkg/setup.cfg new file mode 100755 index 0000000..67de7f1 --- /dev/null +++ b/src/slam_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/slam_pkg +[install] +install_scripts=$base/lib/slam_pkg diff --git a/src/slam_pkg/setup.py b/src/slam_pkg/setup.py new file mode 100755 index 0000000..8e35d36 --- /dev/null +++ b/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', + ], + }, +) diff --git a/src/slam_pkg/slam_pkg/__init__.py b/src/slam_pkg/slam_pkg/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/slam_pkg/slam_pkg/slam_manager.py b/src/slam_pkg/slam_pkg/slam_manager.py new file mode 100755 index 0000000..8fbd057 --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/src/slam_pkg/slam_pkg/teleop_key.py b/src/slam_pkg/slam_pkg/teleop_key.py new file mode 100755 index 0000000..92d86b7 --- /dev/null +++ b/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() diff --git a/src/slam_pkg/slam_pkg/test.py b/src/slam_pkg/slam_pkg/test.py new file mode 100755 index 0000000..c404275 --- /dev/null +++ b/src/slam_pkg/slam_pkg/test.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from slam_pkg.') + + +if __name__ == '__main__': + main() diff --git a/src/slam_pkg/test/test_copyright.py b/src/slam_pkg/test/test_copyright.py new file mode 100755 index 0000000..97a3919 --- /dev/null +++ b/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' diff --git a/src/slam_pkg/test/test_flake8.py b/src/slam_pkg/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/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) diff --git a/src/slam_pkg/test/test_pep257.py b/src/slam_pkg/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/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'