feat: add lms-nav2-gazebo-localization module
This commit is contained in:
@@ -0,0 +1,5 @@
|
|||||||
|
build/
|
||||||
|
install/
|
||||||
|
log/
|
||||||
|
.colcon/
|
||||||
|
*.log
|
||||||
@@ -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}"
|
||||||
@@ -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}" "$@"
|
||||||
+45
@@ -0,0 +1,45 @@
|
|||||||
|
# File: localization.yaml
|
||||||
|
# Description: Localization module parameters for FR-09
|
||||||
|
|
||||||
|
# AMCL
|
||||||
|
amcl:
|
||||||
|
ros__parameters:
|
||||||
|
use_sim_time: True # 실차 전환 시 False로 수정
|
||||||
|
alpha1: 0.05
|
||||||
|
alpha2: 0.05
|
||||||
|
alpha3: 0.05
|
||||||
|
alpha4: 0.05
|
||||||
|
alpha5: 0.05
|
||||||
|
base_frame_id: "base_link"
|
||||||
|
beam_skip_distance: 0.5
|
||||||
|
beam_skip_error_threshold: 0.9
|
||||||
|
beam_skip_threshold: 0.3
|
||||||
|
do_beamskip: false
|
||||||
|
global_frame_id: "map"
|
||||||
|
lambda_short: 0.1
|
||||||
|
laser_likelihood_max_dist: 2.0
|
||||||
|
laser_max_range: 100.0
|
||||||
|
laser_min_range: -1.0
|
||||||
|
laser_model_type: "likelihood_field"
|
||||||
|
max_beams: 60
|
||||||
|
max_particles: 5000
|
||||||
|
min_particles: 1000
|
||||||
|
odom_frame_id: "odom"
|
||||||
|
pf_err: 0.05
|
||||||
|
pf_z: 0.99
|
||||||
|
recovery_alpha_fast: 0.0
|
||||||
|
recovery_alpha_slow: 0.0
|
||||||
|
resample_interval: 1
|
||||||
|
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||||
|
save_pose_rate: 0.5
|
||||||
|
sigma_hit: 0.2
|
||||||
|
tf_broadcast: true
|
||||||
|
transform_tolerance: 1.0
|
||||||
|
update_min_a: 0.1
|
||||||
|
update_min_d: 0.1
|
||||||
|
z_hit: 0.7
|
||||||
|
z_max: 0.05
|
||||||
|
z_rand: 0.2
|
||||||
|
z_short: 0.05
|
||||||
|
scan_topic: /scan
|
||||||
|
set_initial_pose: False # RViz 2D Pose Estimate 으로 사용자가 직접 init
|
||||||
+97
@@ -0,0 +1,97 @@
|
|||||||
|
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.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: "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"
|
||||||
+97
@@ -0,0 +1,97 @@
|
|||||||
|
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: "false"
|
||||||
|
Grid/MaxGroundHeight: "-0.8"
|
||||||
|
Grid/MinGroundHeight: "-1.3"
|
||||||
|
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: "25.0"
|
||||||
|
Mem/LaserScanVoxelSize: "0.01"
|
||||||
|
Odom/FilteringStrategy: "1"
|
||||||
|
Odom/ScanKeyFrameThr: "0.9"
|
||||||
|
Odom/GuessMotion: "false"
|
||||||
|
|
||||||
|
Grid/CellSize: "0.2"
|
||||||
|
Grid/MinClusterSize: "10"
|
||||||
|
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"
|
||||||
+96
@@ -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"
|
||||||
+15
@@ -0,0 +1,15 @@
|
|||||||
|
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
node = Node(
|
||||||
|
package='localization_pkg',
|
||||||
|
executable='localization_manager',
|
||||||
|
name='localization_manager',
|
||||||
|
output='screen',
|
||||||
|
)
|
||||||
|
|
||||||
|
ld = LaunchDescription([])
|
||||||
|
ld.add_action(node)
|
||||||
|
return ld
|
||||||
+65
@@ -0,0 +1,65 @@
|
|||||||
|
# File: localization.launch.py
|
||||||
|
# Description:
|
||||||
|
# 위치 추정 스택 — map_server + AMCL
|
||||||
|
# 사전에 저장된 지도(/map)와 실시간 스캔(/scan)을 비교해 지도 위 차량 위치 추정
|
||||||
|
# Subscribes : /scan, /odom_custom
|
||||||
|
# Publishes : TF(map→odom), /amcl_pose, /map
|
||||||
|
# Requires : perception.launch.py가 먼저 실행되어 /scan, TF(odom→base_link) 발행 중이어야 함
|
||||||
|
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, GroupAction
|
||||||
|
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('use_sim_time', default_value=LaunchConfiguration('sim_mode')),
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||||
|
|
||||||
|
lifecycle_nodes = ['amcl']
|
||||||
|
|
||||||
|
configured_params = ParameterFile(
|
||||||
|
RewrittenYaml(
|
||||||
|
source_file=PathJoinSubstitution([
|
||||||
|
FindPackageShare('localization_pkg'), 'config', 'localization.yaml'
|
||||||
|
]),
|
||||||
|
param_rewrites={'use_sim_time': use_sim_time},
|
||||||
|
convert_types=True,
|
||||||
|
),
|
||||||
|
allow_substs=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
load_nodes = GroupAction(
|
||||||
|
actions=[
|
||||||
|
Node(
|
||||||
|
package='nav2_amcl',
|
||||||
|
executable='amcl',
|
||||||
|
name='amcl',
|
||||||
|
output='screen',
|
||||||
|
parameters=[configured_params],
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package='nav2_lifecycle_manager',
|
||||||
|
executable='lifecycle_manager',
|
||||||
|
name='lifecycle_manager_localization',
|
||||||
|
output='screen',
|
||||||
|
parameters=[{
|
||||||
|
'use_sim_time': use_sim_time,
|
||||||
|
'autostart': True,
|
||||||
|
'node_names': lifecycle_nodes,
|
||||||
|
}],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
ld = LaunchDescription(ARGUMENTS)
|
||||||
|
ld.add_action(load_nodes)
|
||||||
|
return ld
|
||||||
+64
@@ -0,0 +1,64 @@
|
|||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.actions import GroupAction
|
||||||
|
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('robot', default_value='fr09', choices=['mkmini', 'fr09']),
|
||||||
|
]
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
robot = LaunchConfiguration('robot')
|
||||||
|
|
||||||
|
remappings=[
|
||||||
|
('/tf', 'tf'),
|
||||||
|
('/tf_static', 'tf_static'),
|
||||||
|
('odom', 'icp_odom'),
|
||||||
|
('scan_cloud', '/lidar_filtered')]
|
||||||
|
|
||||||
|
# Directories
|
||||||
|
pkg_localization = get_package_share_directory(
|
||||||
|
'localization_pkg')
|
||||||
|
|
||||||
|
rtabmap_params_file = PathJoinSubstitution([
|
||||||
|
pkg_localization,
|
||||||
|
'config',
|
||||||
|
PythonExpression(["'rtabmap_' + '", robot, "' + '.yaml'"])])
|
||||||
|
param_substitutions = {
|
||||||
|
'use_sim_time': 'true'
|
||||||
|
}
|
||||||
|
configured_params = ParameterFile(
|
||||||
|
RewrittenYaml(
|
||||||
|
source_file=rtabmap_params_file,
|
||||||
|
param_rewrites=param_substitutions,
|
||||||
|
convert_types=True),
|
||||||
|
allow_substs=True)
|
||||||
|
|
||||||
|
load_nodes = GroupAction(
|
||||||
|
actions=[
|
||||||
|
Node(
|
||||||
|
package='rtabmap_odom',
|
||||||
|
executable='icp_odometry',
|
||||||
|
name='icp_odometry',
|
||||||
|
output='screen',
|
||||||
|
parameters=[configured_params, {'use_sim_time': True}],
|
||||||
|
remappings=remappings,
|
||||||
|
)
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# Create launch description and add actions
|
||||||
|
ld = LaunchDescription(ARGUMENTS)
|
||||||
|
ld.add_action(load_nodes)
|
||||||
|
|
||||||
|
return ld
|
||||||
|
|
||||||
Binary file not shown.
BIN
Binary file not shown.
@@ -0,0 +1,63 @@
|
|||||||
|
|
||||||
|
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 LocalizationManager(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('localization_manager')
|
||||||
|
self.get_logger().info('Localization Manager node has been started.')
|
||||||
|
|
||||||
|
qos = QoSProfile(
|
||||||
|
depth=1,
|
||||||
|
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||||
|
reliability=ReliabilityPolicy.RELIABLE
|
||||||
|
)
|
||||||
|
self.localization_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"Localization Received config: {msg.data}")
|
||||||
|
config_data = json.loads(msg.data)
|
||||||
|
|
||||||
|
if config_data.get("level") == "2d":
|
||||||
|
self.localization_process = subprocess.Popen(['ros2', 'launch',
|
||||||
|
'localization_pkg',
|
||||||
|
'localization2d.launch.py',
|
||||||
|
f'sim_mode:={config_data.get("sim_mode")}',
|
||||||
|
f'robot:={config_data.get("robot")}'])
|
||||||
|
elif config_data.get("level") == "3d":
|
||||||
|
self.localization_process = subprocess.Popen(['ros2', 'launch',
|
||||||
|
'localization_pkg',
|
||||||
|
'localization3d.launch.py',
|
||||||
|
f'sim_mode:={config_data.get("sim_mode")}',
|
||||||
|
f'robot:={config_data.get("robot")}'])
|
||||||
|
pass
|
||||||
|
|
||||||
|
def destroy_node(self):
|
||||||
|
if self.localization_process:
|
||||||
|
self.localization_process.terminate()
|
||||||
|
self.localization_process.wait()
|
||||||
|
|
||||||
|
return super().destroy_node()
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = LocalizationManager()
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
|
|
||||||
+6
@@ -0,0 +1,6 @@
|
|||||||
|
def main():
|
||||||
|
print('Hi from localization_pkg.')
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
Executable
+18
@@ -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>localization_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>
|
||||||
Executable
+4
@@ -0,0 +1,4 @@
|
|||||||
|
[develop]
|
||||||
|
script_dir=$base/lib/localization_pkg
|
||||||
|
[install]
|
||||||
|
install_scripts=$base/lib/localization_pkg
|
||||||
Executable
+36
@@ -0,0 +1,36 @@
|
|||||||
|
|
||||||
|
import os
|
||||||
|
from glob import glob
|
||||||
|
from setuptools import find_packages, setup
|
||||||
|
|
||||||
|
package_name = 'localization_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 = localization_pkg.test:main',
|
||||||
|
'localization_manager = localization_pkg.localization_manager:main',
|
||||||
|
],
|
||||||
|
},
|
||||||
|
)
|
||||||
Executable
+25
@@ -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'
|
||||||
Executable
+25
@@ -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)
|
||||||
Executable
+23
@@ -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'
|
||||||
Reference in New Issue
Block a user