diff --git a/src/my_robot_bringup/launch/master_launch.py b/src/my_robot_bringup/launch/master_launch.py new file mode 100755 index 0000000..ee5e81c --- /dev/null +++ b/src/my_robot_bringup/launch/master_launch.py @@ -0,0 +1,352 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# ============================================================================ +# master_launch.py —— 机器人系统总启动文件 +# 启动顺序: origincar_base → LiDAR → TTS服务 → USB相机+二维码 → 障碍物检测 → 路径规划 → 轨迹跟随 → VLM +# 用法: +# ros2 launch my_robot_bringup master_launch.py +# ros2 launch my_robot_bringup master_launch.py use_qr:=false +# ros2 launch my_robot_bringup master_launch.py use_vlm:=false +# ros2 launch my_robot_bringup master_launch.py use_planner:=false +# ============================================================================ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + TimerAction, + LogInfo, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # ================================================================ + # 路径 + # ================================================================ + origincar_dir = get_package_share_directory('origincar_base') + lslidar_dir = get_package_share_directory('lslidar_driver') + obstacle_dir = get_package_share_directory('obstacle_scanner') + planner_dir = get_package_share_directory('planner') + vlm_dir = get_package_share_directory('vlm_detect') + + # ================================================================ + # Launch 参数 —— 可通过命令行灵活切换 + # ================================================================ + + # 模块开关 + use_base = LaunchConfiguration('use_base', default='true') + use_lidar = LaunchConfiguration('use_lidar', default='true') + use_qr = LaunchConfiguration('use_qr', default='true') + use_tts = LaunchConfiguration('use_tts', default='true') + use_obstacle = LaunchConfiguration('use_obstacle', default='true') + use_planner = LaunchConfiguration('use_planner', default='true') + use_vlm = LaunchConfiguration('use_vlm', default='true') + + # 车辆模式 + akmcar = LaunchConfiguration('akmcar', default='true') + carto_slam = LaunchConfiguration('carto_slam', default='false') + + # 摄像头设备 + camera_device = LaunchConfiguration('camera_device', default='/dev/video0') + + # TTS 参数 + audio_sink = LaunchConfiguration('audio_sink', + default='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo') + tts_speed = LaunchConfiguration('tts_speed', default='1.5') + + # 地图文件 (AMCL 定位用) + map_file = LaunchConfiguration('map_file', default=os.path.join( + planner_dir, 'maps', 'real_5x5.yaml')) + + # Planner / Tracker 参数文件 + planner_params_file = LaunchConfiguration('planner_params_file', default=os.path.join( + planner_dir, 'config', 'real_hybrid_astar.yaml')) + tracker_params_file = LaunchConfiguration('tracker_params_file', default=os.path.join( + planner_dir, 'config', 'real_pure_pursuit.yaml')) + amcl_params_file = LaunchConfiguration('amcl_params_file', default=os.path.join( + planner_dir, 'config', 'real_amcl.yaml')) + + # VLM 服务器地址 + vlm_host = LaunchConfiguration('vlm_host', default='http://192.168.10.189:8000') + + # 通用 + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + # ================================================================ + # 阶段 1: 底盘驱动 + EKF + TF + IMU (t=0s) + # ================================================================ + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + origincar_dir, '/launch', '/origincar_bringup.launch.py' + ]), + condition=IfCondition(use_base), + launch_arguments={ + 'akmcar': akmcar, + 'carto_slam': carto_slam, + }.items(), + ) + + # ================================================================ + # 阶段 2: 激光雷达 (t=2s) + # ================================================================ + lidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + lslidar_dir, '/launch', '/lsn10_launch.py' + ]), + condition=IfCondition(use_lidar), + ) + + # ================================================================ + # 阶段 3: TTS 语音播报服务 + 二维码播报桥接 (t=3s) + # tts_server: Piper 离线 TTS + espeak 降级，提供 /tts/speak 服务 + # qr_tts_bridge: 订阅 qr_results，调用 /tts/speak + # ================================================================ + tts_server = Node( + package='vlm_detect', + executable='tts_server', + name='tts_server', + output='screen', + condition=IfCondition(use_tts), + parameters=[{ + 'audio_sink': audio_sink, + 'tts_speed': tts_speed, + }], + ) + + qr_tts_bridge = Node( + package='vlm_detect', + executable='qr_tts_bridge', + name='qr_tts_bridge', + output='screen', + condition=IfCondition(use_tts), + ) + + # ================================================================ + # 阶段 4: USB 摄像头 + 二维码识别 (t=4s) + # usb_cam: 驱动 USB 摄像头，发布 /image_raw + # qr_detect: 订阅 /image_raw，识别二维码，发布 qr_results + # ================================================================ + usb_camera = Node( + package='usb_cam', + executable='usb_cam_node_exe', + name='usb_cam', + output='screen', + condition=IfCondition(use_qr), + parameters=[{ + 'video_device': camera_device, + 'image_size': [640, 480], + 'pixel_format': 'YUYV', + 'framerate': 30.0, + }], + ) + + qr_detect = Node( + package='qr_detection', + executable='qr_detect', + name='qr_detect', + output='screen', + condition=IfCondition(use_qr), + parameters=[{'image_topic': '/image_raw'}], + ) + + # ================================================================ + # 阶段 5: 障碍物检测 (t=5s) + # ================================================================ + obstacle_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + obstacle_dir, '/launch', '/obstacle_scanner.launch.py' + ]), + condition=IfCondition(use_obstacle), + ) + + # ================================================================ + # 阶段 6: 地图服务器 + AMCL 定位 (t=7s) + # ================================================================ + map_server_node = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + condition=IfCondition(use_planner), + parameters=[{'yaml_filename': map_file, + 'use_sim_time': use_sim_time}], + ) + + amcl_node = Node( + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + condition=IfCondition(use_planner), + parameters=[amcl_params_file, + {'use_sim_time': use_sim_time}], + ) + + lifecycle_manager = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + condition=IfCondition(use_planner), + parameters=[{'use_sim_time': use_sim_time, + 'autostart': True, + 'node_names': ['map_server', 'amcl']}], + ) + + # ================================================================ + # 阶段 7: Hybrid A* 路径规划 + Pure Pursuit 轨迹跟随 (t=9s) + # ================================================================ + planner_node = Node( + package='planner', + executable='grid_astar_theta_node', + name='grid_astar_theta_planner', + output='screen', + condition=IfCondition(use_planner), + parameters=[planner_params_file, + {'use_sim_time': use_sim_time, + 'obstacles_topic': '/obstacles'}], + ) + + tracker_node = Node( + package='planner', + executable='topology_pure_pursuit_node.py', + name='topology_pure_pursuit', + output='screen', + condition=IfCondition(use_planner), + parameters=[tracker_params_file, + {'use_sim_time': use_sim_time}], + ) + + # ================================================================ + # 阶段 8: VLM 图生文 (t=11s) + # vlm_node 内部自带 TTS 服务客户端，识别完自动调用 /tts/speak + # 注意: TTS 服务和桥接已在阶段 3 启动，这里只启动 vlm_node + # ================================================================ + vlm_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + vlm_dir, '/launch', '/vlm_detect.launch.py' + ]), + condition=IfCondition(use_vlm), + launch_arguments={ + 'vlm_host': vlm_host, + 'use_tts': 'false', # tts_server 已在阶段 3 启动 + 'use_qr_tts': 'false', # qr_tts_bridge 已在阶段 3 启动 + }.items(), + ) + + # ================================================================ + # 组装 —— 通过 TimerAction 保证先后顺序 + # ================================================================ + return LaunchDescription([ + + # 模块 开关 参数 + DeclareLaunchArgument('use_base', default_value='true', + description='底盘驱动 + EKF + TF'), + DeclareLaunchArgument('use_lidar', default_value='true', + description='激光雷达 (lsn10)'), + DeclareLaunchArgument('use_qr', default_value='true', + description='USB 摄像头 + 二维码识别'), + DeclareLaunchArgument('use_tts', default_value='true', + description='TTS 语音播报服务 + 二维码播报桥接'), + DeclareLaunchArgument('use_obstacle', default_value='true', + description='障碍物检测'), + DeclareLaunchArgument('use_planner', default_value='true', + description='路径规划 + 轨迹跟随'), + DeclareLaunchArgument('use_vlm', default_value='true', + description='VLM 图生文'), + DeclareLaunchArgument('akmcar', default_value='true', + description='车辆模型 (true=阿克曼, false=麦轮)'), + DeclareLaunchArgument('carto_slam', default_value='false', + description='使用 Cartographer 替代 EKF'), + DeclareLaunchArgument('camera_device', default_value='/dev/video0', + description='USB 摄像头设备路径'), + DeclareLaunchArgument('audio_sink', + default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo', + description='音频输出设备 (PulseAudio sink)'), + DeclareLaunchArgument('tts_speed', default_value='1.5', + description='TTS 语速倍率 (0.5~2.0)'), + DeclareLaunchArgument('map_file', default_value=os.path.join( + planner_dir, 'maps', 'real_5x5.yaml'), + description='预建地图 YAML 文件'), + DeclareLaunchArgument('planner_params_file', default_value=os.path.join( + planner_dir, 'config', 'real_hybrid_astar.yaml'), + description='Hybrid A* 规划器参数文件'), + DeclareLaunchArgument('tracker_params_file', default_value=os.path.join( + planner_dir, 'config', 'real_pure_pursuit.yaml'), + description='Pure Pursuit 跟踪器参数文件'), + DeclareLaunchArgument('amcl_params_file', default_value=os.path.join( + planner_dir, 'config', 'real_amcl.yaml'), + description='AMCL 定位参数文件'), + DeclareLaunchArgument('vlm_host', default_value='http://192.168.10.189:8000', + description='VLM 服务器地址'), + DeclareLaunchArgument('use_sim_time', default_value='false', + description='使用仿真时间'), + + LogInfo(msg='========================================'), + LogInfo(msg=' Robot System Bringup'), + LogInfo(msg='========================================'), + + # t=0s: 底盘 + EKF + TimerAction(period=0.0, actions=[ + LogInfo(msg='[1/8] Starting base driver + EKF + TF + IMU ...'), + origincar_bringup, + ]), + + # t=2s: 激光雷达 + TimerAction(period=2.0, actions=[ + LogInfo(msg='[2/8] Starting LiDAR (lsn10) ...'), + lidar_launch, + ]), + + # t=3s: TTS 服务 + 二维码播报桥接 + TimerAction(period=3.0, actions=[ + LogInfo(msg='[3/8] Starting TTS Server + QR-TTS Bridge ...'), + tts_server, + qr_tts_bridge, + ]), + + # t=4s: USB 摄像头 + 二维码识别 + TimerAction(period=4.0, actions=[ + LogInfo(msg='[4/8] Starting USB Camera + QR Detection ...'), + usb_camera, + qr_detect, + ]), + + # t=5s: 障碍物检测 + TimerAction(period=5.0, actions=[ + LogInfo(msg='[5/8] Starting Obstacle Scanner ...'), + obstacle_launch, + ]), + + # t=7s: 地图 + AMCL 定位 + TimerAction(period=7.0, actions=[ + LogInfo(msg='[6/8] Starting Map Server + AMCL localization ...'), + map_server_node, + amcl_node, + lifecycle_manager, + ]), + + # t=9s: 路径规划 + 轨迹跟随 + TimerAction(period=9.0, actions=[ + LogInfo(msg='[7/8] Starting Hybrid A* Planner + Pure Pursuit Tracker ...'), + planner_node, + tracker_node, + ]), + + # t=11s: VLM 图生文 + TimerAction(period=11.0, actions=[ + LogInfo(msg='[8/8] Starting VLM ...'), + vlm_launch, + ]), + + LogInfo(msg='========================================'), + LogInfo(msg=' All modules started!'), + LogInfo(msg='========================================'), + ]) diff --git a/src/my_robot_bringup/launch/master_launch.py.bak b/src/my_robot_bringup/launch/master_launch.py.bak new file mode 100755 index 0000000..82f7aab --- /dev/null +++ b/src/my_robot_bringup/launch/master_launch.py.bak @@ -0,0 +1,218 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# ============================================================================ +# master_launch.py — 机器人系统总启动文件 +# 启动顺序: origincar_base → LiDAR → EKF → SLAM → Nav2 → VLM_Detect +# 用法: +# ros2 launch my_robot_bringup master_launch.py +# ros2 launch my_robot_bringup master_launch.py use_vlm:=false +# ros2 launch my_robot_bringup master_launch.py vlm_host:=http://X.X.X.X:8000 +# ============================================================================ +## 障碍物检测 路径规划 轨迹跟随 重定位 +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + TimerAction, + LogInfo, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # ================================================================ + # 包路径 + # ================================================================ + origincar_dir = get_package_share_directory('origincar_base') + lslidar_dir = get_package_share_directory('lslidar_driver') + cyy_slam_dir = get_package_share_directory('cyy_slamtoolbox') + gc_nav_dir = get_package_share_directory('gc_navigation2_slamtoolbox') + vlm_dir = get_package_share_directory('vlm_detect') + + # ================================================================ + # Launch 参数 — 可通过命令行覆盖 + # ================================================================ + + # 模块开关 + use_base = LaunchConfiguration('use_base', default='true') + use_lidar = LaunchConfiguration('use_lidar', default='true') + use_slam = LaunchConfiguration('use_slam', default='true') + use_nav2 = LaunchConfiguration('use_nav2', default='true') + use_vlm = LaunchConfiguration('use_vlm', default='true') + use_tts = LaunchConfiguration('use_tts', default='true') + + # 底盘模式 + akmcar = LaunchConfiguration('akmcar', default='true') + carto_slam = LaunchConfiguration('carto_slam', default='false') + + # SLAM / Nav2 配置文件 + slam_params_file = LaunchConfiguration('slam_params_file', default=os.path.join( + cyy_slam_dir, 'config', 'mapper_params_online_async.yaml')) + nav2_params_file = LaunchConfiguration('nav2_params_file', default=os.path.join( + gc_nav_dir, 'params', 'gc_navigation_slam.yaml')) + + # VLM 推理服务地址 (Windows WSL) + vlm_host = LaunchConfiguration('vlm_host', default='http://192.168.10.189:8000') + + # 通用 + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + # ================================================================ + # 阶段 1: 底盘驱动 + EKF + TF + IMU (t=0s) + # + # origincar_bringup 内部已包含: + # - base_serial.launch.py 底盘驱动 (origincar_base_node) + # - robot_mode_description robot_state_publisher (TF from URDF) + # - imu_filter_madgwick IMU 姿态滤波 + # - robot_localization ekf_node EKF 融合 (odom + IMU → odom_combined) + # - joint_state_publisher 关节状态发布 + # - static TF: base_footprint→gyro_link, base_link→laser + # ================================================================ + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + origincar_dir, '/launch', '/origincar_bringup.launch.py' + ]), + condition=IfCondition(use_base), + launch_arguments={ + 'akmcar': akmcar, + 'carto_slam': carto_slam, + }.items(), + ) + + # ================================================================ + # 阶段 2: 激光雷达 (t=2s) + # lsn10_launch.py 启动 LS-N10 型号激光雷达驱动 + # 如有其他型号，改包名和 launch 文件名即可 + # ================================================================ + lidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + lslidar_dir, '/launch', '/lsn10_launch.py' + ]), + condition=IfCondition(use_lidar), + ) + + # ================================================================ + # 阶段 3: SLAM Toolbox — 实时建图 (t=5s) + # 需要: /scan (LiDAR) + odom→base_link TF (EKF) + # 输出: map→odom TF + /map topic + # ================================================================ + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen', + condition=IfCondition(use_slam), + parameters=[slam_params_file, + {'use_sim_time': use_sim_time}], + ) + + # ================================================================ + # 阶段 4: Nav2 导航栈 (t=8s) + # 需要: /map (SLAM) + /scan + 完整 TF 树 (map→odom→base_link→laser) + # 输出: /cmd_vel → origincar_base → 底盘运动 + # ================================================================ + nav2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare('nav2_bringup'), '/launch', '/navigation_launch.py' + ]), + condition=IfCondition(use_nav2), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': nav2_params_file, + }.items(), + ) + + # ================================================================ + # 阶段 5: VLM 图生文 + TTS 语音播报 (t=10s) + # 完全独立于导航栈，只依赖 WSL 上 vlm_server.py 已启动 + # ================================================================ + vlm_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + vlm_dir, '/launch', '/vlm_detect.launch.py' + ]), + condition=IfCondition(use_vlm), + launch_arguments={ + 'vlm_host': vlm_host, + 'use_tts': use_tts, + }.items(), + ) + + # ================================================================ + # 组装 — 按时间线顺序，TimerAction 保证先后 + # ================================================================ + return LaunchDescription([ + + # —— 参数声明 —— + DeclareLaunchArgument('use_base', default_value='true', + description='底盘驱动 + EKF + TF'), + DeclareLaunchArgument('use_lidar', default_value='true', + description='激光雷达 (lsn10)'), + DeclareLaunchArgument('use_slam', default_value='true', + description='slam_toolbox 建图'), + DeclareLaunchArgument('use_nav2', default_value='true', + description='Nav2 导航栈'), + DeclareLaunchArgument('use_vlm', default_value='true', + description='VLM 图生文 + TTS'), + DeclareLaunchArgument('use_tts', default_value='true', + description='TTS 语音播报开关'), + DeclareLaunchArgument('akmcar', default_value='true', + description='阿克曼底盘 (true=阿克曼, false=差速)'), + DeclareLaunchArgument('carto_slam', default_value='false', + description='使用 Cartographer 替代 EKF'), + DeclareLaunchArgument('slam_params_file', default_value=os.path.join( + cyy_slam_dir, 'config', 'mapper_params_online_async.yaml'), + description='SLAM 参数文件'), + DeclareLaunchArgument('nav2_params_file', default_value=os.path.join( + gc_nav_dir, 'params', 'gc_navigation_slam.yaml'), + description='Nav2 参数文件'), + DeclareLaunchArgument('vlm_host', default_value='http://192.168.10.189:8000', + description='VLM 推理服务地址'), + DeclareLaunchArgument('use_sim_time', default_value='false', + description='使用仿真时间'), + + # —— 时间线日志 —— + LogInfo(msg='========================================'), + LogInfo(msg=' Robot System Bringup'), + LogInfo(msg='========================================'), + + # t=0s: 底盘 + EKF + TimerAction(period=0.0, actions=[ + LogInfo(msg='[1/5] Starting base driver + EKF + TF + IMU ...'), + origincar_bringup, + ]), + + # t=2s: 激光雷达 + TimerAction(period=2.0, actions=[ + LogInfo(msg='[2/5] Starting LiDAR (lsn10) ...'), + lidar_launch, + ]), + + # t=5s: SLAM + TimerAction(period=5.0, actions=[ + LogInfo(msg='[3/5] Starting slam_toolbox ...'), + slam_toolbox_node, + ]), + + # t=8s: Nav2 + TimerAction(period=8.0, actions=[ + LogInfo(msg='[4/5] Starting Nav2 navigation ...'), + nav2_launch, + ]), + + # t=10s: VLM + TimerAction(period=10.0, actions=[ + LogInfo(msg='[5/5] Starting VLM + TTS ...'), + vlm_launch, + ]), + + LogInfo(msg='========================================'), + LogInfo(msg=' All modules started!'), + LogInfo(msg='========================================'), + ]) diff --git a/src/my_robot_bringup/package.xml b/src/my_robot_bringup/package.xml new file mode 100644 index 0000000..165b974 --- /dev/null +++ b/src/my_robot_bringup/package.xml @@ -0,0 +1,23 @@ + + + + my_robot_bringup + 1.0.0 + Master launch file for the entire robot system + sunrise + Apache-2.0 + + ament_cmake + ament_cmake_python + + origincar_base + lslidar_driver + slam_toolbox + nav2_bringup + vlm_detect + robot_localization + + + ament_python + + diff --git a/src/my_robot_bringup/setup.py b/src/my_robot_bringup/setup.py new file mode 100644 index 0000000..b011b58 --- /dev/null +++ b/src/my_robot_bringup/setup.py @@ -0,0 +1,21 @@ +from setuptools import setup +from glob import glob +import os + +package_name = 'my_robot_bringup' + +setup( + name=package_name, + version='1.0.0', + packages=[], + data_files=[ + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='sunrise', + maintainer_email='sunrise@todo.todo', + description='Master bringup for robot system', + license='Apache-2.0', +) diff --git a/src/cyy_navigation2/CMakeLists.txt b/src/navigation/cyy_navigation2/CMakeLists.txt similarity index 100% rename from src/cyy_navigation2/CMakeLists.txt rename to src/navigation/cyy_navigation2/CMakeLists.txt diff --git a/src/cyy_navigation2/bt/follow_point.xml b/src/navigation/cyy_navigation2/bt/follow_point.xml similarity index 100% rename from src/cyy_navigation2/bt/follow_point.xml rename to src/navigation/cyy_navigation2/bt/follow_point.xml diff --git a/src/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/src/navigation/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml similarity index 100% rename from src/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml rename to src/navigation/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml diff --git a/src/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml b/src/navigation/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml rename to src/navigation/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/src/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml b/src/navigation/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml rename to src/navigation/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml diff --git a/src/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/src/navigation/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml rename to src/navigation/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml diff --git a/src/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/src/navigation/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml rename to src/navigation/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_distance.xml b/src/navigation/cyy_navigation2/bt/navigate_w_replanning_distance.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_w_replanning_distance.xml rename to src/navigation/cyy_navigation2/bt/navigate_w_replanning_distance.xml diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml b/src/navigation/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml rename to src/navigation/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml b/src/navigation/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml rename to src/navigation/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_speed.xml b/src/navigation/cyy_navigation2/bt/navigate_w_replanning_speed.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_w_replanning_speed.xml rename to src/navigation/cyy_navigation2/bt/navigate_w_replanning_speed.xml diff --git a/src/cyy_navigation2/bt/navigate_w_replanning_time.xml b/src/navigation/cyy_navigation2/bt/navigate_w_replanning_time.xml similarity index 100% rename from src/cyy_navigation2/bt/navigate_w_replanning_time.xml rename to src/navigation/cyy_navigation2/bt/navigate_w_replanning_time.xml diff --git a/src/cyy_navigation2/bt/odometry_calibration.xml b/src/navigation/cyy_navigation2/bt/odometry_calibration.xml similarity index 100% rename from src/cyy_navigation2/bt/odometry_calibration.xml rename to src/navigation/cyy_navigation2/bt/odometry_calibration.xml diff --git a/src/cyy_navigation2/config/nav2_params.yaml b/src/navigation/cyy_navigation2/config/nav2_params.yaml similarity index 100% rename from src/cyy_navigation2/config/nav2_params.yaml rename to src/navigation/cyy_navigation2/config/nav2_params.yaml diff --git a/src/cyy_navigation2/launch/car_bringup.launch.py b/src/navigation/cyy_navigation2/launch/car_bringup.launch.py similarity index 100% rename from src/cyy_navigation2/launch/car_bringup.launch.py rename to src/navigation/cyy_navigation2/launch/car_bringup.launch.py diff --git a/src/cyy_navigation2/launch/cyy_nav.launch.py b/src/navigation/cyy_navigation2/launch/cyy_nav.launch.py similarity index 100% rename from src/cyy_navigation2/launch/cyy_nav.launch.py rename to src/navigation/cyy_navigation2/launch/cyy_nav.launch.py diff --git a/src/cyy_navigation2/launch/cyy_nav_box.launch.py b/src/navigation/cyy_navigation2/launch/cyy_nav_box.launch.py similarity index 100% rename from src/cyy_navigation2/launch/cyy_nav_box.launch.py rename to src/navigation/cyy_navigation2/launch/cyy_nav_box.launch.py diff --git a/src/cyy_navigation2/maps/cyy_map.data b/src/navigation/cyy_navigation2/maps/cyy_map.data similarity index 100% rename from src/cyy_navigation2/maps/cyy_map.data rename to src/navigation/cyy_navigation2/maps/cyy_map.data diff --git a/src/cyy_navigation2/maps/cyy_map.pgm b/src/navigation/cyy_navigation2/maps/cyy_map.pgm similarity index 100% rename from src/cyy_navigation2/maps/cyy_map.pgm rename to src/navigation/cyy_navigation2/maps/cyy_map.pgm diff --git a/src/cyy_navigation2/maps/cyy_map.yaml b/src/navigation/cyy_navigation2/maps/cyy_map.yaml similarity index 100% rename from src/cyy_navigation2/maps/cyy_map.yaml rename to src/navigation/cyy_navigation2/maps/cyy_map.yaml diff --git a/src/cyy_navigation2/maps/cyy_map1.pgm b/src/navigation/cyy_navigation2/maps/cyy_map1.pgm similarity index 100% rename from src/cyy_navigation2/maps/cyy_map1.pgm rename to src/navigation/cyy_navigation2/maps/cyy_map1.pgm diff --git a/src/cyy_navigation2/maps/cyy_map1.yaml b/src/navigation/cyy_navigation2/maps/cyy_map1.yaml similarity index 100% rename from src/cyy_navigation2/maps/cyy_map1.yaml rename to src/navigation/cyy_navigation2/maps/cyy_map1.yaml diff --git a/src/cyy_navigation2/package.xml b/src/navigation/cyy_navigation2/package.xml similarity index 100% rename from src/cyy_navigation2/package.xml rename to src/navigation/cyy_navigation2/package.xml diff --git a/src/cyy_navigation2/param/nav2_params.yaml b/src/navigation/cyy_navigation2/param/nav2_params.yaml similarity index 100% rename from src/cyy_navigation2/param/nav2_params.yaml rename to src/navigation/cyy_navigation2/param/nav2_params.yaml diff --git a/src/cyy_navigation2/param/slam_toolbox_localization.yaml b/src/navigation/cyy_navigation2/param/slam_toolbox_localization.yaml similarity index 100% rename from src/cyy_navigation2/param/slam_toolbox_localization.yaml rename to src/navigation/cyy_navigation2/param/slam_toolbox_localization.yaml diff --git a/src/cyy_slamtoolbox/CMakeLists.txt b/src/navigation/cyy_slamtoolbox/CMakeLists.txt similarity index 100% rename from src/cyy_slamtoolbox/CMakeLists.txt rename to src/navigation/cyy_slamtoolbox/CMakeLists.txt diff --git a/src/cyy_slamtoolbox/config/angular_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/angular_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/angular_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/angular_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/box_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/box_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/box_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/box_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/footprint_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/footprint_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/footprint_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/footprint_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/intensity_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/intensity_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/intensity_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/intensity_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/laser_filter_config.yaml b/src/navigation/cyy_slamtoolbox/config/laser_filter_config.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/laser_filter_config.yaml rename to src/navigation/cyy_slamtoolbox/config/laser_filter_config.yaml diff --git a/src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml b/src/navigation/cyy_slamtoolbox/config/mapper_params_lifelong.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml rename to src/navigation/cyy_slamtoolbox/config/mapper_params_lifelong.yaml diff --git a/src/cyy_slamtoolbox/config/mapper_params_localization.yaml b/src/navigation/cyy_slamtoolbox/config/mapper_params_localization.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/mapper_params_localization.yaml rename to src/navigation/cyy_slamtoolbox/config/mapper_params_localization.yaml diff --git a/src/cyy_slamtoolbox/config/mapper_params_offline.yaml b/src/navigation/cyy_slamtoolbox/config/mapper_params_offline.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/mapper_params_offline.yaml rename to src/navigation/cyy_slamtoolbox/config/mapper_params_offline.yaml diff --git a/src/cyy_slamtoolbox/config/mapper_params_online_async.yaml b/src/navigation/cyy_slamtoolbox/config/mapper_params_online_async.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/mapper_params_online_async.yaml rename to src/navigation/cyy_slamtoolbox/config/mapper_params_online_async.yaml diff --git a/src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml b/src/navigation/cyy_slamtoolbox/config/mapper_params_online_sync.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml rename to src/navigation/cyy_slamtoolbox/config/mapper_params_online_sync.yaml diff --git a/src/cyy_slamtoolbox/config/mask_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/mask_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/mask_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/mask_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/median_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/median_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/median_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/median_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/median_spatial_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/median_spatial_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/median_spatial_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/median_spatial_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/multiple_filters_example.yaml b/src/navigation/cyy_slamtoolbox/config/multiple_filters_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/multiple_filters_example.yaml rename to src/navigation/cyy_slamtoolbox/config/multiple_filters_example.yaml diff --git a/src/cyy_slamtoolbox/config/pass_through_example.yaml b/src/navigation/cyy_slamtoolbox/config/pass_through_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/pass_through_example.yaml rename to src/navigation/cyy_slamtoolbox/config/pass_through_example.yaml diff --git a/src/cyy_slamtoolbox/config/polygon_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/polygon_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/polygon_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/polygon_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/range_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/range_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/range_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/range_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/scan_blob_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/scan_blob_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/sector_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/sector_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/sector_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/sector_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/shadow_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/shadow_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/shadow_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/shadow_filter_example.yaml diff --git a/src/cyy_slamtoolbox/config/slam_toolbox_default.rviz b/src/navigation/cyy_slamtoolbox/config/slam_toolbox_default.rviz similarity index 100% rename from src/cyy_slamtoolbox/config/slam_toolbox_default.rviz rename to src/navigation/cyy_slamtoolbox/config/slam_toolbox_default.rviz diff --git a/src/cyy_slamtoolbox/config/speckle_filter_example.yaml b/src/navigation/cyy_slamtoolbox/config/speckle_filter_example.yaml similarity index 100% rename from src/cyy_slamtoolbox/config/speckle_filter_example.yaml rename to src/navigation/cyy_slamtoolbox/config/speckle_filter_example.yaml diff --git a/src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py b/src/navigation/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py similarity index 100% rename from src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py rename to src/navigation/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py diff --git a/src/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py b/src/navigation/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py similarity index 100% rename from src/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py rename to src/navigation/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py diff --git a/src/cyy_slamtoolbox/launch/filter.launch.py b/src/navigation/cyy_slamtoolbox/launch/filter.launch.py similarity index 100% rename from src/cyy_slamtoolbox/launch/filter.launch.py rename to src/navigation/cyy_slamtoolbox/launch/filter.launch.py diff --git a/src/cyy_slamtoolbox/package.xml b/src/navigation/cyy_slamtoolbox/package.xml similarity index 100% rename from src/cyy_slamtoolbox/package.xml rename to src/navigation/cyy_slamtoolbox/package.xml diff --git a/src/gc_navigation2_real/CMakeLists.txt b/src/navigation/gc_navigation2_real/CMakeLists.txt similarity index 100% rename from src/gc_navigation2_real/CMakeLists.txt rename to src/navigation/gc_navigation2_real/CMakeLists.txt diff --git a/src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml b/src/navigation/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml similarity index 100% rename from src/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml rename to src/navigation/gc_navigation2_real/behavior_tree/nav_to_pose_ackermann.xml diff --git a/src/gc_navigation2_real/config/slam_toolbox_async_real.yaml b/src/navigation/gc_navigation2_real/config/slam_toolbox_async_real.yaml similarity index 100% rename from src/gc_navigation2_real/config/slam_toolbox_async_real.yaml rename to src/navigation/gc_navigation2_real/config/slam_toolbox_async_real.yaml diff --git a/src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml b/src/navigation/gc_navigation2_real/config/slam_toolbox_localization_real.yaml similarity index 100% rename from src/gc_navigation2_real/config/slam_toolbox_localization_real.yaml rename to src/navigation/gc_navigation2_real/config/slam_toolbox_localization_real.yaml diff --git a/src/gc_navigation2_real/launch/real_bringup.launch.py b/src/navigation/gc_navigation2_real/launch/real_bringup.launch.py similarity index 100% rename from src/gc_navigation2_real/launch/real_bringup.launch.py rename to src/navigation/gc_navigation2_real/launch/real_bringup.launch.py diff --git a/src/navigation/gc_navigation2_real/launch/real_mapping.launch.py b/src/navigation/gc_navigation2_real/launch/real_mapping.launch.py new file mode 100644 index 0000000..a6c6e8d --- /dev/null +++ b/src/navigation/gc_navigation2_real/launch/real_mapping.launch.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +# ============================================================================ +# real_slam_mapping.launch.py +# 功能：实车 SLAM 建图启动文件（无 Gazebo 仿真） +# +# 适用场景： +# - 在实际场地遥控机器人建立环境地图 +# - 在已有地图基础上增量更新 +# - 单独调试 slam_toolbox 建图参数 +# +# 架构： +# origincar_base (bringup) — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF +# lslidar_driver — 镭神激光雷达驱动 → /scan +# async_slam_toolbox_node — 激光 SLAM 建图（mapping 模式） +# 发布 map→odom_combined TF +# 发布 /map 话题（实时地图） +# +# 使用方式： +# ros2 launch gc_navigation2_real real_slam_mapping.launch.py +# +# 保存地图： +# ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \ +# "{filename: '/home/guoch/test_ws/src/gc_navigation2_real/maps/my_map'}" +# ros2 run nav2_map_server map_saver_cli -f +# ============================================================================ + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + """生成 LaunchDescription，启动实车底盘 + 雷达 + slam_toolbox 建图""" + + # ============================ 1. 包路径 ========================================= + pkg_dir = get_package_share_directory('gc_navigation2_real') + origincar_base_dir = get_package_share_directory('origincar_base') + + # ============================ 2. 声明启动参数 ==================================== + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + # slam_params_file: slam_toolbox 配置文件 + # slam_params_file = LaunchConfiguration( + # 'slam_params_file', + # default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml')) + + # map_file_name: 可选，加载已有序列化地图继续建图 + map_file_name = LaunchConfiguration('map_file_name', default='') + + # ============================ 3. 实车底盘 bringup =============================== + # 启动 origincar_base：串口驱动 + EKF + IMU 滤波 + TF 发布 + # 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']), + ) + + # ============================ 4. 镭神激光雷达驱动 =============================== + # 启动 lslidar_driver → /scan 话题 + lslidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('lslidar_driver'), + '/launch', '/lsn10_launch.py']), + ) + + # ============================ 5. slam_toolbox 建图节点 ============================ + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[ + slam_params_file, + { + 'use_sim_time': False, + 'map_file_name': map_file_name, + }, + ], + ) + + # ============================ 6. 组装 LaunchDescription ========================= + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='使用仿真时间（实车必须为 false）'), + DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'), + description='slam_toolbox 参数配置文件路径'), + DeclareLaunchArgument( + 'map_file_name', + default_value='', + description='已有序列化地图路径（.posegraph），留空则从零开始建图'), + + origincar_bringup, + lslidar_launch, + slam_toolbox_node, + ]) diff --git a/src/gc_navigation2_real/launch/real_nav2_slam.launch.py b/src/navigation/gc_navigation2_real/launch/real_nav2_slam.launch.py similarity index 100% rename from src/gc_navigation2_real/launch/real_nav2_slam.launch.py rename to src/navigation/gc_navigation2_real/launch/real_nav2_slam.launch.py diff --git a/src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py b/src/navigation/gc_navigation2_real/launch/real_nav2_slam_online.launch.py similarity index 100% rename from src/gc_navigation2_real/launch/real_nav2_slam_online.launch.py rename to src/navigation/gc_navigation2_real/launch/real_nav2_slam_online.launch.py diff --git a/src/gc_navigation2_real/launch/real_slam_mapping.launch.py b/src/navigation/gc_navigation2_real/launch/real_slam_mapping.launch.py similarity index 100% rename from src/gc_navigation2_real/launch/real_slam_mapping.launch.py rename to src/navigation/gc_navigation2_real/launch/real_slam_mapping.launch.py diff --git a/src/gc_navigation2_real/maps/Readme.txt b/src/navigation/gc_navigation2_real/maps/Readme.txt similarity index 100% rename from src/gc_navigation2_real/maps/Readme.txt rename to src/navigation/gc_navigation2_real/maps/Readme.txt diff --git a/src/gc_navigation2_real/package.xml b/src/navigation/gc_navigation2_real/package.xml similarity index 100% rename from src/gc_navigation2_real/package.xml rename to src/navigation/gc_navigation2_real/package.xml diff --git a/src/gc_navigation2_real/params/gc_navigation_slam_real.yaml b/src/navigation/gc_navigation2_real/params/gc_navigation_slam_real.yaml similarity index 100% rename from src/gc_navigation2_real/params/gc_navigation_slam_real.yaml rename to src/navigation/gc_navigation2_real/params/gc_navigation_slam_real.yaml diff --git a/src/gc_navigation2_slamtoolbox/CMakeLists.txt b/src/navigation/gc_navigation2_slamtoolbox/CMakeLists.txt similarity index 100% rename from src/gc_navigation2_slamtoolbox/CMakeLists.txt rename to src/navigation/gc_navigation2_slamtoolbox/CMakeLists.txt diff --git a/src/gc_navigation2_slamtoolbox/config/mapper_params_lifelong.yaml b/src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_lifelong.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/mapper_params_lifelong.yaml rename to src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_lifelong.yaml diff --git a/src/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml b/src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml rename to src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml diff --git a/src/gc_navigation2_slamtoolbox/config/mapper_params_offline.yaml b/src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_offline.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/mapper_params_offline.yaml rename to src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_offline.yaml diff --git a/src/gc_navigation2_slamtoolbox/config/mapper_params_online_multi_async.yaml b/src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_online_multi_async.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/mapper_params_online_multi_async.yaml rename to src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_online_multi_async.yaml diff --git a/src/gc_navigation2_slamtoolbox/config/mapper_params_online_sync.yaml b/src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_online_sync.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/mapper_params_online_sync.yaml rename to src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_online_sync.yaml diff --git a/src/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml b/src/navigation/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml rename to src/navigation/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml diff --git a/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml b/src/navigation/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml rename to src/navigation/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml b/src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml rename to src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_localization.yaml b/src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_localization.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/slam_toolbox_localization.yaml rename to src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_localization.yaml diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml b/src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml rename to src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py b/src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py similarity index 100% rename from src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py rename to src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py b/src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py similarity index 100% rename from src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py rename to src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py b/src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py similarity index 100% rename from src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py rename to src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py b/src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py similarity index 100% rename from src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py rename to src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py b/src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py similarity index 100% rename from src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py rename to src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py b/src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py similarity index 100% rename from src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py rename to src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py b/src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py similarity index 100% rename from src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py rename to src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py b/src/navigation/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py similarity index 100% rename from src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py rename to src/navigation/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py diff --git a/src/gc_navigation2_slamtoolbox/maps/my_map.data b/src/navigation/gc_navigation2_slamtoolbox/maps/my_map.data similarity index 100% rename from src/gc_navigation2_slamtoolbox/maps/my_map.data rename to src/navigation/gc_navigation2_slamtoolbox/maps/my_map.data diff --git a/src/gc_navigation2_slamtoolbox/maps/my_map.pgm b/src/navigation/gc_navigation2_slamtoolbox/maps/my_map.pgm similarity index 100% rename from src/gc_navigation2_slamtoolbox/maps/my_map.pgm rename to src/navigation/gc_navigation2_slamtoolbox/maps/my_map.pgm diff --git a/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph b/src/navigation/gc_navigation2_slamtoolbox/maps/my_map.posegraph similarity index 100% rename from src/gc_navigation2_slamtoolbox/maps/my_map.posegraph rename to src/navigation/gc_navigation2_slamtoolbox/maps/my_map.posegraph diff --git a/src/gc_navigation2_slamtoolbox/maps/my_map.yaml b/src/navigation/gc_navigation2_slamtoolbox/maps/my_map.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/maps/my_map.yaml rename to src/navigation/gc_navigation2_slamtoolbox/maps/my_map.yaml diff --git a/src/gc_navigation2_slamtoolbox/maps/zhihui.data b/src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.data similarity index 100% rename from src/gc_navigation2_slamtoolbox/maps/zhihui.data rename to src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.data diff --git a/src/gc_navigation2_slamtoolbox/maps/zhihui.pgm b/src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.pgm similarity index 100% rename from src/gc_navigation2_slamtoolbox/maps/zhihui.pgm rename to src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.pgm diff --git a/src/gc_navigation2_slamtoolbox/maps/zhihui.posegraph b/src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.posegraph similarity index 100% rename from src/gc_navigation2_slamtoolbox/maps/zhihui.posegraph rename to src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.posegraph diff --git a/src/gc_navigation2_slamtoolbox/maps/zhihui.yaml b/src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/maps/zhihui.yaml rename to src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.yaml diff --git a/src/gc_navigation2_slamtoolbox/package.xml b/src/navigation/gc_navigation2_slamtoolbox/package.xml similarity index 100% rename from src/gc_navigation2_slamtoolbox/package.xml rename to src/navigation/gc_navigation2_slamtoolbox/package.xml diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_amcl.yaml b/src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_amcl.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/params/gc_navigation_amcl.yaml rename to src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_amcl.yaml diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml b/src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml similarity index 100% rename from src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml rename to src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak b/src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak similarity index 100% rename from src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak rename to src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2 b/src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2 similarity index 100% rename from src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2 rename to src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2 diff --git a/src/gc_navigation2_slamtoolbox/world/zhihui/model.config b/src/navigation/gc_navigation2_slamtoolbox/world/zhihui/model.config similarity index 100% rename from src/gc_navigation2_slamtoolbox/world/zhihui/model.config rename to src/navigation/gc_navigation2_slamtoolbox/world/zhihui/model.config diff --git a/src/gc_navigation2_slamtoolbox/world/zhihui/model.sdf b/src/navigation/gc_navigation2_slamtoolbox/world/zhihui/model.sdf similarity index 100% rename from src/gc_navigation2_slamtoolbox/world/zhihui/model.sdf rename to src/navigation/gc_navigation2_slamtoolbox/world/zhihui/model.sdf diff --git a/src/gc_navigation_fish/CMakeLists.txt b/src/navigation/gc_navigation_fish/CMakeLists.txt similarity index 100% rename from src/gc_navigation_fish/CMakeLists.txt rename to src/navigation/gc_navigation_fish/CMakeLists.txt diff --git a/src/gc_navigation_fish/launch/gc_navigation.launch.py b/src/navigation/gc_navigation_fish/launch/gc_navigation.launch.py similarity index 100% rename from src/gc_navigation_fish/launch/gc_navigation.launch.py rename to src/navigation/gc_navigation_fish/launch/gc_navigation.launch.py diff --git a/src/gc_navigation_fish/maps/test_map.pgm b/src/navigation/gc_navigation_fish/maps/test_map.pgm similarity index 100% rename from src/gc_navigation_fish/maps/test_map.pgm rename to src/navigation/gc_navigation_fish/maps/test_map.pgm diff --git a/src/gc_navigation_fish/maps/test_map.yaml b/src/navigation/gc_navigation_fish/maps/test_map.yaml similarity index 100% rename from src/gc_navigation_fish/maps/test_map.yaml rename to src/navigation/gc_navigation_fish/maps/test_map.yaml diff --git a/src/gc_navigation_fish/package.xml b/src/navigation/gc_navigation_fish/package.xml similarity index 100% rename from src/gc_navigation_fish/package.xml rename to src/navigation/gc_navigation_fish/package.xml diff --git a/src/gc_navigation_fish/param/gc_navigation.yaml b/src/navigation/gc_navigation_fish/param/gc_navigation.yaml similarity index 100% rename from src/gc_navigation_fish/param/gc_navigation.yaml rename to src/navigation/gc_navigation_fish/param/gc_navigation.yaml diff --git a/src/gc_navigation_fish/param/navigation_test.yaml b/src/navigation/gc_navigation_fish/param/navigation_test.yaml similarity index 100% rename from src/gc_navigation_fish/param/navigation_test.yaml rename to src/navigation/gc_navigation_fish/param/navigation_test.yaml diff --git a/src/gc_slam_toolbox_fish/CMakeLists.txt b/src/navigation/gc_slam_toolbox_fish/CMakeLists.txt similarity index 100% rename from src/gc_slam_toolbox_fish/CMakeLists.txt rename to src/navigation/gc_slam_toolbox_fish/CMakeLists.txt diff --git a/src/gc_slam_toolbox_fish/config/gc_2d.lua b/src/navigation/gc_slam_toolbox_fish/config/gc_2d.lua similarity index 100% rename from src/gc_slam_toolbox_fish/config/gc_2d.lua rename to src/navigation/gc_slam_toolbox_fish/config/gc_2d.lua diff --git a/src/gc_slam_toolbox_fish/launch/catograph.launch.py b/src/navigation/gc_slam_toolbox_fish/launch/catograph.launch.py similarity index 100% rename from src/gc_slam_toolbox_fish/launch/catograph.launch.py rename to src/navigation/gc_slam_toolbox_fish/launch/catograph.launch.py diff --git a/src/gc_slam_toolbox_fish/package.xml b/src/navigation/gc_slam_toolbox_fish/package.xml similarity index 100% rename from src/gc_slam_toolbox_fish/package.xml rename to src/navigation/gc_slam_toolbox_fish/package.xml diff --git a/src/navigation/obstacle_nav2/CMakeLists.txt b/src/navigation/obstacle_nav2/CMakeLists.txt new file mode 100755 index 0000000..d04e73f --- /dev/null +++ b/src/navigation/obstacle_nav2/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(obstacle_nav2) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(obstacle_scanner REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +add_library(obstacle_array_layer SHARED + src/obstacle_array_layer.cpp +) +target_include_directories(obstacle_array_layer PUBLIC + $ + $ +) +ament_target_dependencies(obstacle_array_layer + nav2_costmap_2d + pluginlib + rclcpp + tf2 + tf2_ros + tf2_geometry_msgs + obstacle_scanner + geometry_msgs + nav_msgs + std_msgs +) + +pluginlib_export_plugin_description_file(nav2_costmap_2d obstacle_nav2_plugins.xml) + +install(TARGETS obstacle_array_layer + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY include/ DESTINATION include) +install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp + src/obstacle_array_layer.cpp + ) + target_include_directories(test_obstacle_array_layer PRIVATE + $ + ) + ament_target_dependencies(test_obstacle_array_layer + nav2_costmap_2d + pluginlib + rclcpp + tf2 + tf2_ros + tf2_geometry_msgs + obstacle_scanner + geometry_msgs + nav_msgs + std_msgs + ) +endif() + +install(DIRECTORY config launch behavior_tree + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml b/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml new file mode 100755 index 0000000..49ade2e --- /dev/null +++ b/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/navigation/obstacle_nav2/config/nav2_params.yaml b/src/navigation/obstacle_nav2/config/nav2_params.yaml new file mode 100755 index 0000000..4f25cdd --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_params.yaml @@ -0,0 +1,319 @@ +# ============================================================================ +# nav2_params.yaml — Odometry-only obstacle navigation +# +# No static map, no AMCL, no SLAM. +# Both costmaps are rolling windows in odom. The launch file can rewrite every +# global_frame leaf when a different connected odometry frame is required. +# Global planner: Smac Hybrid A* (Reeds-Shepp) +# Local controller: MPPI (Ackermann) +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: odom + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # Injected by obstacle_nav2.launch.py from this package's share directory. + default_nav_to_pose_bt_xml: "" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 36 + model_dt: 0.05 + batch_size: 1000 + vx_std: 0.2 + vy_std: 0.0 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.30 + vy_max: 0.0 + wz_max: 1.5 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: true + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 10 + height: 10 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.25 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 1.3 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 + retrospective_penalty: 0.015 + # 5 m covers the rolling planning horizon without the startup and memory + # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: false + smooth_path: True + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: odom + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp b/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp new file mode 100755 index 0000000..ab5cb48 --- /dev/null +++ b/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp @@ -0,0 +1,106 @@ +#ifndef OBSTACLE_NAV2__OBSTACLE_ARRAY_LAYER_HPP_ +#define OBSTACLE_NAV2__OBSTACLE_ARRAY_LAYER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "rclcpp/rclcpp.hpp" +#include "obstacle_scanner/msg/obstacle_array.hpp" +#include "tf2_ros/buffer.h" + +namespace obstacle_nav2 +{ + +class ObstacleArrayLayer : public nav2_costmap_2d::CostmapLayer +{ +public: + struct CircleObstacle + { + double center_x{0.0}; + double center_y{0.0}; + double radius{0.0}; + }; + + struct SnapshotBounds + { + bool valid{false}; + double min_x{0.0}; + double min_y{0.0}; + double max_x{0.0}; + double max_y{0.0}; + }; + + ObstacleArrayLayer(); + ~ObstacleArrayLayer() override; + + void onInitialize() override; + void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, double * max_y) override; + void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) override; + void reset() override; + bool isClearable() override; + void activate() override; + void deactivate() override; + + static void rasterizeCircle( + nav2_costmap_2d::Costmap2D & grid, + double cx, double cy, double radius, + double resolution, double origin_x, double origin_y); + static double clampRadius(double radius, double min_r, double max_r, double default_r); + static std::vector transformSnapshot( + const obstacle_scanner::msg::ObstacleArray & snapshot, + const std::string & global_frame, + tf2_ros::Buffer & tf_buffer, + const rclcpp::Duration & timeout); + static SnapshotBounds applySnapshot( + nav2_costmap_2d::Costmap2D & grid, + const std::vector & obstacles, + double resolution, double origin_x, double origin_y); + +private: + void obstacleCallback(const obstacle_scanner::msg::ObstacleArray::SharedPtr msg); + static SnapshotBounds mergeBounds( + const SnapshotBounds & first, const SnapshotBounds & second); + void clearExpiredObstacles(const rclcpp::Time & now); + + // Note: node_ (LifecycleNode::WeakPtr), tf_, layered_costmap_, and name_ + // are inherited from nav2_costmap_2d::Layer. + + rclcpp::Subscription::SharedPtr obstacle_sub_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + std::unique_ptr callback_executor_; + std::thread callback_thread_; + std::atomic_bool callback_stop_{false}; + std::mutex data_mutex_; + + rclcpp::Time last_obstacle_time_; + bool has_received_obstacles_{false}; + SnapshotBounds current_bounds_; + SnapshotBounds pending_clear_bounds_; + std::uint64_t bounds_generation_{0}; + std::uint64_t bounds_generation_used_{0}; + + std::string topic_; + std::string global_frame_; + double obstacle_timeout_{0.5}; + double transform_tolerance_{0.2}; + double default_obstacle_radius_{0.05}; + double minimum_obstacle_radius_{0.02}; + double maximum_obstacle_radius_{0.50}; + double extra_inflation_{0.02}; +}; + +} // namespace obstacle_nav2 + +#endif // OBSTACLE_NAV2__OBSTACLE_ARRAY_LAYER_HPP_ diff --git a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py new file mode 100755 index 0000000..e98f616 --- /dev/null +++ b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +# ============================================================================ +# obstacle_nav2.launch.py +# +# Odometry-only Nav2 navigation using obstacle_scanner detections. +# +# Architecture: +# origincar_base (bringup) — chassis serial driver + EKF + TF +# lslidar_driver — lidar → /scan +# obstacle_scanner (optional) — /scan → /obstacles +# Nav2 navigation_launch — rolling costmaps + Hybrid A* + MPPI +# +# No map_server, AMCL, or slam_toolbox in the default path. +# +# Usage: +# ros2 launch obstacle_nav2 obstacle_nav2.launch.py +# ros2 launch obstacle_nav2 obstacle_nav2.launch.py enable_motion:=true +# ============================================================================ + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import SetRemap +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + """Odometry-only obstacle navigation.""" + + pkg_dir = get_package_share_directory('obstacle_nav2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + + # ========================== Launch Arguments ============================== + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + global_frame = LaunchConfiguration('global_frame', default='odom') + use_static_map = LaunchConfiguration('use_static_map', default='false') + map_yaml = LaunchConfiguration('map_yaml', default='') + enable_motion = LaunchConfiguration('enable_motion', default='false') + start_base = LaunchConfiguration('start_base', default='true') + start_lidar = LaunchConfiguration('start_lidar', default='true') + start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true') + + nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_params.yaml') + nav_to_pose_bt_path = os.path.join( + pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml') + configured_params = RewrittenYaml( + source_file=nav2_param_path, + root_key='', + param_rewrites={ + 'global_frame': global_frame, + 'default_nav_to_pose_bt_xml': nav_to_pose_bt_path, + }, + convert_types=True, + ) + base_cmd_vel_topic = PythonExpression([ + "'/cmd_vel' if '", enable_motion, + "' == 'true' else '/cmd_vel_hardware_disabled'", + ]) + + # ========================== Base Bringup ================================== + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('origincar_base'), + '/launch', '/origincar_bringup.launch.py']), + condition=IfCondition(start_base), + ) + safe_base_bringup = GroupAction([ + SetRemap(src='cmd_vel', dst=base_cmd_vel_topic), + origincar_bringup, + ]) + + # ========================== Lidar Driver ================================== + lslidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('lslidar_driver'), + '/launch', '/lsn10_launch.py']), + condition=IfCondition(start_lidar), + ) + + # ========================== Obstacle Scanner ============================== + obstacle_scanner_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('obstacle_scanner'), + '/launch', '/obstacle_scanner.launch.py']), + condition=IfCondition(start_obstacle_scanner), + ) + + # ========================== Nav2 Navigation =============================== + navigation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/navigation_launch.py']), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': configured_params, + 'autostart': 'true', + }.items(), + ) + # ========================== Assembly ====================================== + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock'), + DeclareLaunchArgument( + 'global_frame', + default_value='odom', + description='Global frame for both costmaps'), + DeclareLaunchArgument( + 'use_static_map', + default_value='false', + description='Reserved: enable static map + AMCL (not yet implemented)'), + DeclareLaunchArgument( + 'map_yaml', + default_value='', + description='Reserved: path to map YAML file'), + DeclareLaunchArgument( + 'enable_motion', + default_value='false', + description='Route Nav2 cmd_vel to the real base topic'), + DeclareLaunchArgument( + 'start_base', + default_value='true', + description='Start the base driver, EKF, and robot TF publishers'), + DeclareLaunchArgument( + 'start_lidar', + default_value='true', + description='Start the lidar driver'), + DeclareLaunchArgument( + 'start_obstacle_scanner', + default_value='true', + description='Also start the obstacle_scanner node'), + + safe_base_bringup, + lslidar_launch, + obstacle_scanner_launch, + navigation_launch, + ]) diff --git a/src/navigation/obstacle_nav2/obstacle_nav2_plugins.xml b/src/navigation/obstacle_nav2/obstacle_nav2_plugins.xml new file mode 100755 index 0000000..844d060 --- /dev/null +++ b/src/navigation/obstacle_nav2/obstacle_nav2_plugins.xml @@ -0,0 +1,7 @@ + + + + Dynamic obstacle layer from obstacle_scanner/ObstacleArray messages. + + + diff --git a/src/navigation/obstacle_nav2/package.xml b/src/navigation/obstacle_nav2/package.xml new file mode 100755 index 0000000..8a1c2fc --- /dev/null +++ b/src/navigation/obstacle_nav2/package.xml @@ -0,0 +1,36 @@ + + + + obstacle_nav2 + 0.0.0 + Odometry-only Nav2 navigation using obstacle_scanner detections as a dynamic costmap layer. + sunrise + Apache-2.0 + + ament_cmake + + rclcpp + nav2_costmap_2d + pluginlib + tf2 + tf2_ros + tf2_geometry_msgs + obstacle_scanner + geometry_msgs + nav_msgs + std_msgs + + launch + launch_ros + nav2_bringup + nav2_common + origincar_base + lslidar_driver + + ament_cmake_gtest + + + ament_cmake + + + diff --git a/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp b/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp new file mode 100755 index 0000000..5f959c1 --- /dev/null +++ b/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp @@ -0,0 +1,399 @@ +#include "obstacle_nav2/obstacle_array_layer.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/point_stamped.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace obstacle_nav2 +{ + +ObstacleArrayLayer::ObstacleArrayLayer() = default; +ObstacleArrayLayer::~ObstacleArrayLayer() +{ + callback_stop_.store(true, std::memory_order_release); + if (callback_executor_) { + callback_executor_->cancel(); + } + if (callback_thread_.joinable()) { + callback_thread_.join(); + } +} + +void ObstacleArrayLayer::onInitialize() +{ + // CRITICAL: Match the internal costmap size to the master costmap. + // CostmapLayer::matchSize() is normally called during updateMap(), + // but obstacles arrive during CONFIGURE phase when subscriptions + // are active. We need the buffer sized now so rasterizeCircle works. + CostmapLayer::onInitialize(); + if (layered_costmap_) { + auto * master = layered_costmap_->getCostmap(); + resizeMap( + master->getSizeInCellsX(), master->getSizeInCellsY(), + master->getResolution(), + master->getOriginX(), master->getOriginY()); + } + + // Lock the base-class LifecycleNode weak_ptr + auto node = node_.lock(); + if (!node) { + RCLCPP_ERROR( + rclcpp::get_logger("obstacle_array_layer"), + "Failed to lock node in onInitialize"); + return; + } + + // Parameters (prefixed with layer name per Nav2 convention) + node->declare_parameter(name_ + ".enabled", true); + node->declare_parameter(name_ + ".topic", std::string("/obstacles")); + node->declare_parameter(name_ + ".obstacle_timeout", 0.5); + node->declare_parameter(name_ + ".transform_tolerance", 0.2); + node->declare_parameter(name_ + ".default_obstacle_radius", 0.05); + node->declare_parameter(name_ + ".minimum_obstacle_radius", 0.02); + node->declare_parameter(name_ + ".maximum_obstacle_radius", 0.50); + node->declare_parameter(name_ + ".extra_inflation", 0.02); + + node->get_parameter(name_ + ".enabled", enabled_); + node->get_parameter(name_ + ".topic", topic_); + node->get_parameter(name_ + ".obstacle_timeout", obstacle_timeout_); + node->get_parameter(name_ + ".transform_tolerance", transform_tolerance_); + node->get_parameter(name_ + ".default_obstacle_radius", default_obstacle_radius_); + node->get_parameter(name_ + ".minimum_obstacle_radius", minimum_obstacle_radius_); + node->get_parameter(name_ + ".maximum_obstacle_radius", maximum_obstacle_radius_); + node->get_parameter(name_ + ".extra_inflation", extra_inflation_); + + global_frame_ = layered_costmap_->getGlobalFrameID(); + + // Costmap plugins are configured after the parent node has entered its + // executor. Use an explicit callback group so this late-created subscription + // is guaranteed to be serviced on Humble. + callback_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::SubscriptionOptions subscription_options; + subscription_options.callback_group = callback_group_; + obstacle_sub_ = node->create_subscription( + topic_, rclcpp::QoS(10).reliable(), + std::bind(&ObstacleArrayLayer::obstacleCallback, this, std::placeholders::_1), + subscription_options); + callback_executor_ = std::make_unique(); + callback_executor_->add_callback_group( + callback_group_, node->get_node_base_interface()); + callback_stop_.store(false, std::memory_order_release); + callback_thread_ = std::thread( + [this]() { + while (!callback_stop_.load(std::memory_order_acquire)) { + callback_executor_->spin_once(std::chrono::milliseconds(100)); + } + }); + + current_ = true; + has_received_obstacles_ = false; + last_obstacle_time_ = node->now(); +} + +void ObstacleArrayLayer::activate() {} +void ObstacleArrayLayer::deactivate() {} + +void ObstacleArrayLayer::reset() +{ + std::lock_guard lock(data_mutex_); + pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, current_bounds_); + if (pending_clear_bounds_.valid) { + ++bounds_generation_; + } + resetMap(0, 0, getSizeInCellsX(), getSizeInCellsY()); + current_bounds_ = SnapshotBounds(); + has_received_obstacles_ = false; +} + +bool ObstacleArrayLayer::isClearable() +{ + return true; +} + +void ObstacleArrayLayer::updateBounds( + double robot_x, double robot_y, double /*robot_yaw*/, + double * min_x, double * min_y, + double * max_x, double * max_y) +{ + auto node = node_.lock(); + if (!node) {return;} + + bool enabled; + node->get_parameter_or(name_ + ".enabled", enabled, true); + if (!enabled) {return;} + + std::lock_guard lock(data_mutex_); + if (layered_costmap_->isRolling()) { + updateOrigin( + robot_x - getSizeInMetersX() / 2.0, + robot_y - getSizeInMetersY() / 2.0); + } + clearExpiredObstacles(node->now()); + + const auto bounds = mergeBounds(current_bounds_, pending_clear_bounds_); + if (bounds.valid) { + touch(bounds.min_x, bounds.min_y, min_x, min_y, max_x, max_y); + touch(bounds.max_x, bounds.max_y, min_x, min_y, max_x, max_y); + } + bounds_generation_used_ = bounds_generation_; +} + +void ObstacleArrayLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) +{ + auto node = node_.lock(); + if (!node) {return;} + + bool enabled; + node->get_parameter_or(name_ + ".enabled", enabled, true); + if (!enabled) {return;} + + std::lock_guard lock(data_mutex_); + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + if (bounds_generation_used_ == bounds_generation_) { + pending_clear_bounds_ = SnapshotBounds(); + } +} + +// ============================================================================ +// Static helpers +// ============================================================================ + +void ObstacleArrayLayer::rasterizeCircle( + nav2_costmap_2d::Costmap2D & grid, + double cx, double cy, double radius, + double resolution, double origin_x, double origin_y) +{ + if (radius < 0.0) { + return; + } + + // Handle zero radius: mark only the cell containing the center + if (radius <= 0.0) { + unsigned int cx_cell, cy_cell; + if (grid.worldToMap(cx, cy, cx_cell, cy_cell)) { + grid.setCost(cx_cell, cy_cell, nav2_costmap_2d::LETHAL_OBSTACLE); + } + return; + } + + double radius_sq = radius * radius; + unsigned int size_x = grid.getSizeInCellsX(); + unsigned int size_y = grid.getSizeInCellsY(); + + // Bounding box in cell indices + int ix_min = static_cast(std::floor((cx - radius - origin_x) / resolution)); + int iy_min = static_cast(std::floor((cy - radius - origin_y) / resolution)); + int ix_max = static_cast(std::ceil((cx + radius - origin_x) / resolution)); + int iy_max = static_cast(std::ceil((cy + radius - origin_y) / resolution)); + + if (ix_min < 0) {ix_min = 0;} + if (iy_min < 0) {iy_min = 0;} + if (ix_max >= static_cast(size_x)) {ix_max = static_cast(size_x) - 1;} + if (iy_max >= static_cast(size_y)) {iy_max = static_cast(size_y) - 1;} + + for (int j = iy_min; j <= iy_max; ++j) { + for (int i = ix_min; i <= ix_max; ++i) { + double wx, wy; + grid.mapToWorld( + static_cast(i), + static_cast(j), wx, wy); + double dx = wx - cx; + double dy = wy - cy; + if (dx * dx + dy * dy <= radius_sq) { + grid.setCost( + static_cast(i), + static_cast(j), + nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + } +} + +double ObstacleArrayLayer::clampRadius( + double radius, double min_r, double max_r, double default_r) +{ + if (!std::isfinite(radius) || radius <= 0.0) { + return default_r; + } + if (radius < min_r) { + return min_r; + } + if (radius > max_r) { + return max_r; + } + return radius; +} + +std::vector ObstacleArrayLayer::transformSnapshot( + const obstacle_scanner::msg::ObstacleArray & snapshot, + const std::string & global_frame, + tf2_ros::Buffer & tf_buffer, + const rclcpp::Duration & timeout) +{ + std::vector transformed; + transformed.reserve(snapshot.obstacles.size()); + if (snapshot.obstacles.empty()) { + return transformed; + } + + const rclcpp::Time stamp(snapshot.header.stamp); + const auto transform = timeout.nanoseconds() > 0 ? + tf_buffer.lookupTransform(global_frame, snapshot.header.frame_id, stamp, timeout) : + tf_buffer.lookupTransform(global_frame, snapshot.header.frame_id, stamp); + + for (const auto & obstacle : snapshot.obstacles) { + geometry_msgs::msg::PointStamped input; + geometry_msgs::msg::PointStamped output; + input.header = snapshot.header; + input.point.x = obstacle.center_x; + input.point.y = obstacle.center_y; + input.point.z = 0.0; + tf2::doTransform(input, output, transform); + transformed.push_back({output.point.x, output.point.y, obstacle.radius}); + } + return transformed; +} + +ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::applySnapshot( + nav2_costmap_2d::Costmap2D & grid, + const std::vector & obstacles, + double resolution, double origin_x, double origin_y) +{ + grid.resetMap(0, 0, grid.getSizeInCellsX(), grid.getSizeInCellsY()); + SnapshotBounds bounds; + for (const auto & obstacle : obstacles) { + rasterizeCircle( + grid, obstacle.center_x, obstacle.center_y, obstacle.radius, + resolution, origin_x, origin_y); + if (!bounds.valid) { + bounds.valid = true; + bounds.min_x = obstacle.center_x - obstacle.radius; + bounds.min_y = obstacle.center_y - obstacle.radius; + bounds.max_x = obstacle.center_x + obstacle.radius; + bounds.max_y = obstacle.center_y + obstacle.radius; + } else { + bounds.min_x = std::min(bounds.min_x, obstacle.center_x - obstacle.radius); + bounds.min_y = std::min(bounds.min_y, obstacle.center_y - obstacle.radius); + bounds.max_x = std::max(bounds.max_x, obstacle.center_x + obstacle.radius); + bounds.max_y = std::max(bounds.max_y, obstacle.center_y + obstacle.radius); + } + } + return bounds; +} + +ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::mergeBounds( + const SnapshotBounds & first, const SnapshotBounds & second) +{ + if (!first.valid) { + return second; + } + if (!second.valid) { + return first; + } + SnapshotBounds merged; + merged.valid = true; + merged.min_x = std::min(first.min_x, second.min_x); + merged.min_y = std::min(first.min_y, second.min_y); + merged.max_x = std::max(first.max_x, second.max_x); + merged.max_y = std::max(first.max_y, second.max_y); + return merged; +} + +void ObstacleArrayLayer::clearExpiredObstacles(const rclcpp::Time & now) +{ + if (!has_received_obstacles_) { + return; + } + if ((now - last_obstacle_time_).seconds() <= obstacle_timeout_) { + return; + } + pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, current_bounds_); + if (pending_clear_bounds_.valid) { + ++bounds_generation_; + } + resetMap(0, 0, getSizeInCellsX(), getSizeInCellsY()); + current_bounds_ = SnapshotBounds(); + has_received_obstacles_ = false; +} + +// ============================================================================ +// Private +// ============================================================================ + +void ObstacleArrayLayer::obstacleCallback( + const obstacle_scanner::msg::ObstacleArray::SharedPtr msg) +{ + auto node = node_.lock(); + if (!node) {return;} + + bool enabled; + node->get_parameter_or(name_ + ".enabled", enabled, true); + if (!enabled) {return;} + + std::vector transformed; + try { + transformed = transformSnapshot( + *msg, global_frame_, *tf_, + rclcpp::Duration::from_seconds(transform_tolerance_)); + } catch (const tf2::TransformException & e) { + RCLCPP_WARN_THROTTLE( + node->get_logger(), *node->get_clock(), 5000, + "ObstacleArrayLayer: TF failed at message stamp: %s", e.what()); + return; + } + + std::vector valid_obstacles; + valid_obstacles.reserve(transformed.size()); + const double origin_x = getOriginX(); + const double origin_y = getOriginY(); + const double map_x_max = origin_x + getSizeInMetersX(); + const double map_y_max = origin_y + getSizeInMetersY(); + + for (const auto & obs : transformed) { + if (!std::isfinite(obs.center_x) || !std::isfinite(obs.center_y)) { + continue; + } + + double radius = clampRadius( + obs.radius, minimum_obstacle_radius_, maximum_obstacle_radius_, + default_obstacle_radius_); + double effective_r = radius + extra_inflation_; + + if (obs.center_x + effective_r < origin_x || + obs.center_x - effective_r > map_x_max || + obs.center_y + effective_r < origin_y || + obs.center_y - effective_r > map_y_max) + { + continue; + } + valid_obstacles.push_back({obs.center_x, obs.center_y, effective_r}); + } + + { + std::lock_guard lock(data_mutex_); + const SnapshotBounds previous_bounds = current_bounds_; + current_bounds_ = applySnapshot( + *this, valid_obstacles, getResolution(), origin_x, origin_y); + pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, previous_bounds); + if (previous_bounds.valid || current_bounds_.valid) { + ++bounds_generation_; + } + has_received_obstacles_ = current_bounds_.valid; + last_obstacle_time_ = node->now(); + } +} + +} // namespace obstacle_nav2 + +PLUGINLIB_EXPORT_CLASS( + obstacle_nav2::ObstacleArrayLayer, + nav2_costmap_2d::Layer) diff --git a/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp b/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp new file mode 100755 index 0000000..7b34e03 --- /dev/null +++ b/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp @@ -0,0 +1,247 @@ +#include + +#include +#include +#include + +#include "obstacle_nav2/obstacle_array_layer.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "obstacle_scanner/msg/obstacle.hpp" +#include "obstacle_scanner/msg/obstacle_array.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" + +using obstacle_nav2::ObstacleArrayLayer; + +static constexpr double kResolution = 0.05; +static constexpr double kOriginX = -1.0; +static constexpr double kOriginY = -1.0; + +// ============================================================================ +// rasterizeCircle tests +// ============================================================================ + +TEST(ObstacleArrayLayerTest, RasterizeCircleMarksLethalCells) +{ + nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY); + + ObstacleArrayLayer::rasterizeCircle( + grid, 0.0, 0.0, 0.1, + kResolution, kOriginX, kOriginY); + + // center cell should be lethal + unsigned int mx, my; + grid.worldToMap(0.0, 0.0, mx, my); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE); + + // cell outside radius should be free + grid.worldToMap(0.3, 0.3, mx, my); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE); +} + +TEST(ObstacleArrayLayerTest, RasterizeCircleDisplacedCenter) +{ + nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY); + + ObstacleArrayLayer::rasterizeCircle( + grid, -0.5, -0.5, 0.15, + kResolution, kOriginX, kOriginY); + + unsigned int mx, my; + grid.worldToMap(-0.5, -0.5, mx, my); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE); + + grid.worldToMap(0.5, 0.5, mx, my); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE); +} + +TEST(ObstacleArrayLayerTest, RasterizeCircleAtCostmapEdge) +{ + nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY); + + // should not crash or access out of bounds + ObstacleArrayLayer::rasterizeCircle( + grid, 0.9, 0.9, 0.2, + kResolution, kOriginX, kOriginY); + + unsigned int mx, my; + grid.worldToMap(0.85, 0.85, mx, my); + ASSERT_LT(mx, 40u); + ASSERT_LT(my, 40u); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE); +} + +TEST(ObstacleArrayLayerTest, RasterizeCircleEntirelyOutsideCostmap) +{ + nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY); + + EXPECT_NO_THROW( + ObstacleArrayLayer::rasterizeCircle( + grid, 5.0, 5.0, 0.1, + kResolution, kOriginX, kOriginY)); +} + +TEST(ObstacleArrayLayerTest, RasterizeCircleZeroRadius) +{ + nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY); + + ObstacleArrayLayer::rasterizeCircle( + grid, 0.0, 0.0, 0.0, + kResolution, kOriginX, kOriginY); + + unsigned int mx, my; + grid.worldToMap(0.0, 0.0, mx, my); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE); +} + +TEST(ObstacleArrayLayerTest, RasterizeCircleAllCellsInRadiusAreLethal) +{ + nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY); + + ObstacleArrayLayer::rasterizeCircle( + grid, 0.0, 0.0, 0.1, + kResolution, kOriginX, kOriginY); + + // spot-check cells within radius are lethal + unsigned int mx, my; + grid.worldToMap(0.05, 0.0, mx, my); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE); + + grid.worldToMap(0.0, 0.05, mx, my); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE); +} + +// ============================================================================ +// clampRadius tests +// ============================================================================ + +TEST(ObstacleArrayLayerTest, ClampRadiusPassesThroughValid) +{ + EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.1, 0.02, 0.5, 0.05), 0.1); + EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.02, 0.02, 0.5, 0.05), 0.02); + EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.5, 0.02, 0.5, 0.05), 0.5); +} + +TEST(ObstacleArrayLayerTest, ClampRadiusFallsBackForNonPositive) +{ + constexpr double kDefault = 0.05; + EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.0, 0.02, 0.5, kDefault), kDefault); + EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(-0.1, 0.02, 0.5, kDefault), kDefault); +} + +TEST(ObstacleArrayLayerTest, ClampRadiusFallsBackForNonFinite) +{ + constexpr double kDefault = 0.05; + EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(NAN, 0.02, 0.5, kDefault), kDefault); + EXPECT_DOUBLE_EQ( + ObstacleArrayLayer::clampRadius(INFINITY, 0.02, 0.5, kDefault), kDefault); + EXPECT_DOUBLE_EQ( + ObstacleArrayLayer::clampRadius(-INFINITY, 0.02, 0.5, kDefault), kDefault); +} + +TEST(ObstacleArrayLayerTest, ClampRadiusClampsBelowMinimum) +{ + EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.01, 0.02, 0.5, 0.05), 0.02); +} + +TEST(ObstacleArrayLayerTest, ClampRadiusClampsAboveMaximum) +{ + EXPECT_DOUBLE_EQ(ObstacleArrayLayer::clampRadius(0.6, 0.02, 0.5, 0.05), 0.5); +} + +TEST(ObstacleArrayLayerTest, InitializesEnabledStateFromDefaultParameter) +{ + auto node = std::make_shared("obstacle_layer_test"); + nav2_costmap_2d::LayeredCostmap layered_costmap("odom", true, false); + layered_costmap.resizeMap(40, 40, kResolution, kOriginX, kOriginY); + tf2_ros::Buffer tf_buffer(node->get_clock()); + ObstacleArrayLayer layer; + + layer.initialize( + &layered_costmap, "obstacle_layer", &tf_buffer, node, + rclcpp::CallbackGroup::SharedPtr()); + + EXPECT_TRUE(layer.isEnabled()); +} + +TEST(ObstacleArrayLayerTest, EmptySnapshotClearsPreviousObstacle) +{ + nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY); + const std::vector occupied{{0.0, 0.0, 0.1}}; + + const auto occupied_bounds = ObstacleArrayLayer::applySnapshot( + grid, occupied, kResolution, kOriginX, kOriginY); + + unsigned int mx, my; + ASSERT_TRUE(grid.worldToMap(0.0, 0.0, mx, my)); + EXPECT_TRUE(occupied_bounds.valid); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE); + + const auto cleared_bounds = ObstacleArrayLayer::applySnapshot( + grid, {}, kResolution, kOriginX, kOriginY); + + EXPECT_FALSE(cleared_bounds.valid); + EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE); +} + +TEST(ObstacleArrayLayerTest, TransformSnapshotUsesMessageTimestamp) +{ + auto clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + buffer.setUsingDedicatedThread(true); + + geometry_msgs::msg::TransformStamped old_transform; + old_transform.header.frame_id = "odom"; + old_transform.child_frame_id = "laser_link"; + old_transform.header.stamp = rclcpp::Time(9, 800000000, RCL_SYSTEM_TIME); + old_transform.transform.translation.x = 100.0; + old_transform.transform.rotation.w = 1.0; + ASSERT_TRUE(buffer.setTransform(old_transform, "test", false)); + + geometry_msgs::msg::TransformStamped exact_transform = old_transform; + exact_transform.header.stamp = rclcpp::Time(10, 0, RCL_SYSTEM_TIME); + exact_transform.transform.translation.x = 2.0; + ASSERT_TRUE(buffer.setTransform(exact_transform, "test", false)); + + obstacle_scanner::msg::ObstacleArray snapshot; + snapshot.header.frame_id = "laser_link"; + snapshot.header.stamp = exact_transform.header.stamp; + obstacle_scanner::msg::Obstacle obstacle; + obstacle.center_x = 1.0; + obstacle.center_y = 0.5; + obstacle.radius = 0.04; + snapshot.obstacles.push_back(obstacle); + + const auto transformed = ObstacleArrayLayer::transformSnapshot( + snapshot, "odom", buffer, rclcpp::Duration::from_seconds(0.0)); + + ASSERT_EQ(transformed.size(), 1u); + EXPECT_NEAR(transformed[0].center_x, 3.0, 1e-9); + EXPECT_NEAR(transformed[0].center_y, 0.5, 1e-9); + EXPECT_NEAR(transformed[0].radius, 0.04, 1e-9); +} + +TEST(ObstacleArrayLayerTest, EmptySnapshotDoesNotRequireTransform) +{ + auto clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + obstacle_scanner::msg::ObstacleArray snapshot; + snapshot.header.frame_id = "missing_frame"; + snapshot.header.stamp = rclcpp::Time(10, 0, RCL_SYSTEM_TIME); + + const auto transformed = ObstacleArrayLayer::transformSnapshot( + snapshot, "odom", buffer, rclcpp::Duration::from_seconds(0.0)); + + EXPECT_TRUE(transformed.empty()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + const int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/src/zbw_slamtoolbox/CMakeLists.txt b/src/navigation/zbw_slamtoolbox/CMakeLists.txt similarity index 100% rename from src/zbw_slamtoolbox/CMakeLists.txt rename to src/navigation/zbw_slamtoolbox/CMakeLists.txt diff --git a/src/zbw_slamtoolbox/config/mapper_params_online_async.yaml b/src/navigation/zbw_slamtoolbox/config/mapper_params_online_async.yaml similarity index 100% rename from src/zbw_slamtoolbox/config/mapper_params_online_async.yaml rename to src/navigation/zbw_slamtoolbox/config/mapper_params_online_async.yaml diff --git a/src/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml b/src/navigation/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml similarity index 100% rename from src/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml rename to src/navigation/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml diff --git a/src/zbw_slamtoolbox/config/navigation.yaml b/src/navigation/zbw_slamtoolbox/config/navigation.yaml similarity index 100% rename from src/zbw_slamtoolbox/config/navigation.yaml rename to src/navigation/zbw_slamtoolbox/config/navigation.yaml diff --git a/src/zbw_slamtoolbox/launch/navigation.launch.py b/src/navigation/zbw_slamtoolbox/launch/navigation.launch.py similarity index 100% rename from src/zbw_slamtoolbox/launch/navigation.launch.py rename to src/navigation/zbw_slamtoolbox/launch/navigation.launch.py diff --git a/src/zbw_slamtoolbox/launch/slamtoolbox.launch.py b/src/navigation/zbw_slamtoolbox/launch/slamtoolbox.launch.py similarity index 100% rename from src/zbw_slamtoolbox/launch/slamtoolbox.launch.py rename to src/navigation/zbw_slamtoolbox/launch/slamtoolbox.launch.py diff --git a/src/zbw_slamtoolbox/package.xml b/src/navigation/zbw_slamtoolbox/package.xml similarity index 100% rename from src/zbw_slamtoolbox/package.xml rename to src/navigation/zbw_slamtoolbox/package.xml