diff --git a/README.md b/README.md index 424348f2db1..2eea3cbeaee 100644 --- a/README.md +++ b/README.md @@ -113,38 +113,39 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this ## Build Status -| Package | humble Source | humble Debian | iron Source | iron Debian | jazzy Source | jazzy Debian | -| :---: | :---: | :---: | :---: | :---: | :---: | :---: | -| navigation2 | [![Build Status](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/) | -| nav2_amcl | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) | -| nav2_behavior_tree | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) | -| nav2_behaviors | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) | -| nav2_bringup | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) | -| nav2_bt_navigator | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) | -| nav2_collision_monitor | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) | -| nav2_common | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/) | -| nav2_constrained_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) | -| nav2_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) | -| nav2_core | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/) | -| nav2_costmap_2d | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) | -| nav2_docking | [![Build Status](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) | -| nav2_dwb_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) | -| nav2_graceful_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) | -| nav2_lifecycle_manager | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | -| nav2_map_server | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) | -| nav2_mppi_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) | -| nav2_msgs | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) | -| nav2_navfn_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) | -| nav2_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) | -| nav2_regulated_pure_pursuit | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) | -| nav2_rotation_shim_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | -| nav2_rviz_plugins | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) | -| nav2_simple_commander | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) | -| nav2_smac_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) | -| nav2_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) | -| nav2_system_tests | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) | -| nav2_theta_star_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) | -| nav2_util | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/) | -| nav2_velocity_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) | -| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) | -| nav2_waypoint_follower | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | +| Package | humble Source | humble Debian | jazzy Source | jazzy Debian | +| :---: | :---: | :---: | :---: | :---: | +| navigation2 | [![Build Status](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/) | +| nav2_amcl | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) | +| nav2_behavior_tree | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) | +| nav2_behaviors | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) | +| nav2_bringup | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) | +| nav2_bt_navigator | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) | +| nav2_collision_monitor | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) | +| nav2_common | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/) | +| nav2_constrained_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) | +| nav2_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) | +| nav2_core | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/) | +| nav2_costmap_2d | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) | +| nav2_docking | [![Build Status](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) | +| nav2_dwb_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) | +| nav2_graceful_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) | +| nav2_lifecycle_manager | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | +| nav2_loopback_sim | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/) | +| nav2_map_server | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) | +| nav2_mppi_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) | +| nav2_msgs | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) | +| nav2_navfn_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) | +| nav2_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) | +| nav2_regulated_pure_pursuit | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) | +| nav2_rotation_shim_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | +| nav2_rviz_plugins | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) | +| nav2_simple_commander | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) | +| nav2_smac_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) | +| nav2_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) | +| nav2_system_tests | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) | +| nav2_theta_star_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) | +| nav2_util | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/) | +| nav2_velocity_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) | +| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) | +| nav2_waypoint_follower | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 3aaec265445..2a4d3ad90cf 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -104,6 +104,9 @@ list(APPEND plugin_libs nav2_transform_available_condition_bt_node) add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp) list(APPEND plugin_libs nav2_goal_reached_condition_bt_node) +add_library(nav2_are_poses_near_condition_bt_node SHARED plugins/condition/are_poses_near_condition.cpp) +list(APPEND plugin_libs nav2_are_poses_near_condition_bt_node) + add_library(nav2_globally_updated_goal_condition_bt_node SHARED plugins/condition/globally_updated_goal_condition.cpp) list(APPEND plugin_libs nav2_globally_updated_goal_condition_bt_node) @@ -143,6 +146,9 @@ list(APPEND plugin_libs nav2_would_a_planner_recovery_help_condition_bt_node) add_library(nav2_would_a_smoother_recovery_help_condition_bt_node SHARED plugins/condition/would_a_smoother_recovery_help_condition.cpp) list(APPEND plugin_libs nav2_would_a_smoother_recovery_help_condition_bt_node) +add_library(nav2_would_a_route_recovery_help_condition_bt_node SHARED plugins/condition/would_a_route_recovery_help_condition.cpp) +list(APPEND plugin_libs nav2_would_a_route_recovery_help_condition_bt_node) + add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp) list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node) @@ -158,6 +164,9 @@ list(APPEND plugin_libs nav2_speed_controller_bt_node) add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp) list(APPEND plugin_libs nav2_truncate_path_action_bt_node) +add_library(nav2_concatenate_paths_action_bt_node SHARED plugins/action/concatenate_paths_action.cpp) +list(APPEND plugin_libs nav2_concatenate_paths_action_bt_node) + add_library(nav2_truncate_path_local_action_bt_node SHARED plugins/action/truncate_path_local_action.cpp) list(APPEND plugin_libs nav2_truncate_path_local_action_bt_node) @@ -182,9 +191,21 @@ list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node) add_library(nav2_remove_in_collision_goals_action_bt_node SHARED plugins/action/remove_in_collision_goals_action.cpp) list(APPEND plugin_libs nav2_remove_in_collision_goals_action_bt_node) +add_library(nav2_append_goal_pose_to_goals_action_bt_node SHARED plugins/action/append_goal_pose_to_goals_action.cpp) +list(APPEND plugin_libs nav2_append_goal_pose_to_goals_action_bt_node) + add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp) list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node) +add_library(nav2_extract_route_nodes_as_goals_action_bt_node SHARED plugins/action/extract_route_nodes_as_goals_action.cpp) +list(APPEND plugin_libs nav2_extract_route_nodes_as_goals_action_bt_node) + +add_library(nav2_get_next_few_goals_action_bt_node SHARED plugins/action/get_next_few_goals_action.cpp) +list(APPEND plugin_libs nav2_get_next_few_goals_action_bt_node) + +add_library(nav2_get_current_pose_action_bt_node SHARED plugins/action/get_current_pose_action.cpp) +list(APPEND plugin_libs nav2_get_current_pose_action_bt_node) + add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) @@ -212,6 +233,15 @@ list(APPEND plugin_libs nav2_progress_checker_selector_bt_node) add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) list(APPEND plugin_libs nav2_goal_updated_controller_bt_node) +add_library(nav2_compute_and_track_route_cancel_bt_node SHARED plugins/action/compute_and_track_route_cancel_node.cpp) +list(APPEND plugin_libs nav2_compute_and_track_route_cancel_bt_node) + +add_library(nav2_compute_and_track_route_bt_node SHARED plugins/action/compute_and_track_route_action.cpp) +list(APPEND plugin_libs nav2_compute_and_track_route_bt_node) + +add_library(nav2_compute_route_bt_node SHARED plugins/action/compute_route_action.cpp) +list(APPEND plugin_libs nav2_compute_route_bt_node) + foreach(bt_plugin ${plugin_libs}) target_include_directories(${bt_plugin} PRIVATE diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.hpp new file mode 100644 index 00000000000..41c4b5a1ef2 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.hpp @@ -0,0 +1,56 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__APPEND_GOAL_POSE_TO_GOALS_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__APPEND_GOAL_POSE_TO_GOALS_ACTION_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/goals.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "behaviortree_cpp/action_node.h" + +namespace nav2_behavior_tree +{ + +class AppendGoalPoseToGoals : public BT::ActionNodeBase +{ +public: + AppendGoalPoseToGoals( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("goal_pose", "Goal pose to append"), + BT::InputPort("input_goals", "Input goals to append to"), + BT::OutputPort("output_goals", "Output goals after appending") + }; + } + +private: + void halt() override {} + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__APPEND_GOAL_POSE_TO_GOALS_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp new file mode 100644 index 00000000000..c56da81de5d --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp @@ -0,0 +1,107 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_ + +#include +#include + +#include "nav2_msgs/action/compute_and_track_route.hpp" +#include "nav2_behavior_tree/bt_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputeAndTrackRoute + */ +class ComputeAndTrackRouteAction : public BtActionNode +{ + using Action = nav2_msgs::action::ComputeAndTrackRoute; + using ActionResult = Action::Result; + +public: + /** + * @brief A constructor for nav2_behavior_tree::ComputeAndTrackRouteAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + ComputeAndTrackRouteAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Function to perform some user-defined operation on tick + */ + void on_tick() override; + + /** + * @brief Function to perform some user-defined operation upon successful completion of the action + */ + BT::NodeStatus on_success() override; + + /** + * @brief Function to perform some user-defined operation upon abortion of the action + */ + BT::NodeStatus on_aborted() override; + + /** + * @brief Function to perform some user-defined operation upon cancellation of the action + */ + BT::NodeStatus on_cancelled() override; + + /** + * @brief Function to perform some user-defined operation after a timeout + * waiting for a result that hasn't been received yet + * @param feedback shared_ptr to latest feedback message + */ + void on_wait_for_result( + std::shared_ptr feedback) override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort("start_id", "ID of the start node"), + BT::InputPort("goal_id", "ID of the goal node"), + BT::InputPort( + "start", + "Start pose of the path if overriding current robot pose and using poses over IDs"), + BT::InputPort( + "goal", "Goal pose of the path if using poses over IDs"), + BT::InputPort( + "use_start", false, + "Whether to use the start pose or the robot's current pose"), + BT::InputPort( + "use_poses", false, "Whether to use poses or IDs for start and goal"), + BT::OutputPort("execution_duration", + "Time taken to compute and track route"), + BT::OutputPort( + "error_code_id", "The compute route error code"), + BT::OutputPort( + "error_msg", "The compute route error msg"), + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.hpp new file mode 100644 index 00000000000..a7d79d7a5a8 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/compute_and_track_route.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class + * that wraps nav2_msgs::action::ComputeAndTrackRoute cancellation + */ +class ComputeAndTrackRouteCancel + : public BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ComputeAndTrackRouteCancel + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + ComputeAndTrackRouteCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_CANCEL_NODE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp new file mode 100644 index 00000000000..560c49e5605 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp @@ -0,0 +1,112 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_ROUTE_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_ROUTE_ACTION_HPP_ + +#include + +#include "nav2_msgs/action/compute_route.hpp" +#include "nav_msgs/msg/path.h" +#include "nav2_behavior_tree/bt_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputeRoute + */ +class ComputeRouteAction : public BtActionNode +{ + using Action = nav2_msgs::action::ComputeRoute; + using ActionResult = Action::Result; + +public: + /** + * @brief A constructor for nav2_behavior_tree::ComputeRoute + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + ComputeRouteAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Function to perform some user-defined operation on tick + */ + void on_tick() override; + + /** + * @brief Function to perform some user-defined operation upon successful completion of the action + */ + BT::NodeStatus on_success() override; + + /** + * @brief Function to perform some user-defined operation upon abortion of the action + */ + BT::NodeStatus on_aborted() override; + + /** + * @brief Function to perform some user-defined operation upon cancellation of the action + */ + BT::NodeStatus on_cancelled() override; + + /** + * \brief Override required by the a BT action. Cancel the action and set the path output + */ + void halt() override; + + /** + * @brief Reset output port values on failure + */ + void resetPorts(); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort("start_id", "ID of the start node"), + BT::InputPort("goal_id", "ID of the goal node"), + BT::InputPort( + "start", + "Start pose of the path if overriding current robot pose and using poses over IDs"), + BT::InputPort( + "goal", "Goal pose of the path if using poses over IDs"), + BT::InputPort( + "use_start", false, + "Whether to use the start pose or the robot's current pose"), + BT::InputPort( + "use_poses", false, "Whether to use poses or IDs for start and goal"), + BT::OutputPort( + "route", "The route computed by ComputeRoute node"), + BT::OutputPort("planning_time", + "Time taken to compute route"), + BT::OutputPort("path", "Path created by ComputeRoute node"), + BT::OutputPort( + "error_code_id", "The compute route error code"), + BT::OutputPort( + "error_msg", "The compute route error msg"), + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_ROUTE_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp new file mode 100644 index 00000000000..adfc121af00 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp @@ -0,0 +1,72 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONCATENATE_PATHS_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONCATENATE_PATHS_ACTION_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" + +#include "behaviortree_cpp/action_node.h" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ActionNodeBase to shorten path by some distance + */ +class ConcatenatePaths : public BT::ActionNodeBase +{ +public: + /** + * @brief A nav2_behavior_tree::ConcatenatePaths constructor + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + ConcatenatePaths( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("input_path1", "Input Path 1 to cancatenate"), + BT::InputPort("input_path2", "Input Path 2 to cancatenate"), + BT::OutputPort("output_path", "Paths concatenated"), + }; + } + +private: + /** + * @brief The other (optional) override required by a BT action. + */ + void halt() override {} + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONCATENATE_PATHS_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/extract_route_nodes_as_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/extract_route_nodes_as_goals_action.hpp new file mode 100644 index 00000000000..57cc9e72f4a --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/extract_route_nodes_as_goals_action.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__EXTRACT_ROUTE_NODES_AS_GOALS_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__EXTRACT_ROUTE_NODES_AS_GOALS_ACTION_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/goals.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "behaviortree_cpp/action_node.h" + +namespace nav2_behavior_tree +{ + +class ExtractRouteNodesAsGoals : public BT::ActionNodeBase +{ +public: + ExtractRouteNodesAsGoals( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("route", "Route to extract nodes from"), + BT::OutputPort("goals", "Output goals for navigation") + }; + } + +private: + void halt() override {} + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__EXTRACT_ROUTE_NODES_AS_GOALS_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_current_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_current_pose_action.hpp new file mode 100644 index 00000000000..118edd28524 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_current_pose_action.hpp @@ -0,0 +1,80 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_CURRENT_POSE_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_CURRENT_POSE_ACTION_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" + +#include "behaviortree_cpp/action_node.h" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ActionNodeBase to shorten path by some distance + */ +class GetCurrentPoseAction : public BT::ActionNodeBase +{ +public: + /** + * @brief A nav2_behavior_tree::GetCurrentPoseAction constructor + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + GetCurrentPoseAction( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("global_frame", "Global reference frame"), + BT::InputPort("robot_base_frame", "robot base frame"), + BT::OutputPort("current_pose", "Current pose output"), + }; + } + +private: + /** + * @brief The other (optional) override required by a BT action. + */ + void halt() override {} + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + std::string global_frame_, robot_base_frame_; + std::shared_ptr tf_; + double transform_tolerance_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_CURRENT_POSE_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_next_few_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_next_few_goals_action.hpp new file mode 100644 index 00000000000..83534b34f2b --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_next_few_goals_action.hpp @@ -0,0 +1,56 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_NEXT_FEW_GOALS_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_NEXT_FEW_GOALS_ACTION_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/goals.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "behaviortree_cpp/action_node.h" + +namespace nav2_behavior_tree +{ + +class GetNextFewGoals : public BT::ActionNodeBase +{ +public: + GetNextFewGoals( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("input_goals", "Input goals for navigation"), + BT::InputPort("num_goals", "Number of goals to extract"), + BT::OutputPort("output_goals", "Output goals for navigation") + }; + } + +private: + void halt() override {} + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_NEXT_FEW_GOALS_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp new file mode 100644 index 00000000000..bcd170340d3 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp @@ -0,0 +1,90 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__ARE_POSES_NEAR_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__ARE_POSES_NEAR_CONDITION_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/condition_node.h" +#include "tf2_ros/buffer.h" +#include "nav2_behavior_tree/bt_utils.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that returns SUCCESS when a specified goal + * is reached and FAILURE otherwise + */ +class ArePosesNearCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ArePosesNearCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + ArePosesNearCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + /** + * @brief A destructor for nav2_behavior_tree::ArePosesNearCondition + */ + ~ArePosesNearCondition() override = default; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + + /** + * @brief Checks if the current robot pose lies within a given distance from the goal + * @return bool true when goal is reached, false otherwise + */ + bool arePosesNearby(); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("ref_pose", "Destination"), + BT::InputPort("target_pose", "Destination"), + BT::InputPort("global_frame", "Global frame"), + BT::InputPort("tolerance", 0.5, "Tolerance") + }; + } + +private: + rclcpp::Node::SharedPtr node_; + std::shared_ptr tf_; + double transform_tolerance_; + std::string global_frame_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__ARE_POSES_NEAR_CONDITION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.hpp new file mode 100644 index 00000000000..d9f3b96c125 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.hpp @@ -0,0 +1,43 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_ROUTE_RECOVERY_HELP_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_ROUTE_RECOVERY_HELP_CONDITION_HPP_ + +#include + +#include "nav2_msgs/action/compute_route.hpp" +#include "nav2_msgs/action/compute_and_track_route.hpp" +#include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp" + +namespace nav2_behavior_tree +{ + +class WouldARouteRecoveryHelp : public AreErrorCodesPresent +{ + using Action = nav2_msgs::action::ComputeRoute; + using ActionResult = Action::Result; + using TrackAction = nav2_msgs::action::ComputeAndTrackRoute; + using TrackActionResult = TrackAction::Result; + +public: + WouldARouteRecoveryHelp( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + WouldARouteRecoveryHelp() = delete; +}; + +} // namespace nav2_behavior_tree +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_ROUTE_RECOVERY_HELP_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 9ffeb2b44c6..d21d7695808 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -59,6 +59,11 @@ Server timeout + + Server name to cancel the route compute and tracker server + Server timeout + + Service name Server timeout @@ -98,6 +103,32 @@ "Compute path through poses error message" + + Node ID to start at + Node ID for the goal + Start pose of the route if overriding current robot pose and using poses over IDs + Goal pose of the route if using poses over IDs + Whether to use a fixed start pose or the TF current robot pose + Whether to use start/goal poses or startid and goalids on the graph + Duration to compute route + Path created by ComputeRoute node + Route created by ComputeRoute node + "Compute route to pose error code" + "Compute route to pose error message" + + + + Node ID to start at + Node ID for the goal + Start pose of the route if overriding current robot pose and using poses over IDs + Goal pose of the route if using poses over IDs + Whether to use a fixed start pose or the TF current robot pose + Whether to use start/goal poses or startid and goalids on the graph + Duration to compute route + "Compute route to pose error code" + "Compute route to pose error message" + + Input goals to remove if passed Radius tolerance on a goal to consider it passed diff --git a/nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.cpp b/nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.cpp new file mode 100644 index 00000000000..eb49380f464 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.cpp @@ -0,0 +1,56 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.hpp" + +namespace nav2_behavior_tree +{ + +AppendGoalPoseToGoals::AppendGoalPoseToGoals( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ +} + +inline BT::NodeStatus AppendGoalPoseToGoals::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + geometry_msgs::msg::PoseStamped goal_pose; + getInput("goal_pose", goal_pose); + nav_msgs::msg::Goals input, output; + getInput("input_goals", input); + + output = input; + output.goals.push_back(goal_pose); + + setOutput("output_goals", output); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType( + "AppendGoalPoseToGoals"); +} diff --git a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp new file mode 100644 index 00000000000..cc6db6fbe8b --- /dev/null +++ b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp @@ -0,0 +1,142 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp" + +namespace nav2_behavior_tree +{ + +ComputeAndTrackRouteAction::ComputeAndTrackRouteAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ +} + +void ComputeAndTrackRouteAction::on_tick() +{ + bool use_poses = false, use_start = false; + getInput("use_poses", use_poses); + if (use_poses) { + goal_.use_poses = true; + getInput("goal", goal_.goal); + + goal_.use_start = false; + getInput("use_start", use_start); + if (use_start) { + getInput("start", goal_.start); + goal_.use_start = true; + } + } else { + getInput("start_id", goal_.start_id); + getInput("goal_id", goal_.goal_id); + goal_.use_start = false; + goal_.use_poses = false; + } +} + +BT::NodeStatus ComputeAndTrackRouteAction::on_success() +{ + setOutput("execution_duration", result_.result->execution_duration); + setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus ComputeAndTrackRouteAction::on_aborted() +{ + setOutput("execution_duration", builtin_interfaces::msg::Duration()); + setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("execution_duration", builtin_interfaces::msg::Duration()); + setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); + return BT::NodeStatus::SUCCESS; +} + +void ComputeAndTrackRouteAction::on_wait_for_result( + std::shared_ptr/*feedback*/) +{ + // Check for request updates to the goal + bool use_poses = false, use_start = false; + getInput("use_start", use_start); + getInput("use_poses", use_poses); + + if (goal_.use_poses != use_poses) { + goal_updated_ = true; + } + + if (use_poses) { + geometry_msgs::msg::PoseStamped goal; + getInput("goal", goal); + if (goal_.goal != goal) { + goal_updated_ = true; + } + + if (goal_.use_start != use_start) { + goal_updated_ = true; + } + if (use_start) { + geometry_msgs::msg::PoseStamped start; + getInput("start", start); + if (goal_.start != start) { + goal_updated_ = true; + } + } + } else { + // Check if the start and goal IDs have changed + unsigned int start_id = 0; + unsigned int goal_id = 0; + getInput("start_id", start_id); + getInput("goal_id", goal_id); + if (goal_.start_id != start_id) { + goal_updated_ = true; + } + if (goal_.goal_id != goal_id) { + goal_updated_ = true; + } + } + + // If we're updating the request, we need to fully update the goal + // Easier to call on_tick() again than to duplicate the code + if (goal_updated_) { + on_tick(); + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_and_track_route", config); + }; + + factory.registerBuilder( + "ComputeAndTrackRoute", builder); +} diff --git a/nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.cpp b/nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.cpp new file mode 100644 index 00000000000..431bbb42dbe --- /dev/null +++ b/nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +ComputeAndTrackRouteCancel::ComputeAndTrackRouteCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_and_track_route", config); + }; + + factory.registerBuilder( + "CancelComputeAndTrackRoute", builder); +} diff --git a/nav2_behavior_tree/plugins/action/compute_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_route_action.cpp new file mode 100644 index 00000000000..cc16125c74b --- /dev/null +++ b/nav2_behavior_tree/plugins/action/compute_route_action.cpp @@ -0,0 +1,112 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_behavior_tree/plugins/action/compute_route_action.hpp" + +namespace nav2_behavior_tree +{ + +ComputeRouteAction::ComputeRouteAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ +} + +void ComputeRouteAction::on_tick() +{ + bool use_poses = false, use_start = false; + getInput("use_poses", use_poses); + if (use_poses) { + goal_.use_poses = true; + getInput("goal", goal_.goal); + + goal_.use_start = false; + getInput("use_start", use_start); + if (use_start) { + getInput("start", goal_.start); + goal_.use_start = true; + } + } else { + getInput("start_id", goal_.start_id); + getInput("goal_id", goal_.goal_id); + goal_.use_start = false; + goal_.use_poses = false; + } +} + +BT::NodeStatus ComputeRouteAction::on_success() +{ + setOutput("path", result_.result->path); + setOutput("route", result_.result->route); + setOutput("planning_time", result_.result->planning_time); + // Set empty error code, action was successful + setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); + return BT::NodeStatus::SUCCESS; +} + +void ComputeRouteAction::resetPorts() +{ + nav_msgs::msg::Path empty_path; + setOutput("path", empty_path); + nav2_msgs::msg::Route empty_route; + setOutput("route", empty_route); + setOutput("planning_time", builtin_interfaces::msg::Duration()); +} + +BT::NodeStatus ComputeRouteAction::on_aborted() +{ + resetPorts(); + setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus ComputeRouteAction::on_cancelled() +{ + resetPorts(); + // Set empty error code, action was cancelled + setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); + return BT::NodeStatus::SUCCESS; +} + +void ComputeRouteAction::halt() +{ + resetPorts(); + // DO NOT reset "error_code_id" output port, we want to read it later + // DO NOT reset "error_msg" output port, we want to read it later + BtActionNode::halt(); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_route", config); + }; + + factory.registerBuilder( + "ComputeRoute", builder); +} diff --git a/nav2_behavior_tree/plugins/action/concatenate_paths_action.cpp b/nav2_behavior_tree/plugins/action/concatenate_paths_action.cpp new file mode 100644 index 00000000000..4ebc4df5e4c --- /dev/null +++ b/nav2_behavior_tree/plugins/action/concatenate_paths_action.cpp @@ -0,0 +1,74 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "behaviortree_cpp/decorator_node.h" + +#include "nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp" + +namespace nav2_behavior_tree +{ + +ConcatenatePaths::ConcatenatePaths( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ +} + +inline BT::NodeStatus ConcatenatePaths::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Path input_path1, input_path2; + getInput("input_path1", input_path1); + getInput("input_path2", input_path2); + + if (input_path1.poses.empty() && input_path2.poses.empty()) { + RCLCPP_ERROR( + config().blackboard->get("node")->get_logger(), + "No input paths provided to concatenate. Both paths are empty."); + return BT::NodeStatus::FAILURE; + } + + nav_msgs::msg::Path output_path; + output_path = input_path1; + if (input_path1.header != std_msgs::msg::Header()) { + output_path.header = input_path1.header; + } else if (input_path2.header != std_msgs::msg::Header()) { + output_path.header = input_path2.header; + } + + output_path.poses.insert( + output_path.poses.end(), + input_path2.poses.begin(), + input_path2.poses.end()); + + setOutput("output_path", output_path); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ConcatenatePaths"); +} diff --git a/nav2_behavior_tree/plugins/action/extract_route_nodes_as_goals_action.cpp b/nav2_behavior_tree/plugins/action/extract_route_nodes_as_goals_action.cpp new file mode 100644 index 00000000000..0998a2715f4 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/extract_route_nodes_as_goals_action.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/action/extract_route_nodes_as_goals_action.hpp" + +namespace nav2_behavior_tree +{ + +ExtractRouteNodesAsGoals::ExtractRouteNodesAsGoals( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ +} + +inline BT::NodeStatus ExtractRouteNodesAsGoals::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav2_msgs::msg::Route route; + getInput("route", route); + + if (route.nodes.empty()) { + return BT::NodeStatus::FAILURE; + } + + nav_msgs::msg::Goals goals; + goals.header = route.header; + goals.goals.reserve(route.nodes.size()); + + for (const auto & node : route.nodes) { + geometry_msgs::msg::PoseStamped goal; + goal.header = route.header; + goal.pose.position.x = node.position.x; + goal.pose.position.y = node.position.y; + goals.goals.push_back(goal); + } + + setOutput("goals", goals); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType( + "ExtractRouteNodesAsGoals"); +} diff --git a/nav2_behavior_tree/plugins/action/get_current_pose_action.cpp b/nav2_behavior_tree/plugins/action/get_current_pose_action.cpp new file mode 100644 index 00000000000..b6cc003d3b8 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/get_current_pose_action.cpp @@ -0,0 +1,68 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "behaviortree_cpp/decorator_node.h" + +#include "nav2_behavior_tree/plugins/action/get_current_pose_action.hpp" + +namespace nav2_behavior_tree +{ + +GetCurrentPoseAction::GetCurrentPoseAction( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ + auto node = config().blackboard->get("node"); + tf_ = config().blackboard->get>("tf_buffer"); + node->get_parameter("transform_tolerance", transform_tolerance_); + global_frame_ = BT::deconflictPortAndParamFrame( + node, "global_frame", this); + robot_base_frame_ = BT::deconflictPortAndParamFrame( + node, "robot_base_frame", this); +} + +inline BT::NodeStatus GetCurrentPoseAction::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + geometry_msgs::msg::PoseStamped current_pose; + + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) + { + RCLCPP_WARN( + config().blackboard->get("node")->get_logger(), + "Current robot pose is not available."); + return BT::NodeStatus::FAILURE; + } + + setOutput("current_pose", current_pose); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GetCurrentPose"); +} diff --git a/nav2_behavior_tree/plugins/action/get_next_few_goals_action.cpp b/nav2_behavior_tree/plugins/action/get_next_few_goals_action.cpp new file mode 100644 index 00000000000..e6af2520455 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/get_next_few_goals_action.cpp @@ -0,0 +1,60 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/action/get_next_few_goals_action.hpp" + +namespace nav2_behavior_tree +{ + +GetNextFewGoals::GetNextFewGoals( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ +} + +inline BT::NodeStatus GetNextFewGoals::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Goals input_goals, output_goals; + unsigned int num_goals; + getInput("input_goals", input_goals); + getInput("num_goals", num_goals); + + if (input_goals.goals.empty()) { + return BT::NodeStatus::FAILURE; + } + + output_goals.header = input_goals.header; + for (unsigned int i = 0; i < num_goals && i < input_goals.goals.size(); ++i) { + output_goals.goals.push_back(input_goals.goals[i]); + } + setOutput("output_goals", output_goals); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GetNextFewGoals"); +} diff --git a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp index cab0311a37a..ce042d1f3be 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -46,12 +46,12 @@ inline BT::NodeStatus GetPoseFromPath::tick() } // Account for negative indices - if(pose_index < 0) { + if (pose_index < 0) { pose_index = input_path.poses.size() + pose_index; } // out of bounds index - if(pose_index < 0 || static_cast(pose_index) >= input_path.poses.size()) { + if (pose_index < 0 || static_cast(pose_index) >= input_path.poses.size()) { return BT::NodeStatus::FAILURE; } @@ -60,7 +60,7 @@ inline BT::NodeStatus GetPoseFromPath::tick() output_pose = input_path.poses[pose_index]; // populate pose frame from path if necessary - if(output_pose.header.frame_id.empty()) { + if (output_pose.header.frame_id.empty()) { output_pose.header.frame_id = input_path.header.frame_id; } diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 8e5bec221bf..53dd6db6e78 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -71,7 +71,7 @@ inline BT::NodeStatus RemovePassedGoals::tick() std::vector waypoint_statuses; auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses); if (!waypoint_statuses_get_res) { - RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!"); + RCLCPP_ERROR_ONCE(node_->get_logger(), "Missing [input_waypoint_statuses] port input!"); } double dist_to_goal; @@ -87,7 +87,7 @@ inline BT::NodeStatus RemovePassedGoals::tick() auto cur_waypoint_index = find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[0]); if (cur_waypoint_index == -1) { - RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses"); + RCLCPP_ERROR_ONCE(node_->get_logger(), "Failed to find matching goal in waypoint_statuses"); return BT::NodeStatus::FAILURE; } waypoint_statuses[cur_waypoint_index].waypoint_status = diff --git a/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp b/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp new file mode 100644 index 00000000000..4d03f6aaa50 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp @@ -0,0 +1,86 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_util/robot_utils.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/node_utils.hpp" + +#include "nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp" + +namespace nav2_behavior_tree +{ + +ArePosesNearCondition::ArePosesNearCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf) +{ + auto node = config().blackboard->get("node"); + global_frame_ = BT::deconflictPortAndParamFrame( + node, "global_frame", this); +} + +void ArePosesNearCondition::initialize() +{ + node_ = config().blackboard->get("node"); + tf_ = config().blackboard->get>("tf_buffer"); + node_->get_parameter("transform_tolerance", transform_tolerance_); +} + +BT::NodeStatus ArePosesNearCondition::tick() +{ + if (!BT::isStatusActive(status())) { + initialize(); + } + + if (arePosesNearby()) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +bool ArePosesNearCondition::arePosesNearby() +{ + geometry_msgs::msg::PoseStamped pose1, pose2; + double tol; + getInput("ref_pose", pose1); + getInput("target_pose", pose2); + getInput("tolerance", tol); + + if (pose1.header.frame_id != pose2.header.frame_id) { + if (!nav2_util::transformPoseInTargetFrame( + pose1, pose1, *tf_, global_frame_, transform_tolerance_) || + !nav2_util::transformPoseInTargetFrame( + pose2, pose2, *tf_, global_frame_, transform_tolerance_)) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to transform poses to the same frame"); + return false; + } + } + + double dx = pose1.pose.position.x - pose2.pose.position.x; + double dy = pose1.pose.position.y - pose2.pose.position.y; + return (dx * dx + dy * dy) <= (tol * tol); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ArePosesNear"); +} diff --git a/nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.cpp new file mode 100644 index 00000000000..dc41b0551ef --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.hpp" +#include + +namespace nav2_behavior_tree +{ + +WouldARouteRecoveryHelp::WouldARouteRecoveryHelp( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: AreErrorCodesPresent(condition_name, conf) +{ + error_codes_to_check_ = { + ActionResult::UNKNOWN, + ActionResult::NO_VALID_ROUTE, + ActionResult::TIMEOUT, + TrackActionResult::UNKNOWN, + TrackActionResult::TIMEOUT, + TrackActionResult::NO_VALID_ROUTE + }; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType( + "WouldARouteRecoveryHelp"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index e538e63cd02..742b6487fcf 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -23,6 +23,10 @@ plugin_add_test(test_action_back_up_cancel_action test_back_up_cancel_node.cpp n plugin_add_test(test_action_assisted_teleop_cancel_action test_assisted_teleop_cancel_node.cpp nav2_assisted_teleop_cancel_bt_node) +plugin_add_test(test_action_compute_and_track_route_cancel_action + test_compute_and_track_route_cancel_node.cpp + nav2_compute_and_track_route_cancel_bt_node) + plugin_add_test(test_action_drive_on_heading_cancel_action test_drive_on_heading_cancel_node.cpp nav2_drive_on_heading_cancel_bt_node) plugin_add_test(test_action_clear_costmap_service test_clear_costmap_service.cpp nav2_clear_costmap_service_bt_node) @@ -37,6 +41,10 @@ plugin_add_test(test_action_compute_path_through_poses_action test_compute_path_through_poses_action.cpp nav2_compute_path_through_poses_action_bt_node) +plugin_add_test(test_action_compute_route_action test_compute_route_action.cpp nav2_compute_route_bt_node) + +plugin_add_test(test_action_compute_and_track_route_action test_compute_and_track_route_action.cpp nav2_compute_and_track_route_bt_node) + plugin_add_test(test_action_smooth_path_action test_smooth_path_action.cpp nav2_smooth_path_action_bt_node) plugin_add_test(test_action_follow_path_action test_follow_path_action.cpp nav2_follow_path_action_bt_node) @@ -49,8 +57,24 @@ plugin_add_test(test_truncate_path_action test_truncate_path_action.cpp nav2_tru plugin_add_test(test_truncate_path_local_action test_truncate_path_local_action.cpp nav2_truncate_path_local_action_bt_node) +plugin_add_test(test_concatenate_paths_action test_concatenate_paths_action.cpp nav2_concatenate_paths_action_bt_node) + +plugin_add_test(test_get_current_pose_action test_get_current_pose_action.cpp nav2_get_current_pose_action_bt_node) + plugin_add_test(test_remove_passed_goals_action test_remove_passed_goals_action.cpp nav2_remove_passed_goals_action_bt_node) +plugin_add_test(test_get_next_few_goals_action test_get_next_few_goals_action.cpp nav2_get_next_few_goals_action_bt_node) + +plugin_add_test( + test_append_goal_pose_to_goals_action + test_append_goal_pose_to_goals_action.cpp + nav2_append_goal_pose_to_goals_action_bt_node) + +plugin_add_test( + test_extract_route_nodes_as_goals_action + test_extract_route_nodes_as_goals_action.cpp + nav2_extract_route_nodes_as_goals_action_bt_node) + plugin_add_test(test_remove_in_collision_goals_action test_remove_in_collision_goals_action.cpp nav2_remove_in_collision_goals_action_bt_node) diff --git a/nav2_behavior_tree/test/plugins/action/test_append_goal_pose_to_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_append_goal_pose_to_goals_action.cpp new file mode 100644 index 00000000000..f6cb4baadd5 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_append_goal_pose_to_goals_action.cpp @@ -0,0 +1,145 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.hpp" +#include "utils/test_behavior_tree_fixture.hpp" + +class AppendGoalPoseToGoalsTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + transform_handler_ = std::make_shared(node_); + transform_handler_->activate(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "tf_buffer", + transform_handler_->getBuffer()); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "AppendGoalPoseToGoals", builder); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete config_; + config_ = nullptr; + transform_handler_.reset(); + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; + static std::shared_ptr transform_handler_; +}; + +rclcpp::Node::SharedPtr AppendGoalPoseToGoalsTestFixture::node_ = nullptr; + +BT::NodeConfiguration * AppendGoalPoseToGoalsTestFixture::config_ = nullptr; +std::shared_ptr AppendGoalPoseToGoalsTestFixture::factory_ = nullptr; +std::shared_ptr AppendGoalPoseToGoalsTestFixture::tree_ = nullptr; +std::shared_ptr +AppendGoalPoseToGoalsTestFixture::transform_handler_ = nullptr; + +TEST_F(AppendGoalPoseToGoalsTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = node_->now(); + pose.pose.position.x = 10; + config_->blackboard->set("pose", pose); + + nav_msgs::msg::Goals input_goals; + config_->blackboard->set("goals", input_goals); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check that it removed the point in range + nav_msgs::msg::Goals output_goals; + EXPECT_TRUE(config_->blackboard->get("goals", output_goals)); + + EXPECT_EQ(output_goals.goals.size(), 1u); + EXPECT_EQ(output_goals.goals[0].header.frame_id, "map"); + EXPECT_EQ(output_goals.goals[0].pose.position.x, 10.0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_and_track_route_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_and_track_route_action.cpp new file mode 100644 index 00000000000..0129eb46359 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_compute_and_track_route_action.cpp @@ -0,0 +1,234 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp" + +class ComputeAndTrackRouteActionServer + : public TestActionServer +{ +public: + ComputeAndTrackRouteActionServer() + : TestActionServer("compute_and_track_route") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> goal_handle) + override + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + result->execution_duration = rclcpp::Duration::from_seconds(0.1); + goal_handle->succeed(result); + } +}; + +class ComputeAndTrackRouteActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("follow_path_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_and_track_route", config); + }; + + factory_->registerBuilder( + "ComputeAndTrackRoute", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ComputeAndTrackRouteActionTestFixture::node_ = nullptr; +std::shared_ptr +ComputeAndTrackRouteActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * ComputeAndTrackRouteActionTestFixture::config_ = nullptr; +std::shared_ptr ComputeAndTrackRouteActionTestFixture::factory_ = nullptr; +std::shared_ptr ComputeAndTrackRouteActionTestFixture::tree_ = nullptr; + +TEST_F(ComputeAndTrackRouteActionTestFixture, test_tick_poses) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new start and set it on blackboard + geometry_msgs::msg::PoseStamped start; + start.header.stamp = node_->now(); + start.pose.position.x = 2.0; + config_->blackboard->set("start", start); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); + EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); + EXPECT_TRUE(action_server_->getCurrentGoal()->use_poses); + + builtin_interfaces::msg::Duration time1; + EXPECT_TRUE( + config_->blackboard->get("execution_duration", time1)); + EXPECT_EQ(time1, rclcpp::Duration::from_seconds(0.1)); + + // halt node so another goal can be sent + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal + goal.pose.position.x = -2.5; + config_->blackboard->set("goal", goal); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); +} + +TEST_F(ComputeAndTrackRouteActionTestFixture, test_tick_ids) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goal_id, 10); + EXPECT_EQ(action_server_->getCurrentGoal()->start_id, 20); + EXPECT_EQ(action_server_->getCurrentGoal()->start, geometry_msgs::msg::PoseStamped()); + EXPECT_EQ(action_server_->getCurrentGoal()->goal, geometry_msgs::msg::PoseStamped()); + EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); + EXPECT_FALSE(action_server_->getCurrentGoal()->use_poses); + + builtin_interfaces::msg::Duration time1; + EXPECT_TRUE( + config_->blackboard->get("execution_duration", time1)); + EXPECT_EQ(time1, rclcpp::Duration::from_seconds(0.1)); + + // halt node so another goal can be sent + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + ComputeAndTrackRouteActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(ComputeAndTrackRouteActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_and_track_route_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_and_track_route_cancel_node.cpp new file mode 100644 index 00000000000..de94f9c47a1 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_compute_and_track_route_cancel_node.cpp @@ -0,0 +1,177 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelComputeAndTrackRouteServer + : public TestActionServer +{ +public: + CancelComputeAndTrackRouteServer() + : TestActionServer("compute_and_track_route") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // Assisted Teleop here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelComputeAndTrackRouteActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_compute_and_track_route_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); + client_ = rclcpp_action::create_client( + node_, "compute_and_track_route"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_and_track_route", config); + }; + + factory_->registerBuilder( + "CancelComputeAndTrackRoute", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + client_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelComputeAndTrackRouteActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelComputeAndTrackRouteActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelComputeAndTrackRouteActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelComputeAndTrackRouteActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelComputeAndTrackRouteActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelComputeAndTrackRouteActionTestFixture::tree_ = nullptr; + +TEST_F(CancelComputeAndTrackRouteActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client< + nav2_msgs::action::ComputeAndTrackRoute>::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::ComputeAndTrackRoute::Goal(); + + // BackUping for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is in fact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and back_up on new thread + CancelComputeAndTrackRouteActionTestFixture::action_server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelComputeAndTrackRouteActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_route_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_route_action.cpp new file mode 100644 index 00000000000..308e68727f9 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_compute_route_action.cpp @@ -0,0 +1,271 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/compute_route_action.hpp" + +class ComputeRouteActionServer : public TestActionServer +{ +public: + ComputeRouteActionServer() + : TestActionServer("compute_route") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> goal_handle) + override + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + result->path.poses.resize(2); + result->path.poses[1].pose.position.x = goal->goal.pose.position.x; + if (goal->use_start) { + result->path.poses[0].pose.position.x = goal->start.pose.position.x; + } else { + result->path.poses[0].pose.position.x = 0.0; + } + result->route.nodes.resize(2); + result->route.edges.resize(2); + result->planning_time = rclcpp::Duration::from_seconds(0.1); + goal_handle->succeed(result); + } +}; + +class ComputeRouteActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("compute_path_to_pose_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_route", config); + }; + + factory_->registerBuilder( + "ComputeRoute", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ComputeRouteActionTestFixture::node_ = nullptr; +std::shared_ptr +ComputeRouteActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * ComputeRouteActionTestFixture::config_ = nullptr; +std::shared_ptr ComputeRouteActionTestFixture::factory_ = nullptr; +std::shared_ptr ComputeRouteActionTestFixture::tree_ = nullptr; + +TEST_F(ComputeRouteActionTestFixture, test_IDs) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->start_id, 7); + EXPECT_EQ(action_server_->getCurrentGoal()->goal_id, 2); + EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); + EXPECT_FALSE(action_server_->getCurrentGoal()->use_poses); + EXPECT_EQ(action_server_->getCurrentGoal()->goal, geometry_msgs::msg::PoseStamped()); + EXPECT_EQ(action_server_->getCurrentGoal()->start, geometry_msgs::msg::PoseStamped()); + + // check if returned values are correct + nav_msgs::msg::Path path; + EXPECT_TRUE(config_->blackboard->get("path", path)); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_EQ(path.poses[1].pose.position.x, 0.0); + + nav2_msgs::msg::Route route; + EXPECT_TRUE(config_->blackboard->get("route", route)); + EXPECT_EQ(route.nodes.size(), 2u); + EXPECT_EQ(route.edges.size(), 2u); + + builtin_interfaces::msg::Duration time1; + EXPECT_TRUE(config_->blackboard->get("planning_time", time1)); + EXPECT_EQ(time1, rclcpp::Duration::from_seconds(0.1)); + + // halt node so another goal can be sent + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); +} + +TEST_F(ComputeRouteActionTestFixture, test_poses) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new start and set it on blackboard + geometry_msgs::msg::PoseStamped start; + start.header.stamp = node_->now(); + start.pose.position.x = 2.0; + config_->blackboard->set("start", start); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); + EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); + EXPECT_TRUE(action_server_->getCurrentGoal()->use_poses); + + // check if returned values are correct + nav_msgs::msg::Path path; + EXPECT_TRUE(config_->blackboard->get("path", path)); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 2.0); + EXPECT_EQ(path.poses[1].pose.position.x, 1.0); + + nav2_msgs::msg::Route route; + EXPECT_TRUE(config_->blackboard->get("route", route)); + EXPECT_EQ(route.nodes.size(), 2u); + EXPECT_EQ(route.edges.size(), 2u); + + builtin_interfaces::msg::Duration time1; + EXPECT_TRUE(config_->blackboard->get("planning_time", time1)); + EXPECT_EQ(time1, rclcpp::Duration::from_seconds(0.1)); + + // halt node so another goal can be sent + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal and new start + goal.pose.position.x = -2.5; + start.pose.position.x = -1.5; + config_->blackboard->set("goal", goal); + config_->blackboard->set("start", start); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); + + EXPECT_TRUE(config_->blackboard->get("path", path)); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, -1.5); + EXPECT_EQ(path.poses[1].pose.position.x, -2.5); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + ComputeRouteActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(ComputeRouteActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_concatenate_paths_action.cpp b/nav2_behavior_tree/test/plugins/action/test_concatenate_paths_action.cpp new file mode 100644 index 00000000000..f8c6802e51d --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_concatenate_paths_action.cpp @@ -0,0 +1,162 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp" + + +class ConcatenatePathsTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "ConcatenatePaths", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ConcatenatePathsTestFixture::node_ = nullptr; + +BT::NodeConfiguration * ConcatenatePathsTestFixture::config_ = nullptr; +std::shared_ptr ConcatenatePathsTestFixture::factory_ = nullptr; +std::shared_ptr ConcatenatePathsTestFixture::tree_ = nullptr; + +TEST_F(ConcatenatePathsTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + nav_msgs::msg::Path path1, path2; + path1.header.stamp = node_->now(); + path2.header.stamp = node_->now(); + + int i = 0; + int j = 3; + for (int x = 0; x != 3; x++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path1.poses.push_back(pose); + pose.pose.position.x = j; + path2.poses.push_back(pose); + i++; + j++; + } + + config_->blackboard->set("path1", path1); + config_->blackboard->set("path2", path2); + + // tick until node finishes + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + nav_msgs::msg::Path concat_path; + EXPECT_TRUE(config_->blackboard->get("concat_path", concat_path)); + + EXPECT_EQ(concat_path.poses.size(), 6u); + for (size_t x = 0; x < concat_path.poses.size(); ++x) { + EXPECT_EQ(concat_path.poses[x].pose.position.x, static_cast(x)); + } + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + config_->blackboard->set("path1", nav_msgs::msg::Path()); + config_->blackboard->set("path2", nav_msgs::msg::Path()); + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_extract_route_nodes_as_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_extract_route_nodes_as_goals_action.cpp new file mode 100644 index 00000000000..6c541fc9de5 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_extract_route_nodes_as_goals_action.cpp @@ -0,0 +1,156 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/extract_route_nodes_as_goals_action.hpp" +#include "utils/test_behavior_tree_fixture.hpp" + +class ExtractRouteNodesAsGoalsTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + transform_handler_ = std::make_shared(node_); + transform_handler_->activate(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "tf_buffer", + transform_handler_->getBuffer()); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "ExtractRouteNodesAsGoals", builder); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete config_; + config_ = nullptr; + transform_handler_.reset(); + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; + static std::shared_ptr transform_handler_; +}; + +rclcpp::Node::SharedPtr ExtractRouteNodesAsGoalsTestFixture::node_ = nullptr; + +BT::NodeConfiguration * ExtractRouteNodesAsGoalsTestFixture::config_ = nullptr; +std::shared_ptr ExtractRouteNodesAsGoalsTestFixture::factory_ = nullptr; +std::shared_ptr ExtractRouteNodesAsGoalsTestFixture::tree_ = nullptr; +std::shared_ptr +ExtractRouteNodesAsGoalsTestFixture::transform_handler_ = nullptr; + +TEST_F(ExtractRouteNodesAsGoalsTestFixture, test_tick) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.25; + pose.position.y = 0.0; + + transform_handler_->waitForTransform(); + transform_handler_->updateRobotPose(pose); + + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + nav2_msgs::msg::Route route; + route.header.frame_id = "map"; + route.header.stamp = node_->now(); + route.nodes.resize(4); + route.nodes[0].position.x = 0.0; + route.nodes[1].position.x = 1.0; + route.nodes[2].position.x = 2.0; + route.nodes[3].position.x = 3.0; + + config_->blackboard->set("route", route); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check that it removed the point in range + nav_msgs::msg::Goals output_goals; + EXPECT_TRUE(config_->blackboard->get("goals", output_goals)); + + EXPECT_EQ(output_goals.goals.size(), 4u); + EXPECT_EQ(output_goals.header.frame_id, "map"); + for (size_t i = 0; i < output_goals.goals.size(); ++i) { + EXPECT_EQ(output_goals.goals[i].pose.position.x, route.nodes[i].position.x); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_get_current_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_current_pose_action.cpp new file mode 100644 index 00000000000..495d112aa0d --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_get_current_pose_action.cpp @@ -0,0 +1,108 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "nav_msgs/msg/goals.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/get_current_pose_action.hpp" +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_util/node_utils.hpp" + +class GetCurrentPoseTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + config_->blackboard->set("robot_base_frame", "base_link"); + config_->blackboard->set("global_frame", "map"); + nav2_util::declare_parameter_if_not_declared( + node_, "robot_base_frame", rclcpp::ParameterValue("base_link")); + nav2_util::declare_parameter_if_not_declared( + node_, "global_frame", rclcpp::ParameterValue("map")); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "GetCurrentPose", builder); + } + + void TearDown() + { + tree_.reset(); + } + +protected: + static std::shared_ptr tree_; +}; + +std::shared_ptr GetCurrentPoseTestFixture::tree_ = nullptr; + +TEST_F(GetCurrentPoseTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + geometry_msgs::msg::Pose pose; + pose.position.x = 1.0; + pose.position.y = 2.0; + transform_handler_->updateRobotPose(pose); + std::this_thread::sleep_for(500ms); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // Check the output + geometry_msgs::msg::PoseStamped current_pose; + EXPECT_TRUE(config_->blackboard->get("current_pose", current_pose)); + EXPECT_EQ(current_pose.pose.position.x, pose.position.x); + EXPECT_EQ(current_pose.pose.position.y, pose.position.y); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_get_next_few_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_next_few_goals_action.cpp new file mode 100644 index 00000000000..4926711d333 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_get_next_few_goals_action.cpp @@ -0,0 +1,156 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/get_next_few_goals_action.hpp" +#include "utils/test_behavior_tree_fixture.hpp" + +class GetNextFewGoalsTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + transform_handler_ = std::make_shared(node_); + transform_handler_->activate(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "tf_buffer", + transform_handler_->getBuffer()); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "GetNextFewGoals", builder); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete config_; + config_ = nullptr; + transform_handler_.reset(); + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; + static std::shared_ptr transform_handler_; +}; + +rclcpp::Node::SharedPtr GetNextFewGoalsTestFixture::node_ = nullptr; + +BT::NodeConfiguration * GetNextFewGoalsTestFixture::config_ = nullptr; +std::shared_ptr GetNextFewGoalsTestFixture::factory_ = nullptr; +std::shared_ptr GetNextFewGoalsTestFixture::tree_ = nullptr; +std::shared_ptr +GetNextFewGoalsTestFixture::transform_handler_ = nullptr; + +TEST_F(GetNextFewGoalsTestFixture, test_tick) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.25; + pose.position.y = 0.0; + + transform_handler_->waitForTransform(); + transform_handler_->updateRobotPose(pose); + + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + nav_msgs::msg::Goals goals; + goals.header.frame_id = "map"; + goals.header.stamp = node_->now(); + goals.goals.resize(4); + goals.goals[0].pose.position.x = 0.0; + goals.goals[1].pose.position.x = 1.0; + goals.goals[2].pose.position.x = 2.0; + goals.goals[3].pose.position.x = 3.0; + + config_->blackboard->set("goals", goals); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check that it removed the point in range + nav_msgs::msg::Goals output_goals; + EXPECT_TRUE(config_->blackboard->get("out", output_goals)); + + EXPECT_EQ(output_goals.goals.size(), 2u); + EXPECT_EQ(output_goals.header.frame_id, "map"); + for (size_t i = 0; i < output_goals.goals.size(); ++i) { + EXPECT_EQ(output_goals.goals[i].pose.position.x, goals.goals[i].pose.position.x); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index 106c4a96f27..d7d32012733 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -6,6 +6,8 @@ plugin_add_test(test_condition_path_expiring_timer test_path_expiring_timer.cpp plugin_add_test(test_condition_goal_reached test_goal_reached.cpp nav2_goal_reached_condition_bt_node) +plugin_add_test(test_condition_are_poses_near test_are_poses_near.cpp nav2_are_poses_near_condition_bt_node) + plugin_add_test(test_condition_goal_updated test_goal_updated.cpp nav2_goal_updated_condition_bt_node) plugin_add_test(test_condition_globally_updated_goal test_globally_updated_goal.cpp nav2_globally_updated_goal_condition_bt_node) @@ -37,3 +39,7 @@ plugin_add_test(test_would_a_planner_recovery_help plugin_add_test(test_would_a_smoother_recovery_help test_would_a_smoother_recovery_help.cpp nav2_would_a_smoother_recovery_help_condition_bt_node) + +plugin_add_test(test_would_a_route_recovery_help + test_would_a_route_recovery_help.cpp + nav2_would_a_route_recovery_help_condition_bt_node) diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_poses_near.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_poses_near.cpp new file mode 100644 index 00000000000..6dbf732beb5 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_are_poses_near.cpp @@ -0,0 +1,98 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class ArePosesNearConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue{0.1}); + + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.header.frame_id = "map"; + goal.pose.position.x = 1.0; + goal.pose.position.y = 1.0; + config_->blackboard->set("p1", goal); + + std::string xml_txt = + R"( + + + + + )"; + + factory_->registerNodeType("ArePosesNear"); + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + } + + void TearDown() + { + tree_.reset(); + } + +protected: + static std::shared_ptr tree_; +}; + +std::shared_ptr ArePosesNearConditionTestFixture::tree_ = nullptr; + +TEST_F(ArePosesNearConditionTestFixture, test_behavior) +{ + geometry_msgs::msg::PoseStamped p2; + p2.header.stamp = node_->now(); + p2.header.frame_id = "map"; + config_->blackboard->set("p2", p2); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); + + p2.pose.position.x = 1.0; + p2.pose.position.y = 1.0; + config_->blackboard->set("p2", p2); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); + + p2.pose.position.x = 1.1; + p2.pose.position.y = 1.1; + config_->blackboard->set("p2", p2); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_route_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_route_recovery_help.cpp new file mode 100644 index 00000000000..405761fa852 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_route_recovery_help.cpp @@ -0,0 +1,84 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/would_a_route_recovery_help_condition.hpp" +#include "nav2_msgs/action/compute_path_to_pose.hpp" + +class WouldARouteRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + using Action = nav2_msgs::action::ComputeRoute; + using ActionResult = Action::Result; + void SetUp() + { + uint16_t error_code = ActionResult::NONE; + config_->blackboard->set("error_code", error_code); + + std::string xml_txt = + R"( + + + + + )"; + + factory_->registerNodeType( + "WouldARouteRecoveryHelp"); + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + } + + void TearDown() + { + tree_.reset(); + } + +protected: + static std::shared_ptr tree_; +}; + +std::shared_ptr WouldARouteRecoveryHelpFixture::tree_ = nullptr; + +TEST_F(WouldARouteRecoveryHelpFixture, test_condition) +{ + std::map error_to_status_map = { + {ActionResult::NONE, BT::NodeStatus::FAILURE}, + {ActionResult::UNKNOWN, BT::NodeStatus::SUCCESS}, + {ActionResult::NO_VALID_ROUTE, BT::NodeStatus::SUCCESS}, + }; + + for (const auto & error_to_status : error_to_status_map) { + config_->blackboard->set("error_code", error_to_status.first); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt index de7aa3dfd51..5537046d246 100644 --- a/nav2_bringup/CMakeLists.txt +++ b/nav2_bringup/CMakeLists.txt @@ -9,6 +9,7 @@ nav2_package() install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY graphs DESTINATION share/${PROJECT_NAME}) install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_bringup/graphs/turtlebot3_graph.geojson b/nav2_bringup/graphs/turtlebot3_graph.geojson new file mode 100644 index 00000000000..004008a2dbe --- /dev/null +++ b/nav2_bringup/graphs/turtlebot3_graph.geojson @@ -0,0 +1,92 @@ +{ + "type": "FeatureCollection", + "name": "turtlebot3_graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ -0.6, -1.6 ] } }, + { "type": "Feature", "properties": { "id": 2 }, "geometry": { "type": "Point", "coordinates": [ 0.6, -1.6 ] } }, + { "type": "Feature", "properties": { "id": 3 }, "geometry": { "type": "Point", "coordinates": [ 0.0, -2.0 ] } }, + { "type": "Feature", "properties": { "id": 4 }, "geometry": { "type": "Point", "coordinates": [ -1.6, -1.6 ] } }, + { "type": "Feature", "properties": { "id": 5 }, "geometry": { "type": "Point", "coordinates": [ -0.6, -0.6 ] } }, + { "type": "Feature", "properties": { "id": 6 }, "geometry": { "type": "Point", "coordinates": [ 0.6, -0.6 ] } }, + { "type": "Feature", "properties": { "id": 7 }, "geometry": { "type": "Point", "coordinates": [ -1.6, -0.6 ] } }, + { "type": "Feature", "properties": { "id": 8 }, "geometry": { "type": "Point", "coordinates": [ -1.6, 0.6 ] } }, + { "type": "Feature", "properties": { "id": 9 }, "geometry": { "type": "Point", "coordinates": [ -0.6, 0.6 ] } }, + { "type": "Feature", "properties": { "id": 10 }, "geometry": { "type": "Point", "coordinates": [ 0.6, 0.6 ] } }, + { "type": "Feature", "properties": { "id": 11 }, "geometry": { "type": "Point", "coordinates": [ 1.6, 0.6 ] } }, + { "type": "Feature", "properties": { "id": 12 }, "geometry": { "type": "Point", "coordinates": [ 1.6, -0.6 ] } }, + { "type": "Feature", "properties": { "id": 13 }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 14 }, "geometry": { "type": "Point", "coordinates": [ 1.6, -1.6 ] } }, + { "type": "Feature", "properties": { "id": 15 }, "geometry": { "type": "Point", "coordinates": [ -1.6, 1.6 ] } }, + { "type": "Feature", "properties": { "id": 16 }, "geometry": { "type": "Point", "coordinates": [ -0.6, 1.6 ] } }, + { "type": "Feature", "properties": { "id": 17 }, "geometry": { "type": "Point", "coordinates": [ 0.6, 1.6 ] } }, + { "type": "Feature", "properties": { "id": 18 }, "geometry": { "type": "Point", "coordinates": [ 1.6, 1.6 ] } }, + { "type": "Feature", "properties": { "id": 19 }, "geometry": { "type": "Point", "coordinates": [ -2.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 20 }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.6 ], [ -1.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.6 ], [ -1.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 8, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.6 ], [ -1.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 16, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.6 ], [ -0.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 9, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.6 ], [ -0.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 5, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.6 ], [ -0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 2, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.6 ], [ 0.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 6, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.6 ], [ 0.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 10, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.6 ], [ 0.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 17, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.6 ], [ 1.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 18, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 1.6 ], [ 1.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.6 ], [ 1.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 33, "startid": 12, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.6 ], [ 1.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 34, "startid": 14, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -1.6 ], [ 0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 35, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.6 ], [ -1.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 36, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.6 ], [ -0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 37, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.6 ], [ -1.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 38, "startid": 15, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 1.6 ], [ -0.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 39, "startid": 17, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.6 ], [ 0.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 40, "startid": 10, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.6 ], [ 0.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 41, "startid": 6, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.6 ], [ 0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 42, "startid": 2, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.6 ], [ 1.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 43, "startid": 14, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -1.6 ], [ 1.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 44, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.6 ], [ 1.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.6 ], [ 1.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 46, "startid": 18, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 1.6 ], [ 0.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 1.6 ], [ -1.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 48, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.6 ], [ -1.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 49, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.6 ], [ -1.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 50, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.6 ], [ -0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 51, "startid": 1, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.6 ], [ -0.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 52, "startid": 5, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.6 ], [ -0.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 53, "startid": 9, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.6 ], [ -0.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 54, "startid": 16, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.6 ], [ 0.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 55, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.6 ], [ -0.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 56, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.6 ], [ 0.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 57, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.6 ], [ -0.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 58, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.6 ], [ -0.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 59, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.6 ], [ 0.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 60, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.6 ], [ 0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 61, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.6 ], [ -0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 62, "startid": 5, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.6 ], [ -1.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 63, "startid": 7, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.6 ], [ -0.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 64, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.6 ], [ -1.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 65, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.6 ], [ -0.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 66, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.6 ], [ 1.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 67, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.6 ], [ 0.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 68, "startid": 12, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.6 ], [ 0.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 69, "startid": 6, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.6 ], [ 1.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 70, "startid": 3, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -2.0 ], [ 0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 71, "startid": 3, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -2.0 ], [ -0.6, -1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 72, "startid": 1, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.6 ], [ 0.0, -2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 73, "startid": 2, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.6 ], [ 0.0, -2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 74, "startid": 11, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.6 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 75, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 76, "startid": 12, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.6 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 77, "startid": 13, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.6, 0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 78, "startid": 16, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.6 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 79, "startid": 20, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 80, "startid": 17, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.6 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 81, "startid": 20, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ -0.6, 1.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 82, "startid": 8, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.6 ], [ -2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 83, "startid": 19, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, 0.0 ], [ -1.6, -0.6 ] ] ] } }, + { "type": "Feature", "properties": { "id": 84, "startid": 7, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.6 ], [ -2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 85, "startid": 19, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, 0.0 ], [ -1.6, 0.6 ] ] ] } } + ] +} diff --git a/nav2_bringup/graphs/turtlebot4_graph.geojson b/nav2_bringup/graphs/turtlebot4_graph.geojson new file mode 100644 index 00000000000..f7b922e9e5a --- /dev/null +++ b/nav2_bringup/graphs/turtlebot4_graph.geojson @@ -0,0 +1,119 @@ +{ + "type": "FeatureCollection", + "name": "turtlebot4_graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 0 }, "geometry": { "type": "Point", "coordinates": [ 0.624282608695647, 12.880652173913044 ] } }, + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ 0.584239130434777, 7.835173913043479 ] } }, + { "type": "Feature", "properties": { "id": 2 }, "geometry": { "type": "Point", "coordinates": [ 0.544195652173907, 2.569456521739133 ] } }, + { "type": "Feature", "properties": { "id": 3 }, "geometry": { "type": "Point", "coordinates": [ 7.651913043478253, 7.915260869565218 ] } }, + { "type": "Feature", "properties": { "id": 4 }, "geometry": { "type": "Point", "coordinates": [ 9.453869565217383, 0.727456521739134 ] } }, + { "type": "Feature", "properties": { "id": 5 }, "geometry": { "type": "Point", "coordinates": [ 13.79858695652173, 6.914173913043479 ] } }, + { "type": "Feature", "properties": { "id": 6 }, "geometry": { "type": "Point", "coordinates": [ 13.698478260869557, 1.328108695652176 ] } }, + { "type": "Feature", "properties": { "id": 7 }, "geometry": { "type": "Point", "coordinates": [ 15.820782608695643, 6.974239130434784 ] } }, + { "type": "Feature", "properties": { "id": 8 }, "geometry": { "type": "Point", "coordinates": [ 15.900869565217382, 9.036478260869565 ] } }, + { "type": "Feature", "properties": { "id": 9 }, "geometry": { "type": "Point", "coordinates": [ 15.940913043478252, 11.719391304347827 ] } }, + { "type": "Feature", "properties": { "id": 10 }, "geometry": { "type": "Point", "coordinates": [ 18.383565217391293, 6.934195652173914 ] } }, + { "type": "Feature", "properties": { "id": 11 }, "geometry": { "type": "Point", "coordinates": [ 18.3235, 9.036478260869565 ] } }, + { "type": "Feature", "properties": { "id": 12 }, "geometry": { "type": "Point", "coordinates": [ 18.283456521739122, 11.8195 ] } }, + { "type": "Feature", "properties": { "id": 13 }, "geometry": { "type": "Point", "coordinates": [ 20.125456521739121, 11.839521739130435 ] } }, + { "type": "Feature", "properties": { "id": 14 }, "geometry": { "type": "Point", "coordinates": [ 20.065391304347816, 9.116565217391305 ] } }, + { "type": "Feature", "properties": { "id": 15 }, "geometry": { "type": "Point", "coordinates": [ 19.985304347826077, 6.974239130434784 ] } }, + { "type": "Feature", "properties": { "id": 16 }, "geometry": { "type": "Point", "coordinates": [ 22.167673913043469, 6.914173913043479 ] } }, + { "type": "Feature", "properties": { "id": 17 }, "geometry": { "type": "Point", "coordinates": [ 22.047543478260859, 9.116565217391305 ] } }, + { "type": "Feature", "properties": { "id": 18 }, "geometry": { "type": "Point", "coordinates": [ 23.869521739130423, 11.77945652173913 ] } }, + { "type": "Feature", "properties": { "id": 19 }, "geometry": { "type": "Point", "coordinates": [ 23.8495, 9.216673913043479 ] } }, + { "type": "Feature", "properties": { "id": 20 }, "geometry": { "type": "Point", "coordinates": [ 23.789434782608684, 6.934195652173914 ] } }, + { "type": "Feature", "properties": { "id": 21 }, "geometry": { "type": "Point", "coordinates": [ 26.071913043478251, 6.954217391304349 ] } }, + { "type": "Feature", "properties": { "id": 22 }, "geometry": { "type": "Point", "coordinates": [ 25.951782608695641, 9.196652173913044 ] } }, + { "type": "Feature", "properties": { "id": 23 }, "geometry": { "type": "Point", "coordinates": [ 25.971804347826076, 11.8195 ] } }, + { "type": "Feature", "properties": { "id": 24 }, "geometry": { "type": "Point", "coordinates": [ 28.234260869565205, 11.85954347826087 ] } }, + { "type": "Feature", "properties": { "id": 25 }, "geometry": { "type": "Point", "coordinates": [ 28.55460869565216, 6.954217391304349 ] } }, + { "type": "Feature", "properties": { "id": 26 }, "geometry": { "type": "Point", "coordinates": [ 28.59465217391303, 5.552695652173915 ] } }, + { "type": "Feature", "properties": { "id": 27 }, "geometry": { "type": "Point", "coordinates": [ 28.59465217391303, 3.130065217391307 ] } }, + { "type": "Feature", "properties": { "id": 28 }, "geometry": { "type": "Point", "coordinates": [ 28.614673913043465, 1.448239130434786 ] } }, + { "type": "Feature", "properties": { "id": 29 }, "geometry": { "type": "Point", "coordinates": [ 25.150913043478248, 1.348130434782611 ] } }, + { "type": "Feature", "properties": { "id": 30 }, "geometry": { "type": "Point", "coordinates": [ 22.508043478260859, 1.388173913043481 ] } }, + { "type": "Feature", "properties": { "id": 31 }, "geometry": { "type": "Point", "coordinates": [ 19.825130434782597, 1.348130434782611 ] } }, + { "type": "Feature", "properties": { "id": 32 }, "geometry": { "type": "Point", "coordinates": [ 16.86191304347825, 1.368152173913046 ] } }, + { "type": "Feature", "properties": { "id": 33 }, "geometry": { "type": "Point", "coordinates": [ 13.698478260869557, 1.328108695652176 ] } }, + { "type": "Feature", "properties": { "id": 10000, "startid": 1, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.584239130434777, 7.835173913043479 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10001, "startid": 3, "endid": 1 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.584239130434777, 7.835173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10002, "startid": 0, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.624282608695647, 12.880652173913044 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10003, "startid": 3, "endid": 0 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.624282608695647, 12.880652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10004, "startid": 2, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.544195652173907, 2.569456521739133 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10005, "startid": 3, "endid": 2 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.544195652173907, 2.569456521739133 ] ] } }, + { "type": "Feature", "properties": { "id": 10006, "startid": 6, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10006, "startid": 33, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10007, "startid": 4, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10007, "startid": 4, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10008, "startid": 4, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10009, "startid": 3, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10010, "startid": 3, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10011, "startid": 5, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10012, "startid": 5, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10012, "startid": 5, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10013, "startid": 6, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10013, "startid": 33, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10014, "startid": 5, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10015, "startid": 7, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10016, "startid": 7, "endid": 8 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 15.900869565217382, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10017, "startid": 8, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.900869565217382, 9.036478260869565 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10018, "startid": 8, "endid": 9 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.900869565217382, 9.036478260869565 ], [ 15.940913043478252, 11.719391304347827 ] ] } }, + { "type": "Feature", "properties": { "id": 10019, "startid": 9, "endid": 8 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.940913043478252, 11.719391304347827 ], [ 15.900869565217382, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10020, "startid": 6, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10020, "startid": 33, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10021, "startid": 32, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10021, "startid": 32, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10022, "startid": 7, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10023, "startid": 10, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10024, "startid": 10, "endid": 11 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 18.3235, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10025, "startid": 11, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.3235, 9.036478260869565 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10026, "startid": 11, "endid": 12 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.3235, 9.036478260869565 ], [ 18.283456521739122, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10027, "startid": 12, "endid": 11 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.283456521739122, 11.8195 ], [ 18.3235, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10028, "startid": 12, "endid": 13 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.283456521739122, 11.8195 ], [ 20.125456521739121, 11.839521739130435 ] ] } }, + { "type": "Feature", "properties": { "id": 10029, "startid": 13, "endid": 12 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.125456521739121, 11.839521739130435 ], [ 18.283456521739122, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10030, "startid": 13, "endid": 14 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.125456521739121, 11.839521739130435 ], [ 20.065391304347816, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10031, "startid": 14, "endid": 13 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.065391304347816, 9.116565217391305 ], [ 20.125456521739121, 11.839521739130435 ] ] } }, + { "type": "Feature", "properties": { "id": 10032, "startid": 14, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.065391304347816, 9.116565217391305 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10033, "startid": 15, "endid": 14 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 20.065391304347816, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10034, "startid": 15, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10035, "startid": 10, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10036, "startid": 32, "endid": 31 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 19.825130434782597, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10037, "startid": 31, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.825130434782597, 1.348130434782611 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10038, "startid": 31, "endid": 30 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.825130434782597, 1.348130434782611 ], [ 22.508043478260859, 1.388173913043481 ] ] } }, + { "type": "Feature", "properties": { "id": 10039, "startid": 30, "endid": 31 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.508043478260859, 1.388173913043481 ], [ 19.825130434782597, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10040, "startid": 30, "endid": 29 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.508043478260859, 1.388173913043481 ], [ 25.150913043478248, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10041, "startid": 29, "endid": 30 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.150913043478248, 1.348130434782611 ], [ 22.508043478260859, 1.388173913043481 ] ] } }, + { "type": "Feature", "properties": { "id": 10042, "startid": 29, "endid": 28 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.150913043478248, 1.348130434782611 ], [ 28.614673913043465, 1.448239130434786 ] ] } }, + { "type": "Feature", "properties": { "id": 10043, "startid": 28, "endid": 29 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.614673913043465, 1.448239130434786 ], [ 25.150913043478248, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10044, "startid": 28, "endid": 27 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.614673913043465, 1.448239130434786 ], [ 28.59465217391303, 3.130065217391307 ] ] } }, + { "type": "Feature", "properties": { "id": 10045, "startid": 27, "endid": 28 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 3.130065217391307 ], [ 28.614673913043465, 1.448239130434786 ] ] } }, + { "type": "Feature", "properties": { "id": 10046, "startid": 27, "endid": 26 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 3.130065217391307 ], [ 28.59465217391303, 5.552695652173915 ] ] } }, + { "type": "Feature", "properties": { "id": 10047, "startid": 26, "endid": 27 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 5.552695652173915 ], [ 28.59465217391303, 3.130065217391307 ] ] } }, + { "type": "Feature", "properties": { "id": 10048, "startid": 26, "endid": 25 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 5.552695652173915 ], [ 28.55460869565216, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10049, "startid": 25, "endid": 26 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.55460869565216, 6.954217391304349 ], [ 28.59465217391303, 5.552695652173915 ] ] } }, + { "type": "Feature", "properties": { "id": 10050, "startid": 15, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10051, "startid": 16, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10052, "startid": 16, "endid": 17 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 22.047543478260859, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10053, "startid": 17, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.047543478260859, 9.116565217391305 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10054, "startid": 17, "endid": 18 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.047543478260859, 9.116565217391305 ], [ 23.869521739130423, 11.77945652173913 ] ] } }, + { "type": "Feature", "properties": { "id": 10055, "startid": 18, "endid": 17 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.869521739130423, 11.77945652173913 ], [ 22.047543478260859, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10056, "startid": 18, "endid": 19 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.869521739130423, 11.77945652173913 ], [ 23.8495, 9.216673913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10057, "startid": 19, "endid": 18 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.8495, 9.216673913043479 ], [ 23.869521739130423, 11.77945652173913 ] ] } }, + { "type": "Feature", "properties": { "id": 10058, "startid": 19, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.8495, 9.216673913043479 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10059, "startid": 20, "endid": 19 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 23.8495, 9.216673913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10060, "startid": 20, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10061, "startid": 16, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10062, "startid": 20, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 26.071913043478251, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10063, "startid": 21, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10064, "startid": 21, "endid": 22 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 25.951782608695641, 9.196652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10065, "startid": 22, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.951782608695641, 9.196652173913044 ], [ 26.071913043478251, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10066, "startid": 22, "endid": 23 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.951782608695641, 9.196652173913044 ], [ 25.971804347826076, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10067, "startid": 23, "endid": 22 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.971804347826076, 11.8195 ], [ 25.951782608695641, 9.196652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10068, "startid": 23, "endid": 24 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.971804347826076, 11.8195 ], [ 28.234260869565205, 11.85954347826087 ] ] } }, + { "type": "Feature", "properties": { "id": 10069, "startid": 24, "endid": 23 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.234260869565205, 11.85954347826087 ], [ 25.971804347826076, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10070, "startid": 21, "endid": 25 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 28.55460869565216, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10071, "startid": 25, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.55460869565216, 6.954217391304349 ], [ 26.071913043478251, 6.954217391304349 ] ] } } + ] +} diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 8efd9b490f9..4f60682632c 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -35,6 +35,7 @@ def generate_launch_description() -> LaunchDescription: namespace = LaunchConfiguration('namespace') slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') + graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') @@ -44,11 +45,6 @@ def generate_launch_description() -> LaunchDescription: use_localization = LaunchConfiguration('use_localization') # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] configured_params = ParameterFile( @@ -77,6 +73,11 @@ def generate_launch_description() -> LaunchDescription: 'map', default_value='', description='Full path to map yaml file to load' ) + declare_graph_file_cmd = DeclareLaunchArgument( + 'graph', + default_value='', description='Path to the graph file to load' + ) + declare_use_localization_cmd = DeclareLaunchArgument( 'use_localization', default_value='True', description='Whether to enable localization or not' @@ -167,6 +168,7 @@ def generate_launch_description() -> LaunchDescription: 'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, + 'graph': graph_filepath, 'params_file': params_file, 'use_composition': use_composition, 'use_respawn': use_respawn, @@ -186,6 +188,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 2bd90b5262e..8c7d51bdfe6 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -58,6 +58,7 @@ def generate_robot_actions(name: str = '', pose: dict[str, float] = {}) -> list[ autostart = LaunchConfiguration('autostart') rviz_config_file = LaunchConfiguration('rviz_config') map_yaml_file = LaunchConfiguration('map') + graph_filepath = LaunchConfiguration('graph') use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') # Define commands for launching the navigation instances @@ -83,6 +84,7 @@ def generate_robot_actions(name: str = '', pose: dict[str, float] = {}) -> list[ launch_arguments={ 'namespace': name, 'map': map_yaml_file, + 'graph': graph_filepath, 'use_sim_time': 'True', 'params_file': params_file, 'autostart': autostart, @@ -155,6 +157,11 @@ def generate_launch_description() -> LaunchDescription: description='Full path to map file to load', ) + declare_graph_file_cmd = DeclareLaunchArgument( + 'graph', + default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot3_graph.geojson'), + ) + declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join( @@ -215,6 +222,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_world_cmd) ld.add_action(declare_robots_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_graph_file_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 9a7fded30c2..41b57fb2ffa 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -43,11 +43,6 @@ def generate_launch_description() -> LaunchDescription: lifecycle_nodes = ['map_server', 'amcl'] # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] configured_params = ParameterFile( diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 31c543cbeb8..22accb697ca 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -31,6 +31,7 @@ def generate_launch_description() -> LaunchDescription: namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') autostart = LaunchConfiguration('autostart') + graph_filepath = LaunchConfiguration('graph') params_file = LaunchConfiguration('params_file') use_composition = LaunchConfiguration('use_composition') container_name = LaunchConfiguration('container_name') @@ -42,6 +43,7 @@ def generate_launch_description() -> LaunchDescription: 'controller_server', 'smoother_server', 'planner_server', + 'route_server', 'behavior_server', 'velocity_smoother', 'collision_monitor', @@ -51,11 +53,6 @@ def generate_launch_description() -> LaunchDescription: ] # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Create our own temporary YAML files that include substitutions @@ -91,6 +88,11 @@ def generate_launch_description() -> LaunchDescription: description='Full path to the ROS2 parameters file to use for all launched nodes', ) + declare_graph_file_cmd = DeclareLaunchArgument( + 'graph', + default_value='', description='Path to the graph file to load' + ) + declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', @@ -155,6 +157,16 @@ def generate_launch_description() -> LaunchDescription: arguments=['--ros-args', '--log-level', log_level], remappings=remappings, ), + Node( + package='nav2_route', + executable='route_server', + name='route_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'graph_filepath': graph_filepath}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), Node( package='nav2_behaviors', executable='behavior_server', @@ -261,6 +273,12 @@ def generate_launch_description() -> LaunchDescription: parameters=[configured_params], remappings=remappings, ), + ComposableNode( + package='nav2_route', + plugin='nav2_route::RouteServer', + name='route_server', + parameters=[configured_params, {'graph_filepath': graph_filepath}], + remappings=remappings), ComposableNode( package='nav2_behaviors', plugin='behavior_server::BehaviorServer', @@ -328,6 +346,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) + ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_composition_cmd) ld.add_action(declare_container_name_cmd) ld.add_action(declare_use_respawn_cmd) diff --git a/nav2_bringup/launch/tb3_loopback_simulation_launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py index 8e1f6176568..26c22a31d50 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py @@ -37,6 +37,7 @@ def generate_launch_description() -> LaunchDescription: # Create the launch configuration variables namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') + graph_filepath = LaunchConfiguration('graph') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') @@ -59,6 +60,11 @@ def generate_launch_description() -> LaunchDescription: default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), ) + declare_graph_file_cmd = DeclareLaunchArgument( + 'graph', + default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot3_graph.geojson'), + ) + declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), @@ -131,6 +137,7 @@ def generate_launch_description() -> LaunchDescription: launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, + 'graph': graph_filepath, 'use_sim_time': 'True', 'params_file': params_file, 'autostart': autostart, @@ -189,6 +196,7 @@ def generate_launch_description() -> LaunchDescription: # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_graph_file_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 1a9698ba149..808580af0d4 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -38,6 +38,7 @@ def generate_launch_description() -> LaunchDescription: slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') + graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') @@ -78,6 +79,11 @@ def generate_launch_description() -> LaunchDescription: default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), ) + declare_graph_file_cmd = DeclareLaunchArgument( + 'graph', + default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot3_graph.geojson'), + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', @@ -183,6 +189,7 @@ def generate_launch_description() -> LaunchDescription: 'namespace': namespace, 'slam': slam, 'map': map_yaml_file, + 'graph': graph_filepath, 'use_sim_time': use_sim_time, 'params_file': params_file, 'autostart': autostart, @@ -241,6 +248,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_bringup/launch/tb4_loopback_simulation_launch.py b/nav2_bringup/launch/tb4_loopback_simulation_launch.py index 07866736123..5adf329af71 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation_launch.py @@ -37,6 +37,7 @@ def generate_launch_description() -> LaunchDescription: # Create the launch configuration variables namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') + graph_filepath = LaunchConfiguration('graph') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') @@ -60,6 +61,11 @@ def generate_launch_description() -> LaunchDescription: description='Full path to map file to load', ) + declare_graph_file_cmd = DeclareLaunchArgument( + 'graph', + default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'), + ) + declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), @@ -129,6 +135,7 @@ def generate_launch_description() -> LaunchDescription: launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, + 'graph': graph_filepath, 'use_sim_time': 'True', 'params_file': params_file, 'autostart': autostart, @@ -196,6 +203,7 @@ def generate_launch_description() -> LaunchDescription: # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_graph_file_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index 5bc0db49835..0dc98654ae8 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -41,6 +41,7 @@ def generate_launch_description() -> LaunchDescription: slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') + graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') @@ -82,6 +83,11 @@ def generate_launch_description() -> LaunchDescription: description='Full path to map file to load', ) + declare_graph_file_cmd = DeclareLaunchArgument( + 'graph', + default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'), + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', @@ -183,6 +189,7 @@ def generate_launch_description() -> LaunchDescription: 'namespace': namespace, 'slam': slam, 'map': map_yaml_file, + 'graph': graph_filepath, 'use_sim_time': use_sim_time, 'params_file': params_file, 'autostart': autostart, @@ -246,6 +253,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py index e0cdc96f9d4..ae5c0bb4458 100644 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -68,6 +68,7 @@ def generate_launch_description() -> LaunchDescription: # On this example all robots are launched with the same settings map_yaml_file = LaunchConfiguration('map') + graph_filepath = LaunchConfiguration('graph') autostart = LaunchConfiguration('autostart') rviz_config_file = LaunchConfiguration('rviz_config') @@ -88,6 +89,11 @@ def generate_launch_description() -> LaunchDescription: description='Full path to map file to load', ) + declare_graph_file_cmd = DeclareLaunchArgument( + 'graph', + default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot3_graph.geojson'), + ) + declare_robot1_params_file_cmd = DeclareLaunchArgument( 'robot1_params_file', default_value=os.path.join( @@ -164,6 +170,7 @@ def generate_launch_description() -> LaunchDescription: launch_arguments={ 'namespace': robot['name'], 'map': map_yaml_file, + 'graph': graph_filepath, 'use_sim_time': 'True', 'params_file': params_file, 'autostart': autostart, @@ -227,6 +234,7 @@ def generate_launch_description() -> LaunchDescription: # Declare the launch options ld.add_action(declare_world_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_graph_file_cmd) ld.add_action(declare_robot1_params_file_cmd) ld.add_action(declare_robot2_params_file_cmd) ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_bringup/maps/depot.yaml b/nav2_bringup/maps/depot.yaml index 8c7486b8ab6..08361c71c3e 100644 --- a/nav2_bringup/maps/depot.yaml +++ b/nav2_bringup/maps/depot.yaml @@ -1,7 +1,7 @@ image: depot.pgm mode: trinary resolution: 0.05 -origin: [-7.14, -7.83, 0] +origin: [0.0, 0.0, 0] negate: 0 occupied_thresh: 0.65 free_thresh: 0.25 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 6771397d364..9fabe985846 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -79,6 +79,7 @@ bt_navigator: - nav_to_pose - route - spin + - smoother - undock_robot - wait @@ -301,7 +302,7 @@ global_costmap: service_introspection_mode: "disabled" # The yaml_filename does not need to be specified since it going to be set by defaults in launch. -# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# If you'd rather set it in the yaml, remove the default "map" value in the launch file(s). # file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. map_server: ros__parameters: @@ -330,11 +331,22 @@ planner_server: smoother_server: ros__parameters: - smoother_plugins: ["simple_smoother"] + smoother_plugins: ["simple_smoother", "route_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" tolerance: 1.0e-10 max_its: 1000 + refinement_num: 2 + # True for Hybrid A* or State Lattice to not smooth over directional changes + enforce_path_inversion: True + do_refinement: True + route_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + refinement_num: 5 + # False for Route Server or others where smoothing out sharp corners is OK + enforce_path_inversion: False do_refinement: True behavior_server: @@ -383,6 +395,28 @@ waypoint_follower: enabled: True waypoint_pause_duration: 200 +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + velocity_smoother: ros__parameters: smoothing_frequency: 20.0 diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index df1bc58d5dc..71af24b27bb 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -7,6 +7,8 @@ Panels: - /Global Options1 - /TF1/Frames1 - /TF1/Tree1 + - /MarkerArray2/Status1 + - /MarkerArray2/Topic1 Splitter Ratio: 0.5833333134651184 Tree Height: 287 - Class: rviz_common/Selection @@ -524,6 +526,19 @@ Visualization Manager: Reliability Policy: Reliable Value: waypoints Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + route_graph: true + route_graph_ids: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: route_graph + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml new file mode 100644 index 00000000000..7df6d6e0cb6 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml new file mode 100644 index 00000000000..c25970af506 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_core/include/nav2_core/route_exceptions.hpp b/nav2_core/include/nav2_core/route_exceptions.hpp new file mode 100644 index 00000000000..aed9b304279 --- /dev/null +++ b/nav2_core/include/nav2_core/route_exceptions.hpp @@ -0,0 +1,83 @@ +// Copyright (c) 2023, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_CORE__ROUTE_EXCEPTIONS_HPP_ +#define NAV2_CORE__ROUTE_EXCEPTIONS_HPP_ + +#include +#include +#include + +namespace nav2_core +{ + +class RouteException : public std::runtime_error +{ +public: + explicit RouteException(const std::string & description) + : std::runtime_error(description) {} +}; + +class OperationFailed : public RouteException +{ +public: + explicit OperationFailed(const std::string & description) + : RouteException(description) {} +}; + +class NoValidRouteCouldBeFound : public RouteException +{ +public: + explicit NoValidRouteCouldBeFound(const std::string & description) + : RouteException(description) {} +}; + +class TimedOut : public RouteException +{ +public: + explicit TimedOut(const std::string & description) + : RouteException(description) {} +}; + +class RouteTFError : public RouteException +{ +public: + explicit RouteTFError(const std::string & description) + : RouteException(description) {} +}; + +class NoValidGraph : public RouteException +{ +public: + explicit NoValidGraph(const std::string & description) + : RouteException(description) {} +}; + +class IndeterminantNodesOnGraph : public RouteException +{ +public: + explicit IndeterminantNodesOnGraph(const std::string & description) + : RouteException(description) {} +}; + +class InvalidEdgeScorerUse : public RouteException +{ +public: + explicit InvalidEdgeScorerUse(const std::string & description) + : RouteException(description) {} +}; + +} // namespace nav2_core + +#endif // NAV2_CORE__ROUTE_EXCEPTIONS_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index 4f50614876c..955a8c80af4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -65,6 +65,11 @@ class CostmapSubscriber */ void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg); + std::string getFrameID() const + { + return frame_id_; + } + protected: bool isCostmapReceived() {return costmap_ != nullptr;} void processCurrentCostmapMsg(); @@ -81,6 +86,7 @@ class CostmapSubscriber nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; std::string topic_name_; + std::string frame_id_; std::mutex costmap_msg_mutex_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; }; diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 0426b87be30..bdbc5e11ccb 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -73,6 +73,7 @@ void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr { std::lock_guard lock(costmap_msg_mutex_); costmap_msg_ = msg; + frame_id_ = costmap_msg_->header.frame_id; } if (!isCostmapReceived()) { costmap_ = std::make_shared( diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 4319129ecbc..dc49bf40f01 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -31,9 +31,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/WaypointStatus.msg" + "msg/Route.msg" + "msg/RouteNode.msg" + "msg/RouteEdge.msg" + "msg/EdgeCost.msg" "msg/Trajectory.msg" "msg/TrajectoryPoint.msg" - "msg/WaypointStatus.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" "srv/IsPathValid.srv" @@ -45,6 +49,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveMap.srv" "srv/SetInitialPose.srv" "srv/ReloadDockDatabase.srv" + "srv/SetRouteGraph.srv" + "srv/DynamicEdges.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" @@ -61,6 +67,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/FollowGPSWaypoints.action" "action/DockRobot.action" "action/UndockRobot.action" + "action/ComputeRoute.action" + "action/ComputeAndTrackRoute.action" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs ) diff --git a/nav2_msgs/action/ComputeAndTrackRoute.action b/nav2_msgs/action/ComputeAndTrackRoute.action new file mode 100644 index 00000000000..0631ee20f8b --- /dev/null +++ b/nav2_msgs/action/ComputeAndTrackRoute.action @@ -0,0 +1,34 @@ +#goal definition +uint16 start_id +geometry_msgs/PoseStamped start +uint16 goal_id +geometry_msgs/PoseStamped goal + +bool use_start # Whether to use the start field or find the start pose in TF +bool use_poses # Whether to use the poses or the IDs fields for request +--- +#result definition + +# Error codes +uint16 NONE=0 +uint16 UNKNOWN=400 +uint16 TF_ERROR=401 +uint16 NO_VALID_GRAPH=402 +uint16 INDETERMINANT_NODES_ON_GRAPH=403 +uint16 TIMEOUT=404 +uint16 NO_VALID_ROUTE=405 +uint16 OPERATION_FAILED=406 +uint16 INVALID_EDGE_SCORER_USE=407 + +builtin_interfaces/Duration execution_duration +uint16 error_code 0 +string error_msg +--- +#feedback definition +uint16 last_node_id +uint16 next_node_id +uint16 current_edge_id +Route route +nav_msgs/Path path +string[] operations_triggered +bool rerouted diff --git a/nav2_msgs/action/ComputeRoute.action b/nav2_msgs/action/ComputeRoute.action new file mode 100644 index 00000000000..60bee2205b8 --- /dev/null +++ b/nav2_msgs/action/ComputeRoute.action @@ -0,0 +1,28 @@ +#goal definition +uint16 start_id +geometry_msgs/PoseStamped start +uint16 goal_id +geometry_msgs/PoseStamped goal + +bool use_start # Whether to use the start field or find the start pose in TF +bool use_poses # Whether to use the poses or the IDs fields for request +--- +#result definition + +# Error codes +uint16 NONE=0 +uint16 UNKNOWN=400 +uint16 TF_ERROR=401 +uint16 NO_VALID_GRAPH=402 +uint16 INDETERMINANT_NODES_ON_GRAPH=403 +uint16 TIMEOUT=404 +uint16 NO_VALID_ROUTE=405 +uint16 INVALID_EDGE_SCORER_USE=407 + +builtin_interfaces/Duration planning_time +nav_msgs/Path path +Route route +uint16 error_code 0 +string error_msg +--- +#feedback definition diff --git a/nav2_msgs/action/__init__.py b/nav2_msgs/action/__init__.py index a4036af8cda..e3dee59a2e0 100644 --- a/nav2_msgs/action/__init__.py +++ b/nav2_msgs/action/__init__.py @@ -1,7 +1,9 @@ from nav2_msgs.action._assisted_teleop import AssistedTeleop from nav2_msgs.action._back_up import BackUp +from nav2_msgs.action._compute_and_track_route import ComputeAndTrackRoute from nav2_msgs.action._compute_path_through_poses import ComputePathThroughPoses from nav2_msgs.action._compute_path_to_pose import ComputePathToPose +from nav2_msgs.action._compute_route import ComputeRoute from nav2_msgs.action._dock_robot import DockRobot from nav2_msgs.action._drive_on_heading import DriveOnHeading from nav2_msgs.action._dummy_behavior import DummyBehavior @@ -18,8 +20,10 @@ __all__ = [ 'AssistedTeleop', 'BackUp', + 'ComputeAndTrackRoute', 'ComputePathThroughPoses', 'ComputePathToPose', + 'ComputeRoute', 'DockRobot', 'DriveOnHeading', 'DummyBehavior', diff --git a/nav2_msgs/msg/EdgeCost.msg b/nav2_msgs/msg/EdgeCost.msg new file mode 100644 index 00000000000..5200aee851f --- /dev/null +++ b/nav2_msgs/msg/EdgeCost.msg @@ -0,0 +1,3 @@ +# Edge cost to use with nav2_msgs/srv/DynamicEdges to adjust route edge costs +uint16 edgeid +float32 cost diff --git a/nav2_msgs/msg/Route.msg b/nav2_msgs/msg/Route.msg new file mode 100644 index 00000000000..0ebc591c9c8 --- /dev/null +++ b/nav2_msgs/msg/Route.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +float32 route_cost +RouteNode[] nodes # ordered set of nodes of the route +RouteEdge[] edges # ordered set of edges that connect nodes diff --git a/nav2_msgs/msg/RouteEdge.msg b/nav2_msgs/msg/RouteEdge.msg new file mode 100644 index 00000000000..d257c9edce4 --- /dev/null +++ b/nav2_msgs/msg/RouteEdge.msg @@ -0,0 +1,3 @@ +uint16 edgeid +geometry_msgs/Point start +geometry_msgs/Point end diff --git a/nav2_msgs/msg/RouteNode.msg b/nav2_msgs/msg/RouteNode.msg new file mode 100644 index 00000000000..89f328a50e3 --- /dev/null +++ b/nav2_msgs/msg/RouteNode.msg @@ -0,0 +1,2 @@ +uint16 nodeid +geometry_msgs/Point position diff --git a/nav2_msgs/msg/__init__.py b/nav2_msgs/msg/__init__.py index a6e8fcd4219..6ab637b85c1 100644 --- a/nav2_msgs/msg/__init__.py +++ b/nav2_msgs/msg/__init__.py @@ -9,6 +9,7 @@ from nav2_msgs.msg._missed_waypoint import MissedWaypoint from nav2_msgs.msg._particle import Particle from nav2_msgs.msg._particle_cloud import ParticleCloud +from nav2_msgs.msg._route import Route from nav2_msgs.msg._speed_limit import SpeedLimit from nav2_msgs.msg._voxel_grid import VoxelGrid @@ -24,6 +25,7 @@ 'MissedWaypoint', 'Particle', 'ParticleCloud', + 'Route', 'SpeedLimit', 'VoxelGrid', ] diff --git a/nav2_msgs/srv/DynamicEdges.srv b/nav2_msgs/srv/DynamicEdges.srv new file mode 100644 index 00000000000..e725c7ef75a --- /dev/null +++ b/nav2_msgs/srv/DynamicEdges.srv @@ -0,0 +1,5 @@ +uint16[] closed_edges +uint16[] opened_edges +EdgeCost[] adjust_edges +--- +bool success diff --git a/nav2_msgs/srv/SetRouteGraph.srv b/nav2_msgs/srv/SetRouteGraph.srv new file mode 100644 index 00000000000..d2d8092b67c --- /dev/null +++ b/nav2_msgs/srv/SetRouteGraph.srv @@ -0,0 +1,3 @@ +string graph_filepath +--- +bool success diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index f949f980a95..7d29119f31b 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -384,6 +384,7 @@ void PlannerServer::computePlanThroughPoses() auto goal = action_server_poses_->get_current_goal(); auto result = std::make_shared(); nav_msgs::msg::Path concat_path; + RCLCPP_INFO(get_logger(), "Computing path through poses to goal."); geometry_msgs::msg::PoseStamped curr_start, curr_goal; @@ -515,6 +516,7 @@ PlannerServer::computePlan() // Initialize the ComputePathToPose goal and result auto goal = action_server_pose_->get_current_goal(); auto result = std::make_shared(); + RCLCPP_INFO(get_logger(), "Computing path to goal."); geometry_msgs::msg::PoseStamped start; diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt new file mode 100644 index 00000000000..db7825a0c96 --- /dev/null +++ b/nav2_route/CMakeLists.txt @@ -0,0 +1,269 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_route CXX) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav2_util REQUIRED) +find_package(angles REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nanoflann REQUIRED) +find_package(nlohmann_json REQUIRED) + +nav2_package() + +set(executable_name route_server) +set(library_name ${executable_name}_core) + +include_directories( + include +) + +# Main library +add_library(${library_name} SHARED + src/route_server.cpp + src/route_planner.cpp + src/route_tracker.cpp + src/edge_scorer.cpp + src/operations_manager.cpp + src/node_spatial_tree.cpp + src/path_converter.cpp + src/graph_loader.cpp + src/graph_saver.cpp + src/goal_intent_extractor.cpp +) + +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + nlohmann_json::nlohmann_json +) +target_link_libraries(${library_name} PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +# Main executable +add_executable(${executable_name} + src/main.cpp +) + +target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) + +rclcpp_components_register_nodes(${library_name} "nav2_route::RouteServer") + +# Edge scoring plugins +add_library(edge_scorers SHARED + src/plugins/edge_cost_functions/distance_scorer.cpp + src/plugins/edge_cost_functions/time_scorer.cpp + src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp + src/plugins/edge_cost_functions/penalty_scorer.cpp + src/plugins/edge_cost_functions/costmap_scorer.cpp + src/plugins/edge_cost_functions/semantic_scorer.cpp + src/plugins/edge_cost_functions/goal_orientation_scorer.cpp + src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp +) +target_include_directories(edge_scorers + PUBLIC + "$" + "$" +) +target_link_libraries(edge_scorers PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + angles::angles + nlohmann_json::nlohmann_json +) +target_link_libraries(edge_scorers PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +# Route operations plugins +add_library(route_operations SHARED + src/plugins/route_operations/adjust_speed_limit.cpp + src/plugins/route_operations/trigger_event.cpp + src/plugins/route_operations/rerouting_service.cpp + src/plugins/route_operations/collision_monitor.cpp + src/plugins/route_operations/time_marker.cpp +) +target_include_directories(route_operations + PUBLIC + "$" + "$" +) +target_link_libraries(route_operations PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + nlohmann_json::nlohmann_json +) +target_link_libraries(route_operations PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +# Graph Parser plugins +add_library(graph_file_loaders SHARED + src/plugins/graph_file_loaders/geojson_graph_file_loader.cpp +) +target_include_directories(graph_file_loaders + PUBLIC + "$" + "$" +) +target_link_libraries(graph_file_loaders PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + nlohmann_json::nlohmann_json +) +target_link_libraries(graph_file_loaders PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +add_library(graph_file_savers SHARED + src/plugins/graph_file_savers/geojson_graph_file_saver.cpp +) +target_include_directories(graph_file_savers + PUBLIC + "$" + "$" +) +target_link_libraries(graph_file_savers PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + nlohmann_json::nlohmann_json +) +target_link_libraries(graph_file_savers PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +pluginlib_export_plugin_description_file(nav2_route plugins.xml) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS ${executable_name} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(TARGETS ${library_name} edge_scorers route_operations graph_file_loaders graph_file_savers + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY graphs DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY test/test_graphs DESTINATION share/${PROJECT_NAME}/test) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_dependencies( + rclcpp + rclcpp_lifecycle + rclcpp_components + std_msgs + geometry_msgs + nav2_costmap_2d + pluginlib + visualization_msgs + nav_msgs + tf2_ros + nav2_core + nanoflann + nlohmann_json +) +ament_export_libraries(${library_name} edge_scorers route_operations graph_file_loaders graph_file_savers) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/nav2_route/README.md b/nav2_route/README.md new file mode 100644 index 00000000000..a434276bff8 --- /dev/null +++ b/nav2_route/README.md @@ -0,0 +1,427 @@ +# Nav2 Route Server + +The Route Server is a Nav2 Task server to compliment the Planner Server's free-space planning capabilities with pre-defined Navigation Route Graph planning, created by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) at [Open Navigation](https://www.opennav.org/) with assistance from [Josh Wallace](https://www.linkedin.com/in/joshua-wallace-b4848a113/) at Locus Robotics. It can be used to: +* Fully replace free-space planning when following a particular route closely is required (less Controller plugins' tuning to deviate or track the path closely), or +* Augment the global planner with long-distance routing to a goal and using free-space feasible planning in a more localized fashion for the immediate 10m, 100m, etc future. + +This graph has very few rules associated with it and may be generated manually or automatically via AI, geometric, or probabilistic techniques. +docs.nav2.org includes tutorials for how to generate such a graph by annotations on a grid map created via SLAM, but can also be procedurally generated. +This package then takes a planning request and uses this graph to find a valid route through the environment via an optimal search-based algorithm. It uses plugin-based scoring functions applied each edge based on arbitrary user-defined semantic information and the chosen optimization criteria(s). + +The Nav2 Route Server may also live monitor and analyze the route's process to execute custom behaviors on entering or leaving edges or achieving particular graph nodes. These behaviors are defined as another type of plugin and can leverage the graph's edges' and nodes' arbitrary semantic data. + +There are plugin interfaces throughout the server to enable a great deal of application-specific customization: +- Custom search-prioritization behavior with edge scoring plugins (e.g. minimize distance or time, mark blocked routes, enact static or dynamic penalties for danger and application-specific knowledge, prefer main arteries) +- Custom operations to perform during route execution: (a) triggered when entering or leaving an edge, (b) achieving a graph node on the route, (c) performed consistently (e.g. open door, pause at node to wait for clearance, adjust maximum speed, turn on lights, change mode, check for future collisions) +- Parsers of navigation graph files to use any type of format desirable (e.g. geoJSON, OpenStreetMap) + +Additionally, the server leverages **additional arbitrary metadata** specified within the navigation graph files to store information such as speed limits, added costs, or operations to perform. +Thus, we do not restrict the data that can be embedded in the navigation route graph for an application and this metadata is communicated to the edge scoring and operations plugins to adjust behavior as demanded by the application. +Note that plugins may also use outside information from topics, services, and actions for dynamic behavior or centralized knowledge sharing as well if desired. + +[A rudimentary demo of the basic features can be seen here](https://www.youtube.com/watch?v=T57pac6q4RU) using the `route_example_launch.py` provided in `nav2_simple_commander`. + +## Features + +- 98% Unit Test Coverage +- Optimized Dikjstra's planning algorithm modeled off of the Smac Planner A* implementation +- Use of Kd-trees for finding the nearest node(s) to arbitrary start and goal poses in the graph for pose-based planning requests. Optional use of Breadth-First Search to refine those nearest node(s) from simply Euclidean distance to considering traversibility length in the costmap. +- Highly efficient graph representation to maximize caching in a single data structure containing both edges' and nodes' objects and relationships with localized information +- All edges are directional allowing for single-direction lanes or planning +- Data in files may be with respect to any frame in the TF tree and are transformed to a centralized frame automatically +- Action interface response returns both a sparse route of nodes and edges for client applications with navigation graph knowledge and `nav_msgs/Path` dense paths minimicking freespace planning for drop-in behavior replacement of the Planner Server. +- Action interface request can process requests with start / goal node IDs or euclidean poses +- Service interface to change navigation route graphs at run-time +- Edge scoring dynamic plugins return a cost for traversing an edge and may mark an edge as invalid in current conditions from sensor or system state information +- Graph file parsing dynamic plugins allow for use of custom or proprietary formats +- Operation dynamic plugins to perform arbitrary tasks at a given node or when entering or leaving an edge on the route +- Operation may be graph-centric (e.g. graph file identifies operation to perform at a time) or plugin-centric (e.g. plugins self-identify nodes and edges to act upon during execution) +- Operations may trigger rerouting if necessary (e.g. due to new information, blockages, multi-robot data, etc) +- The nodes and edges metadata may be modified or used to communicate information across plugins including different types across different runs +- The Route Tracking action returns regular feedback on important events or state updates (e.g. rerouting requests, passed a node, triggered an operation, etc) +- If rerouting occurs during Route Tracking along the previous current edge, that state will be retained for improved behavior and provide an interpolated nav_msgs/Path from the closest point on the edge to the edge's end (or rerouting's starting node) to minimize free-space planning connections where a known edge exists and is being continued. + +## Practical Architectures for Use + +There are several practical architectures and designs for how this Route Serve can be assembled into a robotics solution. +Which to use depends on the nature of an application's needs and behavior. +This is not an exhaustive list, but enough to get users started thinking about how to fit this into their system. +Architectures (1) and (2) are implemented and tested in `nav2_bt_navigator/behavior_trees` for immediate use! + +* 1. Route Server's output dense path -> Controller Server for direct route following + - This is most useful when close or exact route following is required, fully replaces the Planner Server + - Considering adding in the Smoother Server or spline fitting between the Route Server and Controller Server in a Behavior Tree to smooth out the edge transitions for following + - Consider ComputingPathToPose for first-mile and last-mile on and off the route if leaving the graph is desirable. +* 2. Route Server's output sparse route -> Planner Server to plan to the next node(s) -> Controller Server to track global path + - This is useful when you want to follow the general route, but wish to have the flexibility to leave the route when obstacles are in the way and need the free-space planner to do so + - This is also useful in conjunction with (1) above as a fallback behavior to follow the route when possible, leave when blocked (after some time, or proactively when it sees blocked in immediate future), and then track the route again. This is great in situations where you want to only leave the route when required but otherwise track the route closely + - Consider using ComputePathToPose to plan to the next node in the route and change nodes as you approach the next + - Consider using ComputePathThroughPoses to plan through several nodes in the future to have smooth interchange +* 3. Route Server's output sparse route -> Waypoint Follower or Navigate Through Poses to navigate with task + - Similar to (2), this is useful to take the nodes and edges and navigate along the intended route using intelligent navigation + - This architecturally puts the route planning in the higher-level application-autonomy logic rather than with in the main navigation-planning task logic, which could be useful separation of concerns for some applications. +* 4. Create a behavior tree to NavigateToPose to the first node in the graph, then select (1) or (2) to track the route, finally NavigateToPose to get to the final goal pose off of the graph + - This is useful when the start and/or goal poses are not on the navigation graph, and thus the robot needs to navigate to its starting node or final goal pose in a 'first-mile' and 'last-mile' style task +* 5. Route Server's `ComputeAndTrackRoute` instead of `ComputeRoute` and send the dense path in (1) or sparse route in (2) or (3) + - This is useful to track the progress of the route in the Route Server while following the route as an application sees fit. This process allows for the triggering of spatial, graph, or contextual behaviors while executing the task like adjusting speeds, turning on lights, rerouting due to multi-robot coordination resource constraints, opening doors, etc. This has a wide ranging set of applications. +* 6. Teleoping a robot -> having a script which automatically stores new nodes and/or operator manually triggers a node capture -> saving this to file -> annotating file with operation plugin to do at each waypoint (if any) -> later using the graph to navigate the robot and perform tasks + - This is one possible way to setup a Teach-and-Repeat behavior using the route server with custom behaviors at each node. There are likely many. +* 7. Multi-floor navigation using graph nodes as terminals for stairs, elevators, etc + - Then free-space planning can be used between on-floor nodes and graph connections for floor connectors to enact specific behaviors to call elevators, climb stairs, etc. + +## Design + +The Nav2 Route Server is designed as several composed objects to make the system easy to understand and easily unit testable. The breakdown exists between different classes of capabilities like ROS 2 Interfaces (e.g. actions, services, debugging topics), the core search algorithm, scoring factory, route progress tracking, operations factory, file parsing, and action request 'intent' extraction. This distinction makes a relatively complex system easier to grasp, as there are a number of moving pieces. Luckily, few of these pieces (tracker is the exception) don't need to know much about each other so they can be isolated. + +The diagram below provides context for how the package is structured from the consititutent files you'll find in this project. + +

+ +

+ +Each have their own complete unit testing files named similarly in the `test/` directory. As the diagram suggests, plugin interfaces exist in the file loader, edge scorer, and operations manager to enable customizable behavior for different applications. + +### Plugin Interfaces + +Several plugin interfaces are provided to enable customizable behavior in the route search, route operation, and route graph file formatting. This allows for a great deal of customization for any number of applications which might want to (1) prioritize time, distance, or other application-specific criteria in routing; (2) perform custom operations or rerouting mechanics at run-time while progressing along the route such as adjusting speed or opening doors; (3) be able to integrate your own custom file format or another format of your interest. + +The interface definitions can be found in the `include/nav2_route/interfaces` directory and are mostly self explanatory via their method names and provided doxygen documentation. + +## Metrics + +A set of 1,000 experiments were run with the route planner with randomly selected start and goal poses, with various sized graphs. These metrics provide some reasonable benchmarking for performance of how the route server will perform in your application with various sized graphs that represent your environment: + +| Graph size | Ave. Search Time | +| ----------- | ------------------- | +| 100 | 0.0031 ms | +| 10,000 | 0.232 ms | +| 90,000 | 3.75 ms | +| 250,000 | 11.36 ms | +| 1,000,000 | 44.07 ms | + +This is in comparison with typical run-times of free-space global planners between 50 ms - 400 ms (depending on environment size and structure). Thus, the typical performance of using the route planner - even in truly massive environments to need hundreds of thousands of nodes of interest or importance - is well in excess of freespace planning. This enables Nav2 to operate in much larger spaces and still perform global routing. There's little reason then to use a heuristic-driven A* when Dikjstra's Algorithm is plenty fast for even massively connected graphs. + +The script used for this analysis can be found in `test/performance_benchmarking.cpp`. + +## Parameters + +``` +route_server: + ros__parameters: + + base_frame: "base_link" # Robot's base frame + route_frame: "map" # Global reference frame + path_density: 0.05 # Density of points for generating the dense nav_msgs/Path from route (m) + max_iterations: 0 # Maximum number of search iterations, if 0, uses maximum possible + max_planning_time: 2.0 # Maximum planning time (seconds) + costmap_topic: 'global_costmap/costmap_raw' # Costmap topic when enable_nn_search is enabled. May also be used by the collision monitor operation and/or the costmap edge scorer if using the same topic to share resources. + + graph_file_loader: "GeoJsonGraphFileLoader" # Name of default file loader + plugin: nav2_route::GeoJsonGraphFileLoader # file loader plugin to use + graph_filepath: "" # file path to graph to use + + edge_cost_functions: ["DistanceScorer", "DynamicEdgesScorer"] # Edge scoring cost functions to use + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + DynamicEdgesScorer: + plugin: "nav2_route::DynamicEdgesScorer" + + operations: ["AdjustSpeedLimit", "ReroutingService"] # Route operations plugins to use + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + ReroutingService: + plugin: "nav2_route::ReroutingService" + + tracker_update_rate: 50.0 # Rate at which to check the status of path tracking + aggregate_blocked_ids: false # Whether to aggregate the blocked IDs reported by route operations over the lifespan of the navigation request or only use the currently blocked IDs. + boundary_radius_to_achieve_node: 1.0 # Radius (m) near boundary nodes (e.g. start/end) to enable evaluation of achievement metric + radius_to_achieve_node: 2.0 # Radius (m) near route nodes as preliminary condition for evaluation of achievement metric + + max_prune_dist_from_edge: 8.0 # Max distance from an edge to consider pruning it as in-progress (e.g. if we're too far away from the edge, its nonsensical to prune it) + min_prune_dist_from_goal: 0.15 # Min distance from goal node away from goal pose to consider goal node pruning as considering it as being passed (in case goal pose is very close to a goal node, but not exact) + min_prune_dist_from_start: 0.10 # Min distance from start node away from start pose to consider start node pruning as considering it as being passed (in case start pose is very close to a start node, but not exact) + prune_goal: true # Whether pruning the goal nodes from the route due to being past the goal pose requested is possible (pose requests only) + enable_nn_search: true # Whether to enable breadth first search considering the costmap to find the node closest to the start and edge poses, rather than using the euclidean nearest neighbor alone. + max_nn_search_iterations: 10000 # Maximum number of iterations for breadth-first search. + num_nearest_nodes: 5 # The number of nearest-neighbors to extract for breadth-first search to consider. +``` + +#### `CostmapScorer` + +This edge scoring plugin will score based on the costmap values of the edge (e.g. using either maximum cost on the edge or average cost) + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| costmap_topic | Costmap topic for collision checking (`global_costmap/costmap_raw`) | +| max_cost | Maximum cost to consider an route blocked (253.0) | +| use_maximum | Whether to score based on single maximum or average (true) | +| invalid_on_collision | Whether to consider collision status as a terminal condition (true) | +| invalid_off_map | Whether to consider route going off the map invalid (true) | +| check_resolution | Resolution to check costs at (1 = costmap resolution, 2 = 2x costmap resolution, etc) | + +#### `DistanceScorer` + +This edge scoring plugin will score based on the distance length of the edge, weighted proportionally to percentage-based speed limits (if set in the graph) + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| speed_tag | Graph metadata key to look for % speed limits (speed_limit) | + +#### `DynamicEdgesScorer` + +This edge scoring plugin will score based on the requested values from a 3rd party application via a service interface. It can set dynamically any cost for any edge and also be used to close and reopen particular edges if they are blocked, in use by other robots locking out its shared use by other robots, higher cost due to overlap with other platforms in service, increased cost due to fleet manager analytics that this space is underperforming throughput, or otherwise temporarily non-traversible. For example, if other robots report an edge to be blocked, all robots can avoid this edge/aisle/etc. + +#### `TimeScorer` + +This edge scoring plugin will score based on the time to traverse the length of the edge. This will use the distance of the edge weighted in proportion to the absolute speed limits of the robot over an edge. If none is set in the graph, a parameterized maximum speed will be used. If an actual, measured time of a previous traversal is in the edge's metadata, this will be used. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| speed_tag | Graph metadata key to look for abs speed limits (abs_speed_limit) | +| time_tag | Graph metadata key to look for abs traversal times (abs_time_taken) | +| max_vel | Maximum velocity to use if speed limit or time taken is not set | + +#### `PenaltyScorer` + +This edge scoring plugin will score based on a statically set penalty in the graph file for a particular edge. This can be based on application known logic to weight preferences of navigation tactics in a space. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| penalty_tag | Graph metadata key to look for penalty value (penalty) | + +#### `SemanticScorer` + +This edge scoring plugin will score based on semantic information provided in the graph file. It can either check for the edge's semantic class via a parameterized key's value **or** search all key names to match known semantic classes to apply weight (e.g. `class: highway` or `highway: `). + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| semantic_classes | A list of semantic classes possible in your graph | +| `` | The cost to apply for a class (e.g. `highway: 8.4`) | +| semantic_key | Key to search edges for for semantic data (class). If empty string, will look at key names instead. | + +#### `StartPoseOrientationScorer` + +This edge scoring plugin will score an edge starting at the start node (vector from start->goal) based on its angular proximity to the starting pose's orientation. +This will either score a weighted-angular distance or reject traversals that are outside of a set threshold to force the route to go down a particular direction (i.e. direction robot is already facing). + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| orientation_weight | Relative scoring weight (1.0) | +| use_orientation_threshold | Whether to use orient. threshold or weighted-angular distance scoring (false) | +| orientation_tolerance | The angular threshold to reject edges' angles if greater than this w.r.t. starting pose, when `use_orientation_threshold: true` (PI/2) | + +#### `GoalPoseOrientationScorer` + +This edge scoring plugin will score a an edge with terminus of the goal node (vector from start->goal) based on its angular proximity to the goal pose's orientation. +This will either score a weighted-angular distance or reject traversals that are outside of a set threshold to force the route to go down a particular direction (i.e. direction robot wants to be facing). + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| orientation_weight | Relative scoring weight (1.0) | +| use_orientation_threshold | Whether to use orient. threshold or weighted-angular distance scoring (false) | +| orientation_tolerance | The angular threshold to reject edges' angles if greater than this w.r.t. goal pose, when `use_orientation_threshold: true` (PI/2) | + +#### `AdjustSpeedLimit` + +This route operation will check the graph at each state change (e.g. node passed) if the new edge entered contains speed limit restrictions. If so, it will publish those to the speed limit topic to be received by the controller server. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| speed_limit_topic | Topic to publish new speed limits to (speed_limit) | +| speed_tag | Graph metadata key to look for % speed limits (speed_limit) | + +#### `CollisionMonitor` + +This route operation will evaluate a future-looking portion of the route for validity w.r.t. collision in the costmap. If it is blocked, it sets the edge blocked as blocked for rerouting around the blocked edge. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| costmap_topic | Costmap topic (global_costmap/costmap_raw) | +| rate | Throttled rate to evaluate collision at, rather than `tracker_update_rate` which might be excessively expensive for forward looking non-control collision checking (1.0 hz) | +| max_cost | Max cost to be considered invalid (253.0) | +| max_collision_dist | How far ahead to evaluate the route's validity (5.0 m) | +| check_resolution | Resolution to check costs at (1 = costmap resolution, 2 = 2x costmap resolution, etc) | +| reroute_on_collision | Whether to reroute on collision or exit tracking as failed when a future collision is detected (default: true) | + +#### `ReroutingService` + +This route operation will receive service requests from a 3rd party application to cause a rerouting request. + +#### `TimeMarker` + +This route operation will track times taken to traverse particular edges to write times to for later improved navigation time estimation. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| time_tag | Graph edge metadata key to write to (abs_time_taken) | + +#### `TriggerEvent` + +This route operation will trigger an external service when a graph node or edge contains a route operation of this name. It uses a `std_srvs/Trigger` interface and is a demonstration of the `RouteOperationClient` base class which can be used to trigger other events of other types of other names as desired (opening doors, calling elevators, etc). + +## ROS Interfaces + + +| Topic | Type | +|-----------------|-------------------------------------| +| plan | nav_msgs/msg/Path | +| speed_limit | nav2_msgs/msg/SpeedLimit | +| route_graph | visualization_msgs/msg/MarkerArray | + +| Service | Description | +|--------------------------------------------------|----------------------------------------------------------------| +| `route_server/set_route_graph` | Sets new route graph to use | +| `route_server//adjust_edges` | Sets adjust edge values and closure from 3rd party application | +| `route_server//reroute` | Trigger a reroute from a 3rd party | + +| Action | Description | +|----------------------------------|-----------------------------------------------------------------------| +| `compute_route` | Computes and returns a sparse graph route and dense interpolated path | +| `compute_and_track_route` | Computes and tracks a route for route operations and progress, returns route and path via process feedback also containing current state and operations triggered | + +## File Formats + +The graphs may be stored in one of the formats the parser plugins can understand or implement your own parser for a particular format of your interest! +A parser is provided for GeoJSON formats. +The only three required features of the navigation graph is (1) for the nodes and edges to have identifiers from each other to be unique for referencing and (2) for edges to have the IDs of the nodes belonging to the start and end of the edge and (3) nodes contain coordinates. +This is strictly required for the Route Server to operate properly in all of its features. + +Besides this, we establish some conventions for route graph files to standardize this information to offer consistent and documented behavior with our provided plugins. +The unique identifier for each node and edge should be given as `id`. The edge's nodes are `startid` and `endid`. The coordinates are given in an array called `coordinates`. +While technically optional, it is highly recommended to also provide: +- The node's frame of reference (`frame`), if not the global frame and you want it transformed +- The Operation's `trigger` (e.g. enter, exit edge, node achieved) and `type` (e.g. action to perform), as relevant + +While optional, it is somewhat recommended to provide, if relevant to your needs: +- The edge's `cost`, if it is fixed and known at graph generation time or edge scoring plugins are not used +- Whether the edge's cost is `overridable` with edge scoring plugins, if those plugins are provided. + +Otherwise, the Node, Edge, and Operations may contain other arbitrary application-specific fields with key-value pairs under the `metadata` key-name. +These can be primitive types (float, int, string, etc), vector types (e.g. a polygon or other vector of information), or even contain information nested under namespaces - whereas a metadata object may exist as a key's value within `metadata`. + +While GeoJSON is not YAML-based, the following YAML file is provided as a more human-readable example for illustration of the conventions above. +Usable real graph file demos can be found in the `graphs/` directory. However, the +`sample_graph.geojson` in the `graph/` directory exists to show the different API for options +for operations, metadata, recursive metadata and vectors. + +``` +example_graph.yaml + +Node1: // <-- If provided by format, stored as name in metadata + id: 1 // <-- Required + coordinates: [0.32, 4.0] // <-- Required + frame: "map" // <-- Highly recommended + metadata: + class: "living_room" // <-- Metadata for node (arbitrary) + operation: + pause: // <-- If provided by format, stored as name in metadata + type: "stop" // <-- Required + trigger: ON_ENTER // <-- Required + metadata: + wait_for: 5.0 // <-- Metadata for operation (arbitrary) + +Edge1: // <-- If provided by format, stored as name in metadata + id: 2 // <-- Required + startid: 1 // <-- Required + endid: 3 // <-- Required + overridable: False // <-- Recommended + cost: 6.0 // <-- Recommended, if relevant + metadata: + speed_limit: 85 // <-- Metadata for edge (arbitrary). Use abs_speed_limit if not a percentage + operations: + open_door: // <-- If provided by format, stored as name in metadata + type: "open_door" // <-- Required + trigger: ON_EXIT // <-- Required + metadata: + door_id: 54 // <-- metadata for operation (arbitrary) + service_name "open-door" // <-- metadata for operation (Recommended) +``` + +### Metadata Conventions for Convenience + +While other metadata fields are not required nor necessarily needed, there are some useful standards which may make your life easier within in the Route Server framework. +These are default fields for the provided plugins, but you're free to embed this information anyway you like (but may come at the cost of needing to re-implement provided capabilities). +A set of conventions are shown in the table below. The details regarding the convention and where they are used within the codebase follows in more detail. + +| Graph File Key | Information Stored | +| --------------- | ------------- | +| `speed_limit` | Speed limit, represented in percentage 0.0-100.0. | +| `abs_speed_limit` | Speed limit, represented in `m/s`. | +| `penalty` | A cost penalty to apply to a node or edge. | +| `class` | The semanic class this node belongs to. | +| `service_name` | Which service to call, if node or edge should trigger an event. | +| `abs_time_taken` | Time taken to traverse an edge in `s`. | + +
+ Click me to see expansive detail about these conventions and uses + + The `DistanceScorer` edge scoring plugin will score the L2 norm between the poses of the nodes constituting the edge as its unweighted score. + It contains a parameter `speed_tag` (Default: `speed_limit`) which will check the edge's metadata if it contains an entry for speed limit information, represented as a percentage of the maximum speed. + If so, it will adjust the score to instead be proportional to the time rather than distance. + Further, the `AdjustSpeedLimit` Route Operation plugin will utilize the same parameter and default `speed_limit` value to check each edge entered for a set speed limit percentage. + If present, it will request an adjusted speed limit by the controller server on each edge entered. + Thus, by convention, we say that `speed_limit` attribute of an edge should contain this information. + + The `TimeScorer` plugin operates with an exact analog to the `DistanceScorer`, except rather than using speed limits based on percentages of maximum, it uses actual speed limits in `m/s`. As such the `speed_tag` parameter (Default: `abs_speed_limit`) is used to represent absolute speed limits in the metadata files. By convention, we say that the `abs_speed_limit` attribute of an edge should contain this information. + + Similarly, the `penalty_tag` parameter (Default: `penalty`) in the `PenaltyScorer` can be used to represent a static unweighted cost to add to the sum total of the edge scoring plugins. + This is useful to penalize certain routes over others knowing some application-specific information about that route segment. Technically this may be negative to incentivize rather than penalize. + Thus, by convention, we say the `penalty` attribute of an edge should contain this information. + + By convention, to use the `SemanticScorer` edge scoring plugin, we expect the semantic class of a node or edge to be stored in the `class` metadata key (though is reconfigurable to look at any using the `semantic_key` parameter). This can be used to store arbitrary semantic metadata about a node or edge such as `living_room`, `bathroom`, `work cell 2`, `aisle45`, etc which is then correlated to a set of additional class costs in the plugin's configuration. However, if the `semantic_key` parameter is set to empty string, then instead it checks **all keys** in the metadata of a node or edge if they match any names of the semantic classes. This way, semantic information may be embedded as keys with other values (for another application) or as values themselves to the `class` key if only needing to specify its membership. + +The Route Operation `TriggerEvent` and more broadly any operation plugins derived from `RouteOperationClient` (a service-typed template route operation base class to simplify adding in custom plugins based on service calls) relies on the parameter and matching metadata key `service_name` to indicate the service name to call with the corresponding route operation. When set in the parameter file, this will be used for all instances when called in the navigation route graph. When `service_name` is set in the operation metadata in the route graph, it can be used to specify a particular service name of that service type to use at that particular node/edge, created on the fly (when a conflict exists, uses the navigation graph as the more specific entry). + That way both design patterns work for a Route Operation `OpenDoor` of service type `nav2_msgs/srv/OpenDoor`, for example: + - A `open_door/door1` (and `door2` and so on) service specific to each node containing a door to open may be called contextually and correctly at each individual door in the graph file metadata. This way you can have individual services (if desired) without having to have individual repetitive operation plugin definitions. + - A `open_doors` general service specified in the parameter file to call to open a door specified in the service's `request` field, so that one service is called for all instances of doors in the graph file without repetition in the graph file and storing client resources (just adding info about which one from the node metadata). + + Thus, we say that `service_name` is a key to correspond to a string of the service's name to call in an operation to use `TriggerEvent` and `RouteOperationClient` plugins and base classes. + + By convention, we reserve the edge metadata field `abs_time_taken` for representing actual navigation times along the edge. This is populated by the `TimeMarker` route operation plugin and utilized by the `TimeScorer` to more accurately represent the times to navigate along an edge based on the real execution time. While this is used as an internal mechanism to share data between live operations and route planning, it is also possible to set `last_time_taken` in your navigation graph file based on many execution runs to use in the `TimeScorer` to optimize system performance for a fleet or over long durations (and remove `TimeMarker` operation for live updates). + + +
+ +## Etc. Notes + +### Metadata Communication + +The metadata contained in the graph's nodes and edges can serve a secondary purpose to communicating arbitrary information from the graph file for use in routing behavior or operations. It may also be used to communicate or store information about a node or edge during run-time to query from a plugin in a future iteration, from another plugin in the system, or from another plugin type entirely. + +For example: +- If the collision monitor Route Operation identifies an edge as being blocked on a regular basis, a counter can be used to track the number of times this edge is blocked and if exceeding a threshold, it adds additional costs to that edge to incentivize taking another direction in an Edge Scorer. +- If we want to minimize the time to traverse the space, rather than estimating the times to traverse an edge, we can store actual times to navigate into the metadata of the edges. This can be stored as part of a route operation after completing an edge and retrieved at planning time by an edge scorer. + +All of this is made possible by the centralized graph representation and pointers back to its memory locations at each stage of the system. + +### Node Achievement + +The Route Tracker will track the progress of a robot following a defined route over time. When we achieve a node, that is to say, we pass it, that triggers events based on reaching a node (Also: exiting an old edge, entering a new edge). Thus, the specification of node achievement is worth some discussion for users so they can best use this powerful feature. + +The node achievement logic will first check if the robot is within a configurable radius of a node. This radius should be **generous** and not equatable to the goal tolerance. This should be sufficiently large that by the mechanics of your control, you can achieve a node when you pass it considering realistic deviations of path tracking by your trajectory planner. This may not need to be very large if using an exact path follower controller but may need to be relatively large for a dynamic obstacle avoidance planner. If in doubt, make it **larger** since this is merely the first stage for checking node achievement, not the metric itself. + +Once we're within the range of a node that we need to consider whether or not we've achieved the node, we evaluate a mathematical operation to see if the robot has moved from the regime of the previous edge into the next edge spatially. We do this by finding the bisecting vector of the two edge vectors of interest (e.g. last edge and next edge) and comparing that with the distance vector from the node. When the dot product is 0, that means the we're at the orthogonal vector to the bisector moving from one regime to the next. Thus, we can check the sign of this dot product to indicate when we transition from one edge to the next while allowing for significant deviation from the node itself due to path tracking error or dynamic behavior. + +For the edge boundary cases where there is no last edge (e.g. starting) or next edge (e.g. finishing), we use the boundary radius only (separate param from otherwise used radius). Recall that this only applies the routing part of the navigation request, the controller will still continue tracking the path until your goal achievement defined in your local trajectory planner's configurations. However, any Route Operations to be performed at the start or end nodes will be slightly preempted when compared to the others, by the choice of this radius. + +A special case exists for rerouting, where as if we reroute along the same edge as we were previously routing through, the tracker state will be reloaded. While the current edge is not reported in the route message (because the last node is passed), that information is inserted back in the tracker to perform edge exit route operations -- and most importantly, the refined bisector-based node achievement criteria (which is why Steve went through the sheer pain to make it possible). + +### Gap and Trivial Routing + +If a routing or rerouting request is made using poses which are not known to be nodes in the graph, there may be a gap between the robot's pose and the start of the route (as well as the end of the route and the goal pose) -- or a "last-mile" problem. This gap may be trivial to overcome with denser graphs and small distances between nodes that a local trajectory planner can overcome. However, with sparser graphs, some kind of augmentation would likely be required. For example: +- (A) Using a behavior tree which uses free-space planning to connect the robot and goal poses with the start / end of the route +- (B) Using the waypoint follower (or navigate through poses or otherwise) that takes the nodes and uses them as waypoints terminating with the goal pose for freespace navigation using the route as a general prior +- (C) Using the Route as a goal route to the goal, global planner to make a feasible plan for the next 100 meters (or so), and use the controller to track the global feasible path rather than the general route +- (D) some other solution which will get the robot from its pose, through the route, to the goal +- (E) The trajectory planner can handle these gaps itself, so Route dense path to the controller directly + +A special condition of this when a routing or rerouting request is made up of only 1-2 edges, whereas both the start and the goal are not 'on or near' the terminal nodes (e.g. they're already along an edge meaningfully, so going to a goal node and then the final pose would involve back tracking), the edges may be pruned away leaving only a single node as the route. This is since we have already passed the starting node of the initial edge and/or the goal is along the edge before the ending node of the final edge. This is not different than the other conditions, but is a useful edge case to consider while developing your application solution. + +This is an application problem which can be addressed above but may have other creative solutions for your application. It is on you as an application developer to determine if this is a problem for you and what the most appropriate solution is, since different applications will have different levels of flexibility of deviating from the route or extremely strict route interpretations. + +Note that there are parameters like `prune_route`, `min_prune_distance_from_start` and `min_prune_distance_from_goal` which impact the pruning behavior while tracking a route using poses. There is also the option to request routes using NodeIDs instead of poses, which obviously would never have this issue since they are definitionally on the route graph at all times. That bypasses the entire issue if the goal(s) are always known to be on the graph. diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson new file mode 100644 index 00000000000..2e6a43af148 --- /dev/null +++ b/nav2_route/graphs/aws_graph.geojson @@ -0,0 +1,132 @@ +{ +"type": "FeatureCollection", +"name": "graph", +"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, +"date_generated": "Wed Feb 22 05:41:45 PM EST 2025", +"features": [ +{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, +{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, +{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, +{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, +{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, +{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, +{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, +{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, +{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, +{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, +{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, +{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, +{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } +] +} diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson new file mode 100644 index 00000000000..162a1a88884 --- /dev/null +++ b/nav2_route/graphs/sample_graph.geojson @@ -0,0 +1,121 @@ +{ + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Tue Feb 28 07:48:03 PM EST 2025", + "features": [ + { + "type": "Feature", + "properties": + { + "id": 0, + "frame": "map", + "metadata": + { + "region": + { + "x_values": [1.0, -1.0, -1.0, 1.0], + "y_values": [1.0, 1.0, -1.0, -1.0], + "properties": { + "class_type": "living_room", + "number_of_lights": 10 + } + } + }, + "operations": + { + "stop": + { + "type": "stop", + "trigger": "NODE", + "metadata": + { + "wait_for": 5.0 + } + } + } + }, + "geometry": + { + "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, + { + "type": "Feature", + "properties": + { + "id": 9, + "startid": 0, + "endid": 1, + "overridable": false, + "cost": 10.0, + "metadata": + { + "speed_limit": 0.85, + "aisle_number": 14 + }, + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_ENTER", + "metadata": + { + "door_id": 54, + "service_name": "open_door" + } + }, + "take_picture": + { + "type": "take_picture", + "trigger": "ON_EXIT", + "metadata": + { + "type": "jpg", + "resolution": [1080, 720] + } + } + } + }, + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + }, + { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } + + ] +} diff --git a/nav2_route/graphs/scripts/README.md b/nav2_route/graphs/scripts/README.md new file mode 100644 index 00000000000..d1bfc83d3a4 --- /dev/null +++ b/nav2_route/graphs/scripts/README.md @@ -0,0 +1 @@ +These scripts are used to help automate parts of route graph generation process. diff --git a/nav2_route/graphs/scripts/export_shapefiles.py b/nav2_route/graphs/scripts/export_shapefiles.py new file mode 100644 index 00000000000..e6fe5515f94 --- /dev/null +++ b/nav2_route/graphs/scripts/export_shapefiles.py @@ -0,0 +1,60 @@ +#! /usr/bin/env python3 +# Copyright 2021 Josh Wallace +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from datetime import datetime +import sys + +import geopandas as gpd +import pandas as pd + +try: + file_prefix = sys.argv[1] + edges = gpd.read_file(sys.argv[2]) + nodes = gpd.read_file(sys.argv[3]) +except Exception: + raise Exception('Incorrect arguments provide') + +# Rename columns to match the expected output +nodes = nodes.rename(columns={'start_node': 'startid'}) +edges = edges.rename(columns={'start_node': 'startid'}) +nodes = nodes.rename(columns={'end_node': 'endid'}) +edges = edges.rename(columns={'end_node': 'endid'}) + +# Remove edgeid and use standard id +edges = edges.rename(columns={'edge_id': 'id'}) + +# Make sure IDs are integers +if 'id' in nodes.columns: + nodes['id'] = nodes['id'].astype(int) +if 'id' in edges.columns: + edges['id'] = edges['id'].astype(int) + +now = datetime.now() + +graph = pd.concat([nodes, edges]) + +# Set start/endids to integers, for nodes, set to -1 (not used) +if 'startid' in graph.columns: + graph['startid'] = graph['startid'].fillna(-1).astype(int) +if 'endid' in graph.columns: + graph['endid'] = graph['endid'].fillna(-1).astype(int) +if 'startid' in graph.columns: + graph['startid'] = graph['startid'].astype(int) +if 'endid' in graph.columns: + graph['endid'] = graph['endid'].astype(int) + +file_name = file_prefix + '_' + now.strftime('%m_%d_%Y_%H_%M_%S') + '.geojson' + +graph.to_file(file_name, driver='GeoJSON') diff --git a/nav2_route/graphs/scripts/generate_start_and_end_id.sql b/nav2_route/graphs/scripts/generate_start_and_end_id.sql new file mode 100644 index 00000000000..e0e34cd5d88 --- /dev/null +++ b/nav2_route/graphs/scripts/generate_start_and_end_id.sql @@ -0,0 +1,22 @@ +WITH start_points AS +( + SELECT n.id AS node_id + ,e.id AS edge_id + ,e.geometry AS edge_geometry + FROM edges AS e, nodes AS n + WHERE ST_INTERSECTS(n.geometry, ST_StartPoint(e.geometry)) +), end_points AS +( + SELECT n.id AS node_id + ,e.id AS edge_id + ,e.geometry AS edge_geometry + FROM edges AS e, nodes AS n + WHERE ST_INTERSECTS(n.geometry, ST_EndPoint(e.geometry)) +) +SELECT sp.edge_id AS edge_id + ,sp.node_id AS start_node + ,ep.node_id AS end_node + ,sp.edge_geometry AS geometry +FROM start_points AS sp +JOIN end_points AS ep +ON sp.edge_id = ep.edge_id diff --git a/nav2_route/graphs/scripts/increment_edge_id.json b/nav2_route/graphs/scripts/increment_edge_id.json new file mode 100644 index 00000000000..0328f9cd081 --- /dev/null +++ b/nav2_route/graphs/scripts/increment_edge_id.json @@ -0,0 +1,21 @@ +{ + "author": "josh", + "exported_at": "2025-03-15T19:27:03", + "expressions": [ + { + "description": "\n\n


", + "expression": "if(maximum(\"id\") is null, 10000, maximum(\"id\")+1)", + "group": "user", + "name": "increment_edge_id", + "type": "expression" + }, + { + "description": "\n\n


", + "expression": "if(maximum(\"id\") is null, 0, maximum(\"id\")+1)", + "group": "user", + "name": "increment_node_id", + "type": "expression" + } + ], + "qgis_version": "3.22.4-Białowieża" +} diff --git a/nav2_route/graphs/scripts/increment_node_id.json b/nav2_route/graphs/scripts/increment_node_id.json new file mode 100644 index 00000000000..4f399c62951 --- /dev/null +++ b/nav2_route/graphs/scripts/increment_node_id.json @@ -0,0 +1,14 @@ +{ + "author": "josh", + "exported_at": "2025-03-15T19:22:14", + "expressions": [ + { + "description": "\n\n


", + "expression": "if(maximum(\"id\") is null, 0, maximum(\"id\")+1)", + "group": "user", + "name": "increment_node_id", + "type": "expression" + } + ], + "qgis_version": "3.22.4-Białowieża" +} diff --git a/nav2_route/graphs/turtlebot3_graph.geojson b/nav2_route/graphs/turtlebot3_graph.geojson new file mode 100644 index 00000000000..ba7aff0c6d2 --- /dev/null +++ b/nav2_route/graphs/turtlebot3_graph.geojson @@ -0,0 +1,92 @@ +{ + "type": "FeatureCollection", + "name": "turtlebot3_graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ -0.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 2 }, "geometry": { "type": "Point", "coordinates": [ 0.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 3 }, "geometry": { "type": "Point", "coordinates": [ 0.0, -1.7446 ] } }, + { "type": "Feature", "properties": { "id": 4 }, "geometry": { "type": "Point", "coordinates": [ -1.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 5 }, "geometry": { "type": "Point", "coordinates": [ -0.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 6 }, "geometry": { "type": "Point", "coordinates": [ 0.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 7 }, "geometry": { "type": "Point", "coordinates": [ -1.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 8 }, "geometry": { "type": "Point", "coordinates": [ -1.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 9 }, "geometry": { "type": "Point", "coordinates": [ -0.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 10 }, "geometry": { "type": "Point", "coordinates": [ 0.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 11 }, "geometry": { "type": "Point", "coordinates": [ 1.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 12 }, "geometry": { "type": "Point", "coordinates": [ 1.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 13 }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.2554 ] } }, + { "type": "Feature", "properties": { "id": 14 }, "geometry": { "type": "Point", "coordinates": [ 1.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 15 }, "geometry": { "type": "Point", "coordinates": [ -1.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 16 }, "geometry": { "type": "Point", "coordinates": [ -0.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 17 }, "geometry": { "type": "Point", "coordinates": [ 0.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 18 }, "geometry": { "type": "Point", "coordinates": [ 1.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 19 }, "geometry": { "type": "Point", "coordinates": [ -2.0, 0.2554 ] } }, + { "type": "Feature", "properties": { "id": 20 }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.2554 ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 8, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 16, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 9, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 5, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 2, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 6, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 10, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 17, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 18, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 1.8554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 33, "startid": 12, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 34, "startid": 14, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -1.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 35, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ -1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 36, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 37, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ -1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 38, "startid": 15, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 1.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 39, "startid": 17, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 40, "startid": 10, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 41, "startid": 6, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 42, "startid": 2, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 43, "startid": 14, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -1.3446 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 44, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 46, "startid": 18, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 1.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 1.8554 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 48, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 49, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 50, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 51, "startid": 1, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 52, "startid": 5, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 53, "startid": 9, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 54, "startid": 16, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 55, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 56, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 57, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 58, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 59, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 60, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 61, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 62, "startid": 5, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 63, "startid": 7, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 64, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 65, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 66, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 67, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 68, "startid": 12, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 69, "startid": 6, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 70, "startid": 3, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -1.7446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 71, "startid": 3, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -1.7446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 72, "startid": 1, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ 0.0, -1.7446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 73, "startid": 2, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 0.0, -1.7446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 74, "startid": 11, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 75, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.2554 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 76, "startid": 12, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 77, "startid": 13, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.2554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 78, "startid": 16, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ 0.0, 2.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 79, "startid": 20, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.2554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 80, "startid": 17, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 0.0, 2.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 81, "startid": 20, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.2554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 82, "startid": 8, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 83, "startid": 19, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, 0.2554 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 84, "startid": 7, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 85, "startid": 19, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, 0.2554 ], [ -1.6, 0.8554 ] ] ] } } + ] +} diff --git a/nav2_route/graphs/turtlebot4_graph.geojson b/nav2_route/graphs/turtlebot4_graph.geojson new file mode 100644 index 00000000000..f7b922e9e5a --- /dev/null +++ b/nav2_route/graphs/turtlebot4_graph.geojson @@ -0,0 +1,119 @@ +{ + "type": "FeatureCollection", + "name": "turtlebot4_graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 0 }, "geometry": { "type": "Point", "coordinates": [ 0.624282608695647, 12.880652173913044 ] } }, + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ 0.584239130434777, 7.835173913043479 ] } }, + { "type": "Feature", "properties": { "id": 2 }, "geometry": { "type": "Point", "coordinates": [ 0.544195652173907, 2.569456521739133 ] } }, + { "type": "Feature", "properties": { "id": 3 }, "geometry": { "type": "Point", "coordinates": [ 7.651913043478253, 7.915260869565218 ] } }, + { "type": "Feature", "properties": { "id": 4 }, "geometry": { "type": "Point", "coordinates": [ 9.453869565217383, 0.727456521739134 ] } }, + { "type": "Feature", "properties": { "id": 5 }, "geometry": { "type": "Point", "coordinates": [ 13.79858695652173, 6.914173913043479 ] } }, + { "type": "Feature", "properties": { "id": 6 }, "geometry": { "type": "Point", "coordinates": [ 13.698478260869557, 1.328108695652176 ] } }, + { "type": "Feature", "properties": { "id": 7 }, "geometry": { "type": "Point", "coordinates": [ 15.820782608695643, 6.974239130434784 ] } }, + { "type": "Feature", "properties": { "id": 8 }, "geometry": { "type": "Point", "coordinates": [ 15.900869565217382, 9.036478260869565 ] } }, + { "type": "Feature", "properties": { "id": 9 }, "geometry": { "type": "Point", "coordinates": [ 15.940913043478252, 11.719391304347827 ] } }, + { "type": "Feature", "properties": { "id": 10 }, "geometry": { "type": "Point", "coordinates": [ 18.383565217391293, 6.934195652173914 ] } }, + { "type": "Feature", "properties": { "id": 11 }, "geometry": { "type": "Point", "coordinates": [ 18.3235, 9.036478260869565 ] } }, + { "type": "Feature", "properties": { "id": 12 }, "geometry": { "type": "Point", "coordinates": [ 18.283456521739122, 11.8195 ] } }, + { "type": "Feature", "properties": { "id": 13 }, "geometry": { "type": "Point", "coordinates": [ 20.125456521739121, 11.839521739130435 ] } }, + { "type": "Feature", "properties": { "id": 14 }, "geometry": { "type": "Point", "coordinates": [ 20.065391304347816, 9.116565217391305 ] } }, + { "type": "Feature", "properties": { "id": 15 }, "geometry": { "type": "Point", "coordinates": [ 19.985304347826077, 6.974239130434784 ] } }, + { "type": "Feature", "properties": { "id": 16 }, "geometry": { "type": "Point", "coordinates": [ 22.167673913043469, 6.914173913043479 ] } }, + { "type": "Feature", "properties": { "id": 17 }, "geometry": { "type": "Point", "coordinates": [ 22.047543478260859, 9.116565217391305 ] } }, + { "type": "Feature", "properties": { "id": 18 }, "geometry": { "type": "Point", "coordinates": [ 23.869521739130423, 11.77945652173913 ] } }, + { "type": "Feature", "properties": { "id": 19 }, "geometry": { "type": "Point", "coordinates": [ 23.8495, 9.216673913043479 ] } }, + { "type": "Feature", "properties": { "id": 20 }, "geometry": { "type": "Point", "coordinates": [ 23.789434782608684, 6.934195652173914 ] } }, + { "type": "Feature", "properties": { "id": 21 }, "geometry": { "type": "Point", "coordinates": [ 26.071913043478251, 6.954217391304349 ] } }, + { "type": "Feature", "properties": { "id": 22 }, "geometry": { "type": "Point", "coordinates": [ 25.951782608695641, 9.196652173913044 ] } }, + { "type": "Feature", "properties": { "id": 23 }, "geometry": { "type": "Point", "coordinates": [ 25.971804347826076, 11.8195 ] } }, + { "type": "Feature", "properties": { "id": 24 }, "geometry": { "type": "Point", "coordinates": [ 28.234260869565205, 11.85954347826087 ] } }, + { "type": "Feature", "properties": { "id": 25 }, "geometry": { "type": "Point", "coordinates": [ 28.55460869565216, 6.954217391304349 ] } }, + { "type": "Feature", "properties": { "id": 26 }, "geometry": { "type": "Point", "coordinates": [ 28.59465217391303, 5.552695652173915 ] } }, + { "type": "Feature", "properties": { "id": 27 }, "geometry": { "type": "Point", "coordinates": [ 28.59465217391303, 3.130065217391307 ] } }, + { "type": "Feature", "properties": { "id": 28 }, "geometry": { "type": "Point", "coordinates": [ 28.614673913043465, 1.448239130434786 ] } }, + { "type": "Feature", "properties": { "id": 29 }, "geometry": { "type": "Point", "coordinates": [ 25.150913043478248, 1.348130434782611 ] } }, + { "type": "Feature", "properties": { "id": 30 }, "geometry": { "type": "Point", "coordinates": [ 22.508043478260859, 1.388173913043481 ] } }, + { "type": "Feature", "properties": { "id": 31 }, "geometry": { "type": "Point", "coordinates": [ 19.825130434782597, 1.348130434782611 ] } }, + { "type": "Feature", "properties": { "id": 32 }, "geometry": { "type": "Point", "coordinates": [ 16.86191304347825, 1.368152173913046 ] } }, + { "type": "Feature", "properties": { "id": 33 }, "geometry": { "type": "Point", "coordinates": [ 13.698478260869557, 1.328108695652176 ] } }, + { "type": "Feature", "properties": { "id": 10000, "startid": 1, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.584239130434777, 7.835173913043479 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10001, "startid": 3, "endid": 1 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.584239130434777, 7.835173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10002, "startid": 0, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.624282608695647, 12.880652173913044 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10003, "startid": 3, "endid": 0 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.624282608695647, 12.880652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10004, "startid": 2, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.544195652173907, 2.569456521739133 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10005, "startid": 3, "endid": 2 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.544195652173907, 2.569456521739133 ] ] } }, + { "type": "Feature", "properties": { "id": 10006, "startid": 6, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10006, "startid": 33, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10007, "startid": 4, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10007, "startid": 4, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10008, "startid": 4, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10009, "startid": 3, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10010, "startid": 3, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10011, "startid": 5, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10012, "startid": 5, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10012, "startid": 5, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10013, "startid": 6, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10013, "startid": 33, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10014, "startid": 5, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10015, "startid": 7, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10016, "startid": 7, "endid": 8 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 15.900869565217382, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10017, "startid": 8, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.900869565217382, 9.036478260869565 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10018, "startid": 8, "endid": 9 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.900869565217382, 9.036478260869565 ], [ 15.940913043478252, 11.719391304347827 ] ] } }, + { "type": "Feature", "properties": { "id": 10019, "startid": 9, "endid": 8 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.940913043478252, 11.719391304347827 ], [ 15.900869565217382, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10020, "startid": 6, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10020, "startid": 33, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10021, "startid": 32, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10021, "startid": 32, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10022, "startid": 7, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10023, "startid": 10, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10024, "startid": 10, "endid": 11 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 18.3235, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10025, "startid": 11, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.3235, 9.036478260869565 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10026, "startid": 11, "endid": 12 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.3235, 9.036478260869565 ], [ 18.283456521739122, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10027, "startid": 12, "endid": 11 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.283456521739122, 11.8195 ], [ 18.3235, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10028, "startid": 12, "endid": 13 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.283456521739122, 11.8195 ], [ 20.125456521739121, 11.839521739130435 ] ] } }, + { "type": "Feature", "properties": { "id": 10029, "startid": 13, "endid": 12 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.125456521739121, 11.839521739130435 ], [ 18.283456521739122, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10030, "startid": 13, "endid": 14 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.125456521739121, 11.839521739130435 ], [ 20.065391304347816, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10031, "startid": 14, "endid": 13 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.065391304347816, 9.116565217391305 ], [ 20.125456521739121, 11.839521739130435 ] ] } }, + { "type": "Feature", "properties": { "id": 10032, "startid": 14, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.065391304347816, 9.116565217391305 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10033, "startid": 15, "endid": 14 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 20.065391304347816, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10034, "startid": 15, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10035, "startid": 10, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10036, "startid": 32, "endid": 31 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 19.825130434782597, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10037, "startid": 31, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.825130434782597, 1.348130434782611 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10038, "startid": 31, "endid": 30 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.825130434782597, 1.348130434782611 ], [ 22.508043478260859, 1.388173913043481 ] ] } }, + { "type": "Feature", "properties": { "id": 10039, "startid": 30, "endid": 31 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.508043478260859, 1.388173913043481 ], [ 19.825130434782597, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10040, "startid": 30, "endid": 29 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.508043478260859, 1.388173913043481 ], [ 25.150913043478248, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10041, "startid": 29, "endid": 30 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.150913043478248, 1.348130434782611 ], [ 22.508043478260859, 1.388173913043481 ] ] } }, + { "type": "Feature", "properties": { "id": 10042, "startid": 29, "endid": 28 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.150913043478248, 1.348130434782611 ], [ 28.614673913043465, 1.448239130434786 ] ] } }, + { "type": "Feature", "properties": { "id": 10043, "startid": 28, "endid": 29 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.614673913043465, 1.448239130434786 ], [ 25.150913043478248, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10044, "startid": 28, "endid": 27 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.614673913043465, 1.448239130434786 ], [ 28.59465217391303, 3.130065217391307 ] ] } }, + { "type": "Feature", "properties": { "id": 10045, "startid": 27, "endid": 28 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 3.130065217391307 ], [ 28.614673913043465, 1.448239130434786 ] ] } }, + { "type": "Feature", "properties": { "id": 10046, "startid": 27, "endid": 26 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 3.130065217391307 ], [ 28.59465217391303, 5.552695652173915 ] ] } }, + { "type": "Feature", "properties": { "id": 10047, "startid": 26, "endid": 27 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 5.552695652173915 ], [ 28.59465217391303, 3.130065217391307 ] ] } }, + { "type": "Feature", "properties": { "id": 10048, "startid": 26, "endid": 25 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 5.552695652173915 ], [ 28.55460869565216, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10049, "startid": 25, "endid": 26 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.55460869565216, 6.954217391304349 ], [ 28.59465217391303, 5.552695652173915 ] ] } }, + { "type": "Feature", "properties": { "id": 10050, "startid": 15, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10051, "startid": 16, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10052, "startid": 16, "endid": 17 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 22.047543478260859, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10053, "startid": 17, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.047543478260859, 9.116565217391305 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10054, "startid": 17, "endid": 18 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.047543478260859, 9.116565217391305 ], [ 23.869521739130423, 11.77945652173913 ] ] } }, + { "type": "Feature", "properties": { "id": 10055, "startid": 18, "endid": 17 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.869521739130423, 11.77945652173913 ], [ 22.047543478260859, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10056, "startid": 18, "endid": 19 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.869521739130423, 11.77945652173913 ], [ 23.8495, 9.216673913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10057, "startid": 19, "endid": 18 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.8495, 9.216673913043479 ], [ 23.869521739130423, 11.77945652173913 ] ] } }, + { "type": "Feature", "properties": { "id": 10058, "startid": 19, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.8495, 9.216673913043479 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10059, "startid": 20, "endid": 19 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 23.8495, 9.216673913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10060, "startid": 20, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10061, "startid": 16, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10062, "startid": 20, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 26.071913043478251, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10063, "startid": 21, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10064, "startid": 21, "endid": 22 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 25.951782608695641, 9.196652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10065, "startid": 22, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.951782608695641, 9.196652173913044 ], [ 26.071913043478251, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10066, "startid": 22, "endid": 23 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.951782608695641, 9.196652173913044 ], [ 25.971804347826076, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10067, "startid": 23, "endid": 22 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.971804347826076, 11.8195 ], [ 25.951782608695641, 9.196652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10068, "startid": 23, "endid": 24 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.971804347826076, 11.8195 ], [ 28.234260869565205, 11.85954347826087 ] ] } }, + { "type": "Feature", "properties": { "id": 10069, "startid": 24, "endid": 23 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.234260869565205, 11.85954347826087 ], [ 25.971804347826076, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10070, "startid": 21, "endid": 25 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 28.55460869565216, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10071, "startid": 25, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.55460869565216, 6.954217391304349 ], [ 26.071913043478251, 6.954217391304349 ] ] } } + ] +} diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp new file mode 100644 index 00000000000..79653f92e3b --- /dev/null +++ b/nav2_route/include/nav2_route/edge_scorer.hpp @@ -0,0 +1,87 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__EDGE_SCORER_HPP_ +#define NAV2_ROUTE__EDGE_SCORER_HPP_ + +#include +#include +#include + +#include "tf2_ros/buffer.h" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +namespace nav2_route +{ + +/** + * @class nav2_route::EdgeScorer + * @brief An class to encapsulate edge scoring logic for plugins and different user + * specified algorithms to influence graph search. It has access to the edge, which + * in turn has access to the parent and child node of the connection. It also contains + * action and arbitrary user-defined metadata to enable edge scoring logic based on + * arbitrary properties of the graph you select (e.g. some regions have a multiplier, + * some actions are discouraged with higher costs like having to go through a door, + * edges with reduced speed limits are proportionally less preferred for optimality + * relative to the distance the edge represents to optimize time to goal) + */ +class EdgeScorer +{ +public: + /** + * @brief Constructor + */ + explicit EdgeScorer( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::shared_ptr costmap_subscriber); + + /** + * @brief Destructor + */ + ~EdgeScorer() = default; + + /** + * @brief Score the edge with the set of plugins + * @param edge Ptr to edge for scoring + * @param goal_pose Pose Stamped of desired goal + * @param score of edge + * @param final_edge whether this edge brings us to the goal or not + * @return If edge is valid + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, + float & score); + + /** + * @brief Provide the number of plugisn in the scorer loaded + * @return Number of scoring plugins + */ + int numPlugins() const; + +protected: + pluginlib::ClassLoader plugin_loader_; + std::vector plugins_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__EDGE_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp new file mode 100644 index 00000000000..3072f2626c9 --- /dev/null +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -0,0 +1,144 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ +#define NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ + +#include +#include +#include + +#include "tf2_ros/transform_listener.h" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_msgs/action/compute_route.hpp" +#include "nav2_msgs/action/compute_and_track_route.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/node_spatial_tree.hpp" +#include "nav2_route/goal_intent_search.hpp" + +namespace nav2_route +{ + +/** + * @class nav2_route::GoalIntentExtractor + * @brief Extracts the start and goal nodes in the graph to use for the routing + * request from the action request. This may use additional information about the + * environment or simply find the corresponding nodes specified in the request. + */ +class GoalIntentExtractor +{ +public: + /** + * @brief Constructor + */ + GoalIntentExtractor() = default; + + /** + * @brief Destructor + */ + ~GoalIntentExtractor() = default; + + /** + * @brief Configure extractor + * @param node Node to use to create any interface needed + * @param graph Graph to populate kD tree using + * @param id_to_graph_map Remapping vector to correlate nodeIDs + * @param tf TF buffer for transformations + * @param costmap_subscriber Costmap subscriber to use for traversability + * @param route_frame Planning frame + * @param base_frame Robot reference frame + */ + void configure( + nav2_util::LifecycleNode::SharedPtr node, + Graph & graph, + GraphToIDMap * id_to_graph_map, + std::shared_ptr tf, + std::shared_ptr costmap_subscriber, + const std::string & route_frame, + const std::string & base_frame); + + /** + * @brief Sets a new graph when updated + * @param graph Graph to populate kD tree using + * @param graph id_to_graph_map to get graph IDX for node IDs + */ + void setGraph(Graph & graph, GraphToIDMap * id_to_graph_map); + + /** + * @brief Transforms a pose into the route frame + * @param pose Pose to transform (e.g. start, goal) + * @param frame_id Frame to transform to + */ + geometry_msgs::msg::PoseStamped transformPose( + geometry_msgs::msg::PoseStamped & pose, + const std::string & frame_id); + /** + * @brief Main API to find the start and goal graph IDX (not IDs) for routing + * @param goal Action request goal + * @return start, goal node IDx + */ + template + NodeExtents + findStartandGoal(const std::shared_ptr goal); + + /** + * @brief Prune the start and end nodes in a route if the start or goal poses, + * respectively, are already along the route to avoid backtracking + * @param inpute_route Route to prune + * @param goal Action request goal + * @param first_time If this is the first time routing + * @return Pruned route + */ + template + Route pruneStartandGoal( + const Route & input_route, + const std::shared_ptr goal, + ReroutingState & rerouting_info); + + /** + * @brief Override the start pose for use in pruning if it is externally overridden + * Usually by the rerouting logic + * @param start_pose Starting pose of robot to prune using + */ + void overrideStart(const geometry_msgs::msg::PoseStamped & start_pose); + + /** + * @brief gets the desired start pose + * @return PoseStamped of start pose + */ + geometry_msgs::msg::PoseStamped getStart(); + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")}; + std::shared_ptr node_spatial_tree_; + GraphToIDMap * id_to_graph_map_; + Graph * graph_; + std::shared_ptr tf_; + std::shared_ptr costmap_subscriber_; + std::string route_frame_; + std::string base_frame_; + geometry_msgs::msg::PoseStamped start_, goal_; + bool prune_goal_, enable_search_; + int max_nn_search_iterations_; + float max_dist_from_edge_, min_dist_from_goal_, min_dist_from_start_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ diff --git a/nav2_route/include/nav2_route/goal_intent_search.hpp b/nav2_route/include/nav2_route/goal_intent_search.hpp new file mode 100644 index 00000000000..cd5a91d5476 --- /dev/null +++ b/nav2_route/include/nav2_route/goal_intent_search.hpp @@ -0,0 +1,266 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__GOAL_INTENT_SEARCH_HPP_ +#define NAV2_ROUTE__GOAL_INTENT_SEARCH_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "std_msgs/msg/color_rgba.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "nav2_route/types.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/line_iterator.hpp" + +namespace nav2_route +{ + +namespace GoalIntentSearch +{ + +/** + * @class Find which position is nearest to a given point on a costmap using breadth-first search + */ +class BreadthFirstSearch +{ +public: + /** + * @brief Constructor + * @param costmap Costmap to check + */ + explicit BreadthFirstSearch(std::shared_ptr costmap) + : costmap_(costmap) + { + } + + /** + * @brief Search for the closest node to the given reference node + * @param reference_node The reference node to search from + * @param candidate_nodes The candidate nodes to search for + * @param max_iterations The maximum number of iterations to perform + * @return True if a candidate node is found, false otherwise + */ + bool search( + const geometry_msgs::msg::PoseStamped & reference_node, + const std::vector & candidate_nodes, + const int max_iterations = std::numeric_limits::max()) + { + closest_node_idx_ = 0u; + + // Convert target to costmap space + unsigned int goal_x, goal_y; + if (!costmap_->worldToMap(reference_node.pose.position.x, reference_node.pose.position.y, + goal_x, goal_y)) + { + return false; + } + unsigned int goal_id = costmap_->getIndex(goal_x, goal_y); + + // Convert candidates to costmap space + std::vector candidate_ids; + candidate_ids.reserve(candidate_nodes.size()); + for (const auto & node : candidate_nodes) { + unsigned int node_x, node_y; + if (!costmap_->worldToMap(node.pose.position.x, node.pose.position.y, node_x, node_y)) { + // Off the costmap, consider this candidate invalid + continue; + } + candidate_ids.push_back(costmap_->getIndex(node_x, node_y)); + } + + if (candidate_ids.empty()) { + return false; + } + + // Main Breadth-First Search + std::queue search_queue; + std::unordered_set visited; + visited.insert(goal_id); + search_queue.push(goal_id); + int iterations = 0; + while (!search_queue.empty() && iterations < max_iterations) { + unsigned int current_id = search_queue.front(); + search_queue.pop(); + iterations++; + + // Check if the current node is a candidate + auto iter = std::find(candidate_ids.begin(), candidate_ids.end(), current_id); + if (iter != candidate_ids.end()) { + closest_node_idx_ = std::distance(candidate_ids.begin(), iter); + return true; + } + + // Get neighbors of the current node + std::vector neighbors = getNeighbors(current_id); + + // Add unvisited neighbors to the queue + for (const auto & neighbor : neighbors) { + if (visited.find(neighbor) == visited.end()) { + visited.insert(neighbor); + search_queue.push(neighbor); + } + } + } + + // No solution found with full expansion or iterations exceeded + return false; + } + + /** + * @brief Get the neighbors of a given node + * @param current_id The current node id + * @return A vector of neighbor node ids + */ + std::vector getNeighbors(const unsigned int current_id) + { + int size_x = static_cast(costmap_->getSizeInCellsX()); + const std::vector offsets = { + -1, 1, -size_x, size_x, + -size_x - 1, -size_x + 1, + size_x - 1, size_x + 1}; + int costmap_size = static_cast(costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY()); + + unsigned int p_mx, p_my; + costmap_->indexToCells(current_id, p_mx, p_my); + + std::vector neighbors; + for (const auto & offset : offsets) { + // If out of bounds of costmap + int signed_neighbor_id = static_cast(current_id) + offset; + if (signed_neighbor_id < 0 || signed_neighbor_id >= costmap_size) { + continue; + } + unsigned int neighbor_id = static_cast(signed_neighbor_id); + + // If wrapping around the costmap + unsigned int n_mx, n_my; + costmap_->indexToCells(neighbor_id, n_mx, n_my); + if (std::fabs(static_cast(p_mx) - static_cast(n_mx)) > 1 || + std::fabs(static_cast(p_my) - static_cast(n_my)) > 1) + { + continue; + } + + // Add if not in collision + if (!inCollision(neighbor_id)) { + neighbors.push_back(neighbor_id); + } + } + + return neighbors; + } + + /** + * @brief Check if a node is in collision with the costmap + * @param id The node id to check + * @return True if the node is in collision, false otherwise + */ + bool inCollision(const unsigned int id) + { + // Check if the node is in collision + float cost = static_cast(costmap_->getCost(id)); + return cost >= 253.0f && cost != 255.0f; + } + + /** + * @brief Get the output closest node in candidate indices + * @return closest_node The closest candidate node index to the given point by search + */ + unsigned int getClosestNodeIdx() + { + return closest_node_idx_; + } + + /** + * @brief Destructor + */ + ~BreadthFirstSearch() = default; + +protected: + std::shared_ptr costmap_; + unsigned int closest_node_idx_{0u}; +}; + +/** + * @class Find if a line segment is in collision with the costmap or not + */ +class LoSCollisionChecker +{ +public: + /** + * @brief Constructor + * @param costmap Costmap to check + */ + explicit LoSCollisionChecker(std::shared_ptr costmap) + : costmap_(costmap) + { + } + + /** + * @brief Destructor + */ + ~LoSCollisionChecker() = default; + + /** + * @brief Find the line segment in cosmap frame + * @param start Start point + * @param end End point + * @return True if the line segment is on the costmap, false otherwise + */ + bool worldToMap(const geometry_msgs::msg::Point & start, const geometry_msgs::msg::Point & end) + { + if (!costmap_->worldToMap(start.x, start.y, x0_, y0_) || + !costmap_->worldToMap(end.x, end.y, x1_, y1_)) + { + return false; + } + return true; + } + + /** + * @brief Check if the line segment is in collision with the costmap + * @return True if the line segment is in collision, false otherwise + */ + bool isInCollision() + { + nav2_util::LineIterator iter(x0_, y0_, x1_, y1_); + for (; iter.isValid(); iter.advance()) { + float cost = static_cast(costmap_->getCost(iter.getX(), iter.getY())); + if (cost >= 253.0f && cost != 255.0 /*unknown*/) { + return true; + } + } + return false; + } + +protected: + std::shared_ptr costmap_; + unsigned int x0_, x1_, y0_, y1_; +}; + +} // namespace GoalIntentSearch + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GOAL_INTENT_SEARCH_HPP_ diff --git a/nav2_route/include/nav2_route/graph_loader.hpp b/nav2_route/include/nav2_route/graph_loader.hpp new file mode 100644 index 00000000000..ea602b11019 --- /dev/null +++ b/nav2_route/include/nav2_route/graph_loader.hpp @@ -0,0 +1,107 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__GRAPH_LOADER_HPP_ +#define NAV2_ROUTE__GRAPH_LOADER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/interfaces/graph_file_loader.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" + +namespace nav2_route +{ + +/** + * @class nav2_route::GraphLoader + * @brief An action server to load graph files into the graph object for search and processing + */ +class GraphLoader +{ +public: + /** + * @brief A constructor for nav2_route::GraphLoader + * @param options Additional options to control creation of the node. + */ + explicit GraphLoader( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame); + + /** + * @brief A destructor for nav2_route::GraphLoader + */ + ~GraphLoader() = default; + + /** + * @brief Loads a graph object with file information from a filepath + * @param graph Graph to populate + * @param idx_map A map translating nodeid's to graph idxs for use in graph modification + * services and idx-based route planning requests. This is much faster than using a + * map the full graph data structure. + * @param filepath The filepath to the graph data + * @param graph_file_loader_id The id of the GraphFileLoader + * @return bool If successful + */ + bool loadGraphFromFile( + Graph & graph, + GraphToIDMap & idx_map, + const std::string & filepath); + + /** + * @brief Loads a graph object with file information from ROS parameter, if provided + * @param graph Graph to populate + * @param idx_map A map translating nodeid's to graph idxs for use in graph modification + * services and idx-based route planning requests. This is much faster than using a + * map the full graph data structure. + * @param graph_file_loader_id The id of the GraphFileLoader + * @return bool If successful or none provided + */ + bool loadGraphFromParameter( + Graph & graph, + GraphToIDMap & idx_map); + + /** + * @brief Transform the coordinates in the graph to the route frame + * @param[in/out] graph The graph to be transformed + * @return True if transformation was successful + */ + bool transformGraph(Graph & graph); + +protected: + std::string route_frame_, graph_filepath_; + std::shared_ptr tf_; + rclcpp::Logger logger_{rclcpp::get_logger("GraphLoader")}; + + // Graph Parser + pluginlib::ClassLoader plugin_loader_; + GraphFileLoader::Ptr graph_file_loader_; + std::string default_plugin_id_; + std::string plugin_type_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GRAPH_LOADER_HPP_ diff --git a/nav2_route/include/nav2_route/graph_saver.hpp b/nav2_route/include/nav2_route/graph_saver.hpp new file mode 100644 index 00000000000..328ec65840e --- /dev/null +++ b/nav2_route/include/nav2_route/graph_saver.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__GRAPH_SAVER_HPP_ +#define NAV2_ROUTE__GRAPH_SAVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/interfaces/graph_file_saver.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" + +namespace nav2_route +{ + +/** + * @class nav2_route::GraphSaver + * @brief An object to save graph objects to a file + */ +class GraphSaver +{ +public: + /** + * @brief A constructor for nav2_route::GraphSaver + * @param node Lifecycle node encapsulated by the GraphSaver + * @param tf A tf buffer + * @param frame Coordinate frame that the graph belongs to + */ + explicit GraphSaver( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame); + + /** + * @brief A destructor for nav2_route::GraphSaver + */ + ~GraphSaver() = default; + + /** + * @brief Saves a graph object to a file + * @param graph Graph to save + * @param filepath The filepath to the graph data + * @return bool If successful + */ + bool saveGraphToFile( + Graph & graph, + std::string filepath = ""); + + /** + * @brief Transform the coordinates in the graph to the route frame + * @param[in/out] graph The graph to be transformed + * @return True if transformation was successful + */ + bool transformGraph(Graph & graph); + +protected: + std::string route_frame_, graph_filepath_; + std::shared_ptr tf_; + rclcpp::Logger logger_{rclcpp::get_logger("GraphSaver")}; + + // Graph Parser + pluginlib::ClassLoader plugin_loader_; + GraphFileSaver::Ptr graph_file_saver_; + std::string default_plugin_id_; + std::string plugin_type_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GRAPH_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp new file mode 100644 index 00000000000..7aa9a3123ee --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp @@ -0,0 +1,88 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__INTERFACES__EDGE_COST_FUNCTION_HPP_ +#define NAV2_ROUTE__INTERFACES__EDGE_COST_FUNCTION_HPP_ + +#include +#include + +#include "tf2_ros/buffer.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "pluginlib/class_loader.hpp" +#include "nav2_route/types.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +namespace nav2_route +{ + +/** + * @class EdgeCostFunction + * @brief A plugin interface to score edges during graph search to modify + * the lowest cost path (e.g. by distance, maximum speed, regions prefer not to travel + * blocked by occupancy, or using arbitrarily defined user metadata stored in the + * edge and nodes of interest.) + */ +class EdgeCostFunction +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + EdgeCostFunction() = default; + + /** + * @brief Virtual destructor + */ + virtual ~EdgeCostFunction() = default; + + /** + * @brief Configure the scorer, but do not store the node + * @param parent pointer to user's node + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) = 0; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + */ + virtual bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) = 0; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + virtual std::string getName() = 0; + + /** + * @brief Prepare for a new cycle, by resetting state, grabbing data + * to use for all immediate requests, or otherwise prepare for scoring + */ + virtual void prepare() {} +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__EDGE_COST_FUNCTION_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/graph_file_loader.hpp b/nav2_route/include/nav2_route/interfaces/graph_file_loader.hpp new file mode 100644 index 00000000000..6774f0d1870 --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/graph_file_loader.hpp @@ -0,0 +1,71 @@ +// Copyright (c) 2025 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__INTERFACES__GRAPH_FILE_LOADER_HPP_ +#define NAV2_ROUTE__INTERFACES__GRAPH_FILE_LOADER_HPP_ + +#include +#include + + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/types.hpp" + +namespace nav2_route +{ + +/** + * @class GraphFileLoader + * @brief A plugin interface to parse a file into the graph + */ +class GraphFileLoader +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + GraphFileLoader() = default; + + /** + * @brief Virtual destructor + */ + virtual ~GraphFileLoader() = default; + + /** + * @brief Configure the graph file loader, but do not store the node + * @param parent pointer to user's node + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + + /** + * @brief Method to load the graph from the filepath + * @param graph The graph to populate + * @param filepath The file to parse + * @param idx_map A map translating nodeid's to graph idxs for use in graph modification + * services and idx-based route planning requests. This is much faster than using a + * map the full graph data structure. + * @return true if graph was successfully loaded + */ + virtual bool loadGraphFromFile( + Graph & graph, + GraphToIDMap & graph_to_id_map, + std::string filepath) = 0; +}; +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__GRAPH_FILE_LOADER_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp b/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp new file mode 100644 index 00000000000..4851655e4dd --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ +#define NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ + +#include +#include + + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/types.hpp" + +namespace nav2_route +{ + +/** + * @class GraphFileSaver + * @brief A plugin interface to parse a file into the graph + */ +class GraphFileSaver +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + GraphFileSaver() = default; + + /** + * @brief Virtual destructor + */ + virtual ~GraphFileSaver() = default; + + /** + * @brief Configure the graph file saver, but do not store the node + * @param parent pointer to user's node + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + + /** + * @brief Method to save the graph to the filepath + * @param graph The graph to save + * @param filepath The path to save the file to + * @return true if graph was successfully saved + */ + virtual bool saveGraphToFile( + Graph & graph, + std::string filepath) = 0; +}; +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/route_operation.hpp b/nav2_route/include/nav2_route/interfaces/route_operation.hpp new file mode 100644 index 00000000000..0c5d140f3ad --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/route_operation.hpp @@ -0,0 +1,138 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__INTERFACES__ROUTE_OPERATION_HPP_ +#define NAV2_ROUTE__INTERFACES__ROUTE_OPERATION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "pluginlib/class_loader.hpp" +#include "nav2_route/types.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +namespace nav2_route +{ + +/** + * @struct OperationResult + * @brief a struct to hold return from an operation + */ +struct OperationResult +{ + bool reroute{false}; + std::vector blocked_ids; +}; + +/** + * @enum nav2_route::RouteOperationType + * @brief The type of operation plugin + */ +enum class RouteOperationType +{ + ON_GRAPH = 0, + ON_STATUS_CHANGE = 1, + ON_QUERY = 2 +}; + +/** + * @class RouteOperation + * @brief A plugin interface to perform an operation while tracking the route + * such as triggered from the graph file when a particular node is achieved, + * edge is entered or exited. The API also supports triggering arbitrary operations + * when a status has changed (e.g. any node is achieved) or at a regular frequency + * on query set at a fixed rate of tracker_update_rate. Operations can request the + * system to reroute and avoid certain edges. Example operations may be to: + * reroute when blocked or at a required rate (though may want to use BTs instead), + * adjust speed limits, wait, call an external service or action to perform a task + * such as calling an elevator or opening an automatic door, etc. + * Operations may throw nav2_core::OperationFailed exceptions in failure cases + */ +class RouteOperation +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + RouteOperation() = default; + + /** + * @brief Destructor + */ + virtual ~RouteOperation() = default; + + /** + * @brief Configure the operation plugin (get params, create interfaces, etc) + * @param node A node to use + * @param name the plugin's name set by the param file that may need to be used + * to correlate an operation instance to the navigation graph operation calls + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) = 0; + + /** + * @brief An API to get the name of a particular operation for triggering, query + * or logging + * @return the plugin's name + */ + virtual std::string getName() = 0; + + /** + * @brief Indication of which type of route operation this plugin is. Whether it is + * be called by the graph's nodes or edges, whether it should be triggered at any + * status change, or whether it should be called constantly on any query. By default, + * it will create operations that are only called when a graph's node or edge requests it. + * Note that On Query, On Status Change, and On Graph are mutually exclusive since each + * operation type is merely a subset of the previous level's specificity. + * @return The type of operation (on graph call, on status changes, or constantly) + */ + virtual RouteOperationType processType() {return RouteOperationType::ON_GRAPH;} + + /** + * @brief The main route operation API to perform an operation when triggered. + * The return value indicates if the route operation is requesting rerouting when returning true. + * Could be if this operation is checking if a route is in collision or operation + * failed (to open a door, for example) and thus this current route is now invalid. + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, + * for additional context (must check nullptr if at goal) + * @param edge_entered Edge entered by node achievement, + * for additional context (must check if nullptr if no future edge, at goal) + * @param edge_exited Edge exited by node achievement, + * for additional context (must check if nullptr if no last edge, starting) + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + virtual OperationResult perform( + NodePtr node_achieved, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * mdata = nullptr) = 0; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__ROUTE_OPERATION_HPP_ diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp new file mode 100644 index 00000000000..ac5347bc0bd --- /dev/null +++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_ +#define NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_ + +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" + +namespace nav2_route +{ + +// Search in XY dimension for nearest neighbors +const size_t DIMENSION = 2; + +/** + * @class nav2_route::GraphAdaptor + * @brief An adaptor for Nanoflann to operate on our graph object without copying + */ +struct GraphAdaptor +{ + explicit GraphAdaptor(const Graph & obj_) + : obj(obj_) {} + + inline size_t kdtree_get_point_count() const {return obj.size();} + + inline double kdtree_get_pt(const size_t idx, const size_t dim) const + { + if (dim == 0) { + return obj[idx].coords.x; + } + return obj[idx].coords.y; + } + + template bool kdtree_get_bbox(BBOX & /*bb*/) const {return false;} + + const Graph & obj; +}; + +typedef nanoflann::KDTreeSingleIndexAdaptor< + nanoflann::L2_Simple_Adaptor, GraphAdaptor, DIMENSION> kd_tree_t; + +/** + * @class nav2_route::NodeSpatialTree + * @brief An object to find kNNs of the graph to determining the start and end + * nodes to utilize for planning in a free-space-style request of start and goal poses. + * Since the graph does not change over time, we can precompute this quadtree and reuse it + * over many requests. + */ +class NodeSpatialTree +{ +public: + /** + * @brief Constructor + */ + NodeSpatialTree() = default; + + /** + * @brief Destructor + */ + ~NodeSpatialTree(); + + /** + * @brief Compute the kd-tree based on the graph node information + * @param graph The graph of nodes for the route + */ + void computeTree(Graph & graph); + + /** + * @brief Find the closest node to a given pose + * @param pose_in Pose to find node near + * @param node_id The return ID of the node + * @return if successfully found + */ + bool findNearestGraphNodesToPose( + const geometry_msgs::msg::PoseStamped & pose_in, + std::vector & node_ids); + + /** + * @brief Set the number of nodes to search in local area for + * @param num Numbers of nearest nodes to return + */ + void setNumOfNearestNodes(int num_of_nearest_nodes); + +protected: + kd_tree_t * kdtree_; + GraphAdaptor * adaptor_; + Graph * graph_; + int num_of_nearest_nodes_{3}; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_ diff --git a/nav2_route/include/nav2_route/operations_manager.hpp b/nav2_route/include/nav2_route/operations_manager.hpp new file mode 100644 index 00000000000..b30045f94c9 --- /dev/null +++ b/nav2_route/include/nav2_route/operations_manager.hpp @@ -0,0 +1,126 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/interfaces/route_operation.hpp" + +#ifndef NAV2_ROUTE__OPERATIONS_MANAGER_HPP_ +#define NAV2_ROUTE__OPERATIONS_MANAGER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::OperationsManager + * @brief Manages operations plugins to call on route tracking + */ +class OperationsManager +{ +public: + typedef std::vector::const_iterator OperationsIter; + + /** + * @brief A constructor for nav2_route::OperationsManager + */ + explicit OperationsManager( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber); + + /** + * @brief A Destructor for nav2_route::OperationsManager + */ + ~OperationsManager() = default; + + /** + * @brief Finds the set of operations stored in the graph to trigger at this transition + * @param node Node to check + * @param edge_entered Edge entered to check for ON_ENTER events + * @param edge_exit Edge exit to check for ON_EXIT events + * @return OperationPtrs A vector of operation pointers to execute + */ + OperationPtrs findGraphOperations( + const NodePtr node, const EdgePtr edge_enter, const EdgePtr edge_exit); + + /** + * @brief Finds the set of operations stored in graph objects, by event + * @param node op_vec Operations vector to check + * @param trigger Trigger for which operations in op_vec should be included + * @param operations Output vector populated with relevant operations + */ + template + void findGraphOperationsToProcess( + T & obj, + const OperationTrigger & trigger, + OperationPtrs & operations); + + /** + * @brief Updates manager result state by an individual operation's result + * @param name Operations' name + * @param op_result Operations' result + * @param result Manager's result to update + */ + void updateResult( + const std::string & name, const OperationResult & op_result, OperationsResult & result); + + /** + * @brief Processes the operations at this tracker state + * @param status_change Whether something meaningful has changed + * @param state The route tracking state to check for state info + * @param route The raw route being tracked + * @param pose robot pose + * @param rerouting_info Rerouting information regarding previous partial state + * @return A result vector whether the operations are requesting something to occur + */ + OperationsResult process( + const bool status_change, + const RouteTrackingState & state, + const Route & route, + const geometry_msgs::msg::PoseStamped & pose, + const ReroutingState & rerouting_info); + +protected: + /** + * @brief Processes a vector of operations plugins, by trigger + * @param operations Operations to trigger + * @param Results to populate from operations + */ + void processOperationsPluginVector( + const std::vector & operations, OperationsResult & result, + const NodePtr node, + const EdgePtr edge_entered, + const EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & pose); + + pluginlib::ClassLoader plugin_loader_; + std::unordered_map graph_operations_; + std::vector change_operations_; + std::vector query_operations_; + rclcpp::Logger logger_{rclcpp::get_logger("OperationsManager")}; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__OPERATIONS_MANAGER_HPP_ diff --git a/nav2_route/include/nav2_route/path_converter.hpp b/nav2_route/include/nav2_route/path_converter.hpp new file mode 100644 index 00000000000..90b61898160 --- /dev/null +++ b/nav2_route/include/nav2_route/path_converter.hpp @@ -0,0 +1,88 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PATH_CONVERTER_HPP_ +#define NAV2_ROUTE__PATH_CONVERTER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::PathConverter + * @brief An helper to convert the route into dense paths + */ +class PathConverter +{ +public: + /** + * @brief A constructor for nav2_route::PathConverter + */ + PathConverter() = default; + + /** + * @brief A destructor for nav2_route::PathConverter + */ + ~PathConverter() = default; + + /** + * @brief Configure the object + * @param node Node to use to get params and create interfaces + */ + void configure(nav2_util::LifecycleNode::SharedPtr node); + + /** + * @brief Convert a Route into a dense path + * @param route Route object + * @param rerouting_info Re-Route info in case partial path to be populated + * @param frame Frame ID from planning + * @param now Time + * @return Path of the route + */ + nav_msgs::msg::Path densify( + const Route & route, + const ReroutingState & rerouting_info, + const std::string & frame, + const rclcpp::Time & now); + + /** + * @brief Convert an individual edge into a dense line + * @param x0 X initial + * @param y0 Y initial + * @param x1 X final + * @param y1 Y final + * @param poses Pose vector reference to populate + */ + void interpolateEdge( + float x0, float y0, float x1, float y1, + std::vector & poses); + +protected: + rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_pub_; + float density_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PATH_CONVERTER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp new file mode 100644 index 00000000000..dfa632962a7 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__COSTMAP_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__COSTMAP_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +namespace nav2_route +{ + +/** + * @class CostmapScorer + * @brief Scores edges by the average or maximum cost found while iterating over the + * edge's line segment in the global costmap + */ +class CostmapScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + CostmapScorer() = default; + + /** + * @brief destructor + */ + virtual ~CostmapScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + + /** + * @brief Prepare for a new cycle, by resetting state, grabbing data + * to use for all immediate requests, or otherwise prepare for scoring + */ + void prepare() override; + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("CostmapScorer")}; + rclcpp::Clock::SharedPtr clock_; + std::string name_; + bool use_max_, invalid_on_collision_, invalid_off_map_; + float weight_, max_cost_; + std::shared_ptr costmap_subscriber_; + std::shared_ptr costmap_{nullptr}; + unsigned int check_resolution_ {1u}; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__COSTMAP_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp new file mode 100644 index 00000000000..6e57c9a78c3 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DISTANCE_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DISTANCE_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class DistanceScorer + * @brief Scores edges by the distance traversed, weighted by speed limit metadata + * to optimize for time to goal, when %-based speed limits are set + */ +class DistanceScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + DistanceScorer() = default; + + /** + * @brief destructor + */ + virtual ~DistanceScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + std::string name_; + std::string speed_tag_; + float weight_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DISTANCE_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp new file mode 100644 index 00000000000..e28c484600f --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DYNAMIC_EDGES_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DYNAMIC_EDGES_SCORER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_msgs/srv/dynamic_edges.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class DynamicEdgesScorer + * @brief Rejects edges that are in the closed set of edges for navigation to prevent + * routes from containing paths blocked or otherwise deemed not currently traversable + */ +class DynamicEdgesScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + DynamicEdgesScorer() = default; + + /** + * @brief destructor + */ + virtual ~DynamicEdgesScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + + /** + * @brief Service callback to process edge changes + * @param request Service request containing newly closed edges or opened edges + * @param response Response to service (empty) + */ + void closedEdgesCb( + const std::shared_ptr request, + std::shared_ptr response); + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("DynamicEdgesScorer")}; + std::string name_; + std::set closed_edges_; + std::unordered_map dynamic_penalties_; + rclcpp::Service::SharedPtr service_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DYNAMIC_EDGES_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp new file mode 100644 index 00000000000..562f5b67339 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp @@ -0,0 +1,92 @@ +// Copyright (c) 2024, Polymath Robotics Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" +#include "angles/angles.h" + +namespace nav2_route +{ + +/** + * @class GoalOrientationScorer + * @brief Scores an edge leading to the goal node by comparing the orientation of the route + * pose and the orientation of the edge by multiplying the deviation from the desired + * orientation with a user defined weight. An alternative method can be selected, with + * the use_orientation_threshold flag, which rejects the edge it is greater than some + * tolerance + */ +class GoalOrientationScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + GoalOrientationScorer() = default; + + /** + * @brief destructor + */ + virtual ~GoalOrientationScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("GoalOrientationScorer")}; + std::string name_; + double orientation_tolerance_; + float orientation_weight_; + bool use_orientation_threshold_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp new file mode 100644 index 00000000000..476fe2f269b --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp @@ -0,0 +1,80 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__PENALTY_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__PENALTY_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class PenaltyScorer + * @brief Adjusts the score of an edge by an amount set as metadata in the graph + */ +class PenaltyScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + PenaltyScorer() = default; + + /** + * @brief destructor + */ + virtual ~PenaltyScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + std::string name_; + std::string penalty_tag_; + float weight_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__PENALTY_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp new file mode 100644 index 00000000000..eabeb980478 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp @@ -0,0 +1,96 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__SEMANTIC_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__SEMANTIC_SCORER_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class SemanticScorer + * @brief Scores an edge based on arbitrary graph semantic data such as set priority/danger + * levels or regional attributes (e.g. living room, bathroom, work cell 2) + */ +class SemanticScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + SemanticScorer() = default; + + /** + * @brief destructor + */ + virtual ~SemanticScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Scores graph object based on metadata's semantic value at key + * @param mdata Metadata + * @param score to add to + */ + void metadataValueScorer(Metadata & mdata, float & score); + + /** + * @brief Scores graph object based on metadata's key values + * @param mdata Metadata + * @param score to add to + */ + void metadataKeyScorer(Metadata & mdata, float & score); + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + std::string name_, key_; + std::unordered_map semantic_info_; + float weight_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__SEMANTIC_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp new file mode 100644 index 00000000000..41d92970704 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025, Polymath Robotics Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" +#include "angles/angles.h" + +namespace nav2_route +{ + +/** + * @class StartPoseOrientationScorer + * @brief Scores initial edge by comparing the orientation of the robot's current + * pose and the orientation of the edge by multiplying the deviation from the desired + * orientation with a user defined weight. An alternative method can be selected, with + * the use_orientation_threshold flag, which rejects the edge it is greater than some + * tolerance + */ +class StartPoseOrientationScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + StartPoseOrientationScorer() = default; + + /** + * @brief destructor + */ + virtual ~StartPoseOrientationScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("StartPoseOrientationScorer")}; + std::string name_; + std::shared_ptr tf_buffer_; + double orientation_tolerance_; + float orientation_weight_; + bool use_orientation_threshold_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp new file mode 100644 index 00000000000..1213a7d9391 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp @@ -0,0 +1,82 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__TIME_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__TIME_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class TimeScorer + * @brief Scores edges by the time to traverse an edge. + * It uses previous times to navigate the edge primarily, then secondarily uses + * maximum speed and absolute speed limits to estimate with edge length. + */ +class TimeScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + TimeScorer() = default; + + /** + * @brief destructor + */ + virtual ~TimeScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + std::string name_; + std::string speed_tag_, prev_time_tag_; + float weight_, max_vel_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__TIME_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp b/nav2_route/include/nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp new file mode 100644 index 00000000000..0cb9f34ae17 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp @@ -0,0 +1,148 @@ +// Copyright (c) 2025 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/graph_file_loader.hpp" + +#ifndef NAV2_ROUTE__PLUGINS__GRAPH_FILE_LOADERS__GEOJSON_GRAPH_FILE_LOADER_HPP_ +#define NAV2_ROUTE__PLUGINS__GRAPH_FILE_LOADERS__GEOJSON_GRAPH_FILE_LOADER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::GeoJsonGraphFileLoader + * @brief A GraphFileLoader plugin to load a geojson graph representation + */ +class GeoJsonGraphFileLoader : public GraphFileLoader +{ +public: + using Json = nlohmann::json; + + /** + * @brief Constructor + */ + GeoJsonGraphFileLoader() = default; + + /** + * @brief Destructor + */ + ~GeoJsonGraphFileLoader() = default; + + /** + * @brief Configure, but do not store the node + * @param parent pointer to user's node + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + /** + * @brief Loads the geojson file into the graph + * @param graph The graph to be populated by the geojson file + * @param graph_to_id_map A map of node id's to the graph index + * @param filepath The path of the file to load + * @return True if the graph was successfully loaded + */ + bool loadGraphFromFile( + Graph & graph, + GraphToIDMap & graph_to_id_map, + std::string filepath) override; + +protected: + /** + * @brief Checks if a file even exists on the filesystem + * @param filepath The filepath to be checked + * @return True if the file path provided exists + */ + bool doesFileExist(const std::string & filepath); + + /** + * @brief Get the nodes and edges from features + * @param[in] features The features that contain the nodes and edges + * @param[out] nodes The nodes found within the features + * @param[out] edges The edges found within the features + */ + void getGraphElements( + const Json & features, std::vector & nodes, std::vector & edges); + + /** + * @brief Add nodes into the graph + * @param[out] graph The graph that will contain the new nodes + * @param[out] graph_to_id_map A map of node id to the graph index + * @param[in] nodes The nodes to be added into the graph + */ + void addNodesToGraph(Graph & graph, GraphToIDMap & graph_to_id_map, std::vector & nodes); + + /** + * @brief Add edges into the graph + * @param[out] graph The graph that will contain the new edges + * @param[in] graph_to_id_map A map of node id to the graph id + * @param[in] edges The edges to be added into the graph + */ + void addEdgesToGraph(Graph & graph, GraphToIDMap & graph_to_id_map, std::vector & edges); + + /** + * @brief Converts the coordinates from the json object into the Coordinates type + * @param node The json object that holds the coordinate data + * @return The coordinates found in the json object + */ + Coordinates convertCoordinatesFromJson(const Json & node); + + /** + * @brief Converts the metadata from the json object into the metadata type + * @param properties The json object that holds the metadata + * @param key The key for the metadata + * @return The converted metadata + */ + Metadata convertMetaDataFromJson(const Json & properties, const std::string & key = "metadata"); + + /** + * @brief Converts the operation from the json object into the operation type + * @param json_operation The json object that holds the operation data + * @return The converted operation data + */ + Operation convertOperationFromJson(const Json & json_operation); + + /** + * @brief Converts the operations data from the json object into the operations type if present + * @param properties The json object that holds the operations data + * @return Operations The converted operations data + */ + Operations convertOperationsFromJson(const Json & properties); + + /** + * @brief Converts the edge cost data from the json object into the edge cost type + * @param properties The json object that holds the edge cost data + * @return EdgeCost The converted edge cost data + */ + EdgeCost convertEdgeCostFromJson(const Json & properties); + + rclcpp::Logger logger_{rclcpp::get_logger("GeoJsonGraphFileLoader")}; +}; + +NLOHMANN_JSON_SERIALIZE_ENUM( + OperationTrigger, { + {OperationTrigger::NODE, "NODE"}, + {OperationTrigger::ON_ENTER, "ON_ENTER"}, + {OperationTrigger::ON_EXIT, "ON_EXIT"}, + }) + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__GRAPH_FILE_LOADERS__GEOJSON_GRAPH_FILE_LOADER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp b/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp new file mode 100644 index 00000000000..6352462c8da --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/graph_file_saver.hpp" +#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" + +#ifndef NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ +#define NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::GeoJsonGraphFileSaver + * @brief A GraphFileSaver plugin to save a geojson graph representation + */ +class GeoJsonGraphFileSaver : public GraphFileSaver +{ +public: + using Json = nlohmann::json; + + /** + * @brief Constructor + */ + GeoJsonGraphFileSaver() = default; + + /** + * @brief Destructor + */ + ~GeoJsonGraphFileSaver() = default; + + /** + * @brief Configure, but do not store the node + * @param parent pointer to user's node + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + /** + * @brief Saves the graph to a geojson file + * @param graph The graph to save to the geojson file + * @param filepath The path to save the graph to + * @return True if successful + */ + bool saveGraphToFile( + Graph & graph, + std::string filepath) override; + +protected: + /** + * @brief Add nodes into the graph + * @param[out] graph The graph that will contain the new nodes + * @param[in] json_features Json array to add the nodes to + */ + void loadNodesFromGraph(Graph & graph, std::vector & json_features); + + /** + * @brief Add edges into the graph + * @param[out] graph The graph that will contain the new edges + * @param[in] json_edges Json array to add the edges to + */ + void loadEdgesFromGraph(Graph & graph, std::vector & json_edges); + + /** + * @brief Convert graph metadata to Json + * @param metadata Metadata from a node or edge in the graph + * @param json_metadata Json entry to store metadata in + */ + void convertMetaDataToJson(const Metadata & metadata, Json & json_metadata); + + /** + * @brief Convert graph operation to Json + * @param Operations Operations information from the graph + * @param json_operations Json entry to store operation data in + */ + void convertOperationsToJson(const Operations & operations, Json & json_operations); + + rclcpp::Logger logger_{rclcpp::get_logger("GeoJsonGraphFileSaver")}; +}; +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operation_client.hpp b/nav2_route/include/nav2_route/plugins/route_operation_client.hpp new file mode 100644 index 00000000000..e0862ed81e2 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operation_client.hpp @@ -0,0 +1,224 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__ROUTE_OPERATION_CLIENT_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATION_CLIENT_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace nav2_route +{ + +using namespace std::chrono_literals; // NOLINT + +/** + * @class RouteOperationClient + * @brief A route operation base class to trigger a service event at a node or edge. + * This is meant to be named accordingly to the event of query in the parameter file + * (e.g. OpenDoor, CallElevator). Thus, a single RouteOperationClient implementation can + * support many different operation instances calling different services. The template type + * may be overridden for any applications. It may be set up to either trigger a + * single service if the `service_name` is set in the parameter file at launch for all calls + * **or** trigger different services depending on the `service_name` set in the metadata of the + * node or edge operation given. + * + * For example: OpenDoor + * RouteOperationClient instance can open a door at service "/open_door" set in the parameter file + * that it calls every time the OpenDoor operation is triggered in the graph (ceneralized service). + * Or the OpenDoor instance can call any service whose `service_name` is set in the operation's + * metadata (e.g. "/open_door1", "/open_door2", ...) corresponding to other instances of similar + * use. In this case, the specification of "OpenDoor" operation is less to do with the service name + * and more to do with the type of service template (nav2_msgs) used to populate the request + * (decentralized). This allows for massive reuse of a single plugin implementation in centralized + * or decentralized applications. + * + * The templated nature of this node makes it a base class for any such service + * containing additional request or response fields by implementing the virtual interfaces + * configureEvent, populateRequest, processResponse appropriately. The std_srvs/Trigger + * example TriggerEvent can be thought of as a useful entry level demo and useful working primitive + */ +template +class RouteOperationClient : public RouteOperation +{ +public: + /** + * @brief Constructor + */ + RouteOperationClient() = default; + + /** + * @brief destructor + */ + virtual ~RouteOperationClient() = default; + + /** + * @brief Configure client with any necessary parameters, etc. + * May change or reset `main_srv_name_` variable to control + * main service name and existence. + */ + virtual void configureEvent( + const rclcpp_lifecycle::LifecycleNode::SharedPtr /*node*/, + const std::string & /*name*/) {} + + /** + * @brief Populate request with details for service, if necessary + */ + virtual void populateRequest( + std::shared_ptr/*request*/, const Metadata * /*mdata*/) {} + + /** + * @brief Process response from service to populate a result, if necessary + */ + virtual OperationResult processResponse( + std::shared_ptr/*response*/) {return OperationResult();} + +protected: + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr, + const std::string & name) final + { + RCLCPP_INFO(node->get_logger(), "Configuring route operation client: %s.", name.c_str()); + name_ = name; + logger_ = node->get_logger(); + node_ = node; + callback_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface()); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".service_name", rclcpp::ParameterValue("")); + main_srv_name_ = node->get_parameter(getName() + ".service_name").as_string(); + + configureEvent(node, name); + + // There exists a single central service to use, create client. + // If this is set to empty string after configuration, then the individual nodes will + // indicate the endpoint for the particular service call. + if (!main_srv_name_.empty()) { + main_client_ = node->create_client( + main_srv_name_, rclcpp::SystemDefaultsQoS(), callback_group_); + } + } + + /** + * @brief The main operation to call a service of arbitrary type and arbitrary name + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr node_achieved, + EdgePtr /*edge_entered*/, + EdgePtr /*edge_exited*/, + const Route & /*route*/, + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const Metadata * mdata) final + { + auto req = std::make_shared(); + std::shared_ptr response; + populateRequest(req, mdata); + + std::string srv_name; + srv_name = mdata->getValue("service_name", srv_name); + if (srv_name.empty() && !main_client_) { + throw nav2_core::OperationFailed( + "Route operation service (" + getName() + ") needs 'server_name' " + "set in the param file or in the operation's metadata!"); + } + + if (srv_name.empty()) { + srv_name = main_srv_name_; + response = callService(main_client_, req); + } else { + auto node = node_.lock(); + auto client = node->create_client( + srv_name, rclcpp::SystemDefaultsQoS(), callback_group_); + response = callService(client, req); + } + + RCLCPP_INFO( + logger_, + "%s: Processed operation at Node %i with service %s.", + name_.c_str(), node_achieved->nodeid, srv_name.c_str()); + return processResponse(response); + } + + std::shared_ptr callService( + typename rclcpp::Client::SharedPtr client, + std::shared_ptr req, + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(500ms)) + { + auto node = node_.lock(); + if (!client->wait_for_service(1s)) { + throw nav2_core::OperationFailed( + "Route operation service " + + std::string(client->get_service_name()) + " is not available!"); + } + + auto result = client->async_send_request(req); + if (callback_group_executor_.spin_until_future_complete(result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + throw nav2_core::OperationFailed( + "Route operation service " + + std::string(client->get_service_name()) + " failed to call!"); + } + + return result.get(); + } + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() final {return RouteOperationType::ON_GRAPH;} + + std::string name_, main_srv_name_; + std::atomic_bool reroute_; + rclcpp::Logger logger_{rclcpp::get_logger("RouteOperationClient")}; + typename rclcpp::Client::SharedPtr main_client_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATION_CLIENT_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/adjust_speed_limit.hpp b/nav2_route/include/nav2_route/plugins/route_operations/adjust_speed_limit.hpp new file mode 100644 index 00000000000..899e0837094 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/adjust_speed_limit.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__ADJUST_SPEED_LIMIT_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__ADJUST_SPEED_LIMIT_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" + +namespace nav2_route +{ + +/** + * @class AdjustSpeedLimit + * @brief A route operation to process on state changes to evaluate if speed limits + * should be adjusted based on graph edge metadata + */ +class AdjustSpeedLimit : public RouteOperation +{ +public: + /** + * @brief Constructor + */ + AdjustSpeedLimit() = default; + + /** + * @brief destructor + */ + virtual ~AdjustSpeedLimit() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() override {return RouteOperationType::ON_STATUS_CHANGE;} + + /** + * @brief The main speed limit operation to adjust the maximum speed of the vehicle + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr node_achieved, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * mdata = nullptr) override; + +protected: + std::string name_; + std::string speed_tag_; + rclcpp::Logger logger_{rclcpp::get_logger("AdjustSpeedLimit")}; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr speed_limit_pub_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__ADJUST_SPEED_LIMIT_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp b/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp new file mode 100644 index 00000000000..9c73ef8f395 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__COLLISION_MONITOR_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__COLLISION_MONITOR_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_core/route_exceptions.hpp" + +namespace nav2_route +{ + +/** + * @class CollisionMonitor + * @brief A route operation to process a costmap to determine if a route is blocked + * requiring immediate rerouting. If using the local costmap topic, then it will check + * in the local time horizon only. if using the global, it may check the full route for + * continued validity. It is however recommended to specify a maximum collision distance + * for evaluation to prevent necessarily long-term evaluation of collision information which + * may not be representative of the conditions in that area by the time the robot gets there. + */ +class CollisionMonitor : public RouteOperation +{ +public: + struct LineSegment + { + unsigned int x0, y0, x1, y1; + }; + + /** + * @brief Constructor + */ + CollisionMonitor() = default; + + /** + * @brief destructor + */ + virtual ~CollisionMonitor() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() override {return RouteOperationType::ON_QUERY;} + + /** + * @brief The main speed limit operation to adjust the maximum speed of the vehicle + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr /*node*/, + EdgePtr curr_edge, + EdgePtr /*edge_exited*/, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * /*mdata*/) override; + +protected: + /** + * @brief Backs out the end coordinate along the line segment start-end to length dist + * @param start Start of line segment + * @param end End of line segment + * @param dist Distance along line segment to find the new end point along + * @return Coordinates of the new end point `dist` away from the start along the line + */ + Coordinates backoutValidEndPoint( + const Coordinates & start, const Coordinates & end, const float dist); + + /** + * @brief Backs out the line end coordinates of the start-end line segment + * where costmap transforms are possible + * @param start Start of line segment + * @param line LineSegment object to replace the x1/y1 values for along segment until invalid + * @return If any part s of the segment requested is valid + */ + bool backoutValidEndPoint(const Coordinates & start, LineSegment & line); + + /** + * @brief Converts a line segment start-end into a LineSegment struct in costmap frame + * @param start Start of line segment + * @param end End of line segment + * @param line LineSegment object to populate + * @return If line segment is valid (e.g. start and end both in costmap transforms) + */ + bool lineToMap(const Coordinates & start, const Coordinates & end, LineSegment & line); + + /** + * @brief Checks a line segment in costmap frame for validity + * @param line LineSegment object to collision check in costmap set + * @return If any part of the line segment is in collision + */ + bool isInCollision(const LineSegment & line); + + /** + * @brief Gets the latest costmap from the costmap subscriber + */ + void getCostmap(); + + std::string name_, topic_; + std::atomic_bool reroute_; + rclcpp::Logger logger_{rclcpp::get_logger("CollisionMonitor")}; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Time last_check_time_; + rclcpp::Duration checking_duration_{0, 0}; + float max_collision_dist_, max_cost_; + bool reroute_on_collision_; + unsigned int check_resolution_{1u}; + std::shared_ptr costmap_subscriber_; + std::shared_ptr costmap_{nullptr}; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__COLLISION_MONITOR_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/rerouting_service.hpp b/nav2_route/include/nav2_route/plugins/route_operations/rerouting_service.hpp new file mode 100644 index 00000000000..30e6e97353b --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/rerouting_service.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__REROUTING_SERVICE_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__REROUTING_SERVICE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_util/node_utils.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace nav2_route +{ + +/** + * @class ReroutingService + * @brief A route operation to process requests from an external server for rerouting + */ +class ReroutingService : public RouteOperation +{ +public: + /** + * @brief Constructor + */ + ReroutingService() = default; + + /** + * @brief destructor + */ + virtual ~ReroutingService() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() override {return RouteOperationType::ON_QUERY;} + + /** + * @brief The main speed limit operation to adjust the maximum speed of the vehicle + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr node, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * mdata = nullptr) override; + + /** + * @brief Service callback to trigger a reroute externally + * @param request, empty + * @param response, returns success + */ + void serviceCb( + const std::shared_ptr request, + std::shared_ptr response); + +protected: + std::string name_; + std::atomic_bool reroute_; + rclcpp::Logger logger_{rclcpp::get_logger("ReroutingService")}; + rclcpp::Service::SharedPtr service_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__REROUTING_SERVICE_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/time_marker.hpp b/nav2_route/include/nav2_route/plugins/route_operations/time_marker.hpp new file mode 100644 index 00000000000..584c5d49c9c --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/time_marker.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TIME_MARKER_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TIME_MARKER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" + +namespace nav2_route +{ + +/** + * @class TimeMarker + * @brief A route operation to add accurate times to the graph's edges after navigation + * to use in later iterations for a more refined estimate of actual travel time + */ +class TimeMarker : public RouteOperation +{ +public: + /** + * @brief Constructor + */ + TimeMarker() = default; + + /** + * @brief destructor + */ + virtual ~TimeMarker() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() override {return RouteOperationType::ON_STATUS_CHANGE;} + + /** + * @brief The main speed limit operation to adjust the maximum speed of the vehicle + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr node_achieved, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * mdata = nullptr) override; + +protected: + std::string name_; + std::string time_tag_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Time edge_start_time_; + unsigned int curr_edge_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TIME_MARKER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/trigger_event.hpp b/nav2_route/include/nav2_route/plugins/route_operations/trigger_event.hpp new file mode 100644 index 00000000000..39e77532392 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/trigger_event.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TRIGGER_EVENT_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TRIGGER_EVENT_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "nav2_route/plugins/route_operation_client.hpp" + +namespace nav2_route +{ + +/** + * @class TriggerEvent + * @brief A route operation to trigger an event at a node or edge. This operation is meant to be + * named accordingly to the event of query in the parameter file (e.g. OpenDoor, CallElevator). + * Thus, a single TriggerEvent plugin type can support many different operation instances + * calling different services. It may be set up to either trigger a + * single service if the `service_name` is set in the parameter file at launch + * **or** trigger different services depending on the `service_name` set in the metadata of the + * node or edge operation given for centralized or decentralized events by node or edge. + * + * See the Route Operation Client for more details + */ +class TriggerEvent : public RouteOperationClient +{ +public: + /** + * @brief Constructor + */ + TriggerEvent() = default; + + /** + * @brief destructor + */ + virtual ~TriggerEvent() = default; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TRIGGER_EVENT_HPP_ diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp new file mode 100644 index 00000000000..3a75c70bf34 --- /dev/null +++ b/nav2_route/include/nav2_route/route_planner.hpp @@ -0,0 +1,162 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__ROUTE_PLANNER_HPP_ +#define NAV2_ROUTE__ROUTE_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/edge_scorer.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::RoutePlanner + * @brief An optimal planner to compute a route from a start to a goal in an arbitrary graph + */ +class RoutePlanner +{ +public: + /** + * @brief A constructor for nav2_route::RoutePlanner + */ + RoutePlanner() = default; + + /** + * @brief A destructor for nav2_route::RoutePlanner + */ + virtual ~RoutePlanner() = default; + + /** + * @brief Configure the route planner, get parameters + * @param node Node object to get parametersfrom + * @param tf_buffer TF buffer to use for transformations + * @param costmap_subscriber Costmap subscriber to use for blocked nodes + */ + void configure( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::shared_ptr costmap_subscriber); + + /** + * @brief Find the route from start to goal on the graph + * @param graph Graph to search + * @param start Start index in the graph of the start node + * @param goal Goal index in the graph of the goal node + * @param blocked_ids A set of blocked node and edge IDs not to traverse + * @return Route object containing the navigation graph route + */ + virtual Route findRoute( + Graph & graph, unsigned int start_index, unsigned int goal_index, + const std::vector & blocked_ids, + const RouteRequest & route_request); + +protected: + /** + * @brief Reset the search state of the graph nodes + * @param graph Graph to reset + */ + inline void resetSearchStates(Graph & graph); + + /** + * @brief Dikstra's algorithm search on the graph + * @param graph Graph to search + * @param start Start Node pointer + * @param goal Goal node pointer + * @param blocked_ids A set of blocked node and edge IDs not to traverse + */ + void findShortestGraphTraversal( + Graph & graph, const NodePtr start_node, const NodePtr goal_node, + const std::vector & blocked_ids, + const RouteRequest & route_request); + + /** + * @brief Gets the traversal cost for an edge using edge scorers + * @param edge Edge pointer to find traversal cost for + * @param travel cost + * @param blocked_ids A set of blocked node and edge IDs not to traverse + * @return if this edge is valid for search + */ + inline bool getTraversalCost( + const EdgePtr edge, float & score, const std::vector & blocked_ids, + const RouteRequest & route_request); + + /** + * @brief Gets the next node in the priority queue for search + * @return Next node pointer in queue with cost + */ + inline NodeElement getNextNode(); + + /** + * @brief Adds a node to the priority queue for search + * @param cost Priority level + * @param node Node pointer to insert + */ + inline void addNode(const float cost, const NodePtr node); + + /** + * @brief Gets the edges from a given node + * @param node Node pointer to check + * @return A vector of edges that the node contains + */ + inline EdgeVector & getEdges(const NodePtr node); + + /** + * @brief Clears the priority queue + */ + inline void clearQueue(); + + /** + * @brief Checks if a given node is the goal node + * @param node Node to check + * @return bool If this node is the goal + */ + inline bool isGoal(const NodePtr node); + + /** + * @brief Checks if a given node is the start node + * @param node Node to check + * @return bool If this node is the start + */ + inline bool isStart(const NodePtr node); + + /** + * @brief Checks edge is a start or end edge + * @param edge Edge to check + * @return EdgeType identifying whether the edge is start, end or none + */ + nav2_route::EdgeType classifyEdge(const EdgePtr edge); + + int max_iterations_{0}; + unsigned int start_id_{0}; + unsigned int goal_id_{0}; + NodeQueue queue_; + std::unique_ptr edge_scorer_; + std::shared_ptr tf_buffer_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__ROUTE_PLANNER_HPP_ diff --git a/nav2_route/include/nav2_route/route_server.hpp b/nav2_route/include/nav2_route/route_server.hpp new file mode 100644 index 00000000000..a6b830c1b0e --- /dev/null +++ b/nav2_route/include/nav2_route/route_server.hpp @@ -0,0 +1,236 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__ROUTE_SERVER_HPP_ +#define NAV2_ROUTE__ROUTE_SERVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/create_timer_ros.h" +#include "nav2_util/simple_action_server.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/service_server.hpp" +#include "nav2_msgs/action/compute_route.hpp" +#include "nav2_msgs/action/compute_and_track_route.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "nav2_msgs/msg/route_node.hpp" +#include "nav2_msgs/srv/set_route_graph.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/graph_loader.hpp" +#include "nav2_route/route_planner.hpp" +#include "nav2_route/path_converter.hpp" +#include "nav2_route/route_tracker.hpp" +#include "nav2_route/goal_intent_extractor.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::RouteServer + * @brief An action server implements a Navigation Route-Graph planner + * to compliment free-space planning in the Planner Server + */ +class RouteServer : public nav2_util::LifecycleNode +{ +public: + using ComputeRoute = nav2_msgs::action::ComputeRoute; + using ComputeRouteGoal = ComputeRoute::Goal; + using ComputeRouteResult = ComputeRoute::Result; + using ComputeRouteServer = nav2_util::SimpleActionServer; + + using ComputeAndTrackRoute = nav2_msgs::action::ComputeAndTrackRoute; + using ComputeAndTrackRouteGoal = ComputeAndTrackRoute::Goal; + using ComputeAndTrackRouteFeedback = ComputeAndTrackRoute::Feedback; + using ComputeAndTrackRouteResult = ComputeAndTrackRoute::Result; + using ComputeAndTrackRouteServer = nav2_util::SimpleActionServer; + + /** + * @brief A constructor for nav2_route::RouteServer + * @param options Additional options to control creation of the node. + */ + explicit RouteServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief A destructor for nav2_route::RouteServer + */ + ~RouteServer() = default; + +protected: + /** + * @brief Configure member variables and initializes planner + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Activate member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Deactivate member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Reset member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Called when in shutdown state + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Main route action server callbacks for computing and tracking a route + */ + void computeRoute(); + void computeAndTrackRoute(); + + /** + * @brief Abstract method combining finding the starting/ending nodes and the route planner + * to find the Node locations of interest and route to the goal + * @param goal The request goal information + * @param blocked_ids The IDs of blocked graphs / edges + * @param updated_start_id The nodeID of an updated starting point when tracking + * a route that corresponds to the next point to route to from to continue progress + * @return A route of the request + */ + template + Route findRoute( + const std::shared_ptr goal, + ReroutingState & rerouting_info = ReroutingState()); + + /** + * @brief Main processing called by both action server callbacks to centralize + * the great deal of shared code between them + */ + template + void processRouteRequest( + std::shared_ptr> & action_server); + + /** + * @brief Find the planning duration of the request and log warnings + * @param start_time Start of planning time + * @return Duration of planning time + */ + rclcpp::Duration findPlanningDuration(const rclcpp::Time & start_time); + + /** + * @brief Find the routing request is valid (action server OK and not cancelled) + * @param action_server Actions server to check + * @return if the request is valid + */ + template + bool isRequestValid(std::shared_ptr> & action_server); + + /** + * @brief Populate result for compute route action + * @param result Result to populate + * @param route Route to use to populate result + * @param path Path to use to populate result + * @param planning_duration Time to create a valid route + */ + void populateActionResult( + std::shared_ptr result, + const Route & route, + const nav_msgs::msg::Path & path, + const rclcpp::Duration & planning_duration); + + /** + * @brief Populate result for compute and track route action + * @param result Result to populate + * @param route Route to use to populate result + * @param path Path to use to populate result + * @param planning_duration Time to create a valid route + */ + void populateActionResult( + std::shared_ptr/*result*/, + const Route & /*route*/, + const nav_msgs::msg::Path & /*path*/, + const rclcpp::Duration & /*planning_duration*/); + + /** + * @brief The service callback to set a new route graph + * @param request_header to the service + * @param request to the service + * @param response from the service + */ + void setRouteGraph( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Log exception warnings, templated by action message type + * @param goal Goal that failed + * @param exception Exception message + */ + template + void exceptionWarning(const std::shared_ptr goal, const std::exception & ex); + + std::shared_ptr compute_route_server_; + std::shared_ptr compute_and_track_route_server_; + + // TF + std::shared_ptr tf_; + std::shared_ptr transform_listener_; + + // Publish the route for visualization + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + graph_vis_publisher_; + + // Set or modify graph + nav2_util::ServiceServer>::SharedPtr set_graph_service_; + + // Internal tools + std::shared_ptr graph_loader_; + std::shared_ptr route_planner_; + std::shared_ptr route_tracker_; + std::shared_ptr path_converter_; + std::shared_ptr goal_intent_extractor_; + + std::shared_ptr costmap_subscriber_; + + // State Data + Graph graph_; + GraphToIDMap id_to_graph_map_; + std::string route_frame_, base_frame_; + double max_planning_time_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__ROUTE_SERVER_HPP_ diff --git a/nav2_route/include/nav2_route/route_tracker.hpp b/nav2_route/include/nav2_route/route_tracker.hpp new file mode 100644 index 00000000000..d4ccb66a51e --- /dev/null +++ b/nav2_route/include/nav2_route/route_tracker.hpp @@ -0,0 +1,134 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "tf2_ros/transform_listener.h" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/simple_action_server.hpp" +#include "nav2_msgs/action/compute_and_track_route.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/operations_manager.hpp" + +#ifndef NAV2_ROUTE__ROUTE_TRACKER_HPP_ +#define NAV2_ROUTE__ROUTE_TRACKER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::RouteTracker + * @brief Takes a processing route request and tracks it for progress + * in accordance with executing behavioral operations + */ +class RouteTracker +{ +public: + using ActionServerTrack = nav2_util::SimpleActionServer; + using Feedback = nav2_msgs::action::ComputeAndTrackRoute::Feedback; + + /** + * @brief A constructor for nav2_route::RouteTracker + */ + RouteTracker() = default; + + /** + * @brief A constructor for nav2_route::RouteTracker + */ + ~RouteTracker() = default; + + /** + * @brief Configure route tracker + * @param node Node to grab info from + * @param route_frame Frame of route navigation + */ + void configure( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + std::shared_ptr action_server, + const std::string & route_frame, + const std::string & base_frame); + + /** + * @brief Determine if a node is to be considered achieved at the current position + * @param pose Current robot pose to query + * @param state Tracker state + * @param route Route to check against + * @return bool if node is achieved + */ + bool nodeAchieved( + const geometry_msgs::msg::PoseStamped & pose, + RouteTrackingState & state, + const Route & route); + + /** + * @brief Determine if a node is the start or last node in the route + * @param idx Idx of the current edge being tracked + * @param route Route to check + * @return bool if this node is the last node + */ + bool isStartOrEndNode(RouteTrackingState & state, const Route & route); + + /** + * @brief Get the current robot's base_frame pose in route_frame + * @return Robot pose + */ + geometry_msgs::msg::PoseStamped getRobotPose(); + + /** + * @brief A utility to publish feedback for the action on important changes + * @param rerouted If the route has been rerouted + * @param next_node_id Id of the next node the route is to pass + * @param last_node_id Id of the last node the route passed + * @param edge_id Id of the current edge being processed + * @param operations A set of operations which were performed this iteration + */ + void publishFeedback( + const bool rereouted, + const unsigned int next_node_id, + const unsigned int last_node_id, + const unsigned int edge_id, + const std::vector & operations); + + /** + * @brief Main function to track route, manage state, and trigger operations + * @param route Route to track progress of + * @param path Path that comprises this route for publication of feedback messages + * @param blocked_ids A set of blocked IDs to modify if rerouting is necessary + * @return TrackerResult if the route is completed, should be rerouted, or was interrupted + */ + TrackerResult trackRoute( + const Route & route, const nav_msgs::msg::Path & path, + ReroutingState & rerouting_info); + +protected: + nav2_msgs::msg::Route route_msg_; + nav_msgs::msg::Path path_; + std::string route_frame_, base_frame_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("RouteTracker")}; + double radius_threshold_, boundary_radius_threshold_, tracker_update_rate_; + bool aggregate_blocked_ids_; + std::shared_ptr action_server_; + std::unique_ptr operations_manager_; + std::shared_ptr tf_buffer_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__ROUTE_TRACKER_HPP_ diff --git a/nav2_route/include/nav2_route/types.hpp b/nav2_route/include/nav2_route/types.hpp new file mode 100644 index 00000000000..dc31c5e01da --- /dev/null +++ b/nav2_route/include/nav2_route/types.hpp @@ -0,0 +1,304 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef NAV2_ROUTE__TYPES_HPP_ +#define NAV2_ROUTE__TYPES_HPP_ + +namespace nav2_route +{ + +/** + * @struct nav2_route::Metadata + * @brief An object to store arbitrary metadata regarding nodes from the graph file + */ +struct Metadata +{ + Metadata() {} + + // For retrieving metadata at run-time via plugins + template + T getValue(const std::string & key, T & default_val) const + { + auto it = data.find(key); + if (it == data.end()) { + return default_val; + } + return std::any_cast(it->second); + } + + // For populating metadata from file + template + void setValue(const std::string & key, T & value) + { + data[key] = value; + } + + std::unordered_map data; +}; + +struct Node; +typedef Node * NodePtr; +typedef std::vector NodeVector; +typedef NodeVector Graph; +typedef std::unordered_map GraphToIDMap; +typedef std::unordered_map> GraphToIncomingEdgesMap; +typedef std::vector NodePtrVector; +typedef std::pair NodeElement; +typedef std::pair NodeExtents; + +/** + * @struct nav2_route::NodeComparator + * @brief Node comparison for priority queue sorting + */ +struct NodeComparator +{ + bool operator()(const NodeElement & a, const NodeElement & b) const + { + return a.first > b.first; + } +}; + +typedef std::priority_queue, NodeComparator> NodeQueue; + +/** + * @struct nav2_route::EdgeCost + * @brief An object to store edge cost or cost metadata for scoring + */ +struct EdgeCost +{ + float cost{0.0}; + bool overridable{true}; // If overridable, may use plugin edge cost scorers +}; + +/** + * @enum nav2_route::OperationTrigger + * @brief The triggering events for an operation + */ +enum class OperationTrigger +{ + NODE = 0, + ON_ENTER = 1, + ON_EXIT = 2 +}; + +/** + * @struct nav2_route::Operation + * @brief An object to store operations to perform on events with types and metadata + */ +struct Operation +{ + std::string type; + OperationTrigger trigger; + Metadata metadata; +}; + +typedef std::vector Operations; +typedef std::vector OperationPtrs; + +/** + * @struct nav2_route::OperationsResult + * @brief Result information from the operations manager + */ +struct OperationsResult +{ + std::vector operations_triggered; + bool reroute{false}; + std::vector blocked_ids; +}; + +/** + * @struct nav2_route::DirectionalEdge + * @brief An object representing edges between nodes + */ +struct DirectionalEdge +{ + unsigned int edgeid; // Edge identifier + NodePtr start{nullptr}; // Ptr to starting node of edge + NodePtr end{nullptr}; // Ptr to ending node of edge + EdgeCost edge_cost; // Cost information associated with edge + Metadata metadata; // Any metadata stored in the graph file of interest + Operations operations; // Operations to perform related to the edge +}; + +typedef DirectionalEdge * EdgePtr; +typedef std::vector EdgeVector; +typedef std::vector EdgePtrVector; + +/** + * @struct nav2_route::SearchState + * @brief An object to store state related to graph searching of nodes + * This is an internal class users should not modify. + */ +struct SearchState +{ + EdgePtr parent_edge{nullptr}; + float integrated_cost{std::numeric_limits::max()}; + float traversal_cost{std::numeric_limits::max()}; + + void reset() + { + integrated_cost = std::numeric_limits::max(); + traversal_cost = std::numeric_limits::max(); + parent_edge = nullptr; + } +}; + +/** + * @struct nav2_route::Coordinates + * @brief An object to store Node coordinates in different frames + */ +struct Coordinates +{ + std::string frame_id{"map"}; + float x{0.0}, y{0.0}; +}; + +/** + * @struct nav2_route::Node + * @brief An object to store the nodes in the graph file + */ +struct Node +{ + unsigned int nodeid; // Node identifier + Coordinates coords; // Coordinates of node + EdgeVector neighbors; // Directed neighbors and edges of the node + Metadata metadata; // Any metadata stored in the graph file of interest + Operations operations; // Operations to perform related to the node + SearchState search_state; // State maintained by route search algorithm + + void addEdge( + EdgeCost & cost, NodePtr node, unsigned int edgeid, Metadata meta_data = {}, + Operations operations_data = {}) + { + neighbors.push_back({edgeid, this, node, cost, meta_data, operations_data}); + } +}; + +/** + * @struct nav2_route::Route + * @brief An ordered set of nodes and edges corresponding to the planned route + */ +struct Route +{ + NodePtr start_node; + EdgePtrVector edges; + float route_cost{0.0}; +}; + +/** + * @struct nav2_route::RouteRequest + * @brief An object to store salient features of the route request including its start + * and goal node ids, start and goal pose, and a flag to indicate if the start and goal + * poses are relevant + */ +struct RouteRequest +{ + unsigned int start_nodeid; // node id of start node + unsigned int goal_nodeid; // node id of goal node + geometry_msgs::msg::PoseStamped start_pose; // pose of start + geometry_msgs::msg::PoseStamped goal_pose; // pose of goal + bool use_poses; // whether the start and goal poses are used +}; + +/** + * @enum nav2_route::TrackerResult + * @brief Return result of the route tracker to the main server for processing + */ +enum class TrackerResult +{ + EXITED = 0, + INTERRUPTED = 1, + COMPLETED = 2 +}; + +/** + * @struct nav2_route::RouteTrackingState + * @brief Current state management of route tracking class + */ +struct RouteTrackingState +{ + NodePtr last_node{nullptr}, next_node{nullptr}; + EdgePtr current_edge{nullptr}; + int route_edges_idx{-1}; + bool within_radius{false}; +}; + +/** + * @struct nav2_route::ReroutingState + * @brief State shared to objects to communicate important rerouting data + * to avoid rerouting over blocked edges, ensure reroute from the current + * appropriate starting point along the route, and state of edges if pruned + * for seeding the Tracker's state. Admittedly, this is a bit complex, so more + * context is provided inline. + */ +struct ReroutingState +{ + // Communicate edges identified as blocked by the operational plugins like collision checkers. + // This is fully managed by the route tracker when populated. + std::vector blocked_ids; + + // Used to determine if this is the first planning iteration in the goal intent extractor + // to bypass pruning. Fully managed in the goal intent extractor. + bool first_time{true}; + + // Used to mark current edge being tracked by the route, if progress was made before rerouting. + // It is reset in the goal intent extractor if the previous progressed edge is different from + // the new edge from planning. Otherwise, used in the path converter to create new dense path + // with partial progress information and in the tracker to seed the state to continue. + // It is managed by both the goal intent extractor and the route tracker. + EdgePtr curr_edge{nullptr}; + Coordinates closest_pt_on_edge; + + // Used to mark the route tracking state before rerouting was requested. + // When route tracking made some progress, the Start ID and pose are populated + // and used by the goal intent extractor to override the initial request's + // start, current pose along the edge, and pruning criteria. Otherwise, the initial request + // information is used. This is managed by the route tracker but used by goal intent extractor. + unsigned int rerouting_start_id{std::numeric_limits::max()}; + geometry_msgs::msg::PoseStamped rerouting_start_pose; + + void reset() + { + rerouting_start_id = std::numeric_limits::max(); + blocked_ids.clear(); + first_time = true; + curr_edge = nullptr; + closest_pt_on_edge = Coordinates(); + rerouting_start_pose = geometry_msgs::msg::PoseStamped(); + } +}; + +/** + * @enum nav2_route::EdgeType + * @brief An enum class describing what type of edge connecting two nodes is + */ +enum class EdgeType +{ + NONE = 0, + START = 1, + END = 2 +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__TYPES_HPP_ diff --git a/nav2_route/include/nav2_route/utils.hpp b/nav2_route/include/nav2_route/utils.hpp new file mode 100644 index 00000000000..9ccf65eb41e --- /dev/null +++ b/nav2_route/include/nav2_route/utils.hpp @@ -0,0 +1,267 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include "std_msgs/msg/color_rgba.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "nav2_route/types.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/line_iterator.hpp" + +#ifndef NAV2_ROUTE__UTILS_HPP_ +#define NAV2_ROUTE__UTILS_HPP_ + +namespace nav2_route +{ + +namespace utils +{ + +/** + * @brief Convert the position into a pose + * @param x X Coordinates + * @param y Y Coordinates + * @return PoseStamped of the position + */ +inline geometry_msgs::msg::PoseStamped toMsg(const float x, const float y) +{ + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = x; + pose.pose.position.y = y; + return pose; +} + +/** + * @brief Convert the route graph into a visualization marker array for visualization + * @param graph Graph of nodes and edges + * @param frame Frame ID to use + * @param now Current time to use + * @return MarkerArray of the graph + */ +inline visualization_msgs::msg::MarkerArray toMsg( + const nav2_route::Graph & graph, const std::string & frame, const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray msg; + visualization_msgs::msg::Marker curr_marker; + curr_marker.header.frame_id = frame; + curr_marker.header.stamp = now; + curr_marker.action = 0; + + auto getSphereSize = []() { + geometry_msgs::msg::Vector3 v_msg; + v_msg.x = 0.05; + v_msg.y = 0.05; + v_msg.z = 0.05; + return v_msg; + }; + + auto getSphereColor = []() { + std_msgs::msg::ColorRGBA c_msg; + c_msg.r = 1.0; + c_msg.g = 0.0; + c_msg.b = 0.0; + c_msg.a = 1.0; + return c_msg; + }; + + auto getLineColor = []() { + std_msgs::msg::ColorRGBA c_msg; + c_msg.r = 0.0; + c_msg.g = 1.0; + c_msg.b = 0.0; + c_msg.a = 0.5; // So bi-directional connections stand out overlapping + return c_msg; + }; + + unsigned int marker_idx = 1; + for (unsigned int i = 0; i != graph.size(); i++) { + if (graph[i].nodeid == std::numeric_limits::max()) { + continue; // Skip "deleted" nodes + } + curr_marker.ns = "route_graph"; + curr_marker.id = marker_idx++; + curr_marker.type = visualization_msgs::msg::Marker::SPHERE; + curr_marker.pose.position.x = graph[i].coords.x; + curr_marker.pose.position.y = graph[i].coords.y; + curr_marker.scale = getSphereSize(); + curr_marker.color = getSphereColor(); + msg.markers.push_back(curr_marker); + + // Add text + curr_marker.ns = "route_graph_ids"; + curr_marker.id = marker_idx++; + curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + curr_marker.pose.position.x = graph[i].coords.x + 0.07; + curr_marker.pose.position.y = graph[i].coords.y; + curr_marker.text = std::to_string(graph[i].nodeid); + curr_marker.scale.z = 0.1; + msg.markers.push_back(curr_marker); + + for (unsigned int j = 0; j != graph[i].neighbors.size(); j++) { + curr_marker.ns = "route_graph"; + curr_marker.id = marker_idx++; + curr_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + curr_marker.pose.position.x = 0; // Set to 0 since points are relative to this frame + curr_marker.pose.position.y = 0; // Set to 0 since points are relative to this frame + curr_marker.points.resize(2); + curr_marker.points[0].x = graph[i].coords.x; + curr_marker.points[0].y = graph[i].coords.y; + curr_marker.points[1].x = graph[i].neighbors[j].end->coords.x; + curr_marker.points[1].y = graph[i].neighbors[j].end->coords.y; + curr_marker.scale.x = 0.03; + curr_marker.color = getLineColor(); + msg.markers.push_back(curr_marker); + curr_marker.points.clear(); // Reset for next node marker + + // Add text + curr_marker.ns = "route_graph_ids"; + curr_marker.id = marker_idx++; + curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + curr_marker.pose.position.x = + graph[i].coords.x + ((graph[i].neighbors[j].end->coords.x - graph[i].coords.x) / 2.0) + + 0.07; + + // Deal with overlapping bi-directional text markers by offsetting locations + float y_offset = 0.0; + if (graph[i].nodeid > graph[i].neighbors[j].end->nodeid) { + y_offset = 0.05; + } else { + y_offset = -0.05; + } + + curr_marker.pose.position.y = + graph[i].coords.y + ((graph[i].neighbors[j].end->coords.y - graph[i].coords.y) / 2.0) + + y_offset; + curr_marker.text = std::to_string(graph[i].neighbors[j].edgeid); + curr_marker.scale.z = 0.1; + msg.markers.push_back(curr_marker); + } + } + + return msg; +} + +/** + * @brief Convert the route into a message + * @param route Route of nodes and edges + * @param frame Frame ID to use + * @param now Current time to use + * @return Route message + */ +inline nav2_msgs::msg::Route toMsg( + const nav2_route::Route & route, const std::string & frame, const rclcpp::Time & now) +{ + nav2_msgs::msg::Route msg; + msg.header.frame_id = frame; + msg.header.stamp = now; + msg.route_cost = route.route_cost; + + nav2_msgs::msg::RouteNode route_node; + nav2_msgs::msg::RouteEdge route_edge; + route_node.nodeid = route.start_node->nodeid; + route_node.position.x = route.start_node->coords.x; + route_node.position.y = route.start_node->coords.y; + msg.nodes.push_back(route_node); + + // Provide the Node info and Edge IDs we're traversing through + for (unsigned int i = 0; i != route.edges.size(); i++) { + route_edge.edgeid = route.edges[i]->edgeid; + route_edge.start.x = route.edges[i]->start->coords.x; + route_edge.start.y = route.edges[i]->start->coords.y; + route_edge.end.x = route.edges[i]->end->coords.x; + route_edge.end.y = route.edges[i]->end->coords.y; + msg.edges.push_back(route_edge); + + route_node.nodeid = route.edges[i]->end->nodeid; + route_node.position.x = route.edges[i]->end->coords.x; + route_node.position.y = route.edges[i]->end->coords.y; + msg.nodes.push_back(route_node); + } + + return msg; +} + +/** + * @brief Finds the normalized dot product of 2 vectors + * @param v1x Vector 1's x component + * @param v1y Vector 1's y component + * @param v2x Vector 2's x component + * @param v2y Vector 2's y component + * @return Value of dot product + */ +inline float normalizedDot( + const float v1x, const float v1y, + const float v2x, const float v2y) +{ + const float mag1 = std::hypotf(v1x, v1y); + const float mag2 = std::hypotf(v2x, v2y); + if (mag1 < 1e-6 || mag2 < 1e-6) { + return 0.0; + } + return (v1x / mag1) * (v2x / mag2) + (v1y / mag1) * (v2y / mag2); +} + +/** + * @brief Finds the closest point on the line segment made up of start-end to pose + * @param pose Pose to find point closest on the line with respect to + * @param start Start of line segment + * @param end End of line segment + * @return Coordinates of point on the line closest to the pose + */ +inline Coordinates findClosestPoint( + const geometry_msgs::msg::PoseStamped & pose, + const Coordinates & start, const Coordinates & end) +{ + Coordinates pt; + const float vx = end.x - start.x; + const float vy = end.y - start.y; + const float ux = start.x - pose.pose.position.x; + const float uy = start.y - pose.pose.position.y; + const float uv = vx * ux + vy * uy; + const float vv = vx * vx + vy * vy; + + // They are the same point, so only one option + if (vv < 1e-6) { + return start; + } + + const float t = -uv / vv; + if (t > 0.0 && t < 1.0) { + pt.x = (1.0 - t) * start.x + t * end.x; + pt.y = (1.0 - t) * start.y + t * end.y; + } else if (t <= 0.0) { + pt = start; + } else { + pt = end; + } + + return pt; +} + +inline float distance(const Coordinates & coords, const geometry_msgs::msg::PoseStamped & pose) +{ + return hypotf(coords.x - pose.pose.position.x, coords.y - pose.pose.position.y); +} + +} // namespace utils + +} // namespace nav2_route + +#endif // NAV2_ROUTE__UTILS_HPP_ diff --git a/nav2_route/media/architecture.png b/nav2_route/media/architecture.png new file mode 100644 index 00000000000..3bab56db5e6 Binary files /dev/null and b/nav2_route/media/architecture.png differ diff --git a/nav2_route/package.xml b/nav2_route/package.xml new file mode 100644 index 00000000000..d33cb7e942b --- /dev/null +++ b/nav2_route/package.xml @@ -0,0 +1,40 @@ + + + + nav2_route + 1.1.0 + A Route Graph planner to compliment the Planner Server + Steve Macenski + Apache-2.0 + + ament_cmake + nav2_common + + rclcpp + rclcpp_lifecycle + std_msgs + geometry_msgs + nav2_costmap_2d + pluginlib + visualization_msgs + nav2_msgs + nav_msgs + tf2_ros + nav2_util + nav2_core + angles + libnanoflann-dev + nlohmann-json-dev + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest + launch + launch_testing + + + ament_cmake + + + diff --git a/nav2_route/plugins.xml b/nav2_route/plugins.xml new file mode 100644 index 00000000000..89d6b745c3b --- /dev/null +++ b/nav2_route/plugins.xml @@ -0,0 +1,80 @@ + + + + Cost function for penalizing edge distance proportionally to potential speed limits + + + + + Cost function for rejecting edge that are closed due to non-traversability or setting remote dynamic costs (as opposed to static costs in the metadata from the Penalty Scorer) + + + + + Cost function for adding a cost based on metadata stored in the navigation graph + + + + + Cost function for adding a cost based on the costmap values in the edge + + + + + Cost function for adding costs to edges based on arbitrary semantic data + + + + + Cost function for adding costs to edges time to complete the edge based on previous runs and/or estimation with absolute speed limits. + + + + + Cost function for checking if the orientation of route matches orientation of the goal + + + + + Cost function for checking if the orientation of robot matches with the orientation of the edge + + + + + + Route tracking operation to adjust speed limit based on graph metadata + + + + + Exposes a route operation to replan upon external service request + + + + + Route operation templated by service types to call arbitrary services on node or edge events + + + + + + Parse the geojson graph file into the graph data type + + + + + Save a route graph to a geojson graph file + + + + + + Route operation to periodically check a costmap for localized viability of the route + + + + + Route operation to add actual times of traversal to edge's metadata for use in later more accurate planning by the TimeScorer + + + diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp new file mode 100644 index 00000000000..d98ddf46973 --- /dev/null +++ b/nav2_route/src/edge_scorer.cpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include + +#include "nav2_route/edge_scorer.hpp" + +namespace nav2_route +{ + +EdgeScorer::EdgeScorer( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::shared_ptr costmap_subscriber) +: plugin_loader_("nav2_route", "nav2_route::EdgeCostFunction") +{ + // load plugins with a default of the DistanceScorer + const std::vector default_plugin_ids({"DistanceScorer", "DynamicEdgesScorer"}); + const std::vector default_plugin_types( + {"nav2_route::DistanceScorer", "nav2_route::DynamicEdgesScorer"}); + + nav2_util::declare_parameter_if_not_declared( + node, "edge_cost_functions", rclcpp::ParameterValue(default_plugin_ids)); + auto edge_cost_function_ids = node->get_parameter("edge_cost_functions").as_string_array(); + + if (edge_cost_function_ids == default_plugin_ids) { + for (unsigned int i = 0; i != edge_cost_function_ids.size(); i++) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_ids[i] + ".plugin", rclcpp::ParameterValue(default_plugin_types[i])); + } + } + + for (size_t i = 0; i != edge_cost_function_ids.size(); i++) { + try { + std::string type = nav2_util::get_plugin_type_param( + node, edge_cost_function_ids[i]); + EdgeCostFunction::Ptr scorer = plugin_loader_.createUniqueInstance(type); + RCLCPP_INFO( + node->get_logger(), "Created edge cost function plugin %s of type %s", + edge_cost_function_ids[i].c_str(), type.c_str()); + scorer->configure(node, tf_buffer, costmap_subscriber, edge_cost_function_ids[i]); + plugins_.push_back(std::move(scorer)); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + node->get_logger(), + "Failed to create edge cost function. Exception: %s", ex.what()); + throw ex; + } + } +} + +bool EdgeScorer::score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & total_score) +{ + total_score = 0.0; + float curr_score = 0.0; + + for (auto & plugin : plugins_) { + plugin->prepare(); + } + + for (auto & plugin : plugins_) { + curr_score = 0.0; + if (plugin->score(edge, route_request, edge_type, curr_score)) { + total_score += curr_score; + } else { + return false; + } + } + + return true; +} + +int EdgeScorer::numPlugins() const +{ + return plugins_.size(); +} + +} // namespace nav2_route diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp new file mode 100644 index 00000000000..210e8911975 --- /dev/null +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -0,0 +1,315 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_route/goal_intent_extractor.hpp" + +namespace nav2_route +{ + +static float EPSILON = 1e-6; + +void GoalIntentExtractor::configure( + nav2_util::LifecycleNode::SharedPtr node, + Graph & graph, + GraphToIDMap * id_to_graph_map, + std::shared_ptr tf, + std::shared_ptr costmap_subscriber, + const std::string & route_frame, + const std::string & base_frame) +{ + logger_ = node->get_logger(); + id_to_graph_map_ = id_to_graph_map; + graph_ = &graph; + tf_ = tf; + costmap_subscriber_ = costmap_subscriber; + route_frame_ = route_frame; + base_frame_ = base_frame; + node_spatial_tree_ = std::make_shared(); + node_spatial_tree_->computeTree(graph); + + nav2_util::declare_parameter_if_not_declared( + node, "prune_goal", rclcpp::ParameterValue(true)); + prune_goal_ = node->get_parameter("prune_goal").as_bool(); + + nav2_util::declare_parameter_if_not_declared( + node, "max_prune_dist_from_edge", rclcpp::ParameterValue(8.0)); + max_dist_from_edge_ = static_cast( + node->get_parameter("max_prune_dist_from_edge").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "min_prune_dist_from_goal", rclcpp::ParameterValue(0.15)); + min_dist_from_goal_ = static_cast( + node->get_parameter("min_prune_dist_from_goal").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "min_prune_dist_from_start", rclcpp::ParameterValue(0.10)); + min_dist_from_start_ = static_cast( + node->get_parameter("min_prune_dist_from_start").as_double()); + + nav2_util::declare_parameter_if_not_declared( + node, "enable_nn_search", rclcpp::ParameterValue(true)); + enable_search_ = node->get_parameter("enable_nn_search").as_bool(); + nav2_util::declare_parameter_if_not_declared( + node, "max_nn_search_iterations", rclcpp::ParameterValue(10000)); + max_nn_search_iterations_ = node->get_parameter("max_nn_search_iterations").as_int(); + + nav2_util::declare_parameter_if_not_declared( + node, "num_nearest_nodes", rclcpp::ParameterValue(5)); + int num_of_nearest_nodes = node->get_parameter("num_nearest_nodes").as_int(); + node_spatial_tree_->setNumOfNearestNodes(num_of_nearest_nodes); +} + +void GoalIntentExtractor::setGraph(Graph & graph, GraphToIDMap * id_to_graph_map) +{ + id_to_graph_map_ = id_to_graph_map; + graph_ = &graph; + node_spatial_tree_->computeTree(graph); +} + +geometry_msgs::msg::PoseStamped GoalIntentExtractor::transformPose( + geometry_msgs::msg::PoseStamped & pose, + const std::string & target_frame) +{ + if (pose.header.frame_id != target_frame) { + RCLCPP_INFO( + logger_, + "Request pose in %s frame. Converting to route server frame: %s.", + pose.header.frame_id.c_str(), target_frame.c_str()); + if (!nav2_util::transformPoseInTargetFrame(pose, pose, *tf_, target_frame)) { + throw nav2_core::RouteTFError("Failed to transform starting pose to: " + target_frame); + } + } + return pose; +} + +void GoalIntentExtractor::overrideStart(const geometry_msgs::msg::PoseStamped & start_pose) +{ + // Override the start pose when rerouting is requested, using the current pose + start_ = start_pose; +} + +template +NodeExtents +GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) +{ + // If not using the poses, then use the requests Node IDs to establish start and goal + if (!goal->use_poses) { + unsigned int start_idx = id_to_graph_map_->at(goal->start_id); + unsigned int goal_idx = id_to_graph_map_->at(goal->goal_id); + const Coordinates & start_coords = graph_->at(start_idx).coords; + const Coordinates & goal_coords = graph_->at(goal_idx).coords; + start_.pose.position.x = start_coords.x; + start_.pose.position.y = start_coords.y; + goal_.pose.position.x = goal_coords.x; + goal_.pose.position.y = goal_coords.y; + return {start_idx, goal_idx}; + } + + // Find request start pose + geometry_msgs::msg::PoseStamped start_pose, goal_pose = goal->goal; + if (goal->use_start) { + start_pose = goal->start; + } else { + if (!nav2_util::getCurrentPose(start_pose, *tf_, route_frame_, base_frame_)) { + throw nav2_core::RouteTFError("Failed to obtain starting pose in: " + route_frame_); + } + } + + // transform to route_frame + start_ = transformPose(start_pose, route_frame_); + goal_ = transformPose(goal_pose, route_frame_); + + // Find closest route graph nodes to start and goal to plan between. + // Note that these are the location indices in the graph + std::vector start_route, end_route; + if (!node_spatial_tree_->findNearestGraphNodesToPose(start_, start_route) || + !node_spatial_tree_->findNearestGraphNodesToPose(goal_, end_route)) + { + throw nav2_core::IndeterminantNodesOnGraph( + "Could not determine node closest to start or goal pose requested!"); + } + + unsigned int start_route_loc = start_route.front(); + unsigned int end_route_loc = end_route.front(); + + // If given cost information, check which of the nearest graph nodes is nearest by + // traversability, not just Euclidean distance, in case of obstacles, walls, etc. + // However, if the closest node has Line of Sight to the goal, then use that node + // skipping the search as we know it is the closest and now optimally traversible node. + std::shared_ptr costmap = nullptr; + std::string costmap_frame_id; + bool enable_search = enable_search_; + if (enable_search) { + try { + costmap = costmap_subscriber_->getCostmap(); + costmap_frame_id = costmap_subscriber_->getFrameID(); + } catch (const std::exception & ex) { + enable_search = false; + RCLCPP_WARN( + logger_, + "Failed to get costmap for goal intent extractor: %s. " + "Falling back to closest euclidean route node instead.", ex.what()); + } + } + + if (enable_search && start_route.size() > 1u) { + // Convert the nearest node candidates to the costmap frame for search + std::vector candidate_nodes; + candidate_nodes.reserve(start_route.size()); + for (const auto & node : start_route) { + auto & node_data = graph_->at(node); + geometry_msgs::msg::PoseStamped node_pose; + node_pose.pose.position.x = node_data.coords.x; + node_pose.pose.position.y = node_data.coords.y; + node_pose.header.frame_id = node_data.coords.frame_id; + candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id)); + } + + auto transformed_start = transformPose(start_, costmap_frame_id); + GoalIntentSearch::LoSCollisionChecker los_checker(costmap); + if (los_checker.worldToMap( + candidate_nodes.front().pose.position, transformed_start.pose.position)) + { + if (los_checker.isInCollision()) { + GoalIntentSearch::BreadthFirstSearch bfs(costmap); + if (bfs.search(transformed_start, candidate_nodes, max_nn_search_iterations_)) { + start_route_loc = start_route[bfs.getClosestNodeIdx()]; + } + } + } + } + + if (enable_search && end_route.size() > 1u) { + // Convert the nearest node candidates to the costmap frame for search + std::vector candidate_nodes; + candidate_nodes.reserve(end_route.size()); + for (const auto & node : end_route) { + auto & node_data = graph_->at(node); + geometry_msgs::msg::PoseStamped node_pose; + node_pose.pose.position.x = node_data.coords.x; + node_pose.pose.position.y = node_data.coords.y; + node_pose.header.frame_id = node_data.coords.frame_id; + candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id)); + } + + auto transformed_end = transformPose(goal_, costmap_frame_id); + GoalIntentSearch::LoSCollisionChecker los_checker(costmap); + if (los_checker.worldToMap( + candidate_nodes.front().pose.position, transformed_end.pose.position)) + { + if (los_checker.isInCollision()) { + GoalIntentSearch::BreadthFirstSearch bfs(costmap); + if (bfs.search(transformed_end, candidate_nodes)) { + end_route_loc = end_route[bfs.getClosestNodeIdx()]; + } + } + } + } + + return {start_route_loc, end_route_loc}; +} + +template +Route GoalIntentExtractor::pruneStartandGoal( + const Route & input_route, + const std::shared_ptr goal, + ReroutingState & rerouting_info) +{ + Route pruned_route = input_route; + + // Grab and update the rerouting state + EdgePtr last_curr_edge = rerouting_info.curr_edge; + rerouting_info.curr_edge = nullptr; + bool first_time = rerouting_info.first_time; + rerouting_info.first_time = false; + + // Cannot prune if no edges to prune or if using nodeIDs in the initial request (no effect) + if (input_route.edges.empty() || (!goal->use_poses && first_time)) { + return pruned_route; + } + + // Check on pruning the start node + NodePtr first = pruned_route.start_node; + NodePtr next = pruned_route.edges[0]->end; + float vrx = next->coords.x - first->coords.x; + float vry = next->coords.y - first->coords.y; + float vpx = start_.pose.position.x - first->coords.x; + float vpy = start_.pose.position.y - first->coords.y; + float dot_prod = utils::normalizedDot(vrx, vry, vpx, vpy); + Coordinates closest_pt_on_edge = utils::findClosestPoint(start_, first->coords, next->coords); + if (dot_prod > EPSILON && // A projection exists + hypotf(vpx, vpy) > min_dist_from_start_ && // We're not on the node to prune entire edge + utils::distance(closest_pt_on_edge, start_) <= max_dist_from_edge_) // Close enough to edge + { + // Record the pruned edge information if its the same edge as previously routed so that + // the tracker can seed this information into its state to proceed with its task losslessly + if (last_curr_edge && last_curr_edge->edgeid == pruned_route.edges.front()->edgeid) { + rerouting_info.closest_pt_on_edge = closest_pt_on_edge; + rerouting_info.curr_edge = pruned_route.edges.front(); + } + + pruned_route.start_node = next; + pruned_route.route_cost -= pruned_route.edges.front()->end->search_state.traversal_cost; + pruned_route.edges.erase(pruned_route.edges.begin()); + } + + // Don't prune the goal if requested, if given a known goal_id (no effect), or now empty + if (!prune_goal_ || !goal->use_poses || pruned_route.edges.empty()) { + return pruned_route; + } + + // Check on pruning the goal node + next = pruned_route.edges.back()->start; + NodePtr last = pruned_route.edges.back()->end; + vrx = last->coords.x - next->coords.x; + vry = last->coords.y - next->coords.y; + vpx = goal_.pose.position.x - last->coords.x; + vpy = goal_.pose.position.y - last->coords.y; + + dot_prod = utils::normalizedDot(vrx, vry, vpx, vpy); + closest_pt_on_edge = utils::findClosestPoint(goal_, next->coords, last->coords); + if (dot_prod < -EPSILON && // A projection exists + hypotf(vpx, vpy) > min_dist_from_goal_ && // We're not on the node to prune entire edge + utils::distance(closest_pt_on_edge, goal_) <= max_dist_from_edge_) // Close enough to edge + { + pruned_route.route_cost -= pruned_route.edges.back()->end->search_state.traversal_cost; + pruned_route.edges.pop_back(); + } + + return pruned_route; +} + +geometry_msgs::msg::PoseStamped GoalIntentExtractor::getStart() +{ + return start_; +} + +template Route GoalIntentExtractor::pruneStartandGoal( + const Route & input_route, + const std::shared_ptr goal, + ReroutingState & rerouting_info); +template +Route GoalIntentExtractor::pruneStartandGoal( + const Route & input_route, + const std::shared_ptr goal, + ReroutingState & rerouting_info); +template NodeExtents GoalIntentExtractor::findStartandGoal( + const std::shared_ptr goal); +template +NodeExtents GoalIntentExtractor::findStartandGoal( + const std::shared_ptr goal); + +} // namespace nav2_route diff --git a/nav2_route/src/graph_loader.cpp b/nav2_route/src/graph_loader.cpp new file mode 100644 index 00000000000..268c04760af --- /dev/null +++ b/nav2_route/src/graph_loader.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_route/graph_loader.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +namespace nav2_route +{ + +GraphLoader::GraphLoader( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame) +: plugin_loader_("nav2_route", "nav2_route::GraphFileLoader"), + default_plugin_id_("GeoJsonGraphFileLoader") +{ + logger_ = node->get_logger(); + tf_ = tf; + route_frame_ = frame; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue(std::string(""))); + graph_filepath_ = node->get_parameter("graph_filepath").as_string(); + + // Default Graph Parser + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_loader", rclcpp::ParameterValue(default_plugin_id_)); + auto graph_file_loader_id = node->get_parameter("graph_file_loader").as_string(); + if (graph_file_loader_id == default_plugin_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_id_ + ".plugin", + rclcpp::ParameterValue("nav2_route::GeoJsonGraphFileLoader")); + } + + // Create graph file loader plugin + try { + plugin_type_ = nav2_util::get_plugin_type_param(node, graph_file_loader_id); + graph_file_loader_ = plugin_loader_.createSharedInstance((plugin_type_)); + RCLCPP_INFO( + logger_, "Created GraphFileLoader %s of type %s", + graph_file_loader_id.c_str(), plugin_type_.c_str()); + graph_file_loader_->configure(node); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + logger_, + "Failed to create GraphFileLoader. Exception: %s", ex.what()); + throw ex; + } +} + +bool GraphLoader::loadGraphFromFile( + Graph & graph, + GraphToIDMap & graph_to_id_map, + const std::string & filepath) +{ + if (filepath.empty()) { + RCLCPP_ERROR( + logger_, "The graph filepath was not provided."); + return false; + } + + RCLCPP_INFO( + logger_, + "Loading graph file from %s, by parser %s", filepath.c_str(), plugin_type_.c_str()); + + if (!graph_file_loader_->loadGraphFromFile(graph, graph_to_id_map, filepath)) { + return false; + } + + if (!transformGraph(graph)) { + RCLCPP_WARN( + logger_, + "Failed to transform nodes graph file (%s) to %s!", filepath.c_str(), route_frame_.c_str()); + return false; + } + + return true; +} + +bool GraphLoader::loadGraphFromParameter( + Graph & graph, + GraphToIDMap & graph_to_id_map) +{ + if (graph_filepath_.empty()) { + RCLCPP_INFO(logger_, "No graph file provided to load yet."); + return true; + } + + RCLCPP_INFO( + logger_, + "Loading graph file from %s, by parser %s", graph_filepath_.c_str(), plugin_type_.c_str()); + + if (!graph_file_loader_->loadGraphFromFile(graph, graph_to_id_map, graph_filepath_)) { + return false; + } + + if (!transformGraph(graph)) { + RCLCPP_WARN( + logger_, + "Failed to transform nodes graph file (%s) to %s!", + graph_filepath_.c_str(), route_frame_.c_str()); + return false; + } + + return true; +} + +bool GraphLoader::transformGraph(Graph & graph) +{ + std::unordered_map cached_transforms; + for (auto & node : graph) { + std::string node_frame = node.coords.frame_id; + if (node_frame.empty() || node_frame == route_frame_) { + continue; + } + + if (cached_transforms.find(node_frame) == cached_transforms.end()) { + tf2::Transform tf_transform; + bool got_transform = nav2_util::getTransform( + node_frame, route_frame_, tf2::durationFromSec(0.1), tf_, tf_transform); + + if (!got_transform) { + return false; + } + + cached_transforms.insert({node_frame, tf_transform}); + } + + tf2::Vector3 graph_coord( + node.coords.x, + node.coords.y, + 0.0); + + tf2::Vector3 new_coord = cached_transforms[node_frame] * graph_coord; + + node.coords.x = static_cast(new_coord.x()); + node.coords.y = static_cast(new_coord.y()); + node.coords.frame_id = route_frame_; + } + + return true; +} + +} // namespace nav2_route diff --git a/nav2_route/src/graph_saver.cpp b/nav2_route/src/graph_saver.cpp new file mode 100644 index 00000000000..be0386ce883 --- /dev/null +++ b/nav2_route/src/graph_saver.cpp @@ -0,0 +1,142 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_route/graph_saver.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +namespace nav2_route +{ + +GraphSaver::GraphSaver( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame) +: plugin_loader_("nav2_route", "nav2_route::GraphFileSaver"), + default_plugin_id_("GeoJsonGraphFileSaver") +{ + logger_ = node->get_logger(); + tf_ = tf; + route_frame_ = frame; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue(std::string(""))); + graph_filepath_ = node->get_parameter("graph_filepath").as_string(); + + // Default Graph Parser + const std::string default_plugin_type = "nav2_route::GeoJsonGraphFileSaver"; + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_saver", rclcpp::ParameterValue(default_plugin_id_)); + auto graph_file_saver_id = node->get_parameter("graph_file_saver").as_string(); + if (graph_file_saver_id == default_plugin_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_id_ + ".plugin", rclcpp::ParameterValue(default_plugin_type)); + } + + // Create graph file saver plugin + try { + plugin_type_ = nav2_util::get_plugin_type_param(node, graph_file_saver_id); + graph_file_saver_ = plugin_loader_.createSharedInstance((plugin_type_)); + RCLCPP_INFO( + logger_, "Created GraphFileSaver %s of type %s", + graph_file_saver_id.c_str(), plugin_type_.c_str()); + graph_file_saver_->configure(node); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + logger_, + "Failed to create GraphFileSaver. Exception: %s", ex.what()); + throw ex; + } +} + +bool GraphSaver::saveGraphToFile( + Graph & graph, + std::string filepath) +{ + if (filepath.empty() && !graph_filepath_.empty()) { + RCLCPP_DEBUG( + logger_, "The graph filepath was not provided. " + "Setting to %s", graph_filepath_.c_str()); + filepath = graph_filepath_; + } else if (filepath.empty() && graph_filepath_.empty()) { + // No graph to try to save + RCLCPP_WARN( + logger_, + "The graph filepath was not provided and no default was specified. " + "Failed to save the route graph."); + return false; + } + + RCLCPP_INFO( + logger_, + "Saving graph file from %s, by parser %s", filepath.c_str(), plugin_type_.c_str()); + + if (!graph_file_saver_->saveGraphToFile(graph, filepath)) { + return false; + } + + if (!transformGraph(graph)) { + RCLCPP_WARN( + logger_, + "Failed to transform nodes graph file (%s) to %s!", filepath.c_str(), route_frame_.c_str()); + return false; + } + + return true; +} + +bool GraphSaver::transformGraph(Graph & graph) +{ + std::unordered_map cached_transforms; + for (auto & node : graph) { + std::string node_frame = node.coords.frame_id; + if (node_frame.empty() || node_frame == route_frame_) { + // If there is no frame provided or the frame of the node is the same as the route graph + // then no transform is required + continue; + } + // Find the transform to convert node from node frame to route frame + if (cached_transforms.find(node_frame) == cached_transforms.end()) { + tf2::Transform tf_transform; + bool got_transform = nav2_util::getTransform( + node_frame, route_frame_, tf2::durationFromSec(0.1), tf_, tf_transform); + + if (!got_transform) { + RCLCPP_WARN( + logger_, + "Could not get transform from node frame %s to route frame %s", + node_frame.c_str(), route_frame_.c_str()); + return false; + } + + cached_transforms.insert({node_frame, tf_transform}); + } + // Apply the transform + tf2::Vector3 graph_coord( + node.coords.x, + node.coords.y, + 0.0); + + tf2::Vector3 new_coord = cached_transforms[node_frame] * graph_coord; + + node.coords.x = static_cast(new_coord.x()); + node.coords.y = static_cast(new_coord.y()); + node.coords.frame_id = route_frame_; + } + + return true; +} + +} // namespace nav2_route diff --git a/nav2_route/src/main.cpp b/nav2_route/src/main.cpp new file mode 100644 index 00000000000..ca98729213f --- /dev/null +++ b/nav2_route/src/main.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_route/route_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_route/src/node_spatial_tree.cpp b/nav2_route/src/node_spatial_tree.cpp new file mode 100644 index 00000000000..19f37ddacf1 --- /dev/null +++ b/nav2_route/src/node_spatial_tree.cpp @@ -0,0 +1,79 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_route/node_spatial_tree.hpp" + +namespace nav2_route +{ + +NodeSpatialTree::~NodeSpatialTree() +{ + if (kdtree_) { + delete kdtree_; + kdtree_ = nullptr; + } + + if (adaptor_) { + delete adaptor_; + adaptor_ = nullptr; + } +} + +void NodeSpatialTree::setNumOfNearestNodes(int num_of_nearest_nodes) +{ + num_of_nearest_nodes_ = num_of_nearest_nodes; +} + +void NodeSpatialTree::computeTree(Graph & graph) +{ + if (kdtree_) { + delete kdtree_; + kdtree_ = nullptr; + } + + if (adaptor_) { + delete adaptor_; + adaptor_ = nullptr; + } + + adaptor_ = new GraphAdaptor(graph); + kdtree_ = new kd_tree_t(DIMENSION, *adaptor_, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + kdtree_->buildIndex(); + graph_ = &graph; +} + +bool NodeSpatialTree::findNearestGraphNodesToPose( + const geometry_msgs::msg::PoseStamped & pose_in, std::vector & node_ids) +{ + size_t num_results = static_cast(num_of_nearest_nodes_); + std::vector ret_index(num_results); + std::vector out_dist_sqr(num_results); + const double query_pt[2] = {pose_in.pose.position.x, pose_in.pose.position.y}; + num_results = kdtree_->knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]); + + if (num_results == 0) { + return false; + } + + for (int i = 0; i < static_cast(ret_index.size()); ++i) { + node_ids.push_back(ret_index[i]); + } + return true; +} + +} // namespace nav2_route diff --git a/nav2_route/src/operations_manager.cpp b/nav2_route/src/operations_manager.cpp new file mode 100644 index 00000000000..ce883de929e --- /dev/null +++ b/nav2_route/src/operations_manager.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_route/operations_manager.hpp" + +namespace nav2_route +{ + +OperationsManager::OperationsManager( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber) +: plugin_loader_("nav2_route", "nav2_route::RouteOperation") +{ + logger_ = node->get_logger(); + + // Have some default operations + const std::vector default_plugin_ids( + {"AdjustSpeedLimit", "ReroutingService"}); + const std::vector default_plugin_types( + {"nav2_route::AdjustSpeedLimit", "nav2_route::ReroutingService"}); + + nav2_util::declare_parameter_if_not_declared( + node, "operations", rclcpp::ParameterValue(default_plugin_ids)); + auto operation_ids = node->get_parameter("operations").as_string_array(); + + if (operation_ids == default_plugin_ids) { + for (unsigned int i = 0; i != operation_ids.size(); i++) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_ids[i] + ".plugin", rclcpp::ParameterValue(default_plugin_types[i])); + } + } + + // Create plugins and sort them into On Query, Status Change, and Graph-calling Operations + for (size_t i = 0; i != operation_ids.size(); i++) { + try { + std::string type = nav2_util::get_plugin_type_param(node, operation_ids[i]); + RouteOperation::Ptr operation = plugin_loader_.createSharedInstance(type); + RCLCPP_INFO( + node->get_logger(), "Created route operation %s of type %s", + operation_ids[i].c_str(), type.c_str()); + operation->configure(node, costmap_subscriber, operation_ids[i]); + RouteOperationType process_type = operation->processType(); + if (process_type == RouteOperationType::ON_QUERY) { + query_operations_.push_back(std::move(operation)); + } else if (process_type == RouteOperationType::ON_STATUS_CHANGE) { + change_operations_.push_back(std::move(operation)); + } else { + graph_operations_[operation->getName()] = operation; + } + } catch (const pluginlib::PluginlibException & ex) { + throw ex; + } + } +} + +template +void OperationsManager::findGraphOperationsToProcess( + T & obj, const OperationTrigger & trigger, + OperationPtrs & operations) +{ + if (!obj) { + return; + } + + Operations & op_vec = obj->operations; + for (Operations::iterator it = op_vec.begin(); it != op_vec.end(); ++it) { + if (it->trigger == trigger) { + operations.push_back(&(*it)); + } + } +} + +OperationPtrs OperationsManager::findGraphOperations( + const NodePtr node, const EdgePtr edge_enter, const EdgePtr edge_exit) +{ + OperationPtrs ops; + findGraphOperationsToProcess(node, OperationTrigger::NODE, ops); + findGraphOperationsToProcess(edge_enter, OperationTrigger::ON_ENTER, ops); + findGraphOperationsToProcess(edge_exit, OperationTrigger::ON_EXIT, ops); + return ops; +} + +void OperationsManager::updateResult( + const std::string & name, const OperationResult & op_result, OperationsResult & result) +{ + result.reroute = result.reroute || op_result.reroute; + result.blocked_ids.insert( + result.blocked_ids.end(), op_result.blocked_ids.begin(), op_result.blocked_ids.end()); + result.operations_triggered.push_back(name); +} + +void OperationsManager::processOperationsPluginVector( + const std::vector & route_operations, + OperationsResult & result, + const NodePtr node, + const EdgePtr edge_entered, + const EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & pose) +{ + for (OperationsIter it = route_operations.begin(); it != route_operations.end(); ++it) { + const RouteOperation::Ptr & plugin = *it; + OperationResult op_result = plugin->perform(node, edge_entered, edge_exited, route, pose); + updateResult(plugin->getName(), op_result, result); + } +} + +OperationsResult OperationsManager::process( + const bool status_change, + const RouteTrackingState & state, + const Route & route, + const geometry_msgs::msg::PoseStamped & pose, + const ReroutingState & rerouting_info) +{ + // Get important state information + OperationsResult result; + NodePtr node = state.last_node; + EdgePtr edge_entered = state.current_edge; + EdgePtr edge_exited = + state.route_edges_idx > 0 ? route.edges[state.route_edges_idx - 1] : nullptr; + + // If we have rerouting_info.curr_edge, then after the first node is achieved, + // the robot is exiting the partial previous edge. + if (state.route_edges_idx == 0 && rerouting_info.curr_edge) { + edge_exited = rerouting_info.curr_edge; + } + + if (status_change) { + // Process operations defined in the navigation graph at node or edge + OperationPtrs operations = findGraphOperations(node, edge_entered, edge_exited); + for (unsigned int i = 0; i != operations.size(); i++) { + auto op = graph_operations_.find(operations[i]->type); + if (op != graph_operations_.end()) { + OperationResult op_result = op->second->perform( + node, edge_entered, edge_exited, route, pose, &operations[i]->metadata); + updateResult(op->second->getName(), op_result, result); + } else { + throw nav2_core::OperationFailed( + "Operation " + operations[i]->type + + " does not exist in route operations loaded!"); + } + } + + // Process operations which trigger on any status changes + processOperationsPluginVector( + change_operations_, result, node, edge_entered, edge_exited, route, pose); + } + + // Process operations which trigger regardless of status change or nodes / edges + processOperationsPluginVector( + query_operations_, result, node, edge_entered /*edge_curr*/, edge_exited, route, pose); + return result; +} + +} // namespace nav2_route diff --git a/nav2_route/src/path_converter.cpp b/nav2_route/src/path_converter.cpp new file mode 100644 index 00000000000..38adea20c25 --- /dev/null +++ b/nav2_route/src/path_converter.cpp @@ -0,0 +1,124 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "nav2_route/path_converter.hpp" + +namespace nav2_route +{ + +void PathConverter::configure(nav2_util::LifecycleNode::SharedPtr node) +{ + // Density to make path points + nav2_util::declare_parameter_if_not_declared( + node, "path_density", rclcpp::ParameterValue(0.05)); + density_ = static_cast(node->get_parameter("path_density").as_double()); + + path_pub_ = node->create_publisher("plan", 1); + path_pub_->on_activate(); +} + +nav_msgs::msg::Path PathConverter::densify( + const Route & route, + const ReroutingState & rerouting_info, + const std::string & frame, + const rclcpp::Time & now) +{ + nav_msgs::msg::Path path; + path.header.stamp = now; + path.header.frame_id = frame; + + // If we're rerouting and covering the same previous edge to start, + // the path should contain the relevant partial information along edge + // to avoid unnecessary free-space planning where state is retained + if (rerouting_info.curr_edge) { + const Coordinates & start = rerouting_info.closest_pt_on_edge; + const Coordinates & end = rerouting_info.curr_edge->end->coords; + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + } + + // Fill in path via route edges + for (unsigned int i = 0; i != route.edges.size(); i++) { + const EdgePtr edge = route.edges[i]; + const Coordinates & start = edge->start->coords; + const Coordinates & end = edge->end->coords; + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + } + + if (route.edges.empty()) { + path.poses.push_back(utils::toMsg(route.start_node->coords.x, route.start_node->coords.y)); + } else { + path.poses.push_back( + utils::toMsg(route.edges.back()->end->coords.x, route.edges.back()->end->coords.y)); + } + + // Set path poses orientations for each point + for (size_t i = 0; i < path.poses.size() - 1; ++i) { + const auto & pose = path.poses[i]; + const auto & next_pose = path.poses[i + 1]; + const double dx = next_pose.pose.position.x - pose.pose.position.x; + const double dy = next_pose.pose.position.y - pose.pose.position.y; + const double yaw = atan2(dy, dx); + path.poses[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + } + + // Set the last pose orientation to the last edge + if (!route.edges.empty()) { + const auto & last_edge = route.edges.back(); + const double dx = last_edge->end->coords.x - last_edge->start->coords.x; + const double dy = last_edge->end->coords.y - last_edge->start->coords.y; + path.poses.back().pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(atan2(dy, dx)); + } + + // publish path similar to planner server + path_pub_->publish(std::make_unique(path)); + + return path; +} + +void PathConverter::interpolateEdge( + float x0, float y0, float x1, float y1, + std::vector & poses) +{ + // Find number of points to populate by given density + const float mag = hypotf(x1 - x0, y1 - y0); + const unsigned int num_pts = ceil(mag / density_); + const float iterpolated_dist = mag / num_pts; + + // Find unit vector direction + float ux = (x1 - x0) / mag; + float uy = (y1 - y0) / mag; + + // March along it until dist + float x = x0; + float y = y0; + poses.push_back(utils::toMsg(x, y)); + + unsigned int pt_ctr = 0; + while (pt_ctr < num_pts - 1) { + x += ux * iterpolated_dist; + y += uy * iterpolated_dist; + pt_ctr++; + poses.push_back(utils::toMsg(x, y)); + } +} + +} // namespace nav2_route diff --git a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp new file mode 100644 index 00000000000..445f2572328 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp @@ -0,0 +1,152 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp" + +namespace nav2_route +{ + +void CostmapScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr costmap_subscriber, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring costmap scorer."); + name_ = name; + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + // Find whether to use average or maximum cost values + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".use_maximum", rclcpp::ParameterValue(true)); + use_max_ = static_cast(node->get_parameter(getName() + ".use_maximum").as_bool()); + + // Edge is invalid if its in collision + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".invalid_on_collision", rclcpp::ParameterValue(true)); + invalid_on_collision_ = + static_cast(node->get_parameter(getName() + ".invalid_on_collision").as_bool()); + + // Edge is invalid if edge is off the costmap + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".invalid_off_map", rclcpp::ParameterValue(true)); + invalid_off_map_ = + static_cast(node->get_parameter(getName() + ".invalid_off_map").as_bool()); + + // Max cost to be considered valid + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".max_cost", rclcpp::ParameterValue(253.0)); + max_cost_ = static_cast(node->get_parameter(getName() + ".max_cost").as_double()); + + // Resolution to check the costmap over (1=every cell, 2=every other cell, etc.) + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".check_resolution", rclcpp::ParameterValue(2)); + check_resolution_ = static_cast( + node->get_parameter(getName() + ".check_resolution").as_int()); + + // Create costmap subscriber if not the same as the server costmap + std::string server_costmap_topic = node->get_parameter("costmap_topic").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".costmap_topic", + rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); + std::string costmap_topic = + node->get_parameter(getName() + ".costmap_topic").as_string(); + if (costmap_topic != server_costmap_topic) { + costmap_subscriber_ = std::make_shared( + node, costmap_topic); + RCLCPP_INFO(node->get_logger(), + "Using costmap topic: %s instead of server costmap topic: %s for CostmapScorer.", + costmap_topic.c_str(), server_costmap_topic.c_str()); + } else { + costmap_subscriber_ = costmap_subscriber; + } + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); +} + +void CostmapScorer::prepare() +{ + try { + costmap_ = costmap_subscriber_->getCostmap(); + } catch (...) { + costmap_.reset(); + } +} + +bool CostmapScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + if (!costmap_) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "No costmap yet received!"); + return false; + } + + float largest_cost = 0.0, running_cost = 0.0, point_cost = 0.0; + unsigned int x0, y0, x1, y1, idx = 0; + if (!costmap_->worldToMap(edge->start->coords.x, edge->start->coords.y, x0, y0) || + !costmap_->worldToMap(edge->end->coords.x, edge->end->coords.y, x1, y1)) + { + if (invalid_off_map_) { + // Edge is invalid if it is off the costmap + return false; + } + return true; + } + + for (nav2_util::LineIterator iter(x0, y0, x1, y1); iter.isValid(); ) { + point_cost = static_cast(costmap_->getCost(iter.getX(), iter.getY())); + if (point_cost >= max_cost_ && max_cost_ != 255.0f /*Unknown*/ && invalid_on_collision_) { + // Edge is invalid if it is in collision or higher than max allowed cost + return false; + } + + idx++; + running_cost += point_cost; + if (largest_cost < point_cost && point_cost != 255.0) { + largest_cost = point_cost; + } + + // Advance the iterator by the check resolution on the edge, pruning to a coarse resolution + for (unsigned int i = 0; i < check_resolution_; i++) { + iter.advance(); + } + } + + if (use_max_) { + cost = weight_ * largest_cost / max_cost_; + } else { + cost = weight_ * running_cost / (static_cast(idx) * max_cost_); + } + + return true; +} + +std::string CostmapScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::CostmapScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp new file mode 100644 index 00000000000..28e313db8cb --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp @@ -0,0 +1,65 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/distance_scorer.hpp" + +namespace nav2_route +{ + +void DistanceScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring distance scorer."); + name_ = name; + + // Find the tag at high the speed limit information is stored + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".speed_tag", rclcpp::ParameterValue("speed_limit")); + speed_tag_ = node->get_parameter(getName() + ".speed_tag").as_string(); + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); +} + +bool DistanceScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + // Get the speed limit, if set for an edge + float speed_val = 1.0f; + speed_val = edge->metadata.getValue(speed_tag_, speed_val); + cost = weight_ * hypotf( + edge->end->coords.x - edge->start->coords.x, + edge->end->coords.y - edge->start->coords.y) / speed_val; + return true; +} + +std::string DistanceScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::DistanceScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp new file mode 100644 index 00000000000..bd0a9a3c4ba --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp" + +namespace nav2_route +{ + +void DynamicEdgesScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring adjust edges scorer."); + name_ = name; + logger_ = node->get_logger(); + service_ = + node->create_service( + std::string(node->get_name()) + "/" + getName() + "/adjust_edges", std::bind( + &DynamicEdgesScorer::closedEdgesCb, this, + std::placeholders::_1, std::placeholders::_2)); + + dynamic_penalties_.clear(); + closed_edges_.clear(); +} + +void DynamicEdgesScorer::closedEdgesCb( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(logger_, "Edge closure and cost adjustment in progress!"); + + // Add new closed edges + for (unsigned int edge : request->closed_edges) { + closed_edges_.insert(edge); + } + + // Removed now opened edges, if stored + for (unsigned int edge : request->opened_edges) { + if (closed_edges_.find(edge) != closed_edges_.end()) { + closed_edges_.erase(edge); + } + } + + // Add dynamic costs from application system for edges + for (auto & edge : request->adjust_edges) { + dynamic_penalties_[edge.edgeid] = edge.cost; + } + + response->success = true; +} + +bool DynamicEdgesScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + // Find if this edge is in the closed set of edges + if (closed_edges_.find(edge->edgeid) != closed_edges_.end()) { + return false; + } + + const auto & dyn_pen = dynamic_penalties_.find(edge->edgeid); + if (dyn_pen != dynamic_penalties_.end()) { + cost = dyn_pen->second; + } + + return true; +} + +std::string DynamicEdgesScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::DynamicEdgesScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp new file mode 100644 index 00000000000..cfd0d82f0c0 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2024, Polymath Robotics Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp" + +namespace nav2_route +{ + +void GoalOrientationScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring goal orientation scorer."); + name_ = name; + logger_ = node->get_logger(); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI / 2.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".orientation_weight", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".use_orientation_threshold", rclcpp::ParameterValue(false)); + + orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double(); + orientation_weight_ = + static_cast(node->get_parameter(getName() + ".orientation_weight").as_double()); + use_orientation_threshold_ = + node->get_parameter(getName() + ".use_orientation_threshold").as_bool(); +} + +bool GoalOrientationScorer::score( + const EdgePtr edge, + const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) +{ + if (!route_request.use_poses) { + throw nav2_core::InvalidEdgeScorerUse( + "Cannot use goal orientation scorer without goal pose specified!"); + } + + if (edge_type == EdgeType::END) { + double edge_orientation = std::atan2( + edge->end->coords.y - edge->start->coords.y, + edge->end->coords.x - edge->start->coords.x); + double goal_orientation = tf2::getYaw(route_request.goal_pose.pose.orientation); + double d_yaw = std::abs(angles::shortest_angular_distance(edge_orientation, goal_orientation)); + + if (use_orientation_threshold_) { + if (d_yaw > orientation_tolerance_) { + return false; + } + } + + cost = orientation_weight_ * static_cast(d_yaw); + } + return true; +} + +std::string GoalOrientationScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::GoalOrientationScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp new file mode 100644 index 00000000000..409054fff75 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp @@ -0,0 +1,63 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp" + +namespace nav2_route +{ + +void PenaltyScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring penalty scorer."); + name_ = name; + + // Find the tag at high the speed limit information is stored + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".penalty_tag", rclcpp::ParameterValue("penalty")); + penalty_tag_ = node->get_parameter(getName() + ".penalty_tag").as_string(); + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); +} + +bool PenaltyScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + // Get the speed limit, if set for an edge + float penalty_val = 0.0f; + penalty_val = edge->metadata.getValue(penalty_tag_, penalty_val); + cost = weight_ * penalty_val; + return true; +} + +std::string PenaltyScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::PenaltyScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp new file mode 100644 index 00000000000..f44fbf18f47 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp @@ -0,0 +1,105 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp" + +namespace nav2_route +{ + +void SemanticScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring semantic scorer."); + name_ = name; + + // Find the semantic data + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".semantic_classes", rclcpp::ParameterValue(std::vector{})); + std::vector classes = + node->get_parameter(getName() + ".semantic_classes").as_string_array(); + for (auto & cl : classes) { + nav2_util::declare_parameter_if_not_declared( + node, getName() + "." + cl, rclcpp::ParameterType::PARAMETER_DOUBLE); + const double cost = node->get_parameter(getName() + "." + cl).as_double(); + semantic_info_[cl] = static_cast(cost); + } + + // Find the key to look for semantic data for within the metadata. If set to empty string, + // will search instead for any key in the metadata. + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".semantic_key", rclcpp::ParameterValue(std::string("class"))); + key_ = node->get_parameter(getName() + ".semantic_key").as_string(); + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); +} + +void SemanticScorer::metadataKeyScorer(Metadata & mdata, float & score) +{ + for (auto it = mdata.data.cbegin(); it != mdata.data.cend(); ++it) { + if (auto sem = semantic_info_.find(it->first); sem != semantic_info_.end()) { + score += sem->second; + } + } +} + +void SemanticScorer::metadataValueScorer(Metadata & mdata, float & score) +{ + std::string cl; + cl = mdata.getValue(key_, cl); + if (auto sem = semantic_info_.find(cl); sem != semantic_info_.end()) { + score += sem->second; + } +} + +bool SemanticScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + float score = 0.0; + Metadata & node_mdata = edge->end->metadata; + Metadata & edge_mdata = edge->metadata; + + // If a particular key is known to have semantic info, use it, else search + // each metadata key field to see if it matches + if (key_.empty()) { + metadataKeyScorer(node_mdata, score); + metadataKeyScorer(edge_mdata, score); + } else { + metadataValueScorer(node_mdata, score); + metadataValueScorer(edge_mdata, score); + } + + cost = weight_ * score; + return true; +} + +std::string SemanticScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::SemanticScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp new file mode 100644 index 00000000000..13fd5762a3e --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp @@ -0,0 +1,88 @@ +// Copyright (c) 2025, Polymath Robotics Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp" + +namespace nav2_route +{ + +void StartPoseOrientationScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring start pose orientation scorer."); + name_ = name; + logger_ = node->get_logger(); + + nav2_util::declare_parameter_if_not_declared( + node, + getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI / 2.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".orientation_weight", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".use_orientation_threshold", rclcpp::ParameterValue(false)); + + orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double(); + orientation_weight_ = + static_cast(node->get_parameter(getName() + ".orientation_weight").as_double()); + use_orientation_threshold_ = + node->get_parameter(getName() + ".use_orientation_threshold").as_bool(); + + tf_buffer_ = tf_buffer; +} + +bool StartPoseOrientationScorer::score( + const EdgePtr edge, + const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) +{ + if (!route_request.use_poses) { + throw nav2_core::InvalidEdgeScorerUse( + "Cannot use start pose orientation scorer without start pose specified!"); + } + + if (edge_type == EdgeType::START) { + double edge_orientation = std::atan2( + edge->end->coords.y - edge->start->coords.y, + edge->end->coords.x - edge->start->coords.x); + double start_orientation = tf2::getYaw(route_request.start_pose.pose.orientation); + double d_yaw = std::abs(angles::shortest_angular_distance(edge_orientation, start_orientation)); + + if (use_orientation_threshold_) { + if (d_yaw > orientation_tolerance_) { + return false; + } + } + + cost = orientation_weight_ * static_cast(d_yaw); + } + return true; +} + +std::string StartPoseOrientationScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::StartPoseOrientationScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp new file mode 100644 index 00000000000..a334ed6e18d --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/time_scorer.hpp" + +namespace nav2_route +{ + +void TimeScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring time scorer."); + name_ = name; + + // Find the tag at high the speed limit information is stored + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".speed_tag", rclcpp::ParameterValue("abs_speed_limit")); + speed_tag_ = node->get_parameter(getName() + ".speed_tag").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".time_tag", rclcpp::ParameterValue("abs_time_taken")); + prev_time_tag_ = node->get_parameter(getName() + ".time_tag").as_string(); + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".max_vel", rclcpp::ParameterValue(0.5)); + max_vel_ = static_cast(node->get_parameter(getName() + ".max_vel").as_double()); +} + +bool TimeScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + // We have a previous actual time to utilize for a refined estimate + float time = 0.0; + time = edge->metadata.getValue(prev_time_tag_, time); + if (time > 0.0f) { + cost = weight_ * time; + return true; + } + + // Else: Get the speed limit, if set for an edge, else use max velocity + float velocity = 0.0f; + velocity = edge->metadata.getValue(speed_tag_, velocity); + if (velocity <= 0.0f) { + velocity = max_vel_; + } + + cost = weight_ * hypotf( + edge->end->coords.x - edge->start->coords.x, + edge->end->coords.y - edge->start->coords.y) / velocity; + return true; +} + +std::string TimeScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::TimeScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/graph_file_loaders/geojson_graph_file_loader.cpp b/nav2_route/src/plugins/graph_file_loaders/geojson_graph_file_loader.cpp new file mode 100644 index 00000000000..51d0fe9bea2 --- /dev/null +++ b/nav2_route/src/plugins/graph_file_loaders/geojson_graph_file_loader.cpp @@ -0,0 +1,241 @@ +// Copyright (c) 2025 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" + +namespace nav2_route +{ + +void GeoJsonGraphFileLoader::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + RCLCPP_INFO(node->get_logger(), "Configuring geojson graph file loader"); + logger_ = node->get_logger(); +} + +bool GeoJsonGraphFileLoader::loadGraphFromFile( + Graph & graph, GraphToIDMap & graph_to_id_map, std::string filepath) +{ + if (!doesFileExist(filepath)) { + RCLCPP_ERROR(logger_, "The filepath %s does not exist", filepath.c_str()); + return false; + } + + std::ifstream graph_file(filepath); + Json json_graph; + + try { + json_graph = Json::parse(graph_file); + } catch (Json::parse_error & ex) { + RCLCPP_ERROR(logger_, "Failed to parse %s: %s", filepath.c_str(), ex.what()); + return false; + } + + auto features = json_graph["features"]; + std::vector nodes, edges; + getGraphElements(features, nodes, edges); + + if (nodes.empty() || edges.empty()) { + RCLCPP_ERROR( + logger_, "The graph is malformed. Is does not contain nodes or edges. Please check %s", + filepath.c_str()); + return false; + } + + graph.resize(nodes.size()); + addNodesToGraph(graph, graph_to_id_map, nodes); + addEdgesToGraph(graph, graph_to_id_map, edges); + return true; +} + +bool GeoJsonGraphFileLoader::doesFileExist(const std::string & filepath) +{ + return std::filesystem::exists(filepath); +} + +void GeoJsonGraphFileLoader::getGraphElements( + const Json & features, std::vector & nodes, std::vector & edges) +{ + for (const auto & feature : features) { + if (feature["geometry"]["type"] == "Point") { + nodes.emplace_back(feature); + } else if ( // NOLINT + feature["geometry"]["type"] == "MultiLineString" || + feature["geometry"]["type"] == "LineString") + { + edges.emplace_back(feature); + } + } +} + +void GeoJsonGraphFileLoader::addNodesToGraph( + Graph & graph, GraphToIDMap & graph_to_id_map, std::vector & nodes) +{ + int idx = 0; + for (const auto & node : nodes) { + const auto properties = node["properties"]; + graph[idx].nodeid = properties["id"]; + graph_to_id_map[graph[idx].nodeid] = idx; + graph[idx].coords = convertCoordinatesFromJson(node); + graph[idx].operations = convertOperationsFromJson(properties); + graph[idx].metadata = convertMetaDataFromJson(properties); + idx++; + } +} + +void GeoJsonGraphFileLoader::addEdgesToGraph( + Graph & graph, GraphToIDMap & graph_to_id_map, std::vector & edges) +{ + for (const auto & edge : edges) { + // Required data + const auto properties = edge["properties"]; + unsigned int id = properties["id"]; + unsigned int start_id = properties["startid"]; + unsigned int end_id = properties["endid"]; + + if (graph_to_id_map.find(start_id) == graph_to_id_map.end()) { + RCLCPP_ERROR(logger_, "Start id %u does not exist for edge id %u", start_id, id); + throw nav2_core::NoValidGraph("Start id does not exist"); + } + + if (graph_to_id_map.find(end_id) == graph_to_id_map.end()) { + RCLCPP_ERROR(logger_, "End id of %u does not exist for edge id %u", end_id, id); + throw nav2_core::NoValidGraph("End id does not exist"); + } + + EdgeCost edge_cost = convertEdgeCostFromJson(properties); + Operations operations = convertOperationsFromJson(properties); + Metadata metadata = convertMetaDataFromJson(properties); + + graph[graph_to_id_map[start_id]].addEdge( + edge_cost, &graph[graph_to_id_map[end_id]], id, + metadata, operations); + } +} + +Coordinates GeoJsonGraphFileLoader::convertCoordinatesFromJson(const Json & node) +{ + Coordinates coords; + const auto & properties = node["properties"]; + if (properties.contains("frame")) { + coords.frame_id = properties["frame"]; + } + + const auto & coordinates = node["geometry"]["coordinates"]; + coords.x = coordinates[0]; + coords.y = coordinates[1]; + + return coords; +} + +Metadata GeoJsonGraphFileLoader::convertMetaDataFromJson( + const Json & properties, + const std::string & key) +{ + Metadata metadata; + if (!properties.contains(key)) {return metadata;} + + for (const auto & data : properties[key].items()) { + if (data.value().is_object() ) { + Metadata new_metadata = convertMetaDataFromJson(properties[key], data.key()); + metadata.setValue(data.key(), new_metadata); + continue; + } + + const auto setPrimitiveType = [&](const auto & value) -> std::any + { + if (value.is_number()) { + if (value.is_number_unsigned()) { + return static_cast(value); + } else if (value.is_number_integer()) { + return static_cast(value); + } else { + return static_cast(value); + } + } + + if (value.is_boolean()) { + return static_cast(value); + } + + if (value.is_string()) { + return static_cast(value); + } + RCLCPP_ERROR( + logger_, "Failed to convert the key: %s to a value", data.key().c_str()); + throw std::runtime_error("Failed to convert"); + }; + + if (data.value().is_array()) { + std::vector array; + for (const auto & el : data.value()) { + auto value = setPrimitiveType(el); + array.push_back(value); + } + metadata.setValue(data.key(), array); + continue; + } + + auto value = setPrimitiveType(data.value()); + metadata.setValue(data.key(), value); + } + + return metadata; +} + +Operation GeoJsonGraphFileLoader::convertOperationFromJson(const Json & json_operation) +{ + Operation operation; + json_operation.at("type").get_to(operation.type); + Json trigger = json_operation.at("trigger"); + operation.trigger = trigger.get(); + Metadata metadata = convertMetaDataFromJson(json_operation); + operation.metadata = metadata; + + return operation; +} + +Operations GeoJsonGraphFileLoader::convertOperationsFromJson(const Json & properties) +{ + Operations operations; + if (properties.contains("operations")) { + for (const auto & json_operation : properties["operations"]) { + operations.push_back(convertOperationFromJson(json_operation)); + } + } + return operations; +} + +EdgeCost GeoJsonGraphFileLoader::convertEdgeCostFromJson(const Json & properties) +{ + EdgeCost edge_cost; + if (properties.contains("cost")) { + edge_cost.cost = properties["cost"]; + } + + if (properties.contains("overridable")) { + edge_cost.overridable = properties["overridable"]; + } + return edge_cost; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::GeoJsonGraphFileLoader, nav2_route::GraphFileLoader) diff --git a/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp b/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp new file mode 100644 index 00000000000..64d618cba44 --- /dev/null +++ b/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp @@ -0,0 +1,177 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp" + +namespace nav2_route +{ + +void GeoJsonGraphFileSaver::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + RCLCPP_INFO(node->get_logger(), "Configuring geojson graph file saver"); + logger_ = node->get_logger(); +} + +bool GeoJsonGraphFileSaver::saveGraphToFile( + Graph & graph, std::string filepath) +{ + if (filepath.empty()) { + RCLCPP_ERROR(logger_, "File path is empty"); + return false; + } + Json json_graph, json_crs, json_properties; + json_graph["type"] = "FeatureCollection"; + json_graph["name"] = "graph"; + json_properties["name"] = "urn:ogc:def:crs:EPSG::3857"; + json_crs["type"] = "name"; + json_crs["properties"] = json_properties; + json_graph["crs"] = json_crs; + std::vector json_features; + try { + loadNodesFromGraph(graph, json_features); + loadEdgesFromGraph(graph, json_features); + json_graph["features"] = json_features; + std::ofstream file(filepath); + file << json_graph.dump(4) << std::endl; + file.close(); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "An error occurred: %s", e.what()); + return false; + } + return true; +} + +void GeoJsonGraphFileSaver::loadNodesFromGraph( + Graph & graph, std::vector & json_features) +{ + for (const auto & node : graph) { + if (node.nodeid == std::numeric_limits::max()) { // Skip "deleted" nodes + continue; + } + Json json_feature, json_properties, json_geometry, json_metadata, json_operations; + json_geometry["type"] = "Point"; + json_geometry["coordinates"] = std::vector{node.coords.x, node.coords.y}; + json_feature["geometry"] = json_geometry; + json_properties["id"] = node.nodeid; + json_properties["frame"] = node.coords.frame_id; + convertMetaDataToJson(node.metadata, json_metadata); + if (json_metadata.size() > 0) { + json_properties["metadata"] = json_metadata; + } + convertOperationsToJson(node.operations, json_operations); + if (json_operations.size() > 0) { + json_properties["operations"] = json_operations; + } + json_feature["properties"] = json_properties; + json_feature["type"] = "Feature"; + json_features.push_back(json_feature); + } +} + +void GeoJsonGraphFileSaver::loadEdgesFromGraph( + Graph & graph, std::vector & json_edges) +{ + for (const auto & node : graph) { + for (const auto & edge : node.neighbors) { + Json json_edge, json_properties, json_geometry, json_metadata, json_operations; + json_geometry["type"] = "MultiLineString"; + json_edge["geometry"] = json_geometry; + json_properties["id"] = edge.edgeid; + json_properties["startid"] = edge.start->nodeid; + json_properties["endid"] = edge.end->nodeid; + convertMetaDataToJson(edge.metadata, json_metadata); + if (json_metadata.size() > 0) { + json_properties["metadata"] = json_metadata; + } + convertOperationsToJson(edge.operations, json_operations); + if (json_operations.size() > 0) { + json_properties["operations"] = json_operations; + } + json_properties["cost"] = edge.edge_cost.cost; + json_properties["overridable"] = edge.edge_cost.overridable; + json_edge["properties"] = json_properties; + json_edge["type"] = "Feature"; + json_edges.push_back(json_edge); + } + } +} + +void GeoJsonGraphFileSaver::convertMetaDataToJson( + const Metadata & metadata, Json & json_metadata) +{ + /* Function partially created using GPT */ + for (auto itr = metadata.data.begin(); itr != metadata.data.end(); itr++) { + if (itr->second.type() == typeid(std::string)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(int)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(unsigned int)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(float)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(bool)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(Metadata)) { + // If the itr->second is another Metadata, recursively convert it to JSON + Json nested_metadata_json; + convertMetaDataToJson(std::any_cast(itr->second), nested_metadata_json); + json_metadata[itr->first] = nested_metadata_json; + } else if (itr->second.type() == typeid(std::vector)) { + // If the itr->second is a vector, convert each element + std::vector arrayJson; + for (const auto & element : std::any_cast>(itr->second)) { + if (element.type() == typeid(std::string)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(int)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(unsigned int)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(float)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(bool)) { + arrayJson.push_back(std::any_cast(element)); + } + } + json_metadata[itr->first] = arrayJson; + } else { + // If we have an unknown type, handle as needed + json_metadata[itr->first] = itr->second.type().name(); + } + } +} + +void GeoJsonGraphFileSaver::convertOperationsToJson( + const Operations & operations, Json & json_operations) +{ + for (const auto & operation : operations) { + Json json_operation, json_metadata; + json_operation["type"] = operation.type; + json_operation["trigger"] = operation.trigger; + convertMetaDataToJson(operation.metadata, json_metadata); + if (json_metadata.size()) { + json_operation["metadata"] = json_metadata; + } + json_operations[operation.type] = json_operation; + } +} +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::GeoJsonGraphFileSaver, nav2_route::GraphFileSaver) diff --git a/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp b/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp new file mode 100644 index 00000000000..46802194e29 --- /dev/null +++ b/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include + +#include "nav2_route/plugins/route_operations/adjust_speed_limit.hpp" + +namespace nav2_route +{ + +void AdjustSpeedLimit::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring Adjust speed limit operation."); + name_ = name; + logger_ = node->get_logger(); + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".speed_tag", rclcpp::ParameterValue("speed_limit")); + speed_tag_ = node->get_parameter(getName() + ".speed_tag").as_string(); + + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".speed_limit_topic", rclcpp::ParameterValue("speed_limit")); + std::string topic = node->get_parameter(getName() + ".speed_tag").as_string(); + + speed_limit_pub_ = node->create_publisher(topic, 1); + speed_limit_pub_->on_activate(); +} + +OperationResult AdjustSpeedLimit::perform( + NodePtr /*node_achieved*/, + EdgePtr edge_entered, + EdgePtr /*edge_exited*/, + const Route & /*route*/, + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const Metadata * /*mdata*/) +{ + OperationResult result; + if (!edge_entered) { + return result; + } + + float speed_limit = 100.0; + speed_limit = edge_entered->metadata.getValue(speed_tag_, speed_limit); + RCLCPP_DEBUG(logger_, "Setting speed limit to %.2f%% of maximum.", speed_limit); + + auto msg = std::make_unique(); + msg->percentage = true; + msg->speed_limit = speed_limit; + speed_limit_pub_->publish(std::move(msg)); + return result; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::AdjustSpeedLimit, nav2_route::RouteOperation) diff --git a/nav2_route/src/plugins/route_operations/collision_monitor.cpp b/nav2_route/src/plugins/route_operations/collision_monitor.cpp new file mode 100644 index 00000000000..9bc7c1c2d7e --- /dev/null +++ b/nav2_route/src/plugins/route_operations/collision_monitor.cpp @@ -0,0 +1,241 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include + +#include "nav2_route/plugins/route_operations/collision_monitor.hpp" + +namespace nav2_route +{ + +void CollisionMonitor::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) +{ + name_ = name; + clock_ = node->get_clock(); + logger_ = node->get_logger(); + last_check_time_ = clock_->now(); + + std::string server_costmap_topic = node->get_parameter("costmap_topic").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".costmap_topic", rclcpp::ParameterValue("local_costmap/costmap_raw")); + std::string costmap_topic = node->get_parameter(getName() + ".costmap_topic").as_string(); + if (costmap_topic != server_costmap_topic) { + RCLCPP_INFO(node->get_logger(), + "Using costmap topic: %s instead of server costmap topic: %s for CollisionMonitor.", + costmap_topic.c_str(), server_costmap_topic.c_str()); + costmap_subscriber_ = std::make_shared(node, costmap_topic); + topic_ = costmap_topic; + } else { + costmap_subscriber_ = costmap_subscriber; + topic_ = server_costmap_topic; + } + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".rate", rclcpp::ParameterValue(1.0)); + double checking_rate = node->get_parameter(getName() + ".rate").as_double(); + checking_duration_ = rclcpp::Duration::from_seconds(1.0 / checking_rate); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".reroute_on_collision", rclcpp::ParameterValue(true)); + reroute_on_collision_ = node->get_parameter(getName() + ".reroute_on_collision").as_bool(); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".max_cost", rclcpp::ParameterValue(253.0)); + max_cost_ = static_cast(node->get_parameter(getName() + ".max_cost").as_double()); + + // Resolution to check the costmap over (1=every cell, 2=every other cell, etc.) + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".check_resolution", rclcpp::ParameterValue(1)); + check_resolution_ = static_cast( + node->get_parameter(getName() + ".check_resolution").as_int()); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".max_collision_dist", rclcpp::ParameterValue(5.0)); + max_collision_dist_ = static_cast( + node->get_parameter(getName() + ".max_collision_dist").as_double()); + if (max_collision_dist_ <= 0.0) { + RCLCPP_INFO( + logger_, "Max collision distance to evaluate is zero or negative, checking the full route."); + max_collision_dist_ = std::numeric_limits::max(); + } +} + +void CollisionMonitor::getCostmap() +{ + try { + costmap_ = costmap_subscriber_->getCostmap(); + } catch (...) { + throw nav2_core::OperationFailed( + "Collision Monitor could not obtain a costmap from topic: " + topic_); + } +} + +OperationResult CollisionMonitor::perform( + NodePtr /*node*/, + EdgePtr curr_edge, + EdgePtr /*edge_exited*/, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * /*mdata*/) +{ + // Not time yet to check or before getting to first route edge + auto now = clock_->now(); + if (now - last_check_time_ < checking_duration_ || !curr_edge) { + return OperationResult(); + } + last_check_time_ = now; + + OperationResult result; + getCostmap(); + + float dist_checked = 0.0; + Coordinates end = curr_edge->end->coords; + Coordinates start = utils::findClosestPoint( + curr_pose, curr_edge->start->coords, end); + unsigned int curr_edge_id = curr_edge->edgeid; + + bool final_edge = false; + while (!final_edge) { + // Track how far we've checked and should check for collisions + const float edge_dist = hypotf(end.x - start.x, end.y - start.y); + if (dist_checked + edge_dist >= max_collision_dist_) { + float dist_to_eval = max_collision_dist_ - dist_checked; + end = backoutValidEndPoint(start, end, dist_to_eval); + final_edge = true; + } + dist_checked += edge_dist; + + // Find the valid edge grid coords, in case the edge is partially off the grid + LineSegment line; + if (!lineToMap(start, end, line)) { + final_edge = true; + if (!backoutValidEndPoint(start, line)) { + break; + } + } + + // Collision check edge on grid within max distance and + // report blocked edges for rerouting or exit task + if (isInCollision(line)) { + RCLCPP_INFO( + logger_, "Collision has been detected within %0.2fm of robot pose!", max_collision_dist_); + + if (reroute_on_collision_) { + result.reroute = true; + result.blocked_ids.push_back(curr_edge_id); + return result; + } + + throw nav2_core::OperationFailed( + "Collision detected, but rerouting is not enabled, canceling tracking task."); + } + + // Restart loop for next edge until complete + start = end; + if (!final_edge) { + auto isCurrEdge = [&](const EdgePtr & edge) {return edge->edgeid == curr_edge_id;}; + auto iter = std::find_if(route.edges.begin(), route.edges.end(), isCurrEdge); + if (iter != route.edges.end() && ++iter != route.edges.end()) { + // If we found the edge and the next edge is also valid + curr_edge_id = (*iter)->edgeid; + end = (*iter)->end->coords; + } else { + final_edge = true; + } + } + } + + return result; +} + +Coordinates CollisionMonitor::backoutValidEndPoint( + const Coordinates & start, const Coordinates & end, const float dist) +{ + Coordinates new_end; + const float dx = end.x - start.x; + const float dy = end.y - start.y; + const float mag = hypotf(dx, dy); + if (mag < 1e-6) { + return start; + } + new_end.x = (dx / mag) * dist + start.x; + new_end.y = (dy / mag) * dist + start.y; + return new_end; +} + +bool CollisionMonitor::backoutValidEndPoint( + const Coordinates & start, LineSegment & line) +{ + // Check if any part of this edge is potentially valid + if (!costmap_->worldToMap(start.x, start.y, line.x0, line.y0)) { + return false; + } + + // Since worldToMap will populate the out-of-bounds (x1, y1), we can + // iterate through the partially valid line until we hit invalid coords + unsigned int last_end_x = line.x0, last_end_y = line.y0; + nav2_util::LineIterator iter(line.x0, line.y0, line.x1, line.y1); + int size_x = static_cast(costmap_->getSizeInCellsX()); + int size_y = static_cast(costmap_->getSizeInCellsY()); + for (; iter.isValid(); iter.advance()) { + if (iter.getX() >= size_x || iter.getY() >= size_y) { + line.x1 = last_end_x; + line.y1 = last_end_y; + return true; + } + last_end_x = iter.getX(); + last_end_y = iter.getY(); + } + + return false; +} + +bool CollisionMonitor::lineToMap( + const Coordinates & start, const Coordinates & end, LineSegment & line) +{ + if (!costmap_->worldToMap(start.x, start.y, line.x0, line.y0) || + !costmap_->worldToMap(end.x, end.y, line.x1, line.y1)) + { + return false; + } + return true; +} + +bool CollisionMonitor::isInCollision(const LineSegment & line) +{ + nav2_util::LineIterator iter(line.x0, line.y0, line.x1, line.y1); + for (; iter.isValid(); ) { + float cost = static_cast(costmap_->getCost(iter.getX(), iter.getY())); + if (cost >= max_cost_ && cost != 255.0 /*unknown*/) { + return true; + } + + // Advance the iterator by the check resolution on the edge, pruning to a coarse resolution + for (unsigned int i = 0; i < check_resolution_; i++) { + iter.advance(); + } + } + return false; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::CollisionMonitor, nav2_route::RouteOperation) diff --git a/nav2_route/src/plugins/route_operations/rerouting_service.cpp b/nav2_route/src/plugins/route_operations/rerouting_service.cpp new file mode 100644 index 00000000000..44cfb4d44a9 --- /dev/null +++ b/nav2_route/src/plugins/route_operations/rerouting_service.cpp @@ -0,0 +1,70 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include + +#include "nav2_route/plugins/route_operations/rerouting_service.hpp" + +namespace nav2_route +{ + +void ReroutingService::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring Rerouting service operation."); + name_ = name; + logger_ = node->get_logger(); + reroute_.store(false); + service_ = node->create_service( + std::string(node->get_name()) + "/" + getName() + "/reroute", + std::bind( + &ReroutingService::serviceCb, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void ReroutingService::serviceCb( + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO(logger_, "A reroute has been requested!"); + reroute_.store(true); + response->success = true; +} + +OperationResult ReroutingService::perform( + NodePtr /*node*/, + EdgePtr /*edge_entered*/, + EdgePtr /*edge_exited*/, + const Route & /*route*/, + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const Metadata * /*mdata*/) +{ + OperationResult result; + if (reroute_.load()) { + reroute_.store(false); + result.reroute = true; + return result; + } + + return result; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::ReroutingService, nav2_route::RouteOperation) diff --git a/nav2_route/src/plugins/route_operations/time_marker.cpp b/nav2_route/src/plugins/route_operations/time_marker.cpp new file mode 100644 index 00000000000..4da0cbd2c19 --- /dev/null +++ b/nav2_route/src/plugins/route_operations/time_marker.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include + +#include "nav2_route/plugins/route_operations/time_marker.hpp" + +namespace nav2_route +{ + +void TimeMarker::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring Adjust speed limit operation."); + name_ = name; + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".time_tag", rclcpp::ParameterValue("abs_time_taken")); + time_tag_ = node->get_parameter(getName() + ".time_tag").as_string(); + clock_ = node->get_clock(); + edge_start_time_ = rclcpp::Time(0.0); +} + +OperationResult TimeMarker::perform( + NodePtr /*node_achieved*/, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & /*route*/, + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const Metadata * /*mdata*/) +{ + OperationResult result; + rclcpp::Time now = clock_->now(); + if (!edge_exited || edge_exited->edgeid != curr_edge_) { + edge_start_time_ = now; + curr_edge_ = edge_entered ? edge_entered->edgeid : 0; + return result; + } + + float dur = static_cast((now - edge_start_time_).seconds()); + if (edge_start_time_.seconds() > 1e-3) { + edge_exited->metadata.setValue(time_tag_, dur); + } + + edge_start_time_ = now; + curr_edge_ = edge_entered ? edge_entered->edgeid : 0; + return result; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::TimeMarker, nav2_route::RouteOperation) diff --git a/nav2_route/src/plugins/route_operations/trigger_event.cpp b/nav2_route/src/plugins/route_operations/trigger_event.cpp new file mode 100644 index 00000000000..91ba6882537 --- /dev/null +++ b/nav2_route/src/plugins/route_operations/trigger_event.cpp @@ -0,0 +1,17 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_route/plugins/route_operations/trigger_event.hpp" +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::TriggerEvent, nav2_route::RouteOperation) diff --git a/nav2_route/src/route_planner.cpp b/nav2_route/src/route_planner.cpp new file mode 100644 index 00000000000..1fc005837c2 --- /dev/null +++ b/nav2_route/src/route_planner.cpp @@ -0,0 +1,218 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "nav2_route/route_planner.hpp" + +namespace nav2_route +{ + +void RoutePlanner::configure( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::shared_ptr costmap_subscriber) +{ + nav2_util::declare_parameter_if_not_declared( + node, "max_iterations", rclcpp::ParameterValue(0)); + max_iterations_ = node->get_parameter("max_iterations").as_int(); + + if (max_iterations_ <= 0) { + max_iterations_ = std::numeric_limits::max(); + } + + edge_scorer_ = std::make_unique(node, tf_buffer, costmap_subscriber); +} + +Route RoutePlanner::findRoute( + Graph & graph, unsigned int start_index, unsigned int goal_index, + const std::vector & blocked_ids, + const RouteRequest & route_request) +{ + if (graph.empty()) { + throw nav2_core::NoValidGraph("Graph is invalid for routing!"); + } + + // Find the start and goal pointers, it is important in this function + // that the start node is the underlying pointer, so that the address + // is valid when this function goes out of scope + const NodePtr & start_node = &graph.at(start_index); + const NodePtr & goal_node = &graph.at(goal_index); + findShortestGraphTraversal(graph, start_node, goal_node, blocked_ids, route_request); + + EdgePtr & parent_edge = goal_node->search_state.parent_edge; + if (!parent_edge) { + throw nav2_core::NoValidRouteCouldBeFound("Could not find a route to the requested goal!"); + } + + // Convert graph traversal into a meaningful route + Route route; + while (parent_edge) { + route.edges.push_back(parent_edge); + parent_edge = parent_edge->start->search_state.parent_edge; + } + + std::reverse(route.edges.begin(), route.edges.end()); + route.start_node = start_node; + route.route_cost = goal_node->search_state.integrated_cost; + return route; +} + +void RoutePlanner::resetSearchStates(Graph & graph) +{ + // For graphs < 75,000 nodes, iterating through one time on initialization to reset the state + // is neglibably different to allocating & deallocating the complimentary blocks of memory + for (unsigned int i = 0; i != graph.size(); i++) { + graph[i].search_state.reset(); + } +} + +void RoutePlanner::findShortestGraphTraversal( + Graph & graph, const NodePtr start_node, const NodePtr goal_node, + const std::vector & blocked_ids, + const RouteRequest & route_request) +{ + // Setup the Dijkstra's search + resetSearchStates(graph); + start_id_ = start_node->nodeid; + goal_id_ = goal_node->nodeid; + start_node->search_state.integrated_cost = 0.0; + addNode(0.0, start_node); + + NodePtr neighbor{nullptr}; + EdgePtr edge{nullptr}; + float potential_cost = 0.0, traversal_cost = 0.0; + int iterations = 0; + while (!queue_.empty() && iterations < max_iterations_) { + iterations++; + + // Get the next lowest cost node + auto [curr_cost, node] = getNextNode(); + + // This has been visited, thus already lowest cost + if (curr_cost != node->search_state.integrated_cost) { + continue; + } + + // We have the shortest path + if (isGoal(node)) { + // Reset states + clearQueue(); + return; + } + + // Expand to connected nodes + EdgeVector & edges = getEdges(node); + for (unsigned int edge_num = 0; edge_num != edges.size(); edge_num++) { + edge = &edges[edge_num]; + neighbor = edge->end; + + // If edge is invalid (lane closed, occupied, etc), don't expand + if (!getTraversalCost(edge, traversal_cost, blocked_ids, route_request)) { + continue; + } + + potential_cost = curr_cost + traversal_cost; + if (potential_cost < neighbor->search_state.integrated_cost) { + neighbor->search_state.parent_edge = edge; + neighbor->search_state.integrated_cost = potential_cost; + neighbor->search_state.traversal_cost = traversal_cost; + addNode(potential_cost, neighbor); + } + } + } + + if (iterations == max_iterations_) { + // Reset states + clearQueue(); + throw nav2_core::TimedOut("Maximum iterations was exceeded!"); + } +} + +bool RoutePlanner::getTraversalCost( + const EdgePtr edge, float & score, const std::vector & blocked_ids, + const RouteRequest & route_request) +{ + // If edge or node is in the blocked list, don't expand + auto is_blocked = std::find_if( + blocked_ids.begin(), blocked_ids.end(), + [&](unsigned int id) {return id == edge->edgeid || id == edge->end->nodeid;}); + if (is_blocked != blocked_ids.end()) { + return false; + } + + // If an edge's cost is marked as not to be overridden by scoring plugins + // Or there are no scoring plugins, use the edge's cost, if it is valid (positive) + if (!edge->edge_cost.overridable || edge_scorer_->numPlugins() == 0) { + if (edge->edge_cost.cost <= 0.0) { + throw nav2_core::NoValidGraph( + "Edge " + std::to_string(edge->edgeid) + + " doesn't contain and cannot compute a valid edge cost!"); + } + score = edge->edge_cost.cost; + return true; + } + + return edge_scorer_->score(edge, route_request, classifyEdge(edge), score); +} + +NodeElement RoutePlanner::getNextNode() +{ + NodeElement data = queue_.top(); + queue_.pop(); + return data; +} + +void RoutePlanner::addNode(const float cost, const NodePtr node) +{ + queue_.emplace(cost, node); +} + +EdgeVector & RoutePlanner::getEdges(const NodePtr node) +{ + return node->neighbors; +} + +void RoutePlanner::clearQueue() +{ + NodeQueue q; + std::swap(queue_, q); +} + +bool RoutePlanner::isGoal(const NodePtr node) +{ + return node->nodeid == goal_id_; +} + +bool RoutePlanner::isStart(const NodePtr node) +{ + return node->nodeid == start_id_; +} + +nav2_route::EdgeType RoutePlanner::classifyEdge(const EdgePtr edge) +{ + if (isStart(edge->start)) { + return EdgeType::START; + } else if (isGoal(edge->end)) { + return EdgeType::END; + } + return nav2_route::EdgeType::NONE; +} + +} // namespace nav2_route diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp new file mode 100644 index 00000000000..aad64e16ec6 --- /dev/null +++ b/nav2_route/src/route_server.cpp @@ -0,0 +1,408 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include "nav2_route/route_server.hpp" + +using nav2_util::declare_parameter_if_not_declared; +using std::placeholders::_1; +using std::placeholders::_2; + +namespace nav2_route +{ + +RouteServer::RouteServer(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("route_server", "", options) +{} + +nav2_util::CallbackReturn +RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + tf_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), + get_node_timers_interface()); + tf_->setCreateTimerInterface(timer_interface); + transform_listener_ = std::make_shared(*tf_); + + auto node = shared_from_this(); + graph_vis_publisher_ = + node->create_publisher( + "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + compute_route_server_ = std::make_shared( + node, "compute_route", + std::bind(&RouteServer::computeRoute, this), + nullptr, std::chrono::milliseconds(500), true); + + compute_and_track_route_server_ = std::make_shared( + node, "compute_and_track_route", + std::bind(&RouteServer::computeAndTrackRoute, this), + nullptr, std::chrono::milliseconds(500), true); + + set_graph_service_ = std::make_shared>>( + std::string(node->get_name()) + "/set_route_graph", + node, + std::bind( + &RouteServer::setRouteGraph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + declare_parameter_if_not_declared( + node, "route_frame", rclcpp::ParameterValue(std::string("map"))); + declare_parameter_if_not_declared( + node, "base_frame", rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter_if_not_declared( + node, "max_planning_time", rclcpp::ParameterValue(2.0)); + + route_frame_ = node->get_parameter("route_frame").as_string(); + base_frame_ = node->get_parameter("base_frame").as_string(); + max_planning_time_ = node->get_parameter("max_planning_time").as_double(); + + // Create costmap subscriber + nav2_util::declare_parameter_if_not_declared( + node, "costmap_topic", + rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); + std::string costmap_topic = node->get_parameter("costmap_topic").as_string(); + costmap_subscriber_ = std::make_shared(node, costmap_topic); + + try { + graph_loader_ = std::make_shared(node, tf_, route_frame_); + if (!graph_loader_->loadGraphFromParameter(graph_, id_to_graph_map_)) { + return nav2_util::CallbackReturn::FAILURE; + } + + goal_intent_extractor_ = std::make_shared(); + goal_intent_extractor_->configure( + node, graph_, &id_to_graph_map_, tf_, costmap_subscriber_, route_frame_, base_frame_); + + route_planner_ = std::make_shared(); + route_planner_->configure(node, tf_, costmap_subscriber_); + + route_tracker_ = std::make_shared(); + route_tracker_->configure( + node, tf_, costmap_subscriber_, compute_and_track_route_server_, route_frame_, base_frame_); + + path_converter_ = std::make_shared(); + path_converter_->configure(node); + } catch (std::exception & e) { + RCLCPP_FATAL(get_logger(), "Failed to configure route server: %s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + compute_route_server_->activate(); + compute_and_track_route_server_->activate(); + graph_vis_publisher_->on_activate(); + graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); + createBond(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RouteServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + compute_route_server_->deactivate(); + compute_and_track_route_server_->deactivate(); + graph_vis_publisher_->on_deactivate(); + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RouteServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + compute_route_server_.reset(); + compute_and_track_route_server_.reset(); + set_graph_service_.reset(); + graph_loader_.reset(); + route_planner_.reset(); + route_tracker_.reset(); + path_converter_.reset(); + goal_intent_extractor_.reset(); + graph_vis_publisher_.reset(); + transform_listener_.reset(); + tf_.reset(); + graph_.clear(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RouteServer::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +rclcpp::Duration +RouteServer::findPlanningDuration(const rclcpp::Time & start_time) +{ + auto cycle_duration = this->now() - start_time; + if (max_planning_time_ > 0.0 && cycle_duration.seconds() > max_planning_time_) { + RCLCPP_WARN( + get_logger(), + "Route planner missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", + 1 / max_planning_time_, 1 / cycle_duration.seconds()); + } + + return cycle_duration; +} + +template +bool +RouteServer::isRequestValid( + std::shared_ptr> & action_server) +{ + if (!action_server || !action_server->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); + return false; + } + + if (action_server->is_cancel_requested()) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling route planning action."); + action_server->terminate_all(); + return false; + } + + if (graph_.empty()) { + RCLCPP_INFO(get_logger(), "No graph set! Aborting request."); + action_server->terminate_current(); + return false; + } + + return true; +} + +void RouteServer::populateActionResult( + std::shared_ptr result, + const Route & route, + const nav_msgs::msg::Path & path, + const rclcpp::Duration & planning_duration) +{ + result->route = utils::toMsg(route, route_frame_, this->now()); + result->path = path; + result->planning_time = planning_duration; +} + +void RouteServer::populateActionResult( + std::shared_ptr result, + const Route &, + const nav_msgs::msg::Path &, + const rclcpp::Duration & execution_duration) +{ + result->execution_duration = execution_duration; +} + +template +Route RouteServer::findRoute( + const std::shared_ptr goal, + ReroutingState & rerouting_info) +{ + // Find the search boundaries + auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); + + // If we're rerouting, use the rerouting start node and pose as the new start + if (rerouting_info.rerouting_start_id != std::numeric_limits::max()) { + start_route = id_to_graph_map_.at(rerouting_info.rerouting_start_id); + goal_intent_extractor_->overrideStart(rerouting_info.rerouting_start_pose); + } + + Route route; + if (start_route == end_route) { + // Succeed with a single-point route + route.route_cost = 0.0; + route.start_node = &graph_.at(start_route); + } else { + // Populate request data (start & goal id, start & goal pose, if set) for routing + RouteRequest route_request; + route_request.start_nodeid = start_route; + route_request.goal_nodeid = end_route; + route_request.start_pose = goal_intent_extractor_->getStart(); + route_request.goal_pose = goal->goal; + route_request.use_poses = goal->use_poses; + + // Compute the route via graph-search, returns a node-edge sequence + route = route_planner_->findRoute( + graph_, start_route, end_route, rerouting_info.blocked_ids, route_request); + } + + return goal_intent_extractor_->pruneStartandGoal(route, goal, rerouting_info); +} + +template +void +RouteServer::processRouteRequest( + std::shared_ptr> & action_server) +{ + auto goal = action_server->get_current_goal(); + auto result = std::make_shared(); + ReroutingState rerouting_info; + auto start_time = this->now(); + + try { + while (rclcpp::ok()) { + if (!isRequestValid(action_server)) { + return; + } + + if (action_server->is_preempt_requested()) { + RCLCPP_INFO(get_logger(), "Computing new preempted route to goal."); + goal = action_server->accept_pending_goal(); + rerouting_info.reset(); + } + + // Find the route + Route route = findRoute(goal, rerouting_info); + RCLCPP_INFO(get_logger(), "Route found with %zu nodes and %zu edges", + route.edges.size() + 1u, route.edges.size()); + auto path = path_converter_->densify(route, rerouting_info, route_frame_, this->now()); + + if (std::is_same::value) { + // blocks until re-route requested or task completion, publishes feedback + switch (route_tracker_->trackRoute(route, path, rerouting_info)) { + case TrackerResult::COMPLETED: + populateActionResult(result, route, path, this->now() - start_time); + action_server->succeeded_current(result); + return; + case TrackerResult::INTERRUPTED: + // Reroute, cancel, or preempt requested + break; + case TrackerResult::EXITED: + // rclcpp::ok() is false, so just return + return; + } + } else { + // Return route if not tracking + populateActionResult(result, route, path, findPlanningDuration(start_time)); + action_server->succeeded_current(result); + return; + } + } + } catch (nav2_core::NoValidRouteCouldBeFound & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::NO_VALID_ROUTE; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::TimedOut & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::TIMEOUT; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::RouteTFError & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::TF_ERROR; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::NoValidGraph & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::NO_VALID_GRAPH; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::IndeterminantNodesOnGraph & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::INDETERMINANT_NODES_ON_GRAPH; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::InvalidEdgeScorerUse & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::INVALID_EDGE_SCORER_USE; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::OperationFailed & ex) { + // A special case since Operation Failed is only in Compute & Track + // actions, specifying it to allow otherwise fully shared code + exceptionWarning(goal, ex); + result->error_code = ComputeAndTrackRoute::Result::OPERATION_FAILED; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::RouteException & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::UNKNOWN; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (std::exception & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::UNKNOWN; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } +} + +void +RouteServer::computeRoute() +{ + RCLCPP_INFO(get_logger(), "Computing route to goal."); + processRouteRequest(compute_route_server_); +} + +void +RouteServer::computeAndTrackRoute() +{ + RCLCPP_INFO(get_logger(), "Computing and tracking route to goal."); + processRouteRequest(compute_and_track_route_server_); +} + +void RouteServer::setRouteGraph( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(get_logger(), "Setting new route graph: %s.", request->graph_filepath.c_str()); + graph_.clear(); + id_to_graph_map_.clear(); + try { + if (graph_loader_->loadGraphFromFile(graph_, id_to_graph_map_, request->graph_filepath)) { + goal_intent_extractor_->setGraph(graph_, &id_to_graph_map_); + graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); + response->success = true; + return; + } + } catch (std::exception & ex) { + } + + RCLCPP_WARN( + get_logger(), + "Failed to set new route graph: %s!", request->graph_filepath.c_str()); + response->success = false; +} + +template +void RouteServer::exceptionWarning( + const std::shared_ptr goal, + const std::exception & ex) +{ + RCLCPP_WARN( + get_logger(), + "Route server failed on request: Start: [(%0.2f, %0.2f) / %i] Goal: [(%0.2f, %0.2f) / %i]:" + " \"%s\"", goal->start.pose.position.x, goal->start.pose.position.y, goal->start_id, + goal->goal.pose.position.x, goal->goal.pose.position.y, goal->goal_id, ex.what()); +} + +} // namespace nav2_route + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_route::RouteServer) diff --git a/nav2_route/src/route_tracker.cpp b/nav2_route/src/route_tracker.cpp new file mode 100644 index 00000000000..7c083a35b3d --- /dev/null +++ b/nav2_route/src/route_tracker.cpp @@ -0,0 +1,246 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_route/route_tracker.hpp" + +namespace nav2_route +{ + +void RouteTracker::configure( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + std::shared_ptr action_server, + const std::string & route_frame, + const std::string & base_frame) +{ + clock_ = node->get_clock(); + logger_ = node->get_logger(); + route_frame_ = route_frame; + base_frame_ = base_frame; + action_server_ = action_server; + tf_buffer_ = tf_buffer; + + nav2_util::declare_parameter_if_not_declared( + node, "radius_to_achieve_node", rclcpp::ParameterValue(2.0)); + radius_threshold_ = node->get_parameter("radius_to_achieve_node").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, "boundary_radius_to_achieve_node", rclcpp::ParameterValue(1.0)); + boundary_radius_threshold_ = node->get_parameter("boundary_radius_to_achieve_node").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, "tracker_update_rate", rclcpp::ParameterValue(50.0)); + tracker_update_rate_ = node->get_parameter("tracker_update_rate").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, "aggregate_blocked_ids", rclcpp::ParameterValue(false)); + aggregate_blocked_ids_ = node->get_parameter("aggregate_blocked_ids").as_bool(); + + operations_manager_ = std::make_unique(node, costmap_subscriber); +} + +geometry_msgs::msg::PoseStamped RouteTracker::getRobotPose() +{ + geometry_msgs::msg::PoseStamped pose; + if (!nav2_util::getCurrentPose(pose, *tf_buffer_, route_frame_, base_frame_)) { + throw nav2_core::RouteTFError("Unable to get robot pose in route frame: " + route_frame_); + } + return pose; +} + +bool RouteTracker::nodeAchieved( + const geometry_msgs::msg::PoseStamped & pose, + RouteTrackingState & state, + const Route & route) +{ + // check if inside a *generous* radius window + const double dx = state.next_node->coords.x - pose.pose.position.x; + const double dy = state.next_node->coords.y - pose.pose.position.y; + const double dist_mag = std::sqrt(dx * dx + dy * dy); + const bool is_boundary_node = isStartOrEndNode(state, route); + const bool in_radius = + (dist_mag <= (is_boundary_node ? boundary_radius_threshold_ : radius_threshold_)); + + // Within 0.1mm is achieved or within radius and now not, consider node achieved + if (dist_mag < 1e-4 || (!in_radius && state.within_radius)) { + return true; + } + + // Update the state for the next iteration + state.within_radius = in_radius; + + // If start or end node, use the radius check only since the final node may not pass + // threshold depending on the configurations. The start node has no last_node for + // computing the vector bisector. If this is an issue, please file a ticket to discuss. + if (is_boundary_node) { + return state.within_radius; + } + + // We can evaluate the unit distance vector from the node w.r.t. the unit vector bisecting + // the last and current edges to find the average whose orthogonal is an imaginary + // line representing the migration from one edge's spatial domain to the other. + // When the dot product is negative, it means that there exists a projection between + // the vectors and that the robot position has passed this imaginary orthogonal line. + // This enables a more refined definition of when a node is considered achieved while + // enabling the use of dynamic behavior that may deviate from the path non-trivially + if (state.within_radius) { + NodePtr last_node = state.current_edge->start; + const double nx = state.next_node->coords.x - last_node->coords.x; + const double ny = state.next_node->coords.y - last_node->coords.y; + const double n_mag = std::sqrt(nx * nx + ny * ny); + + NodePtr future_next_node = route.edges[state.route_edges_idx + 1]->end; + const double mx = future_next_node->coords.x - state.next_node->coords.x; + const double my = future_next_node->coords.y - state.next_node->coords.y; + const double m_mag = std::sqrt(mx * mx + my * my); + + // If nodes overlap so there is no vector, use radius check only (divide by zero) + if (n_mag < 1e-6 || m_mag < 1e-6) { + return true; + } + + // Unnormalized Bisector = |n|*m + |m|*n + const double bx = nx * m_mag + mx * n_mag; + const double by = ny * m_mag + my * n_mag; + return utils::normalizedDot(bx, by, dx, dy) <= 0; + } + + return false; +} + +bool RouteTracker::isStartOrEndNode(RouteTrackingState & state, const Route & route) +{ + // Check if current_edge is nullptr in case we have a rerouted previous + // edge to use for the refined node achievement vectorized estimate + return + (state.route_edges_idx == static_cast(route.edges.size() - 1)) || + (state.route_edges_idx == -1 && !state.current_edge); +} + +void RouteTracker::publishFeedback( + const bool rereouted, + const unsigned int next_node_id, + const unsigned int last_node_id, + const unsigned int edge_id, + const std::vector & operations) +{ + auto feedback = std::make_unique(); + feedback->route = route_msg_; + feedback->path = path_; + feedback->rerouted = rereouted; + feedback->next_node_id = next_node_id; + feedback->last_node_id = last_node_id; + feedback->current_edge_id = edge_id; + feedback->operations_triggered = operations; + action_server_->publish_feedback(std::move(feedback)); +} + +TrackerResult RouteTracker::trackRoute( + const Route & route, const nav_msgs::msg::Path & path, + ReroutingState & rerouting_info) +{ + route_msg_ = utils::toMsg(route, route_frame_, clock_->now()); + path_ = path; + RouteTrackingState state; + state.next_node = route.start_node; + + // If we're rerouted but still covering the same previous edge to + // start, retain the state so we can continue as previously set with + // refined node achievement logic and performing edge operations on exit + if (rerouting_info.curr_edge) { + // state.next_node is not updated since the first edge is removed from route when rerouted + // along the same edge in the goal intent extractor. Thus, state.next_node is still the + // future node to reach in this case and we add in the state.last_node and state.current_edge + // to represent the 'currently' progressing edge that is omitted from the route (and its start) + state.current_edge = rerouting_info.curr_edge; + state.last_node = state.current_edge->start; + publishFeedback( + true, route.start_node->nodeid, state.last_node->nodeid, state.current_edge->edgeid, {}); + } else { + publishFeedback(true, route.start_node->nodeid, 0, 0, {}); + } + + rclcpp::Rate r(tracker_update_rate_); + while (rclcpp::ok()) { + bool status_change = false, completed = false; + + // Check if OK to keep processing + if (action_server_->is_cancel_requested()) { + return TrackerResult::INTERRUPTED; + } else if (action_server_->is_preempt_requested()) { + return TrackerResult::INTERRUPTED; + } + + // Update the tracking state + geometry_msgs::msg::PoseStamped robot_pose = getRobotPose(); + if (nodeAchieved(robot_pose, state, route)) { + status_change = true; + state.within_radius = false; + state.last_node = state.next_node; + if (++state.route_edges_idx < static_cast(route.edges.size())) { + state.current_edge = route.edges[state.route_edges_idx]; + state.next_node = state.current_edge->end; + } else { // At achieved the last node in the route + state.current_edge = nullptr; + state.next_node = nullptr; + completed = true; + } + } + + // Process any operations necessary + OperationsResult ops_result = + operations_manager_->process(status_change, state, route, robot_pose, rerouting_info); + + if (completed) { + RCLCPP_INFO(logger_, "Routing to goal completed!"); + // Publishing last feedback + publishFeedback(false, 0, state.last_node->nodeid, 0, ops_result.operations_triggered); + return TrackerResult::COMPLETED; + } + + if ((status_change || !ops_result.operations_triggered.empty()) && state.current_edge) { + publishFeedback( + false, // No rerouting occurred + state.next_node->nodeid, state.last_node->nodeid, + state.current_edge->edgeid, ops_result.operations_triggered); + } + + if (ops_result.reroute) { + if (!aggregate_blocked_ids_) { + rerouting_info.blocked_ids = ops_result.blocked_ids; + } else { + rerouting_info.blocked_ids.insert( + rerouting_info.blocked_ids.end(), + ops_result.blocked_ids.begin(), ops_result.blocked_ids.end()); + } + + if (state.last_node) { + rerouting_info.rerouting_start_id = state.last_node->nodeid; + rerouting_info.rerouting_start_pose = robot_pose; + } else { + rerouting_info.rerouting_start_id = std::numeric_limits::max(); + rerouting_info.rerouting_start_pose = geometry_msgs::msg::PoseStamped(); + } + + // Update so during rerouting we can check if we are continuing on the same edge + rerouting_info.curr_edge = state.current_edge; + RCLCPP_INFO(logger_, "Rerouting requested by route tracking operations!"); + return TrackerResult::INTERRUPTED; + } + + r.sleep(); + } + + return TrackerResult::EXITED; +} + +} // namespace nav2_route diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt new file mode 100644 index 00000000000..186c93dbfca --- /dev/null +++ b/nav2_route/test/CMakeLists.txt @@ -0,0 +1,129 @@ +# Route planner benchmarking script +add_executable(performance_benchmarking performance_benchmarking.cpp) +target_link_libraries(performance_benchmarking + ${library_name} +) +install(TARGETS + performance_benchmarking + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +# Test utilities and basic types +ament_add_gtest(test_utils_and_types + test_utils_and_types.cpp +) +target_link_libraries(test_utils_and_types + ${library_name} +) + +# Test edge scorer + plugins +ament_add_gtest(test_edge_scorers + test_edge_scorers.cpp +) +target_link_libraries(test_edge_scorers + ${library_name} edge_scorers +) + +# Test path converter +ament_add_gtest(test_path_converter + test_path_converter.cpp +) +target_link_libraries(test_path_converter + ${library_name} +) + +# Test node spatial tree +ament_add_gtest(test_spatial_tree + test_spatial_tree.cpp +) +target_link_libraries(test_spatial_tree + ${library_name} +) + +# Test operation manager + plugins +ament_add_gtest(test_operations + test_operations.cpp +) +target_link_libraries(test_operations + ${library_name} route_operations +) + +# Test graph loader +ament_add_gtest(test_graph_loader + test_graph_loader.cpp +) +target_link_libraries(test_graph_loader + ${library_name} +) + +# Test graph saver +ament_add_gtest(test_graph_saver + test_graph_saver.cpp +) +target_link_libraries(test_graph_saver + ${library_name} +) + +# Test geojson parser +ament_add_gtest(test_geojson_graph_file_loader + test_geojson_graph_file_loader.cpp +) +target_link_libraries(test_geojson_graph_file_loader + ${library_name} graph_file_loaders +) + +# Test geojson saver +ament_add_gtest(test_geojson_graph_file_saver + test_geojson_graph_file_saver.cpp +) +target_link_libraries(test_geojson_graph_file_saver + ${library_name} graph_file_loaders graph_file_savers +) + +# Test collision monitor separately due to relative complexity +ament_add_gtest(test_collision_operation + test_collision_operation.cpp +) +target_link_libraries(test_collision_operation + ${library_name} route_operations +) + +# Test route planner +ament_add_gtest(test_route_planner + test_route_planner.cpp +) +target_link_libraries(test_route_planner + ${library_name} edge_scorers +) + +# Test goal intent extractor +ament_add_gtest(test_goal_intent_extractor + test_goal_intent_extractor.cpp +) +target_link_libraries(test_goal_intent_extractor + ${library_name} +) + +# Test route tracker +ament_add_gtest(test_route_tracker + test_route_tracker.cpp +) +target_link_libraries(test_route_tracker + ${library_name} +) + +# Test route server +ament_add_gtest(test_route_server + test_route_server.cpp +) +target_link_libraries(test_route_server + ${library_name} +) + +# Test goal intent search +ament_add_gtest(test_goal_intent_search + test_goal_intent_search.cpp +) +target_link_libraries(test_goal_intent_search + ${library_name} +) diff --git a/nav2_route/test/performance_benchmarking.cpp b/nav2_route/test/performance_benchmarking.cpp new file mode 100644 index 00000000000..9773a8838cc --- /dev/null +++ b/nav2_route/test/performance_benchmarking.cpp @@ -0,0 +1,144 @@ +// Copyright (c) 2025 Samsung Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/route_planner.hpp" +#include "nav2_route/node_spatial_tree.hpp" + +using namespace nav2_route; // NOLINT + +// This is a script to generate a regularized but arbitrarily sized graph for testing +// the route planner's performance across massive spaces + +// Size of the benchmarking side length (e.g. 1000 x 1000 = 1,000,000 nodes) +const unsigned int DIM = 300; +// Number of tests to average results over +const unsigned int NUM_TESTS = 100; + +inline Graph createGraph() +{ + Graph graph; + graph.resize(DIM * DIM); + + EdgeCost e_cost; + unsigned int curr_edge_idx = DIM * DIM + 1; + + unsigned int curr_graph_idx = 0; + for (unsigned int j = 0; j != DIM; j++) { + for (unsigned int i = 0; i != DIM; i++) { + Node & node = graph[curr_graph_idx]; + node.nodeid = curr_graph_idx + 1; + node.coords.x = i; + node.coords.y = j; + + if (i > 0) { + // (i - 1, j) + node.addEdge(e_cost, &graph[curr_graph_idx - 1], curr_edge_idx++); + graph[curr_graph_idx - 1].addEdge(e_cost, &node, curr_edge_idx++); + } + if (j > 0) { + // (i, j - 1) + node.addEdge(e_cost, &graph[curr_graph_idx - DIM], curr_edge_idx++); + graph[curr_graph_idx - DIM].addEdge(e_cost, &node, curr_edge_idx++); + } + + curr_graph_idx++; + } + } + + return graph; +} + +int main(int argc, char const * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("route_benchmarking2"); + std::shared_ptr tf_buffer; + + Graph graph = createGraph(); + + // To visualize for smaller graphs reasonable for rviz to handle (DIM < 50) + // auto graph_vis_pub = node->create_publisher( + // "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + // graph_vis_pub->publish(utils::toMsg(graph, "map", node->now())); + + std::shared_ptr costmap_subscriber; + RoutePlanner planner; + planner.configure(node, tf_buffer, costmap_subscriber); + std::vector blocked_ids; + Route route; + + // // First test: Plan clear across the maximum diagonal + // auto start = node->now(); + // for (unsigned int i = 0; i != NUM_TESTS; i++) { + // route = planner.findRoute(graph, 0u, static_cast(DIM * DIM - 1), blocked_ids); + // } + // auto end = node->now(); + // RCLCPP_INFO( + // node->get_logger(), + // "Across map took %0.5f milliseconds.", + // (end - start).seconds() * 1000.0 / static_cast(NUM_TESTS)); + // RCLCPP_INFO( + // node->get_logger(), + // "Route size: %li", route.edges.size()); + + // // Second test: Random start and goal poses + // srand (time(NULL)); + // unsigned int route_legs = 0; + // start = node->now(); + // for (unsigned int i = 0; i != NUM_TESTS; i++) { + // unsigned int start = rand() % DIM; + // unsigned int goal = rand() % DIM; + // while (start == goal) { + // goal = rand() % DIM; + // } + // route = planner.findRoute(graph, start, goal, blocked_ids); + // route_legs += route.edges.size(); + // } + // end = node->now(); + // RCLCPP_INFO( + // node->get_logger(), + // "Random planning took %0.5f milliseconds.", + // (end - start).seconds() * 1000.0 / static_cast(NUM_TESTS)); + // RCLCPP_INFO( + // node->get_logger(), + // "Averaged route size: %f", static_cast(route_legs) / static_cast(NUM_TESTS)) + + // Third test: + // Check how much time it takes to do random lookups in the Kd-tree of various graph sizes + std::shared_ptr kd_tree = std::make_shared(); + kd_tree->computeTree(graph); + + auto start = node->now(); + unsigned int seed = 1u; + for (unsigned int i = 0; i != NUM_TESTS; i++) { + std::vector kd_tree_idxs; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast(rand_r(&seed) % DIM); + pose.pose.position.y = static_cast(rand_r(&seed) % DIM); + kd_tree->findNearestGraphNodesToPose(pose, kd_tree_idxs); + } + auto end = node->now(); + RCLCPP_INFO( + node->get_logger(), + "Finding the nodes in the K-d tree took %0.5f milliseconds.", + (end - start).seconds() * 1000.0 / static_cast(NUM_TESTS)); + + return 0; +} diff --git a/nav2_route/test/test_collision_operation.cpp b/nav2_route/test/test_collision_operation.cpp new file mode 100644 index 00000000000..bdb10de1c6f --- /dev/null +++ b/nav2_route/test/test_collision_operation.cpp @@ -0,0 +1,333 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "nav2_route/operations_manager.hpp" +#include "nav2_route/types.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/plugins/route_operations/collision_monitor.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +class CollisionMonitorWrapper : public CollisionMonitor +{ +public: + CollisionMonitorWrapper() = default; + + Coordinates backoutValidEndPointWrapper( + const Coordinates & start, const Coordinates & end, const float dist) + { + return backoutValidEndPoint(start, end, dist); + } + + bool lineToMapWrapper( + const Coordinates & start, const Coordinates & end, LineSegment & line) + { + return lineToMap(start, end, line); + } + + bool backoutValidEndPointWrapper( + const Coordinates & start, LineSegment & line) + { + return backoutValidEndPoint(start, line); + } + + bool isInCollisionWrapper(const LineSegment & line) + { + return isInCollision(line); + } + + void getCostmapWrapper() + { + getCostmap(); + } +}; + +TEST(TestCollisionMonitor, test_lifecycle) +{ + auto node = std::make_shared("test"); + node->declare_parameter("costmap_topic", rclcpp::ParameterValue("dummy_topic")); + CollisionMonitor monitor; + std::shared_ptr costmap_subscriber; + monitor.configure(node, costmap_subscriber, "name"); + EXPECT_EQ(monitor.getName(), std::string("name")); + EXPECT_EQ(monitor.processType(), RouteOperationType::ON_QUERY); +} + +TEST(TestCollisionMonitor, test_geometric_backout_vector) +{ + auto node = std::make_shared("test"); + node->declare_parameter("costmap_topic", rclcpp::ParameterValue("local_costmap/costmap_raw")); + node->declare_parameter("name.max_collision_dist", rclcpp::ParameterValue(-1.0)); + std::shared_ptr costmap_subscriber; + CollisionMonitorWrapper monitor; + monitor.configure(node, costmap_subscriber, "name"); + + geometry_msgs::msg::PoseStamped pose; + Coordinates start, end; + start.x = 0.0; + start.y = 0.0; + end.x = 10.0; + end.y = 0.0; + float dist = 2.0; + + // Test with straight X / Y changes + Coordinates rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 2.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + dist = 5.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + start.x = 0.0; + start.y = 0.0; + end.x = 0.0; + end.y = 1.0; + + dist = 0.1; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.1, 0.01); + + dist = 15.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 15.0, 0.01); + + // Now that we know the basis work, let's try a none vector + start.x = 0.0; + start.y = 10.0; + end.x = 0.0; + end.y = 10.0; + dist = 5.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 10.0, 0.01); + + // Now an x=y vector + start.x = 0.0; + start.y = 0.0; + end.x = 10.0; + end.y = 10.0; + dist = 5.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 3.53, 0.01); + EXPECT_NEAR(rtn.y, 3.53, 0.01); + + // Let's try a random vector that I solved by hand + start.x = 4.0; + start.y = 10.0; + end.x = 50.0; + end.y = 100.0; + dist = 45.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 24.479, 0.01); + EXPECT_NEAR(rtn.y, 50.069, 0.01); +} + + +TEST(TestCollisionMonitor, test_costmap_apis) +{ + auto node = std::make_shared("test"); + node->declare_parameter("costmap_topic", rclcpp::ParameterValue("dummy_topic")); + auto node_thread = std::make_unique(node); + std::shared_ptr costmap_subscriber; + CollisionMonitorWrapper monitor; + monitor.configure(node, costmap_subscriber, "name"); + + // No costmap received yet + EXPECT_THROW(monitor.getCostmapWrapper(), nav2_core::OperationFailed); + + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + nav2_costmap_2d::Costmap2D * costmap = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + nav2_costmap_2d::Costmap2DPublisher publisher( + node, costmap, "map", "local_costmap/costmap", true); + publisher.on_activate(); + publisher.publishCostmap(); + + // Give it a moment to receive the costmap + rclcpp::Rate r(10); + r.sleep(); + monitor.getCostmapWrapper(); // Since would otherwise be called in `perform` + + Coordinates start, end; + start.x = 1.0; + start.y = 1.0; + end.x = 10.0; + end.y = 10.0; + CollisionMonitor::LineSegment line; + EXPECT_TRUE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_EQ(line.x0, 9u); + EXPECT_EQ(line.y0, 9u); + EXPECT_EQ(line.x1, 99u); + EXPECT_EQ(line.y1, 99u); + + end.x = 11.0; + end.y = 11.0; + EXPECT_FALSE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_EQ(line.x0, 9u); + EXPECT_EQ(line.y0, 9u); + EXPECT_EQ(line.x1, 109u); + EXPECT_EQ(line.y1, 109u); + + // Should backout to the costmap edge + EXPECT_TRUE(monitor.backoutValidEndPointWrapper(start, line)); + EXPECT_EQ(line.x1, 99u); + EXPECT_EQ(line.y1, 99u); + + // In this case, even starting point is invalid, reject + start.x = -1.0; + start.y = -1.0; + EXPECT_FALSE(monitor.backoutValidEndPointWrapper(start, line)); + + // Check collision checker + start.x = 1.0; + start.y = 1.0; + end.x = 9.0; + end.y = 1.0; + EXPECT_TRUE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_FALSE(monitor.isInCollisionWrapper(line)); + + start.x = 1.0; + start.y = 1.0; + end.x = 1.0; + end.y = 9.0; + EXPECT_TRUE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_FALSE(monitor.isInCollisionWrapper(line)); + + start.x = 1.0; + start.y = 1.0; + end.x = 9.0; + end.y = 9.0; + EXPECT_TRUE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_TRUE(monitor.isInCollisionWrapper(line)); + + // Sanity check via a realistic graph route + Node node1, node2, node3; + node1.coords.x = 1.0; // lower left corner + node1.coords.y = 1.0; + node2.coords.x = 1.0; // upper left corner + node2.coords.y = 9.0; + node3.coords.x = 11.0; // lower right corner, through center (collision), off edge + node3.coords.y = -1.0; + DirectionalEdge edge1, edge2; + edge1.start = &node1; + edge1.end = &node2; + edge2.start = &node2; + edge2.end = &node3; + edge1.edgeid = 1u; + edge2.edgeid = 2u; + + Route route; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + + geometry_msgs::msg::PoseStamped pose; + EdgePtr curr_edge = &edge1; + pose.pose.position.x = 1.0; + pose.pose.position.y = 2.0; + + // We are along the first edge, outside of 5m check from costmap lethal block + // Need to wait long enough between calls due to collision monitor throttling + OperationResult result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_FALSE(result.reroute); + rclcpp::Rate r1(0.8); + r1.sleep(); + + // Still along first bit of the route, 5m ahead still not in lethal block + pose.pose.position.x = 1.0; + pose.pose.position.y = 5.0; + result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_FALSE(result.reroute); + rclcpp::Rate r2(0.8); + r2.sleep(); + + // Now should be within range of lethal block + pose.pose.position.x = 1.0; + pose.pose.position.y = 9.0; + result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_TRUE(result.reroute); + EXPECT_EQ(result.blocked_ids.size(), 1u); + EXPECT_EQ(result.blocked_ids[0], 2u); + + // But should be not reporting now if we immediately try again due + // to not meeting the timer needs, even if we give it a better edge to use + pose.pose.position.x = 1.0; + pose.pose.position.y = 9.0; + curr_edge = &edge2; + result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.blocked_ids.size(), 0u); + + rclcpp::Rate r3(0.8); + r3.sleep(); + + // Finally let's try once more after the collision block to make sure handles terminal situations + pose.pose.position.x = 8.0; + pose.pose.position.y = 2.0; + curr_edge = &edge2; + result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.blocked_ids.size(), 0u); +} diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp new file mode 100644 index 00000000000..03abe64b788 --- /dev/null +++ b/nav2_route/test/test_edge_scorers.cpp @@ -0,0 +1,959 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_route/edge_scorer.hpp" +#include "nav2_msgs/srv/dynamic_edges.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(EdgeScorersTest, test_lifecycle) +{ + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); +} + +TEST(EdgeScorersTest, test_api) +{ + // Tests basic API and default behavior. Also covers the DistanceScorer plugin. + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 2); // default DistanceScorer, AdjustEdgesScorer + + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // Because nodes coords are 0/0 + + n1.coords.x = 1.0; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 1.0); // Distance is now 1m + + // For full coverage, add in a speed limit tag to make sure it is applied appropriately + float speed_limit = 0.8f; + edge.metadata.setValue("speed_limit", speed_limit); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 1.25); // 1m / 0.8 = 1.25 +} + +TEST(EdgeScorersTest, test_failed_api) +{ + // Expect failure since plugin does not exist + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"FakeScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "FakeScorer.plugin", rclcpp::ParameterValue(std::string{"FakePluginPath"})); + + std::shared_ptr costmap_subscriber; + EXPECT_THROW( + EdgeScorer scorer(node, tf_buffer, costmap_subscriber), pluginlib::PluginlibException); +} + +TEST(EdgeScorersTest, test_invalid_edge_scoring) +{ + // Test API for the edge scorer to maintain proper state when a plugin + // rejects and edge. Also covers the DynamicEdgesScorer plugin to demonstrate. + auto node = std::make_shared("route_server"); + auto node_thread = std::make_unique(node); + auto node2 = std::make_shared("my_node2"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"DynamicEdgesScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "DynamicEdgesScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::DynamicEdgesScorer"})); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // AdjustEdgesScorer + + // Send service to set an edge as invalid + auto srv_client = + nav2_util::ServiceClient( + "route_server/DynamicEdgesScorer/adjust_edges", node2); + auto req = std::make_shared(); + req->closed_edges.push_back(10u); + req->adjust_edges.resize(1); + req->adjust_edges[0].edgeid = 11u; + req->adjust_edges[0].cost = 42.0; + auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp->success); + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // The score function should return false since closed + float traversal_cost = -1; + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + // The score function should return true since no longer the problematic edge ID + // and edgeid 42 as the dynamic cost of 42 assigned to it + traversal_cost = -1; + edge.edgeid = 11; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 42.0); + + // Try to re-open this edge + auto req2 = std::make_shared(); + req2->opened_edges.push_back(10u); + auto resp2 = srv_client.invoke(req2, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp2->success); + + // The score function should return true since now opened up + traversal_cost = -1; + edge.edgeid = 10; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + node_thread.reset(); +} + +TEST(EdgeScorersTest, test_penalty_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"PenaltyScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "PenaltyScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::PenaltyScorer"})); + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // PenaltyScorer + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + float penalty = 10.0f; + edge.metadata.setValue("penalty", penalty); + + // The score function should return 10.0 from penalty value + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 10.0); +} + +TEST(EdgeScorersTest, test_costmap_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + node->declare_parameter("costmap_topic", "dummy_topic"); + auto node_thread = std::make_unique(node); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"CostmapScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "CostmapScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::CostmapScorer"})); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // CostmapScorer + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // The score function should return false because no costmap given + float traversal_cost = -1; + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + nav2_costmap_2d::Costmap2D * costmap = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + nav2_costmap_2d::Costmap2DPublisher publisher( + node, costmap, "map", "global_costmap/costmap", true); + publisher.on_activate(); + publisher.publishCostmap(); + + // Give it a moment to receive the costmap + rclcpp::Rate r(10); + r.sleep(); + + n1.coords.x = 5.0; + n1.coords.y = 8.0; + n2.coords.x = 8.0; + n2.coords.y = 8.0; + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + // Segment in freespace + EXPECT_EQ(traversal_cost, 0.0); + + n1.coords.x = 2.0; + n1.coords.y = 2.0; + n2.coords.x = 2.0; + n2.coords.y = 8.0; + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + // Segment in 100 space + EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); + + n1.coords.x = 4.1; + n1.coords.y = 4.1; + n2.coords.x = 5.9; + n2.coords.y = 5.9; + traversal_cost = -1; + // Segment in lethal space, won't fill in + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + n1.coords.x = 1.0; + n1.coords.y = 1.0; + n2.coords.x = 6.0; + n2.coords.y = 1.0; + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + // Segment in 0 and 100 space, use_max so 100 (normalized) + EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); + + n1.coords.x = -1.0; + n1.coords.y = -1.0; + n2.coords.x = 11.0; + n2.coords.y = 11.0; + traversal_cost = -1; + // Off map, so invalid + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + node_thread.reset(); +} + +TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + node->declare_parameter("costmap_topic", "dummy_costmap/costmap_raw"); + auto node_thread = std::make_unique(node); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"CostmapScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "CostmapScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::CostmapScorer"})); + node->declare_parameter( + "CostmapScorer.use_maximum", rclcpp::ParameterValue(false)); + node->declare_parameter( + "CostmapScorer.invalid_on_collision", rclcpp::ParameterValue(false)); + node->declare_parameter( + "CostmapScorer.invalid_off_map", rclcpp::ParameterValue(false)); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // CostmapScorer + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + nav2_costmap_2d::Costmap2D * costmap = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + nav2_costmap_2d::Costmap2DPublisher publisher( + node, costmap, "map", "global_costmap/costmap", true); + publisher.on_activate(); + publisher.publishCostmap(); + + // Give it a moment to receive the costmap + rclcpp::Rate r(1); + r.sleep(); + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // Off map + n1.coords.x = -1.0; + n1.coords.y = -1.0; + n2.coords.x = 11.0; + n2.coords.y = 11.0; + float traversal_cost = -1; + // Off map, so cannot score + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + n1.coords.x = 4.1; + n1.coords.y = 4.1; + n2.coords.x = 5.9; + n2.coords.y = 5.9; + traversal_cost = -1; + // Segment in lethal space, so score is maximum (1) + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, 1.0, 0.01); + + n1.coords.x = 1.0; + n1.coords.y = 1.0; + n2.coords.x = 6.0; + n2.coords.y = 1.0; + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + // Segment in 0 and 100 space, 3m @ 100, 2m @ 0, averaged is 60 + EXPECT_NEAR(traversal_cost, 60.0 / 254.0, 0.01); + + node_thread.reset(); +} + +TEST(EdgeScorersTest, test_time_scoring) +{ + // Test Time scorer plugin loading + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"TimeScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "TimeScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TimeScorer"})); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // TimeScorer + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + float time_taken = 10.0f; + edge.metadata.setValue("abs_time_taken", time_taken); + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // The score function should return 10.0 from time taken + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight + + // Without time taken or abs speed limit set, uses default max speed of 0.5 m/s + edge.metadata.data.clear(); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 2.0); // 1.0 m / 0.5 m/s * 1.0 weight + + // Use speed limit if set + float speed_limit = 0.85; + edge.metadata.setValue("abs_speed_limit", speed_limit); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, 1.1764, 0.001); // 1.0 m / 0.85 m/s * 1.0 weight + + // Still use time taken measurements if given first + edge.metadata.setValue("abs_time_taken", time_taken); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight +} + +TEST(EdgeScorersTest, test_semantic_scoring_key) +{ + // Test Time scorer plugin loading + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"SemanticScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::SemanticScorer"})); + + std::vector classes; + classes.push_back("Test"); + classes.push_back("Test1"); + classes.push_back("Test2"); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.semantic_classes", + rclcpp::ParameterValue(classes)); + + for (unsigned int i = 0; i != classes.size(); i++) { + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer." + classes[i], + rclcpp::ParameterValue(static_cast(i))); + } + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // SemanticScorer + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + // Should fail, since both nothing under key `class` nor metadata set at all + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // nothing is set in semantics + + // Should be valid under the right key + std::string test_n = "Test1"; + edge.metadata.setValue("class", test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 1.0); // 1.0 * 1.0 weight + + test_n = "Test2"; + edge.metadata.setValue("class", test_n); + n2.metadata.setValue("class", test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight + + // Cannot find, doesn't exist + test_n = "Test4"; + edge.metadata.setValue("class", test_n); + n2.metadata.setValue("class", test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight +} + +TEST(EdgeScorersTest, test_semantic_scoring_keys) +{ + // Test Time scorer plugin loading + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"SemanticScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::SemanticScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.semantic_key", + rclcpp::ParameterValue(std::string{""})); + + std::vector classes; + classes.push_back("Test"); + classes.push_back("Test1"); + classes.push_back("Test2"); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.semantic_classes", + rclcpp::ParameterValue(classes)); + + for (unsigned int i = 0; i != classes.size(); i++) { + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer." + classes[i], + rclcpp::ParameterValue(static_cast(i))); + } + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // SemanticScorer + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // Should fail, since both nothing under key `class` nor metadata set at all + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // nothing is set in semantics + + // Should fail, since under the class key when the semantic key is empty string + // so it will look for the keys themselves + std::string test_n = "Test1"; + edge.metadata.setValue("class", test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight + + // Should succeed, since now actual class is a key, not a value of the `class` key + test_n = "Test2"; + edge.metadata.setValue(test_n, test_n); + n2.metadata.setValue(test_n, test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight + + // Cannot find, doesn't exist + edge.metadata.data.clear(); + n2.metadata.data.clear(); + test_n = "Test4"; + edge.metadata.setValue(test_n, test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight +} + +TEST(EdgeScorersTest, test_goal_orientation_threshold) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.use_orientation_threshold", + rclcpp::ParameterValue(true)); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + RouteRequest route_request; + route_request.goal_pose = goal_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::END; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + + traversal_cost = -1; + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + route_request.use_poses = false; + + EXPECT_THROW( + scorer.score( + &edge, route_request, edge_type, + traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + +TEST(EdgeScorersTest, test_goal_orientation_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + double orientation_weight = 100.0; + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.use_orientation_thershold", + rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.orientation_weight", + rclcpp::ParameterValue(orientation_weight)); + + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + RouteRequest route_request; + route_request.goal_pose = goal_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::END; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, orientation_weight * M_PI, 0.001); + + route_request.use_poses = false; + + EXPECT_THROW( + scorer.score( + &edge, route_request, edge_type, + traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + +TEST(EdgeScorersTest, test_start_pose_orientation_threshold) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer = std::make_shared(node->get_clock()); + std::shared_ptr tf_listener = + std::make_shared(*tf_buffer); + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.use_orientation_threshold", + rclcpp::ParameterValue(true)); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + double yaw = 0.0; + + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + start_pose.header.frame_id = "map"; + start_pose.header.stamp = node->get_clock()->now(); + + start_pose.pose.position.x = 0.0; + start_pose.pose.position.x = 0.0; + start_pose.pose.position.z = 0.0; + + start_pose.pose.orientation.x = q.getX(); + start_pose.pose.orientation.y = q.getY(); + start_pose.pose.orientation.z = q.getZ(); + start_pose.pose.orientation.w = q.getW(); + + RouteRequest route_request; + route_request.start_pose = start_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::START; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + route_request.use_poses = false; + + EXPECT_THROW( + scorer.score( + &edge, route_request, edge_type, + traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + +TEST(EdgeScorersTest, test_start_pose_orientation_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer = std::make_shared(node->get_clock()); + std::shared_ptr tf_listener = + std::make_shared(*tf_buffer); + + double orientation_weight = 100.0; + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.use_orientation_thershold", + rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_weight", + rclcpp::ParameterValue(orientation_weight)); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + double yaw = 0.0; + + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + start_pose.header.frame_id = "map"; + start_pose.header.stamp = node->get_clock()->now(); + + start_pose.pose.position.x = 0.0; + start_pose.pose.position.x = 0.0; + start_pose.pose.position.z = 0.0; + + start_pose.pose.orientation.x = q.getX(); + start_pose.pose.orientation.y = q.getY(); + start_pose.pose.orientation.z = q.getZ(); + start_pose.pose.orientation.w = q.getW(); + + + RouteRequest route_request; + route_request.start_pose = start_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::START; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, orientation_weight * M_PI, 0.001); + + route_request.use_poses = false; + + EXPECT_THROW( + scorer.score( + &edge, route_request, edge_type, + traversal_cost), nav2_core::InvalidEdgeScorerUse); +} diff --git a/nav2_route/test/test_geojson_graph_file_loader.cpp b/nav2_route/test/test_geojson_graph_file_loader.cpp new file mode 100644 index 00000000000..ae7c3cc3ac5 --- /dev/null +++ b/nav2_route/test/test_geojson_graph_file_loader.cpp @@ -0,0 +1,388 @@ +// Copyright (c) 2025 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav2_util/node_utils.hpp" +#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT +using Json = nlohmann::json; + +Json g_simple_graph = nlohmann::json::parse( + R"( + { + "features": [ + { + "type": "Feature", + "properties": + { + "id": 0, + "frame": "map" + }, + "geometry": + { "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { + "type": "Feature", + "properties": + { + "id": 1, + "frame": "map" + }, + "geometry": + { + "type":"Point", + "coordinates": [ 1.0, 0.0 ] + } + }, + { + "type": "Feature", + "properties": + { + "id": 2, + "startid": 0, + "endid": 1 + }, + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + } + ] + } + )"); + +void writeGraphToFile(const Json & graph, const std::string & file_path = "graph.geojson") +{ + std::ofstream missing_id_file(file_path); + missing_id_file << graph; + missing_id_file.close(); +} + +TEST(GeoJsonGraphFileLoader, test_file_does_not_exist) +{ + Graph graph; + GraphToIDMap graph_to_id_map; + std::string file_path; + + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_FALSE(result); +} + +TEST(GeoJsonGraphFileLoader, test_no_nodes_in_graph) +{ + Json json_graph = g_simple_graph; + + auto & features = json_graph["features"]; + features.erase(features.begin(), features.begin() + 2); + + std::string file_path = "no_nodes.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_FALSE(result); + std::filesystem::remove(file_path); +} + +TEST(GeoJsonGraphFileLoader, test_no_edges_in_graph) +{ + Json json_graph = g_simple_graph; + auto & features = json_graph["features"]; + features.erase(2); + + std::string file_path = "no_edges.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_FALSE(result); + std::filesystem::remove(file_path); +} + +TEST(GeoJsonGraphFileLoader, test_missing_node_id) +{ + Json json_graph = g_simple_graph; + auto & properties = json_graph["features"][0]["properties"]; + properties.erase("id"); + + std::string file_path = "missing_id.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + EXPECT_THROW( + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path), + Json::exception); + std::filesystem::remove(file_path); +} + +TEST(GeoJsonGraphFileLoader, test_invalid_start_id_for_edge) +{ + Json json_graph = g_simple_graph; + auto & properties = json_graph["features"][2]["properties"]; + properties["startid"] = 100; + + std::string file_path = "invalid_start_id.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + EXPECT_THROW( + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path), + std::runtime_error); +} + +TEST(GeoJsonGraphFileLoader, test_invalid_goal_id_for_edge) +{ + Json json_graph = g_simple_graph; + auto & properties = json_graph["features"][2]["properties"]; + properties["endid"] = 1000; + + std::string file_path = "invalid_goal_id.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + EXPECT_THROW( + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path), + std::runtime_error); +} + +TEST(GeoJsonGraphFileLoader, test_node_metadata) { + Json json_graph = g_simple_graph; + + Json node_metadata = nlohmann::json::parse( + R"( + { + "metadata": + { + "person": + { + "name": "josh", + "can_drive": true, + "top_speed": 0.85, + "age": 29 + }, + "array": [0.0, 1.0, 2.0] + } + } +)"); + Json edge_metadata = nlohmann::json::parse( + R"( + { + "metadata": + { + "color": "green" + } + } +)"); + + json_graph["features"][0]["properties"].insert(node_metadata.begin(), node_metadata.end()); + json_graph["features"][2]["properties"].insert(edge_metadata.begin(), edge_metadata.end()); + + std::string file_path = "metadata.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + std::filesystem::remove(file_path); + + // Check node metadata + Metadata metadata; + metadata = graph[0].metadata.getValue("person", metadata); + + std::string name; + name = metadata.getValue("name", name); + EXPECT_EQ(name, node_metadata["metadata"]["person"]["name"]); + + bool can_drive = false; + can_drive = metadata.getValue("can_drive", can_drive); + EXPECT_EQ(can_drive, node_metadata["metadata"]["person"]["can_drive"]); + + float top_speed = 0.0f; + top_speed = metadata.getValue("top_speed", top_speed); + EXPECT_NEAR(top_speed, node_metadata["metadata"]["person"]["top_speed"], 1e-6); + + unsigned int age = 0; + age = metadata.getValue("age", age); + EXPECT_EQ(age, node_metadata["metadata"]["person"]["age"]); + + std::vector array; + array = graph[0].metadata.getValue("array", array); + EXPECT_EQ(array.size(), 3u); + + EXPECT_EQ(std::any_cast(array[0]), 0.0f); + EXPECT_EQ(std::any_cast(array[1]), 1.0f); + EXPECT_EQ(std::any_cast(array[2]), 2.0f); + + // Check edge metadata + std::string color; + color = graph[0].neighbors[0].metadata.getValue("color", color); + EXPECT_EQ(color, edge_metadata["metadata"]["color"]); +} + +TEST(GeoJsonGraphFileLoader, operations) +{ + Json json_graph = g_simple_graph; + + Json json_operation = nlohmann::json::parse( + R"( + { + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_EXIT", + "metadata": + { + "color": "green" + } + } + } + } +)"); + + json_graph["features"][0]["properties"].insert(json_operation.begin(), json_operation.end()); + + std::string file_path = "operations.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + std::filesystem::remove(file_path); + + // Check node operations + Operations node_operations = graph[0].operations; + + EXPECT_EQ(node_operations.size(), 1u); + + EXPECT_EQ(node_operations[0].type, "open_door"); + EXPECT_EQ(node_operations[0].trigger, OperationTrigger::ON_EXIT); + + std::string color; + color = node_operations[0].metadata.getValue("color", color); + EXPECT_EQ(color, "green"); +} + +TEST(GeoJsonGraphFileLoader, simple_graph) +{ + std::string file_path = "simple_graph.geojson"; + + writeGraphToFile(g_simple_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + std::filesystem::remove(file_path); + + EXPECT_EQ(graph.size(), 2u); + + // Node 1 + EXPECT_EQ(graph[0].nodeid, 0u); + EXPECT_EQ(graph[0].coords.x, 0.0); + EXPECT_EQ(graph[0].coords.x, 0.0); + EXPECT_EQ(graph[0].coords.frame_id, "map"); + EXPECT_EQ(graph[0].neighbors.size(), 1u); + EXPECT_EQ(graph_to_id_map[0], graph[0].nodeid); + + // Node 2 + EXPECT_EQ(graph[1].nodeid, 1u); + EXPECT_EQ(graph[1].coords.x, 1.0); + EXPECT_EQ(graph[1].coords.y, 0.0); + EXPECT_EQ(graph[1].coords.frame_id, "map"); + EXPECT_EQ(graph[1].neighbors.size(), 0u); + EXPECT_EQ(graph_to_id_map[1], graph[1].nodeid); + + // Edge from node 1 -> 2 + EXPECT_EQ(graph[0].neighbors[0].edgeid, 2u); + EXPECT_EQ(graph[0].neighbors[0].start, &graph[0]); + EXPECT_EQ(graph[0].neighbors[0].end, &graph[1]); + EXPECT_TRUE(graph[0].neighbors[0].edge_cost.overridable); + EXPECT_EQ(graph[0].neighbors[0].edge_cost.cost, 0.0f); +} + +TEST(GeoJsonGraphFileLoader, sample_graph) +{ + auto file_path = ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/sample_graph.geojson"; + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + + Metadata region; + region = graph[0].metadata.getValue("region", region); + EXPECT_EQ(region.data.size(), 3u); + + std::vector x_values; + x_values = region.getValue("x_values", x_values); + EXPECT_EQ(x_values.size(), 4u); + + EXPECT_EQ(graph[0].neighbors[0].edge_cost.cost, 10.0f); + EXPECT_EQ(graph[0].neighbors[0].edge_cost.overridable, false); + + auto & operations = graph[0].neighbors[0].operations; + + EXPECT_EQ(operations[0].type, "open_door"); + + std::string type; + type = operations[1].metadata.getValue("type", type); + EXPECT_EQ(type, "jpg"); +} + +TEST(GeoJsonGraphFileLoader, invalid_file) +{ + auto file_path = ament_index_cpp::get_package_share_directory("nav2_route") + + "/test/test_graphs/invalid.json"; + GeoJsonGraphFileLoader graph_file_loader; + Graph graph; + GraphToIDMap graph_to_id_map; + EXPECT_FALSE(graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path)); +} diff --git a/nav2_route/test/test_geojson_graph_file_saver.cpp b/nav2_route/test/test_geojson_graph_file_saver.cpp new file mode 100644 index 00000000000..249dbea2ab2 --- /dev/null +++ b/nav2_route/test/test_geojson_graph_file_saver.cpp @@ -0,0 +1,354 @@ +// Copyright (c) 2025 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav2_util/node_utils.hpp" +#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" +#include "nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT +using Json = nlohmann::json; + +Json g_simple_graph = nlohmann::json::parse( + R"( + { + "features": [ + { + "type": "Feature", + "properties": + { + "id": 0, + "frame": "map" + }, + "geometry": + { "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { + "type": "Feature", + "properties": + { + "id": 1, + "frame": "map" + }, + "geometry": + { + "type":"Point", + "coordinates": [ 1.0, 0.0 ] + } + }, + { + "type": "Feature", + "properties": + { + "id": 2, + "startid": 0, + "endid": 1 + }, + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + } + ] + } + )"); + +void writeGraphToFile(const Json & graph, const std::string & file_path = "graph.geojson") +{ + std::ofstream missing_id_file(file_path); + missing_id_file << graph; + missing_id_file.close(); +} + +TEST(GeoJsonGraphFileSaver, test_file_empty) +{ + Graph graph; + std::string file_path; + + GeoJsonGraphFileSaver graph_file_saver; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_FALSE(result); +} + +TEST(GeoJsonGraphFileSaver, test_node_metadata) { + Json json_graph = g_simple_graph; + + Json node_metadata = nlohmann::json::parse( + R"( + { + "metadata": + { + "person": + { + "name": "josh", + "can_drive": true, + "top_speed": 0.85, + "age": 29, + "altitude": -10 + }, + "double_array": [0.0, 1.0, 2.0], + "int_array": [0, 1, 2], + "string_array": ["a", "b", "c"], + "bool_array": [true, false, true] + } + } +)"); + Json edge_metadata = nlohmann::json::parse( + R"( + { + "metadata": + { + "color": "green" + } + } +)"); + + json_graph["features"][0]["properties"].insert(node_metadata.begin(), node_metadata.end()); + json_graph["features"][2]["properties"].insert(edge_metadata.begin(), edge_metadata.end()); + + std::string file_path = "metadata.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + GeoJsonGraphFileSaver graph_file_saver; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_TRUE(result); + Graph graph2; + GraphToIDMap graph_to_id_map2; + graph_file_loader.loadGraphFromFile(graph2, graph_to_id_map2, file_path); + EXPECT_EQ(graph.size(), graph2.size()); + for (size_t i = 0; i < graph.size(); ++i) { + EXPECT_EQ(graph[i].nodeid, graph2[i].nodeid); + EXPECT_EQ(graph[i].coords.x, graph2[i].coords.x); + EXPECT_EQ(graph[i].coords.y, graph2[i].coords.y); + EXPECT_EQ(graph[i].coords.frame_id, graph2[i].coords.frame_id); + EXPECT_EQ(graph[i].neighbors.size(), graph2[i].neighbors.size()); + for (size_t j = 0; j < graph[i].neighbors.size(); ++j) { + EXPECT_EQ(graph[i].neighbors[j].edgeid, graph2[i].neighbors[j].edgeid); + EXPECT_EQ(graph[i].neighbors[j].start->nodeid, graph2[i].neighbors[j].start->nodeid); + EXPECT_EQ(graph[i].neighbors[j].end->nodeid, graph2[i].neighbors[j].end->nodeid); + EXPECT_EQ(graph[i].neighbors[j].edge_cost.cost, graph2[i].neighbors[j].edge_cost.cost); + EXPECT_EQ( + graph[i].neighbors[j].edge_cost.overridable, + graph2[i].neighbors[j].edge_cost.overridable); + } + } + EXPECT_EQ(graph_to_id_map.size(), graph_to_id_map2.size()); + for (const auto & pair : graph_to_id_map) { + EXPECT_EQ(pair.first, graph_to_id_map2[pair.second]); + } + // Check node metadata + Metadata metadata; + metadata = graph2[0].metadata.getValue("person", metadata); + + std::string name; + name = metadata.getValue("name", name); + EXPECT_EQ(name, node_metadata["metadata"]["person"]["name"]); + + bool can_drive = false; + can_drive = metadata.getValue("can_drive", can_drive); + EXPECT_EQ(can_drive, node_metadata["metadata"]["person"]["can_drive"]); + + float top_speed = 0.0f; + top_speed = metadata.getValue("top_speed", top_speed); + EXPECT_NEAR(top_speed, node_metadata["metadata"]["person"]["top_speed"], 1e-6); + + unsigned int age = 0; + age = metadata.getValue("age", age); + EXPECT_EQ(age, node_metadata["metadata"]["person"]["age"]); + + std::vector array; + array = graph2[0].metadata.getValue("double_array", array); + EXPECT_EQ(array.size(), 3u); + + EXPECT_EQ(std::any_cast(array[0]), 0.0f); + EXPECT_EQ(std::any_cast(array[1]), 1.0f); + EXPECT_EQ(std::any_cast(array[2]), 2.0f); + + // Check edge metadata + std::string color; + color = graph2[0].neighbors[0].metadata.getValue("color", color); + EXPECT_EQ(color, edge_metadata["metadata"]["color"]); + std::filesystem::remove(file_path); +} + +TEST(GeoJsonGraphFileSaver, operations) +{ + Json json_graph = g_simple_graph; + + Json json_operation = nlohmann::json::parse( + R"( + { + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_EXIT", + "metadata": + { + "color": "green" + } + } + } + } +)"); + + json_graph["features"][0]["properties"].insert(json_operation.begin(), json_operation.end()); + + std::string file_path = "operations.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + GeoJsonGraphFileSaver graph_file_saver; + Graph graph2; + GraphToIDMap graph_to_id_map2; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_TRUE(result); + graph_file_loader.loadGraphFromFile(graph2, graph_to_id_map2, file_path); + EXPECT_EQ(graph.size(), graph2.size()); + for (size_t i = 0; i < graph.size(); ++i) { + EXPECT_EQ(graph[i].nodeid, graph2[i].nodeid); + EXPECT_EQ(graph[i].coords.x, graph2[i].coords.x); + EXPECT_EQ(graph[i].coords.y, graph2[i].coords.y); + EXPECT_EQ(graph[i].coords.frame_id, graph2[i].coords.frame_id); + EXPECT_EQ(graph[i].neighbors.size(), graph2[i].neighbors.size()); + for (size_t j = 0; j < graph[i].neighbors.size(); ++j) { + EXPECT_EQ(graph[i].neighbors[j].edgeid, graph2[i].neighbors[j].edgeid); + EXPECT_EQ(graph[i].neighbors[j].start->nodeid, graph2[i].neighbors[j].start->nodeid); + EXPECT_EQ(graph[i].neighbors[j].end->nodeid, graph2[i].neighbors[j].end->nodeid); + EXPECT_EQ(graph[i].neighbors[j].edge_cost.cost, graph2[i].neighbors[j].edge_cost.cost); + EXPECT_EQ( + graph[i].neighbors[j].edge_cost.overridable, + graph2[i].neighbors[j].edge_cost.overridable); + } + } + EXPECT_EQ(graph_to_id_map.size(), graph_to_id_map2.size()); + for (const auto & pair : graph_to_id_map) { + EXPECT_EQ(pair.first, graph_to_id_map2[pair.second]); + } + std::filesystem::remove(file_path); + + // Check node operations + Operations node_operations = graph2[0].operations; + + EXPECT_EQ(node_operations.size(), 1u); + + EXPECT_EQ(node_operations[0].type, "open_door"); + EXPECT_EQ(node_operations[0].trigger, OperationTrigger::ON_EXIT); + + std::string color; + color = node_operations[0].metadata.getValue("color", color); + EXPECT_EQ(color, "green"); +} + +TEST(GeoJsonGraphFileSaver, simple_graph) +{ + std::string file_path = "simple_graph.geojson"; + + writeGraphToFile(g_simple_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + GeoJsonGraphFileSaver graph_file_saver; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_TRUE(result); + Graph graph2; + GraphToIDMap graph_to_id_map2; + graph_file_loader.loadGraphFromFile(graph2, graph_to_id_map2, file_path); + std::filesystem::remove(file_path); + + // Node 1 + EXPECT_EQ(graph2[0].nodeid, 0u); + EXPECT_EQ(graph2[0].coords.x, 0.0); + EXPECT_EQ(graph2[0].coords.x, 0.0); + EXPECT_EQ(graph2[0].coords.frame_id, "map"); + EXPECT_EQ(graph2[0].neighbors.size(), 1u); + EXPECT_EQ(graph_to_id_map2[0], graph2[0].nodeid); + + // Node 2 + EXPECT_EQ(graph2[1].nodeid, 1u); + EXPECT_EQ(graph2[1].coords.x, 1.0); + EXPECT_EQ(graph2[1].coords.y, 0.0); + EXPECT_EQ(graph2[1].coords.frame_id, "map"); + EXPECT_EQ(graph2[1].neighbors.size(), 0u); + EXPECT_EQ(graph_to_id_map2[1], graph2[1].nodeid); + + // Edge from node 1 -> 2 + EXPECT_EQ(graph2[0].neighbors[0].edgeid, 2u); + EXPECT_EQ(graph2[0].neighbors[0].start, &graph2[0]); + EXPECT_EQ(graph2[0].neighbors[0].end, &graph2[1]); + EXPECT_TRUE(graph2[0].neighbors[0].edge_cost.overridable); + EXPECT_EQ(graph2[0].neighbors[0].edge_cost.cost, 0.0f); +} + +TEST(GeoJsonGraphFileSaver, sample_graph) +{ + auto file_path = ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/sample_graph.geojson"; + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + GeoJsonGraphFileSaver graph_file_saver; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_TRUE(result); + Graph graph2; + GraphToIDMap graph_to_id_map2; + graph_file_loader.loadGraphFromFile(graph2, graph_to_id_map2, file_path); + + Metadata region; + region = graph2[0].metadata.getValue("region", region); + EXPECT_EQ(region.data.size(), 3u); + + std::vector x_values; + x_values = region.getValue("x_values", x_values); + EXPECT_EQ(x_values.size(), 4u); + + EXPECT_EQ(graph2[0].neighbors[0].edge_cost.cost, 10.0f); + EXPECT_EQ(graph2[0].neighbors[0].edge_cost.overridable, false); + + auto & operations = graph2[0].neighbors[0].operations; + + EXPECT_EQ(operations[0].type, "open_door"); + + std::string type; + type = operations[1].metadata.getValue("type", type); + EXPECT_EQ(type, "jpg"); +} diff --git a/nav2_route/test/test_goal_intent_extractor.cpp b/nav2_route/test/test_goal_intent_extractor.cpp new file mode 100644 index 00000000000..0ed6827391f --- /dev/null +++ b/nav2_route/test/test_goal_intent_extractor.cpp @@ -0,0 +1,381 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_route/goal_intent_extractor.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + + +class GoalIntentExtractorWrapper : public GoalIntentExtractor +{ +public: + GoalIntentExtractorWrapper() = default; + + // API to set the start/goal poses that would be set normally + // via `findStartandGoal` when called sequentially, so we can test + // `pruneStartandGoal` independent for many cases easily + void setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) + { + start_ = start; + goal_ = goal; + } + + geometry_msgs::msg::PoseStamped getStart() + { + return start_; + } +}; + +TEST(GoalIntentExtractorTest, test_obj_lifecycle) +{ + auto node = std::make_shared("goal_intent_extractor_test"); + GoalIntentExtractor extractor; + Graph graph; + GraphToIDMap id_map; + std::shared_ptr costmap_subscriber = nullptr; + extractor.configure(node, graph, &id_map, nullptr, costmap_subscriber, "map", "base_link"); +} + +TEST(GoalIntentExtractorTest, test_transform_pose) +{ + auto node = std::make_shared("goal_intent_extractor_test"); + auto node_thread = std::make_unique(node); + GoalIntentExtractor extractor; + Graph graph; + GraphToIDMap id_map; + auto tf = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + tf->setCreateTimerInterface(timer_interface); + auto transform_listener = std::make_shared(*tf); + tf2_ros::TransformBroadcaster broadcaster(node); + std::shared_ptr costmap_subscriber = nullptr; + extractor.configure(node, graph, &id_map, tf, costmap_subscriber, "map", "base_link"); + + // Test transformations same frame, should pass + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "map"; + EXPECT_NO_THROW(extractor.transformPose(pose, "map")); + + // Test transformations when nothing on TF buffer of different frames + pose.header.frame_id = "gps"; + EXPECT_THROW(extractor.transformPose(pose, "map"), nav2_core::RouteTFError); + + // Now transforms are available, should work + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.header.stamp = node->now(); + transform.child_frame_id = "gps"; + broadcaster.sendTransform(transform); + EXPECT_NO_THROW(extractor.transformPose(pose, "map")); +} + +TEST(GoalIntentExtractorTest, test_start_goal_finder) +{ + auto node = std::make_shared("goal_intent_extractor_test"); + node->declare_parameter("enable_nn_search", rclcpp::ParameterValue(false)); + auto node_thread = std::make_unique(node); + GoalIntentExtractorWrapper extractor; + Graph graph; + GraphToIDMap id_map; + auto tf = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + tf->setCreateTimerInterface(timer_interface); + + // Make a 3x3 graph of points 0,0 -> 2,2 (ROS logo) + graph.resize(9); + unsigned int idx = 0; + unsigned int ids = 1; + for (unsigned int i = 0; i != 3; i++) { + for (unsigned int j = 0; j != 3; j++) { + graph[idx].nodeid = ids; + graph[idx].coords.x = i; + graph[idx].coords.y = j; + id_map[graph[idx].nodeid] = idx; + idx++; + ids++; + } + } + std::shared_ptr costmap_subscriber = nullptr; + extractor.configure(node, graph, &id_map, tf, costmap_subscriber, "map", "base_link"); + + // Test sending goal and start IDs to search + nav2_msgs::action::ComputeRoute::Goal raw_goal; + raw_goal.start_id = 1; + raw_goal.goal_id = 9; + raw_goal.use_poses = false; + auto goal = std::make_shared(raw_goal); + auto [start1, goal1] = extractor.findStartandGoal(goal); + EXPECT_EQ(start1, 0u); + EXPECT_EQ(goal1, 8u); + + // Set and check reset + geometry_msgs::msg::PoseStamped p1; + p1.pose.position.x = 45.0; + auto start = extractor.getStart(); + EXPECT_EQ(start.pose.position.x, 0.0); + EXPECT_EQ(start.pose.position.y, 0.0); + extractor.overrideStart(p1); + start = extractor.getStart(); + EXPECT_EQ(start.pose.position.x, 45.0); + + // Test sending a goal with start/goal poses to find closest nodes to + raw_goal.start.header.frame_id = "map"; + raw_goal.start.pose.position.x = -1.0; + raw_goal.start.pose.position.y = -1.0; // Closest on graph should be (0,0) -> 0 + raw_goal.goal.header.frame_id = "map"; + raw_goal.goal.pose.position.x = 1.0; + raw_goal.goal.pose.position.y = 1.0; // Closest on graph should be (1,1) -> 5 + raw_goal.use_start = true; + raw_goal.use_poses = true; + goal = std::make_shared(raw_goal); + auto [start2, goal2] = extractor.findStartandGoal(goal); + EXPECT_EQ(start2, 0u); + EXPECT_EQ(goal2, 4u); + + // Test sending a goal without using start (TF lookup failure) + raw_goal.use_start = false; + raw_goal.use_poses = true; + goal = std::make_shared(raw_goal); + EXPECT_THROW(extractor.findStartandGoal(goal), nav2_core::RouteTFError); + + // Try with an empty graph, should fail to find nodes + Graph empty_graph; + GraphToIDMap empty_id_map; + extractor.setGraph(empty_graph, &empty_id_map); + raw_goal.use_start = true; + raw_goal.use_poses = true; + goal = std::make_shared(raw_goal); + EXPECT_THROW(extractor.findStartandGoal(goal), nav2_core::IndeterminantNodesOnGraph); +} + +TEST(GoalIntentExtractorTest, test_pruning) +{ + auto node = std::make_shared("goal_intent_extractor_test"); + GoalIntentExtractorWrapper extractor; + Graph graph; + GraphToIDMap id_map; + std::shared_ptr costmap_subscriber = nullptr; + extractor.configure(node, graph, &id_map, nullptr, costmap_subscriber, "map", "base_link"); + + // Setup goal to use (only uses the use_poses field) + nav2_msgs::action::ComputeRoute::Goal raw_goal; + raw_goal.use_poses = false; + auto no_poses_goal = std::make_shared(raw_goal); + raw_goal.use_poses = true; + auto poses_goal = std::make_shared(raw_goal); + + // Test that we return identical things if not using poses or route is empty of edges + Route routeA; + routeA.edges.resize(10); + ReroutingState rerouting_info; + rerouting_info.first_time = true; + EXPECT_EQ(extractor.pruneStartandGoal(routeA, no_poses_goal, rerouting_info).edges.size(), 10u); + routeA.edges.clear(); + EXPECT_EQ(extractor.pruneStartandGoal(routeA, poses_goal, rerouting_info).edges.size(), 0u); + EXPECT_EQ(extractor.pruneStartandGoal(routeA, no_poses_goal, rerouting_info).edges.size(), 0u); + + // Create a sample route to test pruning upon with different start/goal pose requests + Node node1, node2, node3, node4; + node1.nodeid = 1; + node1.coords.x = 0.0; + node1.coords.y = 0.0; + node1.search_state.traversal_cost = 0.0; + node2.nodeid = 2; + node2.coords.x = 1.0; + node2.coords.y = 0.0; + node2.search_state.traversal_cost = 1.0; + node3.nodeid = 3; + node3.coords.x = 2.0; + node3.coords.y = 0.0; + node3.search_state.traversal_cost = 1.0; + node4.nodeid = 4; + node4.coords.x = 3.0; + node4.coords.y = 0.0; + node4.search_state.traversal_cost = 1.0; + + DirectionalEdge edge1, edge2, edge3; + edge1.edgeid = 5; + edge1.start = &node1; + edge1.end = &node2; + edge2.edgeid = 6; + edge2.start = &node2; + edge2.end = &node3; + edge3.edgeid = 7; + edge3.start = &node3; + edge3.end = &node4; + + Route route; + route.route_cost = 3.0; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + route.edges.push_back(&edge3); + + // Test overlapping goal/start poses to their respective terminal points on route + geometry_msgs::msg::PoseStamped start, goal; + goal.pose.position.x = 3.0; + extractor.setStartAndGoal(start, goal); + EXPECT_EQ(extractor.pruneStartandGoal(route, poses_goal, rerouting_info).edges.size(), 3u); + + // Test orthogonally misaligned, so should still have all the same points + start.pose.position.y = -10.0; + goal.pose.position.y = 10.0; + extractor.setStartAndGoal(start, goal); + EXPECT_EQ(extractor.pruneStartandGoal(route, poses_goal, rerouting_info).edges.size(), 3u); + + // Test start node before start pose and end node before end pose (no pruning) + start.pose.position.x = -1.0; + goal.pose.position.x = 4.0; + start.pose.position.y = 0.0; + goal.pose.position.y = 0.0; + extractor.setStartAndGoal(start, goal); + auto rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 3u); + + // Test start, and only start is after the start node along edge1, should be pruned + start.pose.position.x = 0.2; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 2u); + EXPECT_EQ(rtn.edges[0]->edgeid, 6u); + EXPECT_EQ(rtn.start_node->nodeid, 2u); + EXPECT_EQ(rtn.route_cost, 2.0); + + // Test start, and only start is after the start node along edge1 + // should not be pruned, within min distance + start.pose.position.x = 0.09; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 3u); + EXPECT_EQ(rtn.edges[0]->edgeid, 5u); + EXPECT_EQ(rtn.start_node->nodeid, 1u); + EXPECT_EQ(rtn.route_cost, 3.0); + + // Test but now with no_poses_goal to test if we trigger with !first_time condition + // when we should be able to prune due to a secondary trigger as the result of rerouting + start.pose.position.x = 0.2; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, no_poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 2u); + EXPECT_EQ(rtn.edges[0]->edgeid, 6u); + EXPECT_EQ(rtn.start_node->nodeid, 2u); + EXPECT_EQ(rtn.route_cost, 2.0); + // But now if we have first_time, doesn't prune! + rerouting_info.first_time = true; + rtn = extractor.pruneStartandGoal(route, no_poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 3u); + + // Test end, and only end is before the end node along edge3, should be pruned + start.pose.position.x = 0.0; + goal.pose.position.x = 2.5; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 2u); + EXPECT_EQ(rtn.edges.back()->edgeid, 6u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 3u); + EXPECT_EQ(rtn.route_cost, 2.0); + + // Test end, and only end is before the end node along edge3, should not be pruned + // As within the min distance + start.pose.position.x = 0.0; + goal.pose.position.x = 2.9; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 3u); + EXPECT_EQ(rtn.edges.back()->edgeid, 7u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 4u); + EXPECT_EQ(rtn.route_cost, 3.0); + + // Test both together can be pruned with realistic tracking offsets from route + // Also, Check route info for resetting to nullptrs since not the same last edge + start.pose.position.x = 0.6; + start.pose.position.y = 0.4; + goal.pose.position.x = 2.6; + goal.pose.position.y = -0.4; + rerouting_info.curr_edge = &edge3; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 1u); + EXPECT_EQ(rtn.edges[0]->edgeid, 6u); + EXPECT_EQ(rtn.edges.back()->start->nodeid, 2u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 3u); + EXPECT_EQ(rtn.route_cost, 1.0); + EXPECT_EQ(rerouting_info.curr_edge, nullptr); + + // Test pruning with route information with same situation as above + // But now check route info is retained because it IS the same edge as last time + // & stores closest point to use + rerouting_info.curr_edge = &edge1; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 1u); + EXPECT_EQ(rtn.edges[0]->edgeid, 6u); + EXPECT_EQ(rtn.edges.back()->start->nodeid, 2u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 3u); + EXPECT_EQ(rtn.route_cost, 1.0); + EXPECT_EQ(rerouting_info.curr_edge->edgeid, 5u); + EXPECT_NEAR(rerouting_info.closest_pt_on_edge.x, 0.6, 0.01); + EXPECT_NEAR(rerouting_info.closest_pt_on_edge.y, 0.0, 0.01); + + // Test both together can be pruned but won't be due to huge offsets > max_dist_from_edge (8m) + start.pose.position.x = 0.6; + start.pose.position.y = 8.1; + goal.pose.position.x = 2.6; + goal.pose.position.y = -8.1; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges[0]->start->nodeid, 1u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 4u); + EXPECT_EQ(rtn.edges.size(), 3u); + + // Test the both pruned condition but goal is not pruned because prune_goal = false + node->set_parameter(rclcpp::Parameter("prune_goal", rclcpp::ParameterValue(false))); + start.pose.position.x = 0.6; + start.pose.position.y = 0.4; + goal.pose.position.x = 2.6; + goal.pose.position.y = -0.4; + GoalIntentExtractorWrapper extractor2; + std::shared_ptr costmap_subscriber2 = nullptr; + extractor2.configure(node, graph, &id_map, nullptr, costmap_subscriber2, "map", "base_link"); + extractor2.setStartAndGoal(start, goal); + rtn = extractor2.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 2u); +} diff --git a/nav2_route/test/test_goal_intent_search.cpp b/nav2_route/test/test_goal_intent_search.cpp new file mode 100644 index 00000000000..722273b76f5 --- /dev/null +++ b/nav2_route/test/test_goal_intent_search.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_route/goal_intent_extractor.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(GoalIntentSearchTest, test_obj_lifecycle) +{ + std::shared_ptr costmap = nullptr; + GoalIntentSearch::BreadthFirstSearch bfs(costmap); + EXPECT_EQ(bfs.getClosestNodeIdx(), 0u); + GoalIntentSearch::LoSCollisionChecker los_checker(costmap); +} + +TEST(GoalIntentSearchTest, test_los_checker) +{ + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + auto costmap = + std::make_shared(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + GoalIntentSearch::LoSCollisionChecker los_checker(costmap); + + // Valid request + geometry_msgs::msg::Point start, end; + start.x = 1.0; + start.y = 1.0; + end.x = 1.0; + end.y = 9.0; + EXPECT_TRUE(los_checker.worldToMap(start, end)); + EXPECT_FALSE(los_checker.isInCollision()); + + // In collision request + start.x = 1.0; + start.y = 1.0; + end.x = 9.0; + end.y = 9.0; + EXPECT_TRUE(los_checker.worldToMap(start, end)); + EXPECT_TRUE(los_checker.isInCollision()); + + // Off of costmap request + end.x = 11.0; + end.y = 11.0; + EXPECT_FALSE(los_checker.worldToMap(start, end)); +} + +TEST(GoalIntentSearchTest, test_breadth_first_search) +{ + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + auto costmap = + std::make_shared(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + GoalIntentSearch::BreadthFirstSearch bfs(costmap); + + std::vector candidate_nodes; + geometry_msgs::msg::PoseStamped target_node; + target_node.header.frame_id = "map"; + target_node.pose.position.x = 1.0; + target_node.pose.position.y = 1.0; + + geometry_msgs::msg::PoseStamped node; + node.header.frame_id = "map"; + node.pose.position.x = 9.0; + node.pose.position.y = 9.0; + candidate_nodes.push_back(node); // First is far + + node.pose.position.x = 5.0; + node.pose.position.y = 5.0; + candidate_nodes.push_back(node); // Second is close but in collision + + node.pose.position.x = 1.0; + node.pose.position.y = 9.0; + candidate_nodes.push_back(node); // Third is closest valid + + // Test successful search + EXPECT_TRUE(bfs.search(target_node, candidate_nodes)); + EXPECT_EQ(bfs.getClosestNodeIdx(), 2u); // Should find the third + + // Test max iterations + int max_it = 1; + EXPECT_FALSE(bfs.search(target_node, candidate_nodes, max_it)); + EXPECT_EQ(bfs.getClosestNodeIdx(), 0u); + + // Test reference off of the map + target_node.pose.position.x = 10000.0; + EXPECT_FALSE(bfs.search(target_node, candidate_nodes)); + + // Test candidates off of the map + target_node.pose.position.x = 1.0; + for (auto & node_c : candidate_nodes) { + node_c.pose.position.x = 10000.0; + } + EXPECT_FALSE(bfs.search(target_node, candidate_nodes)); + + // Test empty candidates + candidate_nodes.clear(); + EXPECT_FALSE(bfs.search(target_node, candidate_nodes)); +} diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp new file mode 100644 index 00000000000..145d84aea3a --- /dev/null +++ b/nav2_route/test/test_graph_loader.cpp @@ -0,0 +1,151 @@ +// Copyright (c) 2025, Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_route/graph_loader.hpp" +#include "tf2_ros/static_transform_broadcaster.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; //NOLINT + +TEST(GraphLoader, test_invalid_plugin) +{ + auto node = std::make_shared("graph_loader_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + // Set dummy parameter + std::string default_plugin = "nav2_route::Dummy"; + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_loader", rclcpp::ParameterValue(default_plugin)); + + EXPECT_THROW(GraphLoader graph_loader(node, tf, frame), pluginlib::PluginlibException); +} + +TEST(GraphLoader, test_api) +{ + auto node = std::make_shared("graph_loader_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + + Graph graph; + GraphToIDMap graph_to_id_map; + std::string filepath; + EXPECT_TRUE(graph_loader.loadGraphFromParameter(graph, graph_to_id_map)); + EXPECT_FALSE(graph_loader.loadGraphFromFile(graph, graph_to_id_map, filepath)); +} + +TEST(GraphLoader, test_transformation_api) +{ + auto node = std::make_shared("graph_loader_test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); + auto tf_listener = std::make_shared(*tf); + auto tf_broadcaster = std::make_shared(node); + + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + + // Test with a file that now works + Graph graph; + GraphToIDMap graph_to_id_map; + std::string filepath; + filepath = + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson"; + EXPECT_TRUE(graph_loader.loadGraphFromFile(graph, graph_to_id_map, filepath)); + + // Test with another frame, should transform + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.child_frame_id = "map_test"; + transform.header.stamp = node->now(); + transform.transform.rotation.w = 1.0; + transform.transform.translation.x = 1.0; + tf_broadcaster->sendTransform(transform); + rclcpp::Rate(1).sleep(); + tf_broadcaster->sendTransform(transform); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1)); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + + graph[0].coords.frame_id = "map_test"; + EXPECT_EQ(graph[0].coords.frame_id, "map_test"); + double or_coord = graph[0].coords.x; + EXPECT_TRUE(graph_loader.transformGraph(graph)); + EXPECT_EQ(graph[0].coords.frame_id, "map"); + EXPECT_NE(graph[0].coords.x, or_coord); + + // Test failing to do so because no frame exists + graph[0].coords.frame_id = "map_test2"; + EXPECT_EQ(graph[0].coords.frame_id, "map_test2"); + or_coord = graph[0].coords.x; + EXPECT_FALSE(graph_loader.transformGraph(graph)); + EXPECT_EQ(graph[0].coords.frame_id, "map_test2"); + EXPECT_EQ(graph[0].coords.x, or_coord); +} + +TEST(GraphLoader, test_transformation_api2) +{ + auto node = std::make_shared("graph_loader_test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); + + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/test/test_graphs/no_frame.json")); + + GraphLoader graph_loader(node, tf, frame); + + // Test with a file that has unknown frames that cannot be transformed + Graph graph; + GraphToIDMap graph_to_id_map; + EXPECT_FALSE(graph_loader.loadGraphFromParameter(graph, graph_to_id_map)); + std::string filepath = ament_index_cpp::get_package_share_directory("nav2_route") + + "/test/test_graphs/no_frame.json"; + EXPECT_FALSE(graph_loader.loadGraphFromFile(graph, graph_to_id_map, filepath)); +} diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp new file mode 100644 index 00000000000..e6a3913f51a --- /dev/null +++ b/nav2_route/test/test_graph_saver.cpp @@ -0,0 +1,166 @@ +// Copyright (c) 2025 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_route/graph_loader.hpp" +#include "nav2_route/graph_saver.hpp" +#include "tf2_ros/static_transform_broadcaster.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; //NOLINT + +TEST(GraphSaver, test_invalid_plugin) +{ + auto node = std::make_shared("graph_saver_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + // Set dummy parameter + std::string default_plugin = "nav2_route::Dummy"; + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_saver", rclcpp::ParameterValue(default_plugin)); + + EXPECT_THROW(GraphSaver graph_saver(node, tf, frame), std::runtime_error); +} + +TEST(GraphSaver, test_empty_filename) +{ + auto node = std::make_shared("graph_saver_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + GraphSaver graph_saver(node, tf, frame); + + Graph first_graph, second_graph; + GraphToIDMap first_graph_to_id_map, second_graph_to_id_map; + std::string file_path = ""; + graph_loader.loadGraphFromParameter(first_graph, first_graph_to_id_map); + EXPECT_TRUE(graph_saver.saveGraphToFile(first_graph, file_path)); +} + +TEST(GraphSaver, test_api) +{ + auto node = std::make_shared("graph_saver_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + GraphSaver graph_saver(node, tf, frame); + + Graph first_graph, second_graph; + GraphToIDMap first_graph_to_id_map, second_graph_to_id_map; + std::string file_path = "test.geojson"; + graph_loader.loadGraphFromParameter(first_graph, first_graph_to_id_map); + EXPECT_TRUE(graph_saver.saveGraphToFile(first_graph, file_path)); + graph_loader.loadGraphFromFile(second_graph, second_graph_to_id_map, file_path); + EXPECT_EQ(first_graph.size(), second_graph.size()); + EXPECT_EQ(first_graph_to_id_map.size(), second_graph_to_id_map.size()); + for (size_t i = 0; i < first_graph.size(); ++i) { + EXPECT_EQ(first_graph[i].nodeid, second_graph[i].nodeid); + EXPECT_EQ(first_graph[i].coords.x, second_graph[i].coords.x); + EXPECT_EQ(first_graph[i].coords.y, second_graph[i].coords.y); + } + for (size_t i = 0; i < first_graph_to_id_map.size(); ++i) { + EXPECT_EQ(first_graph_to_id_map[i], second_graph_to_id_map[i]); + } + std::filesystem::remove(file_path); +} + +TEST(GraphSaver, test_transformation_api) +{ + auto node = std::make_shared("graph_saver_test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); + auto tf_listener = std::make_shared(*tf); + auto tf_broadcaster = std::make_shared(node); + + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + + // Test with a file that now works + Graph graph; + GraphToIDMap graph_to_id_map; + std::string filepath; + filepath = + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson"; + graph_loader.loadGraphFromFile(graph, graph_to_id_map, filepath); + + // Test with another frame, should transform + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.child_frame_id = "map_test"; + transform.header.stamp = node->now(); + transform.transform.rotation.w = 1.0; + transform.transform.translation.x = 1.0; + tf_broadcaster->sendTransform(transform); + rclcpp::Rate(1).sleep(); + tf_broadcaster->sendTransform(transform); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1)); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + + GraphSaver graph_saver(node, tf, frame); + std::string file_path = "test.geojson"; + graph[0].coords.frame_id = "map_test"; + EXPECT_EQ(graph[0].coords.frame_id, "map_test"); + double or_coord = graph[0].coords.x; + EXPECT_TRUE(graph_saver.saveGraphToFile(graph, file_path)); + EXPECT_EQ(graph[0].coords.frame_id, "map"); + EXPECT_NE(graph[0].coords.x, or_coord); + std::filesystem::remove(file_path); + + // Test failing to do so because no frame exists + graph[0].coords.frame_id = "map_test2"; + EXPECT_EQ(graph[0].coords.frame_id, "map_test2"); + or_coord = graph[0].coords.x; + EXPECT_FALSE(graph_saver.saveGraphToFile(graph, file_path)); + EXPECT_EQ(graph[0].coords.frame_id, "map_test2"); + EXPECT_EQ(graph[0].coords.x, or_coord); +} diff --git a/nav2_route/test/test_graphs/error_codes.geojson b/nav2_route/test/test_graphs/error_codes.geojson new file mode 100644 index 00000000000..6712c506d04 --- /dev/null +++ b/nav2_route/test/test_graphs/error_codes.geojson @@ -0,0 +1,21 @@ +{ + "type": "FeatureCollection", + "name": "turtlebot3_graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 399 }, "geometry": { "type": "Point", "coordinates": [ 399.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 400 }, "geometry": { "type": "Point", "coordinates": [ 400.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 401 }, "geometry": { "type": "Point", "coordinates": [ 401.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 402 }, "geometry": { "type": "Point", "coordinates": [ 402.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 403 }, "geometry": { "type": "Point", "coordinates": [ 403.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 404 }, "geometry": { "type": "Point", "coordinates": [ 404.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 405 }, "geometry": { "type": "Point", "coordinates": [ 405.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 406 }, "geometry": { "type": "Point", "coordinates": [ 406.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 407 }, "geometry": { "type": "Point", "coordinates": [ 407.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 408 }, "geometry": { "type": "Point", "coordinates": [ 408.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 409 }, "geometry": { "type": "Point", "coordinates": [ 409.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 410 }, "geometry": { "type": "Point", "coordinates": [ 410.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 399 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 399.0, 0.0 ] ] ] } } + ] +} diff --git a/nav2_route/test/test_graphs/invalid.json b/nav2_route/test/test_graphs/invalid.json new file mode 100644 index 00000000000..41af33f4603 --- /dev/null +++ b/nav2_route/test/test_graphs/invalid.json @@ -0,0 +1,133 @@ +// This file has parsing errors, it is not valid +{ + "type": "FeatureCollection", + "name": "graph" + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Wed Feb 22 05:41:45 PM EST 2025", + "features": + { "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, + { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, + { "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, + { "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, + { "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, + { "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, + { "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, + { "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, + { "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, + { "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, + { "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, + { "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, + { "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, + { "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, + { "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, + { "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, + { "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } + ] + } diff --git a/nav2_route/test/test_graphs/no_frame.json b/nav2_route/test/test_graphs/no_frame.json new file mode 100644 index 00000000000..6c60c370465 --- /dev/null +++ b/nav2_route/test/test_graphs/no_frame.json @@ -0,0 +1,132 @@ +{ + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Wed Feb 22 05:41:45 PM EST 2025", + "features": [ + { "type": "Feature", "properties": { "id": 0, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, + { "type": "Feature", "properties": { "id": 1, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 9, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 10, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 11, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 12, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 13, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 14, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 15, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, + { "type": "Feature", "properties": { "id": 16, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, + { "type": "Feature", "properties": { "id": 17, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, + { "type": "Feature", "properties": { "id": 18, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, + { "type": "Feature", "properties": { "id": 19, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, + { "type": "Feature", "properties": { "id": 20, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, + { "type": "Feature", "properties": { "id": 21, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 22, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 23, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, + { "type": "Feature", "properties": { "id": 24, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, + { "type": "Feature", "properties": { "id": 25, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 26, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 27, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 28, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, + { "type": "Feature", "properties": { "id": 29, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, + { "type": "Feature", "properties": { "id": 30, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 31, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 32, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 33, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, + { "type": "Feature", "properties": { "id": 34, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 35, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 36, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 37, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 38, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 39, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, + { "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, + { "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, + { "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, + { "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } + ] + } diff --git a/nav2_route/test/test_operations.cpp b/nav2_route/test/test_operations.cpp new file mode 100644 index 00000000000..a8e7301625e --- /dev/null +++ b/nav2_route/test/test_operations.cpp @@ -0,0 +1,571 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "nav2_route/operations_manager.hpp" +#include "nav2_route/types.hpp" +#include "nav2_core/route_exceptions.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(OperationsManagerTest, test_lifecycle) +{ + auto node = std::make_shared("operations_manager_test"); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); +} + +TEST(OperationsManagerTest, test_failed_plugins) +{ + // This plugin does not exist + auto node = std::make_shared("operations_manager_test"); + node->declare_parameter("operations", rclcpp::ParameterValue(std::vector{"hi"})); + std::shared_ptr costmap_subscriber; + EXPECT_THROW(OperationsManager manager(node, costmap_subscriber), std::runtime_error); +} + +TEST(OperationsManagerTest, test_find_operations) +{ + auto node = std::make_shared("operations_manager_test"); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + EdgePtr ent{nullptr}, exit{nullptr}; + + // No valid nodes or edges, should fail to find anything but not crash + EXPECT_EQ(manager.findGraphOperations(&node2, ent, exit).size(), 0u); + + // Still shouldn't find anything, but without nullptrs so can evaluate + DirectionalEdge enter, exit2; + EXPECT_EQ(manager.findGraphOperations(&node2, &enter, &enter).size(), 0u); + + // Try again with some operations in the node and edge (2x-ed) + Operation op, op2; + op.type = "test"; + op.trigger = OperationTrigger::ON_ENTER; + enter.operations.push_back(op); + op.trigger = OperationTrigger::NODE; + node2.operations.push_back(op); + op2.type = "test2"; + op2.trigger = OperationTrigger::ON_EXIT; + exit2.operations.push_back(op2); + EXPECT_EQ(manager.findGraphOperations(&node2, &enter, &exit2).size(), 3u); + + // Again, but now the triggers are inverted, so shouldn't be returned except node2 + enter.operations[0].trigger = OperationTrigger::ON_EXIT; + exit2.operations[0].trigger = OperationTrigger::ON_ENTER; + EXPECT_EQ(manager.findGraphOperations(&node2, &enter, &exit2).size(), 1u); + + // Now, should be empty + node2.operations[0].trigger = OperationTrigger::ON_ENTER; + EXPECT_EQ(manager.findGraphOperations(&node2, &enter, &exit2).size(), 0u); +} + +TEST(OperationsManagerTest, test_find_operations_failure2) +{ + // This plugin does not exist + auto node = std::make_shared("operations_manager_test"); + node->declare_parameter("operations", rclcpp::ParameterValue(std::vector{"hi"})); + std::shared_ptr costmap_subscriber; + EXPECT_THROW(OperationsManager manager(node, costmap_subscriber), std::runtime_error); +} + +TEST(OperationsManagerTest, test_processing_fail) +{ + auto node = std::make_shared("operations_manager_test"); + + // No operation plugins to trigger + node->declare_parameter("operations", rclcpp::ParameterValue(std::vector{})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + ReroutingState info; + + // Should trigger nothing + auto result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 0u); +} + +TEST(OperationsManagerTest, test_processing_speed_on_status) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + bool got_msg = false; + nav2_msgs::msg::SpeedLimit my_msg; + auto sub = node->create_subscription( + "speed_limit", + rclcpp::QoS(1), + [&, this](nav2_msgs::msg::SpeedLimit msg) {got_msg = true; my_msg = msg;}); + + Node node2; + DirectionalEdge enter; + float limit = 0.5f; + enter.metadata.setValue("speed_limit", limit); + geometry_msgs::msg::PoseStamped pose; + Route route; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + ReroutingState info; + + // No status change, shouldn't do anything + OperationsResult result = manager.process(false, state, route, pose, info); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.operations_triggered.size(), 1u); // ReroutingService + EXPECT_EQ(result.operations_triggered[0], std::string("ReroutingService")); + + // Status change, may now trigger the only plugin + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 2u); + EXPECT_EQ(result.operations_triggered[0], std::string("AdjustSpeedLimit")); + EXPECT_EQ(result.operations_triggered[1], std::string("ReroutingService")); + rclcpp::Rate r(10); + r.sleep(); + + // Check values are correct + EXPECT_TRUE(got_msg); + EXPECT_TRUE(my_msg.percentage); + EXPECT_NEAR(my_msg.speed_limit, 0.5f, 0.001f); +} + +TEST(OperationsManagerTest, test_rerouting_service_on_query) +{ + auto node = std::make_shared("route_server"); + auto node_thread = std::make_unique(node); + auto node_int = std::make_shared("my_node2"); + + // Enable rerouting service, which conducts on query (not status change) + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"ReroutingService"})); + nav2_util::declare_parameter_if_not_declared( + node, "ReroutingService.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::ReroutingService"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + ReroutingState info; + + // Should trigger, either way! + auto result = manager.process(false, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + + auto srv_client = + nav2_util::ServiceClient( + "route_server/ReroutingService/reroute", node_int); + auto req = std::make_shared(); + auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp->success); + + // Check values are correct after service call + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_TRUE(result.reroute); + + // and resets + result = manager.process(false, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); +} + +TEST(OperationsManagerTest, test_trigger_event_on_graph) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + auto node_int = std::make_shared("my_node2"); + auto node_thread_int = std::make_unique(node_int); + + // Enable trigger event operation, which conducts on node or edge change + // when a graph object contains the request for opening a door only. + // This tests the trigger event plugin, ON_GRAPH actions in the + // Operations Manager as well as the route operations client. + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + nav2_util::declare_parameter_if_not_declared( + node, "OpenDoor.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + Metadata mdata; + ReroutingState info; + + // Setup some test operations + Operation op, op2, op3; + op.type = "test"; + op.trigger = OperationTrigger::NODE; + + op2.type = "OpenDoor"; + op2.trigger = OperationTrigger::NODE; + + op3.type = "OpenDoor"; + op3.trigger = OperationTrigger::NODE; + std::string service_name = "open_door"; + std::string key = "service_name"; + op3.metadata.setValue(key, service_name); + + // Should do nothing in the operations manager, throw + node2.operations.push_back(op); + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); + + // Now, let's try a node that should make it through the operations manager but fail + // because the proper service_name was provided neither in the parameter nor operation + // metadata + node2.operations.clear(); + node2.operations.push_back(op2); + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); + + // Now let's test what should actually work with a real service in the metadata + node2.operations.clear(); + node2.operations.push_back(op3); + + // This should throw because this service is not yet available on wait_for_service + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); + + // Now, let's test with a real server that is really available for use + bool got_srv = false; + auto callback = + [&]( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response + ) -> void + { + got_srv = true; + response->success = true; + }; + + auto service = node_int->create_service(service_name, callback); + + auto result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + EXPECT_TRUE(got_srv); +} + +TEST(OperationsManagerTest, test_trigger_event_on_graph_global_service) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + auto node_int = std::make_shared("my_node2"); + auto node_thread_int = std::make_unique(node_int); + + // Set the global service to use instead of file settings for conflict testing + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + nav2_util::declare_parameter_if_not_declared( + node, "OpenDoor.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); + nav2_util::declare_parameter_if_not_declared( + node, "OpenDoor.service_name", + rclcpp::ParameterValue(std::string{"hello_world"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + Metadata mdata; + ReroutingState info; + + // Setup working case + Operation op3; + op3.type = "OpenDoor"; + op3.trigger = OperationTrigger::NODE; + std::string service_name = "open_door"; + std::string key = "service_name"; + op3.metadata.setValue(key, service_name); + node2.operations.push_back(op3); + + // Setup a server for the node's metadata, to create conflict with the global setting + // If there's a conflict, the file version wins due to more specificity + bool got_srv = false; + auto callback = + [&]( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response + ) -> void + { + got_srv = true; + response->success = true; + }; + + auto service = node_int->create_service(service_name, callback); + + auto result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + EXPECT_TRUE(got_srv); + + // Now, let's reset without the metadata and see that the global version is now called + node2.operations.clear(); + Operation op4; + op4.type = "OpenDoor"; + op4.trigger = OperationTrigger::NODE; + node2.operations.push_back(op4); + + bool got_srv2 = false; + auto callback2 = + [&]( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response + ) -> void + { + got_srv2 = true; + response->success = true; + }; + + auto service2 = node_int->create_service("hello_world", callback2); + + OperationsManager manager2(node, costmap_subscriber); + result = manager2.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + EXPECT_TRUE(got_srv2); +} + +TEST(OperationsManagerTest, test_trigger_event_on_graph_failures) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + auto node_int = std::make_shared("my_node2"); + + // Enable trigger event operation, which conducts on node or edge change + // when a graph object contains the request for opening a door only + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + nav2_util::declare_parameter_if_not_declared( + node, "OpenDoor.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + ReroutingState info; + + // No operations, nothing should trigger even though status changed + auto result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 0u); + EXPECT_FALSE(result.reroute); + + // Setup some test operations + Operation op, op2; + op.type = "test"; + op2.type = "TriggerEvent"; + op.trigger = OperationTrigger::NODE; + op2.trigger = OperationTrigger::NODE; + + // Should also do nothing, this type isn't a plugin type supported + node2.operations.push_back(op); + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); + + // Make sure its using the provided plugin name NOT its type + node2.operations.clear(); + node2.operations.push_back(op2); + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); +} + + +TEST(OperationsManagerTest, test_time_marker) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"TimeMarker"})); + nav2_util::declare_parameter_if_not_declared( + node, "TimeMarker.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TimeMarker"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node1, node2, node3, node4; + DirectionalEdge exit, enter, last; + exit.start = &node1; + exit.end = &node2; + enter.start = &node2; + enter.end = &node3; + last.start = &node3; + last.end = &node4; + geometry_msgs::msg::PoseStamped pose; + + Route route; + route.start_node = &node1; + route.edges.push_back(&exit); + route.edges.push_back(&enter); + route.edges.push_back(&last); + + ReroutingState info; + RouteTrackingState state; + state.last_node = &node1; + state.next_node = &node2; + state.current_edge = &exit; + state.route_edges_idx = 0; + + // No status change, shouldn't do anything ... even after some time + OperationsResult result = manager.process(false, state, route, pose, info); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.operations_triggered.size(), 0u); + rclcpp::Rate r(1); + r.sleep(); + result = manager.process(false, state, route, pose, info); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.operations_triggered.size(), 0u); + + // Status change, may now trigger but state doesn't match + // (new edge) so it won't update times on the first call + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + + float time = 0.0f; + time = enter.metadata.getValue("abs_time_taken", time); + EXPECT_EQ(time, 0.0f); + time = 0.0f; + time = exit.metadata.getValue("abs_time_taken", time); + EXPECT_EQ(time, 0.0f); + + rclcpp::Rate r2(1); + r2.sleep(); + + // The second time around after switching edges, should update the last edge's time + state.last_node = &node2; + state.next_node = &node3; + state.current_edge = &enter; + state.route_edges_idx = 1; + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + time = 0.0f; + time = exit.metadata.getValue("abs_time_taken", time); + EXPECT_GT(time, 0.5f); + time = 0.0f; + time = enter.metadata.getValue("abs_time_taken", time); + EXPECT_EQ(time, 0.0f); + + // Immediately call again on new edge, should also work with an edge change + // But the last edge not completed should still remain empty + state.last_node = &node3; + state.next_node = &node4; + state.current_edge = &last; + state.route_edges_idx = 2; + result = manager.process(true, state, route, pose, info); + time = 0.0f; + time = enter.metadata.getValue("abs_time_taken", time); + EXPECT_GT(time, 1e-6f); + time = 0.0f; + time = last.metadata.getValue("abs_time_taken", time); + EXPECT_EQ(time, 0.0f); + + // Check on terminal conditions + rclcpp::Rate r3(1); + r3.sleep(); + state.last_node = &node4; + state.next_node = nullptr; + state.current_edge = nullptr; + state.route_edges_idx = 3; + result = manager.process(true, state, route, pose, info); + time = 0.0f; + time = last.metadata.getValue("abs_time_taken", time); + EXPECT_GT(time, 0.5f); +} + +// A test operation that does nothing to test `processType()` +class TestRouteOperations : public nav2_route::RouteOperation +{ +public: + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr, + std::shared_ptr, + const std::string &) override + { + } + + std::string getName() override {return "TestRouteOperation";} + OperationResult perform( + NodePtr, + EdgePtr, + EdgePtr, + const Route &, + const geometry_msgs::msg::PoseStamped &, + const Metadata *) override {return OperationResult();} +}; + +TEST(OperationsTest, test_interface) +{ + TestRouteOperations op; + EXPECT_EQ(op.processType(), nav2_route::RouteOperationType::ON_GRAPH); +} diff --git a/nav2_route/test/test_path_converter.cpp b/nav2_route/test/test_path_converter.cpp new file mode 100644 index 00000000000..cadea6ac0c5 --- /dev/null +++ b/nav2_route/test/test_path_converter.cpp @@ -0,0 +1,165 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/path_converter.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(PathConverterTest, test_path_converter_api) +{ + auto node = std::make_shared("edge_scorer_test"); + auto node_thread = std::make_unique(node); + + nav_msgs::msg::Path path_msg; + auto sub = node->create_subscription( + "plan", rclcpp::QoS(1), [&, this](nav_msgs::msg::Path msg) {path_msg = msg;}); + + PathConverter converter; + converter.configure(node); + + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + Route route; + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 10; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 11; + test_node2.coords.x = 10.0; + test_node2.coords.y = 10.0; + test_node3.nodeid = 12; + test_node3.coords.x = 20.0; + test_node3.coords.y = 20.0; + + DirectionalEdge test_edge1, test_edge2; + test_edge1.edgeid = 13; + test_edge1.start = &test_node1; + test_edge1.end = &test_node2; + test_edge2.edgeid = 14; + test_edge2.start = &test_node2; + test_edge2.end = &test_node3; + + route.start_node = &test_node1; + route.route_cost = 50.0; + route.edges.push_back(&test_edge1); + route.edges.push_back(&test_edge2); + ReroutingState info; + + auto path = converter.densify(route, info, frame, time); + EXPECT_EQ(path.header.frame_id, frame); + EXPECT_EQ(path.header.stamp.nanosec, time.nanoseconds()); + + // 2 * sqrt(200) * 20 (0.05 density/m) + 1 (for starting node) + EXPECT_EQ(path.poses.size(), 567u); + EXPECT_NEAR(path.poses[0].pose.position.x, 0.0, 0.01); + EXPECT_NEAR(path.poses[0].pose.position.y, 0.0, 0.01); + EXPECT_NEAR(path.poses.back().pose.position.x, 20.0, 0.01); + EXPECT_NEAR(path.poses.back().pose.position.y, 20.0, 0.01); + + rclcpp::Rate r(10); + r.sleep(); + + // Checks the same as returned and actually was published + EXPECT_EQ(path_msg.poses.size(), path.poses.size()); + node_thread.reset(); +} + +TEST(PathConverterTest, test_path_single_pt_path) +{ + auto node = std::make_shared("edge_scorer_test"); + PathConverter converter; + converter.configure(node); + + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + + Node test_node; + test_node.nodeid = 17; + test_node.coords.x = 10.0; + test_node.coords.y = 40.0; + + Route route; + route.start_node = &test_node; + ReroutingState info; + + auto path = converter.densify(route, info, frame, time); + EXPECT_EQ(path.poses.size(), 1u); + EXPECT_NEAR(path.poses[0].pose.position.x, 10.0, 0.01); + EXPECT_NEAR(path.poses[0].pose.position.y, 40.0, 0.01); +} + +TEST(PathConverterTest, test_prev_info_path) +{ + auto node = std::make_shared("edge_scorer_test"); + PathConverter converter; + converter.configure(node); + + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + + Node test_node; + test_node.nodeid = 17; + test_node.coords.x = 1.0; + test_node.coords.y = 0.0; + + Route route; + route.start_node = &test_node; + + DirectionalEdge edge; + edge.end = &test_node; + + ReroutingState info; + info.closest_pt_on_edge.x = 0.0; + info.closest_pt_on_edge.y = 0.0; + info.curr_edge = &edge; + + auto path = converter.densify(route, info, frame, time); + EXPECT_EQ(path.poses.size(), 21u); // 20 for density + 1 for single node point +} + +TEST(PathConverterTest, test_path_converter_interpolation) +{ + auto node = std::make_shared("edge_scorer_test"); + PathConverter converter; + converter.configure(node); + + float x0 = 10.0, y0 = 10.0, x1 = 20.0, y1 = 20.0; + std::vector poses; + converter.interpolateEdge(x0, y0, x1, y1, poses); + + EXPECT_EQ(poses.size(), 283u); // regular density + edges + for (unsigned int i = 0; i != poses.size() - 1; i++) { + // Check its always closer than the requested density + EXPECT_LT( + hypotf( + poses[i].pose.position.x - poses[i + 1].pose.position.x, + poses[i].pose.position.y - poses[i + 1].pose.position.y), 0.05); + } +} diff --git a/nav2_route/test/test_route_planner.cpp b/nav2_route/test/test_route_planner.cpp new file mode 100644 index 00000000000..4c3d53b2153 --- /dev/null +++ b/nav2_route/test/test_route_planner.cpp @@ -0,0 +1,226 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/route_planner.hpp" +#include "nav2_msgs/action/compute_route.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +inline Graph create4x4Graph() +{ + // * - * - * > * 12 13 14 15 + // | | | ^ + // * - * - * - * 8 9 10 11 + // | | | | + // * - * - * - * 4 5 6 7 + // | | | | + // * - * - * - * 0 1 2 3 + // where `-` and `|` are bidirectional edges and > are single direction edges + + EdgeCost default_cost; + Graph graph; + graph.resize(16); + unsigned int idx = 1; + for (unsigned int j = 0; j != 4; j++) { + for (unsigned int i = 0; i != 4; i++) { + Node & node = graph[idx - 1]; + node.coords.x = i; + node.coords.y = j; + node.nodeid = idx; + idx++; + } + } + + // Bottom row + graph[0].addEdge(default_cost, &graph[1], idx++); + graph[1].addEdge(default_cost, &graph[0], idx++); + graph[0].addEdge(default_cost, &graph[4], idx++); + graph[4].addEdge(default_cost, &graph[0], idx++); + + graph[1].addEdge(default_cost, &graph[2], idx++); + graph[2].addEdge(default_cost, &graph[1], idx++); + graph[1].addEdge(default_cost, &graph[5], idx++); + graph[5].addEdge(default_cost, &graph[1], idx++); + + graph[2].addEdge(default_cost, &graph[3], idx++); + graph[3].addEdge(default_cost, &graph[2], idx++); + graph[2].addEdge(default_cost, &graph[6], idx++); + graph[6].addEdge(default_cost, &graph[2], idx++); + + graph[7].addEdge(default_cost, &graph[3], idx++); + graph[3].addEdge(default_cost, &graph[7], idx++); + + // Second row + graph[4].addEdge(default_cost, &graph[5], idx++); + graph[5].addEdge(default_cost, &graph[4], idx++); + graph[4].addEdge(default_cost, &graph[8], idx++); + graph[8].addEdge(default_cost, &graph[4], idx++); + + graph[5].addEdge(default_cost, &graph[6], idx++); + graph[6].addEdge(default_cost, &graph[5], idx++); + graph[5].addEdge(default_cost, &graph[9], idx++); + graph[9].addEdge(default_cost, &graph[5], idx++); + + graph[6].addEdge(default_cost, &graph[7], idx++); + graph[7].addEdge(default_cost, &graph[6], idx++); + graph[6].addEdge(default_cost, &graph[10], idx++); + graph[10].addEdge(default_cost, &graph[6], idx++); + + graph[7].addEdge(default_cost, &graph[11], idx++); + graph[11].addEdge(default_cost, &graph[7], idx++); + + // third row + graph[8].addEdge(default_cost, &graph[9], idx++); + graph[9].addEdge(default_cost, &graph[8], idx++); + graph[8].addEdge(default_cost, &graph[12], idx++); + graph[12].addEdge(default_cost, &graph[8], idx++); + + graph[9].addEdge(default_cost, &graph[10], idx++); + graph[10].addEdge(default_cost, &graph[9], idx++); + graph[9].addEdge(default_cost, &graph[13], idx++); + graph[13].addEdge(default_cost, &graph[9], idx++); + + graph[10].addEdge(default_cost, &graph[11], idx++); + graph[11].addEdge(default_cost, &graph[10], idx++); + graph[10].addEdge(default_cost, &graph[14], idx++); + graph[14].addEdge(default_cost, &graph[10], idx++); + + graph[11].addEdge(default_cost, &graph[15], idx++); // one direction + + // Top row + graph[12].addEdge(default_cost, &graph[13], idx++); + graph[13].addEdge(default_cost, &graph[12], idx++); + + graph[13].addEdge(default_cost, &graph[14], idx++); + graph[14].addEdge(default_cost, &graph[13], idx++); + + graph[14].addEdge(default_cost, &graph[15], idx++); // one direction + return graph; +} + +TEST(RoutePlannerTest, test_route_planner_positive) +{ + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + + auto node = std::make_shared("router_test"); + std::shared_ptr tf_buffer; + std::shared_ptr collision_checker; + RoutePlanner planner; + planner.configure(node, tf_buffer, collision_checker); + std::vector blocked_ids; + unsigned int start, goal; + + // Create a graph to test routing upon. + Graph graph = create4x4Graph(); + + // Plan across diagonal, should be length 6 + start = 0u; + goal = 15u; + Route route = planner.findRoute(graph, start, goal, blocked_ids, route_request); + EXPECT_NEAR(route.route_cost, 6.0, 0.001); + EXPECT_EQ(route.edges.size(), 6u); + + // If we try in reverse, it should fail though since Node 16 is only + // achievable from the other direction + start = 15; + goal = 0; + EXPECT_THROW( + planner.findRoute( + graph, start, goal, blocked_ids, + route_request), nav2_core::NoValidRouteCouldBeFound); + + // Let's find a simple plan and then mess with the planner with blocking edges + start = 0; + goal = 12; + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); + EXPECT_NEAR(route.route_cost, 3.0, 0.001); + EXPECT_EQ(route.edges.size(), 3u); + + // Now block an edge as closed along the chain, should find the next best path + unsigned int key_edgeid = 19u; + blocked_ids.push_back(key_edgeid); // Edge between node 0-4 in the 0-4-9-12 chain + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); + EXPECT_NEAR(route.route_cost, 5.0, 0.001); + EXPECT_EQ(route.edges.size(), 5u); + for (auto & edge : route.edges) { + EXPECT_NE(edge->edgeid, key_edgeid); + } + + // Now don't block, but instead just increase the cost so that it goes elsewhere + // this should produce the same results + blocked_ids.clear(); + graph[0].neighbors[1].edge_cost.cost = 1e6; + graph[0].neighbors[1].edge_cost.overridable = false; + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); + EXPECT_NEAR(route.route_cost, 5.0, 0.001); + EXPECT_EQ(route.edges.size(), 5u); +} + +TEST(RoutePlannerTest, test_route_planner_negative) +{ + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + + auto node = std::make_shared("router_test"); + std::shared_ptr tf_buffer; + node->declare_parameter("max_iterations", rclcpp::ParameterValue(5)); + std::shared_ptr collision_checker; + RoutePlanner planner; + planner.configure(node, tf_buffer, collision_checker); + std::vector blocked_ids; + unsigned int start = 0; + unsigned int goal = 15; + Graph graph; + + // No graph yet, should fail + EXPECT_THROW( + planner.findRoute( + graph, start, goal, blocked_ids, + route_request), nav2_core::NoValidGraph); + + // Create a graph to test routing upon. + graph = create4x4Graph(); + + // Try with a stupidly low number of iterations + graph[0].neighbors[1].edge_cost.overridable = true; + EXPECT_THROW( + planner.findRoute( + graph, start, goal, blocked_ids, + route_request), nav2_core::TimedOut); + + // If we try to plan but we both cannot override and has 0 cost, will throw + graph[0].neighbors[1].edge_cost.overridable = false; + graph[0].neighbors[1].edge_cost.cost = 0.0; + EXPECT_THROW( + planner.findRoute( + graph, start, goal, blocked_ids, + route_request), nav2_core::NoValidGraph); +} diff --git a/nav2_route/test/test_route_server.cpp b/nav2_route/test/test_route_server.cpp new file mode 100644 index 00000000000..17aa3b87a4f --- /dev/null +++ b/nav2_route/test/test_route_server.cpp @@ -0,0 +1,402 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/lifecycle_node.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/route_tracker.hpp" +#include "nav2_route/route_server.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +class RoutePlannerErrorTester : public RoutePlanner +{ +public: + RoutePlannerErrorTester() = default; + + Route findRoute( + Graph & graph, unsigned int start_index, unsigned int goal_index, + const std::vector & blocked_ids, + const RouteRequest & route_request) override + { + const NodePtr & start_node = &graph.at(start_index); + + // Special inputs to trigger specific exceptions for server testing + if (start_node->nodeid == 399u) { + throw std::runtime_error("runtime_error"); + } else if (start_node->nodeid == 400u) { + throw nav2_core::RouteException("RouteException"); + } else if (start_node->nodeid == 401u) { + throw nav2_core::RouteTFError("RouteTFError"); + } else if (start_node->nodeid == 402u) { + throw nav2_core::NoValidGraph("NoValidGraph"); + } else if (start_node->nodeid == 403u) { + throw nav2_core::IndeterminantNodesOnGraph("IndeterminantNodesOnGraph"); + } else if (start_node->nodeid == 404u) { + throw nav2_core::TimedOut("TimedOut"); + } else if (start_node->nodeid == 405u) { + throw nav2_core::NoValidRouteCouldBeFound("NoValidRouteCouldBeFound"); + } else if (start_node->nodeid == 406u) { + throw nav2_core::OperationFailed("OperationFailed"); + } else if (start_node->nodeid == 407u) { + throw nav2_core::InvalidEdgeScorerUse("InvalidEdgeScorerUse"); + } + + // If none set, just call the base class + return RoutePlanner::findRoute(graph, start_index, goal_index, blocked_ids, route_request); + } +}; + +class RouteServerWrapper : public RouteServer +{ +public: + explicit RouteServerWrapper(const rclcpp::NodeOptions & options) + : RouteServer(options) + {} + + void lifecycleCycle() + { + on_configure(rclcpp_lifecycle::State()); + on_activate(rclcpp_lifecycle::State()); + on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); + on_shutdown(rclcpp_lifecycle::State()); + } + + void startup() + { + on_configure(rclcpp_lifecycle::State()); + on_activate(rclcpp_lifecycle::State()); + } + + void configure() + { + on_configure(rclcpp_lifecycle::State()); + } + + void activate() + { + on_activate(rclcpp_lifecycle::State()); + } + + void shutdown() + { + on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); + } + + void testPrint( + const std::shared_ptr goal, + const std::exception & ex) + { + exceptionWarning(goal, ex); + } + + rclcpp::Duration findPlanningDurationWrapper(const rclcpp::Time & start) + { + return findPlanningDuration(start); + } + + void populateActionResultWrapper( + std::shared_ptr result, + const Route & route, + const nav_msgs::msg::Path & path, + const rclcpp::Duration & planning_duration) + { + return populateActionResult(result, route, path, planning_duration); + } + + bool isRequestValidWrapper() + { + return isRequestValid(compute_and_track_route_server_); + } + + void setNontrivialGraph() + { + graph_.resize(1); + } + + void useErrorCodePlanner() + { + route_planner_ = std::make_shared(); + } +}; + +TEST(RouteServerTest, test_lifecycle) +{ + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + server->lifecycleCycle(); + server.reset(); +} + +TEST(RouteServerTest, test_set_srv) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_route"); + std::string real_filepath = pkg_share_dir + "/graphs/aws_graph.geojson"; + + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + server->declare_parameter("graph_filepath", rclcpp::ParameterValue(real_filepath)); + auto node_thread = std::make_unique(server); + auto node2 = std::make_shared("my_node2"); + + server->startup(); + auto srv_client = + nav2_util::ServiceClient( + "route_server/set_route_graph", node2); + auto req = std::make_shared(); + req->graph_filepath = "non/existent/path.json"; + auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); + EXPECT_FALSE(resp->success); + + auto req2 = std::make_shared(); + req2->graph_filepath = real_filepath; + auto resp2 = srv_client.invoke(req2, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp2->success); + + auto req3 = std::make_shared(); + req3->graph_filepath = ament_index_cpp::get_package_share_directory("nav2_route") + + "/test/test_graphs/invalid.json"; + auto resp3 = srv_client.invoke(req3, std::chrono::nanoseconds(1000000000)); + EXPECT_FALSE(resp3->success); + + server->shutdown(); + node_thread.reset(); + server.reset(); +} + +inline Route getRoute() +{ + static Node node1, node2, node3; + node1.nodeid = 1; + node1.coords.x = 0.0; + node1.coords.y = 0.0; + node2.nodeid = 2; + node2.coords.x = 10.0; + node2.coords.y = 0.0; + node3.nodeid = 3; + node3.coords.x = 20.0; + node3.coords.y = 0.0; + + static DirectionalEdge edge1, edge2; + edge1.edgeid = 5; + edge1.start = &node1; + edge1.end = &node2; + edge2.edgeid = 6; + edge2.start = &node2; + edge2.end = &node3; + + Route route; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + return route; +} + +TEST(RouteServerTest, test_minor_utils) +{ + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + + // Find planning duration should print and provide duration + rclcpp::Time start(1000, 0, RCL_ROS_TIME); + auto dur = server->findPlanningDurationWrapper(start); + EXPECT_TRUE(dur.seconds() > 1e2); + + // This should print the goal info regarding error + nav2_msgs::action::ComputeRoute::Goal goal_raw; + auto goal = std::make_shared(goal_raw); + std::runtime_error ex("Hi:-)"); + server->testPrint(goal, ex); + + // Populate the result message with content + auto result = std::make_shared(); + Route route = getRoute(); + nav_msgs::msg::Path path; + path.poses.resize(406); + server->populateActionResultWrapper(result, route, path, dur); + EXPECT_EQ(result->path.poses.size(), path.poses.size()); + EXPECT_EQ(result->route.edges.size(), route.edges.size()); + + server.reset(); +} + +TEST(RouteServerTest, test_request_valid) +{ + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + + // Should be nullptr + EXPECT_FALSE(server->isRequestValidWrapper()); + + // Should be inactive + server->configure(); + EXPECT_FALSE(server->isRequestValidWrapper()); + + // Should be active, but graph is empty + server->activate(); + EXPECT_FALSE(server->isRequestValidWrapper()); + + // Should be valid now + server->setNontrivialGraph(); + EXPECT_TRUE(server->isRequestValidWrapper()); + + server.reset(); +} + +TEST(RouteServerTest, test_complete_action_api) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_route"); + std::string real_file = pkg_share_dir + "/graphs/aws_graph.geojson"; + + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + server->declare_parameter("graph_filepath", rclcpp::ParameterValue(real_file)); + auto node_thread = std::make_unique(server); + server->startup(); + + // Compute a simple route action request + auto node2 = std::make_shared("my_node2"); + auto compute_client = + rclcpp_action::create_client(node2, "compute_route"); + + nav2_msgs::action::ComputeRoute::Goal goal; + goal.start_id = 1u; + goal.goal_id = 1u; + goal.use_poses = false; + auto future_goal = compute_client->async_send_goal(goal); + + rclcpp::spin_until_future_complete(node2, future_goal); + auto goal_handle = future_goal.get(); + auto result_future = compute_client->async_get_result(goal_handle); + rclcpp::spin_until_future_complete(node2, result_future); + auto result = result_future.get().result; + EXPECT_EQ(result->route.edges.size(), 0u); + EXPECT_NEAR(result->route.route_cost, 0.0, 1e-6); + EXPECT_EQ(result->route.nodes[0].nodeid, 1u); + + // Compute a route and tracking request + auto track_client = + rclcpp_action::create_client( + node2, "compute_and_track_route"); + + nav2_msgs::action::ComputeAndTrackRoute::Goal goal2; + goal2.start_id = 1u; + goal2.goal_id = 2u; + goal2.use_poses = false; + auto future_goal2 = track_client->async_send_goal(goal2); + + rclcpp::spin_until_future_complete(node2, future_goal2); + auto goal_handle2 = future_goal2.get(); + + // Preempt it + rclcpp::Rate r(1.0); + r.sleep(); + auto future_goal3 = track_client->async_send_goal(goal2); + rclcpp::spin_until_future_complete(node2, future_goal3); + auto goal_handle3 = future_goal3.get(); + + // Request a reroute + auto node_int = std::make_shared("my_node2"); + auto srv_client = + nav2_util::ServiceClient( + "route_server/ReroutingService/reroute", node_int); + auto req = std::make_shared(); + auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp->success); + + // Cancel them all + track_client->async_cancel_all_goals(); + + auto result_future3 = track_client->async_get_result(goal_handle3); + rclcpp::spin_until_future_complete(node2, result_future3); + auto code = result_future3.get().code; + EXPECT_EQ(code, rclcpp_action::ResultCode::CANCELED); + + // Make sure it still shuts down completely after real work + server->shutdown(); + node_thread.reset(); + server.reset(); +} + +TEST(RouteServerTest, test_error_codes) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_route"); + std::string real_file = pkg_share_dir + "/test/test_graphs/error_codes.geojson"; + + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + server->declare_parameter("graph_filepath", rclcpp::ParameterValue(real_file)); + auto node_thread = std::make_unique(server); + server->startup(); + + // This uses the error code planner rather than the built-in planner + // to test throwing error conditions in the action and how that's handled by the server. + server->useErrorCodePlanner(); + + // Compute a simple route action request + using nav2_msgs::action::ComputeAndTrackRoute; + auto node2 = std::make_shared("my_node2"); + auto compute_client = + rclcpp_action::create_client(node2, "compute_and_track_route"); + + // These make use of poses to bypass data structure lookups and `map` to bypass TF + // Our test planner will still use the start ID + nav2_msgs::action::ComputeAndTrackRoute::Goal goal; + goal.use_start = true; + goal.use_poses = true; + goal.start.header.frame_id = "map"; + goal.goal.header.frame_id = "map"; + goal.goal.pose.position.x = 1.0; // On graph, just make sure always different + for (unsigned int i = 399; i != 408; i++) { + goal.start.pose.position.x = static_cast(i); + auto future_goal = compute_client->async_send_goal(goal); + + rclcpp::spin_until_future_complete(node2, future_goal); + auto goal_handle = future_goal.get(); + auto result_future = compute_client->async_get_result(goal_handle); + rclcpp::spin_until_future_complete(node2, result_future); + auto result = result_future.get().result; + if (i == 399u) { + EXPECT_EQ(result->error_code, i + 1u); // Also UNKNOWN, just distinguished differently + } else { + EXPECT_EQ(result->error_code, i); + } + } + + server->shutdown(); + node_thread.reset(); + server.reset(); +} diff --git a/nav2_route/test/test_route_tracker.cpp b/nav2_route/test/test_route_tracker.cpp new file mode 100644 index 00000000000..33b4dfcc173 --- /dev/null +++ b/nav2_route/test/test_route_tracker.cpp @@ -0,0 +1,356 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/route_tracker.hpp" +#include "nav2_route/route_server.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + + +class RouteTrackerWrapper : public RouteTracker +{ +public: + RouteTrackerWrapper() = default; + + void setRouteMsgSize(const int & size) + { + nav2_msgs::msg::Route msg; + msg.edges.resize(size); + route_msg_ = msg; + } +}; + +TEST(RouteTrackerTest, test_lifecycle) +{ + auto node = std::make_shared("router_test"); + + RouteTracker tracker; + std::shared_ptr costmap_subscriber; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); +} + +TEST(RouteTrackerTest, test_get_robot_pose) +{ + auto node = std::make_shared("router_test"); + auto tf = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + tf->setCreateTimerInterface(timer_interface); + auto transform_listener = std::make_shared(*tf); + tf2_ros::TransformBroadcaster broadcaster(node); + std::shared_ptr costmap_subscriber; + + RouteTracker tracker; + tracker.configure(node, tf, costmap_subscriber, nullptr, "map", "base_link"); + + EXPECT_THROW(tracker.getRobotPose(), nav2_core::RouteTFError); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.header.stamp = node->now(); + transform.child_frame_id = "base_link"; + broadcaster.sendTransform(transform); + EXPECT_NO_THROW(tracker.getRobotPose()); +} + +TEST(RouteTrackerTest, test_route_start_end) +{ + auto node = std::make_shared("router_test"); + + std::shared_ptr costmap_subscriber; + RouteTrackerWrapper tracker; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); + Route route; + route.edges.resize(7); + DirectionalEdge edge; + RouteTrackingState state; + + state.route_edges_idx = -1; + EXPECT_TRUE(tracker.isStartOrEndNode(state, route)); // Attempting to get to first node + + state.current_edge = &edge; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); // with tracking continued + + state.route_edges_idx = 0; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); + + state.route_edges_idx = 1; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); + + state.route_edges_idx = 3; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); + + state.route_edges_idx = 5; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); + + state.route_edges_idx = 6; + EXPECT_TRUE(tracker.isStartOrEndNode(state, route)); // Approaching final node +} + +TEST(RouteTrackerTest, test_feedback) +{ + auto node = std::make_shared("router_test"); + std::shared_ptr costmap_subscriber; + RouteTrackerWrapper tracker; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); + + Route route; + tracker.setRouteMsgSize(7); + std::vector ops; + + // This will segfault since there is no action server to publish feedback upon + ASSERT_EXIT( + tracker.publishFeedback(false, 0u, 1u, 2u, ops), ::testing::KilledBySignal(SIGSEGV), ".*"); +} + +TEST(RouteTrackerTest, test_node_achievement_simple) +{ + auto node = std::make_shared("router_test"); + std::shared_ptr costmap_subscriber; + + // Test with straight line to do exact analysis easier. More realistic routes in the next test + RouteTrackerWrapper tracker; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); + + // Create a demo route to test upon + Node node1, node2, node3; + node1.nodeid = 1; + node1.coords.x = 0.0; + node1.coords.y = 0.0; + node2.nodeid = 2; + node2.coords.x = 10.0; + node2.coords.y = 0.0; + node3.nodeid = 3; + node3.coords.x = 20.0; + node3.coords.y = 0.0; + + DirectionalEdge edge1, edge2; + edge1.edgeid = 5; + edge1.start = &node1; + edge1.end = &node2; + edge2.edgeid = 6; + edge2.start = &node2; + edge2.end = &node3; + + Route route; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + + RouteTrackingState state; + state.last_node = nullptr; + state.next_node = &node1; + state.current_edge = nullptr; + state.route_edges_idx = -1; + state.within_radius = false; + + geometry_msgs::msg::PoseStamped pose; + + // Test a few cases surrounding the line y = 10 where the triggering should occur. + // In a single straight line for a simple test that the mathematical criteria works exactly + // at the boundary. Tests below will test for odd / real angled routes to ensure functionality + // in realistic conditions but without having to find the exact line equations after being proven + state.last_node = &node1; + state.next_node = &node2; + state.current_edge = &edge1; + state.route_edges_idx = 0; + state.within_radius = false; + pose.pose.position.x = 10.0; // exact + pose.pose.position.y = 0.0; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; + + pose.pose.position.x = 9.99; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = 10.01; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; + + // Set some tracking error + pose.pose.position.y = 0.1; + pose.pose.position.x = 9.99; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = 10.01; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; + + // Test symmetry + pose.pose.position.y = -0.1; + pose.pose.position.x = 9.99; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = 10.01; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; +} + +TEST(RouteTrackerTest, test_node_achievement) +{ + auto node = std::make_shared("router_test"); + std::shared_ptr costmap_subscriber; + + // Minimum threshold is 2m by default + RouteTrackerWrapper tracker; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); + + // Create a demo route to test upon + Node node1, node2, node3, node4; + node1.nodeid = 1; + node1.coords.x = 0.0; + node1.coords.y = 0.0; + node2.nodeid = 2; + node2.coords.x = -10.0; + node2.coords.y = 10.0; + node3.nodeid = 3; + node3.coords.x = -10.0; + node3.coords.y = 20.0; + node4.nodeid = 4; + node4.coords.x = -20.0; + node4.coords.y = 10.0; + + DirectionalEdge edge1, edge2, edge3; + edge1.edgeid = 5; + edge1.start = &node1; + edge1.end = &node2; + edge2.edgeid = 6; + edge2.start = &node2; + edge2.end = &node3; + edge3.edgeid = 7; + edge3.start = &node3; + edge3.end = &node4; + + Route route; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + route.edges.push_back(&edge3); + + RouteTrackingState state; + state.last_node = nullptr; + state.next_node = &node1; + state.current_edge = nullptr; + state.route_edges_idx = -1; + state.within_radius = false; + + geometry_msgs::msg::PoseStamped pose; + + // Test radius for start + pose.pose.position.x = -1.5; + pose.pose.position.y = 1.5; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -0.7; + pose.pose.position.y = 0.7; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + + // Test radius for end + state.last_node = &node3; + state.next_node = &node4; + state.current_edge = &edge3; + state.route_edges_idx = 2; + state.within_radius = false; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -20.7; + pose.pose.position.y = 10.7; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + + // Test exactly on top of a node mid-execution + state.last_node = &node2; + state.next_node = &node3; + state.current_edge = &edge2; + state.route_edges_idx = 1; + state.within_radius = false; + pose.pose.position.x = -5.0; + pose.pose.position.y = 5.0; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.0; + pose.pose.position.y = 20.0; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + + // Test within radius in last iteration, and now not + state.within_radius = true; + pose.pose.position.x = -1000.0; + pose.pose.position.y = 1500.0; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; + + // Test approaching in mid-execution with acute angle edge + // A: Just on initial side in various locations + pose.pose.position.x = -9.5; + pose.pose.position.y = 19.5; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -9.9; + pose.pose.position.y = 19.2; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.0; + pose.pose.position.y = 19.9; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + // B: Just on other side in various locations + pose.pose.position.x = -10.0; + pose.pose.position.y = 20.01; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -9.9; + pose.pose.position.y = 20.4; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.5; + pose.pose.position.y = 20.5; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + + // Repeat similar edges but now with Edge1 with an obtuse angle + state.last_node = &node1; + state.next_node = &node2; + state.current_edge = &edge1; + state.route_edges_idx = 0; + state.within_radius = false; + + // A: Just on initial side + pose.pose.position.x = -9.9; + pose.pose.position.y = 9.9; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -9.7; + pose.pose.position.y = 10.0; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.3; + pose.pose.position.y = 9.7; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + // B: Just on other side + pose.pose.position.x = -10.1; + pose.pose.position.y = 10.1; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.3; + pose.pose.position.y = 10.0; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -9.5; + pose.pose.position.y = 10.5; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); +} diff --git a/nav2_route/test/test_spatial_tree.cpp b/nav2_route/test/test_spatial_tree.cpp new file mode 100644 index 00000000000..f3ffb6b298e --- /dev/null +++ b/nav2_route/test/test_spatial_tree.cpp @@ -0,0 +1,71 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_route/node_spatial_tree.hpp" + +using namespace nav2_route; // NOLINT + +TEST(NodeSpatialTreeTest, test_kd_tree) +{ + // Create a large graph of random nodes + unsigned int seed = time(NULL); + Graph graph; + unsigned int graph_size = 10000; + graph.resize(graph_size); + for (unsigned int i = 0; i != graph_size; i++) { + graph[i].nodeid = i; + graph[i].coords.x = static_cast((rand_r(&seed) % 6000000) + 1); + graph[i].coords.y = static_cast((rand_r(&seed) % 6000000) + 1); + } + + // Compute our kd tree for this graph + std::shared_ptr kd_tree = std::make_shared(); + kd_tree->computeTree(graph); + + // Test a bunch of times to find the nearest neighbor to this node + // By checking for the idx in the Kd-tree and then brute force searching + unsigned int num_tests = 50; + for (unsigned int i = 0; i != num_tests; i++) { + std::vector kd_tree_idxs; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast((rand_r(&seed) % 6000000) + 1); + pose.pose.position.y = static_cast((rand_r(&seed) % 6000000) + 1); + + if (!kd_tree->findNearestGraphNodesToPose(pose, kd_tree_idxs)) { + EXPECT_TRUE(false); // Unable to find nearest neighbor! + } + + unsigned int closest_via_brute_force = 0u; + float closest_dist = std::numeric_limits::max(); + for (unsigned int j = 0; j != graph_size; j++) { + float dist = hypotf( + pose.pose.position.x - graph[j].coords.x, + pose.pose.position.y - graph[j].coords.y); + if (dist < closest_dist) { + closest_dist = dist; + closest_via_brute_force = j; + } + } + + EXPECT_EQ(kd_tree_idxs[0], closest_via_brute_force); + } +} diff --git a/nav2_route/test/test_utils_and_types.cpp b/nav2_route/test/test_utils_and_types.cpp new file mode 100644 index 00000000000..6586e834f9f --- /dev/null +++ b/nav2_route/test/test_utils_and_types.cpp @@ -0,0 +1,346 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/types.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(TypesTest, test_metadata) +{ + Metadata mdata; + float flt = 0.8f; + std::string str = "value"; + unsigned int uintv = 17u; + mdata.setValue("key", str); + mdata.setValue("speed_limit", flt); + mdata.setValue("graph_id", uintv); + + float default_flt = 1.0f; + std::string default_str = ""; + unsigned int default_uint = 0u; + EXPECT_EQ(mdata.getValue("key", default_str), str); + EXPECT_EQ(mdata.getValue("speed_limit", default_flt), flt); + EXPECT_EQ(mdata.getValue("graph_id", default_uint), uintv); +} + +TEST(TypesTest, test_search_state) +{ + DirectionalEdge edge; + SearchState state; + state.parent_edge = &edge; + state.integrated_cost = 25; + state.traversal_cost = 250; + + state.reset(); + EXPECT_EQ(state.integrated_cost, std::numeric_limits::max()); + EXPECT_EQ(state.traversal_cost, std::numeric_limits::max()); + EXPECT_EQ(state.parent_edge, nullptr); +} + +TEST(TypesTest, test_node) +{ + Node node1, node2; + node1.nodeid = 50u; + node2.nodeid = 51u; + + EdgeCost cost; + cost.overridable = false; + cost.cost = 100.0; + EXPECT_EQ(node1.neighbors.size(), 0u); + node1.addEdge(cost, &node2, 52u); + + EXPECT_EQ(node1.neighbors.size(), 1u); + EXPECT_EQ(node1.neighbors[0].edgeid, 52u); + EXPECT_EQ(node1.neighbors[0].start->nodeid, node1.nodeid); + EXPECT_EQ(node1.neighbors[0].end->nodeid, node2.nodeid); + EXPECT_EQ(node1.neighbors[0].edge_cost.cost, 100.0); + EXPECT_FALSE(node1.neighbors[0].edge_cost.overridable); +} + +TEST(UtilsTest, test_to_msg_conversions) +{ + // Test conversion of PoseStamped + auto pose_msg = utils::toMsg(50.0, 20.0); + EXPECT_EQ(pose_msg.pose.position.x, 50.0); + EXPECT_EQ(pose_msg.pose.position.y, 20.0); + + // Test conversion of Route + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 10; + test_node2.nodeid = 11; + test_node3.nodeid = 12; + + DirectionalEdge test_edge1, test_edge2; + test_edge1.edgeid = 13; + test_edge1.start = &test_node1; + test_edge1.end = &test_node2; + test_edge2.edgeid = 14; + test_edge2.start = &test_node2; + test_edge2.end = &test_node3; + + Route route; + route.start_node = &test_node1; + route.route_cost = 50.0; + route.edges.push_back(&test_edge1); + route.edges.push_back(&test_edge2); + + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + auto route_msg = utils::toMsg(route, frame, time); + EXPECT_EQ(route_msg.header.frame_id, frame); + EXPECT_EQ(route_msg.header.stamp.nanosec, time.nanoseconds()); + EXPECT_EQ(route_msg.route_cost, 50.0); + + EXPECT_EQ(route_msg.nodes.size(), 3u); + EXPECT_EQ(route_msg.edges.size(), 2u); + + EXPECT_EQ(route_msg.nodes[0].nodeid, test_node1.nodeid); + EXPECT_EQ(route_msg.nodes[1].nodeid, test_node2.nodeid); + EXPECT_EQ(route_msg.nodes[2].nodeid, test_node3.nodeid); + EXPECT_EQ(route_msg.edges[0].edgeid, test_edge1.edgeid); + EXPECT_EQ(route_msg.edges[1].edgeid, test_edge2.edgeid); +} + +TEST(UtilsTest, test_to_visualization_msg_conversion) +{ + // Test conversion of Route Graph as MarkerArray + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + Graph graph; + graph.resize(9); + unsigned int idx = 0; + unsigned int ids = 1; + for (unsigned int i = 0; i != 3; i++) { + for (unsigned int j = 0; j != 3; j++) { + graph[idx].nodeid = ids; + graph[idx].coords.x = i; + graph[idx].coords.y = j; + idx++; + ids++; + } + } + + EdgeCost default_cost; + graph[0].addEdge(default_cost, &graph[1], ids++); + graph[1].addEdge(default_cost, &graph[0], ids++); + graph[4].addEdge(default_cost, &graph[1], ids++); + graph[1].addEdge(default_cost, &graph[4], ids++); + graph[5].addEdge(default_cost, &graph[4], ids++); + graph[4].addEdge(default_cost, &graph[5], ids++); + graph[0].addEdge(default_cost, &graph[3], ids++); + graph[3].addEdge(default_cost, &graph[6], ids++); + + auto graph_msg = utils::toMsg(graph, frame, time); + EXPECT_EQ(graph_msg.markers.size(), 34u); // 9 nodes and 8 edges (with text markers) + for (auto & marker : graph_msg.markers) { + if (marker.ns == "route_graph_ids") { + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::TEXT_VIEW_FACING); + } else if (marker.ns == "route_graph") { + EXPECT_TRUE( + (marker.type == visualization_msgs::msg::Marker::LINE_LIST) || + (marker.type == visualization_msgs::msg::Marker::SPHERE)); + } + } +} + +TEST(UtilsTest, test_normalized_dot) +{ + // Vectors are orthogonal + float v1x = 0; + float v1y = 1; + float v2x = 1; + float v2y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 0.0, 1e-4); + + // Vectors are identical + v2x = 0; + v2y = 1; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 1.0, 1e-4); + + // vectors are opposite direction + v2x = 0; + v2y = -1; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), -1.0, 1e-4); + + // One vector is null + v2x = 0; + v2y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 0.0, 1e-4); + + // Both are null + v1x = 0; + v1y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 0.0, 1e-4); + + // Try un-normalized overlap / opposite / orthogonal + v1x = 10; + v1y = 0; + v2x = 0; + v2y = 6; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 0.0, 1e-4); + + v2x = 4.5; + v2y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 1.0, 1e-4); + + v2x = -4.5; + v2y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), -1.0, 1e-4); +} + +TEST(UtilsTest, test_find_closest_point) +{ + geometry_msgs::msg::PoseStamped pose; + Coordinates start, end; + start.x = 0.0; + start.y = 0.0; + end.x = 10.0; + end.y = 0.0; + pose.pose.position.x = 0.0; + pose.pose.position.y = 0.0; + + // Test at, far from, and away from initial point + Coordinates rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = -10.0; + pose.pose.position.y = 0.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = -10.0; + pose.pose.position.y = 100.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + // Test at, far from, and away from final point + pose.pose.position.x = 10.0; + pose.pose.position.y = 0.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 10.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = 100.0; + pose.pose.position.y = 0.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 10.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = 100.0; + pose.pose.position.y = -10000.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 10.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = 1000.0; + pose.pose.position.y = 1000.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 10.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + // Test along length of the line + pose.pose.position.x = 5.0; + pose.pose.position.y = 1000.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = 0.1; + pose.pose.position.y = -10.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.1, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + // Let's try a more legit line now that we know the basics work OK + start.x = 0.0; + start.y = 0.0; + end.x = 10.0; + end.y = 10.0; + pose.pose.position.x = 5.0; + pose.pose.position.y = 5.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 5.0, 0.01); + + pose.pose.position.x = 0.0; + pose.pose.position.y = 10.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 5.0, 0.01); + + pose.pose.position.x = 10.0; + pose.pose.position.y = 0.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 5.0, 0.01); + + pose.pose.position.x = 2.0; + pose.pose.position.y = 4.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 3.0, 0.01); + EXPECT_NEAR(rtn.y, 3.0, 0.01); + + pose.pose.position.x = 4.0; + pose.pose.position.y = 10.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 7.0, 0.01); + EXPECT_NEAR(rtn.y, 7.0, 0.01); + + // Try identity to make sure no nan issues + start.x = 0.0; + start.y = 0.0; + end.x = 0.0; + end.y = 0.0; + pose.pose.position.x = 4.0; + pose.pose.position.y = 10.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); +} + +TEST(UtilsTest, test_routing_state) +{ + ReroutingState state; + state.blocked_ids.resize(10); + state.first_time = false; + state.closest_pt_on_edge.x = 1.0; + state.rerouting_start_id = 10u; + state.rerouting_start_pose.pose.position.x = 1.0; + state.reset(); + EXPECT_EQ(state.blocked_ids.size(), 0u); + EXPECT_EQ(state.first_time, true); + EXPECT_EQ(state.closest_pt_on_edge.x, 0.0); + EXPECT_EQ(state.rerouting_start_id, std::numeric_limits::max()); + EXPECT_EQ(state.rerouting_start_pose.pose.position.x, 0.0); +} diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index bd9059d6989..2b9e6c30c44 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(nav2_common REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(nav2_route REQUIRED) find_package(pluginlib REQUIRED) find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(rclcpp REQUIRED) @@ -33,13 +35,23 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/goal_pose_updater.hpp include/nav2_rviz_plugins/goal_tool.hpp include/nav2_rviz_plugins/nav2_panel.hpp + include/nav2_rviz_plugins/route_tool.hpp include/nav2_rviz_plugins/selector.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp ) + foreach(header "${nav2_rviz_plugins_headers_to_moc}") qt5_wrap_cpp(nav2_rviz_plugins_moc_files "${header}") endforeach() +qt5_wrap_ui(route_tool_UIS_H resource/route_tool.ui) + +# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTOUIC ON) +set(CMAKE_AUTORCC ON) + set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED @@ -47,11 +59,13 @@ add_library(${library_name} SHARED src/docking_panel.cpp src/goal_tool.cpp src/nav2_panel.cpp + src/route_tool.cpp src/selector.cpp src/utils.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp ${nav2_rviz_plugins_moc_files} + ${route_tool_UIS_H} ) target_include_directories(${library_name} PUBLIC ${Qt5Widgets_INCLUDE_DIRS} @@ -64,6 +78,7 @@ target_link_libraries(${library_name} PUBLIC nav2_lifecycle_manager::nav2_lifecycle_manager_core ${nav2_msgs_TARGETS} nav2_util::nav2_util_core + nav2_route::route_server_core rclcpp::rclcpp rclcpp_action::rclcpp_action rviz_common::rviz_common @@ -99,6 +114,8 @@ install( DESTINATION "share/${PROJECT_NAME}" ) +install(DIRECTORY rviz launch DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -110,6 +127,7 @@ ament_export_dependencies( geometry_msgs nav2_lifecycle_manager nav2_msgs + nav2_route nav2_util Qt5 rclcpp diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp new file mode 100644 index 00000000000..38c75f33466 --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp @@ -0,0 +1,119 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ +#define NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ + +#include +#include +#include +#include +#include +#include "geometry_msgs/msg/point_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_route/graph_loader.hpp" +#include "nav2_route/graph_saver.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rviz_common/panel.hpp" +#include "std_msgs/msg/int16.hpp" +#include "std_msgs/msg/string.hpp" + + +namespace nav2_rviz_plugins +{ +/** + * Here we declare our new subclass of rviz::Panel. Every panel which + * can be added via the Panels/Add_New_Panel menu is a subclass of + * rviz::Panel. + */ + +class RouteTool : public rviz_common::Panel +{ + /** + * This class uses Qt slots and is a subclass of QObject, so it needs + * the Q_OBJECT macro. + */ + Q_OBJECT + +public: + /** + * QWidget subclass constructors usually take a parent widget + * parameter (which usually defaults to 0). At the same time, + * pluginlib::ClassLoader creates instances by calling the default + * constructor (with no arguments). Taking the parameter and giving + * a default of 0 let's the default constructor work and also let's + * someone using the class for something else to pass in a parent + * widget as they normally would with Qt. + */ + explicit RouteTool(QWidget * parent = nullptr); + + void onInitialize() override; + + /** + * Now we declare overrides of rviz_common::Panel functions for saving and + * loading data from the config file. Here the data is the topic name. + */ + virtual void save(rviz_common::Config config) const; + virtual void load(const rviz_common::Config & config); + + + /** + * Here we declare some internal slots. + */ + +private Q_SLOTS: + void on_load_button_clicked(void); + + void on_save_button_clicked(void); + + void on_create_button_clicked(void); + + void on_confirm_button_clicked(void); + + void on_delete_button_clicked(void); + + void on_add_node_button_toggled(void); + + void on_edit_node_button_toggled(void); + + /** + * Finally, we close up with protected member variables + */ + +protected: + // UI pointer + std::unique_ptr ui_; + +private: + void update_route_graph(void); + nav2_util::LifecycleNode::SharedPtr node_; + std::shared_ptr graph_loader_; + std::shared_ptr graph_saver_; + std::shared_ptr tf_; + nav2_route::Graph graph_; + nav2_route::GraphToIDMap graph_to_id_map_; + nav2_route::GraphToIDMap edge_to_node_map_; + nav2_route::GraphToIncomingEdgesMap graph_to_incoming_edges_map_; + + rclcpp::Publisher::SharedPtr + graph_vis_publisher_; + rclcpp::Subscription::SharedPtr + clicked_point_subscription_; + + unsigned int next_node_id_ = 0; +}; +} // namespace nav2_rviz_plugins +#endif // NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ diff --git a/nav2_rviz_plugins/launch/route_tool.launch.py b/nav2_rviz_plugins/launch/route_tool.launch.py new file mode 100644 index 00000000000..e0914b9d861 --- /dev/null +++ b/nav2_rviz_plugins/launch/route_tool.launch.py @@ -0,0 +1,75 @@ +# Copyright (c) 2025 John Chrosniak +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +import launch_ros.actions + + +def generate_launch_description() -> LaunchDescription: + + # Nodes launching commands + map_file = LaunchConfiguration('yaml_filename') + + declare_map_file_cmd = DeclareLaunchArgument( + 'yaml_filename', + default_value='', + description='Full path to an occupancy grid map yaml file', + ) + + start_route_tool_cmd = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=[ + '-d' + + os.path.join( + get_package_share_directory('nav2_rviz_plugins'), + 'rviz', + 'route_tool.rviz', + ) + ], + ) + + start_map_server = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + output='screen', + parameters=[{'yaml_filename': map_file}], + ) + + start_lifecycle_manager_cmd = launch_ros.actions.Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[ + {'use_sim_time': False}, + {'autostart': True}, + {'node_names': ['map_server']}, + ], + ) + + return LaunchDescription( + [ + declare_map_file_cmd, + start_route_tool_cmd, + start_map_server, + start_lifecycle_manager_cmd, + ] + ) diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index c90944df774..0c509957230 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -16,6 +16,8 @@ nav2_lifecycle_manager nav2_msgs nav2_util + nav2_route + nav_msgs pluginlib rclcpp rclcpp_action diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index a28b1d9992a..920f9424c8b 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -36,4 +36,10 @@ The Particle Cloud rviz display. + + A tool used to create route graphs. + + diff --git a/nav2_rviz_plugins/resource/route_tool.ui b/nav2_rviz_plugins/resource/route_tool.ui new file mode 100644 index 00000000000..0db029c71d1 --- /dev/null +++ b/nav2_rviz_plugins/resource/route_tool.ui @@ -0,0 +1,386 @@ + + + + + + route_tool + + + + 0 + 0 + 394 + 461 + + + + MainWindow + + + + + + + + + + 0 + + + + + 0 + 0 + + + + Add + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + Text: + + + + + + + + + + + + + + 16777215 + 50 + + + + Field 1: + + + + + + + + 500 + 30 + + + + + + + + + + + + + 16777215 + 50 + + + + Field 2: + + + + + + + + 16777215 + 30 + + + + + + + + + + + + Create + + + + + + + + + + Edit + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + ID: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Text: + + + + + + + + + + + + + Field 1: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Field 2: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Confirm + + + + + + + + + + + + + Remove + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + ID: + + + + + + + + 16777215 + 50 + + + + + + + + + + Delete + + + + + + + + + + + + + + + + + + + + Load + + + + + + + Save + + + + + + + + + + + + + + + diff --git a/nav2_rviz_plugins/rviz/route_tool.rviz b/nav2_rviz_plugins/rviz/route_tool.rviz new file mode 100644 index 00000000000..244d39685d3 --- /dev/null +++ b/nav2_rviz_plugins/rviz/route_tool.rviz @@ -0,0 +1,150 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Route Graph1/Topic1 + - /Map1 + - /Map1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 305 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: nav2_rviz_plugins/Route Tool + Name: Route Tool +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Route Graph + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /route_graph + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 22.106815338134766 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1003 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000034dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001bc000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120072006f0075007400650054006f006f006c00000001a4000001e60000006e00fffffffb000000140052006f00750074006500200054006f006f006c01000001ff0000018b0000018b00ffffff000000010000010f0000034dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000034d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000034d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Route Tool: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1850 + X: 70 + Y: 27 + routeTool: + collapsed: false diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 96a4661878d..47539607ebb 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -54,14 +54,14 @@ void CostmapCostTool::onInitialize() rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); local_cost_client_ = std::make_shared>( - "/local_costmap/get_cost_local_costmap", - node, - false /* Does not create and spin an internal executor*/); + "/local_costmap/get_cost_local_costmap", + node, + false /* Does not create and spin an internal executor*/); global_cost_client_ = std::make_shared>( - "/global_costmap/get_cost_global_costmap", - node, - false /* Does not create and spin an internal executor*/); + "/global_costmap/get_cost_global_costmap", + node, + false /* Does not create and spin an internal executor*/); } void CostmapCostTool::activate() {} diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 0b5ee1055ff..c8a977730ee 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -252,7 +252,7 @@ DockingPanel::DockingPanel(QWidget * parent) if (!plugins_loaded_) { RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); plugins_loaded_ = true; } }); diff --git a/nav2_rviz_plugins/src/route_tool.cpp b/nav2_rviz_plugins/src/route_tool.cpp new file mode 100644 index 00000000000..767d682887e --- /dev/null +++ b/nav2_rviz_plugins/src/route_tool.cpp @@ -0,0 +1,269 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_rviz_plugins/route_tool.hpp" +#include +#include +#include +#include +#include +#include "rviz_common/display_context.hpp" + + +namespace nav2_rviz_plugins +{ +RouteTool::RouteTool(QWidget * parent) +: rviz_common::Panel(parent), + ui_(std::make_unique()) +{ + // Extend the widget with all attributes and children from UI file + ui_->setupUi(this); + node_ = std::make_shared("route_tool_node", "", rclcpp::NodeOptions()); + node_->configure(); + graph_vis_publisher_ = node_->create_publisher( + "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + node_->activate(); + tf_ = std::make_shared(node_->get_clock()); + graph_loader_ = std::make_shared(node_, tf_, "map"); + graph_saver_ = std::make_shared(node_, tf_, "map"); + ui_->add_node_button->setChecked(true); + ui_->edit_node_button->setChecked(true); + ui_->remove_node_button->setChecked(true); + // Needed to prevent memory addresses moving from resizing + // when adding nodes and edges + graph_.reserve(1000); +} + +void RouteTool::onInitialize(void) +{ + auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock(); + if (!ros_node_abstraction) { + RCLCPP_ERROR( + node_->get_logger(), "Unable to get ROS node abstraction"); + return; + } + auto node = ros_node_abstraction->get_raw_node(); + + clicked_point_subscription_ = node->create_subscription( + "clicked_point", 1, [this](const geometry_msgs::msg::PointStamped::SharedPtr msg) { + ui_->add_field_1->setText(std::to_string(msg->point.x).c_str()); + ui_->add_field_2->setText(std::to_string(msg->point.y).c_str()); + ui_->edit_field_1->setText(std::to_string(msg->point.x).c_str()); + ui_->edit_field_2->setText(std::to_string(msg->point.y).c_str()); + }); +} + +void RouteTool::on_load_button_clicked(void) +{ + graph_to_id_map_.clear(); + edge_to_node_map_.clear(); + graph_to_incoming_edges_map_.clear(); + graph_.clear(); + QString filename = QFileDialog::getOpenFileName( + this, + tr("Open Address Book"), "", + tr("Address Book (*.geojson);;All Files (*)")); + graph_loader_->loadGraphFromFile(graph_, graph_to_id_map_, filename.toStdString()); + unsigned int max_node_id = 0; + for (const auto & node : graph_) { + max_node_id = std::max(node.nodeid, max_node_id); + for (const auto & edge : node.neighbors) { + max_node_id = std::max(edge.edgeid, max_node_id); + edge_to_node_map_[edge.edgeid] = node.nodeid; + if (graph_to_incoming_edges_map_.find(edge.end->nodeid) != + graph_to_incoming_edges_map_.end()) + { + graph_to_incoming_edges_map_[edge.end->nodeid].push_back(edge.edgeid); + } else { + graph_to_incoming_edges_map_[edge.end->nodeid] = std::vector {edge.edgeid}; + } + } + } + next_node_id_ = max_node_id + 1; + update_route_graph(); +} + +void RouteTool::on_save_button_clicked(void) +{ + QString filename = QFileDialog::getSaveFileName( + this, + tr("Open Address Book"), "", + tr("Address Book (*.geojson);;All Files (*)")); + RCLCPP_INFO(node_->get_logger(), "Save graph to: %s", filename.toStdString().c_str()); + graph_saver_->saveGraphToFile(graph_, filename.toStdString()); +} + +void RouteTool::on_create_button_clicked(void) +{ + if (ui_->add_field_1->toPlainText() == "" || ui_->add_field_2->toPlainText() == "") {return;} + if (ui_->add_node_button->isChecked()) { + auto longitude = ui_->add_field_1->toPlainText().toFloat(); + auto latitude = ui_->add_field_2->toPlainText().toFloat(); + nav2_route::Node new_node; + new_node.nodeid = next_node_id_; + new_node.coords.x = longitude; + new_node.coords.y = latitude; + graph_.push_back(new_node); + graph_to_id_map_[next_node_id_++] = graph_.size() - 1; + RCLCPP_INFO(node_->get_logger(), "Adding node at: (%f, %f)", longitude, latitude); + update_route_graph(); + } else if (ui_->add_edge_button->isChecked()) { + auto start_node = ui_->add_field_1->toPlainText().toInt(); + auto end_node = ui_->add_field_2->toPlainText().toInt(); + nav2_route::EdgeCost edge_cost; + graph_[graph_to_id_map_[start_node]].addEdge( + edge_cost, &(graph_[graph_to_id_map_[end_node]]), + next_node_id_); + if (graph_to_incoming_edges_map_.find(end_node) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[end_node].push_back(next_node_id_); + } else { + graph_to_incoming_edges_map_[end_node] = std::vector {next_node_id_}; + } + edge_to_node_map_[next_node_id_++] = start_node; + RCLCPP_INFO(node_->get_logger(), "Adding edge from %d to %d", start_node, end_node); + update_route_graph(); + } + ui_->add_field_1->setText(""); + ui_->add_field_2->setText(""); +} + +void RouteTool::on_confirm_button_clicked(void) +{ + if (ui_->edit_id->toPlainText() == "" || ui_->edit_field_1->toPlainText() == "" || + ui_->edit_field_2->toPlainText() == "") {return;} + if (ui_->edit_node_button->isChecked()) { + auto node_id = ui_->edit_id->toPlainText().toInt(); + auto new_longitude = ui_->edit_field_1->toPlainText().toFloat(); + auto new_latitude = ui_->edit_field_2->toPlainText().toFloat(); + if (graph_to_id_map_.find(node_id) != graph_to_id_map_.end()) { + graph_[graph_to_id_map_[node_id]].coords.x = new_longitude; + graph_[graph_to_id_map_[node_id]].coords.y = new_latitude; + update_route_graph(); + } + } else if (ui_->edit_edge_button->isChecked()) { + auto edge_id = (unsigned int) ui_->edit_id->toPlainText().toInt(); + auto new_start = ui_->edit_field_1->toPlainText().toInt(); + auto new_end = ui_->edit_field_2->toPlainText().toInt(); + // Find and remove current edge + auto current_start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = current_start_node->neighbors.begin(); + itr != current_start_node->neighbors.end(); itr++) + { + if (itr->edgeid == edge_id) { + current_start_node->neighbors.erase(itr); + break; + } + } + // Create new edge with same ID using new start and stop nodes + nav2_route::EdgeCost edge_cost; + graph_[graph_to_id_map_[new_start]].addEdge( + edge_cost, &(graph_[graph_to_id_map_[new_end]]), + edge_id); + edge_to_node_map_[edge_id] = new_start; + if (graph_to_incoming_edges_map_.find(new_end) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[new_end].push_back(edge_id); + } else { + graph_to_incoming_edges_map_[new_end] = std::vector {edge_id}; + } + update_route_graph(); + } + ui_->edit_id->setText(""); + ui_->edit_field_1->setText(""); + ui_->edit_field_2->setText(""); +} + +void RouteTool::on_delete_button_clicked(void) +{ + if (ui_->remove_id->toPlainText() == "") {return;} + if (ui_->remove_node_button->isChecked()) { + unsigned int node_id = ui_->remove_id->toPlainText().toInt(); + // Remove edges pointing to the removed node + for (auto edge_id : graph_to_incoming_edges_map_[node_id]) { + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + start_node->neighbors.erase(itr); + edge_to_node_map_.erase(edge_id); + break; + } + } + } + if (graph_[graph_to_id_map_[node_id]].nodeid == node_id) { + // Use max int to mark the node as deleted + graph_[graph_to_id_map_[node_id]].nodeid = std::numeric_limits::max(); + graph_to_id_map_.erase(node_id); + graph_to_incoming_edges_map_.erase(node_id); + RCLCPP_INFO(node_->get_logger(), "Removed node %d", node_id); + } + update_route_graph(); + } else if (ui_->remove_edge_button->isChecked()) { + auto edge_id = (unsigned int) ui_->remove_id->toPlainText().toInt(); + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + RCLCPP_INFO(node_->get_logger(), "Removed edge %d", edge_id); + start_node->neighbors.erase(itr); + edge_to_node_map_.erase(edge_id); + break; + } + } + update_route_graph(); + } + ui_->remove_id->setText(""); +} + +void RouteTool::on_add_node_button_toggled(void) +{ + if (ui_->add_node_button->isChecked()) { + ui_->add_text->setText("Position:"); + ui_->add_label_1->setText("X:"); + ui_->add_label_2->setText("Y:"); + } else { + ui_->add_text->setText("Connections:"); + ui_->add_label_1->setText("Start Node ID:"); + ui_->add_label_2->setText("End Node ID:"); + } +} + +void RouteTool::on_edit_node_button_toggled(void) +{ + if (ui_->edit_node_button->isChecked()) { + ui_->edit_text->setText("Position:"); + ui_->edit_label_1->setText("X:"); + ui_->edit_label_2->setText("Y:"); + } else { + ui_->edit_text->setText("Connections:"); + ui_->edit_label_1->setText("Start Node ID:"); + ui_->edit_label_2->setText("End Node ID:"); + } +} + +void RouteTool::update_route_graph(void) +{ + graph_vis_publisher_->publish(nav2_route::utils::toMsg(graph_, "map", node_->now())); +} + +void RouteTool::save(rviz_common::Config config) const +{ + rviz_common::Panel::save(config); +} + +void RouteTool::load(const rviz_common::Config & config) +{ + rviz_common::Panel::load(config); +} +} // namespace nav2_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::RouteTool, rviz_common::Panel) diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index 086554573d4..1fffd8ff425 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -149,35 +149,36 @@ void Selector::setProgressChecker() void Selector::loadPlugins() { - load_plugins_thread_ = std::thread([this]() { - rclcpp::Rate rate(0.2); - while (rclcpp::ok() && !plugins_loaded_) { - RCLCPP_INFO(client_node_->get_logger(), "Trying to load plugins..."); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "controller_plugins", controller_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "planner_server", "planner_plugins", planner_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "goal_checker_plugins", - goal_checker_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "smoother_server", "smoother_plugins", smoother_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "progress_checker_plugins", - progress_checker_); - if (controller_->count() > 0 && - planner_->count() > 0 && - goal_checker_->count() > 0 && - smoother_->count() > 0 && - progress_checker_->count() > 0) - { - plugins_loaded_ = true; - } else { - RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying..."); - } - rate.sleep(); + load_plugins_thread_ = std::thread( + [this]() { + rclcpp::Rate rate(0.2); + while (rclcpp::ok() && !plugins_loaded_) { + RCLCPP_INFO(client_node_->get_logger(), "Trying to load plugins..."); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "controller_plugins", controller_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "planner_server", "planner_plugins", planner_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "goal_checker_plugins", + goal_checker_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "smoother_server", "smoother_plugins", smoother_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "progress_checker_plugins", + progress_checker_); + if (controller_->count() > 0 && + planner_->count() > 0 && + goal_checker_->count() > 0 && + smoother_->count() > 0 && + progress_checker_->count() > 0) + { + plugins_loaded_ = true; + } else { + RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying..."); } - }); + rate.sleep(); + } + }); } } // namespace nav2_rviz_plugins diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index f13cf8491bf..e84c9f4e57c 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -26,11 +26,13 @@ New as of September 2023: the simple navigator constructor will accept a `namesp | spin(spin_dist=1.57, time_allowance=10, disable_collision_checks=False) | Requests the robot to performs an in-place rotation by a given angle. | | backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10, disable_collision_checks=False) | Requests the robot to back up by a given distance. | | cancelTask() | Cancel an ongoing task request.| -| isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | -| getFeedback() | Gets feedback from task, returns action server feedback object. | +| isTaskComplete(task=RunningTask.None) | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. Provide the running task return. | +| getFeedback(task=RunningTask.None) | Gets feedback from task, returns action server feedback object. Provide the running task return. | | getResult() | Gets final result of task, to be called after `isTaskComplete` returns `True`. Returns action server result object. | | getPath(start, goal, planner_id='', use_start=False) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | | getPathThroughPoses(start, goals, planner_id='', use_start=False) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | +| getRoute(start, goal, use_start=False) | Gets a sparse route and a dense path from start to goal using the route server. The start and goal may be either `PoseStamped` or `int` NodeIDs. Returns `[nav_msgs/Path, nav_msgs/Route]`. | +| getandTrackRoute(start, goal, use_start=False) | Obtains a route and path (sent to client via feedback) and tracks progress along it to trigger internal route graph operations and rerouting mechanics. The start and goal may be either `PoseStamped` or `int` NodeIDs. | | smoothPath(path, smoother_id='', max_duration=2.0, check_for_collision=False) | Smooths a given `nav_msgs/msg/Path` path. | | changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | | clearAllCostmaps() | Clears both the global and local costmaps. | @@ -60,9 +62,9 @@ nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()` path = nav.getPath(init_pose, goal_pose) smoothed_path = nav.smoothPath(path) ... -nav.goToPose(goal_pose) -while not nav.isTaskComplete(): - feedback = nav.getFeedback() +go_to_pose_task = nav.goToPose(goal_pose) +while not nav.isTaskComplete(task=go_to_pose_task): + feedback = nav.getFeedback(task=go_to_pose_task) if feedback.navigation_duration > 600: nav.cancelTask() ... @@ -114,6 +116,8 @@ The `nav2_simple_commander` has a few examples to highlight the API functions av - `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods. - `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods. - `example_follow_path.py` - Demonstrates the path following capabilities of the navigator, as well as a number of auxiliary methods such as path smoothing. +- `example_route.py` - Demonstrates the route server's capabilities of the navigator, as well as a number of methods of how to use it, such as via waypoint following or control server path tracking (among other options). Make sure to set the `aws_graph.geojson` in your nav2_params.yaml file +- `example_assisted_teleop.py` - Demonstrates the assisted teleop's capabilities of the navigator. ## Demos The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API: diff --git a/nav2_simple_commander/launch/route_example_launch.py b/nav2_simple_commander/launch/route_example_launch.py new file mode 100644 index 00000000000..8490f70556a --- /dev/null +++ b/nav2_simple_commander/launch/route_example_launch.py @@ -0,0 +1,136 @@ +# Copyright (c) 2025 Open Navigation LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from pathlib import Path +import tempfile + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) +from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') + + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') + graph_filepath = os.path.join(nav2_bringup_dir, 'graphs', 'turtlebot4_graph.geojson') + + # Launch configuration variables + headless = LaunchConfiguration('headless') + + # Declare the launch arguments + declare_headless_cmd = DeclareLaunchArgument( + 'headless', default_value='False', description='Whether to execute gzclient)' + ) + + # start the simulation + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), + ) + + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'graph': graph_filepath}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_route', + emulate_tty=True, + output='screen') + + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + + ld = LaunchDescription() + ld.add_action(declare_headless_cmd) + ld.add_action(set_env_vars_resources) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + ld.add_action(set_env_vars_resources2) + return ld diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index f2acdfe8cb1..87e1be6aefd 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -76,14 +76,14 @@ def main() -> None: inspection_pose.pose.orientation.w = 0.707 inspection_points.append(deepcopy(inspection_pose)) - navigator.followWaypoints(inspection_points) + wpf_task = navigator.followWaypoints(inspection_points) # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) # Simply the current waypoint ID for the demonstration i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=wpf_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=wpf_task) if feedback and i % 5 == 0: print( 'Executing current waypoint: ' @@ -104,8 +104,8 @@ def main() -> None: # go back to start initial_pose.header.stamp = navigator.get_clock().now().to_msg() - navigator.goToPose(initial_pose) - while not navigator.isTaskComplete(): + go_to_pose_task = navigator.goToPose(initial_pose) + while not navigator.isTaskComplete(task=go_to_pose_task): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index c8fe7301272..d9ec76fb971 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -80,15 +80,15 @@ def main() -> None: shelf_item_pose.pose.orientation.z = -0.707 shelf_item_pose.pose.orientation.w = 0.707 print(f'Received request for item picking at {request_item_location}.') - navigator.goToPose(shelf_item_pose) + go_to_pose_task = navigator.goToPose(shelf_item_pose) # Do something during our route # (e.x. queue up future tasks or detect person for fine-tuned positioning) # Simply print information for workers on the robot's ETA for the demonstration i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_to_pose_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=go_to_pose_task) if feedback and i % 5 == 0: print( 'Estimated time of arrival at ' @@ -121,7 +121,7 @@ def main() -> None: ][1] shipping_destination.pose.orientation.z = 1.0 shipping_destination.pose.orientation.w = 0.0 - navigator.goToPose(shipping_destination) + go_to_pose_task = navigator.goToPose(shipping_destination) elif result == TaskResult.CANCELED: print( @@ -136,7 +136,7 @@ def main() -> None: f'{error_code}:{error_msg}') exit(-1) - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_to_pose_task): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py index dcc3904ee4f..2cbc5be99ec 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -50,12 +50,12 @@ def main() -> None: goal_pose.pose.position.y = 1.3 goal_pose.pose.orientation.w = 1.0 - navigator.goToPose(goal_pose) + go_to_pose_task = navigator.goToPose(goal_pose) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_to_pose_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=go_to_pose_task) if feedback and i % 5 == 0: print( f'Estimated time of arrival to destination is: \ @@ -64,23 +64,23 @@ def main() -> None: # Robot hit a dead end, back it up print("Robot hit a dead end (let's pretend), backing up...") - navigator.backup(backup_dist=0.5, backup_speed=0.1) + backup_task = navigator.backup(backup_dist=0.5, backup_speed=0.1) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=backup_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=backup_task) if feedback and i % 5 == 0: print(f'Distance traveled: {feedback.distance_traveled}') # Turn it around print('Spinning robot around...') - navigator.spin(spin_dist=3.14) + spin_task = navigator.spin(spin_dist=3.14) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=spin_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=spin_task) if feedback and i % 5 == 0: print(f'Spin angle traveled: {feedback.angular_distance_traveled}') @@ -95,8 +95,8 @@ def main() -> None: print('Returning to start...') initial_pose.header.stamp = navigator.get_clock().now().to_msg() - navigator.goToPose(initial_pose) - while not navigator.isTaskComplete(): + go_to_pose_task = navigator.goToPose(initial_pose) + while not navigator.isTaskComplete(task=go_to_pose_task): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index fa4be0761e8..9e4bff5f592 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -67,14 +67,14 @@ def main() -> None: pose.pose.position.x = pt[0] pose.pose.position.y = pt[1] route_poses.append(deepcopy(pose)) - navigator.goThroughPoses(route_poses) + go_through_poses_task = navigator.goThroughPoses(route_poses) # Do something during our route (e.x. AI detection on camera images for anomalies) # Simply print ETA for the demonstration i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_through_poses_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=go_through_poses_task) if feedback and i % 5 == 0: print( 'Estimated time to complete current route: ' diff --git a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py index be80705a85a..b04c4b9d369 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py +++ b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py @@ -42,8 +42,8 @@ def main() -> None: # Wait for navigation to fully activate, since autostarting nav2 navigator.waitUntilNav2Active() - navigator.assistedTeleop(time_allowance=20) - while not navigator.isTaskComplete(): + teleop_task = navigator.assistedTeleop(time_allowance=20) + while not navigator.isTaskComplete(task=teleop_task): # Publish twist commands to be filtered by the assisted teleop action sleep(0.2) pass diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py index 9b4439e52f6..c423f3a23dd 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -53,10 +53,10 @@ def main() -> None: smoothed_path = navigator.smoothPath(path) # Follow path - navigator.followPath(smoothed_path) + follow_path_task = navigator.followPath(smoothed_path) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=follow_path_task): ################################################ # # Implement some code here for your application! @@ -65,7 +65,7 @@ def main() -> None: # Do something with the feedback i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=follow_path_task) if feedback and i % 5 == 0: print( 'Estimated distance remaining to goal position: ' diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py index 7585093fd74..45bf3fd4e77 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -86,10 +86,10 @@ def main() -> None: # sanity check a valid path exists # path = navigator.getPathThroughPoses(initial_pose, goal_poses) - navigator.goThroughPoses(goal_poses) + nav_through_poses_task = navigator.goThroughPoses(goal_poses) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=nav_through_poses_task): ################################################ # # Implement some code here for your application! @@ -98,7 +98,7 @@ def main() -> None: # Do something with the feedback i = i + 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=nav_through_poses_task) if feedback and i % 5 == 0: print( 'Estimated time of arrival: ' diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py index 985230915a3..76855435e8d 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -66,10 +66,10 @@ def main() -> None: # sanity check a valid path exists # path = navigator.getPath(initial_pose, goal_pose) - navigator.goToPose(goal_pose) + go_to_pose_task = navigator.goToPose(goal_pose) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_to_pose_task): ################################################ # # Implement some code here for your application! @@ -78,7 +78,7 @@ def main() -> None: # Do something with the feedback i = i + 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=go_to_pose_task) if feedback and i % 5 == 0: print( 'Estimated time of arrival: ' @@ -97,7 +97,7 @@ def main() -> None: if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): goal_pose.pose.position.x = 0.0 goal_pose.pose.position.y = 0.0 - navigator.goToPose(goal_pose) + go_to_pose_task = navigator.goToPose(goal_pose) # Do something depending on the return code result = navigator.getResult() diff --git a/nav2_simple_commander/nav2_simple_commander/example_route.py b/nav2_simple_commander/nav2_simple_commander/example_route.py new file mode 100644 index 00000000000..472d8b28e6f --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_route.py @@ -0,0 +1,149 @@ +#! /usr/bin/env python3 +# Copyright 2025 Open Navigation LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from geometry_msgs.msg import Pose, PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, RunningTask, TaskResult +import rclpy +from std_msgs.msg import Header + +""" +Basic navigation demo to using the route server. +""" + + +def toPoseStamped(pt: Pose, header: Header) -> PoseStamped: + pose = PoseStamped() + pose.pose.position.x = pt.x + pose.pose.position.y = pt.y + pose.header = header + return pose + + +def main() -> None: + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 7.5 + initial_pose.pose.position.y = 7.5 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 + navigator.setInitialPose(initial_pose) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # Go to our demos first goal pose + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = 20.12 + goal_pose.pose.position.y = 11.83 + goal_pose.pose.orientation.w = 1.0 + + # Sanity check a valid route exists using PoseStamped. + # May also use NodeIDs on the graph if they are known by passing them instead as `int` + # [path, route] = navigator.getRoute(initial_pose, goal_pose) + + # May also use NodeIDs on the graph if they are known by passing them instead as `int` + route_tracking_task = navigator.getAndTrackRoute(initial_pose, goal_pose) + + # Note for the route server, we have a special route argument in the API b/c it may be + # providing feedback messages simultaneously to others (e.g. controller or WPF as below) + task_canceled = False + last_feedback = None + follow_path_task = RunningTask.NONE + while not navigator.isTaskComplete(task=route_tracking_task): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback, which contains the route / path if tracking + feedback = navigator.getFeedback(task=route_tracking_task) + while feedback is not None: + if not last_feedback or \ + (feedback.last_node_id != last_feedback.last_node_id or + feedback.next_node_id != last_feedback.next_node_id): + print('Passed node ' + str(feedback.last_node_id) + + ' to next node ' + str(feedback.next_node_id) + + ' along edge ' + str(feedback.current_edge_id) + '.') + + last_feedback = feedback + + if feedback.rerouted: # or follow_path_task == RunningTask.None + # Follow the path from the route server using the controller server + print('Passing new route to controller!') + follow_path_task = navigator.followPath(feedback.path) + + # May instead use the waypoint follower + # (or nav through poses) and use the route's sparse nodes! + # print("Passing route to waypoint follower!") + # nodes = + # [toPoseStamped(x.position, feedback.route.header) for x in feedback.route.nodes] + # navigator.followWaypoints(nodes) + # Or navigator.navigateThroughPoses(nodes) + # Consider sending only the first few and iterating + + feedback = navigator.getFeedback(task=route_tracking_task) + + # Check if followPath or WPF task is done (or failed), + # will cancel all current tasks, including route + if navigator.isTaskComplete(task=follow_path_task): + print('Controller or waypoint follower server completed its task!') + navigator.cancelTask() + task_canceled = True + + # Route server will return completed status before the controller / WPF server + # so wait for the actual robot task processing server to complete + while not navigator.isTaskComplete(task=follow_path_task) and not task_canceled: + pass + + # Do something depending on the return code + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py index a70929b1cd9..de739e82d89 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -87,10 +87,10 @@ def main() -> None: # path = navigator.getPath(initial_pose, goal_pose1) nav_start = navigator.get_clock().now() - navigator.followWaypoints(goal_poses) + follow_waypoints_task = navigator.followWaypoints(goal_poses) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=follow_waypoints_task): ################################################ # # Implement some code here for your application! @@ -99,7 +99,7 @@ def main() -> None: # Do something with the feedback i = i + 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=follow_waypoints_task) if feedback and i % 5 == 0: print( 'Executing current waypoint: ' @@ -124,7 +124,7 @@ def main() -> None: goal_pose4.pose.orientation.z = 0.0 goal_poses = [goal_pose4] nav_start = now - navigator.followWaypoints(goal_poses) + follow_waypoints_task = navigator.followWaypoints(goal_poses) # Do something depending on the return code result = navigator.getResult() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index b65dd7449de..50fd706ec57 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 # Copyright 2021 Samsung Research America +# Copyright 2025 Open Navigation LLC # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -16,17 +17,18 @@ from enum import Enum import time -from typing import Any, Optional +from typing import Any, Optional, Union from action_msgs.msg import GoalStatus from builtin_interfaces.msg import Duration from geographic_msgs.msg import GeoPose from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import (AssistedTeleop, BackUp, ComputePathThroughPoses, ComputePathToPose, - DockRobot, DriveOnHeading, FollowGPSWaypoints, FollowPath, - FollowWaypoints, NavigateThroughPoses, NavigateToPose, SmoothPath, - Spin, UndockRobot) +from nav2_msgs.action import (AssistedTeleop, BackUp, ComputeAndTrackRoute, + ComputePathThroughPoses, ComputePathToPose, ComputeRoute, DockRobot, + DriveOnHeading, FollowGPSWaypoints, FollowPath, FollowWaypoints, + NavigateThroughPoses, NavigateToPose, SmoothPath, Spin, UndockRobot) +from nav2_msgs.msg import Route from nav2_msgs.srv import (ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes) from nav_msgs.msg import Goals, OccupancyGrid, Path @@ -40,6 +42,7 @@ from rclpy.type_support import GetResultServiceResponse +# Task Result enum for the result of the task being executed class TaskResult(Enum): UNKNOWN = 0 SUCCEEDED = 1 @@ -47,17 +50,48 @@ class TaskResult(Enum): FAILED = 3 +# Task enum for the task being executed, if its a long-running task to be able to obtain +# necessary contextual information in `isTaskComplete` and `getFeedback` regarding the task +# which is running. +class RunningTask(Enum): + NONE = 0 + NAVIGATE_TO_POSE = 1 + NAVIGATE_THROUGH_POSES = 2 + FOLLOW_PATH = 3 + FOLLOW_WAYPOINTS = 4 + FOLLOW_GPS_WAYPOINTS = 5 + SPIN = 6 + BACKUP = 7 + DRIVE_ON_HEADING = 8 + ASSISTED_TELEOP = 9 + DOCK_ROBOT = 10 + UNDOCK_ROBOT = 11 + COMPUTE_AND_TRACK_ROUTE = 12 + + class BasicNavigator(Node): def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> None: super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() self.initial_pose.header.frame_id = 'map' + self.goal_handle: Optional[ClientGoalHandle[Any, Any, Any]] = None self.result_future: \ Optional[Future[GetResultServiceResponse[Any]]] = None - self.feedback: str = '' + self.feedback: Any = None self.status: Optional[int] = None + + # Since the route server's compute and track action server is likely + # to be running simultaneously with another (e.g. controller, WPF) server, + # we must track its futures and feedback separately. Additionally, the + # route tracking feedback is uniquely important to be complete and ordered + self.route_goal_handle: Optional[ClientGoalHandle[Any, Any, Any]] = None + self.route_result_future: \ + Optional[Future[GetResultServiceResponse[Any]]] = None + self.route_feedback: list[Any] = [] + + # Error code and messages from servers self.last_action_error_code = 0 self.last_action_error_msg = '' @@ -87,6 +121,9 @@ def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> N self, ComputePathThroughPoses, 'compute_path_through_poses' ) self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') + self.compute_route_client = ActionClient(self, ComputeRoute, 'compute_route') + self.compute_and_track_route_client = ActionClient(self, ComputeAndTrackRoute, + 'compute_and_track_route') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') self.drive_on_heading_client = ActionClient( @@ -136,6 +173,8 @@ def destroy_node(self) -> None: self.follow_path_client.destroy() self.compute_path_to_pose_client.destroy() self.compute_path_through_poses_client.destroy() + self.compute_and_track_route_client.destroy() + self.compute_route_client.destroy() self.smoother_client.destroy() self.spin_client.destroy() self.backup_client.destroy() @@ -152,7 +191,7 @@ def setInitialPose(self, initial_pose: PoseStamped) -> None: self.initial_pose = initial_pose self._setInitialPose() - def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> bool: + def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> Optional[RunningTask]: """Send a `NavThroughPoses` action request.""" self.clearTaskError() self.debug("Waiting for 'NavigateThroughPoses' action server") @@ -174,12 +213,12 @@ def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> bool: msg = f'NavigateThroughPoses request with {len(poses)} was rejected!' self.setTaskError(NavigateThroughPoses.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.NAVIGATE_THROUGH_POSES - def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> bool: + def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> Optional[RunningTask]: """Send a `NavToPose` action request.""" self.clearTaskError() self.debug("Waiting for 'NavigateToPose' action server") @@ -213,12 +252,12 @@ def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> bool: ) self.setTaskError(NavigateToPose.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.NAVIGATE_TO_POSE - def followWaypoints(self, poses: list[PoseStamped]) -> bool: + def followWaypoints(self, poses: list[PoseStamped]) -> Optional[RunningTask]: """Send a `FollowWaypoints` action request.""" self.clearTaskError() self.debug("Waiting for 'FollowWaypoints' action server") @@ -239,12 +278,12 @@ def followWaypoints(self, poses: list[PoseStamped]) -> bool: msg = f'Following {len(poses)} waypoints request was rejected!' self.setTaskError(FollowWaypoints.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.FOLLOW_WAYPOINTS - def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> bool: + def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> Optional[RunningTask]: """Send a `FollowGPSWaypoints` action request.""" self.clearTaskError() self.debug("Waiting for 'FollowWaypoints' action server") @@ -265,14 +304,14 @@ def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> bool: msg = f'Following {len(gps_poses)} gps waypoints request was rejected!' self.setTaskError(FollowGPSWaypoints.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.FOLLOW_GPS_WAYPOINTS def spin( self, spin_dist: float = 1.57, time_allowance: int = 10, - disable_collision_checks: bool = False) -> bool: + disable_collision_checks: bool = False) -> Optional[RunningTask]: self.clearTaskError() self.debug("Waiting for 'Spin' action server") while not self.spin_client.wait_for_server(timeout_sec=1.0): @@ -293,14 +332,15 @@ def spin( msg = 'Spin request was rejected!' self.setTaskError(Spin.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.SPIN def backup( self, backup_dist: float = 0.15, backup_speed: float = 0.025, - time_allowance: int = 10, disable_collision_checks: bool = False) -> bool: + time_allowance: int = 10, + disable_collision_checks: bool = False) -> Optional[RunningTask]: self.clearTaskError() self.debug("Waiting for 'Backup' action server") while not self.backup_client.wait_for_server(timeout_sec=1.0): @@ -322,14 +362,15 @@ def backup( msg = 'Backup request was rejected!' self.setTaskError(BackUp.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.BACKUP def driveOnHeading( self, dist: float = 0.15, speed: float = 0.025, - time_allowance: int = 10, disable_collision_checks: bool = False) -> bool: + time_allowance: int = 10, + disable_collision_checks: bool = False) -> Optional[RunningTask]: self.clearTaskError() self.debug("Waiting for 'DriveOnHeading' action server") while not self.backup_client.wait_for_server(timeout_sec=1.0): @@ -351,12 +392,12 @@ def driveOnHeading( msg = 'Drive On Heading request was rejected!' self.setTaskError(DriveOnHeading.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.DRIVE_ON_HEADING - def assistedTeleop(self, time_allowance: int = 30) -> bool: + def assistedTeleop(self, time_allowance: int = 30) -> Optional[RunningTask]: self.clearTaskError() self.debug("Wanting for 'assisted_teleop' action server") @@ -377,13 +418,13 @@ def assistedTeleop(self, time_allowance: int = 30) -> bool: msg = 'Assisted Teleop request was rejected!' self.setTaskError(AssistedTeleop.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.ASSISTED_TELEOP def followPath(self, path: Path, controller_id: str = '', - goal_checker_id: str = '') -> bool: + goal_checker_id: str = '') -> Optional[RunningTask]: self.clearTaskError() """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -406,13 +447,13 @@ def followPath(self, path: Path, controller_id: str = '', msg = 'FollowPath goal was rejected!' self.setTaskError(FollowPath.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.FOLLOW_PATH def dockRobotByPose(self, dock_pose: PoseStamped, - dock_type: str = '', nav_to_dock: bool = True) -> bool: + dock_type: str = '', nav_to_dock: bool = True) -> Optional[RunningTask]: self.clearTaskError() """Send a `DockRobot` action request.""" self.info("Waiting for 'DockRobot' action server") @@ -435,12 +476,12 @@ def dockRobotByPose(self, dock_pose: PoseStamped, msg = 'DockRobot request was rejected!' self.setTaskError(DockRobot.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.DOCK_ROBOT - def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> bool: + def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> Optional[RunningTask]: """Send a `DockRobot` action request.""" self.clearTaskError() self.info("Waiting for 'DockRobot' action server") @@ -462,12 +503,12 @@ def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> bool: msg = 'DockRobot request was rejected!' self.setTaskError(DockRobot.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.DOCK_ROBOT - def undockRobot(self, dock_type: str = '') -> bool: + def undockRobot(self, dock_type: str = '') -> Optional[RunningTask]: """Send a `UndockRobot` action request.""" self.clearTaskError() self.info("Waiting for 'UndockRobot' action server") @@ -487,10 +528,10 @@ def undockRobot(self, dock_type: str = '') -> bool: msg = 'UndockRobot request was rejected!' self.setTaskError(UndockRobot.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.UNDOCK_ROBOT def cancelTask(self) -> None: """Cancel pending task request of any type.""" @@ -500,17 +541,39 @@ def cancelTask(self) -> None: future = self.goal_handle.cancel_goal_async() rclpy.spin_until_future_complete(self, future) else: - self.error('No goal handle to cancel!') + self.error('Cancel task failed, goal handle is None') + self.setTaskError(0, 'Cancel task failed, goal handle is None') + return + if self.route_result_future: + if self.route_goal_handle is not None: + future = self.route_goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, future) + else: + self.error('Cancel route task failed, goal handle is None') + self.setTaskError(0, 'Cancel route task failed, goal handle is None') + return self.clearTaskError() return - def isTaskComplete(self) -> bool: + def isTaskComplete(self, task: RunningTask = RunningTask.NONE) -> bool: """Check if the task request of any type is complete yet.""" - if not self.result_future: + # Find the result future to spin + if task is None: + self.error('Task is None, cannot check for completion') + return False + + result_future = None + if task != RunningTask.COMPUTE_AND_TRACK_ROUTE: + result_future = self.result_future + else: + result_future = self.route_result_future + if not result_future: # task was cancelled or completed return True - rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10) - result_response = self.result_future.result() + + # Get the result of the future, if complete + rclpy.spin_until_future_complete(self, result_future, timeout_sec=0.10) + result_response = result_future.result() if result_response: self.status = result_response.status @@ -522,8 +585,7 @@ def isTaskComplete(self) -> bool: 'Task with failed with' f' status code:{self.status}' f' error code:{result.error_code}' - f' error msg:{result.error_msg}' - ) + f' error msg:{result.error_msg}') return True else: self.setTaskError(0, 'No result received') @@ -536,9 +598,13 @@ def isTaskComplete(self) -> bool: self.debug('Task succeeded!') return True - def getFeedback(self) -> str: + def getFeedback(self, task: RunningTask = RunningTask.NONE) -> Any: """Get the pending action feedback message.""" - return self.feedback + if task != RunningTask.COMPUTE_AND_TRACK_ROUTE: + return self.feedback + if len(self.route_feedback) > 0: + return self.route_feedback.pop(0) + return None def getResult(self) -> TaskResult: """Get the pending action result message.""" @@ -690,6 +756,117 @@ def getPathThroughPoses( f' error msg:{rtn.error_msg}') return None + def _getRouteImpl( + self, start: Union[int, PoseStamped], + goal: Union[int, PoseStamped], use_start: bool = False + ) -> ComputeRoute.Result: + """ + Send a `ComputeRoute` action request. + + Internal implementation to get the full result, not just the sparse route and dense path. + """ + self.debug("Waiting for 'ComputeRoute' action server") + while not self.compute_route_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputeRoute' action server not available, waiting...") + + goal_msg = ComputeRoute.Goal() + goal_msg.use_start = use_start + + # Support both ID based requests and PoseStamped based requests + if isinstance(start, int) and isinstance(goal, int): + goal_msg.start_id = start + goal_msg.goal_id = goal + goal_msg.use_poses = False + elif isinstance(start, PoseStamped) and isinstance(goal, PoseStamped): + goal_msg.start = start + goal_msg.goal = goal + goal_msg.use_poses = True + else: + self.error('Invalid start and goal types. Must be PoseStamped for pose or int for ID') + result = ComputeRoute.Result() + result.error_code = ComputeRoute.UNKNOWN + result.error_msg = 'Request type fields were invalid!' + return result + + self.info('Getting route...') + send_goal_future = self.compute_route_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle or not self.goal_handle.accepted: + self.error('Get route was rejected!') + result = ComputeRoute.Result() + result.error_code = ComputeRoute.UNKNOWN + result.error_msg = 'Get route was rejected!' + return result + + self.result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, self.result_future) + self.status = self.result_future.result().status # type: ignore[union-attr] + + return self.result_future.result().result # type: ignore[union-attr] + + def getRoute( + self, start: Union[int, PoseStamped], + goal: Union[int, PoseStamped], + use_start: bool = False) -> Optional[list[Union[Path, Route]]]: + """Send a `ComputeRoute` action request.""" + self.clearTaskError() + rtn = self._getRouteImpl(start, goal, use_start=False) + + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.setTaskError(rtn.error_code, rtn.error_msg) + self.warn( + 'Getting route failed with' + f' status code:{self.status}' + f' error code:{rtn.error_code}' + f' error msg:{rtn.error_msg}') + return None + + return [rtn.path, rtn.route] + + def getAndTrackRoute( + self, start: Union[int, PoseStamped], + goal: Union[int, PoseStamped], use_start: bool = False + ) -> Optional[RunningTask]: + """Send a `ComputeAndTrackRoute` action request.""" + self.clearTaskError() + self.debug("Waiting for 'ComputeAndTrackRoute' action server") + while not self.compute_and_track_route_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputeAndTrackRoute' action server not available, waiting...") + + goal_msg = ComputeAndTrackRoute.Goal() + goal_msg.use_start = use_start + + # Support both ID based requests and PoseStamped based requests + if isinstance(start, int) and isinstance(goal, int): + goal_msg.start_id = start + goal_msg.goal_id = goal + goal_msg.use_poses = False + elif isinstance(start, PoseStamped) and isinstance(goal, PoseStamped): + goal_msg.start = start + goal_msg.goal = goal + goal_msg.use_poses = True + else: + self.setTaskError(ComputeAndTrackRoute.UNKNOWN, 'Request type fields were invalid!') + self.error('Invalid start and goal types. Must be PoseStamped for pose or int for ID') + return None + + self.info('Computing and tracking route...') + send_goal_future = self.compute_and_track_route_client.send_goal_async(goal_msg, + self._routeFeedbackCallback) # noqa: E128 + rclpy.spin_until_future_complete(self, send_goal_future) + self.route_goal_handle = send_goal_future.result() + + if not self.route_goal_handle or not self.route_goal_handle.accepted: + msg = 'Compute and track route was rejected!' + self.setTaskError(ComputeAndTrackRoute.UNKNOWN, msg) + self.error(msg) + return None + + self.route_result_future = self.route_goal_handle.get_result_async() + return RunningTask.COMPUTE_AND_TRACK_ROUTE + def _smoothPathImpl( self, path: Path, smoother_id: str = '', max_duration: float = 2.0, check_for_collision: bool = False @@ -932,6 +1109,11 @@ def _feedbackCallback(self, msg: NavigateToPose.Feedback) -> None: self.feedback = msg.feedback return + def _routeFeedbackCallback(self, msg: ComputeAndTrackRoute.Feedback) -> None: + self.debug('Received route action feedback message') + self.route_feedback.append(msg.feedback) + return + def _setInitialPose(self) -> None: msg = PoseWithCovarianceStamped() msg.pose.pose = self.initial_pose.pose diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index 74a15c63541..ca1a6e188bc 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -27,6 +27,7 @@ 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', 'example_follow_path = nav2_simple_commander.example_follow_path:main', + 'example_route = nav2_simple_commander.example_route:main', 'demo_picking = nav2_simple_commander.demo_picking:main', 'demo_inspection = nav2_simple_commander.demo_inspection:main', 'demo_security = nav2_simple_commander.demo_security:main', diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp index 170d31d7cb7..2f793011094 100644 --- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -98,7 +98,7 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother nav_msgs::msg::Path & path, bool & reversing_segment); - bool do_refinement_; + bool do_refinement_, enforce_path_inversion_; int refinement_num_; rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")}; }; diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 1169c9d5ff9..5861f9dae0a 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -121,7 +121,7 @@ class SimpleSmoother : public nav2_core::Smoother double tolerance_, data_w_, smooth_w_; int max_its_, refinement_ctr_, refinement_num_; - bool do_refinement_; + bool do_refinement_, enforce_path_inversion_; std::shared_ptr costmap_sub_; rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")}; }; diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 1e85b02a42e..7523ffc0aa1 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -32,5 +32,6 @@ ament_cmake + diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 20b296a9f42..5ea1f4284cd 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -38,8 +38,11 @@ void SavitzkyGolaySmoother::configure( node, name + ".do_refinement", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, name + ".refinement_num", rclcpp::ParameterValue(2)); + declare_parameter_if_not_declared( + node, name + ".enforce_path_inversion", rclcpp::ParameterValue(true)); node->get_parameter(name + ".do_refinement", do_refinement_); node->get_parameter(name + ".refinement_num", refinement_num_); + node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_); } bool SavitzkyGolaySmoother::smooth( @@ -53,7 +56,11 @@ bool SavitzkyGolaySmoother::smooth( nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; - std::vector path_segments = findDirectionalPathSegments(path); + std::vector path_segments{ + PathSegment{0u, static_cast(path.poses.size() - 1)}}; + if (enforce_path_inversion_) { + path_segments = findDirectionalPathSegments(path); + } for (unsigned int i = 0; i != path_segments.size(); i++) { if (path_segments[i].end - path_segments[i].start > 9) { diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index d1957e38e35..76be2eccfed 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -47,6 +47,8 @@ void SimpleSmoother::configure( node, name + ".do_refinement", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, name + ".refinement_num", rclcpp::ParameterValue(2)); + declare_parameter_if_not_declared( + node, name + ".enforce_path_inversion", rclcpp::ParameterValue(true)); node->get_parameter(name + ".tolerance", tolerance_); node->get_parameter(name + ".max_its", max_its_); @@ -54,6 +56,7 @@ void SimpleSmoother::configure( node->get_parameter(name + ".w_smooth", smooth_w_); node->get_parameter(name + ".do_refinement", do_refinement_); node->get_parameter(name + ".refinement_num", refinement_num_); + node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_); } bool SimpleSmoother::smooth( @@ -69,7 +72,11 @@ bool SimpleSmoother::smooth( nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; - std::vector path_segments = findDirectionalPathSegments(path); + std::vector path_segments{PathSegment{ + 0u, static_cast(path.poses.size() - 1)}}; + if (enforce_path_inversion_) { + path_segments = findDirectionalPathSegments(path); + } std::lock_guard lock(*(costmap->getMutex())); diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index e3152c0bde0..14accf51e39 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -113,6 +113,7 @@ if(BUILD_TESTING) add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) + add_subdirectory(src/route) add_subdirectory(src/gps_navigation) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/spin) diff --git a/nav2_system_tests/src/behavior_tree/server_handler.cpp b/nav2_system_tests/src/behavior_tree/server_handler.cpp index b457fa6736f..9eed165cab3 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.cpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.cpp @@ -43,6 +43,10 @@ ServerHandler::ServerHandler() node_, "wait"); backup_server = std::make_unique>( node_, "backup"); + compute_route_server = std::make_unique>( + node_, "compute_route"); + smoother_server = std::make_unique>( + node_, "smooth_path"); drive_on_heading_server = std::make_unique>( node_, "drive_on_heading"); ntp_server = std::make_unique>( diff --git a/nav2_system_tests/src/behavior_tree/server_handler.hpp b/nav2_system_tests/src/behavior_tree/server_handler.hpp index 8ac79ad1afb..2b852cd085f 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.hpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -31,6 +31,8 @@ #include "nav2_msgs/action/wait.hpp" #include "nav2_msgs/action/drive_on_heading.hpp" #include "nav2_msgs/action/compute_path_through_poses.hpp" +#include "nav2_msgs/action/compute_route.hpp" +#include "nav2_msgs/action/smooth_path.hpp" #include "geometry_msgs/msg/point_stamped.hpp" @@ -119,6 +121,8 @@ class ServerHandler std::unique_ptr> spin_server; std::unique_ptr> wait_server; std::unique_ptr> backup_server; + std::unique_ptr> compute_route_server; + std::unique_ptr> smoother_server; std::unique_ptr> drive_on_heading_server; std::unique_ptr> ntp_server; diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index c33f1711bd2..3a934d0b6eb 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -50,7 +50,7 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites='', convert_types=True + source_file=params_file, root_key='', param_rewrites={}, convert_types=True ) return LaunchDescription( diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 49cee4ea48e..2efe32f7662 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -53,7 +53,7 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites='', convert_types=True + source_file=params_file, root_key='', param_rewrites={}, convert_types=True ) return LaunchDescription( diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index f46819ef7aa..4b2d6b2919b 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -67,8 +67,8 @@ def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] for controller, error_code in follow_path.items(): success = navigator.followPath(path, controller) - if success: - while not navigator.isTaskComplete(): + if success is not None: + while not navigator.isTaskComplete(task=success): time.sleep(0.5) assert ( diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index 140be5ab708..d071eba1a6e 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -46,7 +46,7 @@ def generate_launch_description() -> LaunchDescription: configured_params = RewrittenYaml( source_file=params_file, root_key='', - param_rewrites='', + param_rewrites={}, convert_types=True, ) diff --git a/nav2_system_tests/src/route/CMakeLists.txt b/nav2_system_tests/src/route/CMakeLists.txt new file mode 100644 index 00000000000..a69bbac70a4 --- /dev/null +++ b/nav2_system_tests/src/route/CMakeLists.txt @@ -0,0 +1,13 @@ +ament_add_test(test_route + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_route_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=tester_node.py + ASTAR=True + CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + PLANNER=nav2_navfn_planner::NavfnPlanner +) diff --git a/nav2_system_tests/src/route/README.md b/nav2_system_tests/src/route/README.md new file mode 100644 index 00000000000..88cab0ab93a --- /dev/null +++ b/nav2_system_tests/src/route/README.md @@ -0,0 +1 @@ +This is a series of tests using the python3 simple commander API to test the route server in a practical routing and tracking task. diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py new file mode 100755 index 00000000000..81848d5c6ce --- /dev/null +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2025 Open Navigation LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from pathlib import Path +import sys + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description() -> LaunchDescription: + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') + + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML', ''), + ) + + params_file = os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml') + graph_filepath = os.path.join(nav2_bringup_dir, 'graphs', 'turtlebot3_graph.geojson') + + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if os.getenv('ASTAR') == 'True': + param_substitutions.update({'use_astar': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} + ) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} + ) + param_substitutions.update( + {'route_server.ros__parameters.max_planning_time': '0.0001'} + ) + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, + ) + + new_yaml = configured_params.perform(context) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') + ), + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + ExecuteProcess( + cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro], + output='screen', + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'graph': graph_filepath, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) + + +def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[ + os.path.join(os.getenv('TEST_DIR', ''), os.getenv('TESTER', '')), + '-r', + '-2.0', + '-0.5', + '2.0', + '0.0', + '-e', + 'True', + ], + name='tester_node', + output='screen', + ) + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return_code = lts.run(ls) + return return_code + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/route/tester_node.py b/nav2_system_tests/src/route/tester_node.py new file mode 100755 index 00000000000..f5b6b2924ad --- /dev/null +++ b/nav2_system_tests/src/route/tester_node.py @@ -0,0 +1,492 @@ +#! /usr/bin/env python3 +# Copyright 2025 Open Navigation LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import math +import sys +import time + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import ComputeAndTrackRoute, ComputeRoute +from nav2_msgs.srv import ManageLifecycleNodes +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy +from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy +from std_srvs.srv import Trigger + + +class RouteTester(Node): + + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): + super().__init__(node_name='nav2_tester', namespace=namespace) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) + + pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) + self.initial_pose_received = False + self.initial_pose = initial_pose + self.goal_pose = goal_pose + self.compute_action_client = ActionClient(self, ComputeRoute, 'compute_route') + self.compute_track_action_client = ActionClient( + self, ComputeAndTrackRoute, 'compute_and_track_route') + self.feedback_msgs: list[ComputeAndTrackRoute.Feedback] = [] + + self.navigator = BasicNavigator() + + def runComputeRouteTest(self, use_poses: bool = True) -> bool: + # Test 1: See if we can compute a route that is valid and correctly sized + self.info_msg("Waiting for 'ComputeRoute' action server") + while not self.compute_action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'ComputeRoute' action server not available, waiting...") + + route_msg = ComputeRoute.Goal() + if use_poses: + route_msg.start = self.getStampedPoseMsg(self.initial_pose) + route_msg.goal = self.getStampedPoseMsg(self.goal_pose) + route_msg.use_start = True + route_msg.use_poses = True + else: + # Same request, just now the node IDs to test + route_msg.start_id = 7 + route_msg.goal_id = 13 + route_msg.use_start = False + route_msg.use_poses = False + + self.info_msg('Sending ComputeRoute goal request...') + send_goal_future = self.compute_action_client.send_goal_async(route_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'ComputeRoute' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status # type: ignore[union-attr] + result = get_result_future.result().result # type: ignore[union-attr] + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + + self.info_msg('Action completed! Checking validity of results...') + + # Check result for validity + assert (len(result.path.poses) == 80) + assert (result.route.route_cost > 6) + assert (result.route.route_cost < 7) + assert (len(result.route.nodes) == 5) + assert (len(result.route.edges) == 4) + assert (result.error_code == 0) + assert (result.error_msg == '') + + self.info_msg('Goal succeeded!') + return True + + def runComputeRouteSamePoseTest(self) -> bool: + # Test 2: try with the same start and goal point edge case + self.info_msg("Waiting for 'ComputeRoute' action server") + while not self.compute_action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'ComputeRoute' action server not available, waiting...") + + route_msg = ComputeRoute.Goal() + route_msg.start_id = 7 + route_msg.goal_id = 7 + route_msg.use_start = False + route_msg.use_poses = False + + self.info_msg('Sending ComputeRoute goal request...') + send_goal_future = self.compute_action_client.send_goal_async(route_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'ComputeRoute' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status # type: ignore[union-attr] + result = get_result_future.result().result # type: ignore[union-attr] + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + + self.info_msg('Action completed! Checking validity of results...') + + # Check result for validity, should be a 1-node path as its the same + assert (len(result.path.poses) == 1) + assert (len(result.route.nodes) == 1) + assert (len(result.route.edges) == 0) + assert (result.error_code == 0) + assert (result.error_msg == '') + + self.info_msg('Goal succeeded!') + return True + + def runTrackRouteTest(self) -> bool: + # Test 3: See if we can compute and track a route with proper state + self.info_msg("Waiting for 'ComputeAndTrackRoute' action server") + while not self.compute_track_action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'ComputeAndTrackRoute' action server not available, waiting...") + + route_msg = ComputeAndTrackRoute.Goal() + route_msg.goal = self.getStampedPoseMsg(self.goal_pose) + route_msg.use_start = False # Use TF pose instead + route_msg.use_poses = True + + self.info_msg('Sending ComputeAndTrackRoute goal request...') + send_goal_future = self.compute_track_action_client.send_goal_async( + route_msg, feedback_callback=self.feedback_callback) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + # Trigger a reroute + time.sleep(1) + self.info_msg('Triggering a reroute') + srv_client = self.create_client(Trigger, 'route_server/ReroutingService/reroute') + while not srv_client.wait_for_service(timeout_sec=1.0): + self.info_msg('Reroute service not available, waiting...') + req = Trigger.Request() + future = srv_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + self.info_msg('Reroute triggered') + else: + self.error_msg('Reroute failed') + return False + # Wait a bit for it to compute the route and start tracking (but no movement) + + # Cancel it after a bit + time.sleep(2) + cancel_future = goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + status = cancel_future.result() + if status is not None and len(status.goals_canceling) > 0: + self.info_msg('Action cancel completed!') + else: + self.info_msg('Goal cancel failed') + return False + + # Send it again + self.info_msg('Sending ComputeAndTrackRoute goal request...') + send_goal_future = self.compute_track_action_client.send_goal_async( + route_msg, feedback_callback=self.feedback_callback) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + # Wait a bit for it to compute the route and start tracking (but no movement) + time.sleep(2) + + # Preempt with a new request type on the graph + route_msg.use_poses = False + route_msg.start_id = 7 + route_msg.goal_id = 13 + send_goal_future = self.compute_track_action_client.send_goal_async( + route_msg, feedback_callback=self.feedback_callback) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + self.feedback_msgs = [] + + self.info_msg("Waiting for 'ComputeAndTrackRoute' action to complete") + progressing = True + last_feedback_msg = None + follow_path_task = None + while progressing: + rclpy.spin_until_future_complete(self, get_result_future, timeout_sec=0.10) + if get_result_future.result() is not None: + status = get_result_future.result().status # type: ignore[union-attr] + if status == GoalStatus.STATUS_SUCCEEDED: + progressing = False + elif status == GoalStatus.STATUS_CANCELED or status == GoalStatus.STATUS_ABORTED: + self.info_msg(f'Goal failed with status code: {status}') + return False + + # Else, processing. Check feedback + while len(self.feedback_msgs) > 0: + feedback_msg = self.feedback_msgs.pop(0) + + # Start following the path + if (last_feedback_msg and feedback_msg.path != last_feedback_msg.path): + follow_path_task = self.navigator.followPath(feedback_msg.path) + + # Check if the feedback is valid, if changed (not for route operations) + if last_feedback_msg and \ + last_feedback_msg.current_edge_id != feedback_msg.current_edge_id and \ + int(feedback_msg.current_edge_id) != 0: + if last_feedback_msg.next_node_id != feedback_msg.last_node_id: + self.error_msg('Feedback state is not tracking in order!') + return False + + last_feedback_msg = feedback_msg + + # Validate the state of the final feedback message + if last_feedback_msg is None: + self.error_msg('No feedback message received!') + return False + + if int(last_feedback_msg.next_node_id) != 0: + self.error_msg('Terminal feedback state of nodes is not correct!') + return False + if int(last_feedback_msg.current_edge_id) != 0: + self.error_msg('Terminal feedback state of edges is not correct!') + return False + if int(last_feedback_msg.route.nodes[-1].nodeid) != 13: + self.error_msg('Final route node is not correct!') + return False + + while not self.navigator.isTaskComplete(task=follow_path_task): + time.sleep(0.1) + + self.info_msg('Action completed! Checking validity of terminal condition...') + + # Check result for validity + if not self.distanceFromGoal() < 1.0: + self.error_msg('Did not make it to the goal pose!') + return False + + self.info_msg('Goal succeeded!') + return True + + def feedback_callback(self, feedback_msg: ComputeAndTrackRoute.Feedback) -> None: + self.feedback_msgs.append(feedback_msg.feedback) + + def distanceFromGoal(self) -> float: + d_x = self.current_pose.position.x - self.goal_pose.position.x + d_y = self.current_pose.position.y - self.goal_pose.position.y + distance = math.sqrt(d_x * d_x + d_y * d_y) + self.info_msg(f'Distance from goal is: {distance}') + return distance + + def info_msg(self, msg: str) -> None: + self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + + def error_msg(self, msg: str) -> None: + self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + + def setInitialPose(self) -> None: + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose + msg.header.frame_id = 'map' + self.info_msg('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + self.currentPose = self.initial_pose + + def getStampedPoseMsg(self, pose: Pose) -> PoseStamped: + msg = PoseStamped() + msg.header.frame_id = 'map' + msg.pose = pose + return msg + + def poseCallback(self, msg: PoseWithCovarianceStamped) -> None: + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + + def wait_for_node_active(self, node_name: str) -> None: + # Waits for the node within the tester namespace to become active + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{node_service} service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while state != 'active': + self.info_msg(f'Getting {node_name} state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label # type: ignore[union-attr] + self.info_msg(f'Result of get_state: {state}') + else: + self.error_msg( + f'Exception while calling service: {future.exception()!r}' + ) + time.sleep(5) + + def shutdown(self) -> None: + self.info_msg('Shutting down') + self.compute_action_client.destroy() + self.compute_track_action_client.destroy() + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down navigation lifecycle manager complete.') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down localization lifecycle manager complete') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + def wait_for_initial_pose(self) -> None: + self.initial_pose_received = False + while not self.initial_pose_received: + self.info_msg('Setting initial pose') + self.setInitialPose() + self.info_msg('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1) + + +def run_all_tests(robot_tester: RouteTester) -> bool: + # set transforms to use_sim_time + robot_tester.wait_for_node_active('amcl') + robot_tester.wait_for_initial_pose() + robot_tester.wait_for_node_active('bt_navigator') + result_poses = robot_tester.runComputeRouteTest(use_poses=True) + result_node_ids = robot_tester.runComputeRouteTest(use_poses=False) + result_same = robot_tester.runComputeRouteSamePoseTest() + result = result_poses and result_node_ids and result_same and robot_tester.runTrackRouteTest() + + if result: + robot_tester.info_msg('Test PASSED') + else: + robot_tester.error_msg('Test FAILED') + return result + + +def fwd_pose(x: float = 0.0, y: float = 0.0, z: float = 0.01) -> Pose: + initial_pose = Pose() + initial_pose.position.x = x + initial_pose.position.y = y + initial_pose.position.z = z + initial_pose.orientation.x = 0.0 + initial_pose.orientation.y = 0.0 + initial_pose.orientation.z = 0.0 + initial_pose.orientation.w = 1.0 + return initial_pose + + +def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] + # The robot(s) positions from the input arguments + parser = argparse.ArgumentParser(description='Route server tester node') + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) + args, unknown = parser.parse_known_args() + + rclpy.init() + + # Create test object + init_x, init_y, final_x, final_y = args.robot[0] + tester = RouteTester( + initial_pose=fwd_pose(float(init_x), float(init_y)), + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) + tester.info_msg( + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + ' via route server.' + ) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + # run tests + passed = run_all_tests(tester) + + tester.shutdown() + tester.info_msg('Done Shutting Down.') + + if not passed: + tester.info_msg('Exiting failed') + exit(1) + else: + tester.info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 6a82f015113..275873fe906 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() @@ -56,6 +57,7 @@ ament_export_dependencies( tf2 tf2_geometry_msgs tf2_ros + pluginlib ) ament_export_targets(${library_name}) diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 5cae40170d8..74f0df2dc60 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -20,6 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" +#include "pluginlib/exceptions.hpp" namespace nav2_util { @@ -143,11 +144,11 @@ std::string get_plugin_type_param( if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) { RCLCPP_FATAL( node->get_logger(), "Can not get 'plugin' param value for %s", plugin_name.c_str()); - throw std::runtime_error("No 'plugin' param for param ns!"); + throw pluginlib::PluginlibException("No 'plugin' param for param ns!"); } } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str()); - throw std::runtime_error("No 'plugin' param for param ns!"); + throw pluginlib::PluginlibException("No 'plugin' param for param ns!"); } return plugin_type; diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 9f32e1ffff8..883fb5451f8 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -588,17 +588,17 @@ class SimpleActionServer struct has_error_code>: std::true_type {}; template - std::string get_error_details_if_available(const T & result) + void log_error_details_if_available(const T & result) { if constexpr (has_error_code::value && has_error_msg::value) { - return " error_code:" + std::to_string(result->error_code) + - ", error_msg:'" + result->error_msg + "'."; + warn_msg("Aborting handle. error_code:" + std::to_string(result->error_code) + + ", error_msg:'" + result->error_msg + "'."); } else if constexpr (has_error_code::value) { - return " error_code:" + std::to_string(result->error_code) + "."; + warn_msg("Aborting handle. error_code:" + std::to_string(result->error_code) + "."); } else { - return "."; + warn_msg("Aborting handle."); } } @@ -619,7 +619,7 @@ class SimpleActionServer info_msg("Client requested to cancel the goal. Cancelling."); handle->canceled(result); } else { - warn_msg("Aborting handle" + get_error_details_if_available(result)); + log_error_details_if_available(result); handle->abort(result); } handle.reset(); diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 84e011cd8c6..bdc150c4120 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -29,6 +29,7 @@ tf2_geometry_msgs tf2_msgs tf2_ros + pluginlib ament_lint_common ament_lint_auto diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 96f69b54582..41261f512a5 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -27,6 +27,7 @@ target_link_libraries(${library_name} PUBLIC tf2_ros::tf2_ros tf2::tf2 ${tf2_geometry_msgs_TARGETS} + pluginlib::pluginlib ) target_link_libraries(${library_name} PRIVATE ${bond_TARGETS} diff --git a/navigation2/package.xml b/navigation2/package.xml index 45ad44912b3..325943f50d3 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -33,6 +33,7 @@ nav2_behaviors nav2_smoother nav2_regulated_pure_pursuit_controller + nav2_route nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander diff --git a/tools/pyproject.toml b/tools/pyproject.toml index b4a54ff7b49..257f616a767 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -43,8 +43,11 @@ module = [ "rcl_interfaces.*", "std_msgs.*", "zmq.*", + "std_srvs.*", "graphviz.*", "transforms3d.*", + "geopandas.*", + "pandas.*", ] ignore_missing_imports = true diff --git a/tools/update_readme_table.py b/tools/update_readme_table.py index 94e0daede86..d36b65eac72 100755 --- a/tools/update_readme_table.py +++ b/tools/update_readme_table.py @@ -18,8 +18,8 @@ import requests # Global information about current distributions, shouldn't need to update -OSs = {'humble': 'jammy', 'iron': 'jammy', 'jazzy': 'noble'} -Prefixs = {'humble': 'H', 'iron': 'I', 'jazzy': 'J'} +OSs = {'humble': 'jammy', 'jazzy': 'noble'} +Prefixs = {'humble': 'H', 'jazzy': 'J'} # Set your packages here Packages = [ @@ -39,6 +39,7 @@ 'nav2_dwb_controller', # Controller plugin for DWB packages 'nav2_graceful_controller', 'nav2_lifecycle_manager', + 'nav2_loopback_sim', 'nav2_map_server', 'nav2_mppi_controller', 'nav2_msgs', @@ -59,7 +60,7 @@ ] # Set which distributions you care about -Distros = ['humble', 'iron', 'jazzy'] +Distros = ['humble', 'jazzy'] def getSrcPath(package: str, prefix: str, OS: str) -> str: @@ -103,6 +104,7 @@ def main() -> None: else: entry += f'[![Build Status]({srcURL}badge/icon)]({srcURL}) | ' entry += f'[![Build Status]({binURL}badge/icon)]({binURL}) | ' + entry = entry[:-1] entry += '\n' body += entry