预选点数组的添加和发布节点
This commit is contained in:
@@ -16,6 +16,34 @@ find_package(obstacle_scanner REQUIRED)
|
|||||||
find_package(geometry_msgs REQUIRED)
|
find_package(geometry_msgs REQUIRED)
|
||||||
find_package(nav_msgs REQUIRED)
|
find_package(nav_msgs REQUIRED)
|
||||||
find_package(std_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
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
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
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
ament_target_dependencies(trajectory_guard_lib
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
)
|
||||||
|
|
||||||
add_library(obstacle_array_layer SHARED
|
add_library(obstacle_array_layer SHARED
|
||||||
src/obstacle_array_layer.cpp
|
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)
|
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
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
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
|
ARCHIVE DESTINATION lib
|
||||||
LIBRARY DESTINATION lib
|
LIBRARY DESTINATION lib
|
||||||
RUNTIME DESTINATION bin
|
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||||
)
|
)
|
||||||
install(DIRECTORY include/ DESTINATION include)
|
install(DIRECTORY include/ DESTINATION include)
|
||||||
install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME})
|
install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME})
|
||||||
@@ -67,6 +127,27 @@ if(BUILD_TESTING)
|
|||||||
nav_msgs
|
nav_msgs
|
||||||
std_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
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
)
|
||||||
|
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
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
)
|
||||||
|
target_link_libraries(test_trajectory_guard trajectory_guard_lib)
|
||||||
|
ament_target_dependencies(test_trajectory_guard
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
install(DIRECTORY config launch behavior_tree
|
install(DIRECTORY config launch behavior_tree
|
||||||
|
|||||||
17
src/navigation/obstacle_nav2/config/fastdds_udp_only.xml
Normal file
17
src/navigation/obstacle_nav2/config/fastdds_udp_only.xml
Normal file
@@ -0,0 +1,17 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8" ?>
|
||||||
|
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
|
||||||
|
<transport_descriptors>
|
||||||
|
<transport_descriptor>
|
||||||
|
<transport_id>udp_transport</transport_id>
|
||||||
|
<type>UDPv4</type>
|
||||||
|
</transport_descriptor>
|
||||||
|
</transport_descriptors>
|
||||||
|
<participant profile_name="udp_only_participant" is_default_profile="true">
|
||||||
|
<rtps>
|
||||||
|
<useBuiltinTransports>false</useBuiltinTransports>
|
||||||
|
<userTransports>
|
||||||
|
<transport_id>udp_transport</transport_id>
|
||||||
|
</userTransports>
|
||||||
|
</rtps>
|
||||||
|
</participant>
|
||||||
|
</profiles>
|
||||||
155
src/navigation/obstacle_nav2/config/json/test1.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test1.json
Normal file
@@ -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
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/navigation/obstacle_nav2/config/json/test2.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test2.json
Normal file
@@ -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
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/navigation/obstacle_nav2/config/json/test3.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test3.json
Normal file
@@ -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
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/navigation/obstacle_nav2/config/json/test4.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test4.json
Normal file
@@ -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
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/navigation/obstacle_nav2/config/json/test5.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test5.json
Normal file
@@ -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
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
595
src/navigation/obstacle_nav2/config/q_waypoint_selector.py
Normal file
595
src/navigation/obstacle_nav2/config/q_waypoint_selector.py
Normal file
@@ -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())
|
||||||
@@ -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 节。
|
||||||
@@ -19,6 +19,10 @@
|
|||||||
<depend>geometry_msgs</depend>
|
<depend>geometry_msgs</depend>
|
||||||
<depend>nav_msgs</depend>
|
<depend>nav_msgs</depend>
|
||||||
<depend>std_msgs</depend>
|
<depend>std_msgs</depend>
|
||||||
|
<depend>rcl_interfaces</depend>
|
||||||
|
<depend>nav2_msgs</depend>
|
||||||
|
<depend>rclcpp_action</depend>
|
||||||
|
<depend>yaml-cpp</depend>
|
||||||
|
|
||||||
<exec_depend>launch</exec_depend>
|
<exec_depend>launch</exec_depend>
|
||||||
<exec_depend>launch_ros</exec_depend>
|
<exec_depend>launch_ros</exec_depend>
|
||||||
|
|||||||
Reference in New Issue
Block a user