diff --git a/src/navigation/obstacle_nav2/CMakeLists.txt b/src/navigation/obstacle_nav2/CMakeLists.txt index d04e73f..98f94dd 100755 --- a/src/navigation/obstacle_nav2/CMakeLists.txt +++ b/src/navigation/obstacle_nav2/CMakeLists.txt @@ -16,6 +16,34 @@ find_package(obstacle_scanner REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) + +add_library(nav2_profile_loader + src/nav2_profile_loader.cpp +) +target_include_directories(nav2_profile_loader PUBLIC + $ + $ +) +target_link_libraries(nav2_profile_loader yaml-cpp) +ament_target_dependencies(nav2_profile_loader + rclcpp +) + +add_library(trajectory_guard_lib + src/trajectory_guard.cpp +) +target_include_directories(trajectory_guard_lib PUBLIC + $ + $ +) +ament_target_dependencies(trajectory_guard_lib + geometry_msgs + nav_msgs +) add_library(obstacle_array_layer SHARED src/obstacle_array_layer.cpp @@ -39,10 +67,42 @@ ament_target_dependencies(obstacle_array_layer pluginlib_export_plugin_description_file(nav2_costmap_2d obstacle_nav2_plugins.xml) -install(TARGETS obstacle_array_layer +add_executable(nav2_profile_tuner + src/nav2_profile_tuner.cpp +) +target_link_libraries(nav2_profile_tuner nav2_profile_loader) +ament_target_dependencies(nav2_profile_tuner + rclcpp + rcl_interfaces + std_msgs +) + +add_executable(trajectory_guard_node + src/trajectory_guard_node.cpp +) +target_include_directories(trajectory_guard_node PUBLIC + $ + $ +) +target_link_libraries(trajectory_guard_node trajectory_guard_lib) +ament_target_dependencies(trajectory_guard_node + geometry_msgs + nav2_msgs + nav_msgs + rclcpp + rclcpp_action + tf2 +) + +install(TARGETS + nav2_profile_loader + trajectory_guard_lib + obstacle_array_layer + nav2_profile_tuner + trajectory_guard_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY include/ DESTINATION include) install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME}) @@ -67,6 +127,27 @@ if(BUILD_TESTING) nav_msgs std_msgs ) + + ament_add_gtest(test_nav2_profile_loader test/test_nav2_profile_loader.cpp + src/nav2_profile_loader.cpp + ) + target_include_directories(test_nav2_profile_loader PRIVATE + $ + ) + target_link_libraries(test_nav2_profile_loader yaml-cpp) + ament_target_dependencies(test_nav2_profile_loader + rclcpp + ) + + ament_add_gtest(test_trajectory_guard test/test_trajectory_guard.cpp) + target_include_directories(test_trajectory_guard PRIVATE + $ + ) + target_link_libraries(test_trajectory_guard trajectory_guard_lib) + ament_target_dependencies(test_trajectory_guard + geometry_msgs + nav_msgs + ) endif() install(DIRECTORY config launch behavior_tree diff --git a/src/navigation/obstacle_nav2/config/fastdds_udp_only.xml b/src/navigation/obstacle_nav2/config/fastdds_udp_only.xml new file mode 100644 index 0000000..ef2cd77 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/fastdds_udp_only.xml @@ -0,0 +1,17 @@ + + + + + udp_transport + UDPv4 + + + + + false + + udp_transport + + + + diff --git a/src/navigation/obstacle_nav2/config/json/test1.json b/src/navigation/obstacle_nav2/config/json/test1.json new file mode 100644 index 0000000..cbe6d87 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/json/test1.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-07-24T12:43:47.020Z", + "count": 3, + "points": [ + { + "name": "goal_001", + "captured_at": "2026-07-24T12:43:16.569Z", + "received_at": null, + "yaw_degrees": 4.398705354995568, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 1.4830508474576272, + "y": 0.18107786016949148, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.0383765195035876, + "w": 0.99926335004882 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 4.398705354995568 + } + }, + { + "name": "goal_002", + "captured_at": "2026-07-24T12:43:25.804Z", + "received_at": null, + "yaw_degrees": 15.109575122340486, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 3.2468220338983054, + "y": 0.2393405720338984, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.13147417510981546, + "w": 0.9913195959322068 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 15.109575122340486 + } + }, + { + "name": "goal_003", + "captured_at": "2026-07-24T12:43:44.649Z", + "received_at": null, + "yaw_degrees": 64.70797884297878, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.470338983050848, + "y": 1.3357388771186443, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.5351485964909909, + "w": 0.844757941468278 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 64.70797884297878 + } + } + ] +} diff --git a/src/navigation/obstacle_nav2/config/json/test2.json b/src/navigation/obstacle_nav2/config/json/test2.json new file mode 100644 index 0000000..209ec71 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/json/test2.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-07-24T12:44:38.669Z", + "count": 3, + "points": [ + { + "name": "goal_004", + "captured_at": "2026-07-24T12:44:18.861Z", + "received_at": null, + "yaw_degrees": 12.938056317186437, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 1.6472457627118644, + "y": 0.5094676906779659, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.11266611144839717, + "w": 0.9936329037079525 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 12.938056317186437 + } + }, + { + "name": "goal_005", + "captured_at": "2026-07-24T12:44:28.102Z", + "received_at": null, + "yaw_degrees": 22.02305549681122, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 3.1673728813559325, + "y": 0.5677304025423728, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.1910064921196254, + "w": 0.981588773350712 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 22.02305549681122 + } + }, + { + "name": "goal_006", + "captured_at": "2026-07-24T12:44:37.758Z", + "received_at": null, + "yaw_degrees": 55.12467165539786, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.4173728813559325, + "y": 1.394001588983051, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.4627133768932059, + "w": 0.8865079417828619 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 55.12467165539786 + } + } + ] +} diff --git a/src/navigation/obstacle_nav2/config/json/test3.json b/src/navigation/obstacle_nav2/config/json/test3.json new file mode 100644 index 0000000..cd49568 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/json/test3.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-07-24T12:45:16.793Z", + "count": 3, + "points": [ + { + "name": "goal_007", + "captured_at": "2026-07-24T12:44:56.224Z", + "received_at": null, + "yaw_degrees": 10.701350723899163, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 1.461864406779661, + "y": 0.5783236228813559, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.09325122181983936, + "w": 0.9956426113968341 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 10.701350723899163 + } + }, + { + "name": "goal_008", + "captured_at": "2026-07-24T12:45:04.283Z", + "received_at": null, + "yaw_degrees": 20.94747058695114, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 3.0826271186440675, + "y": 0.7319253177966101, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.18178477679916172, + "w": 0.9833383420390354 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 20.94747058695114 + } + }, + { + "name": "goal_009", + "captured_at": "2026-07-24T12:45:15.848Z", + "received_at": null, + "yaw_degrees": 85.03025927188973, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.735169491525424, + "y": 1.3092558262711866, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.6757848709593751, + "w": 0.7370989134318546 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 85.03025927188973 + } + } + ] +} diff --git a/src/navigation/obstacle_nav2/config/json/test4.json b/src/navigation/obstacle_nav2/config/json/test4.json new file mode 100644 index 0000000..3a96d54 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/json/test4.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-07-24T12:47:26.413Z", + "count": 3, + "points": [ + { + "name": "goal_015", + "captured_at": "2026-07-24T12:47:14.682Z", + "received_at": null, + "yaw_degrees": 17.709724523862732, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 1.5625, + "y": 0.7001456567796612, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.15393202146824025, + "w": 0.9880814403513009 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 17.709724523862732 + } + }, + { + "name": "goal_016", + "captured_at": "2026-07-24T12:47:18.969Z", + "received_at": null, + "yaw_degrees": 22.036226940145426, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 2.9978813559322033, + "y": 0.9490863347457628, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.1911193171514086, + "w": 0.9815668120976683 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 22.036226940145426 + } + }, + { + "name": "goal_017", + "captured_at": "2026-07-24T12:47:24.482Z", + "received_at": null, + "yaw_degrees": 75.9637565320735, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.523305084745763, + "y": 1.3304422669491527, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.6154122094026355, + "w": 0.7882054380161093 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 75.9637565320735 + } + } + ] +} diff --git a/src/navigation/obstacle_nav2/config/json/test5.json b/src/navigation/obstacle_nav2/config/json/test5.json new file mode 100644 index 0000000..98f080c --- /dev/null +++ b/src/navigation/obstacle_nav2/config/json/test5.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-07-24T12:48:05.057Z", + "count": 3, + "points": [ + { + "name": "goal_019", + "captured_at": "2026-07-24T12:47:52.274Z", + "received_at": null, + "yaw_degrees": 9.130176482278682, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 1.4088983050847457, + "y": 0.1387049788135596, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.0795915470489837, + "w": 0.9968275606334074 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 9.130176482278682 + } + }, + { + "name": "goal_020", + "captured_at": "2026-07-24T12:47:56.654Z", + "received_at": null, + "yaw_degrees": 15.2551187030578, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 3.0614406779661016, + "y": 0.18637447033898297, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.1327331510254088, + "w": 0.9911518100769761 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 15.2551187030578 + } + }, + { + "name": "goal_021", + "captured_at": "2026-07-24T12:48:03.562Z", + "received_at": null, + "yaw_degrees": 89.99999999999986, + "odom": { + "header": { + "frame_id": "map", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.703389830508474, + "y": 1.3516287076271185, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.7071067811865467, + "w": 0.7071067811865485 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 89.99999999999986 + } + } + ] +} diff --git a/src/navigation/obstacle_nav2/config/q_waypoint_selector.py b/src/navigation/obstacle_nav2/config/q_waypoint_selector.py new file mode 100644 index 0000000..ce2793b --- /dev/null +++ b/src/navigation/obstacle_nav2/config/q_waypoint_selector.py @@ -0,0 +1,595 @@ +#!/usr/bin/env python3 +"""Q_i candidate waypoint selector for Nav2 NavigateToPose. + +用途 +---- +这个节点用于比赛/实车场景下的多路线候选导航。它会从同一个 JSON +目录中读取多个 ``*.json`` 文件。每个 JSON 文件表示一条完整路线，文件 +中的 ``points`` 数组表示该路线按顺序经过的导航点。 + +多 JSON 编组规则 +-------------- +假设目录中存在多个文件： + + test1.json: p_0, p_1, p_2, ... + test2.json: p_0, p_1, p_2, ... + test3.json: p_0, p_1, p_2, ... + +节点会把每个文件的第 i 个点组合成 Q_i： + + Q_0 = [test1[0], test2[0], test3[0], ...] + Q_1 = [test1[1], test2[1], test3[1], ...] + Q_2 = [test1[2], test2[2], test3[2], ...] + +文件按自然顺序排序，因此 ``test2.json`` 会排在 ``test10.json`` 前面。 +如果某条路线点数较少，缺失的索引会被跳过，不影响其他路线候选。 + +JSON 格式 +--------- +推荐格式为： + + { + "points": [ + { + "name": "goal_001", + "yaw_degrees": 4.39, + "odom": { + "pose": { + "pose": { + "position": {"x": 1.48, "y": 0.18, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.03, "w": 0.99} + } + } + } + } + ] + } + +同时也支持顶层直接是 list 的 JSON。坐标 ``x/y`` 必须与 ``goal_frame`` +一致，例如 ``odom`` 或 ``odom_combined``。 + +候选点选择规则 +-------------- +1. 第一次优先选择第 0 条路线，也就是自然排序后的第一个 JSON 文件。 +2. 到达某个 Q_i 后，进入 Q_{i+1} 时优先保持同一条路线。 +3. 如果该候选点在全局 costmap 中不可用，则按路线顺序循环尝试下一个候选。 +4. 如果某个 Q_i 中所有候选都不可用，则跳过该 Q，进入 Q_{i+1}。 +5. 如果 Nav2 action 被拒绝、失败、取消或超时，则当前候选在本轮 Q 中 + 被标记为失败，下一轮 tick 会尝试下一个候选。 + +不可用判断 +--------- +发送目标前会查询 ``/global_costmap/costmap``： + +* 点在 costmap 外：不可用。 +* costmap 数据异常：不可用。 +* cost >= ``occupied_threshold``：不可用。 +* cost = -1：由 ``treat_unknown_as_occupied`` 决定。 + +当前默认适配无静态地图场景：``treat_unknown_as_occupied=False``，因此未知 +区域不会直接导致候选点被拒绝；真正障碍仍由 cost 阈值过滤。 + +关键参数 +-------- +* ``json_dir``: JSON 路线目录。 +* ``goal_frame``: 下发 NavigateToPose 目标使用的坐标系，默认 ``odom``。 +* ``costmap_topic``: 用于判断候选点占用状态的 costmap，默认 + ``/global_costmap/costmap``。 +* ``navigate_action``: Nav2 action 名称，默认 ``/navigate_to_pose``。 +* ``occupied_threshold``: costmap 占用阈值，默认 50。 +* ``treat_unknown_as_occupied``: 是否把 cost=-1 视为不可通行，默认 False。 +* ``goal_timeout_sec``: 单个候选目标超时时间，默认 180 秒。 +* ``selection_period_sec``: 主循环周期，默认 0.5 秒。 +""" + +from __future__ import annotations + +import json +import math +import re +import sys +import traceback +from dataclasses import dataclass +from pathlib import Path +from typing import Dict, List, Optional, Sequence, Set + +ROS_IMPORT_ERROR: Optional[BaseException] = None + +try: + import rclpy + from geometry_msgs.msg import PoseStamped + from nav2_msgs.action import NavigateToPose + from nav_msgs.msg import OccupancyGrid + from rclpy.action import ActionClient + from rclpy.duration import Duration + from rclpy.node import Node + from rclpy.qos import QoSProfile +except BaseException as exc: # pragma: no cover - used for remote runtime diagnosis + ROS_IMPORT_ERROR = exc + rclpy = None # type: ignore[assignment] + PoseStamped = object # type: ignore[assignment] + NavigateToPose = object # type: ignore[assignment] + OccupancyGrid = object # type: ignore[assignment] + ActionClient = object # type: ignore[assignment] + Duration = object # type: ignore[assignment] + Node = object # type: ignore[assignment] + QoSProfile = object # type: ignore[assignment] + + +@dataclass(frozen=True) +class CandidateWaypoint: + q_index: int + route_index: int + file_name: str + point_name: str + x: float + y: float + yaw: float + + +def yaw_from_quaternion(q: Dict[str, float]) -> float: + x = float(q.get("x", 0.0)) + y = float(q.get("y", 0.0)) + z = float(q.get("z", 0.0)) + w = float(q.get("w", 1.0)) + return math.atan2( + 2.0 * (w * z + x * y), + 1.0 - 2.0 * (y * y + z * z), + ) + + +def pose_from_json_point(point: Dict, q_index: int, route_index: int, file_name: str) -> CandidateWaypoint: + odom = point.get("odom", {}) + pose = odom.get("pose", {}).get("pose", {}) + position = pose.get("position", {}) + orientation = pose.get("orientation", {}) + + x = float(position["x"]) + y = float(position["y"]) + yaw_degrees = point.get("yaw_degrees", None) + if yaw_degrees is None: + yaw = yaw_from_quaternion(orientation) + else: + yaw = math.radians(float(yaw_degrees)) + + return CandidateWaypoint( + q_index=q_index, + route_index=route_index, + file_name=file_name, + point_name=str(point.get("name", f"{Path(file_name).stem}_{q_index}")), + x=x, + y=y, + yaw=yaw, + ) + + +def natural_json_sort_key(path: Path) -> List[object]: + return [ + int(part) if part.isdigit() else part + for part in re.split(r"(\d+)", path.name.lower()) + ] + + +def load_points_from_json(path: Path) -> List[Dict]: + try: + data = json.loads(path.read_text(encoding="utf-8")) + except Exception as exc: + raise RuntimeError(f"failed to parse JSON file {path}: {exc}") from exc + + if isinstance(data, list): + points = data + elif isinstance(data, dict): + points = data.get("points", []) + else: + raise RuntimeError( + f"{path} must contain either a top-level list or a dict field named 'points'" + ) + + if not isinstance(points, list): + raise RuntimeError(f"{path} does not contain a list field named 'points'") + return points + + +def load_q_groups(json_dir: Path) -> tuple[List[List[CandidateWaypoint]], List[str], List[int]]: + if not json_dir.exists(): + raise RuntimeError(f"JSON directory does not exist: {json_dir}") + if not json_dir.is_dir(): + raise RuntimeError(f"JSON path is not a directory: {json_dir}") + + files = sorted(json_dir.glob("*.json"), key=natural_json_sort_key) + if not files: + raise RuntimeError(f"no JSON files found in {json_dir}") + + routes: List[List[Dict]] = [] + for path in files: + routes.append(load_points_from_json(path)) + + max_points = max(len(route) for route in routes) + groups: List[List[CandidateWaypoint]] = [] + for q_index in range(max_points): + group: List[CandidateWaypoint] = [] + for route_index, points in enumerate(routes): + if q_index >= len(points): + continue + group.append( + pose_from_json_point( + points[q_index], + q_index=q_index, + route_index=route_index, + file_name=files[route_index].name, + ) + ) + groups.append(group) + return groups, [path.name for path in files], [len(route) for route in routes] + + +class QWaypointSelector(Node): + def __init__(self) -> None: + super().__init__("q_waypoint_selector") + + self._declare_parameter_if_needed("use_sim_time", False) + self._declare_parameter_if_needed("json_dir", "") + self._declare_parameter_if_needed("goal_frame", "odom") + self._declare_parameter_if_needed("costmap_topic", "/global_costmap/costmap") + self._declare_parameter_if_needed("navigate_action", "/navigate_to_pose") + self._declare_parameter_if_needed("occupied_threshold", 50) + self._declare_parameter_if_needed("treat_unknown_as_occupied", False) + self._declare_parameter_if_needed("goal_timeout_sec", 180.0) + self._declare_parameter_if_needed("selection_period_sec", 0.5) + + json_dir = Path(str(self.get_parameter("json_dir").value)).expanduser() + if not json_dir.is_absolute(): + json_dir = Path.cwd() / json_dir + self.goal_frame = str(self.get_parameter("goal_frame").value) + self.costmap_topic = str(self.get_parameter("costmap_topic").value) + self.navigate_action = str(self.get_parameter("navigate_action").value) + self.occupied_threshold = int(self.get_parameter("occupied_threshold").value) + self.treat_unknown_as_occupied = bool( + self.get_parameter("treat_unknown_as_occupied").value + ) + self.goal_timeout_sec = max( + 1.0, float(self.get_parameter("goal_timeout_sec").value) + ) + self.selection_period_sec = max( + 0.1, float(self.get_parameter("selection_period_sec").value) + ) + + self.q_groups, self.route_file_names, self.route_point_counts = load_q_groups(json_dir) + self.current_q_index = 0 + self.preferred_route_index = 0 + self.rejected_routes_for_current_q: Set[int] = set() + self.active_candidate: Optional[CandidateWaypoint] = None + self.active_goal_handle = None + self.active_goal_start_time = None + self.active_goal_token: Optional[int] = None + self.next_goal_token = 0 + self.done = False + self.latest_costmap: Optional[OccupancyGrid] = None + self.last_costmap_wait_log_ns = 0 + + qos = QoSProfile(depth=1) + self.costmap_sub = self.create_subscription( + OccupancyGrid, self.costmap_topic, self._costmap_callback, qos + ) + self.action_client = ActionClient(self, NavigateToPose, self.navigate_action) + self.timer = self.create_timer(self.selection_period_sec, self._tick) + + self.get_logger().info( + "loaded Q waypoint groups " + f"json_dir={json_dir} groups={len(self.q_groups)} " + f"routes={len(self.route_file_names)} " + f"goal_frame={self.goal_frame} costmap_topic={self.costmap_topic}" + ) + route_summary = ", ".join( + f"{name}:{count}" + for name, count in zip(self.route_file_names, self.route_point_counts) + ) + self.get_logger().info(f"loaded JSON waypoint routes {route_summary}") + + def _declare_parameter_if_needed(self, name: str, default_value) -> None: + if not self.has_parameter(name): + self.declare_parameter(name, default_value) + + def _costmap_callback(self, msg: OccupancyGrid) -> None: + self.latest_costmap = msg + + def _log_waiting_for_costmap(self) -> None: + now_ns = self.get_clock().now().nanoseconds + if now_ns - self.last_costmap_wait_log_ns < 2_000_000_000: + return + self.last_costmap_wait_log_ns = now_ns + + publishers = self.get_publishers_info_by_topic(self.costmap_topic) + if not publishers: + self.get_logger().warn( + f"waiting for costmap {self.costmap_topic}; publisher_count=0; " + "no goal will be sent yet" + ) + return + + details = [] + for info in publishers[:3]: + details.append( + f"{info.node_namespace}/{info.node_name} " + f"type={info.topic_type} qos={info.qos_profile}" + ) + more = "" if len(publishers) <= 3 else f" ... +{len(publishers) - 3} more" + self.get_logger().warn( + f"waiting for costmap {self.costmap_topic}; " + f"publisher_count={len(publishers)} publishers={details}{more}; " + "no goal will be sent yet" + ) + + def _tick(self) -> None: + if self.done: + return + + if self.current_q_index >= len(self.q_groups): + self.done = True + self.get_logger().info("all Q waypoint groups completed") + return + + if not self.action_client.server_is_ready(): + self.get_logger().info( + f"waiting for NavigateToPose action server {self.navigate_action}" + ) + self.action_client.wait_for_server(timeout_sec=0.1) + return + + if self.latest_costmap is None: + self._log_waiting_for_costmap() + return + + if self.active_goal_handle is not None: + if self._active_goal_timed_out(): + self.get_logger().warn( + "active goal timed out; cancelling and trying next candidate " + f"q={self.current_q_index} candidate={self._candidate_label(self.active_candidate)}" + ) + cancel_future = self.active_goal_handle.cancel_goal_async() + cancel_future.add_done_callback(lambda _: None) + self._reject_active_candidate() + return + + candidate = self._select_candidate() + if candidate is None: + self.get_logger().warn( + f"all candidates in Q_{self.current_q_index} are occupied/unusable; skipping group" + ) + self.current_q_index += 1 + self.rejected_routes_for_current_q.clear() + return + + self._send_goal(candidate) + + def _select_candidate(self) -> Optional[CandidateWaypoint]: + group = self.q_groups[self.current_q_index] + if not group: + return None + + route_to_candidate = {candidate.route_index: candidate for candidate in group} + ordered_route_indices = self._cyclic_route_order( + sorted(route_to_candidate.keys()), self.preferred_route_index + ) + + for route_index in ordered_route_indices: + if route_index in self.rejected_routes_for_current_q: + continue + candidate = route_to_candidate[route_index] + occupied, reason = self._candidate_occupied(candidate) + if occupied: + self.rejected_routes_for_current_q.add(route_index) + self.get_logger().info( + f"skip occupied candidate Q_{candidate.q_index} " + f"{self._candidate_label(candidate)} reason={reason}" + ) + continue + return candidate + return None + + @staticmethod + def _cyclic_route_order(route_indices: Sequence[int], preferred: int) -> List[int]: + if not route_indices: + return [] + if preferred not in route_indices: + preferred = route_indices[0] + start = route_indices.index(preferred) + return list(route_indices[start:]) + list(route_indices[:start]) + + def _candidate_occupied(self, candidate: CandidateWaypoint) -> tuple[bool, str]: + costmap = self.latest_costmap + if costmap is None: + return True, "no_costmap" + + origin = costmap.info.origin + resolution = costmap.info.resolution + width = int(costmap.info.width) + height = int(costmap.info.height) + if resolution <= 0.0 or width <= 0 or height <= 0: + return True, "invalid_costmap" + + origin_yaw = yaw_from_quaternion( + { + "x": origin.orientation.x, + "y": origin.orientation.y, + "z": origin.orientation.z, + "w": origin.orientation.w, + } + ) + dx = candidate.x - origin.position.x + dy = candidate.y - origin.position.y + cos_yaw = math.cos(-origin_yaw) + sin_yaw = math.sin(-origin_yaw) + local_x = cos_yaw * dx - sin_yaw * dy + local_y = sin_yaw * dx + cos_yaw * dy + mx = int(math.floor(local_x / resolution)) + my = int(math.floor(local_y / resolution)) + + if mx < 0 or my < 0 or mx >= width or my >= height: + return True, f"outside_costmap cell=({mx},{my})" + + index = my * width + mx + if index < 0 or index >= len(costmap.data): + return True, "costmap_data_too_short" + + cost = int(costmap.data[index]) + if cost < 0: + return self.treat_unknown_as_occupied, f"unknown cost={cost}" + if cost >= self.occupied_threshold: + return True, f"occupied cost={cost}" + return False, f"free cost={cost}" + + def _send_goal(self, candidate: CandidateWaypoint) -> None: + goal_msg = NavigateToPose.Goal() + goal_msg.pose = self._to_pose_stamped(candidate) + self.next_goal_token += 1 + token = self.next_goal_token + self.active_candidate = candidate + self.active_goal_start_time = self.get_clock().now() + self.active_goal_token = token + + self.get_logger().info( + f"send Q_{candidate.q_index} candidate {self._candidate_label(candidate)} " + f"pose=({candidate.x:.3f},{candidate.y:.3f},{candidate.yaw:.3f})" + ) + send_future = self.action_client.send_goal_async(goal_msg) + send_future.add_done_callback( + lambda future, goal_token=token: self._goal_response_callback(future, goal_token) + ) + + def _to_pose_stamped(self, candidate: CandidateWaypoint) -> PoseStamped: + pose = PoseStamped() + pose.header.frame_id = self.goal_frame + pose.header.stamp = self.get_clock().now().to_msg() + pose.pose.position.x = candidate.x + pose.pose.position.y = candidate.y + pose.pose.position.z = 0.0 + pose.pose.orientation.z = math.sin(candidate.yaw * 0.5) + pose.pose.orientation.w = math.cos(candidate.yaw * 0.5) + return pose + + def _goal_response_callback(self, future, goal_token: int) -> None: + if goal_token != self.active_goal_token: + return + try: + goal_handle = future.result() + except Exception as exc: + self.get_logger().error( + "NavigateToPose send_goal_async failed; try next candidate: " + f"{type(exc).__name__}: {exc}" + ) + self._reject_active_candidate() + return + if not goal_handle.accepted: + self.get_logger().warn( + f"goal rejected by Nav2 {self._candidate_label(self.active_candidate)}" + ) + self._reject_active_candidate() + return + + self.active_goal_handle = goal_handle + result_future = goal_handle.get_result_async() + result_future.add_done_callback( + lambda future, token=goal_token: self._goal_result_callback(future, token) + ) + + def _goal_result_callback(self, future, goal_token: int) -> None: + if goal_token != self.active_goal_token: + return + try: + result = future.result() + except Exception as exc: + self.get_logger().error( + "NavigateToPose result failed; try next candidate: " + f"{type(exc).__name__}: {exc}" + ) + self._reject_active_candidate() + return + candidate = self.active_candidate + status = int(result.status) + + if status == 4: + if candidate is not None: + self.preferred_route_index = candidate.route_index + self.get_logger().info( + f"reached Q_{candidate.q_index} {self._candidate_label(candidate)}; " + f"advance to Q_{candidate.q_index + 1}" + ) + self.current_q_index += 1 + self.rejected_routes_for_current_q.clear() + self.active_candidate = None + self.active_goal_handle = None + self.active_goal_start_time = None + self.active_goal_token = None + return + + self.get_logger().warn( + f"goal failed status={status}; try next candidate " + f"{self._candidate_label(candidate)}" + ) + self._reject_active_candidate() + + def _active_goal_timed_out(self) -> bool: + if self.active_goal_start_time is None: + return False + elapsed = self.get_clock().now() - self.active_goal_start_time + return elapsed > Duration(seconds=self.goal_timeout_sec) + + def _reject_active_candidate(self) -> None: + if self.active_candidate is not None: + self.rejected_routes_for_current_q.add(self.active_candidate.route_index) + self.active_candidate = None + self.active_goal_handle = None + self.active_goal_start_time = None + self.active_goal_token = None + + @staticmethod + def _candidate_label(candidate: Optional[CandidateWaypoint]) -> str: + if candidate is None: + return "none" + return ( + f"route={candidate.route_index} file={candidate.file_name} " + f"name={candidate.point_name}" + ) + + +def main() -> int: + if ROS_IMPORT_ERROR is not None: + print( + "FATAL q_waypoint_selector: failed to import ROS/Nav2 Python modules.", + file=sys.stderr, + ) + traceback.print_exception( + type(ROS_IMPORT_ERROR), + ROS_IMPORT_ERROR, + ROS_IMPORT_ERROR.__traceback__, + file=sys.stderr, + ) + print( + "Hint: source /opt/ros/humble/setup.bash and install/source Nav2 " + "packages, especially nav2_msgs.", + file=sys.stderr, + ) + return 1 + + rclpy.init() + node = None + try: + node = QWaypointSelector() + rclpy.spin(node) + except KeyboardInterrupt: + pass + except BaseException: + print("FATAL q_waypoint_selector: unhandled exception", file=sys.stderr) + traceback.print_exc(file=sys.stderr) + return 1 + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/navigation/obstacle_nav2/config/q_waypoint_selector_manual.md b/src/navigation/obstacle_nav2/config/q_waypoint_selector_manual.md new file mode 100644 index 0000000..16bf916 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/q_waypoint_selector_manual.md @@ -0,0 +1,270 @@ +# Q 路点选择器 使用手册 + +## 1. 概述 + +`q_waypoint_selector.py` 是一个独立的 ROS 2 Python 节点，用于比赛场景下的**自动有序路点导航**。它从 JSON 文件中加载多路线、多候选的路点坐标，结合全局 costmap 实时判断候选点是否可通行，并通过 Nav2 的 `NavigateToPose` action 下发导航目标，依次逐个完成所有路点。 + +### 核心能力 + +- **多路线冗余**：同一个路点序号可配置多个候选坐标（来自不同 JSON 文件），一个被障碍物占据时自动切换 +- **costmap 感知**：下发目标前先检查该坐标在全局 costmap 上是否被占用，避免朝障碍物导航 +- **路线偏好记忆**：成功后记住当前路线编号，后续 Q 优先选择同一条路线 +- **超时自动切换**：单个目标超时后自动取消，尝试下一个候选 + +--- + +## 2. 路点 JSON 文件格式 + +### 目录约定 + +所有 JSON 文件放在同一目录下，文件名按字母序排列。节点加载时： + +``` +json_dir/ +├── test1.json # 路线 0 +├── test2.json # 路线 1 +├── test3.json # 路线 2 +└── ... +``` + +每个 JSON 文件代表一条**完整路线**，包含有序的路点列表。 + +### JSON 结构 + +```json +{ + "points": [ + { + "name": "goal_001", + "yaw_degrees": 4.39, + "odom": { + "pose": { + "pose": { + "position": { "x": 1.48, "y": 0.18, "z": 0.0 }, + "orientation": { "x": 0.0, "y": 0.0, "z": 0.03, "w": 0.99 } + } + } + } + }, + { + "name": "goal_002", + "yaw_degrees": 15.10, + "odom": { + "pose": { + "pose": { + "position": { "x": 3.24, "y": 0.23, "z": 0.0 }, + "orientation": { "x": 0.0, "y": 0.0, "z": 0.13, "w": 0.99 } + } + } + } + } + ] +} +``` + +### 字段说明 + +| 字段 | 类型 | 必需 | 说明 | +|------|------|------|------| +| `points` | array | 是 | 有序路点数组，按索引 0, 1, 2... 依次导航 | +| `points[i].name` | string | 否 | 路点名称（日志显示用，缺省为 `{文件名}_{索引}`） | +| `points[i].yaw_degrees` | number | 否 | 目标朝向角（度），优先级高于 orientation 四元数 | +| `points[i].odom.pose.pose.position` | object | 是 | 路点坐标 `{x, y, z}` | +| `points[i].odom.pose.pose.orientation` | object | 否 | 目标朝向四元数 `{x, y, z, w}`，仅在 `yaw_degrees` 缺失时使用 | + +> **注意**：`position.x` 和 `position.y` 必须与 `goal_frame` 参数指定的坐标系一致（如 `odom` 或 `odom_combined`）。 + +--- + +## 3. Q 路点组概念 + +### 路点编组规则 + +假设有 2 个 JSON 文件，经过编组后： + +``` +test1.json: [p0, p1, p2] (路线 0) +test2.json: [p0, p1, p2] (路线 1) + ↓ 转置编组 +Q_0 = [test1[0], test2[0]] ← 两个候选路点，指向同一目标位置的不同路线 +Q_1 = [test1[1], test2[1]] +Q_2 = [test1[2], test2[2]] +``` + +**路径数不同时的处理**：编组时以最长路线为准，较短路线在对应索引上缺失的点被跳过，不会影响其他路线的候选。 + +### 候选选择策略 + +节点在每个 Q 组内按以下优先级选择候选： + +1. **偏好路线优先**（`preferred_route_index`）：上次导航成功的路线编号 +2. **循环顺序**：若偏好路线不可用，按路线编号循环尝试 +3. **costmap 过滤**：被 costmap 判定为占用的候选直接跳过 +4. **已拒绝列表**：本次 Q 内已失败/被拒绝的候选不再重试 + +--- + +## 4. 运行流程 + +``` +启动节点 + │ + ├─ 加载 json_dir 下所有 .json → 编组为 Q_0, Q_1, ... + ├─ 等待 NavigateToPose action server 就绪 + ├─ 等待 costmap 话题有数据 + │ + └─ 定时器循环 (selection_period_sec) + │ + ├─ 所有 Q 完成? → 退出 + │ + ├─ 有活跃 goal ? + │ ├─ 进行中 → 等待 + │ └─ 超时 → 取消 goal，拒绝当前候选，下一轮切换到下一个候选 + │ + ├─ 从 Q_current 选择候选 + │ ├─ 有可用候选 → 通过 NavigateToPose 下发 + │ └─ 全部被占用 → 跳过当前 Q，前进到 Q_{current+1} + │ + └─ goal 结果回调 + ├─ status=4 (SUCCEEDED) → 记录路线偏好，前进到下一 Q + └─ 其他 (失败/拒绝) → 拒绝当前候选，下次 tick 换候选 +``` + +--- + +## 5. 参数说明 + +### 启动参数 + +| 参数 | 类型 | 默认值 | 说明 | +|------|------|--------|------| +| `json_dir` | string | `obstacle_nav2/config/json` | JSON 路点文件目录 | +| `goal_frame` | string | `odom` | 下发导航目标使用的坐标帧，实车应设为 `odom_combined` | +| `costmap_topic` | string | `/global_costmap/costmap` | 全局 costmap 话题，用于避占检测 | +| `navigate_action` | string | `/navigate_to_pose` | Nav2 的 NavigateToPose action 名称 | +| `occupied_threshold` | int | `50` | costmap 占用阈值 (0-100)，栅格值 >= 此值视为被占据 | +| `treat_unknown_as_occupied` | bool | `true` | 是否将未知区域 (costmap 值 = -1) 视为不可通行 | +| `goal_timeout_sec` | float | `180.0` | 单个导航目标超时时间（秒），取值范围 [1.0, ∞) | +| `selection_period_sec` | float | `0.5` | 选择器主循环周期（秒），取值范围 [0.1, ∞) | +| `use_sim_time` | bool | `false` | 是否使用仿真时间 | + +### 参数调优建议 + +| 场景 | 建议设置 | +|------|---------| +| 路点间距大（>10m） | `goal_timeout_sec` 适当增大（如 300s） | +| 障碍物密集区域 | `occupied_threshold` 降低（如 30），更保守地避让 | +| 动态障碍物多 | `selection_period_sec` 设短（如 0.2s），更快响应 costmap 变化 | +| costmap 覆盖不全 | `treat_unknown_as_occupied` 视情况关闭，允许向未知区域导航 | + +--- + +## 6. 启动方式 + +### 方式 A：独立启动（我使用的是这个，测试成功 + +```bash +python3 q_waypoint_selector.py \ + --ros-args \ + -p json_dir:=/home/user/waypoints \ + -p goal_frame:=odom_combined \ + -p costmap_topic:=/global_costmap/costmap \ + -p goal_timeout_sec:=120.0 +``` + +### 方式 B：通过 launch 文件启动 + +```bash +ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \ + use_sim_time:=false \ + global_frame:=odom_combined \ + json_dir:=/home/user/waypoints \ + goal_frame:=odom_combined \ + goal_timeout_sec:=120.0 \ + occupied_threshold:=50 +``` + +这个 launch 文件会**同时启动**：底盘驱动、激光雷达、obstacle_scanner、Nav2 导航栈 和 Q 路点选择器。 + +### 方式 C：在已有导航栈上单独启动 + +```bash +ros2 run obstacle_nav2 q_waypoint_selector.py \ + --ros-args \ + -p json_dir:=/home/user/waypoints \ + -p goal_frame:=odom_combined +``` + +--- + +## 7. 运行日志解读 + +### 正常日志 + +``` +[INFO] loaded Q waypoint groups json_dir=/... groups=5 routes=3 goal_frame=odom +``` +启动成功，加载了 5 个 Q 组，每条 Q 最多 3 个候选路线。 + +``` +[INFO] send Q_0 candidate route=0 file=test1.json name=goal_001 pose=(1.480,0.180,0.076) +``` +下发了 Q_0 的第一个候选路点。 + +``` +[INFO] reached Q_0 route=0 file=test1.json name=goal_001; advance to Q_1 +``` +到达目标，记录路线偏好并进入下一路点。 + +### 异常日志及处理 + +| 日志 | 含义 | 处理方式 | +|------|------|---------| +| `waiting for NavigateToPose action server` | Nav2 未启动或未就绪 | 等待 nav2_bringup 完成启动 | +| `waiting for costmap; publisher_count=0` | costmap 话题无发布者 | 检查 global_costmap 节点是否启动 | +| `skip occupied candidate ... reason=occupied cost=100` | 候选路点栅格被障碍物占据 | 自动尝试下一个候选路线 | +| `skip occupied candidate ... reason=outside_costmap` | 候选路点超出当前 costmap 范围 | 增大 costmap 尺寸或调整路点坐标 | +| `active goal timed out; cancelling` | 导航目标超时 | 自动取消并切换候选，可能需要增大 `goal_timeout_sec` | +| `all candidates in Q_N are occupied; skipping` | 该 Q 所有候选均不可用 | 自动跳过到下一个 Q | +| `all Q waypoint groups completed` | 所有路点已完成 | 正常退出信号 | + +--- + +## 8. Nav2 goal 状态码参考 + +节点根据 `NavigateToPose` action 的结果状态码决定下一步行为： + +| status | 含义 | 节点行为 | +|--------|------|---------| +| 0 | UNKNOWN | 拒绝当前候选，换下一个 | +| 1 | ACCEPTED | (中间状态，不触发回调) | +| 2 | EXECUTING | (中间状态，不触发回调) | +| 3 | CANCELING | (中间状态，不触发回调) | +| 4 | **SUCCEEDED** | 记录路线偏好，前进到下一 Q | +| 5 | CANCELED | 拒绝当前候选，换下一个 | +| 6 | ABORTED | 拒绝当前候选，换下一个 | + +--- + +## 9. 常见问题 + +**Q: 节点启动后一直等待 costmap，不发送目标？** + +检查 `costmap_topic` 参数是否与实际话题一致（默认 `/global_costmap/costmap`）： +```bash +ros2 topic list | grep costmap +``` + +**Q: 所有候选路点都被判定为占用？** + +1. 确认路点坐标与 `goal_frame` 坐标系一致 +2. 检查 `occupied_threshold` 是否过低（costmap 中膨胀区域可能为 50-100） +3. 尝试设置 `treat_unknown_as_occupied:=false` 如果 costmap 覆盖不全 + +**Q: 机器人到达路点后没有立即进入下一个？** + +节点的定时器周期为 `selection_period_sec`（默认 0.5s），最坏情况下需要等待一个周期。如果 goal 结果回调已触发，下一个 tick 就会下发新目标。 + +**Q: 如何录制路点 JSON 文件？** + +路点 JSON 可以通过监听 `/odom_combined` (实车) 或 `/odom` (仿真) 话题并手动记录坐标生成。JSON 格式参见第 2 节。 diff --git a/src/navigation/obstacle_nav2/package.xml b/src/navigation/obstacle_nav2/package.xml index 8a1c2fc..d959894 100755 --- a/src/navigation/obstacle_nav2/package.xml +++ b/src/navigation/obstacle_nav2/package.xml @@ -19,6 +19,10 @@ geometry_msgs nav_msgs std_msgs + rcl_interfaces + nav2_msgs + rclcpp_action + yaml-cpp launch launch_ros