From 0e831766e17cba04834848a5c8585f672b9e49e5 Mon Sep 17 00:00:00 2001 From: chyoo Date: Wed, 29 Jul 2026 15:10:29 +0900 Subject: [PATCH] feat: add NAV2 slam3D module --- .gitignore | 5 + build.sh | 6 + run.sh | 13 ++ src/slam3d_pkg/config/rtabmap_fr09-real.yaml | 107 +++++++++++ src/slam3d_pkg/config/rtabmap_fr09-sim.yaml | 102 ++++++++++ src/slam3d_pkg/config/rtabmap_fr09.yaml | 102 ++++++++++ .../config/rtabmap_mkmini-real.yaml | 96 ++++++++++ src/slam3d_pkg/config/rtabmap_mkmini-sim.yaml | 96 ++++++++++ src/slam3d_pkg/config/rtabmap_mkmini.yaml | 96 ++++++++++ src/slam3d_pkg/launch/slam.launch.py | 74 ++++++++ src/slam3d_pkg/package.xml | 18 ++ src/slam3d_pkg/resource/slam3d_pkg | 0 src/slam3d_pkg/setup.cfg | 4 + src/slam3d_pkg/setup.py | 36 ++++ src/slam3d_pkg/slam3d_pkg/__init__.py | 0 src/slam3d_pkg/slam3d_pkg/teleop_key.py | 174 ++++++++++++++++++ src/slam3d_pkg/slam3d_pkg/test.py | 6 + src/slam3d_pkg/test/test_copyright.py | 25 +++ src/slam3d_pkg/test/test_flake8.py | 25 +++ src/slam3d_pkg/test/test_pep257.py | 23 +++ 20 files changed, 1008 insertions(+) create mode 100644 .gitignore create mode 100755 build.sh create mode 100755 run.sh create mode 100755 src/slam3d_pkg/config/rtabmap_fr09-real.yaml create mode 100755 src/slam3d_pkg/config/rtabmap_fr09-sim.yaml create mode 100755 src/slam3d_pkg/config/rtabmap_fr09.yaml create mode 100755 src/slam3d_pkg/config/rtabmap_mkmini-real.yaml create mode 100755 src/slam3d_pkg/config/rtabmap_mkmini-sim.yaml create mode 100755 src/slam3d_pkg/config/rtabmap_mkmini.yaml create mode 100755 src/slam3d_pkg/launch/slam.launch.py create mode 100755 src/slam3d_pkg/package.xml create mode 100755 src/slam3d_pkg/resource/slam3d_pkg create mode 100755 src/slam3d_pkg/setup.cfg create mode 100755 src/slam3d_pkg/setup.py create mode 100755 src/slam3d_pkg/slam3d_pkg/__init__.py create mode 100755 src/slam3d_pkg/slam3d_pkg/teleop_key.py create mode 100755 src/slam3d_pkg/slam3d_pkg/test.py create mode 100755 src/slam3d_pkg/test/test_copyright.py create mode 100755 src/slam3d_pkg/test/test_flake8.py create mode 100755 src/slam3d_pkg/test/test_pep257.py 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..bfbfd8b --- /dev/null +++ b/run.sh @@ -0,0 +1,13 @@ +#!/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_run_nav2.sh" \ + "lms-nav2-slam3d" \ + "$(dirname "$MODULE_PATH")" \ + "$(basename "$MODULE_PATH")" \ + "${NAV2_DOCKER_IMAGE:-future-nav2:humble}" \ + "${ROS_DOMAIN_ID:-0}" \ + sim_mode:=false robot:=mkmini 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'