From 342816556b89294ccc3266e0b08c57dc603e3570 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 1 Jul 2020 15:59:28 -0700 Subject: [PATCH 01/23] bump to 0.4.0 (#1840) --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_bringup/bringup/package.xml | 2 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_recoveries/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 29 files changed, 29 insertions(+), 29 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index f81dec37ca2..1f4218ba9c5 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.3.4 + 0.4.0

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 5c678f58e42..75cd6909c9b 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.3.4 + 0.4.0 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index 2d764f7b723..ff8061a5743 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.3.4 + 0.4.0 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index d80f199ec2f..1407ea4292e 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.3.4 + 0.4.0 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 1639ca7edec..4238cc2261a 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.3.4 + 0.4.0 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index da741cb44a5..d3130216f9d 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.3.4 + 0.4.0 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 5bbe0ee342e..ad7ad3564c5 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.3.4 + 0.4.0 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 5750d62b3ae..0f80db29508 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.3.4 + 0.4.0 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index f208be38207..bc7be20bebd 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.3.4 + 0.4.0 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index b7bb3ed1933..a25f3caa1e3 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.3.4 + 0.4.0 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 234168543fa..3a881c12b39 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.3.4 + 0.4.0 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index e8a6be0e0a9..ff383437276 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.3.4 + 0.4.0 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index cc2fe01abe6..0b17ebf742a 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.3.4 + 0.4.0 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index f0121c20313..be0469e414f 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.3.4 + 0.4.0 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 655b3d9cc82..4e918dbfbbb 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.3.4 + 0.4.0 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 1abcf6b31b8..0f7aed4d201 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.3.4 + 0.4.0 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 8e322a300b9..3516843e1fb 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.3.4 + 0.4.0 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index f6cf8d0978a..71606074aa9 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.3.4 + 0.4.0 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index d1c9aa8e5ec..739df182749 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.3.4 + 0.4.0 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index b69bb602eca..9b121370029 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.3.4 + 0.4.0 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 7f28ff36e57..1f5b15a1c38 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.3.4 + 0.4.0 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 8fdf85eb178..8f3dc52ffd1 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.3.4 + 0.4.0 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 5bd42ecbb60..795b136dd21 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.3.4 + 0.4.0 TODO Carlos Orduno Steve Macenski diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index f8a42e9b37f..eb3fcbe057a 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.3.4 + 0.4.0 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 195973915f7..d64f3120119 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.3.4 + 0.4.0 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 48f11c8a5f6..670eb6aa093 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.3.4 + 0.4.0 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 7cc1862d4c8..7613120e2e1 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.3.4 + 0.4.0 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 95dbfbe1951..38b539bbbd5 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.3.4 + 0.4.0 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 3c3b35b7320..7c0634c7a0b 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.3.4 + 0.4.0 ROS2 Navigation Stack From aa9394a6b57a23ede7add9cbba901ba1a21a26da Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 6 Jul 2020 16:29:31 -0700 Subject: [PATCH 02/23] Adding missing deps to nav2_util (#1851) * adding launch to nav2_util * bump to 0.4.1 * adding test dep * try explicit --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_bringup/bringup/package.xml | 2 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_recoveries/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 6 +++++- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 29 files changed, 33 insertions(+), 29 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 1f4218ba9c5..e4773adaa0b 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.0 + 0.4.1

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 75cd6909c9b..faba34d4c79 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.0 + 0.4.1 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index ff8061a5743..fadb65738c2 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.4.0 + 0.4.1 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index 1407ea4292e..08df79cdffc 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.4.0 + 0.4.1 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 4238cc2261a..bf8ead1cf38 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.0 + 0.4.1 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index d3130216f9d..f51ddff6659 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.0 + 0.4.1 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index ad7ad3564c5..c678de23c75 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.0 + 0.4.1 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 0f80db29508..d9e50dcd9cb 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.0 + 0.4.1 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index bc7be20bebd..7ca65cc7c2f 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.0 + 0.4.1 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index a25f3caa1e3..490a7190f73 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.0 + 0.4.1 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 3a881c12b39..bd787e51b6f 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.0 + 0.4.1 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index ff383437276..a11a78356bc 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.0 + 0.4.1 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 0b17ebf742a..a6fc418451c 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.0 + 0.4.1 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index be0469e414f..fbc20f0ab3d 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.0 + 0.4.1 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 4e918dbfbbb..b3d19ae98b9 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.0 + 0.4.1 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 0f7aed4d201..be971939c34 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.0 + 0.4.1 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 3516843e1fb..5f0a0a0910a 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.0 + 0.4.1 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 71606074aa9..a9b16bd4867 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.0 + 0.4.1 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 739df182749..dbd43a0410a 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.0 + 0.4.1 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 9b121370029..a2d40860249 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.0 + 0.4.1 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 1f5b15a1c38..4f6a56ce193 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.0 + 0.4.1 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 8f3dc52ffd1..598fe05acbb 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.0 + 0.4.1 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 795b136dd21..327bf898a47 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.0 + 0.4.1 TODO Carlos Orduno Steve Macenski diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index eb3fcbe057a..431f12a0370 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.0 + 0.4.1 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index d64f3120119..eac98acd40d 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.0 + 0.4.1 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 670eb6aa093..57cbbd9f7e9 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.0 + 0.4.1 TODO Michael Jeronimo Mohammad Haghighipanah @@ -24,12 +24,16 @@ rclcpp_action test_msgs rclcpp_lifecycle + launch + launch_testing_ament_cmake libboost-program-options ament_lint_common ament_lint_auto ament_cmake_gtest + launch + launch_testing_ament_cmake std_srvs diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 7613120e2e1..082b4d2e07e 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.0 + 0.4.1 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 38b539bbbd5..2c5f86ea522 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.0 + 0.4.1 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 7c0634c7a0b..29065f56d8b 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.0 + 0.4.1 ROS2 Navigation Stack From 671dde625ade1092130cea68a64eca7e97435033 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 11 Aug 2020 14:27:24 -0700 Subject: [PATCH 03/23] Foxy sync 2 (July 1 to August 7) (#1932) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add slam toolbox as exec dep for nav2_bringup (#1827) * Add slam toolbox as exec dep * Added slam toolbox * Changed version of slam toolbox to foxy-devel * Added modifications to allow for exec depend of slam toolbox without breaking docker / CI * reformatted skip keys * Added tabs to skip key arguments * Removed commented out code block * Add unit tests for follow path, compute path to pose, and navigate to pose BT action nodes (#1844) * Add compute path to pose unit tests Signed-off-by: Sarthak Mittal * Add follow path action unit tests Signed-off-by: Sarthak Mittal * Add navigate to pose action unit tests Signed-off-by: Sarthak Mittal * adding foxy build icons to readme (#1853) * adding foxy build icons * adding dividers * sync master from foxy version updates (#1852) * sync master from foxy version updates * syncing foxy and master * Add unit tests for clear entire costmap and reinitialize global localization BT service nodes (#1845) * Add clear entire costmap service unit tests Signed-off-by: Sarthak Mittal * Add reinitialize global localization service unit test Signed-off-by: Sarthak Mittal * Temporarily disable dump_params test (#1856) Signed-off-by: Sarthak Mittal * commenting out unused validPointPotential (#1854) * Adding ROS2 versions to issue template (#1861) * reload behavior tree before creating RosTopicLogger to prevent nullptr error or no /behavior_tree_log problem (#1849) Signed-off-by: Daisuke Sato * Add citation for IROS paper (#1867) * Add citation We'll want to add the DOI once published * Bump citation section above build status * Fix line breaks * Changes RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED to RCUTILS_LOGGING_BUFFERED_STREAM (#1868) * Added parameters to configure amcl laser scan topic (#1870) * Added parameters to configure amcl topic * changed parameter to scan_topic and added to all the configuration files * added scan_topic amcl param to docs * Satisfied linter * Move dwb goal/progress checker plugins to nav2_controller (#1857) * Move dwb goal/progress checker plugins to nav2_controller Signed-off-by: Siddarth Gore * Move goal/progress checker plugins to nav2_controller Address review comments Signed-off-by: Siddarth Gore * Move goal/progress checker plugins to nav2_controller Use new plugin declaration format and doc update Signed-off-by: Siddarth Gore * Update bringup.yaml for new plugins in nav2_controller Also remove redundent file from dwb_plugins Signed-off-by: Siddarth Gore * Fix doc errors and update remaining yaml files Signed-off-by: Siddarth Gore * Remove mention of goal_checker from dwb docs Signed-off-by: Siddarth Gore * Add .plugin params to doc Signed-off-by: Siddarth Gore * Tests for progress_checker plugin declare .plugin only if not declared before Signed-off-by: Siddarth Gore * Tweak plugin names/description in doc Signed-off-by: Siddarth Gore * Follow pose (#1859) * Truncate path finished Signed-off-by: Francisco Martin Rico * Follow Pose finished Signed-off-by: Francisco Martin Rico * Change names. Add test and doc Signed-off-by: Francisco Martin Rico * Change names and check atan2 values Signed-off-by: Francisco Martin Rico * Checking Inf/NaNs and trucate path changes Signed-off-by: Francisco Martin Rico * Revert changes in launcher Signed-off-by: Francisco Martin Rico * Documenting Tree Signed-off-by: Francisco Martin Rico * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski * Change TruncatePath from decorator to action node Signed-off-by: Francisco Martin Rico * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski Co-authored-by: Steve Macenski * Remove Deprecated Declaration (#1884) * Remove deprecated rclcpp::executor::FutureReturnCode::SUCCESS in favor of rclcpp::FutureReturnCode::SUCCESS Signed-off-by: Hunter L. Allen * Update nav2_util/include/nav2_util/service_client.hpp Co-authored-by: Sarthak Mittal Co-authored-by: Sarthak Mittal * Added new costmap buffer to resize map safely (#1837) * Added new costmap buffer to resize map safely Signed-off-by: Aitor Miguel Blanco * Update map if the layer is not being processed. Signed-off-by: Aitor Miguel Blanco * Updated bool name and added buffer initialization Signed-off-by: Aitor Miguel Blanco * Fix dump params tests (#1886) Signed-off-by: Sarthak Mittal * Move definitions in tf_help to separate src cpp file (#1890) * Move definitions of functions in tf_help to separate src cpp file to avoid multiple definition error * Fix linting issues * Make uncrustify happier * More format fixing * resolved the simulated motion into its x & y components (#1887) Signed-off-by: Marwan Taher * Fix nav2_bt_navigator cleanup (#1901) Signed-off-by: Sarthak Mittal * Adding some more test coverage (#1903) * more tests * removing old cmake * remove on_errors in specific servers in favor of a general lifecycle warning (#1904) * remove on_errors in favor of a general lifecycle warning * adding removed thing * simply nodes to remove lines (#1907) * Bunches of random new tests (#1909) * trying to get A* to work again * make flake 8 happy * adding cancel and preempt test * planner tests use A* * adding A* note * test with topic * Adding failure to navigate test (#1912) * failures tests * adding copyrights * cancel test in recoveries * Costmap lock while copying data in navfn planner (#1913) * acquire the costmap lock while copying data in navfn planner Signed-off-by: Daisuke Sato * add costmap lock to dwb local plannger Signed-off-by: Daisuke Sato * Added map_topic parameters to static layer and amcl (#1910) * see if spin some in cancel will allow result callback (#1914) * Adding near-complete voxel grid test coverage and more to controller server (#1915) * remove erraneous handling done by prior * adding a bunch of voxel unit tests * retrigger * adding waypoint follower failure test, voxel layer integration tests, etc (#1916) * adding waypoint follower failure test * adding voxel, more logging * reset -> clear * minor changes to lower redundent warnings (#1918) * Costmap plugins declare if not declared for reset capabilities (#1919) * fixing #1917 on declare if not declared * fix API * adding CLI test (#1920) * More coverage in map server tests (#1921) * adding CLI test * adding a bunch of new coverages for map_server * Add in range sensor costmap layer (#1888) * range costmap building * range sensor layer working * nav2 costmap pass linter and uncrustify tests * Added back semicolon to range class * Added docs * Added angles dependency * Added BSD license * Added BSD license * Made functions inline * revmoed get_clock * added input_sensor_type to docs * Made defualt topic name empty to cause error * using chorno literal to denote time units * Added small initial test * Fixed linter error, line breaks, enum, logger level, and transform_tolerance issue * fixed segmentation fault in test and added transfrom tolerances to tf_->transform * Got test to pass * Added more tests * removed incorrect parameter declaration * Improved marking while moving tests and added clearing tests * removed general clearing test * changed testing helper import in test * [Test sprint] push nav2_map_server over 90% (#1922) * adding CLI test * adding tests for more lines to map_server * fix last test * adding out of bounds and higher bounds checks * adding planner tests for plugins working * add cleanup * working * ping * [testing sprint] final test coverage and debug logging in planner/controller servers (#1924) * adding CLI test * adding tests for more lines to map_server * fix last test * adding out of bounds and higher bounds checks * adding planner tests for plugins working * add cleanup * working * ping * adding more testing and debug info messages * [testing sprint] remove dead code not used in 10 years from navfn (#1926) * remove dead code not used in 10 years from navfn * ping * inverting period to rate * removing debug statement that could never be triggered by a single non-looping action server * type change * adding removed files * bump to 0.4.2 * add another missing file * add missing files * remove some tests * lint * removing nav2_bringup from binaries Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com> Co-authored-by: Sarthak Mittal Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Co-authored-by: Ruffin Co-authored-by: Siddarth Gore Co-authored-by: Francisco Martín Rico Co-authored-by: Hunter L. Allen Co-authored-by: Aitor Miguel Blanco Co-authored-by: Joe Smallman Co-authored-by: Marwan Taher --- .circleci/config.yml | 3 + .github/ISSUE_TEMPLATE.md | 2 + Dockerfile | 6 + README.md | 70 ++- doc/parameters/param_list.md | 90 ++- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 4 +- nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 19 +- nav2_behavior_tree/CMakeLists.txt | 6 + .../plugins/action/back_up_action.hpp | 48 ++ .../plugins/action/clear_costmap_service.hpp | 38 ++ .../action/compute_path_to_pose_action.hpp | 55 ++ .../plugins/action/follow_path_action.hpp | 50 ++ .../action/navigate_to_pose_action.hpp | 51 ++ ...initialize_global_localization_service.hpp | 36 ++ .../plugins/action/spin_action.hpp | 47 ++ .../plugins/action/truncate_path_action.hpp | 55 ++ .../plugins/action/wait_action.hpp | 48 ++ .../plugins/decorator/goal_updater_node.hpp | 61 ++ nav2_behavior_tree/package.xml | 2 +- .../plugins/action/clear_costmap_service.cpp | 31 +- .../action/compute_path_to_pose_action.cpp | 66 +-- .../plugins/action/follow_path_action.cpp | 67 +-- .../action/navigate_to_pose_action.cpp | 64 +-- ...initialize_global_localization_service.cpp | 26 +- .../plugins/action/truncate_path_action.cpp | 89 +++ .../plugins/decorator/goal_updater_node.cpp | 71 +++ nav2_behavior_tree/test/CMakeLists.txt | 1 + .../test/plugins/action/CMakeLists.txt | 25 + .../plugins/action/test_back_up_action.cpp | 173 ++++++ .../action/test_clear_costmap_service.cpp | 130 +++++ .../test_compute_path_to_pose_action.cpp | 198 +++++++ .../action/test_follow_path_action.cpp | 188 +++++++ .../action/test_navigate_to_pose_action.cpp | 179 ++++++ ...initialize_global_localization_service.cpp | 126 +++++ .../test/plugins/action/test_spin_action.cpp | 168 ++++++ .../action/test_truncate_path_action.cpp | 156 ++++++ .../test/plugins/action/test_wait_action.cpp | 169 ++++++ .../test/plugins/decorator/CMakeLists.txt | 4 + .../decorator/test_goal_updater_node.cpp | 149 +++++ .../test/test_action_server.hpp | 82 +++ nav2_behavior_tree/test/test_service.hpp | 60 ++ nav2_bringup/bringup/launch/bringup_launch.py | 2 +- .../bringup/launch/localization_launch.py | 2 +- .../bringup/launch/navigation_launch.py | 2 +- nav2_bringup/bringup/package.xml | 3 +- .../params/nav2_multirobot_params_1.yaml | 14 + .../params/nav2_multirobot_params_2.yaml | 14 + nav2_bringup/bringup/params/nav2_params.yaml | 54 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/README.md | 18 +- .../behavior_trees/follow_point.xml | 21 + nav2_bt_navigator/doc/follow_point.png | Bin 0 -> 42755 bytes .../nav2_bt_navigator/bt_navigator.hpp | 5 - nav2_bt_navigator/package.xml | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 39 +- nav2_common/package.xml | 2 +- nav2_controller/CMakeLists.txt | 37 +- .../nav2_controller/nav2_controller.hpp | 27 +- .../plugins}/simple_goal_checker.hpp | 10 +- .../simple_progress_checker.hpp} | 40 +- .../plugins}/stopped_goal_checker.hpp | 12 +- nav2_controller/package.xml | 4 +- nav2_controller/plugins.xml | 17 + .../plugins}/simple_goal_checker.cpp | 8 +- .../simple_progress_checker.cpp} | 36 +- .../plugins}/stopped_goal_checker.cpp | 8 +- nav2_controller/plugins/test/CMakeLists.txt | 4 + .../plugins}/test/goal_checker.cpp | 12 +- .../plugins/test/progress_checker.cpp | 135 +++++ nav2_controller/src/nav2_controller.cpp | 118 ++-- nav2_core/include/nav2_core/controller.hpp | 14 - .../include/nav2_core/progress_checker.hpp | 62 +++ nav2_core/package.xml | 2 +- nav2_costmap_2d/CMakeLists.txt | 3 + nav2_costmap_2d/costmap_plugins.xml | 3 + .../nav2_costmap_2d/costmap_2d_ros.hpp | 1 - .../nav2_costmap_2d/range_sensor_layer.hpp | 138 +++++ .../include/nav2_costmap_2d/static_layer.hpp | 2 + nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 11 +- .../plugins/range_sensor_layer.cpp | 524 ++++++++++++++++++ nav2_costmap_2d/plugins/static_layer.cpp | 37 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 15 +- nav2_costmap_2d/src/layered_costmap.cpp | 1 + .../test/integration/CMakeLists.txt | 21 + .../test/integration/inflation_tests.cpp | 2 +- .../test/integration/obstacle_tests.cpp | 2 +- .../test/integration/range_tests.cpp | 293 ++++++++++ .../test_costmap_topic_collision_checker.cpp | 2 +- .../testing_helper.hpp | 11 + nav2_dwb_controller/costmap_queue/package.xml | 2 +- .../include/dwb_core/dwb_local_planner.hpp | 17 - nav2_dwb_controller/dwb_core/package.xml | 2 +- .../dwb_core/src/dwb_local_planner.cpp | 54 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- .../dwb_plugins/CMakeLists.txt | 17 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../dwb_plugins/test/CMakeLists.txt | 3 - .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- .../nav_2d_utils/CMakeLists.txt | 12 +- .../include/nav_2d_utils/tf_help.hpp | 66 +-- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- .../nav_2d_utils/src/tf_help.cpp | 115 ++++ nav2_lifecycle_manager/package.xml | 2 +- .../src/lifecycle_manager_client.cpp | 2 +- .../include/nav2_map_server/map_saver.hpp | 6 - .../include/nav2_map_server/map_server.hpp | 6 - nav2_map_server/package.xml | 2 +- nav2_map_server/src/map_saver/main_server.cpp | 15 +- nav2_map_server/src/map_saver/map_saver.cpp | 8 - nav2_map_server/src/map_server/main.cpp | 15 +- nav2_map_server/src/map_server/map_server.cpp | 7 - nav2_map_server/test/CMakeLists.txt | 1 + .../test/component/test_map_server_node.cpp | 1 + .../test/map_saver_cli/CMakeLists.txt | 13 + .../test/map_saver_cli/test_map_saver_cli.cpp | 146 +++++ nav2_msgs/package.xml | 2 +- .../include/nav2_navfn_planner/navfn.hpp | 8 +- .../nav2_navfn_planner/navfn_planner.hpp | 4 +- nav2_navfn_planner/package.xml | 2 +- nav2_navfn_planner/src/navfn.cpp | 92 +-- nav2_navfn_planner/src/navfn_planner.cpp | 90 +-- .../include/nav2_planner/planner_server.hpp | 23 +- nav2_planner/package.xml | 2 +- nav2_planner/src/planner_server.cpp | 114 ++-- .../nav2_recoveries/recovery_server.hpp | 1 - nav2_recoveries/package.xml | 2 +- nav2_recoveries/plugins/back_up.cpp | 3 +- nav2_recoveries/src/recovery_server.cpp | 7 - nav2_recoveries/test/test_recoveries.cpp | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_rviz_plugins/src/nav2_panel.cpp | 8 +- nav2_system_tests/CMakeLists.txt | 3 +- nav2_system_tests/package.xml | 2 +- nav2_system_tests/src/planning/CMakeLists.txt | 9 + .../src/planning/planner_tester.cpp | 6 + .../src/planning/test_planner_plugins.cpp | 71 +++ .../backup/backup_recovery_tester.cpp | 4 +- .../recoveries/spin/spin_recovery_tester.cpp | 4 +- .../wait/test_wait_recovery_node.cpp | 10 +- .../recoveries/wait/wait_recovery_tester.cpp | 81 ++- .../recoveries/wait/wait_recovery_tester.hpp | 2 + nav2_system_tests/src/system/CMakeLists.txt | 7 +- .../src/system/test_system_launch.py | 3 +- nav2_system_tests/src/system/tester_node.py | 8 +- .../src/system_failure/CMakeLists.txt | 12 + .../src/system_failure/README.md | 3 + .../test_system_failure_launch.py | 106 ++++ .../src/system_failure/tester_node.py | 307 ++++++++++ .../src/waypoint_follower/tester.py | 44 +- .../include/nav2_util/lifecycle_node.hpp | 8 + .../include/nav2_util/service_client.hpp | 4 +- nav2_util/package.xml | 4 +- nav2_util/test/CMakeLists.txt | 32 +- nav2_util/test/test_actions.cpp | 57 +- ...ch.py => test_dump_params_default.test.py} | 36 -- .../test_dump_params_md.test.py | 90 +++ .../test_dump_params_multiple.test.py | 96 ++++ .../test_dump_params_yaml.test.py | 90 +++ nav2_voxel_grid/package.xml | 2 +- nav2_voxel_grid/test/voxel_grid_tests.cpp | 40 ++ .../waypoint_follower.hpp | 6 +- nav2_waypoint_follower/package.xml | 2 +- .../src/waypoint_follower.cpp | 14 +- navigation2/package.xml | 2 +- 168 files changed, 5823 insertions(+), 937 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/truncate_path_action.cpp create mode 100644 nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/CMakeLists.txt create mode 100644 nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_spin_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_wait_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp create mode 100644 nav2_behavior_tree/test/test_action_server.hpp create mode 100644 nav2_behavior_tree/test/test_service.hpp create mode 100644 nav2_bt_navigator/behavior_trees/follow_point.xml create mode 100644 nav2_bt_navigator/doc/follow_point.png rename {nav2_dwb_controller/dwb_plugins/include/dwb_plugins => nav2_controller/include/nav2_controller/plugins}/simple_goal_checker.hpp (92%) rename nav2_controller/include/nav2_controller/{progress_checker.hpp => plugins/simple_progress_checker.hpp} (63%) rename {nav2_dwb_controller/dwb_plugins/include/dwb_plugins => nav2_controller/include/nav2_controller/plugins}/stopped_goal_checker.hpp (88%) create mode 100644 nav2_controller/plugins.xml rename {nav2_dwb_controller/dwb_plugins/src => nav2_controller/plugins}/simple_goal_checker.cpp (94%) rename nav2_controller/{src/progress_checker.cpp => plugins/simple_progress_checker.cpp} (65%) rename {nav2_dwb_controller/dwb_plugins/src => nav2_controller/plugins}/stopped_goal_checker.cpp (93%) create mode 100644 nav2_controller/plugins/test/CMakeLists.txt rename {nav2_dwb_controller/dwb_plugins => nav2_controller/plugins}/test/goal_checker.cpp (94%) create mode 100644 nav2_controller/plugins/test/progress_checker.cpp create mode 100644 nav2_core/include/nav2_core/progress_checker.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp create mode 100644 nav2_costmap_2d/plugins/range_sensor_layer.cpp create mode 100644 nav2_costmap_2d/test/integration/range_tests.cpp rename nav2_costmap_2d/{include/nav2_costmap_2d => test}/testing_helper.hpp (90%) create mode 100644 nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp create mode 100644 nav2_map_server/test/map_saver_cli/CMakeLists.txt create mode 100644 nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp create mode 100644 nav2_system_tests/src/planning/test_planner_plugins.cpp create mode 100644 nav2_system_tests/src/system_failure/CMakeLists.txt create mode 100644 nav2_system_tests/src/system_failure/README.md create mode 100755 nav2_system_tests/src/system_failure/test_system_failure_launch.py create mode 100755 nav2_system_tests/src/system_failure/tester_node.py rename nav2_util/test/test_dump_params/{test_dump_params.launch.py => test_dump_params_default.test.py} (68%) create mode 100644 nav2_util/test/test_dump_params/test_dump_params_md.test.py create mode 100644 nav2_util/test/test_dump_params/test_dump_params_multiple.test.py create mode 100644 nav2_util/test/test_dump_params/test_dump_params_yaml.test.py diff --git a/.circleci/config.yml b/.circleci/config.yml index 7e6fed842ab..ef14bf0d667 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -75,6 +75,9 @@ references: rosdep install -q -y \ --from-paths src \ --ignore-src \ + --skip-keys " \ + slam_toolbox \ + " \ --verbose | \ awk '$1 ~ /^resolution\:/' | \ awk -F'[][]' '{print $2}' | \ diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index d2d3108e77b..884227ea104 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -11,6 +11,8 @@ For Bug report or feature requests, please fill out the relevant category below - Operating System: - +- ROS2 Version: + - - Version or commit hash: - + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/doc/follow_point.png b/nav2_bt_navigator/doc/follow_point.png new file mode 100644 index 0000000000000000000000000000000000000000..825bc4e16c842f6825a36dccf2146e4f858b761e GIT binary patch literal 42755 zcmdqIcT`i|*Dgvgf=ZDly`%Jm-djTGp%bK&&^w_73q?Ss_YP7NL3$CSqjaPQ(nN{^ zO79&6?#}yuzk9xM%Q@qYasImhBs)86&o$Sx=6vQ{Yt8)%uB(3g*8N*JI5@X8U}^?9 zIC#-GIJl2Vh=4oI37?X2a7ZH25HqxUpo6m;3Wpo4`tKDtNW{t07tIY;;|75sZYX=S zkEaK43EX%0L^?Y-qmcjp2ND5^2#QDwg1|B_xxp&HmA12oUm)-uC?#PnA@%^cQugw4Lz$xNw4Bl0AgDN4QV1-8eMAdp zq@&FZQU$KvoLy1CmpaPc)f4*&(#JMH$k`KUE(#VB6A}k*sXF<1x&w`%QeYtwAu&-A zu#o6Ky8n-nK&6F5|Ix-g{ZV6K`fQYJbWF-Zd-Ejt}oRZ(fg zzvJ>p`S>~mp!@^Tf4as#jK+AO{=JJt`8(SKdVsjW5CB@Fv#q0#tvk@f`M=Op6BmKF zfUV8l+`UY^F&Gz9J56A?SRo#^zySUc4F>=e6#Z9-qzM{{l+X}Uhl`1tp%H3g#(L5) zb+Atm+8qfu2Z8J*ZKb8W^$m59AbnposIi9$HwXY4s*KRmFz`0kb5^(4(-rYS`Kv(1 zfaz3mkk&vu2O9W7y@S*tVpzZsQU+>1%2H4SQbH1;=BVy!s}GZey84(Js%zL97crsRhXNnF;v;a#XwIrKua?KV~B`}=8VifAIVY<(r&^#h#( z#qI1o-F^MU#6-nBF`Ck&KPX(OoR|E+BIs5wtqUUc(%)MHLN{ikAz-(cRwOPz#|f2281nh?^$x3D?$8G4{4~($;lA zWBk>nK<4(YP^|qq8pG_=O+0!4>5EJ zFc61&8pB~GU_hLUo2!neniI;?1EOrJ21gjVXo1m&`cmFlhfwh{Rq^o>SMzhhsQVz5 z{e#pcR6JDlG_ZQ;z?@Xg+?D+yNHcweet?*ofsd%Vp0b*~m#dSbvkt^Z4WT0DAr7o_Wf)^-YX0c$uq0fK$~+(gY`s5vY!aD;Vv7aSueQ==nMN8;R*dK~AD>I=bGX9-78FZtCJ-KM5Zx zBTcx6x~-`i3TbDE)B)MLXc?kSQLc7@YDVT_lKLJ#FjqLj+u6XtR@E2Vje)ZYOkFC- zQ9MA*$k$m_O~v0eKm;wS>*Z(UXsBkYr;RXiQ*nSojFcU0r2@5dMIb5$D&8(y4i5TS zVgdT%0ghm>qn)y-pQoy%eUOg7yAQ+{;tlsQaupQ;*}1rxnE)&XML_jY7++7o@l_;{ zVn~Fpo2diH6CrBkY;J3SLI%O0+6JyDu(q>wkgldV*w7#V>T47fASR{@)6z23)YOES zYZz&Pyiu-T4_{Fah=&^>$wSB2TTMa)5@c)$b#yScm&W31;t5d~x7G78c7nqlHFP8c z!3L-Rdw-z0wyBzVps$FFuav8?w>!*M4C8DcS8Ej=mn596d~oRrtJup z@HaJ(KzMl~H2gt&o@o0ZBToYaLLIFGw{=E)BK6E9jJyM-#huiV7!Maoh?RNWX z;t(Hcu(~u*EIy6Yv9WJH^Qu9E-QQpSNScY*>^%FtZX^85YxdEEl z>UxSG&B2;lUVd&UJ5vu^fW_Pc0SbXhKwt()U$}=pI!Hv+UBX0E&(z3AFF-rM)mao{ z?%*V;6(Hj2Y6$aiHS#x=uyd615A?<|fCSJ=M;Yy4Z))cO3V@h;!qEO;bwA)AUmaIp z2UTfT7pSYVhhd<%4|Zzpl%d{^x?nw&3fN4|QN_bl)I`EeRSF67({eV{H}=){z&Mz~ z^&D)~?NuRxwjj6!%EZXWOj6Zf#81i|gNxVFw&Ct(SzC9 zOPY%r1bEq@?R>SwVSWe+06d^Kq@6PmJ5=mUFhD?6(MJWq41=VAfvcFJ4UIKLk+z}+ zK`ua)fNJY#VI;i*%=P{JyzPv19rV0JZ1w$YwcJ72=YxXWjgczeU_X0t3BXhJP1Wsy zz^xKskL4gc1Aku$v;j&@$ITbv>uiQb>gb@cG~tGVo2yIuz=GhWU0fu<|=?K8VVZ+f-Q#J~;+Oi~0xz{t>)|upM1=1Rkb|fIA!M_!`7()LV z@hK!K>Sc(6?-K>;i+f-mBLX#_5$V+&+t3I?{~O@aZg!^VowaO$>;aM(HYV!F+%k%OZsgcQr?-~Mozf%; z0=F`~;uMC`)Q(&w3di%<*htPe*^v5wp0iO9laOTYH6%eg7N)9VbSbJCaoMB&U6n5A z2<$>a5?;}VxY^ScS~Gr1uOtkWQc~)-W8yGz+Jbd zc!`Ur!efV18*BOUnLB}XGcCaq4j|DxAIVV2#)ka-!&m8O)BS32jR3cWlA_kBk?2jZ zG8+*f?C zq4a<}4kET6GZAak(7m^=%4~NiC~E16HPhH{0ZO#W6RZ928&nM&^=|v6Q2Y5G#-S{6 zGQwN;I(Oz<^lOm=`9+r-WmR5>!;(ctr3N3jYQO9)MGF@O9dEEFe!Sdi`EZghY#k?u zIS4Nwy@mRc2HBZw`p|K>IvT(IqjCA?r{_+y^$)11sOlw7cIKniSuh0g@+TPu61gT1 zXfOO`GaxL$9KU47Q=zqg@!^PzWF}InyuW*{$#<+kmH5rOcgwi}3!m`s%2A+)%DbA= zEqq9Mv@>kz(|>q&SaL>}nN%E5w!wu1HYP~Hjw46LEr%nr`vbhMZASCtVcOagc{gXv z@iVok=!71)&ERTS2pdH@2?-)b>%*7)<=dfVNj;yQ6Wfm!LQ1SV6w(9`KDZNqxAyy3 z`?kKJ$_Na`7PvkkK+TB0dtF z8m?$gXp9mdO*~!fm2EVwU~S3Ih94eXG^Q4z*&fXgc#!+R$PncTp9G@o!;9a(W$<^G zb=U3D?9gRw?B^sQ!K+aQY|zas&58i`5#>vo%zW2OWa54>o%IvZot9mN*3*T@?KP}2 zXp-&O4_#T}uJAXoM6!Mkd%BLx9a*DtGyQ16t+68Fg=d%Uf`7c`K6=wj`@DfFY3!I0 z-M$}wx){uA+2Ri=P>E0?v-R--T^w(|&{MoJ;v5L&rbi%|8?>6%Ypq)RE2egVwpBgq zg`l^LwsX`N99U2-Du_4d=Q+mb&k8W&W~4}RC!e8Q`D z{zjyDbwpj+e&MossV9G&5-)II+ZJ>TZ}LI4{yq4@XIM<$8^iG2wkz@%pxJbiV@wic zsz^M=!^28KrKsjTFgkHP1)@6O{ z+hllQAz*+$*0kGR8ij?Fl5|IDpCv<@6O)sY#OU@@9vF&|vDd|Eupq(^wD`n2P3p1t zBiL0V>3O2d`#HVW?o2NpdP>Slifyi?L2?BS7%UgKDgx%biLVb|-<)MF)hKd(LMyHw zN!NX@5ULrknp{peA;eYg%mh3GU2LWHP`T5Hzz)LmcYuTVYniFAc9s|^qlDYnx%wDs zO7fR_CBp^}bj@cQb~b?RG=!n@7+4J!UC zbNM^Y)_||%yaIg1aRc&(ia;B}W`g8M5C9>5Uu;&XFSbAZ5Khyac}(SmK+dt+6K29c zOR3SmGo#h8+U>Y`)(Uta-C9IiUyg#LapHF|TK=nEJAv^R0M$#~1qgWL+bb9sNk$yN z9cpX{31JH8XDlm@S6XWJuN=Xy$#=QH21&S zlpnbsOTBpt5Rn%TKt#Evbcvc2A~d09iLaDu`;|p-!@Z+q19py3kV8y9DzWC(sQf8Y zvEJ*r*$=L+lC67k&=z~z2mJOlyKSdd!?mzY$4_B|-ak8Ww>|S7Fl_i5rZT*#FJ+^|mqwZOzn}HF_`yo$gfh988uP67!psf9>apZ?HWF zY($)&^TvTlG`?}{?m_#>tgCpE*#{S6knGbkTr+3?N@|q=ChDYQU-Pvp^+4+fb{KBhVLVdg z$Yry^MkMXfv6|SI&zQ^ZQ)*M}5dc8S7eE=p*%HA2ge%qmjp6G5mofPN`=B~G2$GVM z_rK{WItM?yK6i``N_5B#>wKGTb?ZtGps5^=w0?FVsIUNuO(Ag#pDG0bngAUJ^MNJJ zGG5B0W3!%3;(4Q>cl@2&B-T61fbL=OQWQ$Rj4 zcHiQyu`-(j35jwm#YP}!bSTMCp8OAWrbhQ4e>mgX9-l&AQE}TzNWSI7W^P1)4YU=g zGeC0K*$6}(Bl=_!N9vB7QxytJurK!0m6qZMu%(%N<>yk5q(K(gJb(c3<-~lRT@pMA zHa3DWsWlba=k}>tSy>TMv_cj$>P(MMww#%4N*@3Uc}IjK^n5gEB%g8z^oh~7SD}sl z)c^z&!QMpLK5^HnvzMHL@iC86IDp1sH~^pW9ReOskN1g$gW4#ggi252;aCZz-|lP; z2&P}unKW1)HF|7w#_MED-0o1usA8l0b8K`kF(6uUAP8s6DwAiA9{ZJjx*ecC0n8as%uZ1NNVK(gLg zlFiXnV-w~CK#`cGsdZu=xjfw+wexeEsV+!E=tSECX-)a2nK?GH{s59bC&r_3?^Xa1 z(UUCNT6)xccg~a}?0o72DYB>w$y;1yC17^Oq`}Jlg;&i|K}RL=QlJ)@2M-2T zD?Jw5+ph4U2mo)lp$DcWvLGoctTTcnfoLsJ}pWcP<9LO=Nzt4{q+-JpD zF>>g&1VF@84ZJ{QgxZV?s~9U_2`CT=o-GG32<=X{5+URm(bujupRUN;3QK3x*1;(D zxDPIqw~Dw+vC7q8Q*i-!nu0}q2!&ceDZIb%@sZ!b%7DPm#=-9|m;LtXLY8x2ibNxd zJrOL^lv^;Eqp@aiM^Sc>w$_RFV(eJ{@rfHNX0FCwowm$=f2pU))nWmFH1s7lkEV1x zNHV3Q2wS^7d<$_D$Llob#qlEM#X zvt!K(5LtL*!|24sgg|%V>6^_Lp{HwOU9%%^{gl~805hXsrDn5wAxtp#Ba5zf&Hc&; zO-3uQm)PgGIfe2zO@Cn6dx_Ie0%Z6={vjG^qh)#mpaKAB|Gg!l$E?sj*t*4kdk`>( zZ^f~rmh*2ks4ofckrLph12Z=E`_{dEU#Jp^-IEat_UQf6fH)a>d3iGLpBpVa~Q&N2oV@2CSxchTI#-v`zkILLH#&A(Pzi+nX|sq>FrrpKss-y_(902I-7 zrnSq%p1`cU68o?xiUwRt=iDoZHP`53JN{K5sMnb>Gm5*c)X6lseFrsshIxjCL<3;1 zaiCt6AOJA=z#OtbP#ByO3F~~Lk^E$3|7iUiVr~+1_(yy^e1ei-2@ktkDsb2(Kj?l% z{gZje#PO4;{{n%ChUQ>E%8z}3TMkjXB7qxs=%&FZEeNk;vJ+4aAz5a(CN ztFMDyf`PS42-aTbCM}!M-n*T^o-G@8!3g5EiwmLX&QN6uUS5QE0Agz{N|>0s9{f?oWxsB%Q~( z`0fCyHeN@-&Gn^cz02f7kLsk**%v?`m(>AJM$wDjhrbuQUfdn!sG?BoD-+%!c;i>& z_PsX$i=Hr$>h}Q&WA}UdfVxk)__|tJUsRY5R|b~rWDZ`mSb20@`9C@N?#O5HQKUDX zHQr;sxyiSaK1MVC5n82yJ0~RpE*v0b;)@LuDU0pbLAn~halj*x>HXuP?7;u9( z3(aUy4Ii09-)f~_#Qi}4ei$#c^P}-Dz=Pk)*T~!QA6)?Jj(|%q$TMOr+Mfn%w8V1x zKnu~Y`1IuV3223_$|)fWJpu${Lc_3*0*bvxp(ta~d*%mC*%UJ!!5#5Z*MNYmwRd{2 zr>;kGWhHy#n4>qVIu>+PsSuV}u<0J~(-w|{4o`k~y1v2{K>JRVnclstaF&l0?Nh-W zSDzs9x&xNY`aVikMlYT-9@0gTA>n>M=wz#u+vA+he4I7-ueac#brhS81lG5p6Fu8w zF8!NjN7iK*@U=fbtEK>^XbD7f1huj$Z7y|&;(y2x8VHG_tLW>Kq7yU^2ejhk=I%MQ z&-0G$i)U50v*XzLxO@j-Gf^5l18xOm{1m6ty=Cibn1peWct4(JN>S~a`=$A4kUYtI<`vF# z6+rXiY)`u?b-LZwP=+8zedkdvl9>-9D9a#c+5xjIAm|rb_G@)05Nyq=4Vi?xX*zH| z+{TdNwoEmZYY7i?FhpAdY*kN&SvT!39Kuy*i^IC5`e0f?&>esS@(fEaiICGgb`l15 zpG5v&#oHlLq}fQiDFLjPVc$VUqwIiL_&)So48e8_)fAxrf9UpOoIo)Te)cuNNfpk> zYoc3exZ@-EO*knfxD#uH_xKoOrAFW4Jf_9FOvhhe#~BZ;;|-m|gmTb?%9!F}$a-}1 zydKYr-<<*J+fHrZ9{X22{%T;)HB=E8(_z?WGcwkNm z(=zm3nR}4`gOAPBgR`QgOgGIhh=|Wj#y6K%56{J-Fb`s=%W6#TyZzxAJ)a48*3^n+ z86^NZkfoT{jX#rP`zcdf!jGDQvFYJ`-61{Z`ps6o!pXK>9+U>}3_521PE!8YAaTMx z-Esc!oeDyeU^4U*qW)Z)Hi5At5l1n2Ef0Kj*C)+pv~ZpCUBEVg4omz^as3W&zTG3p z9RzLLU9Mfc=C+jYo1X(hOzvh0M%2xIn{u|wpI>I!Jff&RvWMiH3wZuoTj73SeM9|K zyy*3sd2(ZgT4$vnkgsS~X#Us_3*jGFI_U+*Su%v7D+KRt$kbg_`!MVMSPzLuw_InVkqhga z^%nN|`xTi!V^kd%j2}E628alGiZD#Y;@_ztmLV5ih?}S<#u{P*3>hiq?1#hFTztwu&n%3*2eoN4N1!c^ki_ql5H1`fQW zJdrkbn$&a(F1TqX(Lu~-{AlvdiGY#p*b|Rw9;(=(PYcJ3DdXQj^IshGaKF9Ae^`%q ze=gy?IGDK(CEC&k`;)5>|NfM~qe8VQ>mVs>TL` z`o9=|jC)hemcWcx$rb%L9S7JA{tBvb6{D^-uBFKo*~gpre$JS%9f$jf`WCIWJf_1fYbV;UCN^@jck{`W zzT=ijkTX*OQ_K6SG~fQ-vR9_^j7h4RswAL-qmnV0*x(MoE32eARh&(&&rr$>Q5$Yo z+|6KI-5vbqc7lW$x7W9r%-&jr)*n?@82EuuQ+?y-7&dGDW4Rgd)%gyg(uFilfN6&! z{0&~UeR0%d0doyoHEE5xM}NuqtmMTM{M|VUxXG%Z{@LSW6`h3v#w98rb~4>T+UyxS z&E%$)#=UV=dP&-}xlbsksl-)B8QOR^o?a?;{yKh`cA^bRm?39-Rp%1xbjWR=EAdy6 ztp@VvQBTA+fy^Ca4Q}*1p-Y1PM+8n@ZBfh)*sq) z+!j==5A%9%dGf%^>-N|4Chf4)Z%FB<^k!!)@CJ7Hstbfx@^5i}fkUP?JbP{{Q7K~T zA$u5?_obN4sYU(|*oOf}UjF(1IT5`bV_IV>HM^odFNrqQ*wk>$`Kr>7PfEXhG?#RO zakz52RfRw0=`R#zEg9HQhr5-aQrNYvdo6@HGRI4 zYteB&ePS~lp!6+NDKj#&V#8xrY5v1U$&Zz%LPRF9glIcY0>nzt7 zl6*;;94?dhfAJmr&Hkaxcltf#{q1VxP0ER$16RA+N8NMIxScB>oe3H828Yj+1rv>T z0GGmWArlpMB;!mMGmp4Z-3g4`Zl_dM?G@C7xR$8-WjDo;)Qy?F8MPkeN>YUXp#g1Lwt5=`K!CpNkbOW;b*>zvfjW#r?SV)&vu+ zM%&))Tt8w~zZu=@%NujVcwRDu`Id@I+~L#Bi_gi6R9B$Q&G^u1z0P_B`KiCD%?Te0 zvvrgs%I#J6hrks~(w|D%)e8|hU3~Z_i7aF#x!WW=fg#zFvmr&0Gf*+3oU4z^!>wo^ z=QovNjp@??_j9tfJ+Xw2z~Y=a3x?6smN|_l(oK&R(he|Qz1#6LG$_ag;XuklyZw1^#VM zJgRKR23>is75rHRf7^Jn1p^|!BJP}Lu?^1t@&M2FlI6|5iyQv+nH#ZEmDz0Sn8c9O zwBB{^d|Z+{tZ_w^u)u?IIr52DYAw{on!pjKx#8@N{d?HK8GjOW!+P#u?&YZApusnP zxRYOw42ejclt_m4>yUaelXU{O?Dx+qoJ_i7baJ`uZ)&x_1SPHu)>HY*a!+p8=8bN~kb93v>ypi3Z>KQ}#y?@!=ij8}3 z3#1@M3Ps1YKT^&(Lxg^I+WUlU2o=>GEAp$n8`VXa9+LWVpDpn{ zEm!39ptuU=gI}{E=LCli_oawztHI^aZEgdGnd=e6!!4CZU3mC*OW*M45*z>x>mpl`pQD$LzRICqjCE={&u~XMvmlIGS;3 zh;EYSjb~xFT4VFxmeS2fY4a)iQCL#13Z0l_mP0`4%{Pud>j!gRsSfbp#XmUpZYs_+ zQ9jWt)C}+_DRg{xp(oZ})c1N_)H%jlwq1Tx*sS?8!ze7>M+9-M?aMHV-`tw;U|^lI zvpn7C@7ugz4a}58`e_^vx;|1%CBAz47qRrt0WF)bx`V5sQU#^J`W=t61xE%C6*$ZhjkA^OQ^OqFDJfIoR!9#hv5vee3nH)6q z`i7FTekyS$@rW=vbu{g1F-ctqW2T1Kp_OTe+u&v9@bRjgUX8({pJ#VM>}_M1MiEHT zC&f4KIj6ei%=UFQ^uIesu8Oe(30{m4Rnc*nyXLH@njS7H+rsJh?PA$B;cJ|w`1|q} zY%|`_f|3R+IfA~Bl6o&dKm7zZ@^)_cJMt@#IeiNI6s%z4&%NInvBsH(+ z6P;>U5Z|zvx8Z*G1~xUi?t8uWi>3E?q)wfISG@``&W#YvfF{XejA**_3Uv~|Nr402 z+kblQBpW@Exy=V%{3fDpDtf3LR5TV@@Q{46@Noy)G6M9YSdw4l1Tl?!DjsxiZ5=co z;6G3pbAHPDVnJbf7!!tXUw&M;8M-Kct;j?_%_^nnqggAMh-QEG>Ef*;I7N5ea#+YY ziL|Oab)now<2}@*W3!*bvUd)7RpB60u&m@p}HfD2Kdb)Kv)|B6l4 zm0WpJQmUEfqX<4($33Uqm6gfgs39_Hhx+5h;w73)io%vIhQq$1kfS0svFHVODzgs1 zqX^>r)YjPGMqg0S*y3&UQ(ulTpF!~Y$7+Vr0mpjHjTfB63Pq)P-4O!4#g_l61@KNm z;}h_^wwn$JUq=u0uYB8_0a%{1D6#pI#9QjeA($#cMIN4vote9;>{6`g`9lWPdkxpQ z;-&l(Ib*9n{*BpYP0%CEXbgr$H_!5w5m7_m8?bqI-6Jp*&EseHBE)i2&Y-r0*J+Hb zC-~69nySx++sFV}GI|VW3Bt)BP``^eE!Sa+83=zykUh3CVl$AC$korFQOL<6CUZ}K zP;5}DZ076Z_0GW>7tdwjAUS73(Eq4JL@p|VD;H@H{d6RN{o*q1y`dJ`vmmkk`7dfW zrYF5$sOgv9zTnu)tR&um>e2r8Wtm^yy$woIa;mrHlo@{D3^Gb`De=UcPV8~NDX~Y< zE3UrwgETUKSJW9hk1AQVrw2o#8SEQKjzVb$eTq>JXpBOy$t`}1%ET~o4R9o8gKyxm)-ZOtg^G=fT59-}+JqR)3HTP>PQ>DtRg;*W5AfK?tmnsaXSxx40Hj&FAt zr{aC>S`@TwDdwG#-hA-V*g=ogF|{YX36Z*^@0(EQk+1SyMM?BKotQ5PxK`r*=XSsE zk4-z<(8Jh@69<)|3EBvxD>A)~d5kYnsO`HF6n7)(^JKBK;n(MP;f-&tspDI@Q}srQc*$XR(+dbHx=&ENb{>o_GtU!yWfVY=eC@-3)j=Lt32@jAi{=v?VfN{+#WJ7 zB+HnhzrJ7UT^dC?Crw?VGHu36czaZGja1b9sQ;D;{(Y0YBfs+KvSiqbGi5$F_kcLp zpTKx=i!w%ysJZM6Uv%Ouc1>ran+Tf|zd`G{~#=0urfK;Tnkr3XY8A#4^1fr8->s{ThoFUXQKlHaJ4Va)eAvy0xsQ zDqq)d8XYkIKn{I)hR#v2PJdP?$9nOhq**Hy36Gz@oAcvnPSj0X@tdU4;uA%Yn$*#- zSk=a}3trESps(>|-+mX&{nal>Hu;avHX6y$B%|0T)+7kAu%wBad$=tUly$^31I5S& za-g8;rxC%H`D$XhmUUR#Vrp$Y(-^mTNsbH@(m3)$Gm89J`uHt(8O`07(b**s@#j~zofY$t`~%O{6GZSdRCM3?=|F~zjn^_ z;ae^w?XcfcIUe>V zJV_>v!v;tpPx3jw1L!Ja(T(~S-Av0c+T`;!7joVQs8W`5;PIzDvZ*S{T!(BiXSV;f-XqN`h*riM_`d7Ku#L9KQAY(tu{Vm;Rs3=3 zWuYoJdz&ZJc|*7$UeAW1qBYSvRTg8DqP0gW-Ny_Znl}sC(~w+*tc_?gpZ`=cI4y4Uz;v zq6z9D=bND#HQKPZsnRWWTo_@aj*AF}0`z<4Z)rN8T}!x(ITsm#li7SOKLG%3V9QlG zqV7TFd}BQ?i4-$-i|X4n_c9gcmFlU!?sWc#15w7D%112jt9m#U+s5XSQNWIM5lln= z3$Q&4%$<_%V#*Lp74UHQbHK>v);E*Bw`i}3_&jx!3Y2XUvTTy89PZe2fw1h-tYg}A zS*qxe)U|7bg3V9Jft=f+i`LE1uADSj5d5h6(uu~iZj1VN^Fe@nreJjH_83GxfPA<4 zMDg)NhhE&{Z7IK8y5|!Zl8?{mTT^Q_evDwvWd~i`zh$|7lAN7eCR@jC&jEeDIT1lE zr{(=)vGdO>oV)v{qj3@~*3QB{z)=`9F1OX03J>U-=hdz&Pm7L5N3sQPw46kksTyGm zsfuUJZUS=sMSZrQ>znQU9izM-km*c?JRA3Y90U)p2w$%Kuj4eX^b6&k#1rA&CORx& zh65ZLRHHr1w{>QJ-xW08vXZ;kyU1*gpQEE6KnoU1KrDWrZ(+WhA@wu{C}ycFkV+WE zvY%5OcQo%rla+L0nG)g*HOS`S!KTaKni!cDy4BmIlW-~_+3{%JqO0~yapMmwOC>+I z0yZ<2O%>=}6-dx;p#k=+JtlKrtTX@Ool2vJPt;+MChRY=dD|zXv4}v$=?wGS-Sj_SW z^1sgyhV=<(Ixvo_WF}Z!iq5Q=um$V?KKRw-&>9tk$gOwU+p}4SU^Mgjt>3LDT+q+4 z%k^5^U|u?}sw60S_1pd*{Htoqx|;NpoOr-4Ecy!%S0iC-PkL>tw<=zsl!5hXLD<#9OiEIdcbd52kl=mbRmCk@S;q_ zB4K>BC5z2p{lc5Ia!^;oIG&Gw9$OhKQDbK?q30~e6XfEHEfE#U8#gBamTj-p*j(bO z*0RU*B5>-iX5}m2ud2~vdhd_YE+Z_pamK1B$he}fnXCR9^vE7)gQ+^NzJ&fcExGT; zA502S!!W8G=PB8d9Wzv9aAj{kGw7~=7JU3<>{7|j7or=R@^WD57h9tU{W^Rj5bqxq zAbQ%Y4B^Ar6AA?GxIC7^B~C7i>;0l+-ZcJxS}?%jnqUU%&y6sJrM`b!fJ&U|D#{LN zjli`^h2YUg3FtmHnfFlAdK-*TAo{68d=YW_?hcY;WpH?3~==R7Qtm-18jXK_Tck}8e8K73+wu*48oje%(|08O6x)28wZP2lLAW3 zgn5X#EZx!w_J71#X21OYax34@Je7OuWD}8d}+|i<~N;E3vWmdYsp8o zJiUH;rKF0Hu8F3|dh!E-T6h{{;npX$Y}!*kpAtH%@nPx^iTHMCVm-yv1waEK48`>%;%|Nk zG1kIP7vt75cY4k}aW&*aYh$+z^XY>AaYI?a4Zn}dMOQ14`bE;;SYK-TG2PkCLoE^4 zk`w z(%XipHKD7j!7QO15~mkFH#jRJW&NOK_TO-iI>(%r**feA4|-aT(mD54i0r@;PQIGo zA#Uz@;jN3&I02zc?ZgR}d85UwRBzs|$o%u#Zo_CSQ`-w%>H}sHc4m%Zq?_1R;?X6KYmt%5Z`!1d1@z1*+)#MK1O=|JgN^NrI zPyNQZbo}08W{I=7@7yx68v6`;Gz8U?;I5-gJ!uWc(#6!I+KrHp!6?pnT~BA0o|DD$3l zD^*9W-_GcplGTY6Rwj#SvS1f=trOXnZNLUYE$pDdwpHd`-1p)bS0vB-F{&tmBs7Te zgL%MY;j@@Hf4!=dmy~;hJRC^Tr3Y-ny|`Ez-udq{?&|m34Y9|UsOly3935^5G-jpa z(U|iy1tt@X7J|*g5+Mt6`UR;`iAT)2z8x}-`f($oQEns0t+lU!wAWDq#=oG}$-g<5 zJzEoC1QcQPEIRE1D>FGdkKZ>TQSQbj2P2t-Ni%y;w%;Pvs9KrB*a*j|{VY)40R8*D zC@@QCSKW37R~NZU9$DWH(4V&V0Id7(Se;moTIb6WAOUN@Nv`bC z3gST+B`uGVR}jPIdeF1=e-ei%58DDQoU6g7dVPsbecD z#T(#$Z)KTjhtd+`aKC@_&Dy5S|CBXbzgJHfnCAVWnweOme!H#exME(Q6fU247Yd*# zL76MSvx_%%jg#`8Y+~#7*TH^Q_AlauBTR(4Tl#&X`=fb;omsn~-L#$SMM@jqY^CYV zh?lz+f2r0)2>MO$&B&3?h%>H9P|Zj)nn~);^B5K5CS-V*!9`Ge%*10Jnq}j4D1xJP**RNQ?p0!z&~Q=a-hy22t~c$D3Fx@o4>f9Bf5j^^y___o^++g z^a)49*F@&>1ot!u;yiHEo{*rQKq)eUy-@(9Rd|j%u!_>U8rVgEv<;cK~?}gJ|@6 zr=niTM^&EpD@J(sL;)jsfviE%k3&(f@p{VeCx!8x*pWoFeG4!{+5f!ODWup$EgdUtL0WqrWW&6Y~?@4DV{zI*1q~P1#isaDk zZ=r>$p`U-_QdL}Bh2E+#E*1k%VXRw>=*N$4q&Aa|7*bzRhL_6QX<+UmU{CUp4*xxRAj^hIgLSNxdW|T)SP(Pa zhGn*@o$@pwubw2y{?@)cP$=Dn*5j}Bnk(WD*5G81j$yVqr-8o(!#|KI`8Yb*ZvE=r z-$>Y=eHx<49Ad#f0y#3cafpM=Mqx`|#(u5-ZbXgxzVMF_kkQtw^fwV=JGIAJ)6!)UG0kDoSsYA;f4#))w!P-OuW@=#8zesW<|B-)UNih z9KwR`)zXu?(6)1u^{IFXx|#PEM*P(LRtc0s7H%3leaAtL){&bVG6Kb+4|B#OdB}K! z$hVzEzHi>oCL^=bq(6Mu6uTHCUF1K{bMbvUjjqAcB&* z?C-~6W!C*)WQH|{^5BR3?YZrrr6XQ9JD?lYc_Lmbg~JDIWHycF;nCg@JIV63o|cy| zmkL~_;r@7sP^TUKJbP>Cv$J8pos?6Lspa>>xdKo2=sVw%1(;qu=+2kAM(*zs*{llq zP5Tbo=DV2Y8@Y#=l%!hYF-PK@N8x4j;3fB4@wdbis6ip;<~mIQ$50Y z7mruk7y8CZ>Ad*YhvXT#OWgAFJ2M@4d_PE}n(ivez;9&oVT5e*x6S@4z~3ZYlv_~e z`@A6gjl9IBnz-mMEI){h)(L>RmFu?k_@7tP?Nl?O)Kb+Pb?ytPI!qfDgl^eHKo!7N z1|iom&cg=669Js5Ym)j_>v`|CC9QlL4YmM0uP40l)+~-6{e(F{0m>O6jE~^WYhnXE z934;MxLr7iMyTrAa`RTc8Mn>$$MV{H6$Ea1Bsp_;*>3ZhGn6c-#zw5{^+WTOG=3gY zvK=FxYMsQVh~AaDPXeh6QQNX?n*8Yg!*POpwQC~s1E0M!RLG->l6!!%uJ1)5{(5}1 zfg3p0KLzvUN$n_q?)Ke72$&JU7H$g!nI=M#>K^Ff1pkR{lQSR&scI%CFl;~-*qQf7^H$G1zw@QEs+U?(lbec%5W-In#t*y zO{uW)>^V8rtbUt%nAsr+Vsp4+7#_HZWW&TK$-~p|%R+QI@ZDJrxw7otu7!UNiQ3HT zDP8sfm1{KKl1q^djIPhW&D6T6&@ZS{{y>N#`_DILYx zX2Cz8C2G*s?_Qf@usLnr8(4ZZ-}$E_?9}S=A?j^QXRQ++V#t&}<5cFWdrHz`2iGov z|1iE8FD9KM{8RtZf#~2>m1E#HmGT!j_sVWftkEzd#27c>-E;p_^`4~lydCt@l&rTJ zsuuuFRaw}(_7{3=7e2}oGUJ)$vMikI@~9UWZ?mjMn-Ch%9be#cyW&YBXibChR|f7* zY-b-oZEn|ibG$lcPHP|W;gho)LYzH1O?mSo5*rzA{rAXVK~Z=1k>k+=;?Q%?_aEhw z$b09+n+f?W>GD&fcXR|DM48W4NolJ30%d4}CB;S$?4Q{86>K==dd-TuAdm)klC{m} z$OJC$@mVpKJfl=1W3MR6Vw8EAbFID_z8vZBWI1XyR7kkw3<`+qto`^GOq1RpsCpuQ z^*tn`ht0!@hnkX~XLX`Y#&wHJ@ZIpP$$&^b9;=x7!pr8Jh`|TrE?D9x`S{XwYcCwDA|zpwPfyUV2z>LduAa zJy_&&=8BH-MXC_Z=T9~RAVw_e>LL>vMpI?*oJ$9{HKQ5op4_d(&G_^MNjccQACPaQ z_?zf7J-OTe_l*(Wt>p&+|0A;)Vrcb!XMAvXfW8lEpvhsf>XL6K%ND>A;>cQgzf2&U zt4h1M0F|0yaEFI-vc-fsW;kdodDrz7-?C8ysITkP)fyM5o=e!Pslek7x%v_U?3#t` z2y10tLDgzGV0|H_t%XDH~PTP=!|Bge^E0EW_)vMVhtIf z&Z8P)#4LA{(zKI+>lOhx_rGkM5sv0`^e8ipc%nu9;5kpLK@f;rYxgO7G&CR0U4o2M zW0wyLs7H_6lPAqT9y|9A*ek-W$O7^H=2k(mu<&2C>R+p0E0r@;Oy@& z4jI~&5aFLzwf*zI67)-u4@!08+h&G;-*rW9&XCPFJVGS!6o0EMNRIY1KmlS`Up@B% zF(9lM&yNvHyN*v!j{xeXD9FelgWlo%&JhWzf6d}?eg~9)PQB>Y7q8{GatzV0z)JuR zpy5J-(t9HkC)JyuXas^&!;4fa5H;)HEqq+`{vX`;Tebh@zSBh{=9m5_vv>~W%{BX$ zXh@OgHf8v&ZlMEc55WY&V%|Trb}q-Ah|}Yz`^!y$hHa=C0U^(;hf!+=e=u33{YF~{ zeb!_ua+@men-3!f-i^NnjP-*yZcpiZNI#A)psL3NR`NFLttLHh2Fr2%Pa4RvKkr1L z-*6f|b;_^HS+P89#{hpw4JaQK)KfbPc8sZlcnjD)?1#S-flj-VJUVf45|=h z|IKl48GcSU?31X`gCJDD56pNDNT!w6x3Z8*1!dVFY+Eha^{7@>vT#KaGd<|126nr@ zf^V#Q%rZ0La%OX>enVME-P-IJzlfCQSv;m4@^4hlqmw)A@n)X4Z53F<`RZI9n{U6M@zh1pziwi2ayLR?f-`>(K$5vlAr*qbfk}(Z<_wc?S?Xj znn|JjUCcC6zy5Ihuc8e2Mp>9fImmyQhS-1)G|!_s5W~x)Pzb3|P0*Vuu2PyT90klg{q_%Rhk3%RAl>IDe+=^X{~c#%M36?gpJYDT`q_H+~Y z0d5&!>Khp0-zAP(-TkC{V`aAWVGFU=0lv>LAu3xtqNG)&mYe|E(8t1DAWkX6=Vt57)%AIx`^YVM-dtFNj!$IIvP%$&CsU_4DkZ|n|X z2#_8N56itq8qF3=5yBEnh;8aL53=}bksN*9;ItE;!pbtA2)>qm)$qu*P}OThf)q&v z7Ikdqp#*rdTdo*hvgT&$8wfGFWPx}f&Wzmjx1nHtEJR8Lbp>-mg)%T5u^tlw%=dA7 zGpCE{j1qb1h6f1Mw)eK-wKz10)1ANXc$KC;y-mdEN>L@Dp9oMn{kf~XO=BD)!Y{v@ zUG~b6QlIa)iJKn=x+0y5X1JZLn0onVAXeaCaM`tQhi+2V;kwKFYqkc|(VsIj@;Xwy z?C3wVyAqA2^M(10jR^UZ`Rs1bh4-V`h{w*ZfjqBdU=UN%LEMrUX=O>G z3F0_w_$ckz<^{>@5<#KlAic?Do4=B?N=LdeqYkmgEmpNHqCzn^MghIpF~K3IC_}X-sjm^Lgc=(a z#YSOM_nD#E6+>YYGuxQK;F+m(t&$0{g;H+vE)K1~mr&P=lrVv{7PvXSPecWDoRh0aH^X+{zNk$1#Z8x^ zc`RSIoQ|MecK)g#I~D08&=Y%R?9~mTjy{vp?rw< z4L1Rw!dvr5QTB|?XFWe^>E1_F^{#$)WPWKFsz0|+7av+9SqUaDD9b)(XL``Pe~zS& z>9Ui$YJWt^8OK%yJX0RwFltM*?evY))mxN;Ab$*oVhb8H+buk9?x}#)ID1 zTRo$%c109)C`>XJQEg-mP0-uvfJ^5^aT-qF)C@FQqD)R$Eb9xIg_03~OGQ;Njt!a} z6g#EO)yiS7HV=p^>Q>g=1{|WTgr4uiyr$5Hy%H+NEJ__6vfI?3K`S9)tSBfF)NSLapQq@2PJPoEzquI^=Q>=uQ2zAk-n zvB3r3#QPJ1n6&DzuV}C7h&a*o19mbQ0}>_(XCFmFR2X>en-I8c*r-|=tJYam-cbo#~N92nj-$s11nCQ)qH z`MkxX0(JZlDcXWxalVb!px8A;q}9IhTVGG~BFEZn{#jqM-yG~j?=2b)O!GT{uzb-A z^W&%RAufaYzWHFJ>|{?fK!lzQ*g4&psY^9K&g1ob_?VFWJK}4{$IxIgSCT`}FYzXb z?X=RdX1GHBg5`#hX`RmOk0voN%C28+vkEte+oDwZo~qBd2tQO!w}Fbs-zC5BDOB*r zIN7VYb8%?hNhU}3%W_`*QcXFJi?}ylBBWmW5UO|GFt}&qQ4|-#XvR-L#(BAyvsd)J z9vvC5?<`C(khWyNdtk&Dcqj96$NBJy6O9Vf@)08W%*OZ1=db75>kl;RxpK#uRGl!F zxyZ@>jdqd{!=(!W`{TMgZ?!-Auil97aH{+=T}@m4p|kMJEq^gF8LpJSuJ5SNJ{sR$ z9(S58UqY(bWOK}Ouc7=(i8tEg_zQjf`k+xs%mVwhl;g7FcC5(N@Nz?+4Qhyz7;_}_ zbj=S+Y%qI>;gno{^+|)*2-(Ev6GGFIC&dLWW6&o86Mr?)SJA}yE$F+0fmZwyMvZbl ze3veG`WGLxLlYl=NZS)p;Z+(|`40AHLIHdcSXTO4#bP8but%$F3kcy%qE(Dz&i#H3 zE~^8DQl-g4pk5eaqvg$*;lmapKeEs4jsVf)7LFwg-{C7$QfH7u+Yo*v=jrZQ;u+x?yGU$`0=j?h2?$TzQXJ4+x=IUbI7`&=T zUiisOZS*zf-H|-BwY3%(Dy2JTq+Uos&)~+-*`B-YgiJH<8Ar3lLyU@jdnRY`q_j2h zl!$U;Yp{&ZOq~414ij=5Z|l#b&t6Csm2Pk&9`uLkQrp2T`gyO_u>5gD^P*aqw)h=t z$5m4TkegW7jdxaK4|sRaBW3k2$@?Rkvl?wSq`R3z%vfz5400JaHV>78*9pG}3dK4fSy~j@pV2^>?eX|K7CTqtWXEx zN*l4IsZW&4vzsNfoz?3HFM)D)p5-%JwmpZCs?5y$<<3`{kwKS@GdVBTdPK(7P338` zl7K^Cv|Gzi=O*!AVbRSThII)Lf8T^}G_(2gVPpKF$!l_H&v~th-<>Pb=#eAWY~f&W zLV&WNfSyyhV-&i9Lt`}KQ)E?Uab#f!@pY^O|LbN&=i+YCr>7q`w@0>FaYflpjkyh&T)Mjtqze)qJH+i5famz4|@Sr_v_5O zvU>(O^htfE*fcph!)-fImcIQ!&|kh$T2-rcn;hirRm5|o&E(NevibP_TXI2uS?3T3 z6kX%MlQB?~(ojkFm=Y997tv!()GG`ynhqD&CQGy>oh|b)Ql#z*Kcyv{o#-N$1Ho~| z0uegDXML-AFrz)~=o_qj=}d|F4r-kgJq%n}l+kAzts*=l-cj2eDWAm^nD@v*T2okS zH~x*gnmLVa;jk3tEghu};3X-}? z9*{XSoZ@8}Q!+mK3y_n0wCl;oFJ5HH#X0R3dJl^6F=tr9**syOP0Qv__lZr zqC3xa)(7qEuT@mYX(6G?c{r7mI|sz0I?bH>9L9kg0fP{A;IN0zhv=7*Nhzvz^e*l^ zzqp2(mYDY3n8cZ_6}n(WfuYz$$xe!%`!{#CGfGrgh?EdUI96K`xOymcwk&@gg z5x?d|G1*$brBn<#zH4YpIxM!ld#bvpxX&HBIihuDBs;aAvRrziAblE ziu;GH_PKB?ry+ zeOpR2wl|08XI)HOT|IH0Qp}N~W(XCd4x}vPdsGnUi*>$s;a$4|9k>j?q%yb8nMQfD zg@08X%k4Bg%UZN%DxvK=>-n1x-iq~pIz87i8^12{vrR!7~8kuqz-bV4G^3L21W$FusI;{f(^tu6jyE7^quhT4RTg>HfN zanzYK-Ac{lyH`ykc%!*R-*MA35*n@6)Eh=yPg=D?kHqedH!~N0Tygj4Jxp*5uJktf zAgxwjMMQ9MpIxfO;S}B0Ya9!VjciYDI|VK9)_2rmg*GfLeQuR-Z{q+osgsH-YoX47 z9`B~{v-cUnD<{1RtOEv@RGbH=JK>xBw6fUYjIAfGXvxb;Q=hmUo=u7a`B%-Yu-V|SZTFPuhsFf)04f_Ga z#=t9)pRQ%0*?bIXmK=XJ)h``Zr@~;Iq1FX`i(Tp2V>?zYy>nR*ZbzMIu%racMgW)v zE7d;E-#(fJ7RF!lO%#RJ;Q}Bs%fbk2f--0uB&iSp=D7S1u@8UGo`id~EbIdq>WCqT z3BDKLGQFv4gli=*_W;UK0Q>Ec@$< z17~wud^QFFcoA@H7*$9R9L@s&CJhIW^ytzjlSvJDfty>a`y>1{fcaCqXR!U>DNC;4noJkcU4eLIn*0kcSpL(rFN*3aTWHfiB5HL;C{(sV|~u zKl#_%w%2)zlNaJ3aI1a@3zX1L5=i=f!@+ZCNvYLnr38EnU2pb$Mj%3U7d5tjofV@= z(SUbH!fw#DkL{0h@al2_NO;D~=!djf%Q?E9G{zI@`-b1asZ*+>6=#VF|K$3*4;en8 z%(o%X%pv#x;B0rjUYv+w2bd>Te=gMO-{vJq$Xy@Z4g{qZBeqKQ`L|Nph%%#J#Jp#N>-j7B2onhTtV1>xx0z6J%Z&@AQ@E)Ka&o40>T89Aw>TeV`g{U z>YET*PKJsIO7-njDAOn?04SMhEfeqrLHr+SKZrH$d#dp-2r*SO)$W+pmK@{Ol+}(L z)i#6*o!kKG6*JFgT40@mkj?|MH2_bj1FK>Il&R;gF7Oj^bwW$_qXD?pbjv#$4SdoP z?YAT$rkw+cLZwup_DU9*c&TuC@|X0Ar5`{7pnpu-KR^XYr>!FLM%3Ty+PK5F&5iw>X1>jK zyI+(npMhj}M4kYLvuo>xZ^Abjye0;j%^%yabsgCst+hiS`6%9k;S-}c+rxl3;ma&% zbdE`xrDB6U)0SL+w0YJm5)j!zv^L35&i2+krzupMX*#tXiyUbRK-YD;>$HWm$UVM7 zhfWRTd9oqx{yH9F<2)-si<&wy-SM74@^#0crA9Ts6(Ct{c=zdq@LPb`kiipyw?eBl z7U4=As?Wl`rd4rUIz?;;7Z<;SzBl+SWUaoC?mv43$ z8@_BSOiqrjPr(na_r}tDCyHT?fs)WB7FX-fB+_|W;omHLKlxDp>vF^&t>f|LU1$x~ zO;#(-CnQGCJ5xX!b-l^=R#<5Wh&VU-!7tBW2^TuF9HT7*ZF=9USlD5N#g_+OJND+< z^l4YyPVvb;oPm~~20dT~Ypw4|vQ7R2KUgxoP9`t1kw#l0YsL0@KB}R+7@R z17^CFkY;U|u(lZEXN%xrNJ1vZD{T5A8e6NFrXVdegCDpaJW7b_+5u#@hkc3m z_<6i)&0JsY*?EchG=QAHTGaOsfUOqfGL9YbWv!=Oydnz5XLvN(kEA@=@azr-S@RC>V)AlrtUbdZQta6C7RO^z0jtDo>K zlDx$}kOGlM0sgk9Eq>t9ClB<=snNX4&$i|>eO;^Y?ArR_fnKmRRvX%Tw?~A>?p&MK z)Zwv)MDOeFo{_A_fxt+`W}G5mfvieH4zvr1X;?w4oS30%%B~`UN2}E)bDOV>vYm9Z zrA-a)P3fe9w^Y)uO#OW`p%zh~1+c^uKYL-hUx0otObuvo9{LIaETJgs=%Wj&W5Sgt zNOrAjAjE#2ud!fxK*lFw2Lx>`fpQ+x&Ki%zA7WY{OMS$6ODlj~TD|8XCjbzjVWk1M zk1l*;kz^+OP`B&_s5Q08%+^_su#V`h-3xq)EK#%2d=vPjpGatg^`ECeXVlF9TBF;A z!R31s;Q}7sj(#ddJQg1^{E>-f{%5Xh_~`%xn@$mufzcNe$P1en7?&G`9O@5T@UJYC z6~5rk@$>Rp07R2nfW@RMk{H*6<*BM;*q0MF|Cyk4xu+gxQn-2VIU>2o>2DUbAICR8 zavTR@QU+hyT=P3$!Ad>|hHY5sVqaDh`)*r7?OyN>oXeOx=#Ls2)DS-`z<&nPAL;Wb zA0K%zIqO%R(|{TU2Bo)(jj;4~pfm-`->!cncEyqn_Y#F7(Gt|8Yxyd43hy(nQ*RVf zEc#9hAS+p@3;v>8YCFV7_{;O7Hso~5*8r$dt{Fj;;URO-Kq-)Ml{nM( zqAtHURoM7Sn>C$a8IcreT_#>#|DC7_T^3JvZNAK`lCW(?d2K?tnwn-0uPNm9BQ%ay znXI{q-sKvl(?UXgH=X&1Z3PpQ)@1FKDooQxdF6$1ccd~aYc2K^6mPpo>1kRmL>g6s z`8!^Ps;pYsi#2y7D0UC%9@R-8_~%rAp47^JTlBHH-R0jDRmFjrrEJ)RztNW>Rqx^> z-SmFMmdFzGdWI!?ZoEbtMI+<$XI}iGq0hEPGxgEcS7UPu1?N=7i_M&cJ}zhn&h0oA zRX)UJMc#q#o0%d_OD~%;eglYjaakg~iIxi^1$6Acj3-Nv3dz4v}*5#bY*v);n%|m>Ua^Dk@6hr0Y z2FOvuV>FYs$VN8*ofN1$;lJs844$4z#Yd)zmnDN18jU$&+V!mfvu|ee$4%s$_S1Vg zB^h)(B%oV!6SvbfmKa1FD$*�~+e4O%dPH;i1OG^~N^Oymx8$;k4mowsBu3k&%pW zaoeG%JE3SaS>uSiJx!m-Zf;02>y?)1fHAKXR9;VyF#ipJ?u$t>jG_Zq_pYx(zui6N z`7fXr_UUG=r{#=z^ly{gTc1?;BWPR$bK&C{)>EUX#;fo`#%?c2rl&I zpj~jtpte17S`tpm0ji9efGVTAHo6@?6nAV7gN#p)K-Owd@XG1L=m)S0--P*wr#SJi z#gtOE1Tc@|_cvMGf2oTqT7WR2C~5rH3Au%Q+yB;KUrPY)=~S>NY9>Ug4K#mn>n+Rs zuh&ir^)`ngQrpQ9xZWfy-_{e8|8m^Lfq%9tT9VhjWh((mN*4sEfAzJJ-xA8o>+!vS z;^v#f-}X)v0*De4#o*~BL>>PvR3V6!{cnZRKS*z}iP#j2-2Zcc0Ms36aFeILmH6!G ztb8{P5TtzT2pD#EYCXWINd8ucW~%`2m-I%MCQ76I#are6Z6veEId=KNeLBJa zCVkyXe)b9^#a*X#WM4OYt~_l{U~g${^{!ZVylyF5QQb%q2(J2ThBo0jjPAEwYg_<# z!Q6=%z(WJB>K!GyeFWPi(pFaY9JU7|XT3vmRC?foTLDXttoHQG2#O?Ois2*tPcA?t@9HOmJt9uk+Ia8<@wV`` zUm(m3{OIz*jVFnm{#*%@b5x9o(AnwXfNPa7q!24uD+a?0^ zVXHy@8b8D^3*xt)X;e$Th(bS_m?=AT-djh4Yhhjla>b$bawF0}FMi^*#ggf5LueFDe1_M&_x{ab1zT!M=V~-nHu3HqA*r(}~17CJuXTKjxh;Yj3N^ zfrJLjy6Q5UN5<^(#+rWb1Gka=%9&Vy#<#Cu|2snJndl%m+x(6y4C+?x|A+QCJ>xYI zFS#To4}HT3HOLNqu@xx`rQN^5V(?f$4n&7K0O5}{SWFxgZ8hAS)+TJM0f;{$bGpj_ zG=*{r(ph8sUt>(!#ytIYFvJbnTO)?#+LkqN_h=6$ca*w@n?tIsQoLs-2plv`(|f{18AGYIE8wI%(iaw z`UO7yOxYC7DDw*_fQ%dLIfWLZgLFztw1iSQ$zDCD4)1HEZE~ts+O_z~zSIS^cLlaP z@N>8&Co=PZuioLk)?5%TMdPpXB~qaL<`zpB7lA`lqSH`9jiWg%*C^-}inLUOJT3O% zo~`FA7Kc^^-}o8!U7}Ll6pr;D6mJ8nYSWy!{k;!j*l8=UTZ1UZoYeFD*Td%-ym?pS zerrK3K)l{+3y-0J{+$9q4SSx`aDn%4n{77`Ci%%3yJv7lbckP`?0%eMnB(7YMfAqb_w;sQ&9INqsMwTUVL<>u*iu|KR#CEt* zKlU2L;jRC3wqR4S(F18@u@P_pxT>9*FF-T_%6&IE-mkGVT7|8J3F-ZY7K6;g4;~*e zFK~W`QnazT@6qX<4cV2aywEOQlZ4{@SQSg_>{cjH?1jTCKD=@Qr<~z~o;j2#1nMiP zNS4DE)=(yTrBgUYUx`=XeZK!$Bs!mKd;T_>=#c@JZF1-%#1Q_%bfm?L0<4=6!aYM2 z_kTIqPVW82Ebzm?aNVI7BZR!~A2~+ELI_?W^LNxYh6gCP91jq}5-R-PV#!fgk@5Pu zyyAred>Adzt#xMfn%;3FfkdTnj^Xylmq`BCwN-aPRcEU&cktLf0W=&(VKg@Zuwvyk zJz<8Y%%S`Eo~Jcbjb^!_V~jVexw(e8NH?pVzc6ud_HB<_TGTx-0N%lC`SmQ)vZLvZ zmnWOAOA7Q-d%^j}IlhyBHd%#5}Kvm}L{7biX$3RpEX-^z_1kOQy0&(_}h`)4)lO=2I`#HpoT}nZy zUUKv_4P>`RD0&I5oQbhCWOl1QjtHS1lWx-3f;i_+39_Oc1QgUir4yre=8t8Vrfs_} z5+=M7@sI5!P^)}>lfS6O^G`X9N3`M6K5&XM&)Ed`=l1=~rCBq}ez<=)XQxwgc3MHG ze_yLVGp5Vm{5}bdQF!Vl(>_e>bVdZ7@Cy^r!Wi z-Zj&2zqzq5>iJp`{cG)ReqX=I;Q!eO4s}+@w~+YS6!9lN=Nt61{i*v=LXHR8VJaVl8lF7~~jParnRRPKZghdWo`vi{~HC5qjUB(Di+{93r+H=+@1`5B(I9zi2y;6Nk z?N>ozw{+O?x^emhB=9*;QZFaSu$% z1sFrDa89q;^}AmEagS+x=)k|(t_WU_m@@DX&deY0>lP zSBRxt9^SFxIrgRRTnS{ctW`pS+p9b^ZkB-3fz*MDsHu~oBEGb!pExIB>VB>ZT`gX8 zf3DMqQ#>fH+nIv0ic83#gF_QkCeYW>J*#jqs7L-H9l;GRZYRlGQ|mCdRBrA0_R)=_ zIWQOkUL3NZ6(Z%>*>UJa?5@Ycac>u=6-gYtAA72Xe{y$^qDnDD*J!~TPC{ckX3TCR zwOh1!Wg0ruMWjcBIx!O*S+)BE$5?jeoOnB+L=rG4Yn4AJvTIi4Rv@#qSktY>Y*zs# z)7Sa?O)_@Lq@tKE1!r?$G%zRbd(Rs%L}!AfvP!O;2ej7y(1A8LoO&JMPV|=}ORaso zkEgWlXFTzw=bkZ`ylu1HB2VbzN^ry133PtjisO6dYcG;JYt7IyDC&iY5s}peg@^|; z@UST%;+8+8>@eX>mSr1amCC#?iR`-W~+I?mkhXF+{} z5;H=UU!VDFi^&6yKTu19i8W>kOg<`K_Pdd2PFLV)kK}r(wQRpv)yDOqtk>rBGM+dc zV8D?&;@YR1G^tjd@{u%V_TsEx)&jbU1Ok)$ zIfku%+KM;}vKjT2sq{vFO4^A%eDdR#r#N-#9}vctVVdaX@9>^vD=bA&W)&N;yFARG zZ&CQhe&14&-z}xCF_%&1-5eOMYpP%~ya`sxbbQ8LdZbh=)s8)UA{`MUNvVi7@*}w! z)r@{C0Y&)0tA4>UBx1f5RLv~7(>-dtd+=97w-XK(k^tqRTJFvq-8&FIW}s>wokY22 z1Q|-lS1o01Kw{5ZJ$M=%EHARwR*j_OIq$G1ln~YFWfod#QnQA7?PXS2tk7M!_zQ&iVA&s48bO%d0s~RgRJt@`g zaTsTnevKTS%by$EaK+MG1A|{)8zT$tYx<(JdLEu$pqQn|dpnO#o3UAUXT$4l-E2|b z71;Sn`-~-61e9s#Kp9HK*%-;a1e8|PO{WDX!<%h}Y&YJFzhR>sQfm@B^yJEnSBcM3 zyLD_S--(p^XgzV)!H)@9Ur+B#`<{}z%IVF$&O3biiABH5G`Y!|3Xk>;aPttb;7&T5 zEn!brP&HIdDxQpsa!NQ5x5x8xdqy|@5LAJ(cNP(7bwQArMg9>^tm2_Q+1_L6Rl*Y( zT~6fh#KFD(dV%3E&r8ot_4~{1GLwpHl}sBg9SS|sco2P)qAQm4?rDkd2MXH~kAD4D zvYX*ke{xlxVr=t=;g$q{Q`Z;&B3C)`&kta?%Ts8swrJIradlMBpPttW*p+)xlh-~0 zI&?%YruzYjj4SH$Hsm6S?ft1u^%k|6TAw^60`;}R`{^Ia59G?CxAVXf~T!P5Wfml9|aMftjSh*h9h?{e!d zX`9y;CCW%5(D?OT>>-n+A?w1 zw4fg_thi+UloSSGhKi{Ilop_sm`7Zy!2uyCVbXy=xzoEDm*Zl0lvK1ECq`(Y9s>mn zO4_vS-b%J974emhOUWWuvAo3FV)FhYFPPk!y7xgoYl?Z@Q0zroL#Ba-= z>;Qydji+Wl?zB+!Yz;OH3CFY#B7TMw#yTUBJ`li3Xnws_F%UiEV-Se>U?}Wx+M%rF zkv=NHnmc=U2M#PU!5TC2s2z^a!4N&U1I?)f&k(>#BoLAMWRPKEnUI*0nlLmV2gh0X63Usk`%#Rx`D09;plc-ISDA*SikRLsFB;RK1s>c>DC{Wd2lMv%`3F$}Y;gnu zd<~Pah5U2O4@Nfq?JU&)d+Pt~f&V{xAQtWC*1!*DV8;>JSV7V-QB4~Kj zKl3$46GieQ6xsvZQvVK7U^pfKt%&_}y1NUnt*xCeo$zjdvY@-pbZmOecCAy)&aN83 z;(8;*8{>w4{KrT~Or`@S6pEYAc(zb7%hz{M(9kHe8d_SpuBWT-1$^4!!1;)jf>8f{ z3Z?xF6`?be_6&h7Y|zos4XQtUPGxhu*h0(;!#_j&ObUtl&qw)#U`;W`X-G69OKCAi z(KzzIhkzxGZ)D&eha~S^PsRpeu?=`1mPN`bowig=C z63A)@!@ofKO!i;TGH5(c_D!aR{<|tg$_VwDE;$TD> z^B(Fmy_UZrGc(M+?mvz4-;(n)WNz_{x$#v3EKC+Q6E*cecb0?$49pMzA7;7d2nt~Q z=uhe&{y9Dl2lSWqoxhq{JkZ(VL?U(nT8Ov^h#teZoE_nTSl|zbD}3vJ&%a9s?j!Kw z^Ju~baGyf1BnOdye;LrNHOYboUd#2s@zlhw7VUoDUo`WqD-j#s@}=fXpan>6_M>XS3RnRp7YBeVmPL|lO-J= zO|-Wh7PH}$^K~`k_JFL}-GKs#iGr-|hs07&e|NTU)tb<-7>|BaN!&ksxYvbg_l8{J zD?kSvM_1cSvcJbNP8lv}J3A$Y^}Bnm7|q@u^m-3^65>lwA6YpiBo!d69C)&=x;v{K z?WCsllpd03LCUQmEdu}kh-L&)dJu`;LbUGdAu4Z3Y4H{ZO8{4^>lfS`#s^~SzVu+R z12+4SEGq$h=5jC?T=AFxsCx0++LZSEjf5pkyF29VdPpqCH|yM+4~A!FVUT+)*NjS< zT4E`O<6$o}2a{5vB6w*y_@>X`#Qx0{)XKuIi)+nnzI^< zjknDWhBV3B{@VRC_p9=|Wx8YX0W@}a;eq=}+COGoP@XhQ!^`RTW23BPnBh!ouEmmm z9@X}}I`#B%4VR{Z*X4$P*v8DggHQW03hGr~K-Oip!9s8G<&1FmZ6&d0R0mSmH<_h1 zh4Kg7^Gqfl0#t_65Kr6=08ThJ+dWUc#j$XHB>2dxoaKp36NcRAK2iQE*L|sX*(qAV z^nzPNL_j^$Gepffu|H=SY^pEN6|yZNlNY1xIDNv;G|DY|4GcKtYrfmh{|ssL3Jqjb zuW$e;`aR5tvm8Pc;7&JWCM6}!s_gP;EMz~IUMX#?YQy|t^@hLA6Yj8gPILjzTOns3 zz5A9}_qKOuAS-Wq_xbO%n?n0;_-HVLa#}Ili>W?Sm`f&r?}&JC{~hVlSRNxr9X(da zyCezT?H0gFV-A177TnS48P6XRz#vY$6>%|UQo)Gyv< z4pDxOVVElx2va|;w78*Uaozt*KD~EAO-R?Ix;7QwKIT~&Vx1YsRc{DUNur+@Xr1cB zVdp|p^heQdoHu=yAE%Qu>i61diFDoE(FL_q(Ma)5i;oq1`;Kp9s_v`sx^x9Hq0L=+ z-${#p@pEVgOMd40(TQfWr%edv!b=r#_v9dc!m>D7o$kbYYmLb@$evPH_3N-c&Itdt zx+NOL!!(2qm9DeRLy2m02a%p4*^bQ99z#3Q(WCT|J84}b$!vl2RAiwOLw1z0_J4sR zbz18=)G3s%+gs1v+yvMmb};~DdVhmC=T+4Bxp(A8TFfE~kufU#ZlkoyVMsbzi8Bf! zs)A2kT|AZgxmyMm1HWiZu~^mkhks{cJ|(&k_#}A-)phh(FytKf3xG+<@jhj)D8@EE z6%hPxxpt!h70_^?5u@Djl1cvH-y%T zo!Me-H?Icl4euQ3)>%t$W1%y`Jmk1SBueEn2& zy?mm{)nUX#$A%YBi{eqXE2^S*t!I)f<3oKNi#dY*3;zeoq+tD5Emn5TsU zH2rGBeUVsRl&Wo$xrlJ8`Ykq3fol5E1#ftmK_#EP@_tJI#@N={m%B`~!)X91%_+s9 zwEy5n^LQKpEsPr-RV=IU4s&^`x51QiLN&=cpCsD;b%OXo{s@Nl0H#+xs`#D(?lj!d zr$k$ay*4*&RLVj_;L2RSMN5L*guFJYk|+m8=b#=B9Rvys>bS_3ea=!n&O*irBj?bH zzEBhh{}@NF|J(K5;oJ|W#ym@PLOpJU0{x|^PevNrN~`2AR}Jpuag>7#CrNb&co@<$ za9r6|&#M;Juj=yEreDwkgJJSsTHkgM#x*b9?|rtjtMr+zZ%J-9;i{ z2P6>KB`i6)Rf|gt8B^viDj8lKPh(U_7_`YJujLK+bY+=2y8n75QevdJ8b=8PvkUKX zdSagKE159ZTYSU<3zpl8N#sv6dmqjlkQ)LYKY47NJKHv+3FH#HUvI&wMHL%-ciM+d z%(tU@pfUn*q#0MrxIz-NDnbR_MHC;6Aikc%zgcT5Dv{YwGY6(pQ-GP$3;jG)k9WSa2H&sVnsb0KDKV%vbR5~uaB7&EJa?$o}=;81B3^d$_3*OMquo#=W%|)6cb!4Whz;e-J$1-~w&BL_Yv5=rIU$US|Q5DInc^XI7Juj2PG!A&nD41em?(;uPc#eF8fo~U2 z;P|1UDU|(64?fUFZy~ZI_k^Tw_i(_{yiEF*Cu4d|f-v9?;LoO~6E6SG_-G;00FC93 zHK=-goxzOCHho;T=aF5-A0iy}>Qw2Jm1g6lXVIQduW6?~cd;utyJvYR^8r8W1a<7|b$ z-d{O;j6v%71jmyLV5oNGvs79HTS;+pO@qnozZ>{4X~7zep_qQ_A)@=BtHDU9?bP?V z-zR13U38qM9}H2C=`a7JY#*}}<_$70EnCC6=)fgbO;Y*Rro z2lgzd`&7K@{vnL3>Gl1t600NN{rBRq1^ydQOTv(U7n6!6*5Vcnt%v%WCdw?ORFzq` zSP~4|vG+r!%=##-)O4FJi9N53R3wovxG=cK8>ysH{~3o(iNMwHkkTn3zBHx9gFciG zs}tJu%c&QQ>aJ(JEt}v7y|CT39W{2zqL8j8#lEq7Z^4Q&w&QVOW~@h#@={^;dbDLs zn+5}w8R(k!y-@Q$fx#BJrPR7O=hb7}=$C!cD03a2YIOL-7ecby;?vwjw&wO@Djovk z&YtIYc>rP`eCV1#VDjbkiLQZOaqMjK{`s=Bw+`^CW|oYc43BZPc+&f+ATVPS_DYsl z%*+#{*iZZ0Cnp#(R+uC2hN*Qo$=_Xf&dwFSvnAk_pjxHpASj%?pfv}=14>&5rhg|Q zi7+sb;<1mS7LL!KUSOT~UGC)mn%<5I=Z5WDAH_ppK{k2Bv%ZVB&L_9KxcpV%ZTA|7 zi+{UNichemMHGrZxIKxv{)22~aEwHUZCdQ$lySPq@j7Gt;k18-4Vb>`$3r2-w<`{+ zxVYG7;-O-Fr5OsPC(&G8yS7xMRP3uZ%iJ?(IaL|)^j6Z-PxwNRKBX@Tsc!jLZ6X%OM ziP%mWq*g*`S4NZ`Y3KfK(W~ln0-M1}#;c{8{hm`5cU9O(m7nkQe3oUW8)$au7kLg5 zOJk<~l0Zg38&%~`>MOu@XWwvu?d!7326*Z@m5-q(DC{H-8jApYxg-+JbAI*{<2P6)3Y8J=cmxn(+0QxwIhcUa4E(i(Erzt>Ywa^_XB zDU#7n6LOXdcZ=t&?596t*>w<|nZ+`?N@6=rU*u4A){@z}8mPdMZcdEHzVXdii?w#! zy%l9PXTI`Ve-V0~;rcCjA{AfXiSTJ5>on4+bX0kMafd$d#4TRV7^80z`*LfPcBYwp z4m0#`LaGpzN0WSaB-Y`~%8$vjxC%k{c*}Y9(7Qcxoy!3nKK``c64yWgTnHbJ5yaUa zR#llhqu75Gp?{$D+#cRydT#N!b-KfZWgvRGTNJK&udn14_0NPSEx`PKvohYhT<#?N zX!QWATO{zY5Kku!1{;WPh++OQpODJb+1xcW&aU#O>0#;;jU~L%kFQs60rV%M%70Oa zY-^}gtZ*fxq+c9aWS=Xnz3LeYu(CB|_lb92{*A;p$g}ZkXk3v8dE>@Bp9?Gyk<=}) zOqIoT(l&P~V>Ey$Ka{Hfe>Ha9@l=2RA0aX?lD$dUTzkhQyK56d8R>(NY_i8SldSAb zk(Ev2nwc3%cJ|K7{+;Xg`F($n-|wHFKRi6H_c-Ug-{*Cn>#&U$K-k={QXq(KdbUli zWR#$o|EP`&UC94{i$=ac4fNBdi~6rRA?k04oNwbgjpOin zi%oD>nk**_)^dA?>H)OBE4y)6Yx*DUTJ3dgkLL%rHewBzug%Lj9W6YL|Ih%KmY8|4 zPol%^OZrLCFWgdK2u~anJr;tu0lb}-s5WXE5Y4fdk!p07!$*e5mWCwP;cPwnQKFso z`$WW}I=zuqtR`nm&k>ZJ;_=@&&3g)Vz59jJPeVMX%-qX4%lsujS-hyKHTiK7VskXm z_G~)QR(ehBy`Xhx_=otBXV)7v8w!@hKjz-r;HnO2l>Biy$_nZ{Uhun=l4Ibj1`0{~ zi8l6Z4f_Yn8(T_Y$+2qwRA7Ko2c*`^mgD>W{ieoqniIbnE4by5#6#wT=oJ7TbS>c>0cIY9?^_BYaAoSZqWpZ_Ks30( z!rCGFW?N6z#4FQ}-9~JxG!h(>A{q{1mZej=z<3+y3Q1HhfhfnyxHxPijWX0l)EB!c zIVKrihZVon9q0*jr19*n%KuolLadzFuR2>wXU0I!VWy2`!pp#CwCu(HQX%kKxm}eI zcwWWWg(D_ClG%NL%rNXF+DFchHt2BEti})mA@Z|}5&Bi`BjpE6eS9e$9MiO5$^cRg z!bz>qrVUp3cLf0}4~&24r)LS5)sK1~_B-l&Yx6w>Ug|EDA%&}#JCj%IjSO|}o&W&e znBtKB={BrP_^KJd?(eyo}CFPOg_d(F!ma_p-dGNgjTW@ zo$j^ISG2P0z7TZmuE!%FY6TIDJHBuAlRpBih16p~2ok&RV}0#|f0%lM$L%(-Nepzb z2l>8y5ir*CxGX~`nL969+Q_8F!Y${7@W#j^cP9)8%nfpBG#xW=XCBHFPCgds-%Jmt z?Htd3>ZcVrN6X=p&te1oy70<$q1o*tinsb>VhiHeGLB3bVcarxu2?duov;Qco1@*2 z-K(!q_WSFDg=bm!M~P#JR~nYxvYod3;O*JT!LUeF7nd8Y7xoW}KDyZzExDhr#i<7O zUWi;6lT2N@7qj@_Ll9(XUsK8mdqin4W>P!njt!Kn=)Gu$*_$0qqVR+hl*glVhm0+F z=*MG985C0pKh?TA=R|mGbJJ`O5FoR6zq?qWAC*06*n>*eSDJxv*^>r#z8yhW(N7?a4gGHz8&t1sHa2 z8KF|hXA7%oI^U)8oT`Komol|@5HI`AbpMta^829`BSO}F1|s)pjA@)LD>UqG5WKOE zBol7YC%Rptq^GoCmKxji02ZhWSRK#f?jJLSC&-{M*uVt6Zjgx9?trW6&C_|?rxIoZZ6Cee;3ID8Cd@>7m)snU|dV53%P-&$8b zwRd9Fr6{l6%&UCZG3AoJlX7`eLaX^^M%f;96fh=7!dPu_uhz0xLwHo}*@~qOY108I zrqi@+4`|&Z~fBJU#Od6?%<~U|#UPct26lI(mfLj(jchc$)PL;j%db~v@gJ|%zu8}#S4vZoq!t~Ne*G;w*trKZ}@Db zp_OmcRq0~oxBNBdzi-p?~e5axj9w zc9}5qvpjIoC*NmMLY+hhtd0L1rou|O$iwe-%O~_?rc6=OYniRnVdEg)yUk=Lp4_|- z?JHkdSWr!-p*R>&P-eH65!`iZg&#Z#xg#VV@u&-y+in~PZ#_y$ybo>=j@?!E7r%L7 zg;OFr$71dQar!~6_On34vyBp1m&GG-m02pUbG8Y#TLLrys^w%Ye?fPJh8a8%gT}=9 z&#%osmW|vf%a{qgEKvsWA<`S-!Up+UM=SV2!OS~pjM@+Ll@Bz9c4m8gH;e|aB|D2T z*~}F`x08ARqufk0o*(^|y3^!A$8P5v<(zR82#~cH3dc;oe0oaj?Re@vd?5TK)tXHR zw7C1L1vK$$%x81R!@v&bK`xS%gCpIzFUsRhy93(}wvtQG_wOqghA(hHurFdTmWg{U zjj`{#XKP0(=ikMfMUisdTl`UYXQ%;*<>^=b6^Tt5b8E_i-3~KZ&1=W6x*@=77f*C) zLw7U|ubq=}v2IbJH}m!6eF|r1#Ex({!`SAWKhSLz`6w?-jQ0H&31a-CS47U4xwQ+z z$$devE%IOi8cUKy2L>(PMO;+31*>m;e~h{L!kWtUENAth4uY^2F3rymCF>7Bu zhtPmU#GT#hiNKw4(Sx?n=gJ@i(4`}PbzAc_loY8)gVyI#HT{-Wr91Vby2|?TEYbU)eDI4WCiQo1f#UDLOn?Sv#m>z~`^{&(wk7 zQ?<|c#na9SZ*!r9?z}bcP=FRW?r=RW%g9Y#wCoPS0C}(uJ?IPaO<5Mu(tG8K=?_Uk^&Qnk-e2tsmNhhOd)Dm&-G5Yv$ zVB#j(2EAiqM}6Ksy*T}sUZH4;%$q-v-JqyIjmBLT(8phbx9=C6$=?uFRNVh9RiL{% zvY0>p-lIy4-2W;u8J2N2|9aP!%%ptfe`bH1(V5E9ctngU0f8Ax^x`zbKCb48DMi_| zbU<2J`p@>&O`YBCK!oj&86h(-RG=2wovjMr%!vWNQj zR_a9$c}^I%+L|mJ;v~KZ#gs9&4jr(@eVe9Uq#^n|ktXD1H~HjAzaHI~5djk$Ck83qbr`PUof_ID4589mv~-j18*NL)hB1Lzhf zZUX`Ktl$mW04?I%FoCEL{8YxT6v+> z_MpfWUh~goik)w_{AUp5QPr9HwX1QTO#g1}P;s4;Il}`Mtzyz)U&|4MZl5^0V$v@* zhM0Go00^v)DG(M}%t=Q&nzdD_gaGC^lIJ$t zHH)~W=Goe;&DH4ZO9Gs)Ebkn7&R-)YeYeCbYG-l^RC{s%o7dqwyfchxLNqJ1I`=UJ ztQ`juaO#xZ29Z~MkhDhrN&e~wD47X}#OsfV1H(DYZnIzxxxn?ka+WsX!uM!Nd|Ij{ zR7ut`!pRRMIs4}|vF7#;Y@J;L%ucz3+hgwNj28>U-Ys|Mn>~8+yV|7m?4V~(Jl=pz zC$y;z0ZqRAccX_h`62HaF0kk3#)|s7^fRTjjq(ElPxxb*&-b-Yhgocghq#O;X!eyT zNXv6Em`MgTBG*C9MBV}-N1Od110Zz-keJj0kr4aoVfWn~!5eQ;4snZ|?Q^C>k9|eE z-WNMZUI9|kk=mqQaH$5l7vTRx7tvonA6SBfD%?Qs#=< zOalh!`gZu&x(NBTt&8I+B9tMX;Cjeux%+?We-VvLj6V#%+>6s?RC{A9eXo7XV`>Ok z-qkPFGmb%=E@EzR$gUldJMfNH(z;r?G`sX}RD+j;Tdt}-&a(c(^Ud$YoYi4*qSb!g zTB^@gjgLS;POK_h)|mK6sd(gRTx?ZI*a!&tWj<}8cS@(5FgRAa1_vvtiEJ~jhnjZ? zdhY>9{3f`UPWhCkaMeR!bO7AfH^08@wmW=;dsl@1MyH)A*yO=Z39F@aUBxc)*hlXe z$Ubyg-VhmCJFM|snhu`2Yrom#8Ag^5Ha-O6t!xTCWC!}0ZX^*)n4=n;=D5uNYAmO1ic1?~{V zHycr{e{|z&$8w^%+I&Q`KKFJSB-E0XbIwS*qJOzx}-4&a`;!0tMDY;QhOK!;jvTR7lG|DaaZ= z##Wb?*Pryvy-G2)Q8v6p=9CzUmP*v0UyvU0ZhT3{nfX%Pru&O)6vKLna%}=+aKU~7 zUrv*v1iOypQw5P)lD!V zfK^UxUHsaGua?DAt0`T&tq~p8q(LKx;Rye$O6JFiNmH?9Y z0XgJ|J--TtKt}y9AScn-o~`-4)PKCOY!Cz!@QCnR0q-`Cli&%MJ}!FE6b7R%a8vIL zmiD{GuW-=s$5iu}hy}GtT9A6AiuGpuQ^SRGjFZ}Xht4g}d3+)A>$lzA5dD^t%v z%EWM#sRK-&vrX`aVPFv2iz;F;l@+4zCBT6U0c3V!T8>e@I3!mn_|Hqa(m=M#--`Kp z&{A6L^>BC_C`qs>M=8KJ9X?8yOR4Y15FxYgc&2%w14wwNcnh~~tA?v+%YIY7L^8Xp z=jE^&-9uSbuvzp&d`P0bka4l*)H`C{;&|~;AFJ<)Zh^})X?n_~KbhUFmkIR{h7G4u zxhjb&!q$pM$uCUQY<{DH#TvK|4Q=|$w&_+sxdAiNxIPO^61)g(y6})gEF3 zeElMiiAuZqd7Q?Edaf6(%GgL+S#hzm(7zM7os#39K0C_>i zguEvIyBO9^d-C%*nFL7C20u1>#-+tsSRE+CQ5-w z*`p+4L1*ghzy(z_-mO0kk$VzHMMGEkk6OHpki@yC>ABFi!4#)kMCe!_KN%)FWmJ_2 z&{1>9lK3;}qqimlW@j_0Hf@g%9&n#Oi-d3fnn&YKHgk0>kma=iP2I;B7oowFm6eU8 z6(!aw&mq)U6z zAQ^a7uT~&AEMpeXPj<&a${eymL`Qf0Z3XO{mX;Ry=G?!x&cp#7GTyvsDwf_e5BM(zFhCvv literal 0 HcmV?d00001 diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 76e63c7d5da..5c87558a2ed 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -82,11 +82,6 @@ class BtNavigator : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::NavigateToPose; diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index bf8ead1cf38..433ac40fdc6 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.1 + 0.4.2 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index f0c063c79a6..fcb761322c6 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -51,6 +51,8 @@ BtNavigator::BtNavigator() "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", + "nav2_truncate_path_action_bt_node", + "nav2_change_goal_node_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", @@ -206,6 +208,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) action_server_.reset(); plugin_lib_names_.clear(); + current_bt_xml_filename_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_.rootNode()); bt_.reset(); @@ -214,13 +217,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -BtNavigator::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { @@ -247,6 +243,19 @@ BtNavigator::navigateToPose() return action_server_->is_cancel_requested(); }; + auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree; + + // Empty id in request is default for backward compatibility + bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; + + if (!loadBehaviorTree(bt_xml_filename)) { + RCLCPP_ERROR( + get_logger(), "BT file not found: %s. Navigation canceled", + bt_xml_filename.c_str(), current_bt_xml_filename_.c_str()); + action_server_->terminate_current(); + return; + } + RosTopicLogger topic_logger(client_node_, tree_); std::shared_ptr feedback_msg = std::make_shared(); @@ -276,19 +285,6 @@ BtNavigator::navigateToPose() action_server_->publish_feedback(feedback_msg); }; - auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree; - - // Empty id in request is default for backward compatibility - bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; - - if (!loadBehaviorTree(bt_xml_filename)) { - RCLCPP_ERROR( - get_logger(), "BT file not found: %s. Navigation canceled", - bt_xml_filename.c_str(), current_bt_xml_filename_.c_str()); - action_server_->terminate_current(); - return; - } - // Execute the BT that was previously created in the configure step nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); // Make sure that the Bt is not in a running state from a previous execution @@ -310,9 +306,6 @@ BtNavigator::navigateToPose() RCLCPP_INFO(get_logger(), "Navigation canceled"); action_server_->terminate_all(); break; - - default: - throw std::logic_error("Invalid status return from BT"); } } diff --git a/nav2_common/package.xml b/nav2_common/package.xml index f51ddff6659..6820a84f4fa 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.1 + 0.4.2 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index b4cb1b50d03..5ca5a22605c 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav2_controller) find_package(ament_cmake REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_common REQUIRED) +find_package(angles REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) @@ -29,12 +30,12 @@ set(library_name ${executable_name}_core) add_library(${library_name} src/nav2_controller.cpp - src/progress_checker.cpp ) target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") set(dependencies + angles rclcpp rclcpp_action std_msgs @@ -46,6 +47,22 @@ set(dependencies pluginlib ) +add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp) +ament_target_dependencies(simple_progress_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(simple_progress_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp) +ament_target_dependencies(simple_goal_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_library(stopped_goal_checker SHARED plugins/stopped_goal_checker.cpp) +target_link_libraries(stopped_goal_checker simple_goal_checker) +ament_target_dependencies(stopped_goal_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + ament_target_dependencies(${library_name} ${dependencies} ) @@ -53,13 +70,23 @@ ament_target_dependencies(${library_name} # prevent pluginlib from using boost target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +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(plugins/test) +endif() + ament_target_dependencies(${executable_name} ${dependencies} ) target_link_libraries(${executable_name} ${library_name}) -install(TARGETS ${library_name} +install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -79,6 +106,10 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name}) +ament_export_libraries(simple_progress_checker + simple_goal_checker + stopped_goal_checker + ${library_name}) +pluginlib_export_plugin_description_file(nav2_core plugins.xml) ament_package() diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index b0b28b462a1..f59b65b9999 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -22,6 +22,8 @@ #include #include "nav2_core/controller.hpp" +#include "nav2_core/progress_checker.hpp" +#include "nav2_core/goal_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/transform_listener.h" #include "nav2_msgs/action/follow_path.hpp" @@ -100,12 +102,6 @@ class ControllerServer : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in Error state - * @param state LifeCycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::FollowPath; using ActionServer = nav2_util::SimpleActionServer; @@ -201,6 +197,22 @@ class ControllerServer : public nav2_util::LifecycleNode std::unique_ptr odom_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_publisher_; + // Progress Checker Plugin + pluginlib::ClassLoader progress_checker_loader_; + nav2_core::ProgressChecker::Ptr progress_checker_; + std::string default_progress_checker_id_; + std::string default_progress_checker_type_; + std::string progress_checker_id_; + std::string progress_checker_type_; + + // Goal Checker Plugin + pluginlib::ClassLoader goal_checker_loader_; + nav2_core::GoalChecker::Ptr goal_checker_; + std::string default_goal_checker_id_; + std::string default_goal_checker_type_; + std::string goal_checker_id_; + std::string goal_checker_type_; + // Controller Plugins pluginlib::ClassLoader lp_loader_; ControllerMap controllers_; @@ -210,15 +222,12 @@ class ControllerServer : public nav2_util::LifecycleNode std::vector controller_types_; std::string controller_ids_concat_, current_controller_; - std::unique_ptr progress_checker_; - double controller_frequency_; double min_x_velocity_threshold_; double min_y_velocity_threshold_; double min_theta_velocity_threshold_; // Whether we've published the single controller warning yet - bool single_controller_warning_given_{false}; geometry_msgs::msg::Pose end_pose_; }; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp similarity index 92% rename from nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 697a0dc10e4..b7e483c1611 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ -#define DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ #include #include @@ -42,7 +42,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_core/goal_checker.hpp" -namespace dwb_plugins +namespace nav2_controller { /** @@ -72,6 +72,6 @@ class SimpleGoalChecker : public nav2_core::GoalChecker double xy_goal_tolerance_sq_; }; -} // namespace dwb_plugins +} // namespace nav2_controller -#endif // DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/include/nav2_controller/progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp similarity index 63% rename from nav2_controller/include/nav2_controller/progress_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index 3d754ad7aa2..92a5374a5e0 100644 --- a/nav2_controller/include/nav2_controller/progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -12,40 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ -#define NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/progress_checker.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" namespace nav2_controller { /** - * @class nav2_controller::ProgressChecker - * @brief This class is used to check the position of the robot to make sure - * that it is actually progressing towards a goal. - */ -class ProgressChecker +* @class SimpleProgressChecker +* @brief This plugin is used to check the position of the robot to make sure +* that it is actually progressing towards a goal. +*/ + +class SimpleProgressChecker : public nav2_core::ProgressChecker { public: - /** - * @brief Constructor of ProgressChecker - * @param node Node pointer - */ - explicit ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); - /** - * @brief Checks if the robot has moved compare to previous - * pose - * @param current_pose Current pose of the robot - * @throw nav2_core::PlannerException when failed to make progress - */ - void check(geometry_msgs::msg::PoseStamped & current_pose); - /** - * @brief Reset class state upon calling - */ - void reset() {baseline_pose_set_ = false;} + void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) override; + bool check(geometry_msgs::msg::PoseStamped & current_pose) override; + void reset() override; protected: /** @@ -72,4 +64,4 @@ class ProgressChecker }; } // namespace nav2_controller -#endif // NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp similarity index 88% rename from nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index 23f727d9456..869edb6330b 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -32,17 +32,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ -#define DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ #include #include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "dwb_plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" -namespace dwb_plugins +namespace nav2_controller { /** @@ -65,6 +65,6 @@ class StoppedGoalChecker : public SimpleGoalChecker double rot_stopped_velocity_, trans_stopped_velocity_; }; -} // namespace dwb_plugins +} // namespace nav2_controller -#endif // DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index c678de23c75..d0c6bbf7763 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,13 +2,14 @@ nav2_controller - 0.4.1 + 0.4.2 Controller action interface Carl Delsey Apache-2.0 ament_cmake nav2_common + angles rclcpp rclcpp_action std_msgs @@ -24,5 +25,6 @@ ament_cmake + diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml new file mode 100644 index 00000000000..ad27127c03d --- /dev/null +++ b/nav2_controller/plugins.xml @@ -0,0 +1,17 @@ + + + + Checks if distance between current and previous pose is above a threshold + + + + + Checks if current pose is within goal window for x,y and yaw + + + + + Checks linear and angular velocity after stopping + + + diff --git a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp similarity index 94% rename from nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp rename to nav2_controller/plugins/simple_goal_checker.cpp index b108b551989..f1ec7dba6ca 100644 --- a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -34,7 +34,7 @@ #include #include -#include "dwb_plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "angles/angles.h" #include "nav2_util/node_utils.hpp" @@ -43,7 +43,7 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop -namespace dwb_plugins +namespace nav2_controller { SimpleGoalChecker::SimpleGoalChecker() @@ -103,6 +103,6 @@ bool SimpleGoalChecker::isGoalReached( return fabs(dyaw) < yaw_goal_tolerance_; } -} // namespace dwb_plugins +} // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(dwb_plugins::SimpleGoalChecker, nav2_core::GoalChecker) +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/src/progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp similarity index 65% rename from nav2_controller/src/progress_checker.cpp rename to nav2_controller/plugins/simple_progress_checker.cpp index 68fe96de50c..498d13256cb 100644 --- a/nav2_controller/src/progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -12,33 +12,37 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_controller/progress_checker.hpp" +#include "nav2_controller/plugins/simple_progress_checker.hpp" #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_util/node_utils.hpp" +#include "pluginlib/class_list_macros.hpp" namespace nav2_controller { static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); -ProgressChecker::ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) -: nh_(node) +void SimpleProgressChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) { + nh_ = node; nav2_util::declare_parameter_if_not_declared( - nh_, "required_movement_radius", rclcpp::ParameterValue(0.5)); + nh_, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( - nh_, "movement_time_allowance", rclcpp::ParameterValue(10.0)); + nh_, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - nh_->get_parameter_or("required_movement_radius", radius_, 0.5); + nh_->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); double time_allowance_param = 0.0; - nh_->get_parameter_or("movement_time_allowance", time_allowance_param, 10.0); + nh_->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); } -void ProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) +bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) { // relies on short circuit evaluation to not call is_robot_moved_enough if // baseline_pose is not set. @@ -47,21 +51,27 @@ void ProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) if ((!baseline_pose_set_) || (is_robot_moved_enough(current_pose2d))) { reset_baseline_pose(current_pose2d); - return; + return true; } if ((nh_->now() - baseline_time_) > time_allowance_) { - throw nav2_core::PlannerException("Failed to make progress"); + return false; } + return true; +} + +void SimpleProgressChecker::reset() +{ + baseline_pose_set_ = false; } -void ProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) +void SimpleProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) { baseline_pose_ = pose; baseline_time_ = nh_->now(); baseline_pose_set_ = true; } -bool ProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) +bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) { if (pose_distance(pose, baseline_pose_) > radius_) { return true; @@ -81,3 +91,5 @@ static double pose_distance( } } // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleProgressChecker, nav2_core::ProgressChecker) diff --git a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp similarity index 93% rename from nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp rename to nav2_controller/plugins/stopped_goal_checker.cpp index c94948e8967..2e24ad181aa 100644 --- a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -35,14 +35,14 @@ #include #include #include -#include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_util/node_utils.hpp" using std::hypot; using std::fabs; -namespace dwb_plugins +namespace nav2_controller { StoppedGoalChecker::StoppedGoalChecker() @@ -80,6 +80,6 @@ bool StoppedGoalChecker::isGoalReached( hypot(velocity.linear.x, velocity.linear.y) <= trans_stopped_velocity_; } -} // namespace dwb_plugins +} // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(dwb_plugins::StoppedGoalChecker, nav2_core::GoalChecker) +PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt new file mode 100644 index 00000000000..d4f34d3918f --- /dev/null +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -0,0 +1,4 @@ +ament_add_gtest(pctest progress_checker.cpp) +target_link_libraries(pctest simple_progress_checker) +ament_add_gtest(gctest goal_checker.cpp) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker) diff --git a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp similarity index 94% rename from nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp rename to nav2_controller/plugins/test/goal_checker.cpp index 3e5b5e0357b..fe175db4450 100644 --- a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -36,13 +36,13 @@ #include #include "gtest/gtest.h" -#include "dwb_plugins/simple_goal_checker.hpp" -#include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/lifecycle_node.hpp" -using dwb_plugins::SimpleGoalChecker; -using dwb_plugins::StoppedGoalChecker; +using nav2_controller::SimpleGoalChecker; +using nav2_controller::StoppedGoalChecker; void checkMacro( nav2_core::GoalChecker & gc, @@ -151,8 +151,8 @@ TEST(VelocityIterator, two_checks) SimpleGoalChecker gc; StoppedGoalChecker sgc; - gc.initialize(x, "dwb"); - sgc.initialize(x, "dwb"); + gc.initialize(x, "nav2_controller"); + sgc.initialize(x, "nav2_controller"); sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false); sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false); diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp new file mode 100644 index 00000000000..418ff1032ae --- /dev/null +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -0,0 +1,135 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "gtest/gtest.h" +#include "nav2_controller/plugins/simple_progress_checker.hpp" +#include "nav_2d_utils/conversions.hpp" +#include "nav2_util/lifecycle_node.hpp" + +using nav2_controller::SimpleProgressChecker; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const std::string & name) + : nav2_util::LifecycleNode(name) + { + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } +}; + +void checkMacro( + nav2_core::ProgressChecker & pc, + double x0, double y0, + double x1, double y1, + int delay, + bool expected_result) +{ + pc.reset(); + geometry_msgs::msg::PoseStamped pose0, pose1; + pose0.pose.position.x = x0; + pose0.pose.position.y = y0; + pose1.pose.position.x = x1; + pose1.pose.position.y = y1; + EXPECT_TRUE(pc.check(pose0)); + rclcpp::sleep_for(std::chrono::seconds(delay)); + if (expected_result) { + EXPECT_TRUE(pc.check(pose1)); + } else { + EXPECT_FALSE(pc.check(pose1)); + } +} + +TEST(SimpleProgressChecker, progress_checker_reset) +{ + auto x = std::make_shared("progress_checker"); + + nav2_core::ProgressChecker * pc = new SimpleProgressChecker; + pc->reset(); + delete pc; + EXPECT_TRUE(true); +} + +TEST(SimpleProgressChecker, unit_tests) +{ + auto x = std::make_shared("progress_checker"); + + SimpleProgressChecker pc; + pc.initialize(x, "nav2_controller"); + checkMacro(pc, 0, 0, 0, 0, 1, true); + checkMacro(pc, 0, 0, 1, 0, 1, true); + checkMacro(pc, 0, 0, 0, 1, 1, true); + checkMacro(pc, 0, 0, 1, 0, 11, true); + checkMacro(pc, 0, 0, 0, 1, 11, true); + checkMacro(pc, 0, 0, 0, 0, 11, false); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index fc7c36e2737..97846ba8eee 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -22,7 +22,6 @@ #include "nav_2d_utils/conversions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_controller/progress_checker.hpp" #include "nav2_controller/nav2_controller.hpp" using namespace std::chrono_literals; @@ -32,6 +31,12 @@ namespace nav2_controller ControllerServer::ControllerServer() : LifecycleNode("controller_server", "", true), + progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), + default_progress_checker_id_{"progress_checker"}, + default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"}, + goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), + default_goal_checker_id_{"goal_checker"}, + default_goal_checker_type_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"} @@ -40,6 +45,8 @@ ControllerServer::ControllerServer() declare_parameter("controller_frequency", 20.0); + declare_parameter("progress_checker_plugin", default_progress_checker_id_); + declare_parameter("goal_checker_plugin", default_goal_checker_id_); declare_parameter("controller_plugins", default_ids_); declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); @@ -61,12 +68,29 @@ ControllerServer::~ControllerServer() nav2_util::CallbackReturn ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { + auto node = shared_from_this(); + RCLCPP_INFO(get_logger(), "Configuring controller interface"); + get_parameter("progress_checker_plugin", progress_checker_id_); + if (progress_checker_id_ == default_progress_checker_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_progress_checker_id_ + ".plugin", + rclcpp::ParameterValue(default_progress_checker_type_)); + } + get_parameter("goal_checker_plugin", goal_checker_id_); + if (goal_checker_id_ == default_goal_checker_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_goal_checker_id_ + ".plugin", + rclcpp::ParameterValue(default_goal_checker_type_)); + } + get_parameter("controller_plugins", controller_ids_); if (controller_ids_ == default_ids_) { for (size_t i = 0; i < default_ids_.size(); ++i) { - declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); + nav2_util::declare_parameter_if_not_declared( + node, default_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_types_[i])); } } controller_types_.resize(controller_ids_.size()); @@ -79,9 +103,30 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->on_configure(state); - auto node = shared_from_this(); - - progress_checker_ = std::make_unique(node); + try { + progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_); + progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_type_); + RCLCPP_INFO( + get_logger(), "Created progress_checker : %s of type %s", + progress_checker_id_.c_str(), progress_checker_type_.c_str()); + progress_checker_->initialize(node, progress_checker_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), + "Failed to create progress_checker. Exception: %s", ex.what()); + } + try { + goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_); + goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_type_); + RCLCPP_INFO( + get_logger(), "Created goal_checker : %s of type %s", + goal_checker_id_.c_str(), goal_checker_type_.c_str()); + goal_checker_->initialize(node, goal_checker_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), + "Failed to create goal_checker. Exception: %s", ex.what()); + } for (size_t i = 0; i != controller_ids_.size(); i++) { try { @@ -96,8 +141,9 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->getTfBuffer(), costmap_ros_); controllers_.insert({controller_ids_[i], controller}); } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); - exit(-1); + RCLCPP_FATAL( + get_logger(), + "Failed to create controller. Exception: %s", ex.what()); } } @@ -105,6 +151,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) controller_ids_concat_ += controller_ids_[i] + std::string(" "); } + RCLCPP_INFO( + get_logger(), + "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); + odom_sub_ = std::make_unique(node); vel_publisher_ = create_publisher("cmd_vel", 1); @@ -165,24 +215,14 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) // Release any allocated resources action_server_.reset(); - for (it = controllers_.begin(); it != controllers_.end(); ++it) { - it->second.reset(); - } odom_sub_.reset(); - vel_publisher_.reset(); action_server_.reset(); + goal_checker_->reset(); return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -ControllerServer::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn ControllerServer::on_shutdown(const rclcpp_lifecycle::State &) { @@ -196,13 +236,10 @@ bool ControllerServer::findControllerId( { if (controllers_.find(c_name) == controllers_.end()) { if (controllers_.size() == 1 && c_name.empty()) { - if (!single_controller_warning_given_) { - RCLCPP_WARN( - get_logger(), "No controller was specified in action call." - " Server will use only plugin loaded %s. " - "This warning will appear once.", controller_ids_concat_.c_str()); - single_controller_warning_given_ = true; - } + RCLCPP_WARN_ONCE( + get_logger(), "No controller was specified in action call." + " Server will use only plugin loaded %s. " + "This warning will appear once.", controller_ids_concat_.c_str()); current_controller = controllers_.begin()->first; } else { RCLCPP_ERROR( @@ -212,6 +249,7 @@ bool ControllerServer::findControllerId( return false; } } else { + RCLCPP_DEBUG(get_logger(), "Selected controller: %s.", c_name.c_str()); current_controller = c_name; } @@ -237,13 +275,8 @@ void ControllerServer::computeControl() rclcpp::Rate loop_rate(controller_frequency_); while (rclcpp::ok()) { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + if (action_server_ == nullptr || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; } @@ -276,7 +309,7 @@ void ControllerServer::computeControl() return; } - RCLCPP_DEBUG(get_logger(), "DWB succeeded, setting result"); + RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result"); publishZeroVelocity(); @@ -294,7 +327,8 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) } controllers_[current_controller_]->setPlan(path); - auto end_pose = *(path.poses.end() - 1); + auto end_pose = path.poses.back(); + goal_checker_->reset(); RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", @@ -310,7 +344,9 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::PlannerException("Failed to obtain robot pose"); } - progress_checker_->check(pose); + if (!progress_checker_->check(pose)) { + throw nav2_core::PlannerException("Failed to make progress"); + } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); @@ -350,7 +386,12 @@ void ControllerServer::updateGlobalPath() void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { auto cmd_vel = std::make_unique(velocity.twist); - vel_publisher_->publish(std::move(cmd_vel)); + if ( + vel_publisher_->is_activated() && + this->count_subscribers(vel_publisher_->get_topic_name()) > 0) + { + vel_publisher_->publish(std::move(cmd_vel)); + } } void ControllerServer::publishZeroVelocity() @@ -362,6 +403,8 @@ void ControllerServer::publishZeroVelocity() velocity.twist.linear.x = 0; velocity.twist.linear.y = 0; velocity.twist.linear.z = 0; + velocity.header.frame_id = costmap_ros_->getBaseFrameID(); + velocity.header.stamp = now(); publishVelocity(velocity); } @@ -375,14 +418,13 @@ bool ControllerServer::isGoalReached() nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); - return controllers_[current_controller_]->isGoalReached(pose, velocity); + return goal_checker_->isGoalReached(pose.pose, end_pose_, velocity); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) { geometry_msgs::msg::PoseStamped current_pose; if (!costmap_ros_->getRobotPose(current_pose)) { - RCLCPP_ERROR(this->get_logger(), "Could not get robot pose"); return false; } pose = current_pose; diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 1e6e98052c3..69aa10f25b0 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -112,20 +112,6 @@ class Controller virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) = 0; - - /** - * @brief Controller isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. - * - * The pose that it checks against is the last pose in the current global plan. - * The calculation is delegated to the goal_checker plugin. - * - * @param pose Current pose - * @param velocity Current velocity - * @return True if the robot should be considered as having reached the goal. - */ - virtual bool isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp new file mode 100644 index 00000000000..2dd9c5bef89 --- /dev/null +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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__PROGRESS_CHECKER_HPP_ +#define NAV2_CORE__PROGRESS_CHECKER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_core +{ +/** + * @class nav2_core::ProgressChecker + * @brief This class defines the plugin interface used to check the + * position of the robot to make sure that it is actually progressing + * towards a goal. + */ +class ProgressChecker +{ +public: + typedef std::shared_ptr Ptr; + + virtual ~ProgressChecker() {} + + /** + * @brief Initialize parameters for ProgressChecker + * @param node Node pointer + */ + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) = 0; + /** + * @brief Checks if the robot has moved compare to previous + * pose + * @param current_pose Current pose of the robot + * @return True if progress is made + */ + virtual bool check(geometry_msgs::msg::PoseStamped & current_pose) = 0; + /** + * @brief Reset class state upon calling + */ + virtual void reset() = 0; +}; +} // namespace nav2_core + +#endif // NAV2_CORE__PROGRESS_CHECKER_HPP_ diff --git a/nav2_core/package.xml b/nav2_core/package.xml index d9e50dcd9cb..2ae59bc25ad 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.1 + 0.4.2 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index d7508415edf..4c9a2a93666 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) find_package(Eigen3 REQUIRED) @@ -72,6 +73,7 @@ set(dependencies tf2_ros tf2_sensor_msgs visualization_msgs + angles ) ament_target_dependencies(nav2_costmap_2d_core @@ -84,6 +86,7 @@ add_library(layers SHARED plugins/obstacle_layer.cpp src/observation_buffer.cpp plugins/voxel_layer.cpp + plugins/range_sensor_layer.cpp ) ament_target_dependencies(layers ${dependencies} diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index 62bfac732b9..a0725392332 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -12,6 +12,9 @@ Similar to obstacle costmap, but uses 3D voxel grid to store data. + + A range-sensor (sonar, IR) based obstacle layer for costmap_2d + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 5524f5737d3..541a246a513 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -97,7 +97,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Subscribes to sensor topics if necessary and starts costmap diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp new file mode 100644 index 00000000000..ce2861599d7 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -0,0 +1,138 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 David V. Lu!! + * Copyright (c) 2020, Bytes Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_ +#define NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_ + +#include +#include +#include +#include + +#include "map_msgs/msg/occupancy_grid_update.hpp" +#include "message_filters/subscriber.h" +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "sensor_msgs/msg/range.hpp" + +namespace nav2_costmap_2d +{ + +class RangeSensorLayer : public CostmapLayer +{ +public: + enum class InputSensorType + { + VARIABLE, + FIXED, + ALL + }; + + RangeSensorLayer(); + + virtual void onInitialize(); + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, double * max_x, double * max_y); + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, + int min_j, int max_i, int max_j); + virtual void reset(); + virtual void deactivate(); + virtual void activate(); + + void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message); + +private: + void updateCostmap(); + void updateCostmap(sensor_msgs::msg::Range & range_message, bool clear_sensor_cone); + + void processRangeMsg(sensor_msgs::msg::Range & range_message); + void processFixedRangeMsg(sensor_msgs::msg::Range & range_message); + void processVariableRangeMsg(sensor_msgs::msg::Range & range_message); + + void resetRange(); + + inline double gamma(double theta); + inline double delta(double phi); + inline double sensor_model(double r, double phi, double theta); + + inline void get_deltas(double angle, double * dx, double * dy); + inline void update_cell( + double ox, double oy, double ot, + double r, double nx, double ny, bool clear); + + inline double to_prob(unsigned char c) + { + return static_cast(c) / nav2_costmap_2d::LETHAL_OBSTACLE; + } + inline unsigned char to_cost(double p) + { + return static_cast(p * nav2_costmap_2d::LETHAL_OBSTACLE); + } + + std::function processRangeMessageFunc_; + std::mutex range_message_mutex_; + std::list range_msgs_buffer_; + + double max_angle_, phi_v_; + double inflate_cone_; + std::string global_frame_; + + double clear_threshold_, mark_threshold_; + bool clear_on_max_reading_; + + tf2::Duration transform_tolerance_; + double no_readings_timeout_; + rclcpp::Time last_reading_time_; + unsigned int buffered_readings_; + std::vector::SharedPtr> range_subs_; + double min_x_, min_y_, max_x_, max_y_; + + float area(int x1, int y1, int x2, int y2, int x3, int y3) + { + return fabs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0); + } + + int orient2d(int Ax, int Ay, int Bx, int By, int Cx, int Cy) + { + return (Bx - Ax) * (Cy - Ay) - (By - Ay) * (Cx - Ax); + } +}; +} // namespace nav2_costmap_2d +#endif // NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index f6f01e8c563..bbd5daa757e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -111,6 +111,8 @@ class StaticLayer : public CostmapLayer bool trinary_costmap_; bool map_received_{false}; tf2::Duration transform_tolerance_; + std::atomic update_in_progress_; + nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 7ca65cc7c2f..02f6c7fd9db 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.1 + 0.4.2 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 86561d334b3..df8405a3c91 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -40,6 +40,8 @@ #include #include #include +#include +#include #include "nav2_costmap_2d/costmap_math.hpp" #include "nav2_costmap_2d/footprint.hpp" @@ -163,7 +165,7 @@ InflationLayer::updateCosts( } // make sure the inflation list is empty at the beginning of the cycle (should always be true) - for (auto & dist:inflation_cells_) { + for (auto & dist : inflation_cells_) { RCLCPP_FATAL_EXPRESSION( rclcpp::get_logger("nav2_costmap_2d"), !dist.empty(), "The inflation list must be empty at the beginning of inflation"); @@ -215,9 +217,10 @@ InflationLayer::updateCosts( // Process cells by increasing distance; new cells are appended to the // corresponding distance bin, so they // can overtake previously inserted but farther away cells - for (const auto & dist_bin: inflation_cells_) { + for (const auto & dist_bin : inflation_cells_) { for (std::size_t i = 0; i < dist_bin.size(); ++i) { - // Do not use iterator or for-range based loops to iterate though dist_bin, since it's size might + // Do not use iterator or for-range based loops to + // iterate though dist_bin, since it's size might // change when a new cell is enqueued, invalidating all iterators unsigned int index = dist_bin[i].index_; @@ -260,7 +263,7 @@ InflationLayer::updateCosts( } } - for (auto & dist:inflation_cells_) { + for (auto & dist : inflation_cells_) { dist.clear(); dist.reserve(200); } diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp new file mode 100644 index 00000000000..8a6124a47a9 --- /dev/null +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -0,0 +1,524 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 David V. Lu!! + * Copyright (c) 2020, Bytes Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "nav2_costmap_2d/range_sensor_layer.hpp" + +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::RangeSensorLayer, nav2_costmap_2d::Layer) + +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; + +using namespace std::literals::chrono_literals; + +namespace nav2_costmap_2d +{ + +RangeSensorLayer::RangeSensorLayer() {} + +void RangeSensorLayer::onInitialize() +{ + current_ = true; + buffered_readings_ = 0; + last_reading_time_ = node_->now(); + default_value_ = to_cost(0.5); + + matchSize(); + resetRange(); + + declareParameter("enabled", rclcpp::ParameterValue(true)); + node_->get_parameter(name_ + "." + "enabled", enabled_); + declareParameter("phi", rclcpp::ParameterValue(1.2)); + node_->get_parameter(name_ + "." + "phi", phi_v_); + declareParameter("inflate_cone", rclcpp::ParameterValue(1.0)); + node_->get_parameter(name_ + "." + "phi", phi_v_); + declareParameter("no_readings_timeout", rclcpp::ParameterValue(0.0)); + node_->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_); + declareParameter("clear_threshold", rclcpp::ParameterValue(0.2)); + node_->get_parameter(name_ + "." + "clear_threshold", clear_threshold_); + declareParameter("mark_threshold", rclcpp::ParameterValue(0.8)); + node_->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); + declareParameter("clear_on_max_reading", rclcpp::ParameterValue(false)); + node_->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_); + + double temp_tf_tol = 0.0; + node_->get_parameter("transform_tolerance", temp_tf_tol); + transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); + + std::vector topic_names{}; + declareParameter("topics", rclcpp::ParameterValue(topic_names)); + node_->get_parameter(name_ + "." + "topics", topic_names); + + InputSensorType input_sensor_type = InputSensorType::ALL; + std::string sensor_type_name; + declareParameter("input_sensor_type", rclcpp::ParameterValue("ALL")); + node_->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name); + + std::transform( + sensor_type_name.begin(), sensor_type_name.end(), + sensor_type_name.begin(), ::toupper); + RCLCPP_INFO( + node_->get_logger(), "%s: %s as input_sensor_type given", + name_.c_str(), sensor_type_name.c_str()); + + if (sensor_type_name == "VARIABLE") { + input_sensor_type = InputSensorType::VARIABLE; + } else if (sensor_type_name == "FIXED") { + input_sensor_type = InputSensorType::FIXED; + } else if (sensor_type_name == "ALL") { + input_sensor_type = InputSensorType::ALL; + } else { + RCLCPP_ERROR( + node_->get_logger(), "%s: Invalid input sensor type: %s. Defaulting to ALL.", + name_.c_str(), sensor_type_name.c_str()); + } + + // Validate topic names list: it must be a (normally non-empty) list of strings + if (topic_names.empty()) { + RCLCPP_FATAL( + node_->get_logger(), "Invalid topic names list: it must" + "be a non-empty list of strings"); + return; + } + + // Traverse the topic names list subscribing to all of them with the same callback method + for (auto & topic_name : topic_names) { + if (input_sensor_type == InputSensorType::VARIABLE) { + processRangeMessageFunc_ = std::bind( + &RangeSensorLayer::processVariableRangeMsg, this, + std::placeholders::_1); + } else if (input_sensor_type == InputSensorType::FIXED) { + processRangeMessageFunc_ = std::bind( + &RangeSensorLayer::processFixedRangeMsg, this, + std::placeholders::_1); + } else if (input_sensor_type == InputSensorType::ALL) { + processRangeMessageFunc_ = std::bind( + &RangeSensorLayer::processRangeMsg, this, + std::placeholders::_1); + } else { + RCLCPP_ERROR( + node_->get_logger(), + "%s: Invalid input sensor type: %s. Did you make a new type" + "and forgot to choose the subscriber for it?", + name_.c_str(), sensor_type_name.c_str()); + } + range_subs_.push_back( + node_->create_subscription( + topic_name, rclcpp::SensorDataQoS(), std::bind( + &RangeSensorLayer::bufferIncomingRangeMsg, this, + std::placeholders::_1))); + + RCLCPP_INFO( + node_->get_logger(), "RangeSensorLayer: subscribed to " + "topic %s", range_subs_.back()->get_topic_name()); + } + global_frame_ = layered_costmap_->getGlobalFrameID(); +} + + +double RangeSensorLayer::gamma(double theta) +{ + if (fabs(theta) > max_angle_) { + return 0.0; + } else { + return 1 - pow(theta / max_angle_, 2); + } +} + +double RangeSensorLayer::delta(double phi) +{ + return 1 - (1 + tanh(2 * (phi - phi_v_))) / 2; +} + +void RangeSensorLayer::get_deltas(double angle, double * dx, double * dy) +{ + double ta = tan(angle); + if (ta == 0) { + *dx = 0; + } else { + *dx = resolution_ / ta; + } + + *dx = copysign(*dx, cos(angle)); + *dy = copysign(resolution_, sin(angle)); +} + +double RangeSensorLayer::sensor_model(double r, double phi, double theta) +{ + double lbda = delta(phi) * gamma(theta); + + double delta = resolution_; + + if (phi >= 0.0 && phi < r - 2 * delta * r) { + return (1 - lbda) * (0.5); + } else if (phi < r - delta * r) { + return lbda * 0.5 * pow((phi - (r - 2 * delta * r)) / (delta * r), 2) + + (1 - lbda) * .5; + } else if (phi < r + delta * r) { + double J = (r - phi) / (delta * r); + return lbda * ((1 - (0.5) * pow(J, 2)) - 0.5) + 0.5; + } else { + return 0.5; + } +} + +void RangeSensorLayer::bufferIncomingRangeMsg( + const sensor_msgs::msg::Range::SharedPtr range_message) +{ + range_message_mutex_.lock(); + range_msgs_buffer_.push_back(*range_message); + range_message_mutex_.unlock(); +} + +void RangeSensorLayer::updateCostmap() +{ + std::list range_msgs_buffer_copy; + + range_message_mutex_.lock(); + range_msgs_buffer_copy = std::list(range_msgs_buffer_); + range_msgs_buffer_.clear(); + range_message_mutex_.unlock(); + + for (auto & range_msgs_it : range_msgs_buffer_copy) { + processRangeMessageFunc_(range_msgs_it); + } +} + +void RangeSensorLayer::processRangeMsg(sensor_msgs::msg::Range & range_message) +{ + if (range_message.min_range == range_message.max_range) { + processFixedRangeMsg(range_message); + } else { + processVariableRangeMsg(range_message); + } +} + +void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::msg::Range & range_message) +{ + if (!std::isinf(range_message.range)) { + RCLCPP_ERROR( + node_->get_logger(), + "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. " + "Only -Inf (== object detected) and Inf (== no object detected) are valid.", + range_message.header.frame_id.c_str()); + return; + } + + bool clear_sensor_cone = false; + + if (range_message.range > 0) { // +inf + if (!clear_on_max_reading_) { + return; // no clearing at all + } + clear_sensor_cone = true; + } + + range_message.range = range_message.min_range; + + updateCostmap(range_message, clear_sensor_cone); +} + +void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::msg::Range & range_message) +{ + if (range_message.range < range_message.min_range || range_message.range > + range_message.max_range) + { + return; + } + + bool clear_sensor_cone = false; + + if (range_message.range >= range_message.max_range && clear_on_max_reading_) { + clear_sensor_cone = true; + } + + updateCostmap(range_message, clear_sensor_cone); +} + +void RangeSensorLayer::updateCostmap( + sensor_msgs::msg::Range & range_message, + bool clear_sensor_cone) +{ + max_angle_ = range_message.field_of_view / 2; + + geometry_msgs::msg::PointStamped in, out; + in.header.stamp = range_message.header.stamp; + in.header.frame_id = range_message.header.frame_id; + + if (!tf_->canTransform( + in.header.frame_id, global_frame_, tf2_ros::fromMsg(in.header.stamp))) + { + RCLCPP_INFO( + node_->get_logger(), "Range sensor layer can't transform from %s to %s", + global_frame_.c_str(), in.header.frame_id.c_str()); + return; + } + + tf_->transform(in, out, global_frame_, transform_tolerance_); + + double ox = out.point.x, oy = out.point.y; + + in.point.x = range_message.range; + + tf_->transform(in, out, global_frame_, transform_tolerance_); + + double tx = out.point.x, ty = out.point.y; + + // calculate target props + double dx = tx - ox, dy = ty - oy, theta = atan2(dy, dx), d = sqrt(dx * dx + dy * dy); + + // Integer Bounds of Update + int bx0, by0, bx1, by1; + + // Triangle that will be really updated; the other cells within bounds are ignored + // This triangle is formed by the origin and left and right sides of sonar cone + int Ox, Oy, Ax, Ay, Bx, By; + + // Bounds includes the origin + worldToMapNoBounds(ox, oy, Ox, Oy); + bx1 = bx0 = Ox; + by1 = by0 = Oy; + touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_); + + // Update Map with Target Point + unsigned int aa, ab; + if (worldToMap(tx, ty, aa, ab)) { + setCost(aa, ab, 233); + touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_); + } + + double mx, my; + + // Update left side of sonar cone + mx = ox + cos(theta - max_angle_) * d * 1.2; + my = oy + sin(theta - max_angle_) * d * 1.2; + worldToMapNoBounds(mx, my, Ax, Ay); + bx0 = std::min(bx0, Ax); + bx1 = std::max(bx1, Ax); + by0 = std::min(by0, Ay); + by1 = std::max(by1, Ay); + touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_); + + // Update right side of sonar cone + mx = ox + cos(theta + max_angle_) * d * 1.2; + my = oy + sin(theta + max_angle_) * d * 1.2; + + worldToMapNoBounds(mx, my, Bx, By); + bx0 = std::min(bx0, Bx); + bx1 = std::max(bx1, Bx); + by0 = std::min(by0, By); + by1 = std::max(by1, By); + touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_); + + // Limit Bounds to Grid + bx0 = std::max(0, bx0); + by0 = std::max(0, by0); + bx1 = std::min(static_cast(size_x_), bx1); + by1 = std::min(static_cast(size_y_), by1); + + for (unsigned int x = bx0; x <= (unsigned int)bx1; x++) { + for (unsigned int y = by0; y <= (unsigned int)by1; y++) { + bool update_xy_cell = true; + + // Unless inflate_cone_ is set to 100 %, we update cells only within the + // (partially inflated) sensor cone, projected on the costmap as a triangle. + // 0 % corresponds to just the triangle, but if your sensor fov is very + // narrow, the covered area can become zero due to cell discretization. + // See wiki description for more details + if (inflate_cone_ < 1.0) { + // Determine barycentric coordinates + int w0 = orient2d(Ax, Ay, Bx, By, x, y); + int w1 = orient2d(Bx, By, Ox, Oy, x, y); + int w2 = orient2d(Ox, Oy, Ax, Ay, x, y); + + // Barycentric coordinates inside area threshold; this is not mathematically + // sound at all, but it works! + float bcciath = -static_cast(inflate_cone_) * area(Ax, Ay, Bx, By, Ox, Oy); + update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath; + } + + if (update_xy_cell) { + double wx, wy; + mapToWorld(x, y, wx, wy); + update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone); + } + } + } + + buffered_readings_++; + last_reading_time_ = node_->now(); +} + +void RangeSensorLayer::update_cell( + double ox, double oy, double ot, double r, + double nx, double ny, bool clear) +{ + unsigned int x, y; + if (worldToMap(nx, ny, x, y)) { + double dx = nx - ox, dy = ny - oy; + double theta = atan2(dy, dx) - ot; + theta = angles::normalize_angle(theta); + double phi = sqrt(dx * dx + dy * dy); + double sensor = 0.0; + if (!clear) { + sensor = sensor_model(r, phi, theta); + } + double prior = to_prob(getCost(x, y)); + double prob_occ = sensor * prior; + double prob_not = (1 - sensor) * (1 - prior); + double new_prob = prob_occ / (prob_occ + prob_not); + + RCLCPP_DEBUG(node_->get_logger(), "%f %f | %f %f = %f", dx, dy, theta, phi, sensor); + RCLCPP_DEBUG(node_->get_logger(), "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob); + unsigned char c = to_cost(new_prob); + setCost(x, y, c); + } +} + +void RangeSensorLayer::resetRange() +{ + min_x_ = min_y_ = std::numeric_limits::max(); + max_x_ = max_y_ = -std::numeric_limits::max(); +} + +void RangeSensorLayer::updateBounds( + double robot_x, double robot_y, + double robot_yaw, double * min_x, double * min_y, + double * max_x, double * max_y) +{ + robot_yaw = 0 + robot_yaw; // Avoid error if variable not in use + if (layered_costmap_->isRolling()) { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + + updateCostmap(); + + *min_x = std::min(*min_x, min_x_); + *min_y = std::min(*min_y, min_y_); + *max_x = std::max(*max_x, max_x_); + *max_y = std::max(*max_y, max_y_); + + resetRange(); + + if (!enabled_) { + current_ = true; + return; + } + + if (buffered_readings_ == 0) { + if (no_readings_timeout_ > 0.0 && + (node_->now() - last_reading_time_).seconds() > no_readings_timeout_) + { + RCLCPP_WARN( + node_->get_logger(), "No range readings received for %.2f seconds, " + "while expected at least every %.2f seconds.", + (node_->now() - last_reading_time_).seconds(), + no_readings_timeout_); + current_ = false; + } + } +} + +void RangeSensorLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) { + return; + } + + unsigned char * master_array = master_grid.getCharMap(); + unsigned int span = master_grid.getSizeInCellsX(); + unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_); + + for (int j = min_j; j < max_j; j++) { + unsigned int it = j * span + min_i; + for (int i = min_i; i < max_i; i++) { + unsigned char prob = costmap_[it]; + unsigned char current; + if (prob == nav2_costmap_2d::NO_INFORMATION) { + it++; + continue; + } else if (prob > mark) { + current = nav2_costmap_2d::LETHAL_OBSTACLE; + } else if (prob < clear) { + current = nav2_costmap_2d::FREE_SPACE; + } else { + it++; + continue; + } + + unsigned char old_cost = master_array[it]; + + if (old_cost == NO_INFORMATION || old_cost < current) { + master_array[it] = current; + } + it++; + } + } + + buffered_readings_ = 0; + current_ = true; +} + +void RangeSensorLayer::reset() +{ + RCLCPP_DEBUG(node_->get_logger(), "Reseting range sensor layer..."); + deactivate(); + resetMaps(); + current_ = true; + activate(); +} + +void RangeSensorLayer::deactivate() +{ + range_msgs_buffer_.clear(); +} + +void RangeSensorLayer::activate() +{ + range_msgs_buffer_.clear(); +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 8180e3f6ba2..cb3a01f1be4 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -58,6 +58,7 @@ namespace nav2_costmap_2d { StaticLayer::StaticLayer() +: map_buffer_(nullptr) { } @@ -123,10 +124,18 @@ StaticLayer::getParameters() declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); + declareParameter("map_topic", rclcpp::ParameterValue("")); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); - node_->get_parameter("map_topic", map_topic_); + std::string private_map_topic, global_map_topic; + node_->get_parameter(name_ + "." + "map_topic", private_map_topic); + node_->get_parameter("map_topic", global_map_topic); + if (!private_map_topic.empty()) { + map_topic_ = private_map_topic; + } else { + map_topic_ = global_map_topic; + } node_->get_parameter( name_ + "." + "map_subscribe_transient_local", map_subscribe_transient_local_); @@ -140,6 +149,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; + update_in_progress_.store(false); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); } @@ -193,6 +203,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) unsigned int index = 0; + // we have a new map, update full size of map + std::lock_guard guard(*getMutex()); + // initialize the costmap with static data for (unsigned int i = 0; i < size_y; ++i) { for (unsigned int j = 0; j < size_x; ++j) { @@ -204,8 +217,6 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) map_frame_ = new_map.header.frame_id; - // we have a new map, update full size of map - std::lock_guard guard(*getMutex()); x_ = y_ = 0; width_ = size_x_; height_ = size_y_; @@ -249,9 +260,15 @@ void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { std::lock_guard guard(*getMutex()); - processMap(*new_map); if (!map_received_) { map_received_ = true; + processMap(*new_map); + } + if (update_in_progress_.load()) { + map_buffer_ = new_map; + } else { + processMap(*new_map); + map_buffer_ = nullptr; } } @@ -311,6 +328,14 @@ StaticLayer::updateBounds( } std::lock_guard guard(*getMutex()); + update_in_progress_.store(true); + + // If there is a new available map, load it. + if (map_buffer_) { + processMap(*map_buffer_); + map_buffer_ = nullptr; + } + if (!layered_costmap_->isRolling() ) { if (!(has_updated_data_ || has_extra_bounds_)) { return; @@ -338,6 +363,7 @@ StaticLayer::updateCosts( int min_i, int min_j, int max_i, int max_j) { if (!enabled_) { + update_in_progress_.store(false); return; } if (!map_received_) { @@ -347,6 +373,7 @@ StaticLayer::updateCosts( RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received"); count = 0; } + update_in_progress_.store(false); return; } @@ -369,6 +396,7 @@ StaticLayer::updateCosts( transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); + update_in_progress_.store(false); return; } // Copy map data given proper transformations @@ -393,6 +421,7 @@ StaticLayer::updateCosts( } } } + update_in_progress_.store(false); } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 75c7552628b..b172bd87df2 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -274,13 +274,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -Costmap2DROS::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn Costmap2DROS::on_shutdown(const rclcpp_lifecycle::State &) { @@ -312,16 +305,16 @@ Costmap2DROS::getParameters() get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); + auto node = shared_from_this(); + if (plugin_names_ == default_plugins_) { for (size_t i = 0; i < default_plugins_.size(); ++i) { - declare_parameter(default_plugins_[i] + ".plugin", default_types_[i]); + nav2_util::declare_parameter_if_not_declared( + node, default_plugins_[i] + ".plugin", rclcpp::ParameterValue(default_types_[i])); } } plugin_types_.resize(plugin_names_.size()); - // Semantic checks... - auto node = shared_from_this(); - // 1. All plugins must have 'plugin' param defined in their namespace to define the plugin type for (size_t i = 0; i < plugin_names_.size(); ++i) { plugin_types_[i] = nav2_util::get_plugin_type_param(node, plugin_names_[i]); diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 48ba1ea467d..a03b79c0dcd 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "nav2_costmap_2d/footprint.hpp" diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 4170ad3875b..5ecbeba923d 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -43,6 +43,17 @@ target_link_libraries(obstacle_tests_exec layers ) +ament_add_gtest_executable(range_tests_exec + range_tests.cpp +) +ament_target_dependencies(range_tests_exec + ${dependencies} +) +target_link_libraries(range_tests_exec + nav2_costmap_2d_core + layers +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -83,6 +94,16 @@ ament_add_test(obstacle_tests TEST_EXECUTABLE=$ ) +ament_add_test(range_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 40262ce9833..2e3aa19c53c 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -44,7 +44,7 @@ #include "nav2_costmap_2d/obstacle_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" #include "nav2_util/node_utils.hpp" using geometry_msgs::msg::Point; diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index 7e1910f5f76..a635cd8e6a8 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -41,7 +41,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" using std::begin; using std::end; diff --git a/nav2_costmap_2d/test/integration/range_tests.cpp b/nav2_costmap_2d/test/integration/range_tests.cpp new file mode 100644 index 00000000000..196406e7e02 --- /dev/null +++ b/nav2_costmap_2d/test/integration/range_tests.cpp @@ -0,0 +1,293 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bytes Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/observation_buffer.hpp" +#include "../testing_helper.hpp" +#include "sensor_msgs/msg/range.hpp" + +using std::begin; +using std::end; +using std::for_each; +using std::all_of; +using std::none_of; +using std::pair; +using std::string; + +class RclCppFixture +{ +public: + RclCppFixture() + { + rclcpp::init(0, nullptr); + } + + ~RclCppFixture() + { + rclcpp::shutdown(); + } +}; + +RclCppFixture g_rclcppfixture; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const string & name) + : nav2_util::LifecycleNode(name) + { + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() + : node_(std::make_shared("range_test_node")), + tf_(node_->get_clock()) + { + // Standard non-plugin specific parameters + node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); + node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); + node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter( + "unknown_cost_value", + rclcpp::ParameterValue(static_cast(0xff))); + node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("range"))); + node_->declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map"))); + + + // Range sensor specific parameters + node_->declare_parameter( + "range.topics", + rclcpp::ParameterValue( + std::vector{"/range/topic"})); + node_->declare_parameter("range.phi", rclcpp::ParameterValue(1.2)); + node_->declare_parameter("range.clear_on_max_reading", rclcpp::ParameterValue(true)); + } + + ~TestNode() {} + +protected: + std::shared_ptr node_; + tf2_ros::Buffer tf_; +}; + +// Test clearing at max range +TEST_F(TestNode, testClearingAtMaxRange) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node_->now(); + transform.header.frame_id = "frame"; + transform.child_frame_id = "base_link"; + transform.transform.translation.y = 5; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr rlayer{nullptr}; + addRangeLayer(layers, tf_, node_, rlayer); + + sensor_msgs::msg::Range msg; + msg.min_range = 1.0; + msg.max_range = 7.0; + msg.range = 2.0; + msg.header.stamp = node_->now(); + msg.header.frame_id = "base_link"; + msg.radiation_type = msg.ULTRASOUND; + msg.field_of_view = 0.174533; // 10 deg + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 254); + + msg.range = 7.0; + msg.header.stamp = node_->now(); + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 0); +} + +// Testing fixed scan with robot forward motion +TEST_F(TestNode, testProbabalisticModelForward) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node_->now(); + transform.header.frame_id = "frame"; + transform.child_frame_id = "base_link"; + transform.transform.translation.y = 5; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr rlayer{nullptr}; + addRangeLayer(layers, tf_, node_, rlayer); + + sensor_msgs::msg::Range msg; + msg.min_range = 1.0; + msg.max_range = 10.0; + msg.range = 3.0; + msg.header.stamp = node_->now(); + msg.header.frame_id = "base_link"; + msg.radiation_type = msg.ULTRASOUND; + msg.field_of_view = 0.174533; // 10 deg + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + transform.transform.translation.y = 5; + transform.transform.translation.x = 4; + tf_.setTransform(transform, "default_authority", true); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + transform.transform.translation.y = 5; + transform.transform.translation.x = 6; + tf_.setTransform(transform, "default_authority", true); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + ASSERT_EQ(layers.getCostmap()->getCost(5, 5), 254); + ASSERT_EQ(layers.getCostmap()->getCost(6, 5), 0); + ASSERT_EQ(layers.getCostmap()->getCost(7, 5), 254); + ASSERT_EQ(layers.getCostmap()->getCost(8, 5), 0); + ASSERT_EQ(layers.getCostmap()->getCost(9, 5), 254); +} + +// Testing fixed motion with downward movement +TEST_F(TestNode, testProbabalisticModelDownward) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node_->now(); + transform.header.frame_id = "frame"; + transform.child_frame_id = "base_link"; + transform.transform.translation.y = 3; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr rlayer{nullptr}; + addRangeLayer(layers, tf_, node_, rlayer); + + sensor_msgs::msg::Range msg; + msg.min_range = 1.0; + msg.max_range = 10.0; + msg.range = 1.0; + msg.header.stamp = node_->now(); + msg.header.frame_id = "base_link"; + msg.radiation_type = msg.ULTRASOUND; + msg.field_of_view = 0.174533; // 10 deg + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + transform.transform.translation.y = 5; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + transform.transform.translation.y = 7; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + ASSERT_EQ(layers.getCostmap()->getCost(3, 3), 254); + ASSERT_EQ(layers.getCostmap()->getCost(3, 4), 0); + ASSERT_EQ(layers.getCostmap()->getCost(3, 5), 254); + ASSERT_EQ(layers.getCostmap()->getCost(3, 6), 0); + ASSERT_EQ(layers.getCostmap()->getCost(3, 7), 254); +} diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 56baa03a4b4..ad1fb15d3a0 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -24,7 +24,7 @@ #include "nav2_costmap_2d/static_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_costmap_2d/costmap_2d_publisher.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/node_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp similarity index 90% rename from nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp rename to nav2_costmap_2d/test/testing_helper.hpp index 013341adf90..82c7d53ec2b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -23,6 +23,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/static_layer.hpp" +#include "nav2_costmap_2d/range_sensor_layer.hpp" #include "nav2_costmap_2d/obstacle_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -97,6 +98,16 @@ void addObstacleLayer( layers.addPlugin(std::shared_ptr(olayer)); } +void addRangeLayer( + nav2_costmap_2d::LayeredCostmap & layers, + tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr & rlayer) +{ + rlayer = std::make_shared(); + rlayer->initialize(&layers, "range", &tf, node, nullptr, nullptr /*TODO*/); + layers.addPlugin(std::shared_ptr(rlayer)); +} + void addObservation( std::shared_ptr olayer, double x, double y, double z = 0.0, double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true) diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 490a7190f73..b1d715f7d14 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.1 + 0.4.2 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 4ded71cb7eb..2da4b44a52c 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -99,20 +99,6 @@ class DWBLocalPlanner : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) override; - /** - * @brief nav2_core isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. - * - * The pose that it checks against is the last pose in the current global plan. - * The calculation is delegated to the goal_checker plugin. - * - * @param pose Current pose - * @param velocity Current velocity - * @return True if the robot should be considered as having reached the goal. - */ - bool isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) override; - /** * @brief Score a given command. Can be used for testing. * @@ -210,9 +196,6 @@ class DWBLocalPlanner : public nav2_core::Controller pluginlib::ClassLoader traj_gen_loader_; TrajectoryGenerator::Ptr traj_generator_; - pluginlib::ClassLoader goal_checker_loader_; - nav2_core::GoalChecker::Ptr goal_checker_; - pluginlib::ClassLoader critic_loader_; std::vector critics_; diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index bd787e51b6f..ccd46651690 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.1 + 0.4.2 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 011ea530422..245323db510 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -59,7 +59,6 @@ namespace dwb_core DWBLocalPlanner::DWBLocalPlanner() : traj_gen_loader_("dwb_core", "dwb_core::TrajectoryGenerator"), - goal_checker_loader_("dwb_core", "nav2_core::GoalChecker"), critic_loader_("dwb_core", "dwb_core::TrajectoryCritic") { } @@ -87,9 +86,6 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".trajectory_generator_name", rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); - declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".goal_checker_name", - rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker"))); declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); @@ -98,7 +94,6 @@ void DWBLocalPlanner::configure( rclcpp::ParameterValue(true)); std::string traj_generator_name; - std::string goal_checker_name; double transform_tolerance; node_->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); @@ -109,7 +104,6 @@ void DWBLocalPlanner::configure( node_->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); node_->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); node_->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); - node_->get_parameter(dwb_plugin_name_ + ".goal_checker_name", goal_checker_name); node_->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); @@ -118,10 +112,8 @@ void DWBLocalPlanner::configure( pub_->on_configure(); traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name); - goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name); traj_generator_->initialize(node_, dwb_plugin_name_); - goal_checker_->initialize(node_, dwb_plugin_name_); try { loadCritics(); @@ -149,7 +141,6 @@ DWBLocalPlanner::cleanup() pub_->on_cleanup(); traj_generator_.reset(); - goal_checker_.reset(); } std::string @@ -263,42 +254,6 @@ DWBLocalPlanner::loadBackwardsCompatibleParameters() /* *INDENT-ON* */ } -bool -DWBLocalPlanner::isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) -{ - if (global_plan_.poses.size() == 0) { - RCLCPP_WARN( - rclcpp::get_logger( - "DWBLocalPlanner"), "Cannot check if the goal is reached without the goal being set!"); - return false; - } - nav_2d_msgs::msg::Pose2DStamped local_start_pose2d, goal_pose2d, local_goal_pose2d; - - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), - nav_2d_utils::poseStampedToPose2D(pose), - local_start_pose2d, transform_tolerance_); - - goal_pose2d.header.frame_id = global_plan_.header.frame_id; - goal_pose2d.pose = global_plan_.poses.back(); - - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), goal_pose2d, - local_goal_pose2d, transform_tolerance_); - - geometry_msgs::msg::PoseStamped local_start_pose, local_goal_pose; - local_start_pose = nav_2d_utils::pose2DToPoseStamped(local_start_pose2d); - local_goal_pose = nav_2d_utils::pose2DToPoseStamped(local_goal_pose2d); - - bool ret = goal_checker_->isGoalReached(local_start_pose.pose, local_goal_pose.pose, velocity); - if (ret) { - RCLCPP_INFO(rclcpp::get_logger("DWBLocalPlanner"), "Goal reached!"); - } - return ret; -} - void DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) { @@ -308,7 +263,6 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) } traj_generator_->reset(); - goal_checker_->reset(); pub_->publishGlobalPlan(path2d); global_plan_ = path2d; @@ -371,6 +325,9 @@ DWBLocalPlanner::computeVelocityCommands( prepareGlobalPlan(pose, transformed_plan, goal_pose); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + for (TrajectoryCritic::Ptr critic : critics_) { if (critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan) == false) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); @@ -390,6 +347,8 @@ DWBLocalPlanner::computeVelocityCommands( critic->debrief(cmd_vel.velocity); } + lock.unlock(); + pub_->publishLocalPlan(pose.header, best.traj); pub_->publishCostGrid(costmap_ros_, critics_); @@ -401,6 +360,9 @@ DWBLocalPlanner::computeVelocityCommands( for (TrajectoryCritic::Ptr critic : critics_) { critic->debrief(empty_cmd); } + + lock.unlock(); + pub_->publishLocalPlan(pose.header, empty_traj); pub_->publishCostGrid(costmap_ros_, critics_); diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index a11a78356bc..1e1c22895ae 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.1 + 0.4.2 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index a6fc418451c..e98ba11d3d7 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.1 + 0.4.2 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index ea4d7868eaa..6e54ad061e3 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -27,19 +27,6 @@ include_directories( include ) -add_library(simple_goal_checker SHARED src/simple_goal_checker.cpp) -ament_target_dependencies(simple_goal_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - - -add_library(stopped_goal_checker SHARED src/stopped_goal_checker.cpp) -target_link_libraries(stopped_goal_checker simple_goal_checker) -ament_target_dependencies(stopped_goal_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - - add_library(standard_traj_generator SHARED src/standard_traj_generator.cpp src/limited_accel_generator.cpp @@ -60,7 +47,7 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -install(TARGETS simple_goal_checker stopped_goal_checker standard_traj_generator +install(TARGETS standard_traj_generator ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -73,8 +60,6 @@ install(FILES plugins.xml ) ament_export_include_directories(include) -ament_export_libraries(simple_goal_checker) -ament_export_libraries(stopped_goal_checker) ament_export_libraries(standard_traj_generator) pluginlib_export_plugin_description_file(dwb_core plugins.xml) diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index fbc20f0ab3d..08b6a78bec0 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.1 + 0.4.2 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index 272f38cf4ca..29845a84998 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -1,7 +1,4 @@ ament_add_gtest(vtest velocity_iterator_test.cpp) -ament_add_gtest(goal_checker goal_checker.cpp) -target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker) - ament_add_gtest(twist_gen_test twist_gen.cpp) target_link_libraries(twist_gen_test standard_traj_generator) diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index b3d19ae98b9..0a48874c49b 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.1 + 0.4.2 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index be971939c34..e08b3d97629 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.1 + 0.4.2 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index 06bb2a5a193..a0b268f3ef7 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -43,7 +43,15 @@ ament_target_dependencies(path_ops ${dependencies} ) -install(TARGETS conversions path_ops +add_library(tf_help SHARED + src/tf_help.cpp +) + +ament_target_dependencies(tf_help + ${dependencies} +) + +install(TARGETS conversions path_ops tf_help ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -60,7 +68,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(conversions path_ops) +ament_export_libraries(conversions path_ops tf_help) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp index dc49c99a0d6..3b4687bd108 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp @@ -37,6 +37,7 @@ #include #include +#include "tf2_ros/buffer.h" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_2d_msgs/msg/pose2_d_stamped.hpp" @@ -55,46 +56,12 @@ namespace nav_2d_utils * @return True if successful transform */ bool transformPose( - const std::shared_ptr tf, const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose, - rclcpp::Duration & transform_tolerance) -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf->transform(in_pose, out_pose, frame); - return true; - } catch (tf2::ExtrapolationException & ex) { - auto transform = tf->lookupTransform(frame, in_pose.header.frame_id, tf2::TimePointZero); - if ((rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > - transform_tolerance) - { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Transform data too old when converting from %s to %s", - in_pose.header.frame_id.c_str(), - frame.c_str()); - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Data time: %ds %uns, Transform time: %ds %uns", - in_pose.header.stamp.sec, in_pose.header.stamp.nanosec, - transform.header.stamp.sec, transform.header.stamp.nanosec); - return false; - } else { - tf2::doTransform(in_pose, out_pose, transform); - return true; - } - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Exception in transformPose: %s", ex.what()); - return false; - } - return false; -} + const std::shared_ptr tf, + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose, + rclcpp::Duration & transform_tolerance +); /** * @brief Transform a Pose2DStamped from one frame to another while catching exceptions @@ -107,19 +74,12 @@ bool transformPose( * @return True if successful transform */ bool transformPose( - const std::shared_ptr tf, const std::string frame, - const nav_2d_msgs::msg::Pose2DStamped & in_pose, nav_2d_msgs::msg::Pose2DStamped & out_pose, - rclcpp::Duration & transform_tolerance) -{ - geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose); - geometry_msgs::msg::PoseStamped out_3d_pose; - - bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance); - if (ret) { - out_pose = poseStampedToPose2D(out_3d_pose); - } - return ret; -} + const std::shared_ptr tf, + const std::string frame, + const nav_2d_msgs::msg::Pose2DStamped & in_pose, + nav_2d_msgs::msg::Pose2DStamped & out_pose, + rclcpp::Duration & transform_tolerance +); } // namespace nav_2d_utils diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 5f0a0a0910a..ab5945eb9d8 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.1 + 0.4.2 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp new file mode 100644 index 00000000000..ac4b7a6a4b0 --- /dev/null +++ b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include "nav_2d_utils/tf_help.hpp" + +namespace nav_2d_utils +{ + +bool transformPose( + const std::shared_ptr tf, + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose, + rclcpp::Duration & transform_tolerance +) +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf->transform(in_pose, out_pose, frame); + return true; + } catch (tf2::ExtrapolationException & ex) { + auto transform = tf->lookupTransform( + frame, + in_pose.header.frame_id, + tf2::TimePointZero + ); + if ( + (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > + transform_tolerance) + { + RCLCPP_ERROR( + rclcpp::get_logger("tf_help"), + "Transform data too old when converting from %s to %s", + in_pose.header.frame_id.c_str(), + frame.c_str() + ); + RCLCPP_ERROR( + rclcpp::get_logger("tf_help"), + "Data time: %ds %uns, Transform time: %ds %uns", + in_pose.header.stamp.sec, + in_pose.header.stamp.nanosec, + transform.header.stamp.sec, + transform.header.stamp.nanosec + ); + return false; + } else { + tf2::doTransform(in_pose, out_pose, transform); + return true; + } + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("tf_help"), + "Exception in transformPose: %s", + ex.what() + ); + return false; + } + return false; +} + +bool transformPose( + const std::shared_ptr tf, + const std::string frame, + const nav_2d_msgs::msg::Pose2DStamped & in_pose, + nav_2d_msgs::msg::Pose2DStamped & out_pose, + rclcpp::Duration & transform_tolerance +) +{ + geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose); + geometry_msgs::msg::PoseStamped out_3d_pose; + + bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance); + if (ret) { + out_pose = poseStampedToPose2D(out_3d_pose); + } + return ret; +} +} // namespace nav_2d_utils diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index a9b16bd4867..f68735154e2 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.1 + 0.4.2 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 8cb747d453a..618d318935c 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -88,7 +88,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) auto future_result = is_active_client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { return SystemStatus::TIMEOUT; } diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 2c096e3dba4..3aa4e2e2332 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -86,12 +86,6 @@ class MapSaver : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when Error is raised - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Map saving service callback diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index d3ace49e90a..700b1753ea2 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -77,12 +77,6 @@ class MapServer : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when Error is raised - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Load the map YAML, image from map file name and diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index dbd43a0410a..165a7234bcf 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.1 + 0.4.2 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_saver/main_server.cpp b/nav2_map_server/src/map_saver/main_server.cpp index c7d2c695eba..320e13cc950 100644 --- a/nav2_map_server/src/map_saver/main_server.cpp +++ b/nav2_map_server/src/map_saver/main_server.cpp @@ -25,15 +25,8 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto logger = rclcpp::get_logger("map_saver_server"); - - try { - auto service_node = std::make_shared(); - rclcpp::spin(service_node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; - } catch (std::exception & ex) { - RCLCPP_ERROR(logger, ex.what()); - RCLCPP_ERROR(logger, "Exiting"); - return -1; - } + auto service_node = std::make_shared(); + rclcpp::spin(service_node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; } diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 001e2ed28b6..7555ebd6326 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -94,13 +94,6 @@ MapSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -MapSaver::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn MapSaver::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { @@ -208,7 +201,6 @@ bool MapSaver::saveMapTopicToFile( return false; } - RCLCPP_ERROR(get_logger(), "This situation should never appear"); return false; } diff --git a/nav2_map_server/src/map_server/main.cpp b/nav2_map_server/src/map_server/main.cpp index 8e57cc09bf9..693b3b9a0b1 100644 --- a/nav2_map_server/src/map_server/main.cpp +++ b/nav2_map_server/src/map_server/main.cpp @@ -23,15 +23,8 @@ int main(int argc, char ** argv) { std::string node_name("map_server"); - try { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; - } catch (std::exception & ex) { - RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), ex.what()); - RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), "Exiting"); - return -1; - } + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); } diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 2c2e174f9c9..d633af10a39 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -154,13 +154,6 @@ MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -MapServer::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn MapServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { diff --git a/nav2_map_server/test/CMakeLists.txt b/nav2_map_server/test/CMakeLists.txt index 6d3a9f00734..c36d920be06 100644 --- a/nav2_map_server/test/CMakeLists.txt +++ b/nav2_map_server/test/CMakeLists.txt @@ -5,3 +5,4 @@ add_definitions( -DTEST_DIRECTORY=\"${CMAKE_CURRENT_SOURCE_DIR}\") add_subdirectory(unit) add_subdirectory(component) +add_subdirectory(map_saver_cli) diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp index 373f8783056..6fa14d9b98e 100644 --- a/nav2_map_server/test/component/test_map_server_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -58,6 +58,7 @@ class MapServerTestFixture : public ::testing::Test { lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE); lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP); + lifecycle_client_->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); } template diff --git a/nav2_map_server/test/map_saver_cli/CMakeLists.txt b/nav2_map_server/test/map_saver_cli/CMakeLists.txt new file mode 100644 index 00000000000..f7e3b52950d --- /dev/null +++ b/nav2_map_server/test/map_saver_cli/CMakeLists.txt @@ -0,0 +1,13 @@ +include_directories(${PROJECT_SOURCE_DIR}/test) + +# map_saver CLI +ament_add_gtest(test_map_saver_cli + test_map_saver_cli.cpp + ${PROJECT_SOURCE_DIR}/test/test_constants.cpp +) + +ament_target_dependencies(test_map_saver_cli rclcpp nav_msgs) +target_link_libraries(test_map_saver_cli + stdc++fs + ${dependencies} +) diff --git a/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp new file mode 100644 index 00000000000..5b0c3a0339e --- /dev/null +++ b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2020 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. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +TEST(MapSaverCLI, CLITest) +{ + std::string path = "/tmp/"; + std::string file = "test_map"; + std::string file_path = path + file; + + rclcpp::init(0, nullptr); + + auto node = std::make_shared("CLI_Test_Node"); + RCLCPP_INFO(node->get_logger(), "Testing Map Saver CLI"); + + auto publisher = node->create_publisher( + "/map", + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + auto msg = std::make_unique(); + msg->header.frame_id = "map"; + msg->header.stamp = node->now(); + msg->info.map_load_time = node->now(); + msg->info.resolution = 0.05; + msg->info.width = 3; + msg->info.height = 3; + msg->info.origin.position.x = 0.0; + msg->info.origin.position.y = 0.0; + msg->info.origin.orientation.w = 1.0; + msg->data.resize(9); + msg->data[0] = 0; + msg->data[2] = 100; + msg->data[1] = 101; + msg->data[3] = 50; + + RCLCPP_INFO(node->get_logger(), "Publishing occupancy grid..."); + + publisher->publish(std::move(msg)); + + rclcpp::Rate(1).sleep(); + + // succeed on real map + RCLCPP_INFO(node->get_logger(), "Calling saver..."); + + EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml")); + + std::string command = + std::string( + "ros2 run nav2_map_server map_saver_cli -f ") + file_path; + auto return_code = system(command.c_str()); + EXPECT_EQ(return_code, 0); + + rclcpp::Rate(0.25).sleep(); + + RCLCPP_INFO(node->get_logger(), "Checking on file..."); + + EXPECT_TRUE(std::experimental::filesystem::exists(file_path + ".pgm")); + EXPECT_EQ(std::experimental::filesystem::file_size(file_path + ".pgm"), 20ul); + + if (std::experimental::filesystem::exists(file_path + ".yaml")) { + std::experimental::filesystem::remove(file_path + ".yaml"); + } + if (std::experimental::filesystem::exists(file_path + ".pgm")) { + std::experimental::filesystem::remove(file_path + ".pgm"); + } + + // fail on bogus map + RCLCPP_INFO(node->get_logger(), "Calling saver..."); + + EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml")); + + command = + std::string( + "ros2 run nav2_map_server map_saver_cli " + "-t map_failure --occ 100 --free 2 --mode trinary --fmt png -f ") + file_path + + std::string("--ros-args __node:=map_saver_test_node"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 65280); + + rclcpp::Rate(0.25).sleep(); + + RCLCPP_INFO(node->get_logger(), "Checking on file..."); + + EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml")); + + RCLCPP_INFO(node->get_logger(), "Testing help..."); + command = + std::string( + "ros2 run nav2_map_server map_saver_cli -h"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 0); + + rclcpp::Rate(0.5).sleep(); + + RCLCPP_INFO(node->get_logger(), "Testing invalid mode..."); + command = + std::string( + "ros2 run nav2_map_server map_saver_cli --mode fake_mode"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 0); + + rclcpp::Rate(0.5).sleep(); + + RCLCPP_INFO(node->get_logger(), "Testing missing argument..."); + command = + std::string( + "ros2 run nav2_map_server map_saver_cli --mode"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 65280); + + rclcpp::Rate(0.5).sleep(); + + RCLCPP_INFO(node->get_logger(), "Testing wrong argument..."); + command = + std::string( + "ros2 run nav2_map_server map_saver_cli --free 0 0"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 65280); + + rclcpp::Rate(0.5).sleep(); + + command = + std::string( + "ros2 run nav2_map_server map_saver_cli --ros-args -r __node:=map_saver_test_node"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 0); +} diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index a2d40860249..dbdc5167c16 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.1 + 0.4.2 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index a3e79cce733..c6a4fa9699d 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -258,13 +258,13 @@ class NavFn /** display callback */ /**< is the number of cycles between updates */ - void display(void fn(NavFn * nav), int n = 100); - int displayInt; /**< save second argument of display() above */ - void (* displayFn)(NavFn * nav); /**< display function itself */ + // void display(void fn(NavFn * nav), int n = 100); + // int displayInt; /**< save second argument of display() above */ + // void (* displayFn)(NavFn * nav); /**< display function itself */ /** save costmap */ /**< write out costmap and start/goal states as fname.pgm and fname.txt */ - void savemap(const char * fname); + // void savemap(const char * fname); }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index e6637ce4a36..e9d34279a56 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -88,8 +88,8 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Check for a valid potential value at a given point in the world // - must call computePotential first // - currently unused - bool validPointPotential(const geometry_msgs::msg::Point & world_point); - bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); + // bool validPointPotential(const geometry_msgs::msg::Point & world_point); + // bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); // Compute the squared distance between two points inline double squared_distance( diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 4f6a56ce193..e3fa60a5942 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.1 + 0.4.2 TODO Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 8025ff150dd..7cc1002892a 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -55,6 +55,8 @@ namespace nav2_navfn_planner // if the size of the environment does not change // +// Example usage: +/* int create_nav_plan_astar( COSTTYPE * costmap, int nx, int ny, @@ -100,7 +102,7 @@ create_nav_plan_astar( return len; } - +*/ // // create nav fn buffers @@ -129,8 +131,8 @@ NavFn::NavFn(int xs, int ys) start[0] = start[1] = 0; // display function - displayFn = NULL; - displayInt = 0; + // displayFn = NULL; + // displayInt = 0; // path buffers npathbuf = npath = 0; @@ -605,9 +607,9 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) updateCell(*pb++); } - if (displayInt > 0 && (cycle % displayInt) == 0) { - displayFn(this); - } + // if (displayInt > 0 && (cycle % displayInt) == 0) { + // displayFn(this); + // } // swap priority blocks curP <=> nextP curPe = nextPe; @@ -691,9 +693,9 @@ NavFn::propNavFnAstar(int cycles) updateCellAstar(*pb++); } - if (displayInt > 0 && (cycle % displayInt) == 0) { - displayFn(this); - } + // if (displayInt > 0 && (cycle % displayInt) == 0) { + // displayFn(this); + // } // swap priority blocks curP <=> nextP curPe = nextPe; @@ -983,12 +985,12 @@ NavFn::gradCell(int n) // is the number of cycles to wait before displaying, // use 0 to turn it off -void -NavFn::display(void fn(NavFn * nav), int n) -{ - displayFn = fn; - displayInt = n; -} +// void +// NavFn::display(void fn(NavFn * nav), int n) +// { +// displayFn = fn; +// displayInt = n; +// } // @@ -996,35 +998,35 @@ NavFn::display(void fn(NavFn * nav), int n) // saves costmap and start/goal // -void -NavFn::savemap(const char * fname) -{ - char fn[4096]; - - RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[NavFn] Saving costmap and start/goal points"); - // write start and goal points - snprintf(fn, sizeof(fn), "%s.txt", fname); - FILE * fp = fopen(fn, "w"); - if (!fp) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn); - return; - } - fprintf(fp, "Goal: %d %d\nStart: %d %d\n", goal[0], goal[1], start[0], start[1]); - fclose(fp); - - // write cost array - if (!costarr) { - return; - } - snprintf(fn, sizeof(fn), "%s.pgm", fname); - fp = fopen(fn, "wb"); - if (!fp) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn); - return; - } - fprintf(fp, "P5\n%d\n%d\n%d\n", nx, ny, 0xff); - fwrite(costarr, 1, nx * ny, fp); - fclose(fp); -} +// void +// NavFn::savemap(const char * fname) +// { +// char fn[4096]; + +// RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[NavFn] Saving costmap and start/goal points"); +// // write start and goal points +// snprintf(fn, sizeof(fn), "%s.txt", fname); +// FILE * fp = fopen(fn, "w"); +// if (!fp) { +// RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn); +// return; +// } +// fprintf(fp, "Goal: %d %d\nStart: %d %d\n", goal[0], goal[1], start[0], start[1]); +// fclose(fp); + +// // write cost array +// if (!costarr) { +// return; +// } +// snprintf(fn, sizeof(fn), "%s.pgm", fname); +// fp = fopen(fn, "wb"); +// if (!fp) { +// RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn); +// return; +// } +// fprintf(fp, "P5\n%d\n%d\n%d\n", nx, ny, 0xff); +// fwrite(costarr, 1, nx * ny, fp); +// fclose(fp); +// } } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 29c97db6342..a873a3e5d1c 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -152,6 +152,9 @@ NavfnPlanner::makePlan( // clear the plan, just in case plan.poses.clear(); + plan.header.stamp = node_->now(); + plan.header.frame_id = global_frame_; + // TODO(orduno): add checks for start and goal reference frame -- should be in global frame double wx = start.position.x; @@ -174,6 +177,8 @@ NavfnPlanner::makePlan( // clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(mx, my); + std::unique_lock lock(*(costmap_->getMutex())); + // make sure to resize the underlying array that Navfn uses planner_->setNavArr( costmap_->getSizeInCellsX(), @@ -181,6 +186,8 @@ NavfnPlanner::makePlan( planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_); + lock.unlock(); + int map_start[2]; map_start[0] = mx; map_start[1] = my; @@ -312,20 +319,17 @@ NavfnPlanner::getPlanFromPotential( int path_len = planner_->calcPath(costmap_->getSizeInCellsX() * 4); if (path_len == 0) { - RCLCPP_DEBUG(node_->get_logger(), "No path found\n"); return false; } - RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps\n", path_len); + auto cost = planner_->getLastPathCost(); + RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps, %f cost\n", path_len, cost); // extract the plan float * x = planner_->getPathX(); float * y = planner_->getPathY(); int len = planner_->getPathLen(); - plan.header.stamp = node_->now(); - plan.header.frame_id = global_frame_; - for (int i = len - 1; i >= 0; --i) { // convert the plan to world coordinates double world_x, world_y; @@ -357,51 +361,47 @@ NavfnPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point) return planner_->potarr[index]; } -bool -NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point) -{ - return validPointPotential(world_point, tolerance_); -} - -bool -NavfnPlanner::validPointPotential( - const geometry_msgs::msg::Point & world_point, double tolerance) -{ - const double resolution = costmap_->getResolution(); - - geometry_msgs::msg::Point p = world_point; - double potential = getPointPotential(p); - if (potential < POT_HIGH) { - // world_point is reachable by itself - return true; - } else { - // world_point, is not reachable. Trying to find any - // reachable point within its tolerance region - p.y = world_point.y - tolerance; - while (p.y <= world_point.y + tolerance) { - p.x = world_point.x - tolerance; - while (p.x <= world_point.x + tolerance) { - potential = getPointPotential(p); - if (potential < POT_HIGH) { - return true; - } - p.x += resolution; - } - p.y += resolution; - } - } - - return false; -} +// bool +// NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point) +// { +// return validPointPotential(world_point, tolerance_); +// } + +// bool +// NavfnPlanner::validPointPotential( +// const geometry_msgs::msg::Point & world_point, double tolerance) +// { +// const double resolution = costmap_->getResolution(); + +// geometry_msgs::msg::Point p = world_point; +// double potential = getPointPotential(p); +// if (potential < POT_HIGH) { +// // world_point is reachable by itself +// return true; +// } else { +// // world_point, is not reachable. Trying to find any +// // reachable point within its tolerance region +// p.y = world_point.y - tolerance; +// while (p.y <= world_point.y + tolerance) { +// p.x = world_point.x - tolerance; +// while (p.x <= world_point.x + tolerance) { +// potential = getPointPotential(p); +// if (potential < POT_HIGH) { +// return true; +// } +// p.x += resolution; +// } +// p.y += resolution; +// } +// } + +// return false; +// } bool NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) { if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY()) { - RCLCPP_ERROR( - node_->get_logger(), "worldToMap failed: wx,wy: %f,%f, " - "size_x,size_y: %d,%d", wx, wy, - costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); return false; } diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 3e67e6d089e..175b69165c6 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -58,6 +58,17 @@ class PlannerServer : public nav2_util::LifecycleNode using PlannerMap = std::unordered_map; + /** + * @brief Method to get plan from the desired plugin + * @param start starting pose + * @param goal goal request + * @return Path + */ + nav_msgs::msg::Path getPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id); + protected: /** * @brief Configure member variables and initializes planner @@ -89,14 +100,9 @@ class PlannerServer : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; - using ActionServer = nav2_util::SimpleActionServer; + using ActionT = nav2_msgs::action::ComputePathToPose; + using ActionServer = nav2_util::SimpleActionServer; // Our action server implements the ComputePathToPose action std::unique_ptr action_server_; @@ -135,9 +141,6 @@ class PlannerServer : public nav2_util::LifecycleNode // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; - - // Whether we've published the single planner warning yet - bool single_planner_warning_given_{false}; }; } // namespace nav2_planner diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 598fe05acbb..f0beb74f099 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.1 + 0.4.2 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index edf273e87fe..ee3a07e4739 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -59,10 +59,8 @@ PlannerServer::PlannerServer() PlannerServer::~PlannerServer() { RCLCPP_INFO(get_logger(), "Destroying"); - PlannerMap::iterator it; - for (it = planners_.begin(); it != planners_.end(); ++it) { - it->second.reset(); - } + planners_.clear(); + costmap_thread_.reset(); } nav2_util::CallbackReturn @@ -104,7 +102,6 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); - exit(-1); } } @@ -112,17 +109,20 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) planner_ids_concat_ += planner_ids_[i] + std::string(" "); } + RCLCPP_INFO( + get_logger(), + "Planner Server has %s planners available.", planner_ids_concat_.c_str()); + double expected_planner_frequency; get_parameter("expected_planner_frequency", expected_planner_frequency); if (expected_planner_frequency > 0) { max_planner_duration_ = 1 / expected_planner_frequency; } else { - max_planner_duration_ = 0.0; - RCLCPP_WARN( get_logger(), - "The expected planner frequency parameter is %.4f Hz. The value has to be greater" - " than 0.0 to turn on displaying warning messages", expected_planner_frequency); + "The expected planner frequency parameter is %.4f Hz. The value should to be greater" + " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); + max_planner_duration_ = 0.0; } // Initialize pubs & subs @@ -186,17 +186,11 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) it->second->cleanup(); } planners_.clear(); + costmap_ = nullptr; return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -PlannerServer::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) { @@ -214,13 +208,8 @@ PlannerServer::computePlan() auto result = std::make_shared(); try { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + if (action_server_ == nullptr || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; } @@ -232,39 +221,15 @@ PlannerServer::computePlan() geometry_msgs::msg::PoseStamped start; if (!costmap_ros_->getRobotPose(start)) { - RCLCPP_ERROR(this->get_logger(), "Could not get robot pose"); + action_server_->terminate_current(); return; } if (action_server_->is_preempt_requested()) { - RCLCPP_INFO(get_logger(), "Preempting the goal pose."); goal = action_server_->accept_pending_goal(); } - RCLCPP_DEBUG( - get_logger(), "Attempting to a find path from (%.2f, %.2f) to " - "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y, - goal->pose.pose.position.x, goal->pose.pose.position.y); - - if (planners_.find(goal->planner_id) != planners_.end()) { - result->path = planners_[goal->planner_id]->createPlan(start, goal->pose); - } else { - if (planners_.size() == 1 && goal->planner_id.empty()) { - if (!single_planner_warning_given_) { - single_planner_warning_given_ = true; - RCLCPP_WARN( - get_logger(), "No planners specified in action call. " - "Server will use only plugin %s in server." - " This warning will appear once.", planner_ids_concat_.c_str()); - } - result->path = planners_[planners_.begin()->first]->createPlan(start, goal->pose); - } else { - RCLCPP_ERROR( - get_logger(), "planner %s is not a valid planner. " - "Planner names are: %s", goal->planner_id.c_str(), - planner_ids_concat_.c_str()); - } - } + result->path = getPlan(start, goal->pose, goal->planner_id); if (result->path.poses.size() == 0) { RCLCPP_WARN( @@ -282,7 +247,6 @@ PlannerServer::computePlan() goal->pose.pose.position.y); // Publish the plan for visualization purposes - RCLCPP_DEBUG(get_logger(), "Publishing the valid path"); publishPlan(result->path); auto cycle_duration = steady_clock_.now() - start_time; @@ -296,8 +260,6 @@ PlannerServer::computePlan() } action_server_->succeeded_current(result); - - return; } catch (std::exception & ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", @@ -306,24 +268,50 @@ PlannerServer::computePlan() // TODO(orduno): provide information about fail error to parent task, // for example: couldn't get costmap update action_server_->terminate_current(); - return; - } catch (...) { - RCLCPP_WARN( - get_logger(), "Plan calculation failed, " - "An unexpected error has occurred. The planner server" - " may not be able to continue operating correctly."); - // TODO(orduno): provide information about fail error to parent task, - // for example: couldn't get costmap update - action_server_->terminate_current(); - return; } } +nav_msgs::msg::Path +PlannerServer::getPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id) +{ + RCLCPP_DEBUG( + get_logger(), "Attempting to a find path from (%.2f, %.2f) to " + "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y, + goal.pose.position.x, goal.pose.position.y); + + if (planners_.find(planner_id) != planners_.end()) { + return planners_[planner_id]->createPlan(start, goal); + } else { + if (planners_.size() == 1 && planner_id.empty()) { + RCLCPP_WARN_ONCE( + get_logger(), "No planners specified in action call. " + "Server will use only plugin %s in server." + " This warning will appear once.", planner_ids_concat_.c_str()); + return planners_[planners_.begin()->first]->createPlan(start, goal); + } else { + RCLCPP_ERROR( + get_logger(), "planner %s is not a valid planner. " + "Planner names are: %s", planner_id.c_str(), + planner_ids_concat_.c_str()); + } + } + + return nav_msgs::msg::Path(); +} + void PlannerServer::publishPlan(const nav_msgs::msg::Path & path) { auto msg = std::make_unique(path); - plan_publisher_->publish(std::move(msg)); + if ( + plan_publisher_->is_activated() && + this->count_subscribers(plan_publisher_->get_topic_name()) > 0) + { + plan_publisher_->publish(std::move(msg)); + } } } // namespace nav2_planner diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 1c02fc1f598..bb9efa571c1 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -46,7 +46,6 @@ class RecoveryServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; std::shared_ptr tf_; std::shared_ptr transform_listener_; diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 327bf898a47..67433aed718 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.1 + 0.4.2 TODO Carlos Orduno Steve Macenski diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 3f258402036..585d8a77410 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -123,7 +123,8 @@ bool BackUp::isCollisionFree( while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_); - pose2d.x += sim_position_change; + pose2d.x += sim_position_change * cos(pose2d.theta); + pose2d.y += sim_position_change * sin(pose2d.theta); cycle_count++; if (diff_dist - abs(sim_position_change) <= 0.) { diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index 49b7428d1f3..d657a9d0563 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -162,13 +162,6 @@ RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -RecoveryServer::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn RecoveryServer::on_shutdown(const rclcpp_lifecycle::State &) { diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp index a838803f87b..6f5c83912ed 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_recoveries/test/test_recoveries.cpp @@ -157,7 +157,7 @@ class RecoveryTest : public ::testing::Test auto future_goal = client_->async_send_goal(goal); if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { std::cout << "failed sending goal" << std::endl; // failed sending the goal diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 431f12a0370..e424873ab42 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.1 + 0.4.2 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 9d1b3156834..5d2b5147fc8 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -410,7 +410,7 @@ Nav2Panel::onCancelButtonPressed() waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); return; @@ -419,7 +419,7 @@ Nav2Panel::onCancelButtonPressed() auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); return; @@ -523,7 +523,7 @@ Nav2Panel::startWaypointFollowing(std::vector p auto future_goal_handle = waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options); if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); return; @@ -563,7 +563,7 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) auto future_goal_handle = navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); return; diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 9eae0646f2f..8d67f4a9785 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -7,7 +7,6 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_map_server REQUIRED) -find_package(nav2_bringup REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) @@ -29,7 +28,6 @@ set(dependencies rclcpp nav2_util nav2_map_server - nav2_bringup nav2_msgs nav_msgs visualization_msgs @@ -55,6 +53,7 @@ if(BUILD_TESTING) add_subdirectory(src/planning) add_subdirectory(src/localization) add_subdirectory(src/system) + add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) add_subdirectory(src/recoveries/spin) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index eac98acd40d..7c80e085d1a 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.1 + 0.4.2 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index 04745b348f5..9c67eadddf7 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -45,3 +45,12 @@ ament_add_test(test_planner_random TEST_EXECUTABLE=$ TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm ) + +ament_add_gtest(test_planner_plugin_failures + test_planner_plugins.cpp +) + +ament_target_dependencies(test_planner_plugin_failures rclcpp geometry_msgs nav2_msgs ${dependencies}) +target_link_libraries(test_planner_plugin_failures + stdc++fs +) diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 534b8237301..1367735aa26 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -67,6 +67,12 @@ void PlannerTester::activate() // The navfn wrapper auto state = rclcpp_lifecycle::State(); planner_tester_ = std::make_shared(); + planner_tester_->declare_parameter( + "GridBased.use_astar", rclcpp::ParameterValue(true)); + planner_tester_->set_parameter( + rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true))); + planner_tester_->set_parameter( + rclcpp::Parameter(std::string("expected_planner_frequency"), rclcpp::ParameterValue(-1.0))); planner_tester_->onConfigure(state); publishRobotTransform(); map_pub_ = this->create_publisher("map", 1); diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp new file mode 100644 index 00000000000..c99c1c36bbd --- /dev/null +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -0,0 +1,71 @@ +// Copyright (c) 2020 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. Reserved. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "planner_tester.hpp" +#include "nav2_util/lifecycle_utils.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::PlannerTester; +using nav2_util::TestCostmap; + +using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; +using ComputePathToPoseResult = nav_msgs::msg::Path; + +void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/) +{ +} + +TEST(testPluginMap, Failures) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("expected_planner_frequency", 100000.0)); + obj->onConfigure(state); + obj->create_subscription( + "plan", rclcpp::SystemDefaultsQoS(), callback); + + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + std::string plugin_fake = "fake"; + std::string plugin_none = ""; + auto path = obj->getPlan(start, goal, plugin_none); + EXPECT_EQ(path.header.frame_id, std::string("map")); + + path = obj->getPlan(start, goal, plugin_fake); + EXPECT_EQ(path.poses.size(), 0ul); + + obj->onCleanup(state); +} + +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_system_tests/src/recoveries/backup/backup_recovery_tester.cpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp index 6d87fb1bd28..290deff0317 100644 --- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp @@ -131,7 +131,7 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -148,7 +148,7 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 521c6759559..ee9f6b63bc2 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -129,7 +129,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -146,7 +146,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp index e14ca5c7dcf..39ee9871d29 100644 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp @@ -62,11 +62,16 @@ WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr; TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) { float wait_time = std::get<0>(GetParam()); + float cancel = std::get<1>(GetParam()); bool success = false; int num_tries = 3; for (int i = 0; i != num_tries; i++) { - success = success || wait_recovery_tester->recoveryTest(wait_time); + if (cancel == 1.0) { + success = success || wait_recovery_tester->recoveryTestCancel(wait_time); + } else { + success = success || wait_recovery_tester->recoveryTest(wait_time); + } if (success) { break; } @@ -81,7 +86,8 @@ INSTANTIATE_TEST_CASE_P( ::testing::Values( std::make_tuple(1.0, 0.0), std::make_tuple(2.0, 0.0), - std::make_tuple(5.0, 0.0)), + std::make_tuple(5.0, 0.0), + std::make_tuple(10.0, 1.0)), testNameGenerator); int main(int argc, char ** argv) diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp index b6bdd62148a..7112289b5af 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp @@ -123,7 +123,7 @@ bool WaitRecoveryTester::recoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -140,7 +140,7 @@ bool WaitRecoveryTester::recoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; @@ -172,6 +172,83 @@ bool WaitRecoveryTester::recoveryTest( return true; } +bool WaitRecoveryTester::recoveryTestCancel( + const float wait_time) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let recovery server be ready for serving in multiple runs + std::this_thread::sleep_for(5s); + + auto start_time = node_->now(); + auto goal_msg = Wait::Goal(); + goal_msg.time = rclcpp::Duration(wait_time, 0.0); + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_cancel_all_goals(); + + RCLCPP_INFO(node_->get_logger(), "Waiting for cancellation"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get cancel result call failed :("); + return false; + } + + auto status = goal_handle_future.get()->get_status(); + + switch (status) { + case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR( + node_->get_logger(), + "Goal succeeded"); + return false; + case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO( + node_->get_logger(), + "Goal was canceled"); + return true; + case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO( + node_->get_logger(), + "Goal is cancelling"); + return true; + case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR( + node_->get_logger(), + "Goal is executing"); + return false; + case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal is processing"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + return false; +} + void WaitRecoveryTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp index 5daef5025b6..4dfb90c6626 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp @@ -52,6 +52,8 @@ class WaitRecoveryTester bool recoveryTest( float time); + bool recoveryTestCancel(float time); + void activate(); void deactivate(); diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 09017bfc6ca..a7ae2aa43fa 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -1,3 +1,6 @@ +# NOTE: The ASTAR=True does not work currently due to remapping not functioning +# All set to false, A* testing of NavFn happens in the planning test portion + ament_add_test(test_bt_navigator GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" @@ -9,7 +12,7 @@ ament_add_test(test_bt_navigator TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=True + ASTAR=False ) ament_add_test(test_bt_navigator_with_dijkstra @@ -37,7 +40,7 @@ ament_add_test(test_dynamic_obstacle TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=True + ASTAR=False ) # ament_add_test(test_multi_robot diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index dde6618cdc7..92ea252eba0 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -41,7 +41,8 @@ def generate_launch_description(): params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') # Replace the `use_astar` setting on the params file - param_substitutions = {'GridBased.use_astar': os.getenv('ASTAR')} + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR')} configured_params = RewrittenYaml( source_file=params_file, root_key='', diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py index a66e2f7b89d..83e9df9dc9f 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/tester_node.py @@ -228,12 +228,8 @@ def run_all_tests(robot_tester): robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() - # TODO(orduno) Test sending the navigation request through the topic interface. - # Need to update tester to receive multiple goal poses. - # Need to fix bug with shutting down while bt_navigator - # is still running. - # if (result): - # result = test_RobotMovesToGoal(robot_tester) + if (result): + result = test_RobotMovesToGoal(robot_tester) # Add more tests here if desired diff --git a/nav2_system_tests/src/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt new file mode 100644 index 00000000000..885234adc21 --- /dev/null +++ b/nav2_system_tests/src/system_failure/CMakeLists.txt @@ -0,0 +1,12 @@ +ament_add_test(test_failure_navigator + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_failure_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/system_failure/README.md b/nav2_system_tests/src/system_failure/README.md new file mode 100644 index 00000000000..4afd4eaac34 --- /dev/null +++ b/nav2_system_tests/src/system_failure/README.md @@ -0,0 +1,3 @@ +# Navigation2 System Tests - Failure + +High level system failures tests diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py new file mode 100755 index 00000000000..c41e78f3048 --- /dev/null +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 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. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +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(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': 'False'} + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-r', '-2.0', '-0.5', '100.0', '100.0'], + name='tester_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py new file mode 100755 index 00000000000..eac29bc6540 --- /dev/null +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -0,0 +1,307 @@ +#! /usr/bin/env python3 +# Copyright 2018 Intel Corporation. +# Copyright 2020 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. + +import argparse +import math +import sys +import time +from typing import Optional + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import NavigateToPose +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy + +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class NavTester(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) + self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) + + pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_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.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + + def info_msg(self, msg: str): + self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + + def warn_msg(self, msg: str): + self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + + def error_msg(self, msg: str): + self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + + def setInitialPose(self): + 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): + msg = PoseStamped() + msg.header.frame_id = 'map' + msg.pose = pose + return msg + + def publishGoalPose(self, goal_pose: Optional[Pose] = None): + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose)) + + def runNavigateAction(self, goal_pose: Optional[Pose] = None): + # Sends a `NavToPose` action request and waits for completion + self.info_msg("Waiting for 'NavigateToPose' action server") + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'NavigateToPose' action server not available, waiting...") + + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + goal_msg = NavigateToPose.Goal() + goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) + + self.info_msg('Sending goal request...') + send_goal_future = self.action_client.send_goal_async(goal_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if 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 'NavigateToPose' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + if status != GoalStatus.STATUS_ABORTED: + self.info_msg('Goal failed with status code: {0}'.format(status)) + return False + + self.info_msg('Goal failed, as expected!') + return True + + def poseCallback(self, msg): + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + + def reachesGoal(self, timeout, distance): + goalReached = False + start_time = time.time() + + while not goalReached: + rclpy.spin_once(self, timeout_sec=1) + if self.distanceFromGoal() < distance: + goalReached = True + self.info_msg('*** GOAL REACHED ***') + return True + elif timeout is not None: + if (time.time() - start_time) > timeout: + self.error_msg('Robot timed out reaching its goal!') + return False + + def distanceFromGoal(self): + 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('Distance from goal is: ' + str(distance)) + return distance + + def wait_for_node_active(self, node_name: str): + # Waits for the node within the tester namespace to become active + self.info_msg('Waiting for ' + node_name + ' to become active') + node_service = 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(node_service + ' service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while (state != 'active'): + self.info_msg('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 + self.info_msg('Result of get_state: %s' % state) + else: + self.error_msg('Exception while calling service: %r' % future.exception()) + time.sleep(5) + + def shutdown(self): + self.info_msg('Shutting down') + self.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(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: + self.error_msg('Service call failed %r' % (e,)) + 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(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: + self.error_msg('Service call failed %r' % (e,)) + + def wait_for_initial_pose(self): + 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): + # set transforms to use_sim_time + result = True + if (result): + robot_tester.wait_for_node_active('amcl') + robot_tester.wait_for_initial_pose() + robot_tester.wait_for_node_active('bt_navigator') + result = robot_tester.runNavigateAction() + + # Add more tests here if desired + + if (result): + robot_tester.info_msg('Test PASSED') + else: + robot_tester.error_msg('Test FAILED') + + return result + + +def fwd_pose(x=0.0, y=0.0, z=0.01): + 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 get_testers(args): + testers = [] + + if args.robot: + # Requested tester for one robot + init_x, init_y, final_x, final_y = args.robot[0] + tester = NavTester( + 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 + '.') + testers.append(tester) + return testers + + return testers + + +def main(argv=sys.argv[1:]): + # The robot(s) positions from the input arguments + parser = argparse.ArgumentParser(description='System-level navigation 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.') + group.add_argument('-rs', '--robots', action='append', nargs=5, + metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), + help="The robot's namespace and starting and final positions. " + + 'Repeating the argument for multiple robots is supported.') + + args, unknown = parser.parse_known_args() + + rclpy.init() + + # Create testers for each robot + testers = get_testers(args) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + for tester in testers: + passed = run_all_tests(tester) + if not passed: + break + + for tester in testers: + # stop and shutdown the nav stack to exit cleanly + tester.shutdown() + + testers[0].info_msg('Done Shutting Down.') + + if not passed: + testers[0].info_msg('Exiting failed') + exit(1) + else: + testers[0].info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 31bd9effbf2..7f5db902fba 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -37,6 +37,7 @@ def __init__(self): self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', 10) self.initial_pose_received = False + self.goal_handle = None pose_qos = QoSProfile( durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, @@ -71,7 +72,7 @@ def setWaypoints(self, waypoints): msg.pose.orientation.w = 1.0 self.waypoints.append(msg) - def run(self): + def run(self, block): if not self.waypoints: rclpy.error_msg('Did not set valid waypoints before running test!') return False @@ -86,16 +87,19 @@ def run(self): send_goal_future = self.action_client.send_goal_async(action_request) try: rclpy.spin_until_future_complete(self, send_goal_future) - goal_handle = send_goal_future.result() + self.goal_handle = send_goal_future.result() except Exception as e: self.error_msg('Service call failed %r' % (e,)) - if not goal_handle.accepted: + if not self.goal_handle.accepted: self.error_msg('Goal rejected') return False self.info_msg('Goal accepted') - get_result_future = goal_handle.get_result_async() + if not block: + return True + + get_result_future = self.goal_handle.get_result_async() self.info_msg("Waiting for 'FollowWaypoints' action to complete") try: @@ -148,14 +152,18 @@ def shutdown(self): except Exception as e: self.error_msg('Service call failed %r' % (e,)) + def cancel_goal(self): + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + def info_msg(self, msg: str): - self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + self.get_logger().info(msg) def warn_msg(self, msg: str): - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warn(msg) def error_msg(self, msg: str): - self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + self.get_logger().error(msg) def main(argv=sys.argv[1:]): @@ -179,7 +187,27 @@ def main(argv=sys.argv[1:]): test.info_msg('Waiting for amcl_pose to be received') rclpy.spin_once(test, timeout_sec=1.0) # wait for poseCallback - result = test.run() + result = test.run(True) + assert result + + # preempt with new point + test.setWaypoints([starting_pose]) + result = test.run(False) + time.sleep(2) + test.setWaypoints([wps[1]]) + result = test.run(False) + + # cancel + time.sleep(2) + test.cancel_goal() + + # a failure case + time.sleep(2) + test.setWaypoints([[100.0, 100.0]]) + result = test.run(True) + assert not result + result = not result + test.shutdown() test.info_msg('Done Shutting Down.') diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index e97dc6a86a2..de543515a73 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -121,6 +121,14 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode rclcpp_lifecycle::LifecycleNode::shared_from_this()); } + nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_FATAL( + get_logger(), + "Lifecycle node %s does not have error state implemented", get_name()); + return nav2_util::CallbackReturn::SUCCESS; + } + protected: void print_lifecycle_node_notification(); diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 26f0eedf03d..e1eb6983817 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -70,7 +70,7 @@ class ServiceClient auto future_result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error(service_name_ + " service client: async_send_request failed"); } @@ -98,7 +98,7 @@ class ServiceClient auto future_result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { return false; } diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 57cbbd9f7e9..4b797e694af 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.1 + 0.4.2 TODO Michael Jeronimo Mohammad Haghighipanah @@ -26,6 +26,7 @@ rclcpp_lifecycle launch launch_testing_ament_cmake + action_msgs libboost-program-options @@ -35,6 +36,7 @@ launch launch_testing_ament_cmake std_srvs + action_msgs ament_cmake diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 8d6ee2c0fd4..4d3b5511dc8 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,9 +41,37 @@ ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) +# This test is disabled due to failing services +# https://github.com/ros-planning/navigation2/issues/1836 + +add_launch_test( + "test_dump_params/test_dump_params_default.test.py" + TARGET "test_dump_params_default" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + +add_launch_test( + "test_dump_params/test_dump_params_yaml.test.py" + TARGET "test_dump_params_yaml" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + +add_launch_test( + "test_dump_params/test_dump_params_md.test.py" + TARGET "test_dump_params_md" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + add_launch_test( - "test_dump_params/test_dump_params.launch.py" - TIMEOUT 30 + "test_dump_params/test_dump_params_multiple.test.py" + TARGET "test_dump_params_multiple" + TIMEOUT 10 ENV TEST_EXECUTABLE=$ ) diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 11d5c54f118..84cef8576c1 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -245,7 +245,7 @@ TEST_F(ActionTest, test_simple_action) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -253,7 +253,7 @@ TEST_F(ActionTest, test_simple_action) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -293,7 +293,7 @@ TEST_F(ActionTest, test_simple_action_with_feedback) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -302,7 +302,7 @@ TEST_F(ActionTest, test_simple_action_with_feedback) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_result), rclcpp::executor::FutureReturnCode::SUCCESS); + future_result), rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -334,7 +334,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Deactivate while running node_->deactivate_server(); @@ -345,7 +345,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The action should be reported as aborted. EXPECT_EQ(future_result.get().code, rclcpp_action::ResultCode::ABORTED); @@ -361,7 +361,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); goal_handle = future_goal_handle.get(); @@ -370,7 +370,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) std::cout << "Getting result, spinning til complete..." << std::endl; EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // Now the action should have been successfully executed. EXPECT_EQ(future_result.get().code, rclcpp_action::ResultCode::SUCCEEDED); @@ -391,7 +391,7 @@ TEST_F(ActionTest, test_simple_action_preemption) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Preempt the goal auto preemption_goal = Fibonacci::Goal(); @@ -403,7 +403,7 @@ TEST_F(ActionTest, test_simple_action_preemption) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -412,7 +412,7 @@ TEST_F(ActionTest, test_simple_action_preemption) std::cout << "Getting result, spinning til complete..." << std::endl; EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -442,7 +442,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); node_->omit_server_preemptions(); @@ -450,7 +450,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Get the results auto goal_handle = future_goal_handle.get(); @@ -459,7 +459,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -480,7 +480,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) future_result = node_->action_client_->async_get_result(goal_handle); ASSERT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result result = future_result.get(); @@ -507,13 +507,38 @@ TEST_F(ActionTest, test_handle_goal_deactivated) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); node_->activate_server(); SUCCEED(); } +TEST_F(ActionTest, test_handle_cancel) +{ + auto goal = Fibonacci::Goal(); + goal.order = 14000000; + + // Send the goal + auto future_goal_handle = node_->action_client_->async_send_goal(goal); + EXPECT_EQ( + rclcpp::spin_until_future_complete( + node_, + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); + + // Cancel the goal + auto cancel_response = node_->action_client_->async_cancel_goal(future_goal_handle.get()); + EXPECT_EQ( + rclcpp::spin_until_future_complete( + node_, + cancel_response), rclcpp::FutureReturnCode::SUCCESS); + + // Check cancelled + EXPECT_EQ(future_goal_handle.get()->get_status(), rclcpp_action::GoalStatus::STATUS_CANCELING); + + SUCCEED(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/nav2_util/test/test_dump_params/test_dump_params.launch.py b/nav2_util/test/test_dump_params/test_dump_params_default.test.py similarity index 68% rename from nav2_util/test/test_dump_params/test_dump_params.launch.py rename to nav2_util/test/test_dump_params/test_dump_params_default.test.py index 55d28b5fac4..e0849b398fb 100644 --- a/nav2_util/test/test_dump_params/test_dump_params.launch.py +++ b/nav2_util/test/test_dump_params/test_dump_params_default.test.py @@ -37,12 +37,6 @@ def generate_test_description(): cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], name='test_dump_params') ) - launch_description.add_action( - ExecuteProcess( - cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py'), - 'test_dump_params_copy'], - name='test_dump_params_copy') - ) processes_to_test = [ ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-h'], @@ -52,30 +46,6 @@ def generate_test_description(): cmd=[os.getenv('TEST_EXECUTABLE')], name='test_dump_params_default', output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'], - name='test_dump_params_yaml', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params'], - name='test_dump_params_markdown', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'], - name='test_dump_params_yaml_verbose', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params', '-v'], - name='test_dump_params_markdown_verbose', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n', 'test_dump_params'], - name='test_dump_params_bad_format', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params,test_dump_params_copy'], - name='test_dump_params_multiple', - output='screen'), ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params_error'], name='test_dump_params_error', @@ -114,12 +84,6 @@ def test_processes_output(self, proc_output, processes_to_test): os.path.join(os.path.dirname(__file__), out) for out in ['dump_params_help', 'dump_params_default', - 'dump_params_yaml', - 'dump_params_md', - 'dump_params_yaml_verbose', - 'dump_params_md_verbose', - 'dump_params_yaml', - 'dump_params_multiple', 'dump_params_error'] ] for process, output_file in zip(processes_to_test[:-1], output_files[:-1]): diff --git a/nav2_util/test/test_dump_params/test_dump_params_md.test.py b/nav2_util/test/test_dump_params/test_dump_params_md.test.py new file mode 100644 index 00000000000..61a85c514cd --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_md.test.py @@ -0,0 +1,90 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params'], + name='test_dump_params_markdown', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params', '-v'], + name='test_dump_params_markdown_verbose', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_md', + 'dump_params_md_verbose'] + ] + for process, output_file in zip(processes_to_test, output_files): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + ) diff --git a/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py b/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py new file mode 100644 index 00000000000..4733c19f584 --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py @@ -0,0 +1,96 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py'), + 'test_dump_params_copy'], + name='test_dump_params_copy') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n', 'test_dump_params'], + name='test_dump_params_bad_format', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params,test_dump_params_copy'], + name='test_dump_params_multiple', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_yaml', + 'dump_params_multiple'] + ] + for process, output_file in zip(processes_to_test[:-1], output_files[:-1]): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + ) diff --git a/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py b/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py new file mode 100644 index 00000000000..545bf4934cf --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py @@ -0,0 +1,90 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'], + name='test_dump_params_yaml', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'], + name='test_dump_params_yaml_verbose', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_yaml', + 'dump_params_yaml_verbose'] + ] + for process, output_file in zip(processes_to_test, output_files): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + ) diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 082b4d2e07e..e9fe98957f8 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.1 + 0.4.2 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_voxel_grid/test/voxel_grid_tests.cpp b/nav2_voxel_grid/test/voxel_grid_tests.cpp index 6ff58812ff8..cfc596f6ae2 100644 --- a/nav2_voxel_grid/test/voxel_grid_tests.cpp +++ b/nav2_voxel_grid/test/voxel_grid_tests.cpp @@ -147,6 +147,46 @@ TEST(voxel_grid, InvalidSize) { EXPECT_TRUE(vg.getVoxelColumn(50, 11, 0, 0) == nav2_voxel_grid::VoxelStatus::UNKNOWN); } +TEST(voxel_grid, MarkAndClear) { + int size_x = 10, size_y = 10, size_z = 10; + nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z); + vg.markVoxelInMap(5, 5, 5, 0); + EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::MARKED); + vg.clearVoxelColumn(55); + EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::FREE); +} + +TEST(voxel_grid, clearVoxelLineInMap) { + int size_x = 10, size_y = 10, size_z = 10; + nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z); + vg.markVoxelInMap(0, 0, 5, 0); + EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::MARKED); + + unsigned char * map_2d = new unsigned char[100]; + map_2d[0] = 254; + + vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, map_2d, 16, 0); + + EXPECT_EQ(map_2d[0], 0); + + vg.markVoxelInMap(0, 0, 5, 0); + vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, nullptr, 16, 0); + EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::FREE); + delete[] map_2d; +} + +TEST(voxel_grid, GetVoxelData) { + uint32_t * data = new uint32_t[9]; + data[4] = 255; + data[0] = 0; + EXPECT_EQ( + nav2_voxel_grid::VoxelGrid::getVoxel(1, 1, 1, 3, 3, 3, data), nav2_voxel_grid::UNKNOWN); + + EXPECT_EQ( + nav2_voxel_grid::VoxelGrid::getVoxel(0, 0, 0, 3, 3, 3, data), nav2_voxel_grid::FREE); + delete[] data; +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 98d85f932b2..83ffa14f69e 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -92,11 +92,6 @@ class WaypointFollower : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Action server callbacks @@ -120,6 +115,7 @@ class WaypointFollower : public nav2_util::LifecycleNode std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; rclcpp::Node::SharedPtr client_node_; + std::shared_future::SharedPtr> future_goal_handle_; bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 2c5f86ea522..4edd90431fa 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.1 + 0.4.2 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 3a0edabce6e..129c3fcbb18 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -95,13 +95,6 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -WaypointFollower::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { @@ -133,7 +126,10 @@ WaypointFollower::followWaypoints() while (rclcpp::ok()) { // Check if asked to stop processing action if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(get_logger(), "Cancelling action."); + auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); + rclcpp::spin_until_future_complete(client_node_, cancel_future); + // for result callback processing + spin_some(client_node_); action_server_->terminate_all(); return; } @@ -157,7 +153,7 @@ WaypointFollower::followWaypoints() std::bind(&WaypointFollower::resultCallback, this, std::placeholders::_1); send_goal_options.goal_response_callback = std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); - auto future_goal_handle = + future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); current_goal_status_ = ActionStatus::PROCESSING; } diff --git a/navigation2/package.xml b/navigation2/package.xml index 29065f56d8b..ecb24e016a0 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.1 + 0.4.2 ROS2 Navigation Stack From db2f917bd5a1bdacf7acc5436f49434be0d02ad5 Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Wed, 19 Aug 2020 11:58:46 -0700 Subject: [PATCH 04/23] windows build fix. (#1959) --- nav2_bt_navigator/src/ros_topic_logger.cpp | 2 +- nav2_costmap_2d/package.xml | 1 + nav2_dwb_controller/nav_2d_utils/CMakeLists.txt | 4 ++++ nav2_map_server/src/map_io.cpp | 2 +- 4 files changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index 472b30d323b..48bffd20c85 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -43,7 +43,7 @@ void RosTopicLogger::callback( event.timestamp = tf2_ros::toMsg(std::chrono::time_point(timestamp)); #else - event.timestamp = tf2_ros::toMsg(timestamp); + event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); #endif event.node_name = node.name(); event.previous_status = toStr(prev_status, false); diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 02f6c7fd9db..c900f284028 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -19,6 +19,7 @@ ament_cmake nav2_common + angles geometry_msgs laser_geometry map_msgs diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index a0b268f3ef7..adb0123d7f9 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -47,6 +47,10 @@ add_library(tf_help SHARED src/tf_help.cpp ) +target_link_libraries(tf_help + conversions +) + ament_target_dependencies(tf_help ${dependencies} ) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 6941b354fe7..7375c64da8a 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -78,7 +78,7 @@ char * dirname(char * path) /* This assignment is ill-designed but the XPG specs require to return a string containing "." in any case no directory part is found and so a static and constant string is required. */ - path = reinterpret_cast(dot); + path = const_cast(&dot[0]); } return path; From fa1a64ad4d47a3793d22ce7da3f7238c92294af7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 24 Aug 2020 12:13:06 -0700 Subject: [PATCH 05/23] Foxy sync 3 (#1965) * waypoint_follower node has _rclcpp_node as well as base node (#1940) * Add #include for vector<> (#1946) To fix cpplint * Add 'angles' dependency to nav2_costmap_2d package.xml (#1947) * transform goal to costmap frame (#1949) The plan recieved is usually in global frame, but our local costmap is often in odom frame. This fixes a regression from #1857 * Add mutex lock into inflation layer to avoid thread issue in updating footprint (#1952) Signed-off-by: Daisuke Sato * Fix being unable to change StandardTrajectoryGenerator parameter vtheta_samples (#1619) * Fix tests declaring parameters real nodes don't Signed-off-by: Shane Loretz * Fix loadParameterWithDeprecation not getting initial parameter values Signed-off-by: Shane Loretz * Create sim_time variable before using it Signed-off-by: Shane Loretz * Line length < 100 Signed-off-by: Shane Loretz * Add missing { Signed-off-by: Shane Loretz * Linter fixes Signed-off-by: Shane Loretz * sim_granularity -> time_granularity Signed-off-by: Shane Loretz * Linter fix Signed-off-by: Shane Loretz * update version to 0.4.3 * removing redundant dep on angles Co-authored-by: Ruffin Co-authored-by: Sarthak Mittal Co-authored-by: Michael Ferguson Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Co-authored-by: Shane Loretz --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_bringup/bringup/package.xml | 2 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_controller/src/nav2_controller.cpp | 6 + nav2_core/package.xml | 2 +- .../nav2_costmap_2d/inflation_layer.hpp | 11 +- nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 8 ++ nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../dwb_plugins/test/twist_gen.cpp | 107 +++++++++--------- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- .../include/nav_2d_utils/parameters.hpp | 22 ++-- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_recoveries/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- .../src/waypoint_follower.cpp | 11 +- navigation2/package.xml | 2 +- 35 files changed, 130 insertions(+), 93 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index be9dc8ac014..4eb1fced5c9 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.2 + 0.4.3

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 02033f8b3e9..a012a86e20e 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.2 + 0.4.3 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index 9e821008e21..aa4fa05bb7d 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.4.2 + 0.4.3 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index 081b1cc9e7e..58ca2100273 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.4.2 + 0.4.3 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 433ac40fdc6..443dfc2f230 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.2 + 0.4.3 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 6820a84f4fa..07e75e63088 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.2 + 0.4.3 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index d0c6bbf7763..0dd318d8dc4 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.2 + 0.4.3 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 97846ba8eee..806df12569d 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -20,6 +20,7 @@ #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" +#include "nav_2d_utils/tf_help.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_controller/nav2_controller.hpp" @@ -328,6 +329,11 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) controllers_[current_controller_]->setPlan(path); auto end_pose = path.poses.back(); + end_pose.header.frame_id = path.header.frame_id; + rclcpp::Duration tolerance(costmap_ros_->getTransformTolerance() * 1e9); + nav_2d_utils::transformPose( + costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), + end_pose, end_pose, tolerance); goal_checker_->reset(); RCLCPP_DEBUG( diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 2ae59bc25ad..91241953ae4 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.2 + 0.4.3 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 08fed09145a..4acf05bd558 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/layer.hpp" @@ -77,7 +78,7 @@ class InflationLayer : public Layer public: InflationLayer(); - ~InflationLayer() override = default; + ~InflationLayer(); void onInitialize() override; void updateBounds( @@ -115,6 +116,13 @@ class InflationLayer : public Layer return cost; } + // Provide a typedef to ease future code maintenance + typedef std::recursive_mutex mutex_t; + mutex_t * getMutex() + { + return access_; + } + protected: void onFootprintChanged() override; @@ -184,6 +192,7 @@ class InflationLayer : public Layer // Indicates that the entire costmap should be reinflated next time around. bool need_reinflation_; + mutex_t * access_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index c900f284028..18c5da7dc86 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.2 + 0.4.3 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index df8405a3c91..4316da9ecb9 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -71,6 +71,12 @@ InflationLayer::InflationLayer() last_max_x_(std::numeric_limits::max()), last_max_y_(std::numeric_limits::max()) { + access_ = new mutex_t(); +} + +InflationLayer::~InflationLayer() +{ + delete access_; } void @@ -160,6 +166,7 @@ InflationLayer::updateCosts( int max_i, int max_j) { + std::lock_guard guard(*getMutex()); if (!enabled_ || (cell_inflation_radius_ == 0)) { return; } @@ -305,6 +312,7 @@ InflationLayer::enqueue( void InflationLayer::computeCaches() { + std::lock_guard guard(*getMutex()); if (cell_inflation_radius_ == 0) { return; } diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index b1d715f7d14..5f075bfca20 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.2 + 0.4.3 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index ccd46651690..76beb12cc3e 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.2 + 0.4.3 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 1e1c22895ae..a1b190038a1 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.2 + 0.4.3 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index e98ba11d3d7..7b21d0881ef 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.2 + 0.4.3 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 08b6a78bec0..754402d6f9b 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.2 + 0.4.3 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp index df8c0a0235f..1e4ca4cc6c1 100644 --- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp +++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp @@ -74,10 +74,14 @@ std::vector getDefaultKinematicParameters() return parameters; } -rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(const std::string & name) +rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode( + const std::string & name, + const std::vector & overrides = {}) { - rclcpp::NodeOptions node_options = nav2_util::get_node_options_default(); + rclcpp::NodeOptions node_options; node_options.parameter_overrides(getDefaultKinematicParameters()); + node_options.parameter_overrides().insert( + node_options.parameter_overrides().end(), overrides.begin(), overrides.end()); auto node = rclcpp_lifecycle::LifecycleNode::make_shared(name, node_options); node->on_configure(node->get_current_state()); @@ -142,8 +146,7 @@ TEST(VelocityIterator, standard_gen) TEST(VelocityIterator, max_xy) { - auto nh = makeTestNode("max_xy"); - nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", 1.0)}); + auto nh = makeTestNode("max_xy", {rclcpp::Parameter("dwb.max_speed_xy", 1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); @@ -155,8 +158,7 @@ TEST(VelocityIterator, max_xy) TEST(VelocityIterator, min_xy) { - auto nh = makeTestNode("min_xy"); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); + auto nh = makeTestNode("min_xy", {rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -167,8 +169,7 @@ TEST(VelocityIterator, min_xy) TEST(VelocityIterator, min_theta) { - auto nh = makeTestNode("min_theta"); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); + auto nh = makeTestNode("min_theta", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -179,10 +180,11 @@ TEST(VelocityIterator, min_theta) TEST(VelocityIterator, no_limits) { - auto nh = makeTestNode("no_limits"); - nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); + auto nh = makeTestNode( + "no_limits", { + rclcpp::Parameter("dwb.max_speed_xy", -1.0), + rclcpp::Parameter("dwb.min_speed_xy", -1.0), + rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -193,14 +195,15 @@ TEST(VelocityIterator, no_limits) TEST(VelocityIterator, no_limits_samples) { - auto nh = makeTestNode("no_limits_samples"); - nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); - int x_samples = 10, y_samples = 3, theta_samples = 5; - nh->set_parameters({rclcpp::Parameter("dwb.vx_samples", x_samples)}); - nh->set_parameters({rclcpp::Parameter("dwb.vy_samples", y_samples)}); - nh->set_parameters({rclcpp::Parameter("dwb.vtheta_samples", theta_samples)}); + const int x_samples = 10, y_samples = 3, theta_samples = 5; + auto nh = makeTestNode( + "no_limits_samples", { + rclcpp::Parameter("dwb.max_speed_xy", -1.0), + rclcpp::Parameter("dwb.min_speed_xy", -1.0), + rclcpp::Parameter("dwb.min_speed_theta", -1.0), + rclcpp::Parameter("dwb.vx_samples", x_samples), + rclcpp::Parameter("dwb.vy_samples", y_samples), + rclcpp::Parameter("dwb.vtheta_samples", theta_samples)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -210,8 +213,7 @@ TEST(VelocityIterator, no_limits_samples) TEST(VelocityIterator, dwa_gen) { - auto nh = makeTestNode("dwa_gen"); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); + auto nh = makeTestNode("dwa_gen", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -222,8 +224,7 @@ TEST(VelocityIterator, dwa_gen) TEST(VelocityIterator, nonzero) { - auto nh = makeTestNode("nonzero"); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); + auto nh = makeTestNode("nonzero", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh, "dwb"); nav_2d_msgs::msg::Twist2D initial; @@ -273,9 +274,8 @@ const double DEFAULT_SIM_TIME = 1.7; TEST(TrajectoryGenerator, basic) { - auto nh = makeTestNode("basic"); + auto nh = makeTestNode("basic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); gen.initialize(nh, "dwb"); dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward); matchTwist(res.velocity, forward); @@ -290,10 +290,11 @@ TEST(TrajectoryGenerator, basic) TEST(TrajectoryGenerator, basic_no_last_point) { - auto nh = makeTestNode("basic_no_last_point"); + auto nh = makeTestNode( + "basic_no_last_point", { + rclcpp::Parameter("dwb.include_last_point", false), + rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.include_last_point", false)}); - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); gen.initialize(nh, "dwb"); dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward); matchTwist(res.velocity, forward); @@ -308,9 +309,8 @@ TEST(TrajectoryGenerator, basic_no_last_point) TEST(TrajectoryGenerator, too_slow) { - auto nh = makeTestNode("too_slow"); + auto nh = makeTestNode("too_slow", {rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); gen.initialize(nh, "dwb"); nav_2d_msgs::msg::Twist2D cmd; cmd.x = 0.2; @@ -326,9 +326,8 @@ TEST(TrajectoryGenerator, too_slow) TEST(TrajectoryGenerator, holonomic) { - auto nh = makeTestNode("holonomic"); + auto nh = makeTestNode("holonomic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); gen.initialize(nh, "dwb"); nav_2d_msgs::msg::Twist2D cmd; cmd.x = 0.3; @@ -346,10 +345,11 @@ TEST(TrajectoryGenerator, holonomic) TEST(TrajectoryGenerator, twisty) { - auto nh = makeTestNode("twisty"); + auto nh = makeTestNode( + "twisty", { + rclcpp::Parameter("dwb.linear_granularity", 0.5), + rclcpp::Parameter("dwb.angular_granularity", 0.025)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); - nh->set_parameters({rclcpp::Parameter("dwb.angular_granularity", 0.025)}); gen.initialize(nh, "dwb"); nav_2d_msgs::msg::Twist2D cmd; cmd.x = 0.3; @@ -370,10 +370,11 @@ TEST(TrajectoryGenerator, twisty) TEST(TrajectoryGenerator, sim_time) { - auto nh = makeTestNode("sim_time"); const double sim_time = 2.5; - nh->set_parameters({rclcpp::Parameter("dwb.sim_time", sim_time)}); - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); + auto nh = makeTestNode( + "sim_time", { + rclcpp::Parameter("dwb.sim_time", sim_time), + rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward); @@ -389,12 +390,13 @@ TEST(TrajectoryGenerator, sim_time) TEST(TrajectoryGenerator, accel) { - auto nh = makeTestNode("accel"); - nh->set_parameters({rclcpp::Parameter("dwb.sim_time", 5.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.discretize_by_time", true)}); - nh->set_parameters({rclcpp::Parameter("dwb.time_granularity", 1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.acc_lim_x", 0.1)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); + auto nh = makeTestNode( + "accel", { + rclcpp::Parameter("dwb.sim_time", 5.0), + rclcpp::Parameter("dwb.discretize_by_time", true), + rclcpp::Parameter("dwb.time_granularity", 1.0), + rclcpp::Parameter("dwb.acc_lim_x", 0.1), + rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); @@ -412,13 +414,14 @@ TEST(TrajectoryGenerator, accel) TEST(TrajectoryGenerator, dwa) { - auto nh = makeTestNode("dwa"); - nh->set_parameters({rclcpp::Parameter("dwb.sim_period", 1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.sim_time", 5.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.discretize_by_time", true)}); - nh->set_parameters({rclcpp::Parameter("dwb.time_granularity", 1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.acc_lim_x", 0.1)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); + auto nh = makeTestNode( + "dwa", { + rclcpp::Parameter("dwb.sim_period", 1.0), + rclcpp::Parameter("dwb.sim_time", 5.0), + rclcpp::Parameter("dwb.discretize_by_time", true), + rclcpp::Parameter("dwb.time_granularity", 1.0), + rclcpp::Parameter("dwb.acc_lim_x", 0.1), + rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh, "dwb"); diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 0a48874c49b..d63553fc44b 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.2 + 0.4.3 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index e08b3d97629..4b87fa5093d 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.2 + 0.4.3 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index a7a2f010ee8..15dd3b9e4dc 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp @@ -93,18 +93,26 @@ param_t loadParameterWithDeprecation( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string current_name, const std::string old_name, const param_t & default_value) { - param_t value = 0; - if (nh->get_parameter(current_name, value)) { - return value; - } - if (nh->get_parameter(old_name, value)) { + nav2_util::declare_parameter_if_not_declared( + nh, current_name, rclcpp::ParameterValue(default_value)); + nav2_util::declare_parameter_if_not_declared( + nh, old_name, rclcpp::ParameterValue(default_value)); + + param_t current_name_value; + nh->get_parameter(current_name, current_name_value); + param_t old_name_value; + nh->get_parameter(old_name, old_name_value); + + if (old_name_value != current_name_value && old_name_value != default_value) { RCLCPP_WARN( nh->get_logger(), "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); - return value; + // set both parameters to the same value + nh->set_parameters({rclcpp::Parameter(current_name, old_name_value)}); + current_name_value = old_name_value; } - return default_value; + return current_name_value; } /** diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index ab5945eb9d8..c1d5a25ed14 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.2 + 0.4.3 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index f68735154e2..97cb21af53a 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.2 + 0.4.3 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 165a7234bcf..ce2a04b735d 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.2 + 0.4.3 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index dbdc5167c16..00265980c7d 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.2 + 0.4.3 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index e3fa60a5942..0776fa8bb04 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.2 + 0.4.3 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index f0beb74f099..ade94a94f43 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.2 + 0.4.3 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 67433aed718..1c8f25d39d8 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.2 + 0.4.3 TODO Carlos Orduno Steve Macenski diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index e424873ab42..0a66d3764ae 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.2 + 0.4.3 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 7c80e085d1a..b3ce62b13af 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.2 + 0.4.3 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 4b797e694af..bcae5f534df 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.2 + 0.4.3 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index e9fe98957f8..68e8fe652cc 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.2 + 0.4.3 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 4edd90431fa..250726bc167 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.2 + 0.4.3 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 129c3fcbb18..55474bd3c89 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -19,8 +19,7 @@ #include #include #include - -// TODO(stevemacenski): Add capability for reading in yaml file and executing +#include namespace nav2_waypoint_follower { @@ -47,9 +46,13 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); - // use suffix '_rclcpp_node' to keep parameter file consistency #1773 + std::vector new_args = rclcpp::NodeOptions().arguments(); + new_args.push_back("--ros-args"); + new_args.push_back("-r"); + new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); + new_args.push_back("--"); client_node_ = std::make_shared( - std::string(get_name()) + std::string("_rclcpp_node")); + "_", "", rclcpp::NodeOptions().arguments(new_args)); nav_to_pose_client_ = rclcpp_action::create_client( client_node_, "navigate_to_pose"); diff --git a/navigation2/package.xml b/navigation2/package.xml index ecb24e016a0..bbf19b3395d 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.2 + 0.4.3 ROS2 Navigation Stack From 603150bcf52a48847772558f30601c5b30cf0797 Mon Sep 17 00:00:00 2001 From: ryan-sandzimier <70606688+ryan-sandzimier@users.noreply.github.com> Date: Wed, 4 Nov 2020 15:25:23 -0800 Subject: [PATCH 06/23] Foxy sync smac planner (#2071) * Fix memory leak (#1900) * Fix memory leak in nav2_recoveries * Fix recovery server memory leak (better interface) * Fix costmap2d memory leak * Fix nav2_navfn_planner memory leak * Fix planner server and navfn planner memory leak * Make all rclcpp::Node::SharedPtr argument passing const * Fix controller server and DWB plugins memory leak * Minor fixes * Fix formatting errors * Change all plugin interfaces to use weak_ptr intead of shared_ptr to parent rclcpp::Node * Convert all SharedPtr to WeakPtr * Check shared_ptr after lock and before dereferencing * Smac/Hybrid-A* planner (#2021) * adding smac_planner to navigation2 metapackage * adding params to metapackage * update config files * adding navfn benchmark testing * updates to costmap_2d for flexility * update planner API for new changes * adding ompl to underlay because ros2 master doesn't contain the rosdep key * patching templated footprint collision checker * fix typo * updating readme config file * Analytic expansion (#43) * Use OMPL to generate heuristics The calculation is run at every planning cycle. It does not seem to slow down the planner - the calculation time seems to be quick enough that the improvement in graph expansion accounts for it. * Use OMPL to calculate analytic solution when near goal * Make angles multiples of the bin size to stop looping behaviour * Uncrustify * Use faster std::sqrt function * Fix analytic path so that the collision checker has coordinates to check! * Pre-allocate variables in analytic path expansion * Rename typedef to NodeGetter to more accurately describe function * Use distance rather than heuristic to determine when to perform analytic expansion Also force the analytic expansion to run on first iteration in case path is trivial. * Move the check for motion model into the main A* loop * Add copyright notices * Remove comment about relaxing node match tolerances The analytic expansion removes the need for this. * Correctly reset node coordinates when aborting from analytic expansion * Move analytic expansion logic to separate function * Uncrustify * Remove unneeded call to get goal coordinates * Fix the calculation of intervals in the analytic path Reserve the number of candidate nodes we are expecting. Base calculations on intervals rather than points - makes distances between nodes work properly. * Rescale heuristic so that analytic expansions are based on distance * Repeatedly split analytic path in half when checking for collision * Add parameter to control rate of analytic expansion attempts * Uncrustify * Fix incorrect type in templated function * Cpplint * Revert "Repeatedly split analytic path in half when checking for collision" This reverts commit 94d9ee0257c47fa6bd5a396ce74d180430c1c814. There was a marginal speed gain (perhaps!) and the splitting approach made the code harder to understand and maintain. * Uncrustify * Add doxygen comments * Add parameter description for analytic expansion ratio * Set lower limit of 2 on number of iterations between analytic expansions * Reduce expected number of iterations because of analytic completion * Refactor analytic expansion ratio calcs to make logic easier to understand * add readme color * fix linting * ceil from floor (and speed up) * a few updates * fix smac tests * fixing smoother test * remove cost check - to be readded at another time * working last test from debug issues * Update README.md * Update README.md * adding getUseRadius API doxygen Co-authored-by: James Ward * Adding additional SmacPlanner tests (#2036) * adding some more tests to smac_planner * addtl smac tests * remove unused functions * adding additional constants and smoothers tests (#2038) * Revert "Fix memory leak (#1900)" This reverts commit 681ccfa73a1c33c3ec387cbd46290ed90bc2cd87. * Changed WeakPtr to SharedPtr for compatibility with Foxy Co-authored-by: Sarthak Mittal Co-authored-by: Steve Macenski Co-authored-by: James Ward --- doc/parameters/param_list.md | 46 +- .../params/nav2_multirobot_params_1.yaml | 1 + .../params/nav2_multirobot_params_2.yaml | 1 + nav2_bringup/bringup/params/nav2_params.yaml | 4 + .../bringup/rviz/nav2_default_view.rviz | 14 + .../include/nav2_costmap_2d/costmap_2d.hpp | 8 + .../nav2_costmap_2d/costmap_2d_ros.hpp | 8 + .../costmap_topic_collision_checker.hpp | 2 +- .../footprint_collision_checker.hpp | 9 +- nav2_costmap_2d/src/costmap_2d.cpp | 5 + .../src/footprint_collision_checker.cpp | 30 +- .../unit/footprint_collision_checker_test.cpp | 15 +- nav2_navfn_planner/src/navfn_planner.cpp | 14 + nav2_planner/src/planner_server.cpp | 2 +- navigation2/package.xml | 1 + smac_planner/CMakeLists.txt | 123 ++++ smac_planner/README.md | 173 +++++ smac_planner/include/smac_planner/a_star.hpp | 321 +++++++++ .../smac_planner/collision_checker.hpp | 113 +++ .../include/smac_planner/constants.hpp | 70 ++ .../smac_planner/costmap_downsampler.hpp | 117 ++++ smac_planner/include/smac_planner/node_2d.hpp | 248 +++++++ .../include/smac_planner/node_basic.hpp | 68 ++ .../include/smac_planner/node_se2.hpp | 423 ++++++++++++ smac_planner/include/smac_planner/options.hpp | 207 ++++++ .../include/smac_planner/smac_planner.hpp | 131 ++++ .../include/smac_planner/smac_planner_2d.hpp | 122 ++++ .../include/smac_planner/smoother.hpp | 141 ++++ .../smac_planner/smoother_cost_function.hpp | 515 ++++++++++++++ smac_planner/include/smac_planner/types.hpp | 42 ++ smac_planner/package.xml | 38 ++ smac_planner/smac_plugin.xml | 5 + smac_planner/smac_plugin_2d.xml | 5 + smac_planner/src/a_star.cpp | 641 ++++++++++++++++++ smac_planner/src/costmap_downsampler.cpp | 129 ++++ smac_planner/src/node_2d.cpp | 151 +++++ smac_planner/src/node_se2.cpp | 432 ++++++++++++ smac_planner/src/smac_planner.cpp | 385 +++++++++++ smac_planner/src/smac_planner_2d.cpp | 339 +++++++++ smac_planner/test/CMakeLists.txt | 98 +++ .../test/deprecated_upsampler/upsampler.hpp | 213 ++++++ .../upsampler_cost_function.hpp | 366 ++++++++++ .../upsampler_cost_function_nlls.hpp | 334 +++++++++ smac_planner/test/path.png | Bin 0 -> 122370 bytes smac_planner/test/test_a_star.cpp | 183 +++++ smac_planner/test/test_collision_checker.cpp | 172 +++++ .../test/test_costmap_downsampler.cpp | 70 ++ smac_planner/test/test_node2d.cpp | 135 ++++ smac_planner/test/test_nodebasic.cpp | 49 ++ smac_planner/test/test_nodese2.cpp | 214 ++++++ smac_planner/test/test_smac_2d.cpp | 80 +++ smac_planner/test/test_smac_se2.cpp | 92 +++ smac_planner/test/test_smoother.cpp | 104 +++ tools/ros2_dependencies.repos | 8 + 54 files changed, 7194 insertions(+), 23 deletions(-) create mode 100644 smac_planner/CMakeLists.txt create mode 100644 smac_planner/README.md create mode 100644 smac_planner/include/smac_planner/a_star.hpp create mode 100644 smac_planner/include/smac_planner/collision_checker.hpp create mode 100644 smac_planner/include/smac_planner/constants.hpp create mode 100644 smac_planner/include/smac_planner/costmap_downsampler.hpp create mode 100644 smac_planner/include/smac_planner/node_2d.hpp create mode 100644 smac_planner/include/smac_planner/node_basic.hpp create mode 100644 smac_planner/include/smac_planner/node_se2.hpp create mode 100644 smac_planner/include/smac_planner/options.hpp create mode 100644 smac_planner/include/smac_planner/smac_planner.hpp create mode 100644 smac_planner/include/smac_planner/smac_planner_2d.hpp create mode 100644 smac_planner/include/smac_planner/smoother.hpp create mode 100644 smac_planner/include/smac_planner/smoother_cost_function.hpp create mode 100644 smac_planner/include/smac_planner/types.hpp create mode 100644 smac_planner/package.xml create mode 100644 smac_planner/smac_plugin.xml create mode 100644 smac_planner/smac_plugin_2d.xml create mode 100644 smac_planner/src/a_star.cpp create mode 100644 smac_planner/src/costmap_downsampler.cpp create mode 100644 smac_planner/src/node_2d.cpp create mode 100644 smac_planner/src/node_se2.cpp create mode 100644 smac_planner/src/smac_planner.cpp create mode 100644 smac_planner/src/smac_planner_2d.cpp create mode 100644 smac_planner/test/CMakeLists.txt create mode 100644 smac_planner/test/deprecated_upsampler/upsampler.hpp create mode 100644 smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp create mode 100644 smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp create mode 100644 smac_planner/test/path.png create mode 100644 smac_planner/test/test_a_star.cpp create mode 100644 smac_planner/test/test_collision_checker.cpp create mode 100644 smac_planner/test/test_costmap_downsampler.cpp create mode 100644 smac_planner/test/test_node2d.cpp create mode 100644 smac_planner/test/test_nodebasic.cpp create mode 100644 smac_planner/test/test_nodese2.cpp create mode 100644 smac_planner/test/test_smac_2d.cpp create mode 100644 smac_planner/test/test_smac_se2.cpp create mode 100644 smac_planner/test/test_smoother.cpp diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index adcd7f2f188..a59fce17364 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -166,7 +166,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo | ``.marking | true | Whether source should mark in costmap | | ``.clearing | false | Whether source should raytrace clear in costmap | | ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | -| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | +| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | # controller_server @@ -276,7 +276,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.``.x_only_threshold | 0.05 | Threshold to check in the X velocity direction | | ``.``.scale | 1.0 | Weighed scale for critic | -## kinematic_parameters +## kinematic_parameters | Parameter | Default | Description | | ----------| --------| ------------| @@ -295,7 +295,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) | | ``.decel_lim_theta | 0.0 | Maximum deceleration rotation (rad/s^2) | -## xy_theta_iterator +## xy_theta_iterator | Parameter | Default | Description | | ----------| --------| ------------| @@ -473,6 +473,46 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ``.use_astar | false | Whether to use A*, if false, uses Dijstra's expansion | | ``.allow_unknown | true | Whether to allow planning in unknown space | +# smac_planner + +* ``: Corresponding planner plugin ID for this type + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.tolerance | 0.5 | Tolerance in meters between requested goal pose and end of path | +| ``.downsample_costmap | false | Whether to downsample costmap | +| ``.downsampling_factor | 1 | Factor to downsample costmap by | +| ``.allow_unknown | true | whether to allow traversing in unknown space | +| ``.max_iterations | -1 | Number of iterations before failing, disabled by -1 | +| ``.max_on_approach_iterations | 1000 | Iterations after within threshold before returning approximate path with best heuristic | +| ``.max_planning_time_ms | 5000 | Maximum planning time in ms | +| ``.smooth_path | false | Whether to smooth path with CG smoother | +| ``.motion_model_for_search | DUBIN | Motion model to search with. Options for SE2: DUBIN, REEDS_SHEPP. 2D: MOORE, VON_NEUMANN | +| ``.angle_quantization_bins | 1 | Number of angle quantization bins for SE2 node | +| ``.minimum_turning_radius | 0.20 | Minimum turning radius in m of vehicle or desired path | +| ``.reverse_penalty | 2.0 | Penalty to apply to SE2 node if in reverse direction | +| ``.change_penalty | 0.5 | Penalty to apply to SE2 node if changing direction | +| ``.non_straight_penalty | 1.1 | Penalty to apply to SE2 node if non-straight direction | +| ``.cost_penalty | 1.2 | Penalty to apply to SE2 node for cost at pose | +| ``.analytic_expansion_ratio | 2.0 | For SE2 nodes the planner will attempt an analytic path expansion every N iterations, where N = closest_distance_to_goal / analytic_expansion_ratio. Higher ratios result in more frequent expansions | +| ``.smoother.smoother.w_curve | 1.5 | Smoother weight on curvature of path | +| ``.smoother.smoother.w_dist | 0.0 | Smoother weight on distance from original path | +| ``.smoother.smoother.w_smooth | 15000 | Smoother weight on distance between nodes | +| ``.smoother.smoother.w_cost | 1.5 | Smoother weight on costmap cost | +| ``.smoother.smoother.cost_scaling_factor | 10.0 | Inflation layer's scale factor | +| ``.smoother.optimizer.max_time | 0.10 | Maximum time to spend smoothing, in seconds | +| ``.smoother.optimizer.max_iterations | 500 | Maximum number of iterations to spend smoothing | +| ``.smoother.optimizer.debug_optimizer | false | Whether to print debug info from Ceres | +| ``.smoother.optimizer.gradient_tol | 1e-10 | Minimum change in gradient to terminate smoothing | +| ``.smoother.optimizer.fn_tol | 1e-7 | Minimum change in function to terminate smoothing | +| ``.smoother.optimizer.param_tol | 1e-15 | Minimum change in parameter block to terminate smoothing | + +| ``.smoother.optimizer.advanced.min_line_search_step_size | 1e-20 | Terminate smoothing iteration if change in parameter block less than this | +| ``.smoother.optimizer.advanced.max_num_line_search_step_size_iterations | 50 | Maximum iterations for line search | +| ``.smoother.optimizer.advanced.line_search_sufficient_function_decrease | 1e-20 | Function decrease amount to terminate current line search iteration | +| ``.smoother.optimizer.advanced.max_num_line_search_direction_restarts | 10 | Maximum umber of restarts of line search when over-estimating | +| ``.smoother.optimizer.advanced.max_line_search_step_expansion | 50 | Step size multiplier at each iteration of line search | + # waypoint_follower | Parameter | Default | Description | diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 53ab496bebd..26b8a54274d 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -159,6 +159,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 + inflation_radius: 0.55 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index a4a62d6b1a5..b5d07000749 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -159,6 +159,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 + inflation_radius: 0.55 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index eb290d50d3c..d0e1ad4c349 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -168,6 +168,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 + inflation_radius: 0.55 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -204,6 +205,7 @@ global_costmap: use_sim_time: True robot_radius: 0.22 resolution: 0.05 + track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -220,6 +222,8 @@ global_costmap: map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 always_send_full_costmap: True global_costmap_client: ros__parameters: diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz index 04e989eb170..c07e3e0cba6 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz @@ -249,6 +249,20 @@ Visualization Manager: Value: /global_costmap/costmap Use Timestamp: false Value: true + - Alpha: 0.3 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Use Timestamp: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 8f9fd438677..d74895b40a2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -125,6 +125,14 @@ class Costmap2D */ unsigned char getCost(unsigned int mx, unsigned int my) const; + /** + * @brief Get the cost of a cell in the costmap + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The cost of the cell + */ + unsigned char getCost(unsigned int index) const; + /** * @brief Set the cost of a cell in the costmap * @param mx The x coordinate of the cell diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 541a246a513..124d1666a51 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -248,6 +248,14 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::shared_ptr getTfBuffer() {return tf_buffer_;} + /** + * @brief Get the costmap's use_radius_ parameter, corresponding to + * whether the footprint for the robot is a circle with radius robot_radius_ + * or an arbitrarily defined footprint in footprint_. + * @return use_radius_ + */ + bool getUseRadius() {return use_radius_;} + protected: rclcpp::Node::SharedPtr client_node_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 636a907f076..9f54728e635 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -68,7 +68,7 @@ class CostmapTopicCollisionChecker CostmapSubscriber & costmap_sub_; FootprintSubscriber & footprint_sub_; double transform_tolerance_; - FootprintCollisionChecker collision_checker_; + FootprintCollisionChecker> collision_checker_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp index 24f35aa1359..39c51328b8d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp @@ -32,20 +32,21 @@ namespace nav2_costmap_2d { typedef std::vector Footprint; +template class FootprintCollisionChecker { public: FootprintCollisionChecker(); - explicit FootprintCollisionChecker(std::shared_ptr costmap); + explicit FootprintCollisionChecker(CostmapT costmap); double footprintCost(const Footprint footprint); double footprintCostAtPose(double x, double y, double theta, const Footprint footprint); double lineCost(int x0, int x1, int y0, int y1) const; bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); double pointCost(int x, int y) const; - void setCostmap(std::shared_ptr costmap); + void setCostmap(CostmapT costmap); -private: - std::shared_ptr costmap_; +protected: + CostmapT costmap_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e5fc9472920..976d09ce263 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -212,6 +212,11 @@ unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const return costmap_[getIndex(mx, my)]; } +unsigned char Costmap2D::getCost(unsigned int undex) const +{ + return costmap_[undex]; +} + void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) { costmap_[getIndex(mx, my)] = cost; diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 15adcd8665b..20d60b3442a 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -31,18 +31,21 @@ using namespace std::chrono_literals; namespace nav2_costmap_2d { -FootprintCollisionChecker::FootprintCollisionChecker() +template +FootprintCollisionChecker::FootprintCollisionChecker() : costmap_(nullptr) { } -FootprintCollisionChecker::FootprintCollisionChecker( - std::shared_ptr costmap) +template +FootprintCollisionChecker::FootprintCollisionChecker( + CostmapT costmap) : costmap_(costmap) { } -double FootprintCollisionChecker::footprintCost(const Footprint footprint) +template +double FootprintCollisionChecker::footprintCost(const Footprint footprint) { // now we really have to lay down the footprint in the costmap_ grid unsigned int x0, x1, y0, y1; @@ -80,7 +83,8 @@ double FootprintCollisionChecker::footprintCost(const Footprint footprint) return footprint_cost; } -double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const +template +double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const { double line_cost = 0.0; double point_cost = -1.0; @@ -96,23 +100,27 @@ double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const return line_cost; } -bool FootprintCollisionChecker::worldToMap( +template +bool FootprintCollisionChecker::worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my) { return costmap_->worldToMap(wx, wy, mx, my); } -double FootprintCollisionChecker::pointCost(int x, int y) const +template +double FootprintCollisionChecker::pointCost(int x, int y) const { return costmap_->getCost(x, y); } -void FootprintCollisionChecker::setCostmap(std::shared_ptr costmap) +template +void FootprintCollisionChecker::setCostmap(CostmapT costmap) { costmap_ = costmap; } -double FootprintCollisionChecker::footprintCostAtPose( +template +double FootprintCollisionChecker::footprintCostAtPose( double x, double y, double theta, const Footprint footprint) { double cos_th = cos(theta); @@ -128,4 +136,8 @@ double FootprintCollisionChecker::footprintCostAtPose( return footprintCost(oriented_footprint); } +// declare our valid template parameters +template class FootprintCollisionChecker>; +template class FootprintCollisionChecker; + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index f0e9c306233..1be0d1282d2 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -39,7 +39,8 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); @@ -51,7 +52,8 @@ TEST(collision_footprint, test_point_cost) std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.pointCost(50, 50); @@ -63,7 +65,8 @@ TEST(collision_footprint, test_world_to_map) std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); unsigned int x, y; @@ -105,7 +108,8 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); @@ -140,7 +144,8 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index a873a3e5d1c..0393fbdaafe 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -19,6 +19,8 @@ // the Global Dynamic Window Approach. IEEE. // https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf +// #define BENCHMARK_TESTING + #include "nav2_navfn_planner/navfn_planner.hpp" #include @@ -114,6 +116,10 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { +#ifdef BENCHMARK_TESTING + steady_clock::time_point a = steady_clock::now(); +#endif + // Update planner based on the new costmap size if (isPlannerOutOfDate()) { planner_->setNavArr( @@ -128,6 +134,14 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( node_->get_logger(), "%s: failed to create plan with " "tolerance %.2f.", name_.c_str(), tolerance_); } + +#ifdef BENCHMARK_TESTING + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + return path; } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ee3a07e4739..b6b4615bb0a 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -46,7 +46,7 @@ PlannerServer::PlannerServer() // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); - declare_parameter("expected_planner_frequency", 20.0); + declare_parameter("expected_planner_frequency", 1.0); // Setup the global costmap costmap_ros_ = std::make_shared( diff --git a/navigation2/package.xml b/navigation2/package.xml index bbf19b3395d..bd72a39fc5e 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -30,6 +30,7 @@ nav2_voxel_grid nav2_controller nav2_waypoint_follower + smac_planner ament_cmake diff --git a/smac_planner/CMakeLists.txt b/smac_planner/CMakeLists.txt new file mode 100644 index 00000000000..d161ba516e8 --- /dev/null +++ b/smac_planner/CMakeLists.txt @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 3.5) +project(smac_planner) + +set(CMAKE_BUILD_TYPE Release) #Debug, Release + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) +find_package(Ceres REQUIRED COMPONENTS SuiteSparse) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(ompl REQUIRED) +find_package(OpenMP REQUIRED) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + +add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) + +include_directories( + include + ${CERES_INCLUDES} + ${OMPL_INCLUDE_DIRS} + ${OpenMP_INCLUDE_DIRS} +) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") + +set(library_name smac_planner) + +set(dependencies + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + eigen3_cmake_module +) + +# SE2 plugin +add_library(${library_name} SHARED + src/smac_planner.cpp + src/a_star.cpp + src/node_se2.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp +) + +target_link_libraries(${library_name} ${CERES_LIBRARIES} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) +target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS}) + +ament_target_dependencies(${library_name} + ${dependencies} +) + +# 2D plugin +add_library(${library_name}_2d SHARED + src/smac_planner_2d.cpp + src/a_star.cpp + src/node_se2.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp +) + +target_link_libraries(${library_name}_2d ${CERES_LIBRARIES} ${OMPL_LIBRARIES}) +target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) + +ament_target_dependencies(${library_name}_2d + ${dependencies} +) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(${library_name}_2d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core smac_plugin.xml) +pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml) + +install(TARGETS ${library_name} ${library_name}_2d + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name} ${library_name}_2d) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/smac_planner/README.md b/smac_planner/README.md new file mode 100644 index 00000000000..5bf0cd4217b --- /dev/null +++ b/smac_planner/README.md @@ -0,0 +1,173 @@ +# Smac Planner + +The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 2 distinct plugins: +- `SmacPlanner`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models. +- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting Moore and Von Neumann models. + +It also introduces the following basic building blocks: +- `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution. +- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Template implementations are provided for grid-A* and SE2 Hybrid-A* planning. Additional template for 3D planning also could be made available. +- `CollisionChecker`: Collision check based on a robot's radius or footprint. +- `Smoother`: A Conjugate-gradient (CG) smoother with several optional cost function implementations for use. This is a cost-aware smoother unlike b-splines or bezier curves. + +We have users reporting using this on: +- Delivery robots +- Industrial robots + +## Introduction + +The `smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support cars, car-like, and ackermann vehicles using the `SmacPlanner` plugin which implements a Hybrid-A\* planner. This plugin is also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. + +The `SmacPlanner` fully-implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), including hybrid searching, CG smoothing, analytic expansions and hueristic functions. + +In summary... + +The `SmacPlanner` is designed to work with: +- Ackermann, car, and car-like robots +- High speed or curvature constrained robots (as to not flip over, skid, or dump load at high speeds) +- Arbitrary shaped, non-circular differential or omnidirectional robots requring SE2 collision checking + +The `SmacPlanner2D` is designed to work with: +- Circular, differential or omnidirectional robots +- Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint. + +## Features + +We further improve on the Hybrid-A\* work in the following ways: +- Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio). +- Multi-resolution search allowing planning to occur at a coarser resolution for wider spaces (O(N^2) faster). +- Cost-aware penalty functions in search resulting in far smoother plans (further reducing requirement to smooth). +- Additional cost functions in the CG smoother including path deflection. +- Approximated smoother Voronoi field using costmap inflation function. +- Fixed 3 mathematical issues in the original paper resulting in higher quality smoothing. +- Faster planning than original paper by highly optimizing the template A\* algorithm. +- Automatically adjusted search motion model sizes by motion model, costmap resolution, and bin sizing. +- Closest path on approach within tolerance if exact path cannot be found or in invalid space. +- Multi-model hybrid searching including Dubin and Reeds-Shepp models. More models may be trivially added. +- Time monitoring of planning to dynamically scale the maximum CG smoothing time based on remaining planning duration requested. +- High unit and integration test coverage, doxygen documentation. +- Uses modern C++14 language features and individual components are easily reusable. +- Speed optimizations: Fast approximation of shortest paths with wavefront hueristic, no data structure graph lookups in main loop, near-zero copy main loop, Voronoi field approximation, et al. +- Templated Nodes and A\* implementation to support additional robot extensions. + +All of these features (multi-resolution, models, smoother, etc) are also available in the 2D `SmacPlanner2D` plugin. + +The 2D A\* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A\* implementation in the NavFn Planner. While this 2D A\* planner is slightly slower, I believe it's well worth the increased quality in paths. Though the `SmacPlanner2D` is grid-based, any reasonable local trajectory planner - including those supported by Navigation2 - will not have any issue with grid-based plans. + +## Metrics + +The original Hybrid-A\* implementation boasted planning times of 50-300ms for planning across 102,400 cell maps with 72 angular bins. We see much faster results in our evaluations: + +- **2-20ms** for planning across 147,456 (1.4x larger) cell maps with 72 angular bins. +- **30-120ms** for planning across 344,128 (3.3x larger) cell map with 72 angular bins. + +For example, the following path (roughly 85 meters) path took 33ms to compute. + +![alt text](test/path.png) + +## Design + +The basic design centralizes a templated A\* implementation that handles the search of a graph of nodes. The implementation is templated by the nodes, `NodeT`, which contain the methods needed to compute the hueristics, travel costs, and search neighborhoods. The outcome of this design is then a standard A\* implementation that can be used to traverse any type of graph as long as a node template can be created for it. + +We provide by default a 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods, but the smoother can be used to smooth it out. We also provide a SE2 node template (`NodeSE2`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing. + +Additional node templates could be added into the future to better support other types of robot path planning, such as including a state lattice motion primitive node and 3D path planning. Pull requests or discussions aroudn this point is encouraged. + +In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path. For the SE2 and `SmacPlanner` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. + +We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. + +## Parameters + +See inline description of parameters in the `SmacPlanner`. This includes comments as specific parameters apply to `SmacPlanner2D` and `SmacPlanner` in SE2 place. + +``` +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: True + + GridBased: + plugin: "smac_planner/SmacPlanner" + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: false # allow traveling in unknown space + max_iterations: -1 # maximum total iterations to search for before failing + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only + max_planning_time_ms: 2000.0 # max time in ms for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. + smooth_path: false # Whether to smooth searched path + motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp + angle_quantization_bins: 72 # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search) + minimum_turning_radius: 0.20 # For SE2 node & smoother: minimum turning radius in m of path / vehicle + reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.20 # For SE2 node: penalty to apply if motion is changing directions, must be >= 0 + non_straight_penalty: 1.05 # For SE2 node: penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 1.3 # For SE2 node: penalty to apply to higher cost zones + + smoother: + smoother: + w_curve: 30.0 # weight to minimize curvature of path + w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight + w_smooth: 30000.0 # weight to maximize smoothness of path + w_cost: 0.025 # weight to steer robot away from collision and cost + cost_scaling_factor: 10.0 # this should match the inflation layer's parameter + + # I do not recommend users mess with this unless they're doing production tuning + optimizer: + max_time: 0.10 # maximum compute time for smoother + max_iterations: 500 # max iterations of smoother + debug_optimizer: false # print debug info + gradient_tol: 1.0e-10 + fn_tol: 1.0e-20 + param_tol: 1.0e-15 + advanced: + min_line_search_step_size: 1.0e-20 + max_num_line_search_step_size_iterations: 50 + line_search_sufficient_function_decrease: 1.0e-20 + max_num_line_search_direction_restarts: 10 + max_line_search_step_expansion: 50 +``` + +## Topics + +| Topic | Type | +|-----------------|-------------------| +| unsmoothed_path | nav_msgs/Path | + + +## Install + +``` +sudo apt-get install ros--smac-planner +``` + +## Etc (Important Side Notes) + +### Potential Fields + +Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. + +Some of the most popular tuning guides for Navigation / Navigation2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. + +This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be very suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. + +So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. + +### 2D Search and Smoothing + +While the 2D planner has the smoother available (albeit, default parameters are tuned for the Hybrid-A\* planner, so you may need to play with that), my recommendation is not to use it. + +The 2D planner provides a 4-connected or 8-connected neighborhood path. This path may have little zig-zags in order to get at another non-90 or non-45 degree heading. That is totally fine. Your local trajectory planner such as DWB and TEB take these points into account to follow, but you won't see any zig-zag behaviors of your final robot motion after given to a trajectory planner. + +The smoothing is more "pleasing" to human eyes, but you don't want to be owning additional compute when it doesn't largely impact the output. However, if you have a more sensitive local trajectory planner like a carrot follower (e.g. pure pursuit), then you will want to smooth out the paths in order to have something more easily followable. + +Take this advise into account. Some good numbers to potentially start with would be `cost_scaling_factor: 10.0` and `inflation_radius: 5.5`. + +### Costmap Resolutions + +We provide for both the Hybrid-A\* and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For 60m paths in an office space, I was able to get it << 100ms at a 2-3x downsample rate. + +I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. + +Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. diff --git a/smac_planner/include/smac_planner/a_star.hpp b/smac_planner/include/smac_planner/a_star.hpp new file mode 100644 index 00000000000..a53b872a913 --- /dev/null +++ b/smac_planner/include/smac_planner/a_star.hpp @@ -0,0 +1,321 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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. + +#ifndef SMAC_PLANNER__A_STAR_HPP_ +#define SMAC_PLANNER__A_STAR_HPP_ + +#include +#include +#include +#include +#include +#include +#include "Eigen/Core" + +#include "nav2_costmap_2d/costmap_2d.hpp" + +#include "smac_planner/node_2d.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/node_basic.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/constants.hpp" + +namespace smac_planner +{ + +inline double squaredDistance( + const Eigen::Vector2d & p1, + const Eigen::Vector2d & p2) +{ + const double & dx = p1[0] - p2[0]; + const double & dy = p1[1] - p2[1]; + return hypot(dx, dy); +} + +/** + * @class smac_planner::AStarAlgorithm + * @brief An A* implementation for planning in a costmap. Templated based on the Node type. + */ +template +class AStarAlgorithm +{ +public: + typedef NodeT * NodePtr; + typedef std::unordered_map Graph; + typedef std::vector NodeVector; + typedef std::pair> NodeElement; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + typedef typename NodeVector::iterator NeighborIterator; + typedef std::function NodeGetter; + + /** + * @struct smac_planner::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; + + /** + * @brief A constructor for smac_planner::PlannerServer + * @param neighborhood The type of neighborhood to use for search (4 or 8 connected) + */ + explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); + + /** + * @brief A destructor for smac_planner::AStarAlgorithm + */ + ~AStarAlgorithm(); + + /** + * @brief Initialization of the planner with defaults + * @param allow_unknown Allow search in unknown space, good for navigation while mapping + * @param max_iterations Maximum number of iterations to use while expanding search + * @param max_on_approach_iterations Maximum number of iterations before returning a valid + * path once within thresholds to refine path + * comes at more compute time but smoother paths. + */ + void initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations); + + /** + * @brief Creating path from given costmap, start, and goal + * @param path Reference to a vector of indicies of generated path + * @param num_iterations Reference to number of iterations to create plan + * @param tolerance Reference to tolerance in costmap nodes + * @return if plan was successful + */ + bool createPath(CoordinateVector & path, int & num_iterations, const float & tolerance); + + /** + * @brief Create the graph based on the node type. For 2D nodes, a cost grid. + * For 3D nodes, a SE2 grid without cost info as needs collision detector for footprint. + * @param x The total number of nodes in the X direction + * @param y The total number of nodes in the X direction + * @param dim_3 The total number of nodes in the theta or Z direction + * @param costmap Costmap to convert into the graph + */ + void createGraph( + const unsigned int & x, + const unsigned int & y, + const unsigned int & dim_3, + nav2_costmap_2d::Costmap2D * & costmap); + + /** + * @brief Set the goal for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3); + + /** + * @brief Set the starting pose for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3); + + /** + * @brief Set the footprint + * @param footprint footprint of robot + * @param use_radius Whether this footprint is a circle with radius + */ + void setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius); + + /** + * @brief Perform an analytic path expansion to the goal + * @param node The node to start the analytic path from + * @param getter The function object that gets valid nodes from the graph + * @return Node pointer to goal node if successful, else return nullptr + */ + NodePtr getAnalyticPath(const NodePtr & node, const NodeGetter & getter); + + /** + * @brief Set the starting pose for planning, as a node index + * @param node Node pointer to the goal node to backtrace + * @param path Reference to a vector of indicies of generated path + * @return whether the path was able to be backtraced + */ + bool backtracePath(NodePtr & node, CoordinateVector & path); + + /** + * @brief Get maximum number of iterations to plan + * @return Reference to Maximum iterations parameter + */ + int & getMaxIterations(); + + /** + * @brief Get pointer reference to starting node + * @return Node pointer reference to starting node + */ + NodePtr & getStart(); + + /** + * @brief Get pointer reference to goal node + * @return Node pointer reference to goal node + */ + NodePtr & getGoal(); + + /** + * @brief Get maximum number of on-approach iterations after within threshold + * @return Reference to Maximum on-appraoch iterations parameter + */ + int & getOnApproachMaxIterations(); + + /** + * @brief Get tolerance, in node nodes + * @return Reference to tolerance parameter + */ + float & getToleranceHeuristic(); + + /** + * @brief Get size of graph in X + * @return Size in X + */ + unsigned int & getSizeX(); + + /** + * @brief Get size of graph in Y + * @return Size in Y + */ + unsigned int & getSizeY(); + + /** + * @brief Get number of angle quantization bins (SE2) or Z coordinate (XYZ) + * @return Number of angle bins / Z dimension + */ + unsigned int & getSizeDim3(); + +protected: + /** + * @brief Get pointer to next goal in open set + * @return Node pointer reference to next heuristically scored node + */ + inline NodePtr getNextNode(); + + /** + * @brief Get pointer to next goal in open set + * @param cost The cost to sort into the open set of the node + * @param node Node pointer reference to add to open set + */ + inline void addNode(const float cost, NodePtr & node); + + /** + * @brief Adds node to graph + * @param cost The cost to sort into the open set of the node + * @param node Node pointer reference to add to open set + */ + inline NodePtr addToGraph(const unsigned int & index); + + /** + * @brief Check if this node is the goal node + * @param node Node pointer to check if its the goal node + * @return if node is goal + */ + inline bool isGoal(NodePtr & node); + + /** + * @brief Get cost of traversal between nodes + * @param current_node Pointer to current node + * @param new_node Pointer to new node + * @return Reference traversal cost between the nodes + */ + inline float getTraversalCost(NodePtr & current_node, NodePtr & new_node); + + /** + * @brief Get total cost of traversal for a node + * @param node Pointer to current node + * @return Reference accumulated cost between the nodes + */ + inline float & getAccumulatedCost(NodePtr & node); + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + inline float getHeuristicCost(const NodePtr & node); + + /** + * @brief Check if inputs to planner are valid + * @return Are valid + */ + inline bool areInputsValid(); + + /** + * @brief Clear hueristic queue of nodes to search + */ + inline void clearQueue(); + + /** + * @brief Clear graph of nodes searched + */ + inline void clearGraph(); + + /** + * @brief Attempt an analytic path completion + * @return Node pointer reference to goal node if successful, else + * return nullptr + */ + inline NodePtr tryAnalyticExpansion( + const NodePtr & current_node, + const NodeGetter & getter, int & iterations, int & best_cost); + + bool _traverse_unknown; + int _max_iterations; + int _max_on_approach_iterations; + float _tolerance; + unsigned int _x_size; + unsigned int _y_size; + unsigned int _dim3_size; + SearchInfo _search_info; + + Coordinates _goal_coordinates; + NodePtr _start; + NodePtr _goal; + + Graph _graph; + NodeQueue _queue; + + MotionModel _motion_model; + NodeHeuristicPair _best_heuristic_node; + + GridCollisionChecker _collision_checker; + nav2_costmap_2d::Footprint _footprint; + bool _is_radius_footprint; + nav2_costmap_2d::Costmap2D * _costmap; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__A_STAR_HPP_ diff --git a/smac_planner/include/smac_planner/collision_checker.hpp b/smac_planner/include/smac_planner/collision_checker.hpp new file mode 100644 index 00000000000..588514901ff --- /dev/null +++ b/smac_planner/include/smac_planner/collision_checker.hpp @@ -0,0 +1,113 @@ +// Copyright (c) 2020, 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. Reserved. + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "smac_planner/constants.hpp" + +#ifndef SMAC_PLANNER__COLLISION_CHECKER_HPP_ +#define SMAC_PLANNER__COLLISION_CHECKER_HPP_ + +namespace smac_planner +{ + +/** + * @class smac_planner::GridCollisionChecker + * @brief A costmap grid collision checker + */ +class GridCollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for smac_planner::GridCollisionChecker + * @param costmap The costmap to collision check against + */ + GridCollisionChecker( + nav2_costmap_2d::Costmap2D * costmap) + : FootprintCollisionChecker(costmap) + { + } + + /** + * @brief Set the footprint to use with collision checker + * @param footprint The footprint to collision check against + * @param radius Whether or not the footprint is a circle and use radius collision checking + */ + void setFootprint(const nav2_costmap_2d::Footprint & footprint, const bool & radius) + { + unoriented_footprint_ = footprint; + footprint_is_radius_ = radius; + } + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param x X coordinate of pose to check against + * @param y Y coordinate of pose to check against + * @param theta Angle of pose to check against + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const float & x, + const float & y, + const float & theta, + const bool & traverse_unknown) + { + // Assumes setFootprint already set + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points + footprint_cost_ = footprintCostAtPose( + wx, wy, static_cast(theta), unoriented_footprint_); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + footprint_cost_ = costmap_->getCost( + static_cast(x), static_cast(y)); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; + } + } + + /** + * @brief Get cost at footprint pose in costmap + * @return the cost at the pose in costmap + */ + float getCost() + { + // Assumes inCollision called prior + return static_cast(footprint_cost_); + } + +protected: + nav2_costmap_2d::Footprint unoriented_footprint_; + double footprint_cost_; + bool footprint_is_radius_; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__COLLISION_CHECKER_HPP_ diff --git a/smac_planner/include/smac_planner/constants.hpp b/smac_planner/include/smac_planner/constants.hpp new file mode 100644 index 00000000000..bc2ccfd8c63 --- /dev/null +++ b/smac_planner/include/smac_planner/constants.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__CONSTANTS_HPP_ +#define SMAC_PLANNER__CONSTANTS_HPP_ + +#include + +namespace smac_planner +{ +enum class MotionModel +{ + UNKNOWN = 0, + VON_NEUMANN = 1, + MOORE = 2, + DUBIN = 3, + REEDS_SHEPP = 4, +}; + +inline std::string toString(const MotionModel & n) +{ + switch (n) { + case MotionModel::VON_NEUMANN: + return "Von Neumann"; + case MotionModel::MOORE: + return "Moore"; + case MotionModel::DUBIN: + return "Dubin"; + case MotionModel::REEDS_SHEPP: + return "Reeds-Shepp"; + default: + return "Unknown"; + } +} + +inline MotionModel fromString(const std::string & n) +{ + if (n == "VON_NEUMANN") { + return MotionModel::VON_NEUMANN; + } else if (n == "MOORE") { + return MotionModel::MOORE; + } else if (n == "DUBIN") { + return MotionModel::DUBIN; + } else if (n == "REEDS_SHEPP") { + return MotionModel::REEDS_SHEPP; + } else { + return MotionModel::UNKNOWN; + } +} + +const float UNKNOWN = 255; +const float OCCUPIED = 254; +const float INSCRIBED = 253; +const float MAX_NON_OBSTACLE = 252; +const float FREE = 0; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__CONSTANTS_HPP_ diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/smac_planner/include/smac_planner/costmap_downsampler.hpp new file mode 100644 index 00000000000..6f23c696e21 --- /dev/null +++ b/smac_planner/include/smac_planner/costmap_downsampler.hpp @@ -0,0 +1,117 @@ +// Copyright (c) 2020, Carlos Luis +// +// 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. + +#ifndef SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ +#define SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ + +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "smac_planner/constants.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::CostmapDownsampler + * @brief A costmap downsampler for more efficient path planning + */ +class CostmapDownsampler +{ +public: + /** + * @brief A constructor for CostmapDownsampler + * @param node Lifecycle node pointer + */ + explicit CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node); + + /** + * @brief A destructor for CostmapDownsampler + */ + ~CostmapDownsampler(); + + /** + * @brief Initialize the downsampled costmap object and the ROS publisher + * @param global_frame The ID of the global frame used by the costmap + * @param topic_name The name of the topic to publish the downsampled costmap + * @param costmap The costmap we want to downsample + * @param downsampling_factor Multiplier for the costmap resolution + */ + void initialize( + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor); + + /** + * @brief Activate the publisher of the downsampled costmap + */ + void activatePublisher() + { + _downsampled_costmap_pub->on_activate(); + } + + /** + * @brief Deactivate the publisher of the downsampled costmap + */ + void deactivatePublisher() + { + _downsampled_costmap_pub->on_deactivate(); + } + + /** + * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap + * @param downsampling_factor Multiplier for the costmap resolution + * @return A ptr to the downsampled costmap + */ + nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor); + + /** + * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version + */ + void resizeCostmap(); + +protected: + /** + * @brief Update the sizes X-Y of the costmap and its downsampled version + */ + void updateCostmapSize(); + + /** + * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell + * @param new_mx The X-coordinate of the cell in the new costmap + * @param new_my The Y-coordinate of the cell in the new costmap + */ + void setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my); + + unsigned int _size_x; + unsigned int _size_y; + unsigned int _downsampled_size_x; + unsigned int _downsampled_size_y; + unsigned int _downsampling_factor; + float _downsampled_resolution; + std::string _topic_name; + nav2_util::LifecycleNode::SharedPtr _node; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _downsampled_costmap; + std::unique_ptr _downsampled_costmap_pub; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/smac_planner/include/smac_planner/node_2d.hpp b/smac_planner/include/smac_planner/node_2d.hpp new file mode 100644 index 00000000000..b80a316cbb1 --- /dev/null +++ b/smac_planner/include/smac_planner/node_2d.hpp @@ -0,0 +1,248 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__NODE_2D_HPP_ +#define SMAC_PLANNER__NODE_2D_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "smac_planner/constants.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::Node2D + * @brief Node2D implementation for graph + */ +class Node2D +{ +public: + typedef Node2D * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + + /** + * @class smac_planner::Node2D::Coordinates + * @brief Node2D implementation of coordinate structure + */ + struct Coordinates + { + Coordinates() {} + Coordinates(const float & x_in, const float & y_in) + : x(x_in), y(y_in) + {} + + float x, y; + }; + typedef std::vector CoordinateVector; + + /** + * @brief A constructor for smac_planner::Node2D + * @param cost_in The costmap cost at this node + * @param index The index of this node for self-reference + */ + explicit Node2D(unsigned char & cost_in, const unsigned int index); + + /** + * @brief A destructor for smac_planner::Node2D + */ + ~Node2D(); + + /** + * @brief operator== for comparisons + * @param Node2D right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const Node2D & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief Reset method for new search + * @param cost_in The costmap cost at this node + */ + void reset(const unsigned char & cost); + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + inline void visited() + { + _was_visited = true; + _is_queued = false; + } + + /** + * @brief Gets if cell is currently queued in search + * @param If cell was queued + */ + inline bool & isQueued() + { + return _is_queued; + } + + /** + * @brief Sets if cell is currently queued in search + */ + inline void queued() + { + _is_queued = true; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @param collision_checker Pointer to collision checker object + * @return whether this node is valid and collision free + */ + bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker); + + /** + * @brief get traversal cost from this node to child node + * @param child Node pointer to this node's child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index + * @param x x coordinate of point to get index of + * @param y y coordinate of point to get index of + * @param width width of costmap + * @return index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & width) + { + return x + y * width; + } + + /** + * @brief Get index + * @param Index Index of point + * @param width width of costmap + * @param angles angle bins to use (must be 1 or throws exception) + * @return coordinates of point + */ + static inline Coordinates getCoords( + const unsigned int & index, const unsigned int & width, const unsigned int & angles) + { + if (angles != 1) { + throw std::runtime_error("Node type Node2D does not have a valid angle quantization."); + } + + return Coordinates(index % width, index / width); + } + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + static float getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates); + + /** + * @brief Initialize the neighborhood to be used in A* + * We support 4-connect (VON_NEUMANN) and 8-connect (MOORE) + * @param x_size_uint The total x size to find neighbors + * @param neighborhood The desired neighborhood type + */ + static void initNeighborhood( + const unsigned int & x_size_uint, + const MotionModel & neighborhood); + /** + * @brief Retrieve all valid neighbors of a node. + * @param node Pointer to the node we are currently exploring in A* + * @param graph Reference to graph to discover new nodes + * @param neighbors Vector of neighbors to be filled + */ + static void getNeighbors( + NodePtr & node, + std::function & validity_checker, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + Node2D * parent; + static double neutral_cost; + static std::vector _neighbors_grid_offsets; + +private: + float _cell_cost; + float _accumulated_cost; + unsigned int _index; + bool _was_visited; + bool _is_queued; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__NODE_2D_HPP_ diff --git a/smac_planner/include/smac_planner/node_basic.hpp b/smac_planner/include/smac_planner/node_basic.hpp new file mode 100644 index 00000000000..3f3941717d1 --- /dev/null +++ b/smac_planner/include/smac_planner/node_basic.hpp @@ -0,0 +1,68 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__NODE_BASIC_HPP_ +#define SMAC_PLANNER__NODE_BASIC_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "smac_planner/constants.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/node_2d.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::NodeBasic + * @brief NodeBasic implementation for priority queue insertion + */ +template +class NodeBasic +{ +public: + /** + * @brief A constructor for smac_planner::NodeBasic + * @param cost_in The costmap cost at this node + * @param index The index of this node for self-reference + */ + explicit NodeBasic(const unsigned int index) + : index(index), + graph_node_ptr(nullptr) + { + } + + typename NodeT::Coordinates pose; // Used by NodeSE2 + NodeT * graph_node_ptr; + unsigned int index; +}; + +template class NodeBasic; +template class NodeBasic; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/smac_planner/include/smac_planner/node_se2.hpp b/smac_planner/include/smac_planner/node_se2.hpp new file mode 100644 index 00000000000..4fb1ae579dc --- /dev/null +++ b/smac_planner/include/smac_planner/node_se2.hpp @@ -0,0 +1,423 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__NODE_SE2_HPP_ +#define SMAC_PLANNER__NODE_SE2_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "smac_planner/constants.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +// Need seperate pose struct for motion table operations + +/** + * @struct smac_planner::MotionPose + * @brief A struct for poses in motion primitives + */ +struct MotionPose +{ + /** + * @brief A constructor for smac_planner::MotionPose + */ + MotionPose() {} + + /** + * @brief A constructor for smac_planner::MotionPose + * @param x X pose + * @param y Y pose + * @param theta Angle of pose + */ + MotionPose(const float & x, const float & y, const float & theta) + : _x(x), _y(y), _theta(theta) + {} + + float _x; + float _y; + float _theta; +}; + +typedef std::vector MotionPoses; + +// Must forward declare +class NodeSE2; + +/** + * @struct smac_planner::MotionTable + * @brief A table of motion primitives and related functions + */ +struct MotionTable +{ + /** + * @brief A constructor for smac_planner::MotionTable + */ + MotionTable() {} + + /** + * @brief Initializing using Dubin model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initDubin( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Initializing using Reeds-Shepp model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initReedsShepp( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Get projections of motion models + * @param node Ptr to SE2 node + * @return A set of motion poses + */ + MotionPoses getProjections(const NodeSE2 * node); + + /** + * @brief Get a projection of motion model + * @param node Ptr to SE2 node + * @return A motion pose + */ + MotionPose getProjection(const NodeSE2 * node, const unsigned int & motion_index); + + MotionPoses projections; + unsigned int size_x; + unsigned int num_angle_quantization; + float num_angle_quantization_float; + float bin_size; + float change_penalty; + float non_straight_penalty; + float cost_penalty; + float reverse_penalty; + ompl::base::StateSpacePtr state_space; +}; + +/** + * @class smac_planner::NodeSE2 + * @brief NodeSE2 implementation for graph + */ +class NodeSE2 +{ +public: + typedef NodeSE2 * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + /** + * @class smac_planner::NodeSE2::Coordinates + * @brief NodeSE2 implementation of coordinate structure + */ + struct Coordinates + { + /** + * @brief A constructor for smac_planner::NodeSE2::Coordinates + */ + Coordinates() {} + + /** + * @brief A constructor for smac_planner::NodeSE2::Coordinates + * @param x_in X coordinate + * @param y_in Y coordinate + * @param theta_in Theta coordinate + */ + Coordinates(const float & x_in, const float & y_in, const float & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + float x, y, theta; + }; + + typedef std::vector CoordinateVector; + + /** + * @brief A constructor for smac_planner::NodeSE2 + * @param index The index of this node for self-reference + */ + explicit NodeSE2(const unsigned int index); + + /** + * @brief A destructor for smac_planner::NodeSE2 + */ + ~NodeSE2(); + + /** + * @brief operator== for comparisons + * @param NodeSE2 right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const NodeSE2 & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + + /** + * @brief Reset method for new search + */ + void reset(); + + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Sets the motion primitive index used to achieve node in search + * @param reference to motion primitive idx + */ + inline void setMotionPrimitiveIndex(const unsigned int & idx) + { + _motion_primitive_index = idx; + } + + /** + * @brief Gets the motion primitive index used to achieve node in search + * @return reference to motion primitive idx + */ + inline unsigned int & getMotionPrimitiveIndex() + { + return _motion_primitive_index; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + inline void visited() + { + _was_visited = true; + _is_queued = false; + } + + /** + * @brief Gets if cell is currently queued in search + * @param If cell was queued + */ + inline bool & isQueued() + { + return _is_queued; + } + + /** + * @brief Sets if cell is currently queued in search + */ + inline void queued() + { + _is_queued = true; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @return whether this node is valid and collision free + */ + bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker); + + /** + * @brief Get traversal cost of parent node to child node + * @param child Node pointer to child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @param width Width of costmap + * @param angle_quantization Number of theta bins + * @return Index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle, + const unsigned int & width, const unsigned int angle_quantization) + { + return angle + x * angle_quantization + y * width * angle_quantization; + } + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @return Index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle) + { + return getIndex( + x, y, angle, motion_table.size_x, + motion_table.num_angle_quantization); + } + + /** + * @brief Get coordinates at index + * @param index Index of point + * @param width Width of costmap + * @param angle_quantization Theta size of costmap + * @return Coordinates + */ + static inline Coordinates getCoords( + const unsigned int & index, + const unsigned int & width, const unsigned int angle_quantization) + { + return Coordinates( + (index / angle_quantization) % width, // x + index / (angle_quantization * width), // y + index % angle_quantization); // theta + } + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + static float getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates); + + /** + * @brief Initialize motion models + * @param motion_model Motion model enum to use + * @param size_x Size of X of graph + * @param size_y Size of y of graph + * @param angle_quantization Size of theta bins of graph + * @param search_info Search info to use + */ + static void initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & angle_quantization, + SearchInfo & search_info); + + /** + * @brief Compute the wavefront heuristic + * @param costmap Costmap to use to compute heuristic + * @param start_x Coordinate of Start X + * @param start_y Coordinate of Start Y + * @param goal_x Coordinate of Goal X + * @param goal_y Coordinate of Goal Y + */ + static void computeWavefrontHeuristic( + nav2_costmap_2d::Costmap2D * & costmap, + const unsigned int & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y); + + /** + * @brief Retrieve all valid neighbors of a node. + * @param node Pointer to the node we are currently exploring in A* + * @param validity_checker Functor for state validity checking + * @param neighbors Vector of neighbors to be filled + */ + static void getNeighbors( + const NodePtr & node, + std::function & validity_checker, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + NodeSE2 * parent; + Coordinates pose; + static double neutral_cost; + static MotionTable motion_table; + +private: + float _cell_cost; + float _accumulated_cost; + unsigned int _index; + bool _was_visited; + bool _is_queued; + unsigned int _motion_primitive_index; + static std::vector _wavefront_heuristic; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__NODE_SE2_HPP_ diff --git a/smac_planner/include/smac_planner/options.hpp b/smac_planner/include/smac_planner/options.hpp new file mode 100644 index 00000000000..01951d8950a --- /dev/null +++ b/smac_planner/include/smac_planner/options.hpp @@ -0,0 +1,207 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__OPTIONS_HPP_ +#define SMAC_PLANNER__OPTIONS_HPP_ + +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" + +namespace smac_planner +{ + +/** + * @struct smac_planner::SmootherParams + * @brief Parameters for the smoother cost function + */ +struct SmootherParams +{ + /** + * @brief A constructor for smac_planner::SmootherParams + */ + SmootherParams() + { + } + + /** + * @brief Get params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.smoother."); + + // Smoother params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_curve", rclcpp::ParameterValue(1.5)); + node->get_parameter(local_name + "w_curve", curvature_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_cost", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_cost", costmap_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_dist", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_dist", distance_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0)); + node->get_parameter(local_name + "w_smooth", smooth_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "cost_scaling_factor", rclcpp::ParameterValue(10.0)); + node->get_parameter(local_name + "cost_scaling_factor", costmap_factor); + } + + double smooth_weight{0.0}; + double costmap_weight{0.0}; + double distance_weight{0.0}; + double curvature_weight{0.0}; + double max_curvature{0.0}; + double costmap_factor{0.0}; + double max_time; +}; + +/** + * @struct smac_planner::OptimizerParams + * @brief Parameters for the ceres optimizer + */ +struct OptimizerParams +{ + OptimizerParams() + : debug(false), + max_iterations(50), + max_time(1e4), + param_tol(1e-8), + fn_tol(1e-6), + gradient_tol(1e-10) + { + } + + /** + * @struct AdvancedParams + * @brief Advanced parameters for the ceres optimizer + */ + struct AdvancedParams + { + AdvancedParams() + : min_line_search_step_size(1e-9), + max_num_line_search_step_size_iterations(20), + line_search_sufficient_function_decrease(1e-4), + max_num_line_search_direction_restarts(20), + max_line_search_step_contraction(1e-3), + min_line_search_step_contraction(0.6), + line_search_sufficient_curvature_decrease(0.9), + max_line_search_step_expansion(10) + { + } + + /** + * @brief Get advanced params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.optimizer.advanced."); + + // Optimizer advanced params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "min_line_search_step_size", + rclcpp::ParameterValue(1e-20)); + node->get_parameter( + local_name + "min_line_search_step_size", + min_line_search_step_size); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_num_line_search_step_size_iterations", + rclcpp::ParameterValue(50)); + node->get_parameter( + local_name + "max_num_line_search_step_size_iterations", + max_num_line_search_step_size_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "line_search_sufficient_function_decrease", + rclcpp::ParameterValue(1e-20)); + node->get_parameter( + local_name + "line_search_sufficient_function_decrease", + line_search_sufficient_function_decrease); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_num_line_search_direction_restarts", + rclcpp::ParameterValue(10)); + node->get_parameter( + local_name + "max_num_line_search_direction_restarts", + max_num_line_search_direction_restarts); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_line_search_step_expansion", + rclcpp::ParameterValue(50)); + node->get_parameter( + local_name + "max_line_search_step_expansion", + max_line_search_step_expansion); + } + + + double min_line_search_step_size; // Ceres default: 1e-9 + int max_num_line_search_step_size_iterations; // Ceres default: 20 + double line_search_sufficient_function_decrease; // Ceres default: 1e-4 + int max_num_line_search_direction_restarts; // Ceres default: 5 + + double max_line_search_step_contraction; // Ceres default: 1e-3 + double min_line_search_step_contraction; // Ceres default: 0.6 + double line_search_sufficient_curvature_decrease; // Ceres default: 0.9 + int max_line_search_step_expansion; // Ceres default: 10 + }; + + /** + * @brief Get params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.optimizer."); + + // Optimizer params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "param_tol", rclcpp::ParameterValue(1e-15)); + node->get_parameter(local_name + "param_tol", param_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7)); + node->get_parameter(local_name + "fn_tol", fn_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10)); + node->get_parameter(local_name + "gradient_tol", gradient_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_iterations", rclcpp::ParameterValue(500)); + node->get_parameter(local_name + "max_iterations", max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_time", rclcpp::ParameterValue(0.100)); + node->get_parameter(local_name + "max_time", max_time); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "debug_optimizer", rclcpp::ParameterValue(false)); + node->get_parameter(local_name + "debug_optimizer", debug); + + advanced.get(node, name); + } + + bool debug; + int max_iterations; // Ceres default: 50 + double max_time; // Ceres default: 1e4 + + double param_tol; // Ceres default: 1e-8 + double fn_tol; // Ceres default: 1e-6 + double gradient_tol; // Ceres default: 1e-10 + + AdvancedParams advanced; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__OPTIONS_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp new file mode 100644 index 00000000000..379f0c92a5b --- /dev/null +++ b/smac_planner/include/smac_planner/smac_planner.hpp @@ -0,0 +1,131 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__SMAC_PLANNER_HPP_ +#define SMAC_PLANNER__SMAC_PLANNER_HPP_ + +#include +#include +#include + +#include "smac_planner/a_star.hpp" +#include "smac_planner/smoother.hpp" +#include "smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" + +namespace smac_planner +{ + +class SmacPlanner : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlanner(); + + /** + * @brief destructor + */ + ~SmacPlanner(); + + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_msgs::Path of the generated path + */ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + + /** + * @brief Create an Eigen Vector2D of world poses from continuous map coords + * @param mx float of map X coordinate + * @param my float of map Y coordinate + * @param costmap Costmap pointer + * @return Eigen::Vector2d eigen vector of the generated path + */ + Eigen::Vector2d getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Create quaternion from A* coord bins + * @param theta continuous bin coordinates angle + * @return quaternion orientation in map frame + */ + geometry_msgs::msg::Quaternion getWorldOrientation(const float & theta); + + /** + * @brief Remove hooking at end of paths + * @param path Path to remove hooking from + */ + void removeHook(std::vector & path); + +protected: + std::unique_ptr> _a_star; + std::unique_ptr _smoother; + nav2_util::LifecycleNode::SharedPtr _node; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _costmap_downsampler; + std::string _global_frame, _name; + float _tolerance; + int _downsampling_factor; + unsigned int _angle_quantizations; + double _angle_bin_size; + bool _downsample_costmap; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + SmootherParams _smoother_params; + OptimizerParams _optimizer_params; + double _max_planning_time; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMAC_PLANNER_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner_2d.hpp b/smac_planner/include/smac_planner/smac_planner_2d.hpp new file mode 100644 index 00000000000..6d89887c3d3 --- /dev/null +++ b/smac_planner/include/smac_planner/smac_planner_2d.hpp @@ -0,0 +1,122 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ +#define SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ + +#include +#include +#include + +#include "smac_planner/a_star.hpp" +#include "smac_planner/smoother.hpp" +#include "smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" + +namespace smac_planner +{ + +class SmacPlanner2D : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlanner2D(); + + /** + * @brief destructor + */ + ~SmacPlanner2D(); + + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_msgs::Path of the generated path + */ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + + /** + * @brief Create an Eigen Vector2D of world poses from continuous map coords + * @param mx float of map X coordinate + * @param my float of map Y coordinate + * @param costmap Costmap pointer + * @return Eigen::Vector2d eigen vector of the generated path + */ + Eigen::Vector2d getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Remove hooking at end of paths + * @param path Path to remove hooking from + */ + void removeHook(std::vector & path); + +protected: + std::unique_ptr> _a_star; + std::unique_ptr _smoother; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _costmap_downsampler; + nav2_util::LifecycleNode::SharedPtr _node; + std::string _global_frame, _name; + float _tolerance; + int _downsampling_factor; + bool _downsample_costmap; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + SmootherParams _smoother_params; + OptimizerParams _optimizer_params; + double _max_planning_time; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ diff --git a/smac_planner/include/smac_planner/smoother.hpp b/smac_planner/include/smac_planner/smoother.hpp new file mode 100644 index 00000000000..756e6b57939 --- /dev/null +++ b/smac_planner/include/smac_planner/smoother.hpp @@ -0,0 +1,141 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__SMOOTHER_HPP_ +#define SMAC_PLANNER__SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "smac_planner/types.hpp" +#include "smac_planner/smoother_cost_function.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace smac_planner +{ + +/** + * @class smac_planner::Smoother + * @brief A Conjugate Gradient 2D path smoother implementation + */ +class Smoother +{ +public: + /** + * @brief A constructor for smac_planner::Smoother + */ + Smoother() {} + + /** + * @brief A destructor for smac_planner::Smoother + */ + ~Smoother() {} + + /** + * @brief Initialization of the smoother + * @param params OptimizerParam struct + */ + void initialize(const OptimizerParams params) + { + _debug = params.debug; + + // General Params + + // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT + _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT; + _options.line_search_type = ceres::WOLFE; + _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE; + _options.line_search_interpolation_type = ceres::CUBIC; + + _options.max_num_iterations = params.max_iterations; + _options.max_solver_time_in_seconds = params.max_time; + + _options.function_tolerance = params.fn_tol; + _options.gradient_tolerance = params.gradient_tol; + _options.parameter_tolerance = params.param_tol; + + _options.min_line_search_step_size = params.advanced.min_line_search_step_size; + _options.max_num_line_search_step_size_iterations = + params.advanced.max_num_line_search_step_size_iterations; + _options.line_search_sufficient_function_decrease = + params.advanced.line_search_sufficient_function_decrease; + _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction; + _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction; + _options.max_num_line_search_direction_restarts = + params.advanced.max_num_line_search_direction_restarts; + _options.line_search_sufficient_curvature_decrease = + params.advanced.line_search_sufficient_curvature_decrease; + _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion; + + if (_debug) { + _options.minimizer_progress_to_stdout = true; + } else { + _options.logging_type = ceres::SILENT; + } + } + + /** + * @brief Smoother method + * @param path Reference to path + * @param costmap Pointer to minimal costmap + * @param smoother parameters weights + * @return If smoothing was successful + */ + bool smooth( + std::vector & path, + nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + { + _options.max_solver_time_in_seconds = params.max_time; + + double parameters[path.size() * 2]; // NOLINT + for (uint i = 0; i != path.size(); i++) { + parameters[2 * i] = path[i][0]; + parameters[2 * i + 1] = path[i][1]; + } + + ceres::GradientProblemSolver::Summary summary; + ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params)); + ceres::Solve(_options, problem, parameters, &summary); + + if (_debug) { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { + return false; + } + + for (uint i = 0; i != path.size(); i++) { + path[i][0] = parameters[2 * i]; + path[i][1] = parameters[2 * i + 1]; + } + + return true; + } + +private: + bool _debug; + ceres::GradientProblemSolver::Options _options; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/smac_planner/include/smac_planner/smoother_cost_function.hpp b/smac_planner/include/smac_planner/smoother_cost_function.hpp new file mode 100644 index 00000000000..82d1c88a6dc --- /dev/null +++ b/smac_planner/include/smac_planner/smoother_cost_function.hpp @@ -0,0 +1,515 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#define SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ + +/** + * @struct smac_planner::UnconstrainedSmootherCostFunction + * @brief Cost function for path smoothing with multiple terms + * including curvature, smoothness, collision, and avoid obstacles. + */ +class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction +{ +public: + /** + * @brief A constructor for smac_planner::UnconstrainedSmootherCostFunction + * @param original_path Original unsmoothed path to smooth + * @param costmap A costmap to get values for collision and obstacle avoidance + */ + UnconstrainedSmootherCostFunction( + std::vector * original_path, + nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + : _original_path(original_path), + _num_params(2 * original_path->size()), + _costmap(costmap), + _params(params) + { + } + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = true; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0.0, 0.0}; + Eigen::Vector2d delta_xi_p{0.0, 0.0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + virtual bool Evaluate( + const double * parameters, + double * cost, + double * gradient) const + { + Eigen::Vector2d xi; + Eigen::Vector2d xi_p1; + Eigen::Vector2d xi_m1; + uint x_index, y_index; + cost[0] = 0.0; + double cost_raw = 0.0; + double grad_x_raw = 0.0; + double grad_y_raw = 0.0; + unsigned int mx, my; + bool valid_coords = true; + double costmap_cost = 0.0; + + // cache some computations between the residual and jacobian + CurvatureComputations curvature_params; + + for (int i = 0; i != NumParameters() / 2; i++) { + x_index = 2 * i; + y_index = 2 * i + 1; + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + if (i < 1 || i >= NumParameters() / 2 - 1) { + continue; + } + + xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]); + xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]); + xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]); + + // compute cost + addSmoothingResidual(_params.smooth_weight, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, cost_raw); + addDistanceResidual(_params.distance_weight, xi, _original_path->at(i), cost_raw); + + if (valid_coords = _costmap->worldToMap(xi[0], xi[1], mx, my)) { + costmap_cost = _costmap->getCost(mx, my); + addCostResidual(_params.costmap_weight, costmap_cost, cost_raw); + } + + if (gradient != NULL) { + // compute gradient + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + addSmoothingJacobian(_params.smooth_weight, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian( + _params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, + grad_x_raw, grad_y_raw); + addDistanceJacobian( + _params.distance_weight, xi, _original_path->at( + i), grad_x_raw, grad_y_raw); + + if (valid_coords) { + addCostJacobian(_params.costmap_weight, mx, my, costmap_cost, grad_x_raw, grad_y_raw); + } + + gradient[x_index] = grad_x_raw; + gradient[y_index] = grad_y_raw; + } + } + + cost[0] = cost_raw; + + return true; + } + + /** + * @brief Get number of parameter blocks + * @return Number of parameters in cost function + */ + virtual int NumParameters() const {return _num_params;} + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature; + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + return; + } + + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + + // Old formulation we may require again. + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + + j0 += weight * jacobian[0]; // xi x component of partial-derivative + j1 += weight * jacobian[1]; // xi x component of partial-derivative + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param r Residual (cost) of term + */ + inline void addDistanceResidual( + const double & weight, + const Eigen::Vector2d & xi, + const Eigen::Vector2d & xi_original, + double & r) const + { + r += weight * (xi - xi_original).dot(xi - xi_original); // objective function value + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addDistanceJacobian( + const double & weight, + const Eigen::Vector2d & xi, + const Eigen::Vector2d & xi_original, + double & j0, + double & j1) const + { + j0 += weight * 2 * (xi[0] - xi_original[0]); // xi y component of partial-derivative + j1 += weight * 2 * (xi[1] - xi_original[1]); // xi y component of partial-derivative + } + + + /** + * @brief Cost function term for steering away from costs + * @param weight Weight to apply to function + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param r Residual (cost) of term + */ + inline void addCostResidual( + const double & weight, + const double & value, + double & r) const + { + if (value == FREE) { + return; + } + + r += weight * value * value; // objective function value + } + + /** + * @brief Cost function derivative term for steering away from costs + * @param weight Weight to apply to function + * @param mx Point Xi's x coordinate in map frame + * @param mx Point Xi's y coordinate in map frame + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCostJacobian( + const double & weight, + const unsigned int & mx, + const unsigned int & my, + const double & value, + double & j0, + double & j1) const + { + if (value == FREE) { + return; + } + + const Eigen::Vector2d grad = getCostmapGradient(mx, my); + const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value; + + j0 += common_prefix * grad[0]; // xi x component of partial-derivative + j1 += common_prefix * grad[1]; // xi y component of partial-derivative + } + + /** + * @brief Computing the gradient of the costmap using + * the 2 point numerical differentiation method + * @param mx Point Xi's x coordinate in map frame + * @param mx Point Xi's y coordinate in map frame + * @param params Params reference to store gradients + */ + inline Eigen::Vector2d getCostmapGradient( + const unsigned int mx, + const unsigned int my) const + { + // find unit vector that describes that direction + // via 7 point taylor series approximation for gradient at Xi + Eigen::Vector2d gradient; + + double l_1 = 0.0; + double l_2 = 0.0; + double l_3 = 0.0; + double r_1 = 0.0; + double r_2 = 0.0; + double r_3 = 0.0; + + if (mx < _costmap->getSizeInCellsX()) { + r_1 = static_cast(_costmap->getCost(mx + 1, my)); + } + if (mx + 1 < _costmap->getSizeInCellsX()) { + r_2 = static_cast(_costmap->getCost(mx + 2, my)); + } + if (mx + 2 < _costmap->getSizeInCellsX()) { + r_3 = static_cast(_costmap->getCost(mx + 3, my)); + } + + if (mx > 0) { + l_1 = static_cast(_costmap->getCost(mx - 1, my)); + } + if (mx - 1 > 0) { + l_2 = static_cast(_costmap->getCost(mx - 2, my)); + } + if (mx - 2 > 0) { + l_3 = static_cast(_costmap->getCost(mx - 3, my)); + } + + gradient[1] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60; + + if (my < _costmap->getSizeInCellsY()) { + r_1 = static_cast(_costmap->getCost(mx, my + 1)); + } + if (my + 1 < _costmap->getSizeInCellsY()) { + r_2 = static_cast(_costmap->getCost(mx, my + 2)); + } + if (my + 2 < _costmap->getSizeInCellsY()) { + r_3 = static_cast(_costmap->getCost(mx, my + 3)); + } + + if (my > 0) { + l_1 = static_cast(_costmap->getCost(mx, my - 1)); + } + if (my - 1 > 0) { + l_2 = static_cast(_costmap->getCost(mx, my - 2)); + } + if (my - 2 > 0) { + l_3 = static_cast(_costmap->getCost(mx, my - 3)); + } + + gradient[0] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60; + + gradient.normalize(); + return gradient; + } + + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + std::vector * _original_path{nullptr}; + int _num_params; + nav2_costmap_2d::Costmap2D * _costmap{nullptr}; + SmootherParams _params; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ diff --git a/smac_planner/include/smac_planner/types.hpp b/smac_planner/include/smac_planner/types.hpp new file mode 100644 index 00000000000..e39564d333b --- /dev/null +++ b/smac_planner/include/smac_planner/types.hpp @@ -0,0 +1,42 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef SMAC_PLANNER__TYPES_HPP_ +#define SMAC_PLANNER__TYPES_HPP_ + +#include +#include + +namespace smac_planner +{ + +typedef std::pair NodeHeuristicPair; + +/** + * @struct smac_planner::SearchInfo + * @brief Search properties and penalties + */ +struct SearchInfo +{ + float minimum_turning_radius; + float non_straight_penalty; + float change_penalty; + float reverse_penalty; + float cost_penalty; + float analytic_expansion_ratio; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__TYPES_HPP_ diff --git a/smac_planner/package.xml b/smac_planner/package.xml new file mode 100644 index 00000000000..8de16907864 --- /dev/null +++ b/smac_planner/package.xml @@ -0,0 +1,38 @@ + + + + smac_planner + 0.3.0 + Smac global planning plugin + Steve Macenski + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_lifecycle + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + nav2_common + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + libceres-dev + eigen3_cmake_module + eigen + ompl + + ament_lint_common + ament_lint_auto + + + ament_cmake + + + diff --git a/smac_planner/smac_plugin.xml b/smac_planner/smac_plugin.xml new file mode 100644 index 00000000000..09f17b666fc --- /dev/null +++ b/smac_planner/smac_plugin.xml @@ -0,0 +1,5 @@ + + + SE2 version of the SMAC planner + + diff --git a/smac_planner/smac_plugin_2d.xml b/smac_planner/smac_plugin_2d.xml new file mode 100644 index 00000000000..3845a0ffe2a --- /dev/null +++ b/smac_planner/smac_plugin_2d.xml @@ -0,0 +1,5 @@ + + + 2D version of the SMAC Planner + + diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp new file mode 100644 index 00000000000..3464442b70c --- /dev/null +++ b/smac_planner/src/a_star.cpp @@ -0,0 +1,641 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include "smac_planner/a_star.hpp" +using namespace std::chrono; // NOLINT + +namespace smac_planner +{ + +template +AStarAlgorithm::AStarAlgorithm( + const MotionModel & motion_model, + const SearchInfo & search_info) +: _traverse_unknown(true), + _max_iterations(0), + _x_size(0), + _y_size(0), + _search_info(search_info), + _goal_coordinates(Coordinates()), + _start(nullptr), + _goal(nullptr), + _motion_model(motion_model), + _collision_checker(nullptr) +{ + _graph.reserve(100000); +} + +template +AStarAlgorithm::~AStarAlgorithm() +{ +} + +template +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations) +{ + _traverse_unknown = allow_unknown; + _max_iterations = max_iterations; + _max_on_approach_iterations = max_on_approach_iterations; +} + +template<> +void AStarAlgorithm::createGraph( + const unsigned int & x_size, + const unsigned int & y_size, + const unsigned int & dim_3_size, + nav2_costmap_2d::Costmap2D * & costmap) +{ + if (dim_3_size != 1) { + throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); + } + _costmap = costmap; + _dim3_size = dim_3_size; // 2D search MUST be 2D, not 3D or SE2. + clearGraph(); + + if (getSizeX() != x_size || getSizeY() != y_size) { + _x_size = x_size; + _y_size = y_size; + Node2D::initNeighborhood(_x_size, _motion_model); + } +} + +template<> +void AStarAlgorithm::createGraph( + const unsigned int & x_size, + const unsigned int & y_size, + const unsigned int & dim_3_size, + nav2_costmap_2d::Costmap2D * & costmap) +{ + _costmap = costmap; + _collision_checker = GridCollisionChecker(costmap); + _collision_checker.setFootprint(_footprint, _is_radius_footprint); + + _dim3_size = dim_3_size; + unsigned int index; + clearGraph(); + + if (getSizeX() != x_size || getSizeY() != y_size) { + _x_size = x_size; + _y_size = y_size; + NodeSE2::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); + } +} + +template +void AStarAlgorithm::setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius) +{ + _footprint = footprint; + _is_radius_footprint = use_radius; +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( + const unsigned int & index) +{ + return &(_graph.emplace(index, Node2D(_costmap->getCharMap()[index], index)).first->second); +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( + const unsigned int & index) +{ + return &(_graph.emplace(index, NodeSE2(index)).first->second); +} + +template<> +void AStarAlgorithm::setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + if (dim_3 != 0) { + throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3."); + } + _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); +} + +template<> +void AStarAlgorithm::setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + _start = addToGraph(NodeSE2::getIndex(mx, my, dim_3, getSizeX(), getSizeDim3())); + _start->setPose( + Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); +} + +template<> +void AStarAlgorithm::setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + if (dim_3 != 0) { + throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); + } + + _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); + _goal_coordinates = Node2D::Coordinates(mx, my); +} + +template<> +void AStarAlgorithm::setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + _goal = addToGraph(NodeSE2::getIndex(mx, my, dim_3, getSizeX(), getSizeDim3())); + _goal_coordinates = NodeSE2::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3)); + _goal->setPose(_goal_coordinates); + + NodeSE2::computeWavefrontHeuristic( + _costmap, + static_cast(getStart()->pose.x), + static_cast(getStart()->pose.y), + mx, my); +} + +template +bool AStarAlgorithm::areInputsValid() +{ + // Check if graph was filled in + if (_graph.empty()) { + throw std::runtime_error("Failed to compute path, no costmap given."); + } + + // Check if points were filled in + if (!_start || !_goal) { + throw std::runtime_error("Failed to compute path, no valid start or goal given."); + } + + // Check if ending point is valid + if (getToleranceHeuristic() < 0.001 && + !_goal->isNodeValid(_traverse_unknown, _collision_checker)) + { + throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); + } + + // Check if starting point is valid + if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { + throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan."); + } + + return true; +} + +template +bool AStarAlgorithm::createPath( + CoordinateVector & path, int & iterations, + const float & tolerance) +{ + _tolerance = tolerance * NodeT::neutral_cost; + _best_heuristic_node = {std::numeric_limits::max(), 0}; + clearQueue(); + + if (!areInputsValid()) { + return false; + } + + // 0) Add starting point to the open set + addNode(0.0, getStart()); + getStart()->setAccumulatedCost(0.0); + + // Optimization: preallocate all variables + NodePtr current_node = nullptr; + NodePtr neighbor = nullptr; + float g_cost = 0.0; + NodeVector neighbors; + int approach_iterations = 0; + NeighborIterator neighbor_iterator; + int analytic_iterations = 0; + int closest_distance = std::numeric_limits::max(); + + // Given an index, return a node ptr reference if its collision-free and valid + const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3(); + NodeGetter neighborGetter = + [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool + { + if (index < 0 || index >= max_index) { + return false; + } + + neighbor_rtn = addToGraph(index); + return true; + }; + + while (iterations < getMaxIterations() && !_queue.empty()) { + // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue + current_node = getNextNode(); + + // We allow for nodes to be queued multiple times in case + // shorter paths result in it, but we can visit only once + if (current_node->wasVisited()) { + continue; + } + + iterations++; + + // 2) Mark Nbest as visited + current_node->visited(); + + // 2.a) Use an analytic expansion (if available) to generate a path + // to the goal. + NodePtr result = tryAnalyticExpansion( + current_node, neighborGetter, analytic_iterations, + closest_distance); + if (result != nullptr) { + current_node = result; + } + + // 3) Check if we're at the goal, backtrace if required + if (isGoal(current_node)) { + return backtracePath(current_node, path); + } else if (_best_heuristic_node.first < getToleranceHeuristic()) { + // Optimization: Let us find when in tolerance and refine within reason + approach_iterations++; + if (approach_iterations > getOnApproachMaxIterations() || + iterations + 1 == getMaxIterations()) + { + NodePtr node = &_graph.at(_best_heuristic_node.second); + return backtracePath(node, path); + } + } + + // 4) Expand neighbors of Nbest not visited + neighbors.clear(); + NodeT::getNeighbors( + current_node, neighborGetter, _collision_checker, _traverse_unknown, neighbors); + + for (neighbor_iterator = neighbors.begin(); + neighbor_iterator != neighbors.end(); ++neighbor_iterator) + { + neighbor = *neighbor_iterator; + + // 4.1) Compute the cost to go to this node + g_cost = getAccumulatedCost(current_node) + getTraversalCost(current_node, neighbor); + + // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach + if (g_cost < getAccumulatedCost(neighbor)) { + neighbor->setAccumulatedCost(g_cost); + neighbor->parent = current_node; + + // 4.3) If not in queue or visited, add it, `getNeighbors()` handles + neighbor->queued(); + addNode(g_cost + getHeuristicCost(neighbor), neighbor); + } + } + } + + return false; +} + +template +bool AStarAlgorithm::isGoal(NodePtr & node) +{ + return node == getGoal(); +} + +template<> +AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + ompl::base::ScopedState<> from(node->motion_table.state_space), to( + node->motion_table.state_space), s(node->motion_table.state_space); + const NodeSE2::Coordinates & node_coords = node->pose; + from[0] = node_coords.x; + from[1] = node_coords.y; + from[2] = node_coords.theta * node->motion_table.bin_size; + to[0] = _goal_coordinates.x; + to[1] = _goal_coordinates.y; + to[2] = _goal_coordinates.theta * node->motion_table.bin_size; + + float d = node->motion_table.state_space->distance(from(), to()); + NodePtr prev(node); + // A move of sqrt(2) is guaranteed to be in a new cell + constexpr float sqrt_2 = std::sqrt(2.); + unsigned int num_intervals = std::floor(d / sqrt_2); + + using PossibleNode = std::pair; + std::vector possible_nodes; + possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal + std::vector reals; + // Pre-allocate + unsigned int index = 0; + NodePtr next(nullptr); + float angle = 0.0; + Coordinates proposed_coordinates; + // Don't generate the first point because we are already there! + // And the last point is the goal, so ignore it too! + for (float i = 1; i < num_intervals; i++) { + node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); + reals = s.reals(); + angle = reals[2] / node->motion_table.bin_size; + while (angle >= node->motion_table.num_angle_quantization_float) { + angle -= node->motion_table.num_angle_quantization_float; + } + while (angle < 0.0) { + angle += node->motion_table.num_angle_quantization_float; + } + // Turn the pose into a node, and check if it is valid + index = NodeSE2::getIndex( + static_cast(reals[0]), + static_cast(reals[1]), + static_cast(angle)); + // Get the node from the graph + if (node_getter(index, next)) { + Coordinates initial_node_coords = next->pose; + proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; + next->setPose(proposed_coordinates); + if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { + // Save the node, and its previous coordinates in case we need to abort + possible_nodes.emplace_back(next, initial_node_coords); + prev = next; + } else { + next->setPose(initial_node_coords); + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + n->setPose(node_pose.second); + } + return NodePtr(nullptr); + } + } else { + // Abort + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + n->setPose(node_pose.second); + } + return NodePtr(nullptr); + } + } + // Legitimate path - set the parent relationships - poses already set + prev = node; + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + n->parent = prev; + prev = n; + } + _goal->parent = prev; + return _goal; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + return NodePtr(nullptr); +} + +template<> +bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +{ + if (!node->parent) { + return false; + } + + NodePtr current_node = node; + + while (current_node->parent) { + path.push_back( + Node2D::getCoords( + current_node->getIndex(), getSizeX(), getSizeDim3())); + current_node = current_node->parent; + } + + return path.size() > 1; +} + +template<> +bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +{ + if (!node->parent) { + return false; + } + + NodePtr current_node = node; + + while (current_node->parent) { + path.push_back(current_node->pose); + current_node = current_node->parent; + } + + return path.size() > 1; +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() +{ + return _start; +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +{ + return _goal; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + return node.graph_node_ptr; +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + + if (!node.graph_node_ptr->wasVisited()) { + node.graph_node_ptr->pose = node.pose; + } + + return node.graph_node_ptr; +} + +template +void AStarAlgorithm::addNode(const float cost, NodePtr & node) +{ + NodeBasic queued_node(node->getIndex()); + queued_node.graph_node_ptr = node; + _queue.emplace(cost, queued_node); +} + +template<> +void AStarAlgorithm::addNode(const float cost, NodePtr & node) +{ + NodeBasic queued_node(node->getIndex()); + queued_node.pose = node->pose; + queued_node.graph_node_ptr = node; + _queue.emplace(cost, queued_node); +} + +template +float AStarAlgorithm::getTraversalCost( + NodePtr & current_node, + NodePtr & new_node) +{ + return current_node->getTraversalCost(new_node); +} + +template +float & AStarAlgorithm::getAccumulatedCost(NodePtr & node) +{ + return node->getAccumulatedCost(); +} + +template +float AStarAlgorithm::getHeuristicCost(const NodePtr & node) +{ + const Coordinates node_coords = + NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); + float heuristic = NodeT::getHeuristicCost( + node_coords, _goal_coordinates); + + if (heuristic < _best_heuristic_node.first) { + _best_heuristic_node = {heuristic, node->getIndex()}; + } + + return heuristic; +} + +template +void AStarAlgorithm::clearQueue() +{ + NodeQueue q; + std::swap(_queue, q); +} + +template +void AStarAlgorithm::clearGraph() +{ + Graph g; + g.reserve(100000); + std::swap(_graph, g); +} + +template +int & AStarAlgorithm::getMaxIterations() +{ + return _max_iterations; +} + +template +int & AStarAlgorithm::getOnApproachMaxIterations() +{ + return _max_on_approach_iterations; +} + +template +float & AStarAlgorithm::getToleranceHeuristic() +{ + return _tolerance; +} + +template +unsigned int & AStarAlgorithm::getSizeX() +{ + return _x_size; +} + +template +unsigned int & AStarAlgorithm::getSizeY() +{ + return _y_size; +} + +template +unsigned int & AStarAlgorithm::getSizeDim3() +{ + return _dim3_size; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpansion( + const NodePtr & current_node, const NodeGetter & getter, int & analytic_iterations, + int & closest_distance) +{ + if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP) { + // This must be a NodeSE2 node if we are using these motion models + + // See if we are closer and should be expanding more often + const Coordinates node_coords = + NodeT::getCoords(current_node->getIndex(), getSizeX(), getSizeDim3()); + closest_distance = + std::min( + closest_distance, + static_cast(NodeT::getHeuristicCost( + node_coords, + _goal_coordinates) / NodeT::neutral_cost) + ); + // We want to expand at a rate of d/expansion_ratio, + // but check to see if we are so close that we would be expanding every iteration + // If so, limit it to the expansion ratio (rounded up) + int desired_iterations = std::max( + static_cast(closest_distance / _search_info.analytic_expansion_ratio), + static_cast(std::ceil(_search_info.analytic_expansion_ratio)) + ); + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = + std::min(analytic_iterations, desired_iterations); + + // Always run the expansion on the first run in case there is a + // trivial path to be found + if (analytic_iterations <= 0) { + // Reset the counter, and try the analytic path expansion + analytic_iterations = desired_iterations; + return getAnalyticPath(current_node, getter); + } + analytic_iterations--; + } + // No valid motion model - return nullptr + return NodePtr(nullptr); +} + +// Instantiate algorithm for the supported template types +template class AStarAlgorithm; +template class AStarAlgorithm; + +} // namespace smac_planner diff --git a/smac_planner/src/costmap_downsampler.cpp b/smac_planner/src/costmap_downsampler.cpp new file mode 100644 index 00000000000..6955469831a --- /dev/null +++ b/smac_planner/src/costmap_downsampler.cpp @@ -0,0 +1,129 @@ +// Copyright (c) 2020, Carlos Luis +// Copyright (c) 2020, 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. Reserved. + +#include "smac_planner/costmap_downsampler.hpp" + +#include +#include +#include + +namespace smac_planner +{ + +CostmapDownsampler::CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node) +: _node(node), + _costmap(nullptr), + _downsampled_costmap(nullptr), + _downsampled_costmap_pub(nullptr) +{ +} + +CostmapDownsampler::~CostmapDownsampler() +{ +} + +void CostmapDownsampler::initialize( + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor) +{ + _topic_name = topic_name; + _costmap = costmap; + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + _downsampled_costmap = std::make_unique( + _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, + _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); + + _downsampled_costmap_pub = std::make_unique( + _node, _downsampled_costmap.get(), global_frame, _topic_name, false); +} + +nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( + const unsigned int & downsampling_factor) +{ + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + // Adjust costmap size if needed + if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || + _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || + _downsampled_costmap->getResolution() != _downsampled_resolution) + { + resizeCostmap(); + } + + // Assign costs + for (uint i = 0; i < _downsampled_size_x; ++i) { + for (uint j = 0; j < _downsampled_size_y; ++j) { + setCostOfCell(i, j); + } + } + + if (_node->count_subscribers(_topic_name) > 0) { + _downsampled_costmap_pub->publishCostmap(); + } + + return _downsampled_costmap.get(); +} + +void CostmapDownsampler::updateCostmapSize() +{ + _size_x = _costmap->getSizeInCellsX(); + _size_y = _costmap->getSizeInCellsY(); + _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); + _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); + _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); +} + +void CostmapDownsampler::resizeCostmap() +{ + _downsampled_costmap->resizeMap( + _downsampled_size_x, + _downsampled_size_y, + _downsampled_resolution, + _costmap->getOriginX(), + _costmap->getOriginY()); +} + +void CostmapDownsampler::setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my) +{ + unsigned int mx, my; + unsigned char cost = 0; + unsigned int x_offset = new_mx * _downsampling_factor; + unsigned int y_offset = new_my * _downsampling_factor; + + for (uint i = 0; i < _downsampling_factor; ++i) { + mx = x_offset + i; + if (mx >= _size_x) { + continue; + } + for (uint j = 0; j < _downsampling_factor; ++j) { + my = y_offset + j; + if (my >= _size_y) { + continue; + } + cost = std::max(cost, _costmap->getCost(mx, my)); + } + } + + _downsampled_costmap->setCost(new_mx, new_my, cost); +} + +} // namespace smac_planner diff --git a/smac_planner/src/node_2d.cpp b/smac_planner/src/node_2d.cpp new file mode 100644 index 00000000000..b6afd5b944b --- /dev/null +++ b/smac_planner/src/node_2d.cpp @@ -0,0 +1,151 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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 "smac_planner/node_2d.hpp" + +#include +#include + +namespace smac_planner +{ + +// defining static member for all instance to share +std::vector Node2D::_neighbors_grid_offsets; +double Node2D::neutral_cost = 50.0; + +Node2D::Node2D(unsigned char & cost_in, const unsigned int index) +: parent(nullptr), + _cell_cost(static_cast(cost_in)), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _is_queued(false) +{ +} + +Node2D::~Node2D() +{ + parent = nullptr; +} + +void Node2D::reset(const unsigned char & cost) +{ + parent = nullptr; + _cell_cost = static_cast(cost); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _is_queued = false; +} + +bool Node2D::isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker /*collision_checker*/) +{ + // NOTE(stevemacenski): Right now, we do not check if the node has wrapped around + // the regular grid (e.g. your node is on the edge of the costmap and i+1 + // goes to the other side). This check would add compute time and my assertion is + // that if you do wrap around, the heuristic will be so high it'll be added far + // in the queue that it will never be called if a valid path exists. + // This is intentionally un-included to increase speed, but be aware. If this causes + // trouble, please file a ticket and we can address it then. + + auto & cost = this->getCost(); + + // occupied node + if (cost == OCCUPIED || cost == INSCRIBED) { + return false; + } + + // unknown node + if (cost == UNKNOWN && !traverse_unknown) { + return false; + } + + return true; +} + +float Node2D::getTraversalCost(const NodePtr & child) +{ + // cost to travel will be the cost of the cell's code + + // neutral_cost is neutral cost for cost just to travel anywhere (50) + // 0.8 is a scale factor to remap costs [0, 252] evenly from [50, 252] + return Node2D::neutral_cost + 0.8 * child->getCost(); +} + +float Node2D::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates) +{ + return hypotf( + goal_coordinates.x - node_coords.x, + goal_coordinates.y - node_coords.y) * Node2D::neutral_cost; +} + +void Node2D::initNeighborhood( + const unsigned int & x_size_uint, + const MotionModel & neighborhood) +{ + int x_size = static_cast(x_size_uint); + switch (neighborhood) { + case MotionModel::UNKNOWN: + throw std::runtime_error("Unknown neighborhood type selected."); + case MotionModel::VON_NEUMANN: + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size}; + break; + case MotionModel::MOORE: + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, + -x_size + 1, +x_size - 1, +x_size + 1}; + break; + default: + throw std::runtime_error( + "Invalid neighborhood type selected. " + "Von-Neumann and Moore are valid for Node2D."); + } +} + +void Node2D::getNeighbors( + NodePtr & node, + std::function & NeighborGetter, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + // NOTE(stevemacenski): Irritatingly, the order here matters. If you start in free + // space and then expand 8-connected, the first set of neighbors will be all cost + // _neutral_cost. Then its expansion will all be 2 * _neutral_cost but now multiple + // nodes are touching that node so the last cell to update the back pointer wins. + // Thusly, the ordering ends with the cardinal directions for both sets such that + // behavior is consistent in large free spaces between them. + // 100 50 0 + // 100 50 50 + // 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes + // Therefore, it is valuable to have some low-potential across the entire map + // rather than a small inflation around the obstacles + int index; + NodePtr neighbor; + int node_i = node->getIndex(); + + for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) { + index = node_i + _neighbors_grid_offsets[i]; + if (NeighborGetter(index, neighbor)) { + if (neighbor->isNodeValid(traverse_unknown, collision_checker) && !neighbor->wasVisited()) { + neighbors.push_back(neighbor); + } + } + } +} + +} // namespace smac_planner diff --git a/smac_planner/src/node_se2.cpp b/smac_planner/src/node_se2.cpp new file mode 100644 index 00000000000..9764df0dbd2 --- /dev/null +++ b/smac_planner/src/node_se2.cpp @@ -0,0 +1,432 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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 + +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" + +#include "smac_planner/node_se2.hpp" + +using namespace std::chrono; // NOLINT + +namespace smac_planner +{ + +// defining static member for all instance to share +std::vector NodeSE2::_wavefront_heuristic; +double NodeSE2::neutral_cost = sqrt(2); +MotionTable NodeSE2::motion_table; + +// Each of these tables are the projected motion models through +// time and space applied to the search on the current node in +// continuous map-coordinates (e.g. not meters but partial map cells) +// Currently, these are set to project *at minimum* into a neighboring +// cell. Though this could be later modified to project a certain +// amount of time or particular distance forward. + +// http://planning.cs.uiuc.edu/node821.html +// Model for ackermann style vehicle with minimum radius restriction +void MotionTable::initDubin( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + + // angle must meet 3 requirements: + // 1) be increment of quantized bin size + // 2) chord length must be greater than sqrt(2) to leave current cell + // 3) maximum curvature must be respected, represented by minimum turning angle + // Thusly: + // On circle of radius minimum turning angle, we need select motion primatives + // with chord length > sqrt(2) and be an increment of our bin size + // + // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size + // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) + float angle = 2.0 * asin(sqrt(2.0) / (2 * search_info.minimum_turning_radius)); + // Now make sure angle is an increment of the quantized bin size + // And since its based on the minimum chord, we need to make sure its always larger + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + // Search dimensions are clean multiples of quantization - this prevents + // paths with loops in them + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + // find deflections + // If we make a right triangle out of the chord in circle of radius + // min turning angle, we can see that delta X = R * sin (angle) + float delta_x = search_info.minimum_turning_radius * sin(angle); + // Using that same right triangle, we can see that the complement + // to delta Y is R * cos (angle). If we subtract R, we get the actual value + float delta_y = search_info.minimum_turning_radius - + (search_info.minimum_turning_radius * cos(angle)); + + projections.clear(); + projections.reserve(3); + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Left + projections.emplace_back(delta_x, -delta_y, -increments); // Right + + // Create the correct OMPL state space + state_space = std::make_unique(search_info.minimum_turning_radius); +} + +// http://planning.cs.uiuc.edu/node822.html +// Same as Dubin model but now reverse is valid +// See notes in Dubin for explanation +void MotionTable::initReedsShepp( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + + float angle = 2.0 * asin(sqrt(2.0) / (2 * search_info.minimum_turning_radius)); + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + float delta_x = search_info.minimum_turning_radius * sin(angle); + float delta_y = search_info.minimum_turning_radius - + (search_info.minimum_turning_radius * cos(angle)); + + projections.clear(); + projections.reserve(6); + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Forward + Left + projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right + projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0); // Backward + projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left + projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right + + // Create the correct OMPL state space + state_space = std::make_unique( + search_info.minimum_turning_radius); +} + +MotionPoses MotionTable::getProjections(const NodeSE2 * node) +{ + MotionPoses projection_list; + for (unsigned int i = 0; i != projections.size(); i++) { + projection_list.push_back(getProjection(node, i)); + } + + return projection_list; +} + +MotionPose MotionTable::getProjection(const NodeSE2 * node, const unsigned int & motion_index) +{ + const MotionPose & motion_model = projections[motion_index]; + + // transform delta X, Y, and Theta into local coordinates + const float & node_heading = node->pose.theta; + const float cos_theta = cos(node_heading * bin_size); // needs actual angle [0, 2PI] + const float sin_theta = sin(node_heading * bin_size); + const float delta_x = motion_model._x * cos_theta - motion_model._y * sin_theta; + const float delta_y = motion_model._x * sin_theta + motion_model._y * cos_theta; + float new_heading = node_heading + motion_model._theta; + + // normalize theta + while (new_heading >= num_angle_quantization_float) { + new_heading -= num_angle_quantization_float; + } + while (new_heading < 0.0) { + new_heading += num_angle_quantization_float; + } + + return MotionPose(delta_x + node->pose.x, delta_y + node->pose.y, new_heading); +} + +NodeSE2::NodeSE2(const unsigned int index) +: parent(nullptr), + pose(0.0f, 0.0f, 0.0f), + _cell_cost(std::numeric_limits::quiet_NaN()), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _is_queued(false), + _motion_primitive_index(std::numeric_limits::max()) +{ +} + +NodeSE2::~NodeSE2() +{ + parent = nullptr; +} + +void NodeSE2::reset() +{ + parent = nullptr; + _cell_cost = std::numeric_limits::quiet_NaN(); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _is_queued = false; + _motion_primitive_index = std::numeric_limits::max(); + pose.x = 0.0f; + pose.y = 0.0f; + pose.theta = 0.0f; +} + +bool NodeSE2::isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker) +{ + if (collision_checker.inCollision( + this->pose.x, this->pose.y, this->pose.theta * motion_table.bin_size, traverse_unknown)) + { + return false; + } + + _cell_cost = collision_checker.getCost(); + return true; +} + +float NodeSE2::getTraversalCost(const NodePtr & child) +{ + const float normalized_cost = child->getCost() / 252.0; + if (std::isnan(normalized_cost)) { + throw std::runtime_error( + "Node attempted to get traversal " + "cost without a known SE2 collision cost!"); + } + + // this is the first node + if (getMotionPrimitiveIndex() == std::numeric_limits::max()) { + return NodeSE2::neutral_cost; + } + + float travel_cost = 0.0; + float travel_cost_raw = NodeSE2::neutral_cost + motion_table.cost_penalty * normalized_cost; + + if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) { + // straight motion, no additional costs to be applied + travel_cost = travel_cost_raw; + } else { + if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) { + // Turning motion but keeps in same direction: encourages to commit to turning if starting it + travel_cost = travel_cost_raw * motion_table.non_straight_penalty; + } else { + // Turning motion and changing direction: penalizes wiggling + travel_cost = travel_cost_raw * motion_table.change_penalty; + travel_cost += travel_cost_raw * motion_table.non_straight_penalty; + } + } + + if (getMotionPrimitiveIndex() > 2) { + // reverse direction + travel_cost *= motion_table.reverse_penalty; + } + + return travel_cost; +} + +float NodeSE2::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coords) +{ + // Dubin or Reeds-Shepp shortest distances + // Create OMPL states for checking + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + from[0] = node_coords.x; + from[1] = node_coords.y; + from[2] = node_coords.theta * motion_table.bin_size; + to[0] = goal_coords.x; + to[1] = goal_coords.y; + to[2] = goal_coords.theta * motion_table.bin_size; + + const float motion_heuristic = motion_table.state_space->distance(from(), to()); + + const unsigned int & wavefront_idx = static_cast(node_coords.y) * + motion_table.size_x + static_cast(node_coords.x); + const unsigned int & wavefront_value = _wavefront_heuristic[wavefront_idx]; + + // if lethal or didn't visit, use the motion heuristic instead. + if (wavefront_value == 0) { + return NodeSE2::neutral_cost * motion_heuristic; + } + + // -2 because wavefront starts at 2 + const float wavefront_heuristic = static_cast(wavefront_value - 2); + + return NodeSE2::neutral_cost * std::max(wavefront_heuristic, motion_heuristic); +} + +void NodeSE2::initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & num_angle_quantization, + SearchInfo & search_info) +{ + // find the motion model selected + switch (motion_model) { + case MotionModel::DUBIN: + motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); + break; + case MotionModel::REEDS_SHEPP: + motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); + break; + default: + throw std::runtime_error( + "Invalid motion model for SE2 node. Please select between" + " Dubin (Ackermann forward only)," + " Reeds-Shepp (Ackermann forward and back)."); + } +} + +void NodeSE2::computeWavefrontHeuristic( + nav2_costmap_2d::Costmap2D * & costmap, + const unsigned int & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y) +{ + unsigned int size = costmap->getSizeInCellsX() * costmap->getSizeInCellsY(); + if (_wavefront_heuristic.size() == size) { + // must reset all values + for (unsigned int i = 0; i != _wavefront_heuristic.size(); i++) { + _wavefront_heuristic[i] = 0; + } + } else { + unsigned int wavefront_size = _wavefront_heuristic.size(); + _wavefront_heuristic.resize(size, 0); + // must reset values for non-constructed indices + for (unsigned int i = 0; i != wavefront_size; i++) { + _wavefront_heuristic[i] = 0; + } + } + + const unsigned int & size_x = motion_table.size_x; + const int size_x_int = static_cast(size_x); + const unsigned int size_y = costmap->getSizeInCellsY(); + const unsigned int goal_index = goal_y * size_x + goal_x; + const unsigned int start_index = start_y * size_x + start_x; + unsigned int mx, my, mx_idx, my_idx; + + std::queue q; + q.emplace(goal_index); + + unsigned int idx = goal_index; + _wavefront_heuristic[idx] = 2; + + static const std::vector neighborhood = {1, -1, // left right + size_x_int, -size_x_int, // up down + size_x_int + 1, size_x_int - 1, // upper diagonals + -size_x_int + 1, -size_x_int - 1}; // lower diagonals + + while (!q.empty() || idx == start_index) { + // get next one + idx = q.front(); + q.pop(); + + my_idx = idx / size_x; + mx_idx = idx - (my_idx * size_x); + + // find neighbors + for (unsigned int i = 0; i != neighborhood.size(); i++) { + unsigned int new_idx = static_cast(static_cast(idx) + neighborhood[i]); + unsigned int last_wave_cost = _wavefront_heuristic[idx]; + + // if neighbor is unvisited and non-lethal, set N and add to queue + if (new_idx > 0 && new_idx < size_x * size_y && + _wavefront_heuristic[new_idx] == 0 && + static_cast(costmap->getCost(idx)) < INSCRIBED) + { + my = new_idx / size_x; + mx = new_idx - (my * size_x); + + if (mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0) { + continue; + } + if (my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0) { + continue; + } + + _wavefront_heuristic[new_idx] = last_wave_cost + 1; + q.emplace(idx + neighborhood[i]); + } + } + } +} + +void NodeSE2::getNeighbors( + const NodePtr & node, + std::function & NeighborGetter, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + unsigned int index = 0; + NodePtr neighbor = nullptr; + Coordinates initial_node_coords; + const MotionPoses motion_projections = motion_table.getProjections(node); + + for (unsigned int i = 0; i != motion_projections.size(); i++) { + index = NodeSE2::getIndex( + static_cast(motion_projections[i]._x), + static_cast(motion_projections[i]._y), + static_cast(motion_projections[i]._theta), + motion_table.size_x, motion_table.num_angle_quantization); + + if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { + // Cache the initial pose in case it was visited but valid + // don't want to disrupt continuous coordinate expansion + initial_node_coords = neighbor->pose; + neighbor->setPose( + Coordinates( + motion_projections[i]._x, + motion_projections[i]._y, + motion_projections[i]._theta)); + if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { + neighbor->setMotionPrimitiveIndex(i); + neighbors.push_back(neighbor); + } else { + neighbor->setPose(initial_node_coords); + } + } + } +} + +} // namespace smac_planner diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp new file mode 100644 index 00000000000..f3db10830ad --- /dev/null +++ b/smac_planner/src/smac_planner.cpp @@ -0,0 +1,385 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "smac_planner/smac_planner.hpp" + +#define BENCHMARK_TESTING + +namespace smac_planner +{ + +using namespace std::chrono; // NOLINT + +SmacPlanner::SmacPlanner() +: _a_star(nullptr), + _smoother(nullptr), + _node(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr) +{ +} + +SmacPlanner::~SmacPlanner() +{ + RCLCPP_INFO( + _node->get_logger(), "Destroying plugin %s of type SmacPlanner", + _name.c_str()); +} + +void SmacPlanner::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _node = parent; + _costmap = costmap_ros->getCostmap(); + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + + bool allow_unknown; + int max_iterations; + int max_on_approach_iterations = std::numeric_limits::max(); + int angle_quantizations; + SearchInfo search_info; + bool smooth_path; + std::string motion_model_for_search; + + // General planner params + nav2_util::declare_parameter_if_not_declared( + _node, name + ".tolerance", rclcpp::ParameterValue(0.125)); + _tolerance = static_cast(_node->get_parameter(name + ".tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); + _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + _node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".angle_quantization_bins", rclcpp::ParameterValue(1)); + _node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); + _angle_bin_size = 2.0 * M_PI / angle_quantizations; + _angle_quantizations = static_cast(angle_quantizations); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".allow_unknown", rclcpp::ParameterValue(true)); + _node->get_parameter(name + ".allow_unknown", allow_unknown); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + _node->get_parameter(name + ".max_iterations", max_iterations); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".smooth_path", rclcpp::ParameterValue(false)); + _node->get_parameter(name + ".smooth_path", smooth_path); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + _node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); + _node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".change_penalty", rclcpp::ParameterValue(0.5)); + _node->get_parameter(name + ".change_penalty", search_info.change_penalty); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05)); + _node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); + _node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); + _node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0)); + _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); + _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + MotionModel motion_model = fromString(motion_model_for_search); + if (motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _node->get_logger(), + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + motion_model_for_search.c_str()); + } + + if (max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _node->get_logger(), "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + max_on_approach_iterations = std::numeric_limits::max(); + } + + if (max_iterations <= 0) { + RCLCPP_INFO( + _node->get_logger(), "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + max_iterations = std::numeric_limits::max(); + } + + // convert to grid coordinates + const double minimum_turning_radius_global_coords = search_info.minimum_turning_radius; + search_info.minimum_turning_radius = + search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); + + _a_star = std::make_unique>(motion_model, search_info); + _a_star->initialize( + allow_unknown, + max_iterations, + max_on_approach_iterations); + _a_star->setFootprint(costmap_ros->getRobotFootprint(), costmap_ros->getUseRadius()); + + if (smooth_path) { + _smoother = std::make_unique(); + _optimizer_params.get(_node.get(), name); + _smoother_params.get(_node.get(), name); + _smoother_params.max_curvature = 1.0f / minimum_turning_radius_global_coords; + _smoother->initialize(_optimizer_params); + } + + if (_downsample_costmap && _downsampling_factor > 1) { + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(_node); + _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + } + + _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO( + _node->get_logger(), "Configured plugin %s of type SmacPlanner with " + "tolerance %.2f, maximum iterations %i, " + "max on approach iterations %i, and %s. Using motion model: %s.", + _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, + allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(motion_model).c_str()); +} + +void SmacPlanner::activate() +{ + RCLCPP_INFO( + _node->get_logger(), "Activating plugin %s of type SmacPlanner", + _name.c_str()); + _raw_plan_publisher->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->activatePublisher(); + } +} + +void SmacPlanner::deactivate() +{ + RCLCPP_INFO( + _node->get_logger(), "Deactivating plugin %s of type SmacPlanner", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->deactivatePublisher(); + } +} + +void SmacPlanner::cleanup() +{ + RCLCPP_INFO( + _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + _costmap_downsampler.reset(); + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlanner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + } + + // Set Costmap + _a_star->createGraph( + costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), + _angle_quantizations, + costmap); + + // Set starting point, in A* bin search coordinates + unsigned int mx, my; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; + while (orientation_bin < 0.0) { + orientation_bin += static_cast(_angle_quantizations); + } + unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); + _a_star->setStart(mx, my, orientation_bin_id); + + // Set goal point, in A* bin search coordinates + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; + while (orientation_bin < 0.0) { + orientation_bin += static_cast(_angle_quantizations); + } + orientation_bin_id = static_cast(floor(orientation_bin)); + _a_star->setGoal(mx, my, orientation_bin_id); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _node->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // Compute plan + NodeSE2::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _node->get_logger(), + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; + } + + // Convert to world coordinates and downsample path for smoothing if necesssary + // We're going to downsample by 4x to give terms room to move. + const int downsample_ratio = 4; + std::vector path_world; + path_world.reserve(path.size()); + plan.poses.reserve(path.size()); + + for (int i = path.size() - 1; i >= 0; --i) { + path_world.push_back(getWorldCoords(path[i].x, path[i].y, costmap)); + pose.pose.position.x = path_world.back().x(); + pose.pose.position.y = path_world.back().y(); + pose.pose.orientation = getWorldOrientation(path[i].theta); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) { + _raw_plan_publisher->publish(plan); + } + + // If not smoothing or too short to smooth, return path + if (!_smoother || path_world.size() < 4) { +#ifdef BENCHMARK_TESTING + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + return plan; + } + + // Find how much time we have left to do smoothing + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + double time_remaining = _max_planning_time - static_cast(time_span.count()); + _smoother_params.max_time = std::min(time_remaining, _optimizer_params.max_time); + + // Smooth plan + if (!_smoother->smooth(path_world, costmap, _smoother_params)) { + RCLCPP_WARN( + _node->get_logger(), + "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", + _name.c_str()); + return plan; + } + + removeHook(path_world); + + // populate final path + // TODO(stevemacenski): set orientation to tangent of path + for (uint i = 0; i != path_world.size(); i++) { + pose.pose.position.x = path_world[i][0]; + pose.pose.position.y = path_world[i][1]; + plan.poses[i] = pose; + } + + return plan; +} + +void SmacPlanner::removeHook(std::vector & path) +{ + // Removes the end "hooking" since goal is locked in place + Eigen::Vector2d interpolated_second_to_last_point; + interpolated_second_to_last_point = (path.end()[-3] + path.end()[-1]) / 2.0; + if ( + squaredDistance(path.end()[-2], path.end()[-1]) > + squaredDistance(interpolated_second_to_last_point, path.end()[-1])) + { + path.end()[-2] = interpolated_second_to_last_point; + } +} + +Eigen::Vector2d SmacPlanner::getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) +{ + // mx, my are in continuous grid coordinates, must convert to world coordinates + double world_x = + static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); + double world_y = + static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); + return Eigen::Vector2d(world_x, world_y); +} + +geometry_msgs::msg::Quaternion SmacPlanner::getWorldOrientation(const float & theta) +{ + // theta is in continuous bin coordinates, must convert to world orientation + tf2::Quaternion q; + q.setEuler(0.0, 0.0, theta * static_cast(_angle_bin_size)); + return tf2::toMsg(q); +} + +} // namespace smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner, nav2_core::GlobalPlanner) diff --git a/smac_planner/src/smac_planner_2d.cpp b/smac_planner/src/smac_planner_2d.cpp new file mode 100644 index 00000000000..3c4b8ca83a1 --- /dev/null +++ b/smac_planner/src/smac_planner_2d.cpp @@ -0,0 +1,339 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include +#include +#include + +#include "smac_planner/smac_planner_2d.hpp" + +// #define BENCHMARK_TESTING + +namespace smac_planner +{ +using namespace std::chrono; // NOLINT + +SmacPlanner2D::SmacPlanner2D() +: _a_star(nullptr), + _smoother(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr), + _node(nullptr) +{ +} + +SmacPlanner2D::~SmacPlanner2D() +{ + RCLCPP_INFO( + _node->get_logger(), "Destroying plugin %s of type SmacPlanner2D", + _name.c_str()); +} + +void SmacPlanner2D::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _node = parent; + _costmap = costmap_ros->getCostmap(); + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + + bool allow_unknown; + int max_iterations; + int max_on_approach_iterations; + bool smooth_path; + double minimum_turning_radius; + std::string motion_model_for_search; + + // General planner params + nav2_util::declare_parameter_if_not_declared( + _node, name + ".tolerance", rclcpp::ParameterValue(0.125)); + _tolerance = static_cast(_node->get_parameter(name + ".tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); + _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + _node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".allow_unknown", rclcpp::ParameterValue(true)); + _node->get_parameter(name + ".allow_unknown", allow_unknown); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + _node->get_parameter(name + ".max_iterations", max_iterations); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); + _node->get_parameter(name + ".max_on_approach_iterations", max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".smooth_path", rclcpp::ParameterValue(false)); + _node->get_parameter(name + ".smooth_path", smooth_path); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + _node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(1000.0)); + _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + + nav2_util::declare_parameter_if_not_declared( + _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); + _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + MotionModel motion_model = fromString(motion_model_for_search); + if (motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _node->get_logger(), + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + motion_model_for_search.c_str()); + } + + if (max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _node->get_logger(), "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + max_on_approach_iterations = std::numeric_limits::max(); + } + + if (max_iterations <= 0) { + RCLCPP_INFO( + _node->get_logger(), "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + max_iterations = std::numeric_limits::max(); + } + + _a_star = std::make_unique>(motion_model, SearchInfo()); + _a_star->initialize( + allow_unknown, + max_iterations, + max_on_approach_iterations); + + if (smooth_path) { + _smoother = std::make_unique(); + _optimizer_params.get(_node.get(), name); + _smoother_params.get(_node.get(), name); + _smoother_params.max_curvature = 1.0f / minimum_turning_radius; + _smoother->initialize(_optimizer_params); + } + + if (_downsample_costmap && _downsampling_factor > 1) { + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(_node); + _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + } + + _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO( + _node->get_logger(), "Configured plugin %s of type SmacPlanner2D with " + "tolerance %.2f, maximum iterations %i, " + "max on approach iterations %i, and %s. Using motion model: %s.", + _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, + allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(motion_model).c_str()); +} + +void SmacPlanner2D::activate() +{ + RCLCPP_INFO( + _node->get_logger(), "Activating plugin %s of type SmacPlanner2D", + _name.c_str()); + _raw_plan_publisher->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->activatePublisher(); + } +} + +void SmacPlanner2D::deactivate() +{ + RCLCPP_INFO( + _node->get_logger(), "Deactivating plugin %s of type SmacPlanner2D", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->deactivatePublisher(); + } +} + +void SmacPlanner2D::cleanup() +{ + RCLCPP_INFO( + _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner2D", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + _costmap_downsampler.reset(); + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlanner2D::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + } + + // Set Costmap + _a_star->createGraph( + costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), + 1, + costmap); + + // Set starting point + unsigned int mx, my; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + _a_star->setStart(mx, my, 0); + + // Set goal point + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + _a_star->setGoal(mx, my, 0); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _node->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // Compute plan + Node2D::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _node->get_logger(), + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; + } + + // Convert to world coordinates and downsample path for smoothing if necesssary + // We're going to downsample by 4x to give terms room to move. + const int downsample_ratio = 4; + std::vector path_world; + path_world.reserve(_smoother ? path.size() / downsample_ratio : path.size()); + plan.poses.reserve(_smoother ? path.size() / downsample_ratio : path.size()); + + for (int i = path.size() - 1; i >= 0; --i) { + if (_smoother && i % downsample_ratio != 0) { + continue; + } + + path_world.push_back(getWorldCoords(path[i].x, path[i].y, costmap)); + pose.pose.position.x = path_world.back().x(); + pose.pose.position.y = path_world.back().y(); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) { + _raw_plan_publisher->publish(plan); + } + + // If not smoothing or too short to smooth, return path + if (!_smoother || path_world.size() < 4) { +#ifdef BENCHMARK_TESTING + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + return plan; + } + + // Find how much time we have left to do smoothing + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + double time_remaining = _max_planning_time - static_cast(time_span.count()); + _smoother_params.max_time = std::min(time_remaining, _optimizer_params.max_time); + + // Smooth plan + if (!_smoother->smooth(path_world, costmap, _smoother_params)) { + RCLCPP_WARN( + _node->get_logger(), + "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", + _name.c_str()); + return plan; + } + + removeHook(path_world); + + // populate final path + for (uint i = 0; i != path_world.size(); i++) { + pose.pose.position.x = path_world[i][0]; + pose.pose.position.y = path_world[i][1]; + plan.poses[i] = pose; + } + + return plan; +} + +void SmacPlanner2D::removeHook(std::vector & path) +{ + // Removes the end "hooking" since goal is locked in place + Eigen::Vector2d interpolated_second_to_last_point; + interpolated_second_to_last_point = (path.end()[-3] + path.end()[-1]) / 2.0; + if ( + squaredDistance(path.end()[-2], path.end()[-1]) > + squaredDistance(interpolated_second_to_last_point, path.end()[-1])) + { + path.end()[-2] = interpolated_second_to_last_point; + } +} + +Eigen::Vector2d SmacPlanner2D::getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) +{ + float world_x = + static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); + float world_y = + static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); + return Eigen::Vector2d(world_x, world_y); +} + +} // namespace smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner) diff --git a/smac_planner/test/CMakeLists.txt b/smac_planner/test/CMakeLists.txt new file mode 100644 index 00000000000..c5ff757d1c8 --- /dev/null +++ b/smac_planner/test/CMakeLists.txt @@ -0,0 +1,98 @@ +# Test costmap downsampler +ament_add_gtest(test_costmap_downsampler + test_costmap_downsampler.cpp +) +ament_target_dependencies(test_costmap_downsampler + ${dependencies} +) +target_link_libraries(test_costmap_downsampler + ${library_name} +) + +# Test Node2D +ament_add_gtest(test_node2d + test_node2d.cpp +) +ament_target_dependencies(test_node2d + ${dependencies} +) +target_link_libraries(test_node2d + ${library_name} +) + +# Test NodeSE2 +ament_add_gtest(test_nodese2 + test_nodese2.cpp +) +ament_target_dependencies(test_nodese2 + ${dependencies} +) +target_link_libraries(test_nodese2 + ${library_name} +) + +# Test NodeBasic +ament_add_gtest(test_nodebasic + test_nodebasic.cpp +) +ament_target_dependencies(test_nodebasic + ${dependencies} +) +target_link_libraries(test_nodebasic + ${library_name} +) + +# Test collision checker +ament_add_gtest(test_collision_checker + test_collision_checker.cpp +) +ament_target_dependencies(test_collision_checker + ${dependencies} +) +target_link_libraries(test_collision_checker + ${library_name} +) + +# Test A* +ament_add_gtest(test_a_star + test_a_star.cpp +) +ament_target_dependencies(test_a_star + ${dependencies} +) +target_link_libraries(test_a_star + ${library_name} +) + +# Test SMAC SE2 +ament_add_gtest(test_smac_se2 + test_smac_se2.cpp +) +ament_target_dependencies(test_smac_se2 + ${dependencies} +) +target_link_libraries(test_smac_se2 + ${library_name} +) + +# Test SMAC 2D +ament_add_gtest(test_smac_2d + test_smac_2d.cpp +) +ament_target_dependencies(test_smac_2d + ${dependencies} +) +target_link_libraries(test_smac_2d + ${library_name}_2d +) + +# Test smoother +ament_add_gtest(test_smoother + test_smoother.cpp +) +ament_target_dependencies(test_smoother + ${dependencies} +) +target_link_libraries(test_smoother + ${library_name}_2d +) diff --git a/smac_planner/test/deprecated_upsampler/upsampler.hpp b/smac_planner/test/deprecated_upsampler/upsampler.hpp new file mode 100644 index 00000000000..8bfc36c00e5 --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler.hpp @@ -0,0 +1,213 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "smac_planner/types.hpp" +#include "smac_planner/upsampler_cost_function.hpp" +#include "smac_planner/upsampler_cost_function_nlls.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace smac_planner +{ + +/** + * @class smac_planner::Upsampler + * @brief A Conjugate Gradient 2D path upsampler implementation + */ +class Upsampler +{ +public: + /** + * @brief A constructor for smac_planner::Upsampler + */ + Upsampler() {} + + /** + * @brief A destructor for smac_planner::Upsampler + */ + ~Upsampler() {} + + /** + * @brief Initialization of the Upsampler + */ + void initialize(const OptimizerParams params) + { + _debug = params.debug; + + // General Params + + // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT + _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT; + _options.line_search_type = ceres::WOLFE; + _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE; + _options.line_search_interpolation_type = ceres::CUBIC; + + _options.max_num_iterations = params.max_iterations; // 5000 + _options.max_solver_time_in_seconds = params.max_time; // 5.0; // TODO + + _options.function_tolerance = params.fn_tol; + _options.gradient_tolerance = params.gradient_tol; + _options.parameter_tolerance = params.param_tol; // 1e-20; + + _options.min_line_search_step_size = params.advanced.min_line_search_step_size; // 1e-30; + _options.max_num_line_search_step_size_iterations = + params.advanced.max_num_line_search_step_size_iterations; + _options.line_search_sufficient_function_decrease = + params.advanced.line_search_sufficient_function_decrease; // 1e-30; + _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction; + _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction; + _options.max_num_line_search_direction_restarts = + params.advanced.max_num_line_search_direction_restarts; + _options.line_search_sufficient_curvature_decrease = + params.advanced.line_search_sufficient_curvature_decrease; + _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion; + + if (_debug) { + _options.minimizer_progress_to_stdout = true; + } else { + _options.logging_type = ceres::SILENT; + } + } + + /** + * @brief Upsampling method + * @param path Reference to path + * @param upsample parameters weights + * @param upsample_ratio upsample ratio + * @return If Upsampler was successful + */ + bool upsample( + std::vector & path, + const SmootherParams & params, + const int & upsample_ratio) + { + _options.max_solver_time_in_seconds = params.max_time; + + if (upsample_ratio != 2 && upsample_ratio != 4) { + // invalid inputs + return false; + } + + const int param_ratio = upsample_ratio * 2.0; + const int total_size = 2 * (path.size() * upsample_ratio - upsample_ratio + 1); + double parameters[total_size]; // NOLINT + + // 20-4hz regularly, but dosnt work in faster cases + // Linearly distribute initial poses for optimization + // TODO(stevemacenski) generalize for 2x and 4x + unsigned int next_pt; + Eigen::Vector2d interpolated; + std::vector temp_path; + for (unsigned int pt = 0; pt != path.size() - 1; pt++) { + next_pt = pt + 1; + interpolated = (path[next_pt] + path[pt]) / 2.0; + + parameters[param_ratio * pt] = path[pt][0]; + parameters[param_ratio * pt + 1] = path[pt][1]; + temp_path.push_back(path[pt]); + + parameters[param_ratio * pt + 2] = interpolated[0]; + parameters[param_ratio * pt + 3] = interpolated[1]; + temp_path.push_back(interpolated); + } + + parameters[total_size - 2] = path.back()[0]; + parameters[total_size - 1] = path.back()[1]; + temp_path.push_back(path.back()); + + // Solve the upsampling problem + ceres::GradientProblemSolver::Summary summary; + ceres::GradientProblem problem(new UpsamplerCostFunction(temp_path, params, upsample_ratio)); + ceres::Solve(_options, problem, parameters, &summary); + + + path.resize(total_size / 2); + for (int i = 0; i != total_size / 2; i++) { + path[i][0] = parameters[2 * i]; + path[i][1] = parameters[2 * i + 1]; + } + + // 10-15 hz, regularly + // std::vector path_double_sampled; + // for (int i = 0; i != path.size() - 1; i++) { // last term should not be upsampled + // path_double_sampled.push_back(path[i]); + // path_double_sampled.push_back((path[i+1] + path[i]) / 2); + // } + + // std::unique_ptr problem = std::make_unique(); + // for (uint i = 1; i != path_double_sampled.size() - 1; i++) { + // ceres::CostFunction * cost_fn = + // new UpsamplerConstrainedCostFunction(path_double_sampled, params, 2, i); + // problem->AddResidualBlock( + // cost_fn, nullptr, &path_double_sampled[i][0], &path_double_sampled[i][1]); + // // locking initial coordinates unnecessary since there's no update between terms in NLLS + // } + + // ceres::Solver::Summary summary; + // _options.minimizer_type = ceres::LINE_SEARCH; + // ceres::Solve(_options, problem.get(), &summary); + + // if (upsample_ratio == 4) { + // std::vector path_quad_sampled; + // for (int i = 0; i != path_double_sampled.size() - 1; i++) { + // path_quad_sampled.push_back(path_double_sampled[i]); + // path_quad_sampled.push_back((path_double_sampled[i+1] + path_double_sampled[i]) / 2.0); + // } + + // std::unique_ptr problem2 = std::make_unique(); + // for (uint i = 1; i != path_quad_sampled.size() - 1; i++) { + // ceres::CostFunction * cost_fn = + // new UpsamplerConstrainedCostFunction(path_quad_sampled, params, 4, i); + // problem2->AddResidualBlock( + // cost_fn, nullptr, &path_quad_sampled[i][0], &path_quad_sampled[i][1]); + // } + + // ceres::Solve(_options, problem2.get(), &summary); + + // path = path_quad_sampled; + // } else { + // path = path_double_sampled; + // } + + if (_debug) { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { + return false; + } + + return true; + } + +private: + bool _debug; + ceres::GradientProblemSolver::Options _options; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp b/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp new file mode 100644 index 00000000000..cf84acf6234 --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp @@ -0,0 +1,366 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ +/** + * @struct smac_planner::UpsamplerCostFunction + * @brief Cost function for path upsampling with multiple terms using unconstrained + * optimization including curvature, smoothness, collision, and avoid obstacles. + */ +class UpsamplerCostFunction : public ceres::FirstOrderFunction +{ +public: + /** + * @brief A constructor for smac_planner::UpsamplerCostFunction + * @param num_points Number of path points to consider + */ + UpsamplerCostFunction( + const std::vector & path, + const SmootherParams & params, + const int & upsample_ratio) + : _num_params(2 * path.size()), + _params(params), + _upsample_ratio(upsample_ratio), + _path(path) + { + } + // TODO(stevemacenski) removed upsample_ratio because temp upsampling on path size + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = false; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0, 0}; + Eigen::Vector2d delta_xi_p{0, 0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + virtual bool Evaluate( + const double * parameters, + double * cost, + double * gradient) const + { + Eigen::Vector2d xi; + Eigen::Vector2d xi_p1; + Eigen::Vector2d xi_m1; + uint x_index, y_index; + cost[0] = 0.0; + double cost_raw = 0.0; + double grad_x_raw = 0.0; + double grad_y_raw = 0.0; + + // cache some computations between the residual and jacobian + CurvatureComputations curvature_params; + + for (int i = 0; i != NumParameters() / 2; i++) { + x_index = 2 * i; + y_index = 2 * i + 1; + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + if (i < 1 || i >= NumParameters() / 2 - 1) { + continue; + } + + // if original point's neighbors TODO + if (i % _upsample_ratio == 1) { + continue; + } + + xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]); + + // TODO(stevemacenski): from deep copy to make sure no feedback _path + xi_p1 = _path.at(i + 1); + xi_m1 = _path.at(i - 1); + // xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]); + // xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]); + + // compute cost + addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw); + + if (gradient != NULL) { + // compute gradient + addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw); + + gradient[x_index] = grad_x_raw; + gradient[y_index] = grad_y_raw; + } + } + + cost[0] = cost_raw; + return true; + } + + /** + * @brief Get number of parameter blocks + * @return Number of parameters in cost function + */ + virtual int NumParameters() const {return _num_params;} + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Get path curvature information + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + */ + inline void getCurvatureParams( + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * + _params.max_curvature; + // TODO(stevemacenski) is use of upsample_ratio correct here? small number? + // TODO(stevemacenski) can remove the subtraction with a + // lower weight value, does have direction issue, maybe just tuning? + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + } + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + getCurvatureParams(pt, pt_p, pt_m, curvature_params); + + if (!curvature_params.isValid()) { + return; + } + + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + j0 += weight * jacobian[0]; // xi y component of partial-derivative + j1 += weight * jacobian[1]; // xi x component of partial-derivative + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + } + + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + int _num_params; + SmootherParams _params; + int _upsample_ratio; + std::vector _path; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp b/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp new file mode 100644 index 00000000000..a6fd595f969 --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp @@ -0,0 +1,334 @@ +// Copyright (c) 2020, 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. Reserved. + +#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ +/** + * @struct smac_planner::UpsamplerConstrainedCostFunction + * @brief Cost function for path upsampling with multiple terms using NLLS + * including curvature, smoothness, collision, and avoid obstacles. + */ +class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1> +{ +public: + /** + * @brief A constructor for smac_planner::UpsamplerConstrainedCostFunction + * @param num_points Number of path points to consider + */ + UpsamplerConstrainedCostFunction( + const std::vector & path, + const SmootherParams & params, + const int & upsample_ratio, + const int & i) + : _path(path), + _params(params), + _upsample_ratio(upsample_ratio), + index(i) + { + } + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = true; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0, 0}; + Eigen::Vector2d delta_xi_p{0, 0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + + bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const override + { + Eigen::Vector2d xi = Eigen::Vector2d(parameters[0][0], parameters[1][0]); + Eigen::Vector2d xi_p1 = _path.at(index + 1); + Eigen::Vector2d xi_m1 = _path.at(index - 1); + CurvatureComputations curvature_params; + double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0; + + // compute cost + addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw); + + residuals[0] = 0; + residuals[0] = cost_raw; // objective function value x + + if (jacobians != NULL && jacobians[0] != NULL) { + addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw); + + jacobians[0][0] = 0; + jacobians[1][0] = 0; + jacobians[0][0] = grad_x_raw; // x derivative + jacobians[1][0] = grad_y_raw; // y derivative + jacobians[0][1] = 0.0; + jacobians[1][1] = 0.0; + } + + return true; + } + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Get path curvature information + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + */ + inline void getCurvatureParams( + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * + _params.max_curvature; + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + } + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + getCurvatureParams(pt, pt_p, pt_m, curvature_params); + + if (!curvature_params.isValid()) { + return; + } + + // objective function value + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + j0 += weight * jacobian[0]; // xi x component of partial-derivative + j1 += weight * jacobian[1]; // xi y component of partial-derivative + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + } + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + std::vector _path; + SmootherParams _params; + int _upsample_ratio; + int index; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ diff --git a/smac_planner/test/path.png b/smac_planner/test/path.png new file mode 100644 index 0000000000000000000000000000000000000000..fc98e7709b16eae856c3ed685714529f9e3c96e7 GIT binary patch literal 122370 zcmYg%Wmr_-7w%9B3v(Gwvuf5`3v7@y$m7m~}c7o7T%B-XSWG>+p{YzsvA0I!rr6`$C<=TeHZ8Ftk~qbnMR8e?H?$Y}gkYJW_%CPH zuZ23mNG z4ls%_o~hmoAR`BIqKIb#F;kfp>==ca4_tPji&CMu2Z~?d!%B53) zh#AAc0KtV=OI*&QEu(UYU-dT?xR;D6AvtPJWJ2!y9ZfCYMN zK_N+a3sW-2JvTvJ>}Qbwe|IC%L&B47h!iq0+qzUB3-@r z@!^4V^mMKc@}ZvEjSoQ&X;QR72ma@!MgyqZstAX!wjwUAn1M^ctM!yn-ymL6GF%Y; zcyTQg#B8EFl@Yq{i4gXLswPPtK<`}lk9Io_N7V~fP&ADQfT5_5IWPY+5M|BDh4t<{ z@yOe~ChmjafEh;!yd@}&5mtI)vabB^#r0$WcPYQ*UzD9W@?_anMKJEmO(d02NU9V) zWdP{ZYoxsRM=<2;Aix#DJSd)5e7Asntx5$92^pi8N9)yjyW@og z{GxmYy#H|4Fc=9%tUEDo(^Du^Aeq0wlh^k2yv;_KW6w2FWLp!sq*34hca6hGhp%Za zG~o`6+iI(PU#~QcG<*g%ybF9$JgP?t0%eLa)UlQW0@s6m$ol-MMwP0h>Z#?-J01EA z6d5NXfR78s6;bc#Xn|)!v5UXZB;7OMnaS_n3zpi%P5#JETaS?TMosD7Y8Ou zg7c}26wem{PE;wy>JTYKNALmn>v@1ln`^n)UVVxM=SOHKO-)|8?$2ZQ0{6`IuvtN( zqTMsHNiOGZDH^0>^G&Wy5JMyV1pKNf;E%oVzbO8!7^Um|?9~Z&u-tzHr&dL`{GXFdX=O9FqHSQjnx>xNlN?@r>+;5rUw}R}h1cjY_H6 zT6EkPWSRl(*Wu@7{sxb!QJVSBLPSM|po1vu6a35TY}ZwjYR}v8Ul`?M-E>;Bs0@%K z0$bDTPLDE?9#1g2W*_7x7MWuQ;1XZc#Zh}}S}C6mbN+cxohqx}=2!XPT$JALTzhc_ z`4R+7OO*Qm=a6}M5mg`r8A07wi4WgAuD!a@Ol&^PuRq8&KR&LKp0J7OpRej^xnVDP zy${j7=zthx${R+1EA0M9$^|8}Gbj^tW^M8ik;eoP@fU5+3J$0RUewD5Oc4~p$w0UC zn+BL%BN1F$&7fay$#8SgTG*5`faO{yRsjQLtqxI!EWrCyJ`)?Kujq{aowq6#a%EM+ z#G26NK#piGWl9V6K9El|iZ5amboTf{gurfxO#c&5Q+w`82WD2Ppy8mDRCAOxGAHw7 zS*cNZz$4?T159SdNRKW$=PY*{{xbro^a=yARWCF}zXCsVJui5?dWcL!QFp>d&0<<; zadXmlZhb;Z#RN-P>L6EYKaLTROKaeV^{b0WRlS9DCiZu=^xg4>nHumAB@eL8!nM^e zdea{nRfhY%k6AaZL}R*D|0L5#`p=r0XP4ao{v7pj3k1w+ z{Zm*b&JOsFH0Yu5s6=^U_i49u@GJlLOMX-ugCu#jocji995Bqvj-J52&C2MRB^x zH>~96Jx;sRJ65oTcirrK*|BhXmReHnMN`RJyj{BPR)C7ABp|uZ0;F%WAN*?7Ak{Kq zv*%`eVou$x4U;HGq=^HF0(R_Cu69;j;)on?-8o|=E?oVb9@1fA! zJ*@H5QUUWZ^JX#Z2l4VteBN10Sm$2%gXrH+jQi`s4>S}KJI-%sS{UtXtDP$44MB7y zFU)69toWZ%Z-}vO4V@X4OVqceSE+V-MgjW&b||`v9*)I7RG+eHE3WpyRR}OyynS zG!vi^{XAD(a^Dy!|Mm%BKHP!;0h$~se^^E*1>Vx@hXP!L2)4xIx#jWO z`6-KXee$W?uqBS{h5G?8z=>y8HMyIbXJU<~gClQGESE0-b_{slEf0`&qmd$^zvfjF z9S$q~#Jd3Im?c5#=mxvVzG=V%#!&JD+!}v5BeD5^<+Ox-tFI9U9vdDp0?%Mv1RG|x zA?a(9<7C`WJAg8QA(3_;oh@leurzS0|AZX)q*a=)UscTePsWuPOn>7!x7#Q9rx)g#&Vjk zg^9E4CIOZ%ID3rv7-3)&cC@W^QPoYr!S;4igW;7dJECsVdz494m1>j%#!enA_6!ZB z4U`h-xV@Ppr3=b;nC!_NWDLNykTV6qTPm%tY6xZ<2+(lqSv20R$1uk^VO0+MpDyC+ z8V6(9d+O1l>=P%RWAEGIs1^aDL6rUbXDVr*aW$HmFwh&8;^W?rp@YF12PX}$mSJPw zi`d*M?qUiU{BLk_}ZK-9>v33z;b>z5Jv?jXPc4ElImW;ISf(=hb z1(OJPFoavs8pW0Wd3_n(PMu9aT6f-e5(8Mq`l$|)GUSs|JX`0TzEA!%GC)r%Cfo@n zWabe7Bx12%VxL4kTmQrgkPOy?+|%$iRTCl68YGx<+$*YLLasL25(m+ES`*VzSZwW} zFh*89A6J7Ski&tJgBTQ^%0I|iGh);{?EnXD+rtf$v66v z*M1J23>8X>kY23YLC2A0Zd@S%_ilDpIA!_$gHgf|{0c;FG z%DN(U!n?_e1q0upW*BmYa?^{Qo!QMwS|-X7(IvZ#0T$h(+8GB1xrf*Tg8f$(p3f-`*XIIMT3$Y_$wU^sFiyJ|YK!I%NQ#Z#wyY(YI7Gj+w6YJ*w2?20M{y*S@&`7QHB}vAbI%!A znD$lknIJUBYqm7mv>0x;t;Wa=_ig~e;F96?Mt|VuhgF8_m+mT;;=}KUQ2`E!5*4bN zT;^u-e)>iMzS>Ro`o48Am%LWec564U^TDJxx|l|ct&-0No{*DL;&{hdiF=tM z>?(LDhd!_mV2$_*9?%9OiXXiMj3P z_}r�$RxDf`Ro(77$rqrca&5fx91j5#T^l*sUXym&A$z*A>2N}4Z&4H52#whfc|3I3nV z_VX1J9&cP<>m;aqqk(eJ0sgqj+?Sfo%MxZ|{|110Q4JQ76PUYsIw=KC&U-sPHiNPI z8s9#xkzCk_Q3d?0I`QJMn$0QB8JKIHQ1TwP)qk|5? zsGU5mY+fs_6*4QEm7b;f+o%-#`#d7z=4|-PgAN~<*~SZ02nHAyh4owBqgh^eiL+KB zR~Qih^B194=Z;dAg;w)Y!*2CMBy-a;wLRY_gK(imQs8?bf6Xk#aqr4XVs642N(uW+ zVjY2jB&B$$Yz6nBKYI4>-vq&2_bYL-!H+<_wZKa>73P2Jo2GjU$ZdIoixfR_GPSuH zJT(XmuV{p%oznhWDAsJLI2jp!R`kQo<+lXmul4mc;-5fd#(9qh8UW%Lw=(1~)%e%- zlOTr5McckGI!|avLsE9|C(cA7jd!P(EX6fgmE;okXcP#n%^OamddBnyJ^-JJI|J){ z3z#S(Yx_Hzi`v(3%d8)NY}lggYW};)(q*6WpMKR%P-%I`$0DwvF0ZMBhu4j?-0gRF ztIl1ZwjX{i=IcR|OzCw0VUfYto`WJK-qnN@GARQJK@V*BB{QxP?L-(JpuCOGzpy~( zLO(W7^pPMNniRHueCm{;BKKVL2`F>A8wacby@-n2QS`cEKD=DwIx;$mT!YASi&9TY ztuOsQ6#$ldz*m2)Xs)9Y2bY(`EARc~4ox6)eR9?O?>}d5RsWjLvVKa?VK3y~oIj-8 zRm~EX!7+9bbSay3zIl9?HG#K~zaq9lIP6=rg#p@_1j2)Oqvnvyn75TC+TEWOD zTE6CY(Pug!)-l1~W0YL)Q_>V)Ac|(cPig&ns~W_`DfsDx;Rik5E<1UT_#Cr88{W_y zI>%P5KDP25e1D!%t zDoQ`Bo?!R>7TEJBRE$*EEd`c(CF@|rS)xHX`+3a4s7&9Q%ZPAr*gg>SUMHz{Q$;$# z>M0ti6~*2+HI-|&(0%L7;k@{m^BA3V3E8T&WFVzsAIKc)x?>vySB4@?5;l)S{LpgL zG^A$AVl5XZgzhrYqqt45)v^h+e+`5CrFP2VTk1H$Kmr9oLZWh0io1nZpx$F3kn5`? z)P2tDKc3RPaC8JcFERA~8=E?XR|*g!MiFw8=yZ*Z zPFb?XFD)~1s+o~pQh)3T+WnC)(%SxcO$i6EY`gCOOcLJkX$?m#t-Gf(WR#9{NuF+f zJ?_XTbKK=w#Tu4unIi@Wp z5wYdfrjJ>b?c5UmPO7jvnvkP2@TIdYyH{>cWL|n?n*H@|OI$2BKNpaj1K0x6d`Iw~ z2CfH~(>!U!jyM4nUs;gQyznT207}SQz6a{eg zCG)zIpIvDi`L=Pi#k6TMwA$J7a5>gbWq~yi|2-a2-y?5XWSZaeFocK5TnIovON&VI z_3OtVyMFus?S5Mxtl|)lVSPok-1-TxY-P)fZRqns2lV)+ga287e52_u5Mr{r zlOK(S3S3k;N*tNf;wmdWRS)jm{7(`yvP`}eDpV~#cQ z3pNnK;iEQ5MS&#E6YB7hk^C={%@F-OabeKZLW}u%+=!1iT~D>(u4iJmVF>I1nWOf1 z0?ADwq%>dyA7BO&OrpvFRHclg>kMmGkNB5l0djPDZE8H9_=$?vFqoI7*1gN*>}l{u z*gwN}#VNKd7G1wFhrdAdX|!bsA0txpya3D%G20uMb0^$J>qy0XahVCIQ*XyYEqXi( zwX`(_NbV_3bWApJpBF(~8NMz$>F_A9I3#??*(>$bxANz6v>#Yrh@+6m;ukO5>#jXF zHbk1Hqp8fI`*#H18zO4}?zcz@-P?9y012S(t_wQe_Glc?@OkblxF@`IT=CLq`3vkE zb{SaY$pZvT?*J?-N)O}&bc1Q|5KJg1SjA6_%3kcBd7wx3@c6Ts(-^5();Op2fk0V+ zl~LZ)nNQN!@~tq^`|6V>H}i!D`tCK?LpEY<-B2xOY3_%qS3?H1yi1#hkcnq%$uE*g zT?6(+8^`OUP%ktW95bG*!;;&t%)BaNky*Mz#{zBFT0K4zoplrjCOl6ZSFoK-&KCov zZ)hvUK6t6O_qO`iZV&&b8t+L2UhzH&R}==BQ=n`ey+(5aaZ^yEd^2|HLWEK&qv+Q@ z%Gpb}$$<$}g}`QT(lm(FuLt18U@qa`@S=K*ZI@ z`O&-7n3?DYk$B0mdp@*o8gNB-OEryV+!7~XQNeZ`zMDb)y$-0E=a1Uhn3Y`;=X-BP zLM+fSD(pbd?oP~c(D{OW^aeZu2*g||niSM{nC5EI{>lBx)O#`R*3hkO43oL}EgzqC zahB8+0fpT#XDnh5CUed=XrSFNz@{WWJ0dh8{H~s+M_v+W&g32JyG$y{2GQ$+pAqvD z>FO+gph09%_n0jxB#WyLo!NZEJbXRBdzpe!DIYG=c?>uoBf0_*WkwSCdqt%=qiV9* zUAx2Zf)I1#NyZ^53L=4*=&T75f?6>yW*#(YvYTo&J0oQujz`xrpS7@s{XpP3q!3z?iJfK02qDqJd}u-xd-+_xu?<*Q+724w*tR>JaG991Ktt z5U05*6qSYXVA)?vX}qrL$J^;w*x~+JWLf|8!=>v>{0-l`BX^I^xV18F+Tvz&iO8!a z0`IbjI}!0(`Og_~L6O}vbVA!1#mg>dwqa*4lhA$vS;O6AZzZ*3q(IkL5@x>pN2&3f z#i*Cj^9`)dUnB%+aX}0CKmdidpEcnU1ASG$jD8QPvkI7FO2)d?4@t6#o%(Gw-K^YI zOVSMDO*32BMfw|s(9PF>EFb|uJ+N)Pb_xDncn5l?=XY3~41GQn#gD?)m`Sjq zX#e$0?~`!odh#VB=r(VRj8q0ZaB569k6lcgl+2Dd)Tm1C3nn{^GB#(fol2oOX2=v9 z!;%5hpC$?3ry~9MyCP~ug`DrHTocdznr;tf( z?fw0K!gMJeJTAq$Q3s^V7YqyZ0pvL*d1+=%yMGF@>jmXL=w+$JH-JepdxKJiEylR; zU>h~no963VC09K|KfH&EgopcSJJCUIqu8tkhW_r~6$vz_CZR!ySEmIOK(_b?I(xNx z#KnM$5|Gb!(BJOpA_7t7C-p|qI7|2}$#aXIwMjoHCY_)3JS50o86)5JKhBLh-`sqG z4eFGaL0br4i7?N?Ifb~M`-v^NrN;3>?y1XXeYuqC1PQYFcH$?lG!uau0P}_W!qXR> zTaD7Ci{Ob4yHuqw)6ymh*~?Rij@87v)I;VSAGM>fE1kMk&sWlkpq#MoM*-0MvD$82 zn?bv!ZuJNaTWp0m^Jz=dbw0~K|M;sc^AEHxo&riFE zf=Yu`KadzKFNk5*LvOjARWT6j2=>Eo3_=s(U*bXa0f0wmXvAg~PZ7L5i_w8}Ov(Fn z$R7Y2I3+(1Qz@uvq?OyNm}VX608%_V)As|6~v9@1DXwnZv`V@9ab z%g6S2B`r2nP@YAGQnk3haEs^Yr2VFiv)MTz!L)6qU|Yz)HdyU@C?=@)o={t+Jjv{j zU^036b|0@vMOl%#oSA@9@cI;=;w@Gs7E^6na2-0vY)LrlAZX)u`(V=(+~++)r^-A~ za`p8cfNA;}3PRqbg&Tpk}5~+4GhBfwv zQtx4fC*mjhj!!@fK$5&S`ZpPx6ot%idF&;mXW&HKgjg}_;6(3Qq=9!lPDMZ-(X0m} zDTgM&N+&)Ak@)Zh7VeI}CIx^^q@|WO?VF2^#feiBX*Cncy7P>MPu~kw^-eyqi~yn0 zTwllCb7a4|&;Cf@l9!L(zI!|ugw^jjTgq%<;uHw_8Cz880oenPuDCZ>&VA5THnOjt zL+@)aP3PmgeqrmBxop~y>94zZQQUWun}{v=WbRul`mgql!1QkQaK0ZIx$b49 z9XQh|-n2n7;uHqYdp?EvWci|Lh!XKA0^~F<9{Oj+&qXi66F>q128SQ4O;W9tR>qGJ zyN69}oRdyw97Eg~phFTgET>qgdVv`vtBNZz-+RgqzWetc2dEcT5zln0BwsNw9c5%2 zW6yqE^sHCB3U_HF$NJvJ50#@b6cM zi(22O`4qh64-`Cr67mM_)br z)ckMY6XQkij;H61|1kWM}6bpEUC)s-rxyz*m7M5)%7p08* z_I?)aCbU|wmSU zv$0nvr4RYDC=zpTBUOo(&F- z|I^D0g%L+uao`7cs=;qgz}?E3IM6Et_N$D2!7?b8>EX({r{H}RQB136s9v3A6tj+BeoxrUaA7WsGd-9b-vOdYEL9aq?3>1timTdqY} zPBL#}$^4zYL00Y4pQ=n{nGxb4-(JGCSf1Zelj>2=Fr_nn44|Y@S-Fs-`WLxwj(S5! z6-i!AITwHQYb897QM?f$7k(3)ZY?|DInW%FQ%StY9BsL73nV!8vBF+s$>z|h66*wV z17uBxZS_c1G5%vpX7iAAX&qC27MPIjQ?}TAjPz{y0)=E!?@IndvmqEQZo#di@APLA z8x~`KtDcEF3)uhNK`TC&qNtSnAJkoz!%zo#r<~VR>>Lb&X>FoGu?0MHhVPe{LXsavMeF@raMVJqP9PA|H?%IK{v0$CuxQ}YnZ4?{-Z~Q z%?#?@q%UyWiF|FR07O2X^K=?7t~9R4yDTi{5r^3(X&ImOgF9K9=BLDS0&d=}!lA67 zT?XHAw!g5mpi4T6Ngy2%6&ixh=xp;yR5L+{T3vvoWcGL|+=V=psS=mDEMyS5&-d>i@9OKku(NuLTUjX#fNaqzcz8_mK! zVI`AoKi(VK%Q?b$e{ZBWC58(Mu~e2pE35_A18z)7xAW0CPyxPv@k+mJtBA43C~qd1 zW~u&TL(=<&fN3>fOxUEus7nSxUI|)3#7nfMispeZiqaScPX}vh5W5uN)N_@Du)X^n zb7QNi`KS~@{4v$$tvG7w;i~%eUqoG$PMOpob4maf#GjW=w3S;m`(DCMFe{fT*RK>8 zfxwL+*Px1D%cGcf-~+k#5}y}UcC*R%2eqEGXD5tGveh#L{uF8A3lGYrq&_+JZ2ebA z>#4U|8~V3L6Tu%DVNKVrkiTB{?c6b0cbSE8cJuo*GcHoEe#6|lNhd4ZYOCEWjuV>B zuT?QhQ-sj}xdJ5!j|otkyTJNb2d%pgn$2uEErR$MUg=W^Bf+Y#o^1*K3hLHTZPWLb zI58p3F8Lo6gE=;{V?)ydcUCyc;tW4|MW)~D4oic0~ zOzqb?-sT~62|92D3~v>E14fri@UdCh@-rTl|Cn{YyK4FL>chu-2{+shf0j{iV;&{v zO*Tl+q`YfW53Ah^tB;fyri0dxx5=BbY@yFmxl1w~U`(3!L<5gk@rLb4krH$yi4#}~ zY7-CtYpbCUVRS4+ys37?`~|dGka)JLyqJlMlP69L@}o)>2kCV9?jI3!J;Uz(-L$@( zhXv)lb}RK;od9B3asV(tcKM0KT{yn0gcjuyPXEH6FWtEq{2@rOX_BA_ z6YHMmVG;`0s(utZ=7IHKdy~HF^hoZuC7RfOswC)3_Zt3X%el9JrH}0Z{X&TIa9reh z=Up{kp(2Nolf)U2d()A%d}v9^1M6r@IE*^%@<>Fz*f-d10Mvp@?~`MZqW_obn&v<*jNoCOZ<^3D=ldNan|f7lbXR?lZ5 zqXa$?6tF0=5m4Iap6Dud zQ5`zk)AsXgQ(uU$36dZuW}>FjlrVIFc<-6eRj5b$|LzAr3fEs`GDFKVHj7t z*25RoaiRO0KWKk!UgiqvwBE$WQ-R!afTi%vW#WJI`64`5prtEG>qbLF+mSaw;Qz|Z zw5-!(>WwhF_rj&3@v*IWXUtUC0(VhyQkWMSwImOR=e6qk69@je^zId*>MrrM%Lep? z*p;w_@$h`_=XnAhf6rZ1$50cH1aiXTgAhxbmr&c?S*wfCGpeXpgMisGg>%u7+xvl~HKWjl1 zpet+$zi6oI>V`MyxhQ`7VR#bl4^)jQO~t|55b_s~qdiDaC-e1u1JKe-fre#1;<455 zf%PQHLnrO^+a<+vGs1K$=8VD#j<)wl%h2=A5BFj~b%iJP=)Ia6wXUykGEz*bn6;QM z#phM0_7xfV{b$Js%3G`lpeiPC@HrGZB;*=59%^5#f7AK!VzKA1Ot$L=`x93?lmBM8 zkA81WB}vrNMTs~&qA;J1qJ0O=`=LT23eOMlmc(ulY|G#&t5 z4*;aVUrMxz*9~~xHYUpBys&WbxwL4jX&)oOU$S7P5b0oFi4pf>Jl#wMa`aiXg9(W8=lz$TzDIGc-|g7$ zvk!4Tf~87B%apr(ycZEy+A2+axA>0pcV>Im+B?y~_{6Mdzn+;L1VF}QjMGhRl(S~1 zyiHT&&12hZ(?VyD3#yD6Gfjy;NxjlJlV8?oD^T433=!}lSD0!0fj+daPaiy*hI49c zsHi~+OOsE5X-kem#r<;#; z-z5GdCAo$G^IjYC-FM&pykfJz-y6`t&rOpnzypQ6Kot+)jJ_YFE7Bzd3zm9+&HgVT z;^OM=3*PXTx$;w~!=OW{$P~iW_0_qUdT(XJM%Hf)6w895S*j`7 z_#ZWSk^&zsuuCuu+a*>NXrxR{`sPcGjNZ?rccnfGW{#7-%3s#S^a5>&KGp7pG&Z~D zGo4QV`8np(4>OBsZdhezmcH(=fGi4{<>%HPD=spp2qv9R(2c3T&r5LqF4~H-MOZ3P z3ToX2(pBWkCKr+RuY<1n3->6KRVhTI9CaVWL|ot^f?Szt{3X#_3zZ5`ph!q-zN-li z@BUoYsAC+~B|NRpO9TqhpeqPT-a(8X$&C>sgLKG*iV#cg)&rQ(pWRP^mVz?Hq}C(F zlO7;#?NB@NQs%qXr@>V%2(hfbI1tt^em(+32RWfL5qaKiw}bt?X1-6jWSWFtXVPZx zdv-<5QPpws*}~18;PXczTR?B}LP(+aw|=0F8h|Y|-pqR2{6v97N{~3ESSZ8y)|Q3Q zi>8HlrWmT(GZuHeMKe~*xrY>3Byp=vJ?Sft>m3t1=F*WgkTLxSKe+x>`F{E0 zPjeCi&y?7xMfR2Hkxp2UxQ1lQEYZuj28|0LPfou$57}_VJMo?2TH?56Rd=488F6E~ z|2ZNS24B8i#JIEjJ<#IuH(bA!^z0Yml%l1}j{k=ke##G~+BMDdiV)?%gOUlgl&>BV zoxV&KM?vWMX^U11afM_+TR*CdEnv9xg?G4ikKeuWzE%DK$io@LOhdJ*PsvDhyYqi& z!;Hx;zi^3e4|Gam!X!%^24>dByKUOknw0X+*}o95QD+)9o?fX2zlL7Ti|9l=O*uX6 zq7iz2IQ-hqhTSV6`@HLMV5^*W=j_6ecez}OCL=#k**Bdmt^Qp^vzuYlrD$!)Q+xHU zOUXwQF{1yz@u1`nryxpa1Oc5{{8yKIowFEWd z2z?OyXV8GZXBKpeact1 zu9_J6k&fk|I&6(p+EhgQM7Q|l{;#6<0dzo_NSU%a$tgPtiY$~(-+N&ZNRz(er#R$i zE&K{EZ-c>LH;;}C0{SOJEId`4sZ}uo>maqHY|P&sAMYL7(vc*QX_@BWFPA^-><<+e zh4GwUXE-YDW%H{3{b66Z^26V+GJGbLD@iK95UAZYFEY5*Aj1qQD`X2xZz;@}w+A>> z$7UWHYa(_AA?-cBLhdP5>(4F2Ep7JpAjtu7MWdbPz6){GCZ+lhh7WUp--19XK<2V( zcHW}1gtBkGZP{5GQT1AV>sGym6(9F+Ls^nZv4*4>Meca!)imSFnD-N(Zb->{*`TzL zyZo41#c`mcGUi|gW`k)A?zOpV)t-7OrREUSuS;^+Tgh#|?%8b5Tnutx>W^6l4 zERFNvu0dIYH$VLTc?@kL?T;~o> z;kBU2W8_RyUfm6C{JXIl`ff0BCnEcM#Igm_@2}Ky`d(jm^UCf`LEftmofpzvO{CNp z9kbWt#Bw?e`aa@XBZ(=&mASJ(Yv5%JQ0_7jTrI5j+jQCqC?{}KGHPU9QDEq=<%`mBCSvf7#xW(6_M&G^e%oIsCFSOA^?cc?wX{h0M59-c zdom_5EhM<-Ca%PWC)Df98*az132K5&Td%8coTW9r@!Z{NN(TBwhHj9kMc)G{zp3lC z^8k8z>4z)dwohwf_qWe;S4$2|rG>#<;z3FS=)J$`f!!Bq{^m0i{lHe~eg!V~3f*ra zD*FAQTH3YtQj*1gyxks~DN-=yFo3%~#=4Q+9JvPOREeMNShUua8r;Yh^U;uJeKM&S z_w>jesK2RIjUx=tXMnCHUce8=*ha zY;{^@=}!0-e^Xf13ckg`b+vy`bJDXT@B+x;-DOL<9AAvkC8I_4HxjF6B?fI}C71+b z=`GpP{g*1}Efn>sF)EA9Sn6#K$HbQBW6&pba^^!k0EbExRK;A?HC7kpQ zUZiZ!_+XF>4LWW}a;JFAR<0-A zF<`8!)1Hv7Ic+o}kskxdSxCI3NWf^bg(b=Qpw5SU>CSSQOu@E6Ert)wPXWE-VHO7t zive}e!%!_6DuLBaj&yv9mor~@4e1j{J};DY=MfxP^1RxJr}8Ef;(aA#ki(f^VM3F9 z6jWBcL!9*}?&p%_61{xCF7gV}QH?`c4S$-#oVC3bK%-JU8rqFlU6`WgD+JU3vmpww7j+q`O6T&g1l{734W+mznFcXNR6+09|+h zMRUR)UiI`u*w%j{nFe$}z9yPV$9OA2vntB_t1=6Wt=UVSJeJJYn7R$5 zuFj6afacC__q57c#}#L%v~eIcQtg*>Bq)t2nraFQdcIrD^QpV!A#{E|oAyc+dNN;b zZdEm5RIeV*XUWoO#QaSp=D`IvHXL#5CwzBEfD?iN8plAxDv+PY+E0}ta4>n!)vG39 zolc$n(3TKSXUP%q6P^m)J3p(DP-njxZ=k)e$G6lbKcaRbz>HZ@{5ky{H5>PdfIRvJlvNq+ z-^q}L&|1M8xaQ*2u|2mMT`I*BG9mQC5p_TpsGAlH`FYDaeb?V~tJ$AK(LKRuI5|&v zsV5@c$4|v)!jFU2>ywhkaagojKrpuGN5}rr-{d}l>_IAif<<#Etq zljW5_^WnfcHy6wgUOk@oT&n}GiTE%`wG*M{UY~G?l~b*8-oJVE=wCN+?^E{G&Gl`& zLDrEXMgfb}afEy}*6{}+-ZOZa3((fqb$l>{Jrs?u3qPer71-x`79AbFVnUFq_iXm5 zU3~M_xe-$rLF9dTkL$JMk(^#bLK2+zE)KSa5=1XhIP^)9B!b@Qr>HRN%P^pATL#EH z$`sU979Q2^z~?nu2wj1LMcMl)Mh}mo=v`jjWEcRX~GlAE#uQ;M;AMv_#aQsBto&QSMbkT*S*6PF~ytynRJp zD@@CL!TO&gk8igYrhX~$>}(^YZr%U9f{YDePMuwAKFnQrQmwZHjoajuNMrOMOqSHX z9Su^`@>PAZuN2V1={9$Z-A^ZB!_-B0eB$yd4x`*Zm5R=oKHcEytXp}Y<&-j1 z%_L6y(DB*QEa6o5{5tbMk8sef_0W}XTnlB>SvI29^~y0J8O*so`T1$~pT@&agk7%1 z1sVE6G*ligYSL!egfPv+l`WvY8nOjs>h{dz5RsR3Dg&IF6{=XW30z|>F+HM($p>$ZS zQ@o(|Hi;zkN@(6%d0)+2(0*UdjNNYrx_;lCHCovJ``7D}NfB7&{;=k_Dsx> zfp$g5#6$;hV%lK1K29SxenLc=Qi9**eFO!=ZWDWnL!pG%?r-~VCwuW zU^KoLT6+R)hf#?Lf~^}41zA2esWwG2J)M)>80+HM!Xi%#LpDWwnP(uk3_#A<&AykV z2M0Y}HaOs&ed8w3pR}dsxL7)iF2FUm;6+jAiyYWEv3Mi~zmh?c-PmWkZ&JjT5oZY# zzkt{4RP4{g`+chiJ_gng-BkGI3he^`=VD&fpPSGMXo*F2^@fDkS4&~q*MajZ&B3Ox zq;^zsogN==(cGIfQy9WU>rdVm*Wgx~SP`uHH7i~VX5#6J_whY>RKi56`*%S_qPCX& zH09(8{PyM)FX#MHig(W!7D_o{q&k6Rh+ILcYaX~6NX^_NS&?(x881P zwk#|dK_C3udatrSE)oD$A)v7z8!wmYSkaQW?xKnD33=1PY(4MY6;MJb^B&z-+)(&7 z!0uw=b@=+&QKxnAEQ<+K$NcLt9~WEi-#=@?>=s=U+HMa;YDuomK=UXFCp=# z&WIMU$0X@EyfGyJi2{dA-R3%o8>IJ~yS6&_O*|>{O+N&jQ-AQ;@`%Gw+eS){dW+fZ zPDORyZev#l2ih3!{78N+ZaC;&(8#dHvq7$27JoMKa*}A`y3PZkkgYThoZm4qb{-1zqsW!Pei97| zNve|1?L{Z;iH=pG-mA#0RWX&$p!J9F)gcsIR!kZQ$#?R9S_;*y++Qz@utkvm^x6%E3J`5arE~z>+&%-?WlRpL|_XOn@ys!rEY&)9K zYNE-%cE6T-VC_8~I7J7=gMqlD#dP&-5_(@jX6YU52}Lwq-FOz$6)!t`u#`Kd`&6aB z%sMti)}G0|-Q)YatS61%4T)%80FTWh!hDQ+?vyGPQaS?3c~#XWsnre2S5b<8D`vhA+`K!>GdncpeNC z)8xn9YgE}zRiX;qi5nPpFA=#mMKAWsK32X-e(9Ens#(4(wk$o6wA_sm+N1fci4`?8 zirL8aZ&qyi5?g?Cpg=KkA!Da+8|c~5UFfZMrAu5+J>&-hJKzXIro+@=ed64RI%ywI zUy#XheX8Kmh$M|`Dx|S0HN7e}iH-=zgJkwT>{c5CN2apjPD4|&xRCleS&D>FT1YC5 z$}c_X{+x%>&kB9NG+b@BhVx`2SJu{7 ziMb*V9f8KEPb3~%BNNrG*BKNX%PkozbLiEAZ`nTad%oa!AoE;91UM0=?vC_`bi1QP zHt(WNc>QZ$6A!Z9Z<(a%ngPO3svB8Q|EAw{nWXX79ZGe*oEviCGcxCi6H~ z!pxBz(TLG^@S(>?tD0)N2^fUBP0c^82STwZS%UG!-fhU40l9_e8EM15JJX)lwG(nK zm~}l5wfDH5BFwGL(Uv3+?HbBUn$6VtEDzUL+ zcB1b*KJ1agb)#6iswM!ioP#?05kI~P4VW>8gnU0KOINQ;T#8{`H*#tT5UET+7=02@ zZQzJ0<|vkRxGdHFPc&~_Q3^W;EcMh9mG0Y1&BR(2k-z$9THH7uDXf#$(1}^e{ zXgbeuIJ>XyBS_Q`W%S-_^cLOdgwe|o5xqt4HAwU}q6ecBy?2rzI)hv9y+>!1JXikj z@qYKgTzl`e*E-j4otwF|qWecd73HQ@MX>+!kT{3k#@;x+cD=!U_O9ZeOwLi06m3Nq57Jw;)0?>*OD9-LQ+*a43A$?izG@PeM0y+79vgA>16gp+fSexG-1jXTzu2{A) z^3YJ-GkH)PAD|iF$y5v0?7}xQK7OMKHLjt?W553bG}=y=cF{nsy0=g(mMsc2k+m-4?e3leu_eS z5y4U0KN;A}qw)io&VUO9s6fqtO9Y$LC|yK|a~ zRt%2^oa@o*HNi|Rdk2gg6-6wV_z)yY-sHt#4?)v9Vf z5pC-)AW7OX<6mEo(3GeT+}rEDP#wte$J(teM>k?__r#Nb|Q~Wfp`+J+!(SGT}>Tksqlkn{>>zLZs zs>`CMk_%TaNeZ{`03d1dm-LFvDCcc~OkP=#R|O>MVf%}@y8aE(b6T#s&FI8wgO4v^ zI58q;)Jeqk*V*0J%#%{wqnoM{NpZkOu7h4n)V*shv(=9u=REREh4oHWRRz`}gch4(Tm=~OQE6Tx*dKB$L2HD zyQJL7mO=XMmrXT*&eB;=u3@PB%zE;+>$2(Fc-NpL{@5Z58Y0jLmpKmX-h7;eO|NH! z7KHX?`lvq9VIv(d3aTe9Jh;db_P8X+0p@wxS8aOJY z-n(#+eb~PXVjVdcbuVBDtp1FMe}woV9g&R)595nZE0G!_<&0z9PXIh47%)xoku~@+5qNC zsG&+`tS@-IE4_8E`V2rRmC=dgJGlTUIo_6zU}Zf(t!L&xamN_3iOq zx{Es3Ilw~%oi23}H`F7*b@IqUgR8*#);uPi{6k+)t9sM+T_O$8J_0niINSOO%u?7g z{=**Ky&VYk910W!H@W8b>eYU{4fmxAjBHbi-+=1>1E<1x@34?7lMX71{=e2cOPcs2 zFe%4R!~$8C$p63^{N2=At|8$?j2xlxN)*(^wR6Rf!;uzI zGl=_%UUgvh$@E6SHla~n0H4KglK1O4Uz+Rsv-i0vh!9hwNh!QR@k7p--y*J#Pf{0g z`ctdV>gVBvriqyV?9CE??Q?yBisFb5Yz)w`U+u8u$VA3^tM0w6X`wDuG%YrU%g$`B z@z2*x4aS06KRd>nGSWDfScq?+|IiT6PTMrlke%|C$WAjQ(!Otl1-n#2mMU)S4a78^ zIewM%_!~YY23kjAE<~c|x^06YMtJwajvKB4H^F6MO|AyApOPVjG7u=D?nL*Cv}Vxa z5agXWdWbt%X1xczU->x`GvaXY%vZL-_h@J0@< zfY#&tFC&{05m?$n92Z1}OhzdA@$q>yJ1S2pOH&{cKGctdFJZWNBrSM;H$Y||>zccn zYd>qQS|Nc3lmwL>g)RL(a_xF}p?4qPSXXA>_VHjbe>?{SA75yT`K@uX;KFfF#{nio zz8R3J4#4`FW23)Y5R(`Ceao|EQDHOqm}*t1ExC*ipdSByp}75~fEN;EpO3U>uWUK9 ztMhf}g@p*W(jDPl63d+zM?#~ITtJ?6z0aX{uxJ!d+5{cHe>{?}xc{4m%!Thx6}dE1 z>=qn$)*SBBCLyGNQ#v5nBhfHvf9V1o*fMdG1g8z#-E1J-){pZ&-jtRx&I8U`c>h9J zvd8_B$LFrd5IbwR{rB&G6Gz)E^R5@2q`fPkce&J}HF2)BK5Bt)UVtF`u9=9wyS6wy zP}SMjN8G^Xr%Q$j7W98Sa=LEY9Hjsux~KwlHwTKrqYCO=+>1)Q_DkG2OK4ri+Gp)L zeQ+VS?;24mA4}Qy?Gtft*hHFjI|i%*eV&BkU=4E@5|o~Oy1b7H{bP|*vrpL2YHeLs z6Uc!URJ|#SPnE;|_nfEte#D7sTBXO_B?D-#xHOx|U?-WQwQ@CQ1jAdntU;Gu50VVn z1RdmP)I0A!CDX03#A30)Xr9LmUCe-F63{raY#S&vB5Ll*#YuihP3cnzZN1sMQ?GPy z!+-XX1~|YDb+CPCZA=AQ+#)}bDn&RLBqaYyU!J%8KPN|9P;~wLk3e%z)PsSVdGI^f zE9s`F&50pFvIJ#%Q?JgPrN$bCAyCe@S-N_V5`k zNYYTKx%4znlE#qGO7tz{d0E>WS=&#s58BQkUBGgH64Wx|>Sex#wB#TUY#wj0%%-Ij zv423TE=(H~R<%Y#X4LK}0i1(Kr6QRzc+pc0+siXv>gy^->)I|LYHtQ`lzD1i>F>@& znF(4@{3bWyT73p_=LmnydifQZEPkq*8>6U|X?Zt-`cIxTA%&(pU!*;^BHS%&*m`e% zv?0Z_J?CioGs^7XQrA2U;O$)YbidRR$;S6BFWak|z#c7PZjz4@Y?(|vC~-zotz)*3 zM!2<6)+wUUr^l^&I0Q%I=S?E94ek~{34>it>*=E(7tV}^pfRZ-qLSh_uGp0n*$}P zvc9TWj6%7Bx;xnFW9mV(gNs|FyHD^HkpkGXt+JR0OThFm!rv-7{{=Q9lW)wKkVlT^ z7yhLTTF2SQ>_Pi|harc61VY+$*nhMI8I<++ok6X&1E~M=**cPSe(tykN?>kx zDfwENSg}~5ChZ2eN!eJA=p5$KHF{eH7Rw1*SH=XIpEU7iPGh{3uLr|(RR+fk8s@hW zC*${d-2BYTmI8O5j)}+m<|^gyp({dHa>m`UcGS!oRH+|?u5z9O<2+Mc{F8^SFQj?@ z=heX`I0K&qoOf24hmN;O;5g-MYm=#dho_8wBTi`xTZ)=qP_B4x$}a%f7ztx;_YV3z zPg=gpYF9bl_WQb)4n=5e;uXfbl!Z7alBea>rn|sYpia0Cxpjpt^3UqcQO%51f93No z^}H{38yZt1DV!|SiFADx>F%k}o zMmuLVOLkX2c_XU*CJX_CRUWndB}MuEDdfjj$Aa3~Dplr>4_;84MbPW{{=WBuuA8Uc z-DT}<{(;o%Q!xvH-K2V; z-XVVL8osp)Vc#f;QU**xGy%EdWmj$>dZm}-R}t?nN^u&ubzy*O$3oFSCmvgxkiuaG z=p3woEib9aYt0t2319J9&qdlSsB&CRz?wJon87h^zZvSN{;r`u06*}>>+I9KZ<Fbrl^KRXQ$qnvs& zmMGQL)5?g2&{Eb;`w#C`M(a|S(pB6Y$qP`%o})%^tLPZ0c{4qXTqm2qwoZw;OMimChO~G}`qpIwfIr+L3IljyP!QKQGR}ddg1Q$$)(;IJ(!jX}RVD zlS7}5$l|_zKWUe=Mg-*oO3{Ulx*c(dJYcuTI_$OYG}|SZ%Ady4dh+0xD`Wv8+SfE( zW$SC`3Z!kfW?pWqynII#hd29}0mCHq62hS_VWglGYfu!Q4aWGnOMwUQJlYw3hqbNt=5RQa+GhV%(#H|-Scq~fsZLuYbL5BAQ@(qN)@00uUkU&5>bR0ZB zTCq}T0MS-xUU{ugYwoUm@awc9`7Y6R2z1ib(laJz>fZyQb8mx7qwFT(A}`C4Zg`8u z<;a~B$>Iq$e0%ldw_4(=XY75KJk)oah-;Ig1xiwoVQ7n zm{TI*_xlHjeAc}SS99)An#WdB6d4(dLsg1P-ym7boSmsJjfFWjys)LQ)qYaMB$^B8 zDpQ^n2lij+OKNwKw_ehZ&Yz3^DhXv+KErCg45mdVyU#SyLK_%fDd{52AwKONOfB^| z?$T&16(EA^o-dtrz2!_RcML7%Vrh($Sv+`JY>&JaSJ*Y1TJEU+u80KAD0$$~ks_K# zAi2)|-I!DYbeFA6ob?UUti|Z+x@&O6!T7Pe?(E*6?MR_7!s>BOdq-D%y+BZ_uk~3E z3!#=M!Q_eaEH7D^grw$zX$@+LgX6x6f!9x<(H3t<0=YV&O3Qd%=8ehOit`I#0TV22 z2cDqga;wEl5m!Y!7RxQu9X{7M4?B`zQcq;f>Ro4dJD!q{-~M6OM1$06B@ZIu)F$MZ zoxKr|`ejeUXVT|QY0)+*8;f&EHl+o!{O}=N-NO@ReEWg;l9<8w8r*WnyO*C<4*t!; zJ!U_M4WyZ>oSnvXUF7KIq#<%G_8SwW^;P0f&sug}F98x}Xh(w@CxQ)ceVy+MpO3}Srxhm1ct{oeg!nstPWzNMC^`bG`sD;aWLLH#@Iac z8WnV6VK(EuE1~xdd`V{~Rot(#x&z$yU1x5bdHs8Xe7Buo@!HJMGzdGtfSR>yeLIkH1-00?#GCUk5vfd?pSLMS*vgY$NCB;hJx`%Nb>D_2N#C zVc==#WqP3RY&w|XRH#t&B`R4=$PY}s<0Y<29-ab^v#JPd7>!L+YG=bEof*({ca_DqR(Q^%Q=j@P)w3pbCjw`Ljh_t`{R(jj!T815; z`d^${#f5@7c~AMIk8_?c%dQ{@< z=nU)V#%gz69wmbiacXZRpGLx?4kax>bJz9FG0HB#$9N;NzSMp8ENm~HycaSMw^aRP zFLpgAeB6HnQ)xeTF-B5VuJ2E9H9#>6RK3~@ic_+1InYqNfDC|UTaj`H(PvW+X36Z# zIg0Smzf|D8Vwk_>Q`g?)OZocZ&7Spfsqb`Acj0vJ=~jH@Gm%!}fbLK9o-^;$JR3Ea zmn#*K;DSHJD%>U2tyBwV6uTBzU8QJ*fq@tZ_g<;EW2D*nN;eQ2ydgy`yYl8@h(tT4 z-SpH%9Jfc;^Tjci+JOVA;QlNKQsND^^87v9?6ZmnD(gMKP7S%7?XXe0wfaELBt_4@|qYm0`K?npY0(!RaU~n2Q>cV;>D^b=v6g04#};>$9>*bgCk;@ZA;R2 zG|@oWq*^@m(Fy8nTal@id-;S%EpQP&_EU76Y#6Xi&!VHlcOq~rEfv-ohopp@DD z*Hl;$NfnG3&EskaBbe`3*wx`ZssWEm#>Wd!M7q z{uCkUk+#UI${i>Wm${osuX?|f#=VI$L7^BLW;H$DX zf86cc!p@UQRU8WiFSu?5PTS&MDip>9dwC~a80Y?Y^&7BtyOa>>gjTI8IHMQsAMU^R zF@N@jKx;0qYR3{PQEGss5)?PtU!ZmpHh16^ol3IOW|9zbUryt36kuLC8Ro4Dyy$=R z1gg{Jc62&J+44$NC5>ziiTAiA$7PoAdh%46^8EZ2b&G{;Ywo6~NUx}Eh^k-MZkYNa z(Q-7EG4MaVKvPk3W=SWvZX}Fnzgn+VejJgfMxv8WcKUghX@91{$HmkAMc0wQ@=|dH+a^!Eq1v zrW{+nrrvCy!w3(ixn4r-w%CSeXM#++b`Uq?dqb}>4(n1g60=nKy=o;VV^;QQ*JmR& zA>{3^j~oiqo^bIcYeIfr>L3VLg0IKQaPAY&8jB(YnMNhASDzlz9EE?im0dQqm=44& zjQUjE2jy^oqRT>)Pt}B@el&Z*_MP57lLSiU3FR6EnzKrPO^^#ePOl>arG;`-$m+U! zmKc&x{g@?^v!d^a>CT@@Svf73sMF)CLF{AEya>TgYu=J1Ean-)F;7*eki16N~1rJ=O5a^wN^{K!xn2(|93)Roool z$!<*l*n;{60akRD&VGZfh$x?<2tdDt$=sL?rQ|&wHx0w||0yvwk%EEMmki^7(XOVb zZR^~-oG%k&#T~jqZPtdo)6R%}?~#HoY>1xwz5^$7)?=sfv~F{-k<^%=@<8@~1Mkx~ zv@yXRrqF<5MGjO^RlwK zULeAW?7r(jEz8{7K7k#u68nnrr*ny6#Mvba?QPrl+#9vTh_7E|9VK*nPY$!f4(Wr( zj7t6{>8|_&q15=I6XPq-j+s`?r;p1emiOfztL`XvRc1xhj72EvD(!o(LiW)n4DCJ< zEA1EbL1+7Zr|zsrEEjLLd6^&XjzPzL__~eJOU9*}dyY*67avR78A4QmQJ;|76;|3S zo<&1XM*%$@aSHzCp1BWNTdhL+&C;S&7{&0OZ|s#WQeWSVPsqseyradyGUs0P4vY(Q zHS@ZVQxXD@@@g32aSu&)kaQQA^4Jn&G<_J${CKjxy$%27bv< zp{>@T_?TEUteIeQW9abXB}O~jvH~7YH?Ht0tgZ1a6s3^!6CCOOcN)99;>3Hps4j}A zNQeGNu2QVd6X7FOeL4LCp*Nl%$OqEcp7Z|t;|I5HLBEN%3+{a;w57O631T$(>8k4s zzxohG;M%3SyK%iD3+&Zf=3+wXR7f} z32ghTz6s9NupZzqq1Nh~*$%llVSr|b$mK<$dWMG0x@G9~K9Z;O5hIOQ zgipPl!Gl{)pYP6<5qn@r(&n%$5NIPfPHg+DD8$ry@X^NQsC&B=fa{6hj{RO<>0<5d z+^@JXWg2)YG>F5fnf|`7?%k|L+FL^WtJm7GpsBgASwj#p<84vxEHK809`c!IFY*J+ zoUgobD*6$_h2xhhcLYnOz=nt6f5qKAWD+C{po#G-55Bi;RLi2mH@>R|AD{m-sh>-% zikU*3Dev2@loW$k`YKiaKMP=olpo03c06F}iUOOQTyf543}$eppk4?Nk9|7c`BqR` zW(MOqtq&^P=`(GY-Q&=)Fn=U|;4y=Zap>)4mx@H{Leq_VZ1?6GJ(!^J)mcsK&-Z^U zNYhS{P#q5aH4vi-5982a#K@@jX$bbuF-(mst;Irh)eDw3b-O2SMj80x8}=fZaSf>HthpnTQGxmV^5gjC!wJ{Jy)WP$Vp-dX6xQ54kXiAo=8 zmWmQvdNI1|paxAWj`20k`k4aW?p2lb1od3F608dh64Gtn*U0L zF~9+>%NtvgEMyZbV8t#yRj<`7*jmnc%f0Nf-qDh~S+cKSnk7rrD^*_k z&Q9CzzaRE~Q%sAZUX&VCQ{O<@7a>0tWh{QGl761VFJs@wkH3ID$z6k91tl>QmECUk zI~0L9fDt_S;>)rd*aAqDcX%S9JHeLFadj8eE1xNfh4f2_m6?bIWCj-V z?kC$irG;N3-Tly*>!=Nu`2Uo-mt{wxYlKXnnxmrd_icEFM$R*A7RSUFBa-{3B_0b% zzL$4*m+;vkGrffD1)&3zX`!GTe}h?BjUo+0ZlAQn)FhJ{8dW;LSq&96f>Cf%&1;qe z$=p_r6A-KpZ4b}VOj7q2d8pc{QlnOl?d*y7Vn<+9nYX-jk`eQ;5pR>sDc5cr&YcoE zvsQ1?k6OB`Yxq!nnEqfh^X>EMe9DG)_v9J%0h+Jlf!pBw>E{~UT1|&6?LAF+IoKBP zC+sfV+7Ay6XGS>3{k18*z=nK^!=eyE8;A<|5xGrzr40-6ph(vkh- zQPLW7{Nm$!Bo6?)YwTiZ&I)3!ex`GJwGA#HEGQ$G*@QNcGD*!rlFe3T=^Dh(4ZwJE zjA!sxN8^yc?X=JgYmTitmZPLXy~yE-{*bSVM^ia!_l6#Wss0`q21P!)*+S8l-sc{V zs(+V&$2g3dFS!p|6RlIP9-TE=*|uXxm63keP6MsYk{s=74eYNA`8;*SePdPq_)!1x zianaR$4zfMKG%ewW(PUqwIg|$+DKbQjgRba>UlW}8k!?S8dfdQ&hh>&da*k6UjAUlfo`(AXFMNZj0~#F?M*(YDtVNNfX)3~8o# zYG40i@qFw`MGm~pVso%aLJxW9_>KTh$<@6S-tF#~KXAHVUb*9CQ=(x@yc7DZ4U_7m zQ&oE7t1a$WWxd2XD6T9aJ?=T;p8+Ipd#8z}&$b$pqrA|+6XpMJW?lU^v zc9ZFgYy-=>d$g{D>KyQSL4qfX%clqsZgTnRtT4@g&)uiS0|v#)J#^mf`OL|~B(DFl z)0~Utg;a6}-$-Xtg+v>o3YLX|e4}zm12vlz4O^){C~Bi&+dk192tZ$guhb+C_@xEO z1RK~5PQ+k>YL_u{4ZRlAHN$WD0mzuG`RDrpoxbEG7_=MxZ#tWXPKeDTL6~Pu^DsII$<=u2pFY=9u8I2{%*+M^X&mBeF>J!u}7wsoBr) z8*zV7T~zT2{|t^UaGxr!!)>0q^<8}mzFv`1t~z{k;otUL;I@zpg$MPThs}g;4IA$I zei)IQZ-vWdt)V7h(bSY)zF$bpUWUyv`E^)RAC4YHhPx#bPqgSz-OR!DS2j`wuUtRX zZPIq|GLjUvuSu$-ZWsqM{*P=__)$m$<{Ie5yVp}W($3vCT{lZN+5Th4i_6U}-Q65f zpE90W9$m*JcO}Q~-@N&%vtawjNfv#{=?f$|x`G<*Xf-*CkxmIzT`%Sa7*wlh%Nv5V zk@7g$&mxEY2f2pyxlsB`O3cex1nGar;4otP=Hun8ef;~m@Gqi^;47RE$`Y?2@{Pb# z2ge4btj}F}DO0U2_p|OT+WDsBaBP*J24(9yb3)?Q_@RXUa!)&Ip}&`B31mH;-?82Y z7iG~8gQ_f1e%FZv;*S~XAiMN7w4aqcTH3|2Z9#|e&kMyx%h5GwEj7b$_A$a3rY-SM zJfr$8@{gR+@U8s%HVQ`Qn%$4SZNyIeB3s}q)A&CPbon`kWiL}wUaUW-4Bw*omabKVkZIe>`J&r-0I4ZUyl`;D-tPA(#$|ne zS9CUJ_SIj6M?q2-dKaG3^M(^YlHsseQessZei??7cl8F}wb`WH408ohdU(ACuR5Y3 zm|#5oR!Fb663rJDamXdWe*iA$Ih4Ija<5c97?Wny*Yjy* zZ8L@i9u$Gzv#KT+jVlyq9rXqyI=qR1{ZsMkVMT(7ob-9Harj*(&51*In2`E&{}Ez6 zr}-V3S*g42S~D34{dDt3#p?S#orJ!fxM(=rCj}gA)uLLlA6YTNU37ICii+gyJN1;o zrGc_XBG2Jb8Lc(jPW)0l%lVxVQkcqBt7-|$r-fC^C&rH`a)D?I$IkZaxJ(CWJelO1 zZ19+3alN;%>A^HXD+n5d|nyzWX$_*i~^?VG#qUl-9 zR_PM2Z%TfmO4t^f#-|fpGhQlkJ(i}~3sSEa_>=Qta^mlK7(50Hv;&gfPr(1OtPW40 zJzuvJC#8VyJB+b$k|6p>#dEzju6_)#M#y~KW_G1+-`o|joY*64LF>~snp>zU9DnvP zp}KlQ^6XT7YLS5hi?S_57bz?5a^HD<-~Y3X%@2<@?LCAIjAK(3_P746G|}`c`bPQh zwQ_F~uLqMB7|zwHhNH3ciLHEWWo5M^W9J!u&b&ZGkbm)dX3N(=B!Y4asvB7zwBQys zP0>j+z)TxJ{0fr*XW_49CxRay0_0)0Q?<&IDdFU@MvC;IFXC!016PTy<_H~phtOr> zB}YwU@cp?cF@Kt(FA{TBSti2inVGhu-e$(Y37~@6Wdu}H?x9NB2-D$g&N%n`7`QG8 zH(P8-5Z)iTq?1$QSpEFAaaQJ?lSpDQFwRfRnDos`{2p7X9_XoLr=nYL@bg9RJ{`f} z-=<6C??CUWBFnNLHWjh>2E#8{kJXZjW5Yhj)O9d9*N2B}y@3nHY#9dN5*PGUALS?B zWR}H6^)rI3vU8;@W%aPJWC{cF$61p&SL3CXz*u@taRP|L#P?PAlAl}0(IiRlxNkW~ z9VdH4YN3Lz=u5{04>p6pCJRwN7&9dgU?mX$cb5bwR>Hw1mE+a?srsfylDE53+>BFL z&aZ6v!MXL@tf{TwL=>ymJk4ma9`5|8HgEdKxX5?w@G6zqyX+lHQgC$5rTGy{%E@R8 zW0%V%mDFj{r8rWuOKV=h#Yl_gW!CO!;aj9qW5~{ABFlrJPv|%own`SNRj)~c;xRd# zEt=mBSi`6?M4w2R&$#Gz^3Dd^!R^?|N3;U=nwG*R#MptKqXIE^q0jP)jZOMn;pcp3 zTu7R>_z0SmQ?EZc_~)EjNK2|k8j8KkZRT1FVx~Pw!+~O5_;Wf+w#@c@nJ;j9b~wsU z+5|+mP3bMd}47d7Gp?&sCT=J zu3T)>;_yzBNOlDk2n{<{zP4(3gP-C&9;mARl4vCw270T4nH%^oitH(xN`xPtQQZI=QP7M1I_dG4rog;Qdm4J2+|$*^=>vh z!7_s2Y@T2oY|Mg>#$ytg^}MeWIDW~HM)1<99MCY#IKxe%^q+&zdQ$~mK>}sV?sP^U zkNYadP8*HB?8EoDT9kp;Qk+Z&24WYq z^#Q))A5h%=!(=XL-@QBP*zhN=@Vc`#+M(GIGmfcF*=u#j{=|3SkO^?v*0^kf1&77$ zMzDr0yEZXaZbZuPW*BLV&fJ&a*#vL2V6#5WJ?DxB0`EZ5s&JNgUF9!Ni@k#Xp`e)f z4=P@EruRaHa`dzR9F|nPP#?MMa#)fdC^9DVx~zi7;uvd{zR1VvNsO`Ad;VtM6SAF2 zNsP?oW0BrfQrDwi$j^_`laAlrJkLy>oQh=yra{B8a%=8RP8Ml6eOHdEm`G&)d##gc z@Tu?xpGGHB8wN4nq?33fS%N5!RgRj5ji0IGA@{51O>1ig7pba4UlOb7+aQsmMQS|2 zbB>yqpj8{S|yfHsZo&Y3;A9AGS40xtbLF`N(@t_yOQ3l)=u;?n*zGG3tJf4_&2hDP%*wl$~6*nfLvM@=b1?EiwFtqdO<` zHzzvsNHufd4!^1Xh6x(tn3omR3&Y0AcH~TbOJK*Z$Q+F$eM$r-C+$!+`Ox)FM>kpg z2-6Eqnbh)dj*0Xp)(tV8BsE{O(RxXtyB%9~mC#8w&L(uwp{a#(|;(xDd;_a4t zNsoTm=kFSP=(p>tV(S7zcNxZ}XlR-)IsH($N()=7-z#|>_u^hlGYbpY@ylp_=kWgD zcSCbC68WY9)s$^1p`Ylp6gZjmbu!vf-$Jy*f?6qWl8NcH|Ia>%- z{)^vkhViW?`b@y%C)OeA;^=Qj*P_TDd{DdP)NSdt#K%4ua7(`bDE56nY2DxSHSzOT z5555d1+G-xse?Q7CAnwO!XACUkg-^J9^WPI9y-=tW56+*^f==CVz;6W{rrgZ(O?^v zBjrE;p;L4N)><4Lcj>0yNlsX6I1XKTCKRDt zsKk#F^dF9n>MqGz_vO1Br=w0_8w)x(Dcq@il_ppzU4R0RMMy`U_+U>YnYZbw6$mZLLE{ZqpCuUstbt z*MJ3anF?Fvh}gFy`&%b0@%dWggKT)}7%7x_%kLa&8&T5o@cBqI@ZpF4Q&piywl!6;(VvzIgaP=h-^M~|Zh<7G zXR+PK0hDO2aIVdgZu$7{mXH`iZ2qxTr=v(qnazQ=WJSFmEd3TYtj7XU*)A%_uri$p z3*(2Bj>oY)R{d^nkPm1wAeY%jrUt&^KF1}y+Q?_KmhPHUos_ER91O zj1i_z)x23e^=q<(8trWL$Li<3Cg4WyZKjrs!Br)h43onm7HehINLtH^J6iN+Zxlxn z-`73T%H_Vntznk_L+$Q9AUQr)&4uwSL0=#hJKz;OM@>ot4~rb#?Gdcsfbm_)2R5=p_&Qci-muyN#QZrX#Xq~n6?xDk@3p?s<5vr zhB_e>@=THWW&QH!VL@hQ8CwqY#Fcq?j;w(=`rjbnQHd_)_T&e{(95d)46~$?yw@Np zbQkDrjSgoEhmLVFxKA>SBE`PHRGp{F3ykINxM|>fGWhOIcBt)064QZjo2@lz zemIPcZq$xRz;4#i(nMx#jmLRy86|{r#9+V**NFH+z@t&#u?E~a+@WP3^TNpVz8u0t zq-195qlBXn4WTgBB4h9s>xhOc?5bq!em<@}M?8B$!!grPN_>0LPp3{*pWvBf^kv1_*n2jCqBiQ&zAlC`QsQ9JIbVC zI#<(x`^ds-7yp5LDc<78sYP?tv+?=4L3jj7{kAoM6j4f7Xc_KHVCCdLo3F_ud3^uU zUx^#k8WdQe%P37=`;VCe%r!LoTc6ALB`1=VM3vzlj~IvUuCE_ec9pD~v)9 zq8`WiuXOfFQo}JZ82^_eB$u`;tLN2gHyT~Z!RHb?h~NXQ+!QL$u21h$?;cU=hGK+9 z-sznBl)lcEDt3d!m_A`ojuY}+$b~gOQ;u=SGdwCxh_Y0cKo6#_YWgy9)2sVCyeWcB zHFYBF2SLVkRHF4UM?UN>XaA;yardu=VFIgta&4|)5;|NZ%>nV%=G=DU!Z4pD-oyVO za!kOzVem^JbCa6|J-7SU_%s%Sd~EE@aJ4_TTHp+Y7VS#%UQtIY2oaRG-l~3$IOscc z!tHIe=hmha0XJgsMmf5CyHW=wy;En1_o7e7$FVH)y81%y`1*@e6`md1e{|ebYuikYme1E zCu)Y&d?&eh!3^SG9rNR)txKaU&zA_L0Q<5x+LF3~zsM8Fm1nqV)YQ#YnTC%!PQ*AQ-S2@n7Q_e(D_8MgBWe z2N5jd=_lMoW`#7hVMsUz2ACYB z39R;Qf2?qm@tCUcA?59#5qRif{U(ut-G;TIs#~UFuR4Fwr}+A zfWM63!3$8L*CFb>c_=7z#H*?Ett6?E0ulI;hsDIFiXh8N}{kHi1 z+yp6B-}8UVB});atf}~{&VfI9r8VwPoH2z^7x`c~{>V(2l7IZOWMr=hN!SaO&xzZ) z7D=uw9epa|`YN*YeQIcS7#MLxREu}5k^6>bsy78#+rSm`eu*Vdu_7Ar5J zmaD`)CDwGgoc{w1tE-$*p!{pUMwbj%fu2BN-dMum8=AEDmFfYD4269{bj|V~8}kyP zV>dU2WD<&^D55uj43`*OMpYFPq-_=UHnqz(G}`Y^=#hdOzLmfKHnzw#(2A!=!q(cDCSWXhF;Z3=@HkAzqSYgJ z(g$>a7;;@@Ef!E!P)(7Xx`q6u!N(%mp4;WTK$ZYv%=`-ujb&~1hP*snsx-z9?)J9j z6ZlAJ5eL8KKshex+o=@$94W(+c_=4ap^pk(WgeYryJ0|p+p(f5kt+YOTc06EG=Eo1 z^hi~yu~g*EVNMSp$PMvhZ2RS=e8!->RI^KS`^E-^%t7h?<6?|~nobEa6NF3`J@dFnV1ERX_G z-YVBnE%ze5n`g>#_L@v;mdbvLuIQ(Dv6@1)ie6(!BI@9e44&UjO9Cv zGUeEkdplV7{>Oa==iEt_K|@`mJPLs@@F+k;N7IY>U$rrwYCvwpt1w%pMKqw2^NigB z_|@cW0fat>q;~qI%uQJGFA5u+{#VrgvFdkrZLgP z0=t5ZImw`J2Cn3pB-K8#H&k(PgZ5id({+Rtvu7M^x`3DTo};KNyT6q^8jUH&)$YM9 z&RftqqkSp=){%@YMPpJwUH1HgCB2k)fUzS}aD>p;Y^DrRw7c4gm7! zlUI1AXxqvraHxyjO)x@$q7T*mdDdpAU{b*LuD*z2#2Y46dt%$}42V^LF=!~c3O_~R z6{W`47Er`I(Q$Ni!CAQT&q0AV^(3K}0S2&-h13Cb;tI;Gb!9dAf>4=N z3C8E`IykA(r)x`}nNS8fAXu(;q|GfK2){TrE1eC2t78Ll0x_774bVje_SYf0X?JN3 zA_~!E>V*|vt>sbEQh5kHa2aS^>QQ~^fB?aGY;VF9#@%U$=^%@9BFnYYKw!vS-sHO# zqx+k_Z-b(^PxpmPe8iYqL?VK2&sY8D#+BJ*c8T0aBBhHttU>c$63ViqFTzwBbCb>F( z^7xt&b6ejN!&?_(O7Kzb8V$ZAB)C|#VOOFbmjSO|{l7N?@P*5|or3mt(l6rZFdf-7 z2|NDqHiDIp2`Cj^!@5#T{N3%r>_q>fR~UXes?rKha72a^43g5wOXUYkM|Ts zPCxwKVAT%li7n9~n4v;t(QfpGYs$)mKE5~g8m2Q?CgiLwjuLN^P?%zJ9Oj64spJ<( zFW3Y+2d(2N{4`0I(?2%8aqM?;l~uoSCq(SHmIl&aAfcF9(X^gav#0Os)3H%+?>Sqr z538C8mOoVR0&BtXmSl>0I>fcE_E>+~^*sMJf42i-<>6RPT0p~#0PlQX7-seSy*;p| z<5-^QE!TY@U>=My_=`}vMAVf?i4J;u!{@4A0U}uacESs6JU0pS(a!3T(;L6Qd_q`( z6bR-96a)wn1d41srFTtH2)U&PC#^ug#f|+P@hguF;a1c8t+bpoYj8JZ)&21=JQTa1 zvRS67oBSNGC6M~2;uxDAjueKH-Dl_NvGr&ONeibW_bcCE@47@4Vs(Y?-LCPw`=hCG zQSZYt>CR}rsz)~7Lf7End)0$g!T71&H1A;cT1H*&4g1_Vwmjn`<2lozc$H72_t6lm z|DC>qn4~i>$BRjH&GIp|+(xmpZT*G^Ui6z^;%z}EX|GzKWx0v(q>!6m+kW|tBj;| z?hRKRcJBDKo%?*&TNe*K7*+^A@E3KU>i%8MBsP+@ZE?w= z7L{BKtk2FO2Wm z^VvTX%e{$|+cmSmm~mOUurt}@?^E%%a5-gRw2|d8{xMfS8VYmg_x=-_ti7p5r(_bX z?KKCFqC2bi&=-)HVp1+v%8UDJAP(vC;WLoP0pi6ugnsX-xLfWyo?{=k>CbPm)jueT zH!paZ!XOX_`!lPK$*?d;`kPbq5X{!sr|9nllVo2Ve9mK=Q7}$K@iJn0jO#Du$mrmR zh7<6x22wQtHi>?40g4H03s!C*+{!FjU4Yp?b))=$v`Jj$bruyu^!9Z8h28c ziY!niH+^N6(I;O}rz)d{C$THBMN|ypq=<_&5m#UPUwiLsd*K(prt~E$(%8s;jLoJyr?=SP(+J(uuJ}~ zF3h!`#OXKlA#?y!M(i8bejlE6u)LS}T{a`719?)3UtTw1mqa-W5YBMpDNdcFFGy#F zLXAa7#AYStzG>S|>~h4-Q(Jd9aE@`LsDa{c{Q>FMb@KPsaxw#3QTU6V6K<;~4RYFl zTa|PO*3G%u`*fY^b7n_DV7 zS{OoDRr+Ri_u4liSdQOSaFPB$LqZw*n2#Bvna2b&0#lg1`Qy;&ysgOio!UiVJr7OE zZ~xMSh+j#<3w>MKN%@udA(H&IWY6MAU%@P1y}!-a9VXz%N?}n$oOBL*F0eX7!m`97 zkNn&`bw2NB-tu`&jvT?Y8j6n}C*6&-KMY+etSgQn?i^0SR3&n(dv8da`gf)JjH@hl zRBIIyP!-B?FZjGRJP-MR?&a@5XqB=WokY-m=5;F!n}-%xrwsK&C*SSHsuFkdsuZDQ zvO|}RzyIUYTxfx;d>p0+e6ibWo$vjPAXjYcTVFr z7X~e#Ml+hgC-rndP^;@kGZ1tX?Z;vdOAb=9j8iM4dt5A9n2^(H{HJzk-4-p+N}!Ty zd`RRVA_fD@YQBnG+A(fhB7#2Jho{9$eD*apuX58GDav0cZ%!%Okk8b91c6)Mt3<$1 z#N>@BOL(z}93hh0z%)j6@VVvB$sY{tF9N0+e1mZJ2V6N#TKH3S%Rca{?33yB26P8X z$=m&unZkp$0TBhP%|g>9!^6=DL!vl=%YkMGBlC7KxC%;$Tm)K#&rn1cBvbskx~fB0 z`}N+XY&D_OsQCOJO;;I6*B|$>naSzyxLn=c&E%ES-8pS~OgF>y)eJK|-Q68i*K{*& zp5y;K&kJ6kd(Q8C<5OqiZDxkKO>}6idyS+Q7%z&twJGs&nh z>rWF2n9Fr(zcUUaOTn)EK*w1{@0S8Y;J;hWq%#8Bfiox#t zzTWGPh{384$OC@{Of*1?DP>yR=G(ZQj=kJNxhCX2)ZN#X4N7keP_7Ti*>*!D|H7s4SKv^aTXW=VQYZb={38df3z5FCFM##NV6>Y}1d9@=se=;m7iVdP>VN?`&IlR85ocTGS>B)}$ z$NZq5dn%EKcZJQjqA- zRaNSV(3bK_jA5HS9jfve450i*LF@|+L!XYBrq+FvIoATzEXto& zYwnIL_PTY|`{O5h8Ngv>C@HSY?a>A@;HU{6-CqqDhrT3$L@0v~p_BOi(;1H|w`&oH ztG6bz*cbm}oe*Al?mpXF;Lh%60xV^js3;{<*sF5m?0E@Ua5wwU(nEyoU16q`e&S*c z9Ae5g)V<-U>qrS04qm(4V0^NM#l_pbCMbhepI4&BHb>^3UIBsi5B$Ou>RW@u?6q5- zw-hlyqu(b9;zf8CII;wKknwr6=gk5nH^W*J_1RuX8G}~O98_vuKUY-67}hR4BS-m7 zcUh_90t9J*ZH$kJ`iD=eM5-co$p2aQBVR{=e(>Jdi4(2SVO_XDS2ESF&MRYp#V|>J z)$OUPrK?ejVuX`5eT`fgoy3aMjVWlOn~Xir!Gg~Vzl8P?edeVDfd`6|SbYEu81ZXN z!$4(o%YY{jyc^~EkV6A|N|MWAJk%<;zPSL~an-srF+$)dq$o}wW?fyJYa+q9{`_1i zs7%kRoFr+P71f={N}Z3frWJ0T%k-~xhDqrxTMDl1cTd2aM!P5)Fg);H*Bf{79B~Na z^pEH!S7-#69#^9rXi^l)Fx0Dji~g|AKcJ4ij3FWKh}E$5uRj4LU6BWaaJ{Y-Nm=VzS55f8+G`TqWy3_B z7O@*9dcGE*7kVjmf>w&{B$h=C_)?fw9hbF2p##HQhD^&s6>Ygus0;Pe9@*rtZE-{c zizEpm6SqoJP-Mo?=za~#YEO6g*oQ4~u^Q5BYggM5U<2xrqBuDsQFKpPg`FYLV^(6- zOoN*xy^{qwQ(Z!J*z-SNFo81E_ttwW;|EQ}xzEv5_|blff-2!xX(NHr#UYqq5!qIdi?NWT1p=nUYQ*sR4hE z(efi%Ec7tseG_XH>nY-XYbuDrN}^Y8lQMBvN%^Nq)aO@R%v|)0BuZ_Rf1U4p2d6O1 zO837xv+X9wBEpp~L;Bx|A|-5~qTl;yDu!_;LJ95`VGzZoh?5x;F*(f-$dqN8)>{n= zLIr}%GBwbsPP6~Is-D@(*sjWwWrN@?1GUZOer7!oOF3eZM4pS{ceseFDmGnVo1y+v^zOAY&5P_HSSu(TZlX}LcT1)==-EJ zoilt2u5IzB=dlilWrI~IdI9mP_NCPak5$d`(eX>;0pKsb4VQRt>!ET2QKY{KAEywu zV}zar8AfvJ(yG51U}oo`%;Z3ri$I;|@f_1LHy1W^zR(P!l7(s6d{#@re7U4#=_Z+v zTQ(utjvnHtd`Nf5SInvdz>|r#ijLVwG?>C z-9gG&6G!>s(7g__*4@Y44eNN35I!r4-ptcLcF~UF$*$GzmNOs+JqDfo-0n~Z#&L+> zP^^~{RC=pJpNbHO{zNP>zxkvl8}FQ1u;jT*A_Tr;O1H&)&8Nz%OnNu5T8S9Ji&lEZ zhIaw%6z*9K3p7v4(L$cU??g)_`fj_P#p$AHRGrdA-uiWDediwbQ&|N2ZR)vG8X=80C*#4BXJnzq-xSekA*8Llb4E5jsfm`*@T4c^lmsjd|d zcrcynjZgp-M^VsO44uCQ7iVw#wB+*;OPuvTS7SjP(j0XGd9=aXR&A1e;P-IcaCtKas{(2Z+y>RR4 z)pG>Fcx$YoZhf4;aC4vVI3VfSl3W^Tqm78J#PMDqCU8BwQaHLR;PU>PhVN2VpYln` z&}v8CCRhR-&7)MJ7a;UKd(xpY*$?j)2B$DeJs@YREgic9W1Dk+$g1}F894@EB#ly- ztt?QcN7i4)P@^+ju_I{4oqlOo!BAnH0&u10TWSxI!t6=#${#=SL9iUIj0HE#>pB7O z$uE($Gr#2&5}IKTZ$WH($2zG*;FVL*W~$`-au0=D2U4DoK@HZlnMugHzd@{u^)K&S zd!x4lWxRh`edzm2x+Bp*jF$1-z5)E=AA+9EGe5dQI#d|bi(mBmVY z9`e;s-1|PrXDqJ+m_pkhuIuNNiFg2D$gEHz8dS0HgZ_^b2CddoX`q5WS;{$8;lKhS zTOC%_>BA2TSMFfc*b-7}KXeOZ3Zkk86b6{|o&YAw`ri_>#j49)m-T(Nt)mRSDRKH6 zzbzKU9gg>Xg1$M2wH-k(e`$KE8||sdHqhvJ7NyOLEg*4mq!P9jM~8QHV0BQ+kpBV= zea5C~g5;OlbqyE?1Nb_e`nlg2!?a5-fI)HGJJXatCHxzI{%UaQ6aurnEFo_xoF8ekLEP9!RL|K)v4Da6Tpip<#7I;Bx2 zy@i35KETK?!PnD2t1op(`j=Dq^j=sKnXW)2*kPk+p5R9%-_G{Xa*uur_U8|9M9E7O zb)vBq-lTA6YpHCT|I8`6ZbToMbf3ddyNl+}V;)+n%V^}Z(EDo2wBcBs81D?SI|#(x zmD3$Ke>4dI1EFr4~11IVflIx-J#;JCjf@x!Bddrs)pjlH?i=nQLNLGxj zxK^Af2g=(zYlnU^?#Zs>HTyvr@GH(Bhp4xasTy!W@vIKrMMjFfUOXWXEEmHEfgOZ| zMxklhZzKFaXly?3F}Fr%q%0s{I=$w9i$_dhJ?uL=>FprtJJ!n5ZTZq9A_5xX!yt*q zoZj4EyL*un;AmiNerSRF%dnJ+4AM*0PC3vBcblS1+m2@X_QRD+Hb6hFE_#^aebgum z!ulPzGO3|_B{pqKcJ~)u?pKfZmX}T?4B{5O-ahBZ296@4XS>TG`scKUij^E*%+(F# z18<7YJ|4QgTzymh1BGcoe&qY;f%Bdvf$!IP)Wqb+6v7;msVJN7Q!!!`$<(M?Zmnte zkI1IL?12nU{@d$f-?$#|yHf7Y@OKSE%A4zQ?o6Ao$ z_lYU-2h`nOWWxtiT3_NP%;d}7iK6BLTM*~)YbU4!ey1eC;HuwU`;2k#r8Th0VLYX? zWtO8{yjqIOl;Cijj{7rG34DAk=b^9VrFM|>Ut_{;OmDdw9Qbg^w`2S1%s8Y?c84S( z`5V*0I;Fw5Q>HR$m^7V$JW!1>^pN+_fM;UGbo|{gx9heYA{_W{#7OT{==1p8H-<6- zUKqwN8x?7ia^RdeTiaYF+4mQKqo8OcX^)vCLQ~^d%EOWb;s*77G;YBz$j#D&)N~>{ zpdmr=t9t|gqbC*K)bj$Iy_>{=;M8Pq+w;pJ(H-`tRlzu*HuL}ZiAHG`dF%E3J=U>e zeR#DYTGqftEsBe|-fpc}JaKoJ3o~Vzviso?p1kd3rLlvkQKO~{zyn$9@F4pHK<@%O zd}!gdt*%U0YZz5lmvq4!@*_x^mgLzs6Tw(NcjcFjt46K&X(3weBX4~}c}FgvYgtNE zP&ru6V2Kq-FM>lR?=+&E63vK1dm8$RX@uY9p4N!}($jVgxvqk6z1ZY?b)C6_TL!D! z?~dsx2#~hmj{&r~`n8X&zdv3jm#A)b?w3}=%q9Z6mH#<(6a*`uZe zSbzGiCaFOc0W7fKdW3{J7q6*0e6Pa64`u=Fn-0p{bEs357qGI!kpFm1o?_f8hoOuQ z-lZ!tl4TfvHIm8rIp1GNbDk%h^8bZ*2A1-t!t8eI+yKyGjWM#EZ~WMryJdyXOS*ef=W!N&{vF;XUIs?RvnrW-T(fJhi-kv57O9h;EUI7cK-Aq# zfvl7WvODVjU^q_MIPvAt#Eo*qHEj4nP#pG7f0XLT>*q$zMee}ehYb8|CIFRS>;c9a zoEZC(pf_OFV;-iD)7#qWxJAZMO4xu59Sn_>x*6dz2b2aTL{p!h#B2P;rw_#acV#{; zRhz0hIahIJOrAUir&_+$-!%PPramEf`IbN7rDPZQcvgTY-PS4rcb&HS4XaiM*8_a3 z@WbWHL3z+t*&54!rrII+*3?9ZhqtWzvmxMPeDd$zi9WIFaq@=tV%F^eV!1wq*hG@_ zD1sJ9R;45Gou}5V&n!h!5T9&M*2gvemV@?AHUm!jAar+pwa;o5Tm;DTcsb;w zl(1S!t0LgHhrIUq4MKr-x0e@L{crlsiM~MOZ?>Gi$8JuSt=B(jEATyY#ll~eq}4n; zHa8`>JlD82EduiQLOY~HXaLfVS5S(u529u6t``{dfdk_;Hdr))!NR~Qj*PoiiIi^d zycr75pe@}{CUOC#8k_G9fPwv^qe)k+seBBBV#^vl>3^Hdg@kl(2q6}GAyl`VM+tlR zznN~aoE}G1i`%m`IsPB}0=;g!#|E&V?CC!4aO!Gf#5l`M+Wi?~P|sk5!qjYZ|u zB$^=?@$q-BheI`+*tMK-nR~@&M|Fuy%zKqrL(N-B{+YwHlnSFSk7R^DE_-*v^ybcu zf6-DLikJhtbqBg zjoaXD!ZbSi=+g!gQ5XIi{gMsEN0mGsFcyog9NvZySR$xLhJ;*`zav;s37$Pjs3+IJ z?XxiGuMUWFOK1tI8rTtm6$>x>#kk&);`W8plc2&)_pw6sr&P9mtDCfCxoNwi&5=II z#!O@@6?mf{O13|!<N6zNHZTre$AJ9sPKi;qEgtZ&0g6ope*-ycT`+EiSRS zy?yYx;p#WobKbJ$C@f{xxk}(?U8hG0u3AOt!-GU%svgLul@DF9V8UhWt|j z;Epw}^GOk_Gb(XBS2C|-!Fps*!KJ+Jp7M)e& zE;-4ng)_4z6vwMO_YZ}uZyuVJFmgmAopXK3mEFw|p`5x#cK+~iXUBoJ!lLd+rZ0u< zv_U22Iu9j@fW$V5ugXP3*Z878OCm#SK%j8+MLI$m{0ryya z%hiUKx6(6I2LvUNDcoJiJnMysUafH9tr`jwQqeAYCQ}yghikq5c6XNQpu>{w!BAzggD zNgJn?wkV%G{ux#8qG?rtlY?h>%>Ko~qOy3jA_2P6B6B9bF3+Tbxf#^8jGxaC^==2l zAw#o`v7MkAVKOq6lfTN`%WCVi7PV;M%rD3PKEvBuIp4^S z{{5TbZ$zztgGy4gk%Q;mrrWkP^YbFMedyt)#5HJtDXHUzo$WiP(5~&gMuI!p$Aett z^t^FK|M6FIVuSU1qkh(Ps@qq{$;u*cRc)kLZ!eEZw!hho;SPf`ki3lY^dSfPHz3V> zZysDq*3Ip_oh(a%Be#EB*QADyQN;{1Q;sDZJ0%o5-M2Ot!wX@;xpnk+{p<3o(>TkQ z{H8JssKKLb+^()1jEr?Pn&4E)1vTSJ^JEQWtKmP+ruM?QtKd5Da{nIyvajjx=^dD@ zVGM1M1(Z3Zni9e9pn^nYtir^WIP)41BbhSd&kj9h2i{z9h$6NZv_l0ReBxz7@c~<~ z4Dut>DJL&i7fYfRW8rA$?0Y@CiRn2b8oHwS-KhwT+y}!+JY}__%dh!4+cJvNl+(Zfc;QDd*{G9| zlPOk>tiv?vI(6)r-0z_j4T=-z-t-U%vVSjqFG47Td(t&Ca*ol&sdv3Kr}+~t+bvCh zz?EoVN`i(F?&*rO-Sq$wuHamYJ@xyL_cYqq5x!5b&rFqrp(J>}`o(5ur*2k>`v@&Q zsVMNkLgMccxJ;)IbScG$PfSh3*Qg z&DbF%J@ETs!0PF0LT49dle-@?`@=P0?RU8rU3q{bsoXD{LXxSVl8mJPQ!0Gned@8yH`!vBE&|#*c|f1Y8}pk9jMwdfK4I^e zRd+eT)BK0?R@g%U3nRyh<*YJ9$c|g2N@ult$wp<;~1;hB6mRL*K|1M^SJYO4(HvA*)#PNxPM@D z(DaV@?`o=oJZ!`;bVU~mQU&mHq#a9=!N zSkK1JjVx`RaecSud2RJglqV_DJRA?dsg6*-f1mYR2$cm_fRg0$5_jN&lULXf$ajK3 zgxp_y*qdFRraxdp)wXJN(6M&g4ST5XWyL{skye+MX3?GZ8Ctf|@^hAm(PDk-xl)9a%H-wQZ{34JA~s^{P- z%Szgyw*83-XcItq(?EjxG=9%1VG@}Pkpa*mSWJ82Jz{@*0jt zhcSjk6c%-Qj$hXUp6Qkw&LWPOs$7Pn3#eKn52}6lRvK zPZklW!!V1tlGoVbFZ2EAA`+SM-CN<`@DaZ0Nc(x!42mK{m&yq#$XLIFed$BDAb`=6 zcyuUYHnZ94s8s~kM~x=n3xF4sQAGEH3Ihg44l5E_fr2DeT4)8VoCxcmvdv1h(bHXU~f&PFM2DBq~a$8sEqlb zgf$h!g|MrLDb%D=h zeC=FyTatsf-690tqogE6)nBe|leDDJ%?$QS=;+_&#=cUZfZ+~b316#IAb_DA{gJ^!>fW+5a8 z@_^);R^Y2AYml$f25uN(+NoQq>C%t4kzW7+$~%UNCxknr>@$|b)SA*z<%9C$AY7ap z5z>7mo=DTr7yvZX%@#rHG!EXYg@Ll;s~)Dt%W?)DlqwbA!(~5RiY@w6?6H8vp3s&3Y5#K?uI>v$z!fy_PZD! z`*e(NOm~w&8^`QCc-ZBni)2`<)tz!OSi6a|v_-x_=$!bCN-7X?`tLsZwrt=zQ=I}2 z4*YSNjVy#e;OxVRF4m5|;;=Sy8?`(#JqlK0s8n>1#hi2-7Y!aDrd!1zy}g z41~XJ{Q3S%gv=HQk8k-xEr=R2tj?lhFqL2k>k!w=5C_yv;5kCCunXyPevGYeG`zViO{5jPyfiQBf0&;|xO-7Z61MrI88I*UK;zz;d2XS=a7s z?6NetH`7v3uCmt`0ucFr-~{R0DR;m=+{kEjj{2l9NkrT@rA5XA2e1y1{`Q;4jMr(V zY_F06PvRB;PJ^Owaf6P8XLW*MweUlD;|8CTKZPze3Rq(mwEuKv!M`)(wW!)2(HCn#ei@@iyb;8wr};nE=Ustpb2P3*iV&M= z>38~M%KVcPp!+smj;v^g=8U-&Yrtr<*o;(t*Pdm^#fOx|AM6!y6aV8lEzovWPQ#Xg zs2c#+8D^}5VR~tcIjVyAX%=GcN`Lws73#&Cumz`gVm+TxeI3eD$>cvtVfswPiXt=b z=o;4FZ8E5bNscA@Gae1UPL^l=`d0-`mSmwAsKR-XVzF!h^;5whot*Xy!tIHjFjgOA zLyd2iH78?Ed5l>#!0AQT?2PS5sPwmRi1&kN!aCm}X7o!spTAJAt99)fAKwg=}Y%@Ng16#4nc48BxVW=RfpFo2IDjF z|FmLTX?K+WuutFBkf`p#&G{H)1xCTcSSNorZjJd6Ap4^BI5`}1>ya{zJS60`1nGs^KW z9H>=`0kCz9?V41(AIOJgvq<_>b_>;yI2*Bn1WyH4$e2~GVH?NVGD-;6E__ZnAM52Q zSops-p_oitmA?Pe?|hnRWkR#9*UQnS5L9Zjz`;wBa-Iiac_%{_ZP=I!64NaE3Zkwu z{CBvBPMMT^cguLIX2qjVtQMsr>#bSXAP6Xg-asS--2I3M5Qp+y|9<+GUhEiURuCLF zN3j7koYs}y3;$AtAz3Z%b^U>2U0ZQfLY)-;8)bP<)q;W5PR-VI_dW=QuaU(A8tW*9 zA`v%&dnSssaM^ETgj+f{46$LP31(D-1o5=aC>8~d&=T@=fuT++l+I*M=x5HiIm*E zN;-LLb~2c_x6Z@FxP8}n%WPJ(I7IXv{w$i;J%j(nqVGKm^PFDXgmswP2Qo;i!xVgt zNAbzvpwQyXW)wQtmWOiK@sedtose_Y?h2N&~FMcHdB~Kj_75H>Efkd`OM7B z@OiUynDgIW$G@j#?woIHdIn`C_u%xWg-7S4S0YqpWaY)0c>>_HdHfF2FWFD#mFrZQ(^s#?KFBxp8$JMfB}n#}a0MZZo{_#mn{7C|HvtNw_hwn(UPY zC$qTbVEd72h^89zgKP=a&rY1JhbTbHqpnOTJw#&k!vxeygmLAO9S~Ob>SAk z_ytJsfUw+ixW1GrBxVy~IE9|pMBrnm`m?p-g9hs0s^!9j66OcToutyb2!Yz@y=1{2| zzZrrusxvlK!9eW`9%(U#2k~j35>3i?hRhGlafU2tk6ye|JZV0K)w9OkkIdip%vRRd zy8raiZ%Kwgy=&@HN^wD@r5hJI5dm=wJ-t|GstE-(q4@hRsD(td7P!2Dp}n%CwJAZ4 zyIT&%AFHMB%H+gCBzK^z1lRxL50yod*=Hs^gEsfpaMma1o4ie5mGU};WO6r?p=2qA z#U!xz6^g0K`0XJSmeu{`gpzUi?RlbzHvEDN1ft}4a4c`bG-op$ZO&eZ1h1LqHLE-%D0agPOZ?A(yB>%G=#Kb7jW98=us$IJFzl?NCgnG z__3aE;qK<&2@RP1}mQ}u(ne50J?6&phneiQG{_v`;$ zhUWbWou{AZc6CL@S)R*3)Ft4eImOR`eAI2!T|Bw*nRsDs$GEU~O7Ec9D9sxM= z&!5s#@#oBhZpr_AnGvm;Z=*s|{qXMr{ibuzxd7jY?(IF{1VR9dY3f^qv3(LF;#(U_A+hrcE(#@nhE+r0in52r3xwB3%ObfQqe5Jk}t z^3%6g6DK#4Ve{k84xPKL>fn*(Zev0eTEiB?x`%eRciG=CfRqNGsfty56o3D^B<##; zwx}3}3HAXqv(UAp?2g07%4$03>U}HqeA~I+TXTf20r)hF@No-=!W}T)p8ev?Hfbdp zVpT%^T!A?%ceKO&Q`xUP346-EW*}=4acJ}|N7Br!*^2O4zA1**hiANGPC^|JB&|T* zj34mqgWb4JY?!o^iL``k;x~<6{alc|YK%+k2&ot}V+D*YoKJBBB??E*#_$Oaf@f}} z5wq(Sz{=Ak`e|_lcW!_qhgZO+ivIV!*A#Obn^tTmYmzW^8NXfEssTh^*Qmepvhl~? z)r(#Re91j%VkQc_G%P6v_`fibpcyd<)taK#nbM}O0tSw<=WG>Imc6Xl(NfvW{X81o<4^q3O6PdDHV0A3FBVq z>H*;V$awofOA-NGeYqf(a(Hzo=z9r> zN+zN9C(UiI41{IzEYU{p-$2ZdU2O02!=a6byVF&lu0XDy`4LR zuj&oq$IRWvqGp3|wi|Zc1Zv7KCg$kD%f-3Xw8iJ5B2L<$fOLFfKyK z?h&n2FLq8iCNwCI{WazRgysMSf*Q?S(l$N;VYmpQ?|swwY1TopKuE>L18`p%qPE^; z5PSr-B-t#2j<1&?{0VgzL&v^L-GRWjpF%SEzg1#Yc}8R|<|LBM*t;q`(Y=V@CiVH> zquR_IC8*XpDk%H7#23CQQQf-ZYWz`o(=1O8ayO_GC$r!ThM1Q|xNJ#p*Rtn_A*Q7m z0U0#V^9d{Vmbm?PWztCXoEfK{C5vRPUgxq=LM6Hwy$0czFpJ2MZyXA56ZO9>g1Bw@ zzSJ3abI(J{Ur~yHIp|HE_|p}KUsDEdb(vG&x!SdShQ`E|8xdXt-le5WviwTTC{96) z?^(S?eXy|8(~s6{Qu~z0Ki(sLEmn7jF6p-XW~qE9INLduD!gSsD<6vQB00nH1%-%@ zl}%0t@je@hhFqnYu%IknNT*tTTF4RPBy~8Lxp*R+Mjr`5L8($WC2z%l^yckJq$ag9 zHo5Ct`i-TN*d@*Bof1PkcKv0n>6jT?-uR+*p^!nbzWg|fv+V5fn~(ob!%Mirsxf!N z;%vSEWpZi8(Af!xOP$3|&Eois9gIv@Asfm41JkdM;HUyw>t(0lpa zF@!B3>CHV0dzzKzxP7hn`nJWiKLwwoO!LcYHfE(CjR*sOYCE3*-0|EQeWoZKFkV*j z=DRVktY0cUjA&_-oAc|1Y~#K5w_Tc908K+& z_uWygK|b`)fKE5F`N*MRi>_1EN6g-v=*QgkdtFYfUF-Tp8|TU^oXkkL^(!Q*7(1{4 zbaJit#-h|z2SHlU9ZrErDT;(lnRkH~^vT)}HP{pK^HXCeWf5UH=*7XS&YhrOAQ{}*ivO38kNw9@JXm!wAdTX}lsky6T za=OC*6v-w{>!PV4YBZM#iJ!5X8qRUwx?ZtglGYg`#4Ccv{9@nYG;4|b9U>G6`JD@? z)sX?eR(>2wDCY~koZKZ%8TZ$4-TWr>_p@W1`@6OAR=(D`83SfMbFC24b;p~j;R8a+ zN5R|mLV1Pu+ky#g$gjne_NwLr+$ASs-5s(|3^MxC+Sa>21cd!0h1}q8!dtM$Z4D*%(~`r^E_I)rSbLNa)GGZ%2Xqfm)F8gh}yvB}H`uT!lCWE042uadFDWNBk(<2Yq&zMCj;|*rliV| zKhG@*)J?dvKW8V47X+V}zqfgOKxbiPr{zf{4I?qkrvn})StL`R$}jx7q{xCpKQH=t zw1LhgdPq_g*stpp(u=iE{pyJ|`)pcfhFD@WP;T-;+?HMIt zaeY9?w4B~M-n9Mo89SHSG$gkko-2D*aYk0n4%-Uxo;Y!7{v-$g34aG)uxEiznb2=UGoD!aVLGuL zA?}8Q4<+>UFj`qAWc2w9X zbV`H7r+)+BffadR(wa3L>H1}}jRbsI7@4n_H367+!6Op%>%4my^uQaBD*qssK1{K~ zv1rrSQs#CJh)ViLqI3HWYbp9!zenv-V+M%s_={R*uRr?OK%N-aUcoi}CQ0L+w8(dw zdT1r`%7Vp5GHGW`R2!gRckf;@{$l9h>{u_KCRzvyIc%c^SmFZLzi%)e_fNOBwy-g( zvKao<&tlp7aq^KAs54YzY?$LYzHynkgW5SlL=6U<;3DZ&kj7^3P?~npfMpJaDMu;F zz!+|+aGfWP-kr)PYH=esa?CpraO~&9b(-uLzR&}f+qbx*QNNOIVC5AI!#3wE7rTJ5 z@&qbm;&pL?f+Cg%0mMpF1i`Z;d{2qtU%*(q-TWD3T=zy?kl5G;3!P(94;gRAOi#9=(d_HYkt-gUf&tmy@ve7-DgPO3Q z59uQQ;j_7&8#v`S-8jM|@7yg2SiSB|(l4td9@zd)ieMUmz4SZWgv9AajrYf>41=$?;~eS;rU!i>gXR* z6_A6RTmP_N?=9mWiFLl5#i^KymbbZ03Y;!}8;gnZ^3^hkSUR+S*PKqDjNe^N8vMx!z2IlJPrc3{CK$F$CAt1n_&)$9IyKUa5nk==jYk7UwDQhW$ zgle{8YK#PB#eLonF#As=8Uz=+G;QHMKz=DtA02%%qoMnB3%;|5z7n`chTa-b)_$V~ z#f3Uz10a=41)nXswYK%U_9|sL^BXr=H}R3SLkfV%p1r%ZH*trOJ|lr zm~wU8%Wq>%I1{GBtNVUGYv3q3f|CpHaqGtjTJ3Ap%|n!bPDH;+a@CLMmLcf3 z1%9|W&|F#Tzq$a!0Nhzk7}&tT05XTrgBMBi4aM{`2?hvOZ0tvUSC?Yjzeip7`*(?n zvU$N3UjPa5%Agpnh*4oG+3y@SjAycibTs|H`7bW(H&Kp12a!~DQb>-aU2Lnsc(1zk zNgM`>Q32TtQqr%g_3c85bbgHJ=;-(S-o8X>u70Hx6|)rgCzoSot;%q4;N53O5gAF~ zlir2!#VZe9AIIBfeG$aB={`Q!#BVsi3nb=y?&5`0D+&<@qC?;19~%#Q1JTncoXocT zUO~mkSS&|2*mPuRtWCIm%48^b`KjItWqJQqEF0Ie*vqL7p|&8IPO(qq2?c|<<_Q!s z41j+6tq>e4=U3~(z+?+ z&~`rwG#!fl#*i2G8l9+d_-}%paM&n7)wu01t2XuUbjq73xtjQqg8a0_>HSyT(J)=i z!m*_#tdhtW(Wq(smoXXzAhC{u2)6e}v%Vr8#^IxD#}j{%j&lQt&Va0p8IXHby~`TNE;z!l zZ;nEuP5(3!t-A3dF0j9L@nfx9`8`bFA}f;h=`KgL<T@7O6_meA#d+zo-P=?@g)|Ii7KYmM`Kbe^CKg36uYxzgw z!g<<#4<;1T@n}iDnnE93iOMd0*Y{Uid6!jS+?fc|jANi7O5#457)HO+Z%u-tAsAaC zm?$>wJt(xrlU2YjMZ8z^6BE3Yp3sUt)M&3hwN6V-aI!CtyFO#gGNpE!3w0+z$?Wm^CK@K$Z)+(R_H`9d>djy+{F76v8$hJTGbqqryG znxJ=S=g^c5IB6k5P5Hd*H%5lPV^+=`Q_m$+k%#$vem_Ymk$TnFCOr>x0IZ@AlAkyY ztz57p684}htE|ZM+!{Jxvb3O0UL>m^xVHBQ(zE*E&rTG`C)1MKIj>JMr;Mg;-FaLz zVqxN07~?xLP9hH;{_0wu=WAPH|sLMMurdelF$wad_pIMwtAUSn4B ztGIYh34iS2qSCtr=qwg7KyqSm;33Foo`u~rr~C{O@FN7^u*W3QOh7`@$H1AHEX3#G zNp~?hUI2(a8Q^Kl^DqWmFc#y^yC3sYDBwEVnB2PrZC<3Ar-pafq8E|510&D36tJXz z1&ml=8fy|q(X{nrA5$N29slDt%A1sK$`ZT_KTMwjkC{p>YkT@hJ z0HGdx2mYi*n^58?@bGrmSVxmyum;enBeu({(hbT6*%@)Vp`vBwy@ZVKuthzh*@McDu8cieK-2Pvq(JPV%rFt^j7_q+2BYty_ z@O@6|NA|Y#`5NR*-*pPxG=07FB@}*c`es1vEGCY0I~VvrG+k9dR9&5@jIVE_pw1OaJDX^^4s;lKAj@CeN8bN1eAed`N=4E!MS!n_5g z$6J*fDQXFUlJm*&+jA7fpKAf9C`T!h11yLzohi)Wf=;}$a4CW3*9#z9jLs-`fTC*2 zin2Ie=mfaZDp81_d}dEMjB}$wSA?o6p%e8}2pS^y0Aqc|s{J$DBj%^>_xNA*o(S@m zUX}^AJ!2+&Gf)(Tf9miS154?3LOC}0rtSB-jt~g--`A2oz_RpJ8b>kyZy&ssbF6kN zfLA#^Af+SAMG-(SB7aUF+sZTW{`>^WS)%CSgsVdM?adSTG&>W|yyHeg6V>6}KBOgo zDTuHWDs%N^*sjDI4_d(hOy%@THW0EqmA_tIwNx7c$d}{5Snfbx%fjcn!LWZ;Y`rEf z!N?s=1nW-0l=u1}6gQ7|L@x}Sp=-mw*38){U8jg#=fNMz-}q?K_j?s%0Q0NZs(<4^ z0jPvddLd#zoMmyXrD2k#nte{2hL2JHFD9h67HHVYM&Iyggoi)ZYi%#Cn&@bp*#~M& zgd5qU_{>;85!Nu*BWSIb*V@D=;=gLu{BdrS={b3m4^{0p%CaBYxv#hKv--E^@!TAH zHl-aJp{pJm-O#ScYz5c|c_5r3ut<6%k+$07kC>a^RN6QWN5xhLcreJ5G@R(TXBn2Nj>c~_ zbB~4kY~`vkIPerg@^R1(DV&jU+FmKXtjpL1kc)Zx-4+=P$AxK=54mWjbIsh0`AiX_V8NX|w|{^OqwxRdrOT^eLtfm~0o)%^))Ri%Ybhwoq=?{l0@J!3X%?3> zya82G)7P8{r7*l2CFU0Q!J;qw+m18}niFM7I;hn3tlQN%QUTd|O3}cqkJwzeB^;p1 zMn$qok%2YerD2ea=cMKfU5tT2Kjr9_T!M}H-a=)-$rd4M95(PQ^3}vtj~wR%aS}z& z7vT#iWw9K5wb0{V*jxYpqMQrXXDoGGko~Wo);##o@Nef6g-Mk2??eZ%S5P0y&QA z)i*_awk?kk|4;Nb6ds)cTifF9k+^>c_(52t&uTJpHX-`}r z60dJXXmJ4LJs`C7n9s3L{MFTSsGyCoxG~Sqx&zpfocev21%}w3Ecd00jq1?=AOQt< z-Y3R}qX^KH4YEozV%TMu7EPxSTpCgG71;^tjIRJ(xsTu_Ar-kpC18QYmynzm#1(`t z@yW5|8~r)scJG;Yda}50-%pOeXs?HMJdcEW*H@~3^%=BEM1FCz>bvFvK z^B)UDEu9-)Yd>95cJi>h@yQx)0F(O!9%&f8FvxIw#G&pTn=72uXb|%ZXLup6ckCK} zm2PlHsE!M!+&a;o_Q8G=CRlX=-YxM=mkmb%56VAj;49DBky6&|iHl7x8ljw+ z4uu~eYK}B$y?|aBa(U(pQg_Fk7pPM69IiwzDoN_u1f|hp1kcg5U+u~?bb_e^)h?F6 zC(=kDxAqkH`ZCyk*7~qsPb3|NsQdeKUJ%ZG&Y3rAgHF}kW<^%?+`Z59hplV(6sehi z9FeGd7QlCyOeJIA0jtO=G`{LI>p$LFU^8@Z^vVgNa{!C1WTm=$N_Ve*Y)CRu^%U^^ zkVO9io_);M8(L0ifPCxa{(iA;;7IN=+^+S*wr%=(vo#d$_S)oPAPrqYitmP}aSnow z%ng>n=xwa6%B8C&4chI)74w0hqwV$_9zLAP3Yy)zyqIgap7v5v87`&tJ-Iz&um0*F zlKd#XbxleTy|ms7-`_9>zZz5CyInXc{}bOhiatyysT`jMZR`tt{H42KL#yl2v7U19 z6;LjV+YsP;{!R*nDMHIv?q%I?#nzdLqe)BNM9UMbNy=5TZsTlE&@qK?HD~xX37efM zN?>@aF1>#u|39_X8gGV^&>r^CXy$`LVn4^d@BpFzUn`jH)_tzzw@HEUhnexKfzFt( zKO^9^zI4AP70;D>N&$)G%|{X8`>k4=&C|!u3*PjPqx!ri^$zTaYMgo+YF(nWI;+l~a5J;GOCzr-JtL~~PJCVpyr4PdI7d6r}_lhH&z z?vjCDi$P;{=QD0j8R@s}iu*DKKzfzd9%1Q=dJ^F7z&4s=JuLq0EU^+)8A!MSVHIi3 zj5mf&a_T?dOFNcTa|@9*BHh zRwjPI|AzcdCQXC(mxh3|MXY^9%-qB_|C6*6N^*GN97ib9;^X8!HhQ)sDZ%Hpceah6 z6i_!fJa3&z9f5wsQC@RdI8?3%&h2{6bc=D5|bZ`tLznj1yLTY<#%` zsspid2^}m|3x}W{p1#l~6v8m}kXZ#s&g-j7fCFtbyapS&Z}$B&bI#j$>;G{5d?q)) zz@=8rRC0g=)UWK)h>8kov&Z3zw=7fDVXaCT8@4HcxKDd#8~OxW->|~a25uR|ph~zm z3mu)*$uzrmKjpeRL71b%n$2ZM(P$BjqCJi_dEXQnlR3QPJ^(;VDm-<9z&NW`Y%oH1IYPJ% zNKU%91Dp~OR1aIoz)T)-x)4e2W89N3jEhT9a5=J;MxrJ%jamNSseOg$R~=w!4`H#c zx6-a*M_0Tr|B=H;QJMOy7tohiFv1iI>9^%vCmoDL2;TGV&T!_w$Kpw-T=$z0iwO>r zn4uz_GF*Ie1^S{QZf|y1Je5%>;He^^mo?4b>-UO9VwYs}>+0Q`wU5C#H&lVB+0QQ# zg+eb8g+bkL@WqOTv=+v#6yqt1{Up)#2I)*6aFvU7)ZQpdwN5uUBAfD32HJgI{^h3y z7HDQ?MO^xxqjmwE)bn98UjD`Iz-J5ye@`ZvWt~UT*`?HIe>K+|Sm2XUJAhlACkUmK z@a5nUzu!ZTZ~g_U_wB8EeQ^r~5(2oL(MP|5)Dm@hmM$|6J|-=G!evlB#t?>EITvJCvdm=2SE=P1d;2 zvJi19%-$w<7D2PIhy^!BX#rNzH@#e^@3lS#RQlZ?V|g<=`@G*=h`L_MFlqhSNyrFd zFn4dCi>g_vg8HS$7|)gDKXX*yro}5kfj3g8TmVIG*ZD*sXnQ|nN%BTx@yeIUFbH$w z^PKcrr5^`DP!J(=uLOqUmH`>S?xkj9HC9n+Il}!?6n}N=YN1fRXu7j8uo243yJX0W z*!RsEegXdb!Uz%rx@kuhjJ=KjCJVzHoV;ji?`)0z4ol~LS3O2|M)?m?B|hCaiZ&$$ z8Z|DJLt-D$jczM`Dc1P5m(Q4dAyU=0yrRS}GmARidC!)xCEqPyII;k$5w{DgGQm&V z&WM*UNVVORn2JI(CmVx~}Bl^uloY39AjOfp5Mx}=dnWiahzB568gp==HG z0USf?hXf>?EfhLYx2>25pbFH?8_Ss{$#D=Bkx~A{Myf~jryo?0l=6wuO^?zJ9FUm^ zAO<^DJt7W+QWRViu6#Q#7_)zwj$ff>Ydb-=M741z1J}82j@&2dDXWX2R0Eu(u_VI* z2AWKlG_KP*)&pJPaLFk;0eO5GG!RBRzS&g;&CKaR0Uvtt=U|^-W;vZSZ+I>VC-fx+ z;*GI>8|rFoBx<%qYnA?B4DZFdkm5Q8J8q#O%J~&hOqW}|ua_Wp+*&G|oJ4#+x_DV0 zhqWzwU=)k520vn;cxirPEk?xW zHuL@4os=iJn~cd=6M1XgsW{iGujb+yM@Fg2-8p=v_8Wmza+=mAqLDv-FHgV}zgw#< z`+JWYM02BqcKk4v-m&)06rso{p0%XN_2guUrqc0@zvs z#4FSAI3G2_@#yG&){i=2Q} z3B?jB3iVvMd_s)(<}!oVt8RyO{NeNu=;4xjUnk<_hgDeHj{@psGRne@U> znks{2go?^0Vs$MoJa>ayHAtmnF~nJ((MRf!rC^-$w9==m-28RHgX?{)ZeZ_>wrf!J z|A|X72T%VYnrXemAw@1C%Suz>`IdAub$W8Vd@417l&JzQ4<7#F4kpk zeZIPEDULN~{1|)B;sarF1FRw~25Fi7m4}yfiVX}-A{3X*=o7W*8FAB~c}9Xoz>IhM ztbmxhbFAJ9$1t-dK>k)vn#Zo3mBKVi2wcIN0Lj#=eLZ_7t60`g%bs%8qn$ZQb1a5y zQoR8gy8BqhflaY?$2fiz?oHn{d%2?0ujjSh`RCyVq?74x=o}kk0XeMytqSjwI&y!o z4ZJKCa4g5}@aKRn+H8AVzYV=b_aMYDp z-w|)FhTogbXmi$fc>ud?!`ZcmWu|g?`!cu%u+AFjkz$2`=w}vnPywzT*E*g4UwZQ; zf%~spMRV~Fd}2UkTp#IuGkime!URBVKvg&m) z@>@aZ=S0tCJ0qg12BqoV0?1RbYyLn6 zHF70qx{Ph^6G~jXS&jO_=({mI|1L9aK&h+qd-d>$+FiL!&S4p5yW9w|#tP}Uo(9%* zmpyJ=>SAJ49uKP$ik3Pg(xC@l_D_Y>{Dabk0w`j=(I%{{DT>#^d2zT_SrJlLfV2o&Be<@=eRGdD;)=@S=x9EqHSIsM3 zw6qmwx-A9aj2{XT+9?6jX)7l}?M(TF(2xNV4nLanjSi}eQIw+S$FRSR{lVO`o)DB- zeh)xiS@=7C+Djs5Xi=?3dWHJaR4bpGI*?+R zAoGab@w>lUP4*T5nqhB$!K?KaZ2LP356+6%=<-M=b6x5g35-^yIup4PJqKP;1cjVd`EAmD6kmyV zE;lliRP4s+#3zZ|2s8B^PWE7QQ@oR!<f zFU1hzXQFDU5cZ+wGj$$R;5@86MkDFO96R-(B1ZnB{Fu19H3Sc(?LapQWF&%5Qoyz9qH+mPmCF_GP`>_x`0llFdwc>W3 z_w;8(1nxVWqszT_x>q$IwzkcLagoGYxcQTN4Yz?IXe3SY z9^kmthPlBb@PmplwA#R#^aL*Jh&{VCFaEU@Us>VvWMDxBgcaY(3ZKDOA?zE$Vqb{Cn=mLhb`5l;c|$mf{Rwi2mUhV4BF%e-2M^D!O>{gk49q0cucA<<5!@xy-o-pa&Zfz6~s z$)Q10mkyX`F7K&dU}DwbcA=e%A7|}A21z$IjxG?Gc}w!6W=x^@+!0a#M?ZtjCvXm5 zSLF?-Dgu}tlUZqRlNVW$6*)VgWQ!Ny#RgP5ft)X?#^s-+Mt=(*wu})(2uij+>U${i z3%--&fAr59eGMcvxcwLYY@ev)Mf1l=i&r;Kz1}L0J%?>1KKlZD1LbzcVrG_BE%-(h z);3N7JE?reM5r}~dOj(_{z|nr6&Jm~2lAE}1~KpiWs%{FwZvC!wFz4;bS-t|x|zQ} z9>5PCRQyCSRb-WVzgecao@gdii7)3MmMVEi4B7}Oxjx2hsJ>9!Xolo?W9=Xo(;>K}?P%Pg&^cF{ZP=8z|p2OCyXnqkQm`bmaSlQ=cK zZE_-jx#v2hee=k;rZ1L&%Oj6yW7j6T)En7A$dr8^Ouj2bBOY46d&~FaRT1HI{0RW{ z9B3ftgu;fU(aSv?-l~c9UR& zY>|KRPa!7#96^$ysFJ1+V!%P)X7Ica8k6;a*-lYNihFz|p0-Te4P=B~3C(Un z2n}@jKRKQw`!pT8pK-3$YLBls{L?xc?*^JEI-2le^$(2U5~4y6&2x@+8bugDzFxHU zQABX7P+}HKM}8okLloHJ>~vK5WZRMn$k04#2FVuekU4DaCls`br`!g$`^5PqBk7XP z++1RB@gQ^cw8n`mt8br-OVsU-HV152EXrzhiyc5{gZqy7XP{-N3>TU&=GW2l2y&5K zr8voF7@hB)qQ}ayQ4=^?0~NlQNYo@RHLnPhw_FK?q8HLpdRAz9Z%o z%TUat?z6@FY6eJNsE5gjPw84=$Y6V%OH6!OX)CU4gFUXG8vVKVW{4$&^||~>_LgRK{Sket2s`*C&pyi)*3IWNUF9xGk{b)?NxbJwE}wfj8J z-kC%+rKp&P51%#ixy5onoL=%WKFC|VFxiM`v3>kO1o@m55NLn6nWY{}#_zIDc~-^a z!Ix`(E$DNLPdXo^YYT(Xs;N-8Ucd7u@KCz|V(2vh>)?$>*DDqc27ZssWpW0&_?=KV zK6B04sWT+zU}Oi4Zzh=uC~QCjPKZG)+#A>IiTKCpqYJHWj5%@@n-8}y52s63T0eZ4AAXIBjtPEDyFVu1cfI@PuP5HZ^0&)fsXPz{4KoCbsuaT zrST%5P)%kc6^cSXkT9((go7xVTfLmUj#wi_mOQ4yKoKM9|9u3T@n|ej* zuHJ7{qNeEBqixbF1QsRXhr1JqLsOJ)cnj z5!sy&u|eH$tf@X!+MZ3$3h8+PEpDYTy2@@&24FJAC1=ljA(-#qd-RbHBNq6ynOGHT zl)vR`UUya(`{xVgn!IlpE<%+paA--PslzsoSBavhk5p*EP#w}t8b=q1f zQX$2)vNbS!e1ET}P4hQr9JJzm!oVUMZh`rkz;iYXs;U(}t0Q_=x-R#3SowBllLz^1 zct>)eck69u+bVAJq!FzO?50Ft@;LxFW-R5D0>?-nBR^%!oPBYu)-frh7tmlx#zbnv*I@9EY1#q+?@JXx8E>4oL5uQD` zO;);M9$1aqCn6;eRaR0dE)H{dsv?;VvmVFP>BQ;B>EXYcN7e;?kufLtM=M~=!e5x< zGWy`p3;OBV@p8igTb4sX1O7(k-@B2x;#FJCrh*_!&p*UCt->&QU-%zgBAXX7a{woR zbQgrL)2F;7KgxD_^)$=fxCgc@UO+8}`)<=?kgH++?gR{|m9A-A_~G5GK&2xHi(3Jr zJ;mQ|Xx0?D_Z6n(Pq5Fzf20Wih329%fI|RZmB4BS8@RN@SIPG2_Mh$=DF4lfM=_H09H%Kw*O%mbBrlLW~VJBZ(Hw0y=f&W zo_x*EX-v+Clsmten83`Q6rHde-U5LKxEOB1TvjF~2nqO_gG7E3pGo!Io;akn4BhZ6 zW<_TIc3p-iV3&6s9k6$M#Y=4_gxgjcpyJP@F$vO|%*d?YT$U zR*2^L{zdW$V#Lk4ZV#T6O~av4$IJO?S;ap-kQsvXn5kKoUyerHUNzR)*R{cl7{T~z zHR-!GM)1$YiAtFU%)k3ClLZj11v)(^v6@XHETe}Ed|e(5`p(S7H>8*rLry=6GU>nM zCX~|CxxyK$JC|_=H6MtqXC$q^oTFdM7xBT58+30|vE^l zeowA+V?mm~+#3mgB@q0zJxH4-%s%ncd}%3T%D_>JzsHih3v=E$jR~A`N7+apn=8b< z&F;8AW}n33&IKq4fgTuJoA#%I>j3cGXF~51`#XB!CcTT+W%2+{c=ez_GfDrSnkTTC zpYvnn@q}$1s-uW!t$5=bb*E>+q=_jfjrdxL6`}|UT*LiU^$)wErG(aCei8hNKisi zDCe#HWkSpF>W$V1_RZ~b^~4SO&r0UNIBu`mScef5e!kUCjZ(|7;)|2rGmB;t>P3JJ z3!G$%Xou396p}WS_J3f`(gluSk=)H~_1Gu0-zjcD3Dn2Nar>q<;5D1L89QSNs1DdG z#RgRi!|Bv-PkD>(<`k8qyhEuA48H5#n=?<@m^X)6fzpZwltPfpM&QG!87#Z%rj2}Z zg*o1UqT!pG!rE$GhtE`p`UfJ-T=3P2&^v#({H7fe*t4gb7GxNTYP?-zEy8wdzKmc? zi`dh~R|ha=X+BZ7zq?7hGDZiccLoE@@mnB8TeGF{Uc~OVpO}x6PlfYhm_UG2e3C;0 z@msUj8}7xeR0qPDvgY1__rZP+uieOjOC%FezSIf01kDDh*hDr?G-?9uboTgS;1(rn zPMB5e6u=j3dV4jSutpd^Nq%q&qJ8243yz0b^i0OMD8g~>V$-2m)cPTYKCh>iwO&Rp z-KsZ?NIv*xbe>334vYp$M8}UiFX5g~YPt6r_lc3=<5l^c&1(}QN364Rz~`!*2`YPR z?hL}O*@<5qw&~)W7;?W$3dA2IhJPC5#pTCA~@SN{oAkz+{RMvt5Us?`?SyL!gHzF1y;7_09e5bq?PI6eFkh{-`A zS4XJ`pkBWluchny#;pmvXNtA^$4kFQ1UlH4z)+I=r@yHlf3mvv5zc94O2V!;jk$8h zH)g3q(Pkd@8n@~rsor9#Tb;s$2tgl>I-^Ezo@s2?7J5h1UMe+34Dg&d!CA&K(5QL6VQ3Y7JjpQO=S{Y9rx!DO24vy!5)oX8yJ_o`DWa>qy*zDq z5F>17@LCYe0Y->&GtY97X$_}{&q9`M!AI$a>El-{%zr-1+6sy{(n4_--*O_Z&U>ST z?C=SlGZGglK0OYH`r-vopZ<7?$_4f0m*N}|4-?)E15Bpu&QrOC5vuDwvzni&#iZMp{#vhN2^`LXZkDSM&XHqV`F8TT4 zO7E=bV5g#M=Oq`X;n+dck*6UyM}E-CXR@p3pV0K@r~IW`m^u7j>P9bH>PBPX_y&SG z?0wJ~(WO*=!b>O^AAv;E{Opas0dt_VQv$ zW(DwA))r4Rkhw6lm%l0p!Z7~qWxrUM6cwuUEpqGFYzc*ZGh=@}e<-C`^y=K*c6Md> zhYEpaKrD9eDItJVO`G1Km?Tr_GyF9fo^~$)ecJJSEiM=eCwa*`Fhd0cY4~k(a)&%> z`&o2_i55NLH{2z%tZMfb?G)gMIkBVz(JjsSOt!K`jk7zlBep<~Xga{9XB>>`A~&L6 zx*SM5Zb1i(&Gkh}{Yc|a0)W+Mow> zFLv9Mhkvc_pxaccCWZVA)^zA*{p6B2o2y-W&F`<1L93dFtMr;8X5bNc5Q-1y*6cQh zK@cjeOtfgwYDDzRdy~3wu#$^cJAU{N;6c}vJ7!Fg_Y!9NLF34Rl$i{^GaLR!6I9C7 z*A=*-#YoR@cI7YL0~yOglcKKOg0NDHH1A}o7Y7VNQG$rTGI;iv?bq$~&Q1Rm$L*vO3Nb@morm9j1SW8AIw3%? zbUALz+0c}-L$AVRpV0~}gmAwz-s-xC-MG!YjtQ-vC=vGaaCuq**6zMeEOn6?6=}296*63U-Z!@KWXimDy6})IR|Pr-&NA zj&D>axxu7XV@%1rCq~q|*+lGg%89VMAB~_hUPT}FeR>m5pM$3cOCf2wfCjZRU+UMr zSk#Wldya`*>vkL!zM84Phh}C|x#f3DM7_J)o@ybepm48v3(M)88zbrXr8ti(DC{1B zM)qe185r5_Q4kE@HM6wy&Ygo<2?LJt@JNF8lGle|>cI#oz7&X~p&-=y-veLSG}9eg zMQ}bTrKIwbjUrb9k0YhbTBk+u$LW=3+8sNZ7d3duPg4fM02jWQcp8OVH3qVQ+I@+L z*rrqe1C#ym+6W)`#`)E9;6K}?s^2?9kT@!KBZ_QUBW8204Z~SEwwhjxu4}O(hMG~= z7^P{^#Hm`H$kb<(XE#RgMADqY%2MG{CG@U|d!LB3N*YQDtMwSI-^}`0Xi8N;>c$vjS(Bsc%i3{(RJ41m@FENh%1#qUYc zHI4CQ_0J{;A&^gRae`XX71ZE=2#}oUBfy}lxPesFt>S5m)@Om&;|^=^A`o*K?JwyQ|9l;46QYB zbn(CK(C1rjZ?fwgVKadQht|$ORZI!H3<_MC#t%DN>aOZQHAycXNB9hAjeWILWnM@@;x`L15;FkKEJP674w#}+ zd&UQa-sT+z9#^c)^53%$m2JTL7^cpAkkM35@1!h3q7(;1ds1kw-YKil+b_uI0@v@x zo~8O}h7RT#ch0}tk1(r@Lm&-QqdT>})=l=`-^-;nJbt@!S|s)OCW0Q5Cw^&Bh|??0 z-}1q7UH!X`S(m>MuWqrF8^QX73A1HqTNt{t6@zjntNOTk{>?7)Ui@0z8mHyjnwkIl z7s=C?B3sgn=F;i~JjD!wDdG+cK1^#LP4?uz9j$&Q=MDSe6oq*cm%{Dspc6( zw)9#J=CJwOo*{nr7RDUvcR^I3@F|{(x}#Q6K1AwO{rXmXEr)){lTOQYy(YcW48%(!6XWmyq7?G`wFEGuRHolvV<^ONGE84Ds7 z0B9&|7uDW9nhj8^r>FNSTs?Z^qBFAS{Yv1ly|UJ+|FsAcd?xBrcvkJ*>|zVYb@6BEmuBFrnaz z--~{2vfRk`AQn?s7bEu?&d`beyw+UONBCq-hXr|3kaNc;UR)PTyJQOqZE4GDP%Oi^j56jyqpz zZ`L*OC59GLiI|ofp;0-v!BJ@DxSoA-b{E6(WpW zy4z|6Q80p4ndY1m-P_clNn7X8C6G>(1U%mPUuK^sh+=X*S^vhL2NDvhRZ!h zNPjgZ@$Hit=@~6q^4GSSMxi#w^8aGW@}!D1&lGWIMm$s}8U|A9L$|pzHmw$f#+4%J z9Wj9;fVO^IeReOs@Y`P3Hoi_fk}vxcfBW_(hR=F|4jFJ060_CfjRkSXlMqvL7OPhG zc?Y&!Lm1BmY|Pk7MtWz(C_}Hfo!}IfiC1JpVWwLEop)JnJ0D%TQ~O70LSi|FO!b(Y zvH4ZE38e*tbxnfw?>Ym;i)Bcju;u;k**_ha(hya>HUJJ;${kWUfAEb^pTWugo6m~4 zh}4fl*|vjV-gM7QrYNncuA6@5Lcq0xIiG8!nW?6bB{l_siEwP%bM+EQsM$$`C`B@{ zIu_^g$@E)<$JurVx6A_6OTWP*0UXWp3iy_li%Iuofehne0!zT zWr?CFG5T67z3ivcOZX7m5wZ7fE~^sV!9T}_SEj@)N}kTUS`ry_REE7*H+(Y-eB1sR ztLrx1lO=kw;reUC>(6Uc??BmKjVS1=s*bm*c8rQ$cu^RPmKeo3)@Rm+7g6GUCf}Rr zqk2jE1#F4by(>TT!M0-@k-^6anu%>jFD|X29*Nk28x`+m9rV98+?coA72@*t5_U*& z#h3m;@c&!@eG;NDi-!vqiIL>5a0Ks_>#Z97#Z~5-G=a7>?yABFN-e0o?dY!Uidw<DMz(5aBL*~bSXL6V+qKi)O*a4JHdvUOvAK}|BVLK!Fb41gf$`u;^7Y3PV@=Cw z76}fJboh8BY^TNHXytH2yK{HD$O(@Zd=E5zneC-|dlp2y@f$U^i!G5lZsBEwL!9Oi zCI5=`j#{G<(^NLJf?ls`=Bb#Xo9mixlSFZ&E6)spY`0`5_z!w(9h?nT=hup~JfNRg zNsGZMN_H9aF3L)Mp5W7X%pGr(e#zaY8+dV_qP+-> zLH^O))B*8+KTi@I{F-uE9__ZKsZ8{#GjL*=X%_C=_b?`Jvj1*1az|qu+U+k#8*{v} zovb%)YFpmYRr7BJ@#O%zs98)ZXABY&Ris$8h1)cH5TP!o$|_DgA0li}?ND&7eIg>b z3}02CsceETnJ_V%TZ4cdplhj_Bk7}uP3eZs$+hSI4g+?P5stJOH{ zxR4t_C2?bjV}&@hVolqz2PSU&q>IyIVNLeUM6T{~$uvumkp9E`1<+Ql72)xEe`yN!r-%$$-WXM zEwQiK<8Ipk@EuVr-lSySqDwGtnrov@%Lg(yC+#L7$ek9fHQKoaUjD=Q9fLU%wSh&a zCDMUMN7C=Wxi%_yBJZGc&2mq5HZ3ezfDt$*Jj^se9oCB686*M|Yq!jFDS|3t=(ddY zV_*s_hOTVuKkcTFA$d#ubTIL^!SXCK5IJ+xhMZ#e+0Ne5nPR(y@U+Wx9p6_2+=F3; zA*T-{K)sSs%7@px-^}^1RMSGrTkFFxwB*McUoS9&1+b<|HQ)&WNB_|JoL`t- zIA^Cdd7i#K=d>hKbm{duSYlqHzbG}7$eA?-x5l@rLI9P+x8WZ_)dx{KO~(yffROBW zJ}=g1YK1L{GAYZmmS@SRgXl_fKrczJinhtl?LhICPAD`44G&Kkl(oGLHFG*2zhe6p zp4@~KcgBKqb}9!MGc%S+KfVJP6BTssm`Y3XjO6Bi zKfV#5I6_+67N7t?g;3SwCaYl(N7%FMp}DTq-rjnJ0=&lW&U(!HGdJ6OHWVFOU(67h zzs4QqF`Mt%u=pV}{1p)(0=%!n!G+dodzU>|AHjsWCciA&DbB2R>*2&cCsQKCnJa=Z zj004%9r9S>W4|L0REx>Ke{RLkeTB%5-0&}={F0hVuEzU~2V=aWMtx3BJl5)}wh~(U zbe|80qe<{}@@AiDA$ji-L}zOhgqOX{=I|}K+=t7p5uk7!P(##6`PyVOGhK)I<d7niYykR^jheJ=|GzR^KI7=vk#04=h3b?LauV;rw1ABL(4g$Z~&jw3I46T?Pd zDehLhGp-<=5VQEa`2!eu_SJvOzOMk=NC7-sITo9wSJ2t;Ar=^t?-6AEw{S{ghM8x} z@isk={m1dAPWk4U2KT68aS$mU&hozT-RK`(sJ{A*ZzOOeka1eSDyFJKE!R?svQ=y% zz74yP<(~b#{-=IP7)f79-~nUS;*I;)!E`;NsAaOT%JnQ`KwPy@`BuEY#vzv0PSEpD zmheZ3M|P9I4)3WC&;!YhZ_vmNG90eIh zS8Vp+F3-a)7MB@ocP9XgJ`}HX;vl+O4B0R-A9{WthH`&0NhTLEH!V^S0qiID@T@Xx zBT5y+V)3lVb_VVs6ib`JwuZsz6({4oj5wGzx%dNr7*Mo;&Vv)`2N%13W%eFK7y#6* zK(0h&Ex=eqsbbByXx#0n(eHR9Rlhf}rE3R0x|It9CwYelxo$lRqS2qWF_KJ=LD9lI zc+Sm~RHi3abBpe5rY8Z_bfOKv5)xowy*c3078ZI4b}cuOHln}7-|aR8e;;pd;ZTW| zX>kv>GdYmkeDp}x*NU3)as*DsrdK+9%`5xD2(yQx|9QmvKy5m+!{02RShIX}+6wZt zG>4takIH-QtaYZAmu119&;IIygV{Eli@qk*;%u@Gi+#+k6Y+I;*y4`1g;yirz0K!ih^l3tNS0_fC8g}yOVI( zi5LziYgAuB{Ei)$!CBJ-TyX`73c;S%du%YZqKnNYJijS=(yGONdrm)o!>pF>sGLXK zA7c+O0NCiUjMfhhmf9xP%@n-?+BN1fpOgr++_>W;%_{1$oYR-Etl$X5yNX7T1~z^M z^_gya3bxqIB%cc&dUii+>wQ#wOxCh>YhgX{l%&V8Yn8p=E(+mWjIN_V_IxRSz!;10 zT8>ELpcgG@`uw9igqH*q@eOx{}tJ_9420mlh@ zZB}~W8GZpR;2&#dw#RbDIi&mLUPmmKQqCFB|75M$`;NtV1NT|j3E=?xc-PK1JADO= zaJ99uw?-alOf=Utuj5a>y6%b!J2@T%NqSk(74Duo2{jPc<(T4oQzKX+51UIVLy@{0 z%g@fv)$9pwLN4&8N0@1DmvW+ktc4Mb7^x<|`UbQWxhF6gENPF*XU50FM!Pr zC2d3FXS35NBVOMMY0+vg-d84kn&46ckdb6D+Y9I{$|WM0$dqd8ytJ+DG4N-L!(SA} zaW;7yd1~^hAQNph?GD|fAL*}WyFU;=2Xd8TWe5VD3E-+2Z*~EoW?d=)ktDdW88@-3 zOsR&7-{smV*1qyIX4ZsbBrGaGO=)RpvIW>9L%EB&L)etGgr; zb&XCjKLHEG-gN9-82V!J@$!+rH6Lq$A*#I~cHx8#2X;iqtl5=G@?f#BL(}3BcIv4c zHiV6uTgev&ubKFI15B7INOkd0H=9}M>o5V4GjKV7=Yb4cG!jIir(MhdJ{we%+@gtE zrTeCfFkQ9>uL;t`+qz<%4XfZk>z?o4?R{6>wDK59oXU=+%$D?J3etCeId9W>s$Imh zJc$03U(gI@=8Uiqn0RWFI@@av5&)eQdU3XFf6xH$%h7I(O3aT-Evz^+^=|Q>Lx^_rFrGzb`Z+c1pS6~(*CbpS(8Y? z%QSJF?Wp;=f2cX)53vHo4>uPmuCCeNIk7Ds4xz?tvwj@lUy4t_O(qI-j;72GP{cqN zSCYdT*%aObqfpJPX==47{eN$kbUDNd)JB@AIZEV|S zW20%z#s!>dB6XlCq2r2?LD*C%&fJhEigRR@Z(};NHeN<{(1&C zgVI3XGyOSP*E}04U>p7h4^ej7{ou&}^4!$`&@Y96&jQDZ?IAcv zI5@Dt6+?lLo~}2dPm3HY{EMUmHZZmAzQMn7KdkN$bGwqLb9ggd$f?i-5aTseOpTjJ z#g+bT#tom<4|;$X3z3Gug#Y3$q z)-R-hBS71t(@eVpFFJF%^1x`m7qiD@4i+w@ihwzQmrwTU4Osh zx6o^ejm**hpM_>+pyhi~Q`<*|OqmM>tQ8AVQ%l&B+ ze%Y&=Drx4qrDJkALbg?7)TkcJTrNOoK5wOXXA=LpII(%E0dulCHRMtSKL=|lyb)ctO(tRMrV&xUqC?G7&P(v^4E z%|8G$knkDl8QGZ=n|6toik0v3$~w|mlvf0ig&G3`5{b91MFr8^DbkTz#H|V8dC^kR zjC*8Ova#t*kgb?Vg0Dk@1Jh@(T!A*g*0Ox!%w2cMk#ym^&tFM8zi?3NOJ28^0jr{K zNsj7xH`0%=omQYWu#h|eE}?=vf0*jy_6TOHe|BqFm-~`OK>PLJBV6dUA=r-E8;io> zCli1K0VIxAi~3K31n-Djp7%y=mngjzLzMeEDx)C6Y%2` z2&B`MYT6ab%^Nq2-N^(u_sCCty_4apha!S(l;aMvtTcF$M=XT*3k@9mM$nihTKHY{ zPK#p03+Fpdk7o)xzaMi=N=fIb;JAvf%zG4B2&L|@vvfRRdbIU&do zEWqT4)9_c7n`q*`#Ib!otDu*`(A*6zbJ5g`;3@*gAr{pWTSwYkM<}w?b+2?v@iPM# zs_ovj!S_Yfw$;QHp<<*c`El0NPDaUWl)8isY?79lib~PX&m^4#s5t6!tAJ_-3mZm<7qEKIHLL2MDFLRhb zwswizCT|P&{{lIEX#WYjS;m1~&0u5k*N9!t1VGf+QU@ECR0dl#)QV?`ElydL1~zM= zmzS*|0SOff+_?{rliKysT&m;fY_#{0(pE3s=I(1FBqPmt8iBxXLnm#`zT(malF4^< z#$cr-M7Liq7G>0kl9|@2iiF6U)^BncDj`Y`9gi#76~cmf+fq6-D5I#e4VQG(>#`N0gtDkt6mj-FlR&kL$73jq&s z{?VeX$=`g)+x&x2VH>}FdpMc9)8$DQr*}y`Gfyx5pis`S*sQU?3?MaI@TCV90=);; z!XfRC2U3x!WA%?H9QHrRCNYMiSR=nS!CQa;>ix|5hS;&vy~f5aDIW}E-Et${AgRS) z)hjf5SEguI-0l}X2Lc;5kC|uu-w021>^Ghtymc9V3Z}{(8&Z6=H~;QTDjY6JM>W`I zLZ#2QrfhBbY(n=hR*AzrjOUB3E%^ zVsATIEp`2exz=pAy!AEpyx6~V-+WuggW-e+j)$Zho%!c4$}`PIaZ&JItt;4n1n+n` zJpZVkCa5cIGy`LnzQD0bCw!8v!S+7}$x5h}UjHXS1BhBCw)LN=OOOFQmM}02tH{V``_QScnq*lv8Raw09lRO;7}|VVnMZzhswGFxmn=+4(<# z#V1eOh%lW9?!Evn9BQkLh{$Frx{{!mI@^elo6_zmgcZMb8F@*lU?Yh7?p32i4*qp( zJLY*{v=A(b?{hqx&E>W8C2Q>c3qY@$Tkzj!Ga;QE!^8y#d;6{4EI`ckxnK{5@kQ}S zFcSMF)Y0c^n=BCRJm(Z$l}sk&J3GW@Ctzf1h2`zdDL>0(OiiC5>1K3vB)be{J?{!7 z-8RA>Tpv>aP8dsDr=HY!Wg}P~C%lMVL7Zt2ObM(5$%|&h_Rcq#U4!1nmM#xYsF2}k`{z~5!PEQp#z=?{VU+Y*!>Aizsyil7Hp=E>P zl);VW6cMtr?MVco*;zCGk|S||RzJN=l$i?o${)kA^-cjS?{WGG1vNAVWToodXTE6y zud1a`v){yqMHT44JS+&}Sg0OZfS)r8q7Hi^mdMYH#MQBF&XrtUgfk5jFy_10Zg{Ud&o z(R#4M{t4zgg-_xR>DDj&yk=H zvLEMtK!X`;%s8@Pr$wMKwUu|a!#p$q52fq`?`~Z(M~sHmWgHc2(4IHV2IR74(vRI# zuBKl$iLzXX8s;}!U03BP{DS4QI7;3PkbC?bk~v{lSWVSAZ0}zonp=FUUs`T2S>co4 zG-CaNAGMk_oB?_V8BkM%#0o|q#rVPa8@99|48SiIjhH;S4$3{m z>Z|MjJ&x@^ide&`=CZpi=d$jj);rO_lZC@yc>!!{^^hhko?ctHVbhI{4V)OHW`BCP zEOwph#t*21U{}xAdtRz&pg9!RrWupNe9jJw5qjb4j5||^LGR?2&U#G&ra(`|muyOL zhjOCkoxnc7HTF9KGEW86exI0I%jCF^fnl{55dttGgjj~#z8n=@ZE#Z66X}hY12A-V z+b2fcIy8<&mtvV!O`Q*6>8q~bNHt|idQF%6%16^4;&RL@xf3p-GpnDNDa#;i|8zsFW3HK-RbeHyn1E&)r zp;v3v?QVOkEy`z3NT=lL>tGLWwcij2+=-tSW&Tt=vAi$Cdia*sH&6#ZpTwQ%_WxUs z4!_V;{2N`Plb>_sM2iV1*TFcx)mb+40IDV}zpGI9liBwe;mQi>SP&*kj{XxyNGg`& zj43goNY~r${26A@-ks3{Du`Jg5SG|H0KaxkH%QyRWV)@2>_!Si~0u%<4sbe;} z7^#rx>X8&+Q7qXshqi#*&Op9jK%+7U(iwTZAnd}toev+6aCa>{3Goh~04%do4Byap z$^IG+aq5P%qCwxw>CnbE^gbnPf`uf;%XNi?BPt(Tf)_HA6tuy<7FzTE_x;nrQFq|QVQd&x%xwdJenv=t-f#H;&fum9fK)wnde)?mbpzSb8+$ z;|;Z!DMP)`qj?0}FyQ@-uuXub$@n6N!MNZ~Q{dOY`^fZGXgLG!@7%}jPu7VBw8$Fx%+l6F3>^IQVwo3M6ruiYBG|MmQzj!L22TLv~^TnyAKUA zBuT}GMN9CwiNr=;&ooIWfiGpItBtlVy20b?a#HedsFi&-S84D0do?ws4deq-fN=2u zRDjhS2)nKGr0$2AzAL~Nqt^80UD&)c1uB+3mge{&p!&4u|4t3UK~4Gw=t*d>iqyh( z(!Qv$JJ}BHR0FpTE1@5eWJ%M^0=*2da!vq=ML@2kWe#34o_hd{FQKjQ7&y}j?2UsV!vWcasYz=U9vTXZ%9Vuozs~QKgGSV zZk(*cBg(Bnie#)uLf*s10V+F^;i`fnU@(Lqe_u{*p(ZWb zWSY}K<{^yPQ%Tp!lS?PN4ww2&tEC~;OA}xkKc1T>K$5(5c*d@Cwoh&db6|UdG|5eW zCc$-56Z+rZZANt@#>A4RC#n-Wf0=*lja5|UiI$iyoL?Pt(BLp-Tt$4@tAxTz?OWwOX; z5wn%g^o_@tICf8K>RJW+`!d=j**7h!zqzx&WkqBXzG^pS*0yT+1#=bmZ%Ljn1c;Ox zKa)`;_nIm7KfgaIe$aXhv@BK36L)B7+1LXgGfPQe0HLPZx<^IDTi|7JZsi z_Uo!#qxLTSfCffE@?4y>)jfB}Oh~4U6H#aGnf{;xyZQBG=^KF4WoiSj?h5yjl6N^| zeqsk?v^}}WL6I@EA>t*Jr_cq%bTUoy=p5M5aQwtb-y!B9#C=Qx24Y{GDf#dG z?-=F?QDO~A^_%_@qRrH64KpTQGtmGYr{v?1Nfp2)rO>!Q>gG{GJZKHB$Enw>&K!|o zRY5NpzXhA4fr6EM1e=9b8-R3;Ke3Xu^ceYVaO#>zK)qw(KJr{5;>z=bhte$Hyiw^@ zlmAM%d@TrY__}uV;r6Zvb!Q(c`(Tr<`-tbTp8ET|IkKoKn~HO?2L+3($fW4+6n^W^ zk`c{z1X`Jd_ilZudM}5L*MB+qS09xQKj&7)8(4{y%DzNqY#87~lLfSiY@BSPdN)U=L=xy=m?FT8T3*#A(=C5VQz5!q^WH_WHf#)};%AgTBg&K^NDy=Ft#1 zf2#`?=p3rw|1s{-17lHfUNf8>G2aGQw-|fK)F7P+L9Z{TfD1IYCwu{*Q4>wg%1B>m z>+^GlI=u|9iXxcj1^NBgiu9PznA2o07cQ{~-uk+3hf z63VJq0_>Ly9m?phm3Pl;4CHL<{mL1($UF5bF@g?e1oT+7tltm}o{nph{Lr)R8+RR1 zc?ZOE9C%O&S6=1gXgi+%?$2t;6l1S(`qdx)7bQTKan?r1>z|;}1$hrL$rF*={tKYR z(GO5m%=U;U%%tC$iG})n-YEUcZ&8kd+NRg1QozSR+VzudVsQ~VDIF8veTQK$kVXLu ztnj4`E{#tFh|vVVrXe1IvHvif^gM`blS>KU6YwSctD;+iR32Oa`BMICmf^Yl+wMVm z#Ct>a^6Xo4$tyyK@=T-b<3o^;rPzH`Hc_c_{YaMJ=_B?1?}L%oEB+zYim;ybH7f0w z5QoP*4Pp^}3T66_C+JFlPy-V^BA7+kUuvL|HpZTCXZ^?Fi(Huw$Ul6HAw}So60u64 z&^(4ego6px2N^?^vrfdDBCUUeDIRVC9Gd#swIE=o@mUfw1M1H`cC3+^H6Hc#!ZK3e z1P*M?qg4sS()u;FeiF4R?d+;V>>NWexS+YcKdf3O!~@yPNJqdv(M+M7oarAGH=68E z*)u}k1){uDW9v(Kby!j!b8sb5-Gg=*z`g-VT<&fqb){n$J%v&Mrn2fyi2l#Qbfo)P zu<41A>n8EJ{W89Iz*8zSx^q6(4Ww{6E)QNvKt_5zfCZq!60lYsvM~F0V57slyPk;z zXB4%-pO$CBmbG5w8Ymt{bIBv~t05CzN3VDK>?rxOo&NuB==%EPPZ%d!26IE?eX76< zSGZ&mPR>tI0R=Mi{jQu*gj7xvQMYeRiC`uKo4@{n%4iGcONwZJ-~--6f93*5^VDuY z=YPtZk&v?w4t~*&oETFI2BNW<9rmMsj9)rC|9V!w{}EIrA7Ki~Yy=O(X-fHNTG9s? zkJYJR6gT{j@4`~QDk1n~fXi|rFh7XlW~qGe?apK&1NlIOzSVWkC$69p_59xmwnTG0 zT&`7zxJ)o2ulaRJ_&O<--VKrmUu^DC6FP>p_rMFnu3J!Ef4@eA#Ij7Vf)dVkfk+ER z!106O_A`y$P3J?U z>z&Y)@Sj-kgS=^=0~<{MwjARob84@~hXw3Mhn2^Hz06s ztd#I?5%uPVG71yh-zF4H4C=8OjL(X>Y0+i@2_i7aj?*2KTGb&cR6LR-(6Qq?=WVX( zKy~I1FwHl73PzB#Ug?wHKSB~HR{%+P<7vx>`d(vvC^#|WX->U0O6T$7K_vLe=cM!K z`(KaBMM=CNfBTf~FM+SlpMqIuv*CNDuWSfV{0OTsSbx?O-u=Yzh9|}qLqY-Ll1JfA zdUE=3TH3Z|-L(JQ?E4p+%Dyb4XWnaAP_%)uAjmd(B^1)mvo*ri$t22PKZ#@QHHY2( zCbfP$?mTftItVe8HW@MVlI#;{n9E-%mShm292^W8IWX8wqK$Lu=T;V5jpNRrqwIO9@Ju zb#=S)`+;%GC)6@@cpM;QMvgSciPO4b_e!Sk@H}?d8C^Ox{Z1s^4o5-J!zc<@x>PM6 z|D2)NOuOe@0SDkCnNhEw`ga3bx~Q@IK8H)K-@wfp(Hs54H>z(7rN|Tke&mJeaPurr>}J6?xUg;M3L1r<2u7aM z^fGLE9#;UtV`m$g`s@Mb{*!T@q8y<@B@5g8^bwYM)a@7Ds0^sXV<)qy+*d#n^<81_ z58y6K=mkBd4p}cv!QbvPcYcGbR#TT>VSS~;q`)nkG5s6zh@Cbfs)kw@lTVjU z=SL_6O?}Ny19rIA&*1v*$j{+?fSMFN&aon1S@4}m;7pOJT|HT7B@$tEXn0Hb@7bf4 zp1u@~q!<>$e-b-BS5{k>%eCUu<$J9wQ65SBHueh@x=Ry)$Q@%RKv_=UZ**@ULkG5D zKA+8Uv5TJnrRi zyKlJ%-p<_p9Z9m%CnW~1;wR9+KrC&1zmvsDwDq`PNQ?508X|~y^WUbg^U=nkwo#K-M;!uo z|2RJ|SNpQ^Gp0#zni%dQyH*3%Ov0l?%W^em~d{(cm zU0@IYrI-{4*KS1Y3dTHS5X+RqHt+j!vD222&~!x>eFGIlD5j>WckEfG7Ch#c%jJEc zQ|O3I>mN8fN!B?#`YbO5boK zjiM#E1t4XjVTNJ3wzfdLo}#x%J0sec$kmM4LP&KG&Xv9nkHBuY2xhDg@~0HgoB;F! zZC|#StL=-Q-{R(@*Fo zRJ)E_X7cGY4O)f#-OWoO^9WoCJ)Wg9$ z@}A06V7r7&{WH&Y@xTjYTq$}M{?imL1&kg_CFupCzoVx(k( z=odCtKHA;O1-^Id=x~?K8uTMX0cQmv1i0*v ztjSoILv-@^h&J4<30`8}uPX(pLM@QnWsYif6au)EH5e;nHv)hO?p3tk>-*l>QO-d5 z%f-qo!Hb?%XrxsF94hnHB^<;nBU2xQkUEtM%a;c)>Le|vuwhJ{)w#5R_$`Mg{rQ;! z)u&y|VuMk^K9@7svzJ5y!2u4Wj%i7fZN5=8j?1OP#Ua?U>w)hR5$71x*bqSs{tK;c2w-er|g~$(w+jPunf+BCJQ!Sm#d4G zVg|jQPc#5NNX~n-hXcDr%!z0o4cdn0kDWJo0jS*~ag)>@5tX6RzjbykHo@1l(cGn2 zhwHg#^orzO-Cug@fg^w?^uH(yqN14b-lavX7C>Jc1E_AtcVJ_#KX!SEckR@_XVw7=aPCzu!!Z-3*y3$Dlfw%dd7qDn`q4+2+#2b?qy&DaN%5XatJ+V( zZps~OUu_|)tdkLl6KPfe9$_*g$wRKmZ22i=^1jbDM~27hoy(+AX_xBrSfyKSK_lr@J`{E#bcdKFN zc`tPljZ@mv9`z9~iETfUbPpZ4^vj&vH0(Nb3rBe_ZFLN6rcY41hMb%MpCe5&kMFH} ztU|>l^)kugItjxEIAJ9s8@N8-kv!Gm{;eTrG|;btu)@V3R%XK*`<(_cT2B1i>AL@q znPh*!JpKcPwRDTkkXAvaR@YE04EE*pUEl}o1G*i^dG#c?Mj;yYZs4FOaeGt-rA1y+ z?vKuoWfd?fwrL5n3C%N*HYy}B>;G_C>KYR4RkvV!7(GzP%J4*~ITDBhh(#Hmd@@kg zDUO(qdc2W9rs_snItV)-{Op=R$G4@(Oc}dMRQjL0Xt(1n;)<=<;}o3c_^Sj zn6>&_$OP7KEUI3hdn*4}wB!gGpnp^(cd2dqY3G|(>lVWdH|HAKk7-dJ0nKLvy>+1m z>io7^#h~>Hp;fu{s0XFlMmP%Ntoohn+Q|h zQ&Z2I#Q5$vCX{wf!I*UKuf;VQ158{RBRlO58?1xbTMEu(S~bcayL#p=RUFRc_CFvWMy5shRw>rMIe z@Q-yw%@S~)gdtWf76IqaMW}uQSsT{g)X`Zft1ENN<#*touVecaA_Efz{XwtyJ?6`E z;5a^T*V2Z&;xOA%L`59L_m#Y9d0d;%+@t*V0lQZa3c2WIm{F$P)wbTTD;Dj?yB z0es0^*g`V9pQ0=FT+xw`n(9l|wrNQeCNT&CB~$=`Vv*M(LRRV8&IM%p83ENDPQ68R zo^_CroTP%bD1b;musJ;d5_Z_zGZsTUoZ9P6=hSxwxASp)OM_TO6abglz_=6y%He)e zi;OM0xh%(8rzjTM$L6T+p7PA$9rMvg-~qgb<%3-JjHjj-=Jz3zXAS^wR!3ZjkLu&Y z&+Ktl5)1?oWCnqkN>kblrgrxbjHFA!^OZyRJ{iR!1mV)!nceD}y+SQ!abg^nY6)w( zhnJA+sGIZ#{%5M?2!~ALOJ}HU4H(cabRRIc_J8^U_n87V!^Zp^dr)OU;}ZttpaETe%3jOsaviRO-sE460BMwvo*Gh0MEBBnHVgTwKE z!m~Bgb<8+ZD2v2FI&=F$3c+npeVm68(4g7hIaqhJ{v!q3f^`+T*zRvL_`=w*3S6bgiHo3@eQ z7bmL+Vu%Y`ZBDq>jPd3&y%8fVhMFRYAA}Z7WAw8RPSSYJ);@^hR@fYPD!aCJE;Zo5 z_A@&~MsHhw7n328GKC%fl-E^A|1eo6UWLw#XjxCUkIu@$$|*vld`Kp1Z;L8)N2BUS z^_(Qo2Lt)5_ITq0eU4V&^Bb=FQoktv`&axIns;7Cf%bD#{52`Rs;T_!QvJ&V*xZyP z*D3T1@yUB`CXpF+NXNVQ1;7y59vGA}HgnqO*+Z>hzcYe%CiQ#%L%(YeOG(ExbSL1}e-EjH2XI$c?-2Wi^&Ke(A|p*B zjujdJh$gsVHx=LrP#Sq0UBEdpz?B*hY5}eDi5tp&7kke$pa&iB+qP+oE(+S|S}Jzr zY+yTiwD0!6_>JHW_~|yMU|mHfI*j}w?1|<4XTJ}G14wP7%7@|s?Io>9Uz(>{VO@BC zL^i?H>b{n(sxoCjX4Y#Uny6SyhUBe5sZgRAb2Xy8*@jc8qwXsfrJmru2cO%m?A8`s z^&pa}_>hmmJc~zFs76e@9@4uG< zPUOUowI-Y9n7$Ng+p*J8N(Vcv`J2bz4jt#RQoLMEJSxuV0`fBk6PrI)zPWw){LSq# zu#N5j_v?dg3!njoBSF>Ig1<%|-cRp)enUAcxiBE_j(5A>7JF#(+bs2V7~9%H;-x3M@T;PI&+Fkgz7%Oi+ zKb8zP)&$p8M&R5v@)yUt;xT)kV9VQ#Ib`dRT<^*0{=Y%G`JnVpSX~>q65UsA$DkUn zbIiDCSOin^{i;>JKQpP=9W=IsT{h=eKf_#iH|&J|+F!ilYu zCb%1bc(!Oa``+3@YaLIKDzx)zI*Dp4%KX1aBmMY=Zj(`^LW3K5D1RF|#E7@<6QSAs zon#DH3b#b|LTFR;XVGk2O1uJL6*FewHk(ydD8Qtd8ZbaZE5?t6`_Kao$ zoEkUOmJ817%u>1({ce?mVlPYM_PuC$M;MNP+W3BK0x)d}kC44_074`;aZeCKwZRkK zKc-jA4}zP_A>q;oR;W2d4r!^7i@Si0fDIOTG-B(GC-3XwEd%BeOc#(Vk56=4xNVI0 zi;m)0*Fy%p9up{-5aU1EbqQN>_W?6!AWO^`(_o(~-xgxD{ zjdvZU7Pm!7dUojT{2|C|u<0b5!<{~Q+x);FS@+S9r0N94lx*t0>G^5`fc{wh@h88t^>~dx92qa7os1;Q99(NR!zI$2)yp*f;579T!bmsjFI zkihAEO@nenONRu1L}T0Knx8ajg z0tW?M>)2Nsd#u_cHD*PIv(9S;S?|xr?nVR_hE7;j&p73e%(PKZ2m8bGP|LmrzPES8 zUZ4$G+_HVX!(f(lR(W2zP9EI6+YOYda{vAH|6Y7#g>EDpz{^nIh5X<9(mT*r;sT8H zYF)o)$wRY`?Jt{~(?NxZ0P8|xcD5yBzGqpJO8d3WuV0Re_`ZqxRrhvb6hR@*K4c4| zf(zlrPC9g6JPNnq0WQmc+~jy5r3JSzJSmT~NqLwsm-)rTG<}}z$@z1qe#G16cz=MT zww5y40}Z-+eV{$g9o4Vvyv9#3pY|-Q=16z`Sg>QeS9ks~7)tv4joU9Hu*)WXgYgY} z-fIn3%6W96A&~ZKP9f`$;~Q#e{tFPr!Uhr5%m%Qy26(FKK#B#tmL-zbKD+qAPBP-N*?4 z+;JDFiTr2dQ~kvxWh)wKN=GhtmN;n>z87`0N8Hr?6B2XD?My%gcs<~B*4h@PI}81e zRGSX}Mq$Bxa5YlLpUWvBJLvqXxm&_S$ArQJ6@IHdTjlXV!iA+srEBKRqsh0kS0BM-}B zB^E^sF=e2(-au+C#!w?Vw6@^&KNxyy+FB8RLH~Reu@`E_|NmToup(z3!WN9;*OTFo zni)#`s$dRS)S@#*)Z4b0Q!~dw#!W6g)Uym%FB>pJJks*cIW^`Hl#aUr3HtP=k*7ye z6a1drb&r(rmuKcB=ln4c3`P$@C>$r^hZq8db@bt4^|*m{y$RT@wcXlht#hg?7&(>t zG`)rFSN8Z@V@ZfdAUM>6}p=NP%VR zet0VJIT07SXez{x>5>U%GCSXRg8j@3Ij&txY^ZqO+?k)};is9X$IO6Y~OTImmU=crz8@W^gtfmwEWoK;Kq!HmuTuSbvr`+bvSYp5Yc#xTj0reGn2Mx|KQ%0kh zUU!z>do=XhW(}|$xc+AI7dH0BJ>%j{Kd=O|zId#T5w&+`1bq7-Cvs?PZ*B>b@7oo^ zL!j(Ed=pP=?xt@S;We-neXNIZ@>JigJ`If+*5=+I^UVbb)hii(AS_8w$2v{9Rfv;N zD%T5rA#E7>yew_V8&hIlBfi+Y?)UfZ@94m4(9cEgJDCuFETt#PP;1v08C+RZZ5otC zBG~MQApN0&pT0eQtM@-v7f#4e>Vhsy;E$M5Xm3Po^?MF!w|icQIGx2oib~ zk;j61Jd`j^P4^>aDo>3RG6%}8m!yZBUa*?G>%+NenHF1XQT}**1^8cUUDMU!g*lZW zQQ-q>^q`Z~Hf1_$k=F15&^((3Nn6c{{@TIzchVoUB;h<@BQd*wt``aglJl>B&%>xX zleQ#_3(!K?(6e8P?}pG6Gk9B3onIx?t6gvJ3D>bhP`jw;y~qO1sUZZ~e_JTKwba zb`n*dlHhdq*1l6|^IGFLAC`|LRcfZs`sB27u|IlhQGK1u63h)VX7>BJfR?|67^Idw zYeTC6Zy-VXrAA&kSGFD_iWnQuOCqW!n|lR!-MsG1&MWNRmB!b%`$j=3cH+4XcZs>$ z>-I3K!%nX2cU2t*VTDhyya-21fvms^TWTGVBGsp6zBp^gUGQ_8(9KyL%RM4<)p$vv zU(sAerD;hswe<@X5iU2Bl=+dc^z^M>$0n;QmPUU$-=Tinzj@e2BBaa&B*TN5-t%;n zqj=uao60e$QqEpj_|1;P{l#12O~SM5bhl81npdaf@XzT{bvs(o$!qho=?bKeZPpn}%D>Jrfr5KZ;gNQ6?wZxBWMq z_E}@#F{jud&ZF|e2iAvBd_>l;(tX*)csf}Qn!}932Q_2r%+3WR)eS1v~kiq9k172k_0Oc8AmN057j_;5V}qu^ENpIK9A==M?o*(IcCl(g237%H>c7n3puEF1`d-$rf7I znG(zRgV~DItKZabV_WQp%p``MZ%_?OU{b#bD>XuWryUj-?>a}2Z0_|1O1njr46Zb5 z7ZFswX7Io1Bg$TtjU=od+cxu{fK1foKw3&TxZ(t97X&Xf=MzfQmJGmR3B8i%>CLh2 zS1+GfJY@eMQ})yDEtOSzrc?aHq%bY?5q%5gpM}#K&0(upb-p^N)%##K&vqR0hhocZ zj+52IH=OTC{<(N}R=47otVPE$o+}qrlkdaX&xj)G8dkwNVBQFOO2t*W42V`X^Ra)i zklKDo45QOwj9RvOe|TCvs@>cjxooCC2?jQMqctQmzi)4;hC_6)!-s#w&^R zb7P6MI!!y@7bl|$O|Cc}k`90PBl^xtQ~`(_ZzZD?gPK%5U^ziW>j{wbkHWLiW!)8H zcT=+l@W3wZV@0@>rrKY_a@JQxGf*_my?nk`xG#Ohju%0lWuwB)Jgv&9c%1pdtyQhWkqal3TIEx<+zrLPT z5e-ykMwvZ`j`ni-5ygV_`pum;YmL2IYiYrrxGbZsS00b?&EOgewCjRyrI+rE$ML8c z4ui5LGG&Q5Vk=?^MUF4D_Y?kh6#{?!rVK@Jh;2^-M>kD7kF&Q6lp(LP{bH%IRR*EB z2wUjf8)x<67L!i#`XZh3WED<#5v3uRVN$bya(7baMSI@iO_L*Lv%g(8-N+~I8l*cP z!^tAER-6Hr{b1==Txu8a-hh578q>7zyWp4C6TvYShq!+?*D7;VMScdlJA|&0 zW;Tv)hti{MAStJF73B!-$!PxOpb7t(!q6#+()Xerz2k><=E?=nqX{+airUB4y!1+S z^Q~SBN%Rl*5%+t8asETWZSH6&{+it6ZH&%=QzIo25s{Zw(Y7sOFjEa= zxrbLwJj<`H4)Oc4KEKtu-d>+|;znlBf1*Az)8?n7J6%i%H!-M6KtIl|y$8j}CN9CQqbkql| zyifgoh{M3PR@0h&*ccK0Me0xS4GkbB{QFvwMC8l%yRvfU{i)Y#!|x9{$kxhj^k|gL zqggFhh|+AziT9G9S}N}yX%n}dc!SWQwq8_m{|LZE)=52+J+bV+ z3bzzYonhX>m7~M_wy152e{bbs0otvT;4Wwz%09>q>7$Tc5{cjTN-cLopmH5W%J=d3lKjtjEHxCZwQathEFCBFxYTyxqab;G{q?-^6&RuXH1Nb)YYis*w#Fc`jm4; ztd#OuOikG^{-AWZ9Gj#?s!>0%6^QN#tr2(QqnnV74U*8Jf@}GeP%MS4yDCgxs~=<2QI_ZLb+p!-QSY?!(`40oez@qkD__F zq@+bLQL?OHc_R709I*>i9^fs^uk+LI1yA^1#zJY<;J0gM zTfNeA_mu=HxH3F~S%B^0@lUJKH#e5^Q+aDwmTaredG|6!SpyYW}~+V%B|MpWx|SGogR7!!LYgI%}XKGPieXC^3FUDkQ)b&9v+rL$S!@ zQJH={8<-S5q0d~az)~S4b?V5-uwY1$o7RX^bnQrdy?2T?KC$gOMUmccei{g|EN=(d zIv-O$m!VFkgI4s%pP;$`&RtKV*X;I=+=lGU?UOFnaSJgCTzkm97RSeL)B)EJK=bAY z1m`=Np>B2Pwum4ga_xB;O1-*@q)+p+LBRhswcEsg=7M-GqB%jV?N&I`~7OXtNE9u>kFy~yo#C1P+j~YJ{^V>lP4*- z7@osn8M=t;=~MW3ve(^?lgkh>DdjdhXrk4?v1F`T;$Kk!Bp4`mOLTx8{paL#xW*>PM>9X6EH$mSAo4Q@pLKcc=WEUqTlb|8=+3+_I+6P(}-?(Xgc z5+t}whTt%`1b27$1b3GJ0|a+>y_0jkd+)=3-@A8r^{Ta2Rc-0gSTxBwA@PkdXqrsR z^Vhl;YOEsPWFdPQAMszNVetH};uOFe>ThllZE~;NySrXj?vMcuz?*D;`J$%&%oSJ_ zEcq(Z=4s_+e?PzuI_w)5n9$uXmR`6t1-=8X+L~`=Nr&Go*A=+8;hHLY4u07b9|^gL z`%Zl(;6AR@h-^5$N5h+^*wW^U9$QtlF{NYjE5A7V*F2ZX*5PFf1^}fVOd{ zlF>Wg{lxFUl7+$xBOfpK1m8qD9;Ha153@I(R496W+K7anvAr_56TE4!7k?`I4Z>|S1Ow1#$eC{PmNO*;aK#|f*3u~yQWWq15u>S5W zU6)13To55+b+Tc1*Rxkg%;eTN4A6J!ydh)@2!FrB4?sH;xwRmh2l0I>E(ee_ssmF; zQxXCw&hW9jo^ke^EG{(n?3o*FM0qu=E%ASjzG1HSi~c?WpjW=~k_$c|bUE*`tQo%| z0-)Nx5VG!$+kUfXQ4YZZhe$AsXK4S$Jl@8MPF^JPRs^ACOt59tIla9W7SW_T$Gq1Y zD2Y-`T^H+}q;6rqTZx^G~Bn!5ZGlpQK6*emiM1(M-q^mDgjc~lN^c$q9^r= zQxxM|8Uol@Ew@dY6pH89%sE)mItzFx4hMd5DX3T*UxKRq!81MM19ldbv2y3RAA+)6 z2s9d#HwG}5>*ha<9i2d7=cwlnUuB~@`OSsY@P+kwkj(e;+P~uxBLJ16>+0<<$!Is& zCl!(O;kX0^wKa_R@8!-!3%eWv-}@<<7uXLz|MzShR{wR(@PmJQ{gWauDm_0fh1eN+ zDlNB*7gT9aB)h&R>VNCJ>Upfm8i}kv_4Bub=4gTYWp~Vf>Z11fM7um#fsSksZ0PU+ z_m2s7JW8)8>n-0PbN!-bgo@igsM7d(7b*IJLKmE$Dnrz=&&$Dm!7VkO3Xqfwn&8>MQ0aYpR+3U;X<`WE3 zv-11xdTQILd{OxX=;(Y#wXNnoe`oVX{k%T;DiU})kz;K6T}*p}{GnmGC)egx1NK3ePcH-rwcYG{t ztcgaFdS=jy`|ZSFQm>C$f};9xw+^g32Q%hH<>X*)RYM5A?!QhjypOLiiZbAzBFeat ze0MH;!e%nCI{AjXz}5)J9|w+p{iJYMcGc{srr=sgpAh3l7MT)hIQUe1phX9c`qZRm z|3cHk(bx247~S0V>^8iSxuyBJ{+`*F&FlEEddtElfb<_Va$!dJ;A78-W$aJ1z9%&1 zU7b#Qvft+G_mFb&-6jD0%;!esd~GxK!!}{U$iM9xw`CEhIwQvH5>pGqXGQC95aSJp z6V~7$hd3r)Yjp}7RT0#zL2D^j9iy}j68gfAlqj05qMvs{pA|K^9{rqTR^PV;MDDH` zx)ErUZo)&@4Eykjk%9iZ9tL) zt5e~aKo*ldyjEY2QUoe)OY-PqN&wN@*(D0Ok>7iY*ZJ(;#kT{;=v_%tmwsjLU1J3a z7Oo{_y!fDdbr|{q??(mrDoUddo1Dzq{7;;3HeRcK4KF6DPnatnuc9 zXh8l*W1q(v=_Bp#K;V)GREzUTvO8&FN%*-$eriUdnk0wU$zg7W{Vq)tBy1XJ=`jw~ zLOp>lL|RhnVe)ZA!!C-g-g1+k!HHM?uIFQj_fo&%G%JlHE*49QAEhDC!t1XkG`p+) zKEFq5W2(G*L(u0vM2Uyhsc`D?jHYiSe=u{aocVq^?U5Ph(TapiI6DcSIP*wwz)+Ps zdxsu=vX^y9yK-pujGr;&b7U1$e%!(QthB&40@+u0{YnUFvEX~%5NS%aY%nf?1nRk- zOSixzGIFiysKk4YdCB!H^Lcb{XNvr9`!-G^0x@n}H+2MfX}oL-wt3Lj+%XG_0=8jr zl;9~srR~q}T3-^wQ`mS%;MG+Qne3?u#gY{JjSWO%w~C9M?&n8K`@l15tRE`hwn@|d z7_eAQSC}i%g3E2P24p7vv0>XXbnyq~;HMGty^8Jsa@zUZVC3XfJrGb(xSjlG9kvZg zyIbHJRvPb=7^wAf7E9`;z1CUS11nPH6cBs)jX^^#Vj9xw)*Bc^$9Ps`ANoCW1@w&GrYj~HriCRo` z)yBCFJAEa(pD<1ug70oQ$5w$u=kTSRtC4{^$(sCK`%YLld`%T;r-~8Esz8jnr ziE$M5M-T1(mV4Wepl3J|Hmgz>XfK=D;W&KrISuu;DV$DprKdxO3=8|Op4JENROBwsIy&+n%HUNg9+t% zb>{9Li>*?=Q8bN#I3er!`}V^)u4YF!JCj?>m$UrWhs^(;`-9opmR3(}K8h!>Jd5qM zkd9)v+wsIs&A^Sl!XV(9O*yFHf(pX&V)^&wqCR$Oz@qqkF<@&j^jnr&&OUq!E^N9R z+11$EHf1@(*HXOgy9RwYfV);Mqe5%L_cX=E_{om>W!DDYHzXpPu1-I^vur<080C|i z+bop0Qw_L(dRPq!@>d)}7~_KoFY52#>SKB(rC5gH$X%I5B-oS~6OsEl^@FK92t zph|YuK)L9hPip(cUS@Kb+}`~m%Gz_^-g%|Ui{^$cm4Pa>j!DHS`}0O~5&8{}pD*a@Jah7Vutp>Fd8s8Txz`ME)HU zTUn9YvU?Kw-b1qTH%KcIMQd+hv$UZ5g*_XNeBIv9DY8{sp*JT2DSz94RT;`2hzRt3U~!ZLC&rd5+Ir$6>20O5d*4Aje@8S5wnFtv=*nWf zlE4scA3)p3`rGe&feL#yt{Zq8NjiZ)h@A_=&MzV6bY!cN6g}iClC%W&Jq-7W}N&*6snjlqI#drE|9KXYK7FgO@KmTo8iL6sQ%PEnYs2v~ zK4T@$8C(=aZeAYAaf0bhIfDbF`~w&M6$>d`;v0RJcC9!hFuLnpsc7l{i|?OiOXq)v9d>_bMEewd3j@_D z`H*BLOt{-klPxy>8{HiyN?%rFXk5X8mdw8H$o1k4BMl~q?&q1`89BPrj%ZaGj3SYz zVfqFdHVAA*tq@G*gZ&?;2Nqy~{(aOtE1k=UEJfoMibX*>Z#I!q?{}|=V+?7_B(rT6 z22JS_iLhjfUn=q%|J0KuB!4kQG}ePaH0U3*N)Q=(PAnemC+4eg0wZD9BDJ4{yt z;j%H{{)vvf(L+k=YkxNb!)ath6*n1OnMkADboM$h;+`fjt3)=McMlHx;E10kUP6-+ ztQq1@&*IgcJK3wgdx9zBl>DDD>Vn^j1t2p`Z~pYge2d*t96thtu5DKGwiy#UqUJ*)SY z^!V}wx?&5K!Tv?Qoh!-N@E@0D<+3@=tgCNqMR^68;F+a*k~&~1X8xXY8-!u;^?J<$ zq(t_gm8Le2FDphb+d_Bqy~VvpB>hEPy=VwV^()Gn(KA9<=l#|@(bJD!mOPAfiuzx6 zOWdCt*FjMNc`=6wM3a#Wt=o5o)FWyeU3c30VKG3rl}z+aC@_-|SfD7+%y3XROStD z%S(y@Q^G1HyUx}Q?KxHl(2N6a1AgVsuMc2ky~qK27AK#vySko7%Ra6iRb!^xoRv*o zDw0B7Vq2x%j^Z+UsR_CR*V`LNo=L%w*IcR~MHDwJdH|0mgJv8{X~F#LZHc53B7fwa zwaFcM{h{D-S8D!|XU>%{)aI*_(5wl4_wn-g~y zC;x8kj_;!Du4_}6g4OR`JY4dRA7C`}PE``ExQSxTDZ1}k9|;?xEP{?NhP}xEH5dls ztk_mH2ECggXlvKA-WOAHN45ZTjH*}SCiJ(DB3|C2UH-c%Qy{LP9%sx_OyJ_Z(Q)r> z7WdQNd7IjY_U`CK|GSkW{R%7)5)s9}9Cr1cqs39?BOYFfDk0TBD&@Z4WRiQ?JzcT5EGD3hEhe8C*BGbm zX2k}4Q5`ggSFOo`r1+iWue*lj%_#XzoVx61BI!7+YvPb9_|x5^_2 zeX&NfCYx?g`j(%#xUB8h2u6!!Qd0H(3ew^tLgR>O9K)8nNt&mED?ojx?m3sBF^)1;Pe%infg_~+IO7Ii~gclIU8J|55OX!NG&%cg{4oEo9nMB-H~U{ z*XtxVr4Wuj<1fW4S*!5+cg}(H*!oEzcpyzyxCX;L3LHm*i;3fI%RkJ&z@}v{uBx-T zV)c&IMeW^Eb}bVtB{Fa;wpO|$rFX^6Dg7$zd0@hHk>re&4dh&!?9ldpEkj?RB_ zhby&87J1OSyXfR>NRIFj?6Mh1)%vtBz|%Q9;hj*3mewzP7E_KVT0p-cwm4*izrt-n zgELyAJjT)Q^f|-RvcNQ9&u4PAh7)2`&UYSW+P!E zo#cYEg!{KTY$DjTtoL{kc%;sH2$s85rnF2{jKGe-r1OfH%K4oy zD9o;=Ft*sUUa0@QKU3Qt29AQ!R(=_(0SKv70B7-1?Dwz+{dR8sp3&n)eF>F5fIfln z$gkdTcXR?s4_?xh<3?@C`i!tYAgv#%51TepZaIVP@w6(vu5--xJDtg){@{2@$%4LQ z{@2U0KH>UrI6!z=@6Fo5o-y_e@9rt-4?1=|x|naL>jwTPZF4h<5ppXgw{qjeX~u z0|vz#{*w8kK(e>WaZO|*ou)M}%=NXdh`UJ<&O?oGsO;zdOzo zmy&%m$LvFz?74*jjUl2QNpzO8XPNnrqyobN+RdM%HW#_`?C|?dlD;+*c=K>4(&dDe5m9J+DG3zA@?eS&{pClClh<#1^8q1 zh@Txf)oxPv*LoY$q{2RZ|J0C|BeN9nnPJ!|Oeh8-RJH_U+@PJ3OX>GTpczY`IeGjJygLpBUpkZj6WDQucB&FH=?LC5AXyvJqPs z9ZkvC^3B`C5TkUfl1 z84zJ=_?)WEbg%AYc+eTudBkt-%{DZ-?JR=qJ&*rYZ@THJHXPCLKg_eBEw+Zrkmh-5 zx-+X1idgpGR%Kt82Nq)6ztP=KPmCc^t9-H?+r*6phi#Nw6QO1LroitKvERMX5HN+& zXQJX1i7jkK_9OLhwR$=`p%8rHw5k=s!wU!U+v!HWJWbT(IP*`vj(mGo= z`VNv*6+GIuNysWU#iQdZSA~(C8RJzH5|9QsSxX^%CO{Bf6Uk$a(?UpDi#BTWw{ID* za7y-c^3lKY1q-mTw$@F|eEDZj-tom>;)PH3S^wQg_u8fRGOGy&L0Yq(;OL|9!WEG6 z#ovh(2*wnJ`O3&>nCj5dlA zhDb_P86C0@DE{Ocu_xnmkp@YNC+VAl6dm2lzqjuZwbjJHmd)4iZ(9v!Uh0%jL7<-_ zrf*TR789Lj+)sX?LyZz#s@es*86!8VOTFXdN)qv^>wO`ScEq|85pfxI6e-awFX`OC z%D#s4QrVl-ohIA1VI?po=y|R*6(EU^dupC-aWBb2Ze;WeCc%>2e4c|hS+5qnL8^2e zB>o~hp4w9NP{5+`){wOc3bgigxUh7N0zzC@4jnl_vH^kfUAniU{AX z0;!3rhy&&xNUaSWWEc7Vk4)tXV2|}5CklFG$zx5c@}*-ra2<-4+xQTnnN~4btSwTg zgdFKbkL%{{_{qXu;Uv8+Bt$m}?C)R_34YQq*PI$!erVKm5W7HhGy2rz{&lWc)rZ*D z2M;6yg5E|FhqUecv}R$zz1nABf@Im-+i&EN##k6ES^J=*u17n!Y++E`ty3B|otw2s z!{3Ok-;-=+U*Sa@;rQ(AFoo(K(YoAsNnxTS=@JJL@FMD4F24HeOnb3+u?F267<{~I zWFWBc;OHS^JSeR%kJt=V71bHL$&WM-_IJp(`<0@&2#VvCdl-B4A zF)l1py$=u@vhM%DW$>sE4kyHD>D^}2&LYkvRFrnUITWI(|K%%U>a+$$P}Md``65IJ zv>%lSLkZR#(K4!+NH2Y~dLzO}i#UOf9D5pp;eG3zLy5`AN4i}h1*FAHPPKx**3Rq< z@7qku5_81s(zrF8K_liYN<@_<(jMPPn8s1HSnnb0c$Hv9PrlmjdI?jjpDUc^pWIs# zMI!&H(4l7?G3y3w^w1zobw_hp*AD^rO4~N^Zs{-Xb89TA>?)usCK5*YBkh6jo_tRp z!j<-V{ZaIs{w6{MS#~D!!Xp0`XHz}^W~Y|4vMzoISD?a0D{pH#&7=Pxa(P2@-8Nq3JKR_K#LFz6^Ve! z1?2l2Aou;N&5p7z7t!YCUYlVty#u;}*Euh$YyvEz@^3ii1>KKct9xi#fy3ptr9Khd z;`alK5Y2eORGql&Iyi{0aOn6LO;CrA#)41cjUOX7xJVcaH1bQH(S_PRwyM*tNe2sr zeW}~s^JC@rHE1ID(iwHQSUAGD z&gneJBc8~Khu{GV(2vKugASrGVjj7J!_*nLrw)(FQM=Hrn_0~>ygY9r?|0KCF>X?4 z@&(buHmc|E9Q-0t@2KN2FR0hO{$=YL~+*?pWvyZZO8ppw&B)Y@W zARTc?XTpAD;+Q;bT%N8+sm|9yyK=lC?{TV|6hx1#Pi9fx%AyNQ`*6eS@rc6eT87yf zmc9o!_E@JB`CcqQD;HD$_XZ^SB9R||%v?hDgeNNX0#76M%k$(Zz*Ojb9U-`>0FC7UzIroWx<-!N1l|uf1qpwxAotP zcJ9SIO+3F89MPC}HtU_)mxxnFBgiqFR(0O~H>HSJ@(~IeW~dc1W#I|Ny{sFvN?5O> z8uaNjEm4P2Y0>)4LidmCu^X9L^i%Ca$=nzW4pfPuTF4(b+>dYFKYuc6!u=RaUEUqz znDL3^#Rm)-mWo<&&E-;WO+B6n&t%i?sC6#`W$z`?}yKEhGQq9{b_+ zyY>oV8maj}Al-6lssUxCbSl(dV_3c%?zuAI``R^CQ>;qZ@5FutkX?}~(m^+xv&p<) zA}B0emFRo3u_s1(u_~P%h3*T*`8Zd$&MQ{*%%s`pQnMzHmKWP^8nocK|AdY2-CZQi zyMAUtq2`aw&<0EkJ6l8>;v6 zAd}>T2`y-z396^eyz>>VAo9PiXdD8a%6IpBB`hqMpfa(eSH9^EH`lr1I8;0ozsvLf zzdt#wopD0p3YhQm^da(2q9Ul@wOLyRoAe#c4a<;$^Hz}l1WxBjp!cFCz98q?2t0ix zp;hu!>|L$uujQ~u|VW6wb*4>hg4z_Uo0t+%8_$@KS^vx zRPDH@ahXFjyqi!f=ibEx4GBK6Z3VT%WDVk0#k+RO2F}#VmVAd;t3@H%03(^@GtqFz zj8@0FVh(pp){Q0(oDLjWS(X>VlDJ{9$Dl$2e#_c?QFguT%qdXyfX7+W7-7)!Q>9Uh z^64T_dhpR}`_uA-aKL}F!nKK2e=YvlzLTTuJkR=;SHt?g(}D>ZRmE$J$WK>J7CIjH zI8iI>H$B~UtxQ?KKkE@=eui)SnQ8hGb4I5a@55;UePV^Y3u<%@R8ahpo#J^s;3Fp2 zz|`UnzK#3*VwIh^@w(AiX;aWzQ&{RyJ?9)U5ZhE(FOP_th)1tK1On>iwHBz9bAdx8 zf>J|oKG%B8aF0(on3fNYDWW|tIQKDsij_mEfmIbH|4Qe5K=rE3U*q_aM@9pUCRH&{hwEM`%z*PWd6A20NXyk;)45~*Q+WH^n#CUJDg&4nz z&ou@O7)FK%<|@g{zc_K-acD*cc-a--SPG!ds;vHXVN7M3=?jc;|ZD^IkI>& zn{GIo!_oUeW)PBG4&zqp)!)d;O%vl~S*5wa*Ro|P6xww6JyLH2d5|Ce2)8rKNNuBi zsmsDkj7TXduy0r7ODc}npW%gm4Fz)aZ5ebFAl(md=fC)0t|I7r55G1&Gqk-KvCwha ze_0v$VpB^WSqi%vakD}3$q1RI8%+Xz-p`4dzvqNgzPwQYC4Wh@4e&S$S++KNB#v5M7Jiqg^ecQu47s5<;Wl~* z>Qfxz#DI@S&Zi9-T~pgXBy@!eza+5f|Ao*l&f{cstbPT+s33xW_MVndJuZ*-AsUv^ zmN6UCM3Dg2c?AN3>&Y^+X-V0ebG)JmCS1uhO4K_mnsV1dm2Ly4L z;cBz7l@Nk!@4H*mwVA-oBN}$+?N4WM#%s-GK<;Y+H99`Jw@fU}IU=ak9J+iX!vD%G zkVXqUiO~oCep2GyDu}1-4jI>4UzZ_+r&dq?Y0LID6KSxjN&jy@Z49l62$A)e(8=0&53!c*TAG)sM*fxbJ*h+< zUxr<$K+M#n4<+<=bG>RiILYUPhFtcSU%4oXkfMmB`wbq?~>HvX_~ZuL{Dv#e;bg4*Y2g~@J6X&EIL!c^aG;OFLL z76O_Efmc5kq~M;fS8V^d{2*kw?`d^4)X%;FvxmY$9ZtW3>=8uItPh(uDvQ@Y^!Qw9zXZDRQ_bu(2FiG_6& zUNiHR4<$^LFL`_eZ8}=IaLEVg;O+X>*NqYtV9IhTr+lFgN_!?rkpbc)=RN#P>nu`VCbuNqLeZe_F@fFP zr8!A0{;6&I?0ChB%vyyRn5+BJwqS9I6fX46JIMe@r+a!ex@E+pIo)r&59Xv{loecz zUFNO~N^Zu>HR+G=KjA<@dTDhUCqy3J952Lx-zU@YJ(Oh5H6xP|ke|VnioXR7wONm7 z0-S&8i-?4rC>D?ZTEm3JPT=3Uyq(;Ne%&mgr5CRf`$oZ9A=sY6pYeKcBKED`g*qX= z-;MJxO8Br{=NpZKP;v~E_{ISPSYS9pj8hrnbL{ejzXA+F1eM9+o&MC$poKWRBxF}^ zA>lhgxj9Iih=&D?(@x&&*HVUb2u>7*un8P4C_wEBvo$QznA+B_I*YGsD8Y)v|GY2% ztA-ph;o0S@67u}>CU?L74EWDJ<a$CQ{495X?Q7`b|#+RhtQHnr0P}%C9d- zl$?vV;SsiglRsso4U%<({auf0Gv>9*2)5&|+&*J?h?S`*dbYczQg!6j6wD{z1$OJ@ zt)sw@i=x^T*7pTCKLU@vSPaOJ!t+P^5puSdhB&iFGY!L8R4i_I+H|cgx;p-jWhQh# z>-2=67M@htZgJWJ7WT42tt?#$o&ZZA|ZR3b8G z+KdP)uoN@+R_~lhzt#4kNik~M&^Ho`2@k2(j8ndKsHHiwbON4vp<>dg<3Y$2M9tVt z=17wHXy(Px!qh5MHkGA{?^P-<=B}R@lZ4P#3Flo;%}w+J;@m4w2w#V)uStiAJs9*r z^yO>RBTF5TzBq6okfnj1V{QYrW_%!6Pfrd7Tmk_Bx31ygf*E2*eZt$>7GPr^pA$~Hc5N8cOs(OEOtBq^ggSpOJaWq{o=HnLz}tRXAhGQ3SE`zWYhdXNOfp|ZI1Q$sQ?rN;a|G^ zXj$j*BgE&sf+oFgyXSBK0T;#fPG)=GDK9vkU%w2IGHq_nB?@>&nrEaEIwj(ylLWef zCz83YB7=0jFR;<8+7h&b!!5^*;J6-5fJ8pZDd)2>-u zz#;vIHdCK8v~?@_wS9!};D7R)jIfGL%Fbve7emSwLCtOdAm+~mF4vc3#-BlXx7ai7$ENcRI0%Kg$sX;AGFw#}#bg8a z5iLu=hTdhYJi$N3w(eXHSihm!i`z5f#b4DXq9kKWLKayBTfI;}w$gV*-krRV+>(b7 zqU(eE(3&&Iv?G~tNzCU4s8;{47rM5VoX)9EWAYNJ z*$IM}Yc{3=N`imqFtuB;Z(m=9u;iSXo7m(L{~Y&w+Q9hR)Be^JTYnPYsKFUxR2`?* z4O>y&_78zyH(MG#a9}Q6bbB3T{95m6LgT4~b;VfN=UN#*e;rEQRlYnkHE#UGlW|o( zV4gIG+T6nlgYGu)5DL++MA;}JrQMkEfH*#kQGHAdJM!sjqU#!z2jIzvW~ZDzzl2~@ z)Nw^A?KRNq;-OVxi0fYN0oB2>{!a@MVBhHf6p9-wxeOdEfqwVNj%sT=1zJbZ)!7$0 z#=t6)j|gjjYV;i3zR(T1IGP8jP79&ClYy?QjOxQQi#K%I@#K45a!( zM=~}(Yzni@;QuR8A1;{Xbar*7@AB&WvtS85d6$A*&FhhH6<>Y#_jJlf0GWsm7#Zts zDy|}mA7+5`V_;ohH_}>7qh-%9gsh{m@WrMV)4? zRg)#IQY;XRn?J*fp~Zv-#}DnHr(q>h zbb+q<%Y}AKc?ujSJiDgF-wY9;wth?`9lU#n^oebsD0bpYI`q5e@trfF$dfMN)QC&N zEp8ZLMm$_Ar)66#5-IkUWeSEE+m7gB1sjbjL`C4%6{O^6AR&4AW$EEmf zk_RP3QFuVI{IE6>&KtRItN!+8bHy{gy12L%{xYojNhAu%QB9%obI97|y_D|qhmz%W zP*F>4B7(vQb{)SF-5i@wcLZkgO7cglXq4a0@bA>K1oV)_^0~87weapE++@y`505x` z5qxL-{FFRUkLJ^4_WeD}osej%CyQBFAGyT4b+C^P9t8`V-K5tAc-VCB2tY#{CFUG( zgbBg$s(EtTfH`D0sv^;uy?VekvF}&&?mv7;_nC>6U0s-ak7Q%1S*T(H?5m6IhI>+U zz}}+DIZ5Kysw`rz{@8NOA^nme534xpGI(L*Hr*|?lICckfC|#}VpUY4t(&SEt0lqX z5+MQ)om53$?>l3ea(Ms3#F($qf6|&~K*4-%@aK#;BiEjo<}lO-e31wNh3iJHRa}Sb ztwyf!v^$xJouD7U28o6=ZWfhs-{W&QBeY4YYe^E|3|X4v#neUEXtj~o@3S$rW3_VD z)^AtpO}t1&{j!WBq~2rCH~J3Y99~HzT=0`P{nz9CTV;_Og_coYYf(7IMFNK|M|#!Y zyh3@TaIA3!31@>C-+=EOwk9w!T(FUa0S!%!K0{RWkgHU9<_h(C?K)`f(8Kt9xZ?Y5 zXHK4)rzD()tv24>#4Be4L9drAH(gz9(2v;a`?{uuQ=)ZW!5wd`-N?`A7frR>_sOJ2 zF5l za>dyHj38J$?_i5Vgs!CtGbFJy?EiFLh+dC~Cgo0M89VesIQ(pwuxNjnM_b?ZQ%g2c zO%|hoO<+h;-JO-Uw%Y~03z-%O>ZUWd!l`RNTXCLkw@>civ94XzUM%Vw2rLLBG>2V0XD^r{>j~(gJq8_R&Cvl#y~q*YP3o8QCGU^B&!cXE&);W(082lG_t zz_P7%dfuV^_#(3io5S(KZ+s*j#iZ$kwKEX^ZmM2wg7_VV4J`w}lgTAR+SWr#%f3-G57yU51AfNQ{dM za_lg-v{X`WAF%k8FsD(Bl3k2peT}V7VQG$}xg{`8{O*RW22((Mp|(w|t!K}LWKzN6 z*IGTGDRkIq>@TK+rO1Cojs-PU2GGD?xG^xzoXIa=p(IP6IJhFnRm}lHzdDvngC08o`Dq5_i zGMDU@x|#@HVUV^p@c=9Vv=-^+4WSFJFD5711P!fq?+RJ}5Ub>+_n!tw)#UXDKpSc?VO_%f=GjASXd zq_mysuU|8Z6#+9MIKAo3n#f1O$!7F1@SBmjP^dA6Q5FZfH8q;^o!!?R>GOt*}8P; zE?+QS<<{svSs#sc;=Om*&KVV0n&@B;&=#;xw=7KprtpxNTW*D%?dN?bw2L36ab-a< z8tZ49@|{ew!d$@Mj5*NGE8=R>Q>%V_{1)lgW%T@ThZE{T0$g@vE3=)ZyeKR~tR5 z)Y&*I_ZT8Rq|4eDvB!Uw6SnubU@iR~te!&m?|$jCJgS3WW|}C1|7SCfMsH+d&?NG! zKU;na8P*^CtVVQb%36uub+5RtTv2s8+UK;wVF2rH#JFsmPUkc{*_N>6zIRu9mnv-? z*@tdD<|zcl&Yd*c`q@$!tTIBQxhBx)G2U_r53oIbQCxj2uCX8JfWJM}M`GD_5C5!D z69Kps?pzW00-Lh;I%*>|AnCzkUCrxUiwjV}^<}fSWE6BOqR>j4UrxgdXl+`UAZ1RF zgdmWqq0%hot~8Y@Q9~2s=$7s9lSGV3#>d=AnglU9b;~pr0e0G|L&P~1uhtxzv)F8U z8%J`A+Haln=y9R*hXBA+jTKwQ&MGo+2Zu9vdaGBwWAVzx#Ch~9GZHJIm0AHSm9#^- z^G?&u#Fl~yV5XEsY^Kh#unzD#{ITaZ+<_GeYc;*H8m*auaB*V~{Ij|fWqZ$0+UPq* zo&vxLmuZ8s&We}gcHK?j@-MWiDs9uWa(V=24Ws<+9wJeNVrDi+Uz$=45$DF%TyrC(u^rg&NVc<(CU)G zP}(DmPZ;aEv|E=#yaG~!&G!w_dX7Nmf^$b0d*B|l?G?Y}8foNH{cEWeuYn3Y-9v+a z@_;#0RHP+bH7RjNd>*jorpX(qY1jl?$LdbTV|c*sZNxxRS^dhlEqgNT)r$~g6!7)r zz<_cgx1WWHYD=B#Z|-imcqx+wvClWIyJn37fMH3y)hA*eyzcV?hCI&vkCp>5KSo8d z6hgYtLh6@~0L*-v3ZCrIcw!maV#y96E3T;vddSNBxQ?Fs;}xa9RzKf0(P&6Ha_2uP zSOx%{OOlII)CpQ569KY!&G9Ke(mX8$5d3=dyHKd{rSJ`5ISJolehzI}CxEsJhk0F>gfnM7mEC+D?s;!DcSH(*ESpmFBO{_sE4ohbe40p)iDwc zj^5%D$DE_YtKiX<_(1kgezYDo!q2niOTQ)bETq1uO%T&tm>Wl)Rz-7ZWB4wBeeq!w z++Qyd5)MWhx32%N%;Rv2+9ILgZ9Drst>M)}q`o`qUvXGWlr8Ir8{1w`GwI>|-N;KSH9YR7t95gaJA5S@GT;%5cy z3Ou5|n+!7wjK82$xq;lZdx3GYxs&N*y3f~-{LgxgOdAt%4XgexEp(__!9$Ku_35Gj z(elVZFjylAW*LUPgwfQCmsqNt5?PqrcnWeNL>Pg0}l86`0(!(gEf1_{%pD$+$v_2}oxg77ooKJ1C}=7}6LG!wy6A#SWH)PM8WNDZlG$RzOEFp$f~?Hp?HJE1FR)>ORFjq|Mc` z8TO*JSl~W&o0z;Aoz^X?wu%u1D>AuYbVOHrluh4$boa2#e&=F~@3fGI9r(wDE;bI) zbrG69@Exof1UZ_}U(%DOq3leJJ2IrRd8(Nwq6bA0NK86n;*Kj$`Ush-{3sl=_KMdP z9NjXsr0nMhlaxMHvZpuvWyx!Jif-(|mg}hInWUX;T-Rc{pYZxxX|&G;tKj4uma=|W z;h8#o79L%jf)U_quxiuHPcut+xUvuT+ZhfQQa|U13*;P*yO=!F_^yO7en6J!fRm2Y zdb;lPMYVJ>FeNj`f*uOe&MKv*mji45XA+gWLOc)Zz1A91XVKeAS5Y(ftB%4aB#YY< zDRvAZ$=9Ef=Vz8H5bQr>C6}G<`D!vZiC6zUvv_1mP6G@!r42?}?qw|REDm>A*1Ctn6QjJ`hOM9(|1sSE_0~49d=0uQRDEDq=uGnEM zvF%JE9{r5K33t*+9G{e4+jSbn8sSTs+e#qCn&*XDEx1akrCeRD!!Q*P(NjkWMe|Dd zqLcjVhCSmqLyQ>f;=qMN8orjVO{ACg z$|$H@G^{@j?fp|t2%;gM$GeI}UOB{!se2i|YQtA^R#$r4Q#O_9ruXTT*gY{EbH>a{ z4NfXV?~eE-^gf38c1h%k-SDdpq*9I`th?xMsjpQP?vBia$Jq5MVaMHJ)OgtVp(+!F zT0q~RRI51I53ZA5NBBtDZB#U%8F|z=AFM4CIU?P~OuQGS7Q}it(u){1tFk<#G)>qM z4$OSu``F3xn7W`(fE#%HYbc#YZ{VZXgq*D;2qyFy3` zrMu%uib#jRp}R}EyOHjaZoHe%_jm7e|K?$zz1NyG^Ugan??MPttGPt{02b+sUw$1x zY`3?;iR;((z$L;XD zjT0rx^TASISBl+Gc%!=_7;A{mg-;+Vc0a;m35#E4Ipb$22FR(2?97l@6J~jQXg^+( zOF)G!7S2d=?i|S;6|(5pAcjnDiqWm&y}W7uq+x&#tr6^1g@c&^G1DwG523! zZMJS4p>+{w0$d*XQ;1{BgewZ95p*|G=;|3Y4!ukJ7DPQq+a(Nqtv4U)R&(5WduvMk zp~~xjM7&!$w1Ryb^2uUe8oij*Z*aG?{)k&G@XF@J%&*e9VP%GTO7bVaq0{!AHClA_ z+w0XoOOfC_Sg^#W`GG7WmB9dbrVMCmt$t8Aml*B6!dCq154~U#T{XM4>9UoOw~j80 zG}TNh4g_X(923!L)e)FUtlBZ2AkksS-|7>scCQ272RWMRS(6TxOJIKXgEaR+`6FRv z)M#ax5b^b?I$2lgodApZ#7m8!;v`1HYWmWwBb=EVmSF$DFT{XK2E+Abe1DdG@SibS zE*Q$z2;qAQf#BuLugNDCNyd;(kLh6HjIs}eQ-wiM2#e^x5%ZT+q+k!=uS}e5B1W>6 zRnkQR^*plgIM&St&f}+>?3yy1>TzhR*R-Uw zZYU~0aT?q)9(5;$d2&*BB^phK6+ag1?b$j7MGYP(wthdW1)YjxHX2y=kU~P@@&L7F zuHyT>wNon|6!AM;CsQ zpnqkP@LTB`w+_V-U!?Im!cY<2eNu0F#pGs{^-b1lY*e8Jkrdh9ILMrw$h>!^)b;6p zIM3^Y^P~1bwmb^I;_|u)Q-dFPvBN#N=1m0VSyf|mY>+(Or%%)0O;l?$m1@M{Q{;1`l6?(8+%aJ$F$)#Hb(ncl}Eu8mk#^m2>e_vC8_!+a5o?kS zeNB}s4XBFBMHQ$2Ch@7a{6FC!5x$A@0$Z^xc2#d0`mQ3pXHl#}6wNcQl!Ady)-Iq= zAfvI__bY)~>8`C4H4|&S^3R|t+`&`Iu*lsSdGac^EkMw10=&v^*A2Nu8fcWV&>qf4 zcXnc5K9{azEh(vy`^0p!0GY@&ut`FEDa|5-QV1`MKT^ye62-Z(nvAnPp4URl8mN;2 zF=pjFvD$Ss9%0}xkAuosOo66lj1(+z6oSXa=<<*f>Cmy6FJ3*?7yT2_{r~gOgV!h*Gmf#=nl3AITuSx}0 z$$Gs9ek7;+W>w9VI79q##3MfcAIF%xqg42O*vHbGE&5fk_e2J^ z&k`BY;qF=%*nN0hi%qf3jX>{jTZWfNVqG8>sq??3#Q(5zjN^_ijq#W-uG!>NF&nxS z3s7%;TeXQD-_Cbpw=$XDoZloLxcKx}=tC0PE7-4>&%vqa@{(*JKuTHHBIxHqW|Sf-1huKj@pL9I6AuZA8GqnchPXJ9J!Pdi)$xqehoB(* zY;BO*&|v4^btAT#{uVw(e?=VeZG(hcSNwbqNhsWHvC>5+$ZQ>@I>$W_6_rkrbIXH4 z^HYZm*X2cv!Ax@}MQX2FqLL{jU5$@6D2l`OE44}Rq49mDHRN&S`2KaICpd@%+?ddx z6U*<(Sj#^wvFC#UR22aZkZ5@6#19THDWN_3kymV@WZ7%pXW@O zZ^g*2Zas>6mnY3NHwp3la@+T61v1ao+^5)CLK320v^IRQG#+{ML~_;e7GU*C4Q;L8 z2fHFtD#&tBnDBpaz5HCx`0EINc7R^Ip4;p0ZSk}K=>1=xd9iA89{F+!%~R^Wg*R=yx5cBN7k5jYv4 z#6nhgDa(>?9Odqsn|ts${vQ2}r|e6sgG!>+pr|XM863Go&#m(cYE#|Mp4qO|Mp4T1kM(w7-S7zG_~afmfa`?>cwPKSq5OhC z(G$bL&QGnA;#CYWZWJH5C@%JF-XkmMdxjAhVy(BN_bQxOj7Mxt3rlKkfdT7C6i&w}N#tcbI2Q$S z9n0M8LO++N0zzd;X^ffDpY(4DX?mzS+;wf_6Gwdz*ldq#Nb6_k7POnV+BB_SxO=gBG;<|jg0drE^8m@-sg_a!gj4F>*EWqA znv8DvxDVi6VYMK&JgwKB|2#V!gG*xYNZ*4KZ$i%P!_$yU8bcv zZ5~6So|UimSKq?7eeQG4p8CH0Nk1s}1Fm>q?(d`*VB;g<8>Wj54{X{<`@t?;X@VC? z;W_FoeH(tJoh-;<_DiN`P1=>IZ(XNkk*P}xDmEJ+rK{r83=80_F+p0c`z_ODLjuwm zYi&hUopeICaf%go=|S7pO&M`IJJ_ZW^->~Qy|<8>w?MqfVd?a72n2&|88*$kF&RPi zmKjLAonOa-dfpGrN+wipa1-H9VC$T`3;TbAVgG-hZ+4W~jnxI|-6+*>vBK;Cr-dpV z706A7dm=u{E^okFIZN3Mp=1&`OTnPfhg80&rtL^|PX8zoHE8(#0++*@Xe^hQaMS(g zWTdsYuj_9zM#y)NFZnCBi*lai@wi6h6{792Mg!nU(TZu3<#q#@V6hKGSOjk!3918| z4`8bjsR9GWA9Sxv^pBPLi{)j_h+|8zOz~1W+^iV{ZHRR|i~DNuZK?%In}WdB>EiX) zQDqSau`RhXR8IY3_^+r~q8fDWR3_{)a6NWIkX#Hbh1;B+7pUlHCQ^O5d7!H0;feDq z6>Ao~t83PNyqO9|2W$E+R_ClQegG*UW&@wgYCF9ku1s#^a3`mXoy+c)x5(3;oflvfQE z{0>LOMGA4WXqiKL{%lkce-Hpg31Ot0e4yZH^&9Jyui+P|Dq7;wj9J$T21Y`;(2QT( zpM7T|kYh+Ek*zw(orhRAx;?+EvAOlPDOA6%(;#>i1a?py)y1riO!T-==n5?J4sk79bVEqkb))sX%KPynUC*I~ra#porulx;Eswn-F{42*4H z#zQYCX1`QSGX=XYy60C+GIesY$Ki;tTQ2{jf7VyqoMUCg;ere5`UpQyA9KJrfv=YD zJn8vn(pxq-iQNO9}=&dXnL7xXH- zoT5I81~yLM^iqUG~&Pm;a zEuM-nSY536KMZtXQkALXC_Yq*Y!s#@^^cC&ItUabeHmL4?9m4HME6(J4(sW$EW|-8 z`v4DZwg0yl;9LQ03hi%;9NvnVsQDGloKaGcCCD9ez7|BlUuuv#Un z>7oBm;t?}xCk-Rpn=%^BI3uD?>E*ESKUJm94cqVGxTDiz#Mjaz5|Jhx%g%)KUgP#R zPFK=)6_JDlA25OK$$}@*gPGF`W`~5J0_jS*2=C+#g#C`~D8s?RP0Ggha>2?0urj#1vj^c$g0S z0nl;W3E44@{vDB{G;p52o~mxRH;uOR&y`hy({}Fftk0Q8>?~PW3fr~U*ISk23 zasQIi&uTqvrH9gbEHE_GL*7Ba9yPDl5)!pr)}$aLfJuPe%8>2BnDZu2 zwC^zO`N3G8J_|8QPFjg%XX%4(eilOjqKL1PGzKrR2My0N&{!kc)am)~(sCLn zT)^Z>q}D;!5{K*u7oNDa!g#tZOtN!;%^h>)M-L6G8q4mwmQoyd0x8z@fNAq-w<*RTC_C!_ zz6zEqBM$I8x@-gZ*n_#ZX75V>)M6Hj>_Ld(Q8#`RowVHJU7y;3VthzDuB#)Ebo(Ye z0ny_VV&7&a{wrF@uOicE&#&el_NP|hqbsy})K066OlcuO2@?O;fMwZhBIb|7^}2Ea z;>D?=ZqHAR{?DF(!Y`dqfSM(OtL4p{RM}vw;=mFQ=3;*MD<(qFc6ZABkJ-NvmY1p> zyd*S0n)TxZWt|GFO@C7I7IV_Vx(CF2{*zR($niRo{|6EY9&d(>K{a#nyj3g)NYGRy zO=Jm+90M1cdAq$KXV%e|VAHfuTGx0zjkxFsZ@?Qjv||l|s&5Vr7os8&i+1S5P#~^m z|2h}vbgA?Y5YW*G{o=W~P&LuXRz7}D)G5u`1w47Djy`2{xG83MkS*&OT7`}^MQeq` z2!7{OAoqXh*6csTaL|_~#l4S(v<5&)Y_PQV z4QB3%RK8EdjvrxieQe}xXX6&MvxMOM3-)XK8l#){&n_bs zeSc&9UY`TK26M z1o&pl_5H<3X-b-6kgTp{c5@{zu=`a{*Ii*Zgmv^L&8)~$FBXCvm9$Ph*ylPK2gjuF+dG4Us+8hAg-Zt=5~7NH5S zMcvS1>^dH;16AYk^bxZpH%QY&N?i9YMTdaGId@Bn01afpKlzRC31*KO>VlSt5o1J% z4-gTcKnS~+JJNbXc+vI7@U8pZ+@D95N4YT(DkCwDVoH8c2x{er=iGZ}YJS%R3;bym z4jQljNKMLn0LVN1I7PyrE1D_2N_^F^Yft!oH%cuziaoteQpw7a!VX6%xf2&~8*=Qe zkg(yE6=6{E-OUi~`10P>jc)*z1jbSB-vquHPi_k7QFh?-ok^ z{#;M(%wfFlh0*^xCIokm_$anj*VFzI(BVdAS)-7k!_0pBLRNTC z?m?NY>yGupW}UIeEXhNxzSd>v{*JE)M}^YthXoebvJnBB^0A{hQIskJIA8P`oYjDL z+g@qE=y~zNhxK`fYcj*$@tJno2yrNM_mz*Kt24dkNmD0-p{umcisyX#`!X>(K=G;7 z|EsOB2(X%N&4Onf{nC49^~q-($$I=ox}Azx1}tG?#2bNQRzw8IXbMv(g$!W(4FD>8 z#E2l^O^c&P$DmL!rlUbjC6(ZBh5A$_F=Mmw^ljjLMC{##EzagATJG#Kd={1apiYHV z?*#KR@y-M!dT~}Z3i5y0-qg{Xaw}ztqCiM5n$A&f7$%a!UqP!SE$hxUJ)ON%`OuP5 za;SU9T6P*Ka%GVnOzZo0K1NBJydy^ruHOC$-jl*G*8wB`;$R2@W0)h%9n-{|k90e|y18ZvdqfQItY7^4O5TgYGO;tvP)y zfp7bhFAT5tIoRm_u}ygBmB%495wsYOW>HlVMa>*6tW5<%!~L%*#HD&?1b@qTtm?JA zXXK&J^+K*mO?LGTxnu2vKLZmnE=OBZW7lQMH7TY2CubOiIFTJF9ly=*97UrB-d`iA z9H9=$F9zEHK}yEoYi5~!{IT#`b%(u(#4rIzfm?8on~8bl68Kw$?y3{==K^kx4w!RW5|J! zsURZ6ambiV(6bxc?gwganU02U0)e8(293Y9uIIvmcr3);)*GmttXZ>ZZ`s_&5WsTlX_456GdKSUyKvJ(f367I z((P0w7Z|gt4n}z&Y83j`&iL1a-%G#S^+fY0(Q;vy$0HGh%gf^A?q`g@YC7B#{N-_k{U zLW=`-!P@z$UqfU=`rHD}R=g`YEOW!hfuNfmTwM1nj^^m$%Z?v!Dl?UEj#a>X#S2@`<7DV$12h)W(UZs5u30aF#TFf6Rws9Ud7h2*^LJ;y(qCf>AS1A(d zDMU$3v=D_0Y*#3wzL6ppV(;hMjDd)1u$d4tODxcM?davlfgdIP1H68zWqy0rP%I1a z%f_pqDL5u`f@QN!O%35J`jA$i$HvcgdnA6QxHfqLz(dqGL?R+^L&@jPl*ITtK%3jo zl>dAx^My^9|6Z(_cnS-$oAjcNF1tjaZn-vm%q>jwoF~RRm%Uizc3m`w*3~Q5tOuo? z+m!r@XEQRQIkG`@hz+QoN>5U}HLa2CDj*UcO)iFLjrjlxO0g zB%*>kAT^?>GQnS*3wiP9&NOhUM+2@jrG1bu>qE{R#0%O2Y6hk8d+(f3ctPDqd|8J^ z1V4X7LdUqWfmUOcX1~&a`H0=>+hmK-kG!S z8T5b#xUMPqQ;h}|*51#KDs~d5gX{+c$OAl>)$-vj zKB<;J5F#E`LANT##9vMKmY`m0;9Rihd=VV$;#5dMDs}XZm2Ls4UU@}gQk(aMP+|l0 zn@LFV`W$ZaZZ|3sf5fNZ~L3m|aAH+Vkzy4sCmLVus>y}&Mh5{;s zB&H{!G*QrqDL(xBcXgas_w8K!`sVk#WZX1GE=6uh$aqiBUKBU~dGwpU7fP$&euW5~ zS)BEW??$CX!g0 zd$!0`hMsgHAfRA1<@2G4XS9!o$+W^Q^&~w(S{}6KiHnr{dZyLhp2Q?kEO=($@JxMa z!Bc)F>6>Y%(>5)iw+ylg8xcMx@1Oex-9%(SPht=OFl5<|n1Kgs#cg9b{3=(vv$ruP ztU4B8rw6MmzYg{nsRBdz8aw#QUI0LCU{;h)!leL zzll|)P7SqHiIY(}qjI{%PqUw}*&Cb>;ny06QVYy7_gegsQlXqg-lHg5CaITI$s4?X z_;I(_X`VdZ#jFSs+3>tPy)IVou;!;>ej?JP%lNxz^Mf1~=mcc_kv^-%IY8f3JNJB)JHU)6rU5`IPpXr0Q64redPhLZ)}qae`fB zy!`5x0`ObbeAuR%pg;{SvAPeX0`3bBJr&i-yVjXbxT- z#9g`To2zZsq<`75-W;C#9IsuP{)xrpz}(vo^Q|0B*<&Gk87i>L6~YA%XdA5+YC|XL z#HCN??N-!Lm;_0VjBmXP{;I#Q67tfK`8b~;2>Bynd=T1{_R225XNiCIaAY_vw*Ibu zb!3=tLT_JS9J+GQ`XD^6(AxH!oxUBQ#+D$3J47;PP{nuZ;mWn{unT={uhV5N*X5BH zFsShQCWNnN_Uc8BnzP-SIp~L_6@ea}o^o^g_qqv02N$kN@H()whIEhet=NJk`TYu#zdh z@>spoL7{6o8cNnqWsHOcWP1OQel~3&)44JrrrI^3w74q7-Z1su;*VAX!{v95Z{M(y zMElO13`zOZv)Hb$r8r9VsRo*f2w;8sW5c#tv#U;?Ks902A2-Yc?e~wg(?cMQBaiEx zS&l=5A2U;B1m?)J%<6zn4avqOStJ7$hG@?R)0*EjIo2J#ye6*yLjSz~HWdwt(E_!# z7Ws-L+c8(;&*}6Hnq^yrGBh@3<0|8AT1(jN12PZgF<08VZ(5hwUa;S(o5BMNJKN?9 zeO!j%X(``{xX>}s|H*+sjV_RF<~RQ7Og@i=w5v3!|FXHGfIx$Fil=M*nq<-K>h+QP z>+#PeF8TuwuF@MiOr#zy#fx&3lWD0&sF6MrWY`NMyl>XVs;w|hmF>;cx!~ST1oEQN z*8FpX^J8iBneTIaw%h~`zakh{2gSz_FRf9vK7nRw&`~G4r3BLqPcf7iX?+V4B^ye6 zOKOlUt+KC(%a7Z=cA2Mr)dF)CFO1XN{=mq5RM(n!VN~3&-O)L8Kc%P2r zYYy&D60ylCo-f#CZ^K*OHgnwT^Qs>iP2eE`<*}Au0igPiH1@~-iy~vtYp%Gn5DnM| zGro*^OS+jLRI4MLLEfV&`7I)(vX?xTm<%-*x&jmA{jCw+SsUv_KUl(wVs=7z{4;#| z@Vf_|7%)aur@`U!@Q`*aJe8_Sodx=KulUL3j9^L$A~FOPuQMK zt$b@R;s)e^>BKd6qqmC^03?;N0ANl)u*xI1X$ah&o|ynsbc|)DbNf8;(KmDB#Xb`{ z`ufb2Q8QY?1{T3l4xifJjbqX9Og8!*H+iy-@}AmQoL}Pq^Spd1dwdvH{n!38be)-2 zZoArBUtC4|Yy?lFpP%IT++s9ZO>=pd+w~1@xD%lFhGgbnLjZVIrOc3w>>jAmGT-sS ztuLZMx{VfDl><&{YiUFIuDGx5@1eCK79ASv!V|xHfKTWXu`YGZWvwP={o0CqFnt-A zr6b3Y4#FNiPHy>5(zQLXn;+{iYAr&KiUjZ(YSGOQbv2yhRR%S-aVP8)rqHzPVH;ol{Nb`2!)%#DSXde1zaay0j@v-xI^(Pu!eEutOlH$D z{fEm&@6-$^Zx`p2??3v0>^2Q1_GO2&>xkcfSBp2QO<{tsxmmuMNqZq!#{0DW86^gv ze)v8STfI28nEj}sX$D_tk|QqSQmN@5)*dIeo|skB+L6TLk`_~gz`sV)s749$yo;)3 zNnSK=2tPH}^47>l=w@2Rs^8VEr^IGz6_iziBc3sC zy%q)Z3Ih(2jAR!LC0JH<_?>n_1222NCrSfAyM@!$%1gaR3%lM|pzV6*+D=RLl~*)V zvmDhjyks9eY!F(4urRU}R2o|4KUHFOQcN+HWVp&d*>0=8-LxZsuAkjPqv5s8&c8k` zP55A$ycewZwdrThl$cvpa{Fslx&O+CLk24`dL?6?PKeqH!j_I(?kr{qfE z2DfwhljQ!7z;1-%eVL2X)kn}rTJuwar%u#{E->r-bW6J?pnCt?RY_`8U|70`wyAya zcRFUWB|~n0iw=q_;!3v(%+IzqlIo;_6TzkF3F|9cwi^x68P~1mc`q1RU$!MxqiG(w zNBq8+>?ST0S?mqM^m|m&_|E|XB@1dV) z5Em^(d=C%Y2Fp{@lwuq%bF2Rlefz08KH2QlZkNc3{c`VybBxtv2r3pkFB4dg_)nGh z&*DgHRVj25onL$PQh^8p|!Qq7oRcYM)wl;x*eVs6FSs7pih0d zZW+r|LeYtN!%D4N7DxTf&vtPkYGU(#w6ut10y#tWfKW#AX`6;EfLTd;4A{j39aQG4 z0Uyh2%!qTDdz>a%R%CnWZ3tfyVXF|e_Py2&XJrv=$!`vRO_4HC(||Al(|6OBAIL=c z4+@TA##DO5NW68a7a5dIntId4St|P%)3G`uZ&>4RMF;fw6muF?)Y0k$Zq{QxEseTx z;_oNeJ8NrpB*1p@Xd0bD#P?Sd_H;bj^MYw>2oG(gB__-Su4TjiENFiFT$UphGPPCG zf_TPCfTV_BUpa%jFslV)tg`=Ihf3=LP(QG|i>wt;Eis4lD=mDLLjCL{T$C(T@`gZM zD%_M(Lr8O5HGCO#5NGkBP<{^czGZj{8|^7t$8JOC(aK@e=5SDgk~!ltN`-yTO>PtOaIbdRcNy$^P%;cLFSa6004?Ix8Q&)rIfVGtXYR9=vq`(5r$*J z`kVAtNQ{aTrm2Tcm#`{ZR4;lQwI$n%F8azLBGbq1ykVK$lE<2!gTaD0AP#f`NFyBA^f#>J-YXbTu}MFQ7okFM|=J)nJ->{iM-PK z!DaiK3B!2-6K8?Lw_Z9@$Um%e!*i!)rMu3wDRPIeXaP|HX7DXbuI|>SOHTrFFLLEl zz!!UG(ZSm_ep5tr_+Y`s=ODM5w)tmN-(NNSo^SDPZlN8I&@#Z+4ZL&YNS$u(5Dg}~ zYm@(KSw?%fsvIsADCgK#^i^?rteoHk&0JJLoCW`?q{M$7*HW-f1p@m)t>BfGetvmZ zG0Q@&?!T|wGPJR{#rFNkCW$}l+dOJTF=4vaURfV$&i<2UX5rlrnF(#YtiF(#+7=E3 z(XgU^#PA?^U{y469)u1Z@=h#W?{jL-zR1`(6HU~L-z=cIeKi;P>YLqr7co8S(<-_U zGo23x^3}G1{ZUh!ZsV^>@9uAe7&7y8sX$|)ig}V7J^81(NAfU>4loxrwpI)}7-0P458fg?-myOg__wAA=5-rO{QHUn$P$12~}I_pitO^U~)u10?43->LitOJ8v8j(8HWjZW zt-++?#k#c8h>Q@wb@8td zw)*87y^XxaAN>8KD@x}QhCk||)Bo(TbQ5Ok-U5K}_@$iC?Sm}UN_wI&G8jMr_)Sbl z?*2DvRv_r8H*4qZQjQ4BG2PuG+t&PGxa7P~ldFc~maUVv*OGgfezrN~E%T=fo`rE7 z8B%7o_ciEJp_l7H!o_>BgU31F~6(!cy{TX6ZmsY4LY?{7Mflm#`dyI?0<_#v#hK(BJrt zGWoE|r>kYPZ%MX;KjLv3k(~@O`&&%wIOE7C{CpujtRHOSf*R(@8pvrbZc|eh? zW@s*!f@H$(iTUqx)OXxHvaRIt@N8WdbaJ#M%nz6Q*p7tf8#Lb>cP^W{cS0ozaW|J%j#SFKfxSC_4q5x<0D zxSWZ+U<`RTLnUR_Vi5?l)u15T!mOj3KiTe%P{=z81=>Udu?D4t+j@0$j_WEmc)%}T zL2YU|cnp;XU7_g@3o!tY_w3p&Rkh0n$6wcoLk>LVHU;BfF5p4B!$UkN_FB?s zeOAUWcQyuB`O%w%oliXNnvU4tQmv)_2d!*s&J3 zB`RWrzHDe90y6Q>ND~?rFjo)mQ6zTc_(dn1dufd!sB^OD@lCyEtQN#gvPez5Rb3|E zEqB3{SieHK*K4xFmDHxGB@Bx^)-mj2Ih)gg%*NZv3W82o!6mYH}yyn5!jp<6{_CNP>%+Fq$*kXKSu7csmkuQNi`)n8Y4P!zc!)E8?iZ9CRl6n>AoJxj#`7~gF6y&Gbh1Ay z+%HBhoa;faGX|r)vi7hRR2|IJU2l+uWV>fR{SaU#UNX7Sra5e#bN8~teFiGc$5p#H zz0g}?Nqp350ifMYa+c$A^PI)o%Ts;V1H^nl%9-paKWvU|X1^Sb`atL4%jHolnH|Ql z4V~Seo_TD8nzw$s3HyvlK&EHJ|8gonJg*MpGmF;2VC3ljgBt$TF8kaT%Cl|On_6*c z7(e=iwfCAdHkk-VWBP9F&6vrq z$H2{sDlY(Bkv(s{m3_D;#XcG{ZqMwo-`xcdM>R!u=`c>e3Ed8B{W+m|_&xYMJC>m8 zRyQuw)&NHhi9h@AC#;1f(@Nd~ejd*!7)V-jI#;~pkTLQ%oMi1Q7Rcqic7qQ=`q6x^ zWGb}vJ}25JR_izU&PU^{k=y_o%1MwSP1^liGL*1>nGN_oN`aLGkagVVq|P|`T2<9Y zRG+cutu!`5YRo!?zJRtiq9LtLHg{QiF%Od6YmQg%4eBsO-~mxBFqm3-+(`wdas-q~ zfCm+rqTyyTJtwO~e&G4G)VHX)-sF{XnQ$$@%sr$WJB{INv22Q;h}Pq7TgOIY8kUo4 zSRM`ODH-{Gri>2={@!|7SAm#>YBQmk|k~C_R+=TM_7qGtBzJO-ed__~KN<=Fi1Ko>T8271}XKZq+2qlYl zQ!7iEHX<Z*bJb*O<)=9QzMf7rgaXuqd*=f*!A2W2)&=Fa(4maHjdK7d3pDD{# z{{|U!l7Wg(EB}~WzCOEAGx@87vQMME$%1+YDGV@*&PT0r zqu8tOc4WORF|K8=B(+%)F4C)7$CGuTZi`rOz?>q686se)XohKgfwXNWOJmr^3eSQc zrMFNkytHrHFf)b6a$G+0YFTceR39b96%JrL-aJgRFVjcVO9Ig=P@o`fXu(2Ib}=vY z+UlHei66(#(p_dmr7x}~tFxt5XghuTrRCkt@@1^K^X1RCDEGd;-Qp_vgcy>sR@aWL zUIdPwy@67L>am^_auW-8{om;0X5ulmWJ0P}(8G}dMt1K1B~h7|mCX3Io_;VEv8ghF zWfXcg=tYf0HKYOzWUV=%nT%N>##q9}W=+p{4&~71Nv|#LzVd5y7V#b)KFU9urM7$V z8;Hdw%hV}39Dp?EVXb!}nih=VSFDIL{>T2J;9qx);k76rBpM#RIBgTZ1;HpVrlOUI zI^+8!-)4I$JZ)9f!KL}O7HL&BBX1Kyr)0s!{150X2Q zpW~}#a5GSR1V=j-r07!hPR^k}$rP_=OPzBFzlvxeHtn$;ls^ zJ@gZ)-$16*6U$FR54=e7N2^W@#HkuTVuf6>3 zwCA6|O4Y0fjm)1~lvJU$T50Jdm$0_k{4RJP^AX&s-Toa7uwKS`UHR97GM|Y~TMausc8e-wWQK%>8nkY+ve)eCcgxcnb ztUXbAvEsh?QxX}-6b>}fTu*s6xpu7EIEdsnYki!KC~b)vGwlvj(40Asg!)zhD9FA* zGZDf+GG(A6wD;1gQn}^zxRQvTSA`pYoH~Go%Z3f+;|UJy%>*xm?26}<%LFw0o^2DCOqKs>`84w7BKA*oLIj;x&U>T9j+EibQzVwG}C z9DGb66P!slZ2{*F@bk55R!6kLRD_Q|@+=kTlUT*^!(JVo*c#wz0frz8GkI|ljm&Fc zsvqXj^1sn@Bj*M!&z(VlzHW;!DAPNw* zH!ig^k-C6@Q^v9V{=j6+2H}Ob_em&0&@H^Grw?(&j5ZknR49gg*uO5K6Ufn5JeO>s zwK*ffKG(|Im^#`q@buuCdvK^p8QFP$aZ7}OwDH{F?4^>I^!!!gmCQ&?T7}dyEm6_9 zu*H>Sv;k8y@#Zz-HLsRRrA-AFnaii>GhWQYKh*m+B$AC^zpDz2?0wD+y_KK*dfRo> zZ&;w!of||eL_u-j;iR!g!Vvf^|2&0q`;E}SKaRAYdV5#y+Y!&GAE?g3VRfOhuV z5f4h)zk=h^*8;MS4P{4gjGYSl?+3ejbrBTkZtL{sFv4m2*_oq&Ls)#3dt-~ow>#(O z7tO4RlR-j1W8Ho8)nWzPDm{SCiayp3ZSP6HQD$j7C6Ah3G%oeff6 z@D2K?UvFa(;RGJ4%FSn4*x&Lanq#vV7Y-)Yc~EgA>iHN5x}d<>r^T_*#;?3spfB<8 zZ}SH9plWIV5of?|aq$t>tM0%aTfvY);vt{^04d0o&a{evC6%PG=Bh__8zHS)vAN6-ydBUPEL`7~bW_7+TEO-N;qw-_ zn(q^9kN3WmsrL#;s>_C?*q8I;yc!k0EVd)2FDuIGlSeGV2VD9Mzo)l9N1!=0VbL7a zw{mNszry`nbIGIemdRL#xb9H6Vf{7CI5TGFw;kEd-a7iDPTTpPj|*tBR?c%-Iu+#& z=3H+zMg_Z+&*~(*5J+c3ITxU^I=8!b2O0lpnC$PRg%u%f9M@c5lN}BY@PG*~FVCdA zH~-l)1G2~1`&TkgYY`A}Z7Mh1c4ZlV?jc2BjOQ8e11_SKXlv5fgSp^EX#Ym%|w5y;9#sIm~5M3mbIcDWFTntzVI>;Pl>b!K{&z9Y}IcuJR836Iyp-gK+&M zoUYe`pZIT^d_?Yk_l!PA5WkW;d8#CXUzld{@k9mM2Z1M*3WYY%^_H0C5_A=h6`x8j z6XMu5%(tmvkXG2eesm^y{x={K7jll`l}N0fWfY!7mPQ03Q=dc3k|IVL2Rd5dFcmBj zG_No`drM|i%mICa8<8k!m}q5jGwOpH55Kfy_PCRwVc)=q>sC$Z(#3|svi_Z+#I;Xe z3;kIwQ?5|V`Kj3&BnCl&w-p;;{J-kYPS5q%WDqOnO=+o|pwm``Fq>cQBA91dnd9Z6 z4(S4oul6rp-Z9R2?EDzniSQ7@%r01e44XQ+n|DEB_bUC4`YykNMaG?-4p z21Fg>=T+S&hi{o7$3Gib>n1~G)c|!m<*U~3;4fTmmQHY{$HcV6?yOX?4BtI_WIzD2 zS?o5W8zIpAvQ7Wqeb-PzZYfBWIf%TMP@rqv)UMsKkqacGyMH}u2>h>q!sdV5qz&3M z3ho4!QZ;i&iTG=Ko`AAu{kS`@8wam=NEpW4k$8D-ce!$J|7%xU5#ZxK9SX2cMAG{v z|G#(&Vm?>f;JF!O3?LL1lNG9lneDT2z41Q?W&7wx)*=ZS8xdNG_+Lzwi^Su`Zm>N(u7w7z6<2O{yJMI_W9g zD=oLf;!dd-TuZ~U@#mBa`uDjXX7v5dW(Ccf{0UvKop~Qj7KQ(Jy4w1QIyY@RtJ-Me z{qbN;`mX+IQN?q1c4SS3qCi9U2(oKNB(H#kb|ongw^}PWust7anS-r|HsM-!c`6Yi zianqCbts`Iv6ao|;A*}b_6?sCu$imwZ80tYFqJCt{Rha@WssiFF+icZ!wMJ#80WsY z^ z-b9RN?)}zlsg=7v=sXda6LJZ8=X4Ac5BkP~WERl=2Qm%^TzWbQOf3)ZuRL$N=&ONr zWe4N-%oEl&HHfCxKZeHmYHeJti#fI93-jgLiBaz7*vgxH;`F>5-R z@i?qo-n(M}V=tf6KED>CxcBA%Pfb@I4c8UMZ!DR{9#a`YjWDC4N-PaU#>5sRlVFTg zawbENNJ){5W9dvXEwN0kBV$Sq2_}{pOK1dP5Va>FLWw1{oCae;RYfzZdf)5m`|q81 z?tS-u_q)IQ`+nbd-Wy?8{w?dR-y(+ag^_;+PiblZPM6J1SuP~@JOnZh4v*R-faik+ zww;X_z#;xmiFuvjd_K=jGa#O}kD`(+y4HMkpuje|lm^15v6)<>VrC;tX z=Vw}`8hVcO2MU*2{t|7U+H#-vgt(iI+NKg{Z!=$ujlsy>6Xm=A{EEJlb70QQ76hJO zEmLnPnx@(JG%QeeSOW1UCO@wSHnlp1V)gQFHx-inS}b5!+pqmzeQ?~gY8UUimS-u3 zBbM#B;NkDk)Ss5^#j)q<1U$xGV=8~0&t0}9k#FGFa~OI#1UDKsYikM(!?gu|2TJJ2D> z%7s}LF{RX*B|q!UFQ^Qvs?8V~o58F#npp`OkHvyI`7@+;o+}a>k%?~SaV@^LqH2vO zHar3ai`T(%^Qs8BFqBsS%&v$7wR|C>9sm&Kz1K&W!qwgX-e+lY09IP9|8}R<8a~*5 zu*B^X4X#}y2Txvw*#pDq)hjhc!Z^EyjMK!lxpd(D@`z;ReY9$@Zjz`d1d${s(rN@< zI0^wu1(E-8dQ}8^IrKg+lGMsFNDFgT77>t>9g?U+wES^DP#c|ntBh_aS||}rPD-+S z&f$i71+sQo*326n=(C7|4>*2Mcot>^D6y+$;p{1WGaqN=kASrH({hw0f?`R9Nf@;X z{^O;&)oy%nUDtD@EtBYZq9vD5C~-HcR@ZRAGpvyJ_06QUmB;nj`mI2!k)mr~51 zpQ-Dedt*`MTsM$p-6r^0Y{>fs111g@mM5IO9E+nLvoH7ko9k&O<#C&wN;jnwY}7%n28aSEg>QiX7Klvd>_yY0EEAxD2ewu6J_ zxbJu6j6*|hBZ)Xbc9{)l)FNSmlM8L`^i0b!# z%9o$=W4>~y)3mUv)GSKLKJqwRjk1l`HEYR)D;jPI6P;{ySapgD!+^u?LQVrekAQX0R!qF&+wkp#jO(`i1~ zwK~7oTLE?SV~syN;tOz#&4M61BMx2Hm0jqUnhGN}YQ;I7C}%^GQ7hTvNOY zFB9W(%z>oZB4v_LGr}P_K7N%3k?el0Q>v`M;s8Xnu_%VVDa}MreZJbBrk_QCEyiBN z+BORs9;l$+RUw5(6}9Tnv}qC@e#{U^Li^Pl5kd)<_bECt^5+B3T;TG4_7-LlPat`B zFmYW$3OL5-E`Ya6h)m?In_Ru;ol6YVMh+vO4=>JBlnDlg*NAtbOTV6%!p{jt$#K+B nqa*Etlc9Hj#?04hV3@s&H@uLTooq~KK-zk`op!Bp4*dRKSe$tS literal 0 HcmV?d00001 diff --git a/smac_planner/test/test_a_star.cpp b/smac_planner/test/test_a_star.cpp new file mode 100644 index 00000000000..a1e5dcf2080 --- /dev/null +++ b/smac_planner/test/test_a_star.cpp @@ -0,0 +1,183 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(AStarTest, test_a_star_2d) +{ + smac_planner::SearchInfo info; + smac_planner::AStarAlgorithm a_star(smac_planner::MotionModel::MOORE, info); + int max_iterations = 10000; + float tolerance = 0.0; + float some_tolerance = 20.0; + int it_on_approach = 10; + int num_it = 0; + + a_star.initialize(false, max_iterations, it_on_approach); + a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // functional case testing + a_star.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + a_star.setStart(20u, 20u, 0); + a_star.setGoal(80u, 80u, 0); + smac_planner::Node2D::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_EQ(num_it, 556); + + // check path is the right size and collision free + EXPECT_EQ(path.size(), 81u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + // setting non-zero dim 3 for 2D search + EXPECT_THROW( + a_star.createGraph( + costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 10, costmapA), std::runtime_error); + EXPECT_THROW(a_star.setGoal(0, 0, 10), std::runtime_error); + EXPECT_THROW(a_star.setStart(0, 0, 10), std::runtime_error); + + path.clear(); + // failure cases with invalid inputs + smac_planner::AStarAlgorithm a_star_2( + smac_planner::MotionModel::VON_NEUMANN, info); + a_star_2.initialize(false, max_iterations, it_on_approach); + a_star_2.setFootprint(nav2_costmap_2d::Footprint(), true); + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.setStart(50, 50, 0); // invalid + a_star_2.setGoal(0, 0, 0); // valid + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.setStart(0, 0, 0); // valid + a_star_2.setGoal(50, 50, 0); // invalid + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + num_it = 0; + // invalid goal but liberal tolerance + a_star_2.setStart(20, 20, 0); // valid + a_star_2.setGoal(50, 50, 0); // invalid + EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); + EXPECT_EQ(path.size(), 32u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + EXPECT_TRUE(a_star_2.getStart() != nullptr); + EXPECT_TRUE(a_star_2.getGoal() != nullptr); + EXPECT_EQ(a_star_2.getSizeX(), 100u); + EXPECT_EQ(a_star_2.getSizeY(), 100u); + EXPECT_EQ(a_star_2.getSizeDim3(), 1u); + EXPECT_EQ(a_star_2.getToleranceHeuristic(), 1000.0); + EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); + + delete costmapA; +} + +TEST(AStarTest, test_a_star_se2) +{ + smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 2.0; // in grid coordinates + unsigned int size_theta = 72; + smac_planner::AStarAlgorithm a_star( + smac_planner::MotionModel::DUBIN, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + int num_it = 0; + + a_star.initialize(false, max_iterations, it_on_approach); + a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // functional case testing + a_star.createGraph( + costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), size_theta, costmapA); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + smac_planner::NodeSE2::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + + // check path is the right size and collision free + EXPECT_EQ(num_it, 61); + EXPECT_EQ(path.size(), 75u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + delete costmapA; +} + +TEST(AStarTest, test_constants) +{ + smac_planner::MotionModel mm = smac_planner::MotionModel::UNKNOWN; // unknown + EXPECT_EQ(smac_planner::toString(mm), std::string("Unknown")); + mm = smac_planner::MotionModel::VON_NEUMANN; // vonneumann + EXPECT_EQ(smac_planner::toString(mm), std::string("Von Neumann")); + mm = smac_planner::MotionModel::MOORE; // moore + EXPECT_EQ(smac_planner::toString(mm), std::string("Moore")); + mm = smac_planner::MotionModel::DUBIN; // dubin + EXPECT_EQ(smac_planner::toString(mm), std::string("Dubin")); + mm = smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp + EXPECT_EQ(smac_planner::toString(mm), std::string("Reeds-Shepp")); + + EXPECT_EQ(smac_planner::fromString("VON_NEUMANN"), smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(smac_planner::fromString("MOORE"), smac_planner::MotionModel::MOORE); + EXPECT_EQ(smac_planner::fromString("DUBIN"), smac_planner::MotionModel::DUBIN); + EXPECT_EQ(smac_planner::fromString("REEDS_SHEPP"), smac_planner::MotionModel::REEDS_SHEPP); + EXPECT_EQ(smac_planner::fromString("NONE"), smac_planner::MotionModel::UNKNOWN); +} diff --git a/smac_planner/test/test_collision_checker.cpp b/smac_planner/test/test_collision_checker.cpp new file mode 100644 index 00000000000..e6590d610de --- /dev/null +++ b/smac_planner/test/test_collision_checker.cpp @@ -0,0 +1,172 @@ +// Copyright (c) 2020 Shivang Patel +// Copyright (c) 2020 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 +#include + +#include "gtest/gtest.h" +#include "smac_planner/collision_checker.hpp" + +using namespace nav2_costmap_2d; // NOLINT + +TEST(collision_footprint, test_basic) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + collision_checker.inCollision(5.0, 5.0, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_point_cost) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::Footprint footprint; + collision_checker.setFootprint(footprint, true /*radius / pointcose*/); + + collision_checker.inCollision(5.0, 5.0, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_world_to_map) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::Footprint footprint; + collision_checker.setFootprint(footprint, true /*radius / point cost*/); + + unsigned int x, y; + + collision_checker.worldToMap(1.0, 1.0, x, y); + + collision_checker.inCollision(x, y, 0.0, false); + float cost = collision_checker.getCost(); + + EXPECT_NEAR(cost, 0.0, 0.001); + + costmap_->setCost(50, 50, 200); + collision_checker.worldToMap(5.0, 5.0, x, y); + + collision_checker.inCollision(x, y, 0.0, false); + EXPECT_NEAR(collision_checker.getCost(), 200.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_footprint_at_pose_with_movement) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254); + + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap_->setCost(i, j, 0); + } + } + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + + collision_checker.inCollision(50, 50, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + + collision_checker.inCollision(50, 49, 0.0, false); + float up_value = collision_checker.getCost(); + EXPECT_NEAR(up_value, 254.0, 0.001); + + collision_checker.inCollision(50, 52, 0.0, false); + float down_value = collision_checker.getCost(); + EXPECT_NEAR(down_value, 254.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_point_and_line_cost) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( + 100, 100, 0.10000, 0, 0.0, + 0.0); + + costmap_->setCost(62, 50, 254); + costmap_->setCost(39, 60, 254); + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + + collision_checker.inCollision(50, 50, 0.0, false); + float value = collision_checker.getCost(); + EXPECT_NEAR(value, 0.0, 0.001); + + collision_checker.inCollision(49, 50, 0.0, false); + float left_value = collision_checker.getCost(); + EXPECT_NEAR(left_value, 254.0, 0.001); + + collision_checker.inCollision(52, 50, 0.0, false); + float right_value = collision_checker.getCost(); + EXPECT_NEAR(right_value, 254.0, 0.001); + delete costmap_; +} diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/smac_planner/test/test_costmap_downsampler.cpp new file mode 100644 index 00000000000..040a2eb760c --- /dev/null +++ b/smac_planner/test/test_costmap_downsampler.cpp @@ -0,0 +1,70 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/costmap_downsampler.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(CostmapDownsampler, costmap_downsample_test) +{ + nav2_util::LifecycleNode::SharedPtr node = std::make_shared( + "CostmapDownsamplerTest"); + smac_planner::CostmapDownsampler downsampler(node); + auto map_sub = nav2_costmap_2d::CostmapSubscriber(node, "unused_topic"); + + // create basic costmap + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + costmapA.setCost(0, 0, 100); + costmapA.setCost(5, 5, 50); + + // downsample it + downsampler.initialize("map", "unused_topic", &costmapA, 2); + nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(2); + + // validate it + EXPECT_EQ(downsampledCostmapA->getCost(0, 0), 100); + EXPECT_EQ(downsampledCostmapA->getCost(2, 2), 50); + EXPECT_EQ(downsampledCostmapA->getSizeInCellsX(), 5u); + EXPECT_EQ(downsampledCostmapA->getSizeInCellsY(), 5u); + + // give it another costmap of another size + nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0); + + // downsample it + downsampler.initialize("map", "unused_topic", &costmapB, 4); + downsampler.activatePublisher(); + nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(4); + downsampler.deactivatePublisher(); + + // validate size + EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u); + EXPECT_EQ(downsampledCostmapB->getSizeInCellsY(), 1u); + + downsampler.resizeCostmap(); +} diff --git a/smac_planner/test/test_node2d.cpp b/smac_planner/test/test_node2d.cpp new file mode 100644 index 00000000000..bc42988aca4 --- /dev/null +++ b/smac_planner/test/test_node2d.cpp @@ -0,0 +1,135 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/node_2d.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(Node2DTest, test_node_2d) +{ + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(&costmapA); + + // test construction + unsigned char cost = static_cast(1); + smac_planner::Node2D testA(cost, 1); + smac_planner::Node2D testB(cost, 1); + EXPECT_EQ(testA.getCost(), 1.0f); + + // test reset + testA.reset(10); + EXPECT_EQ(testA.getCost(), 10.0f); + + // Check constants + EXPECT_EQ(testA.neutral_cost, 50.0f); + + // check collision checking + EXPECT_EQ(testA.isNodeValid(false, checker), true); + testA.reset(254); + EXPECT_EQ(testA.isNodeValid(false, checker), false); + testA.reset(255); + EXPECT_EQ(testA.isNodeValid(true, checker), true); + EXPECT_EQ(testA.isNodeValid(false, checker), false); + testA.reset(10); + + // check traversal cost computation + EXPECT_EQ(testB.getTraversalCost(&testA), 58.0f); + + // check heuristic cost computation + smac_planner::Node2D::Coordinates A(0.0, 0.0); + smac_planner::Node2D::Coordinates B(10.0, 5.0); + EXPECT_NEAR(testB.getHeuristicCost(A, B), 559.016, 0.01); + + // check operator== works on index + unsigned char costC = '2'; + smac_planner::Node2D testC(costC, 1); + EXPECT_TRUE(testA == testC); + + // check accumulated costs are set + testC.setAccumulatedCost(100); + EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); + + // check visiting state + EXPECT_EQ(testC.wasVisited(), false); + testC.queued(); + EXPECT_EQ(testC.isQueued(), true); + testC.visited(); + EXPECT_EQ(testC.wasVisited(), true); + EXPECT_EQ(testC.isQueued(), false); + + // check index + EXPECT_EQ(testC.getIndex(), 1u); + + // check static index functions + EXPECT_EQ(smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u); + EXPECT_EQ(smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u); + EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u); + EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u); + EXPECT_THROW(smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error); +} + +TEST(Node2DTest, test_node_2d_neighbors) +{ + // test neighborhood computation + smac_planner::Node2D::initNeighborhood(10u, smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -10); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 10); + + smac_planner::Node2D::initNeighborhood(100u, smac_planner::MotionModel::MOORE); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -100); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 100); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[4], -101); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[5], -99); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[6], 99); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[7], 101); + + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(&costmapA); + unsigned char cost = static_cast(1); + smac_planner::Node2D * node = new smac_planner::Node2D(cost, 1); + std::function neighborGetter = + [&, this](const unsigned int & index, smac_planner::Node2D * & neighbor_rtn) -> bool + { + return true; + }; + + smac_planner::Node2D::NodeVector neighbors; + smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors); + delete node; + + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); +} diff --git a/smac_planner/test/test_nodebasic.cpp b/smac_planner/test/test_nodebasic.cpp new file mode 100644 index 00000000000..0ec90a112f3 --- /dev/null +++ b/smac_planner/test/test_nodebasic.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/node_basic.hpp" +#include "smac_planner/node_2d.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeBasicTest, test_node_basic) +{ + smac_planner::NodeBasic node(50); + + EXPECT_EQ(node.index, 50u); + EXPECT_EQ(node.graph_node_ptr, nullptr); + + smac_planner::NodeBasic node2(100); + + EXPECT_EQ(node2.index, 100u); + EXPECT_EQ(node2.graph_node_ptr, nullptr); +} diff --git a/smac_planner/test/test_nodese2.cpp b/smac_planner/test/test_nodese2.cpp new file mode 100644 index 00000000000..2f23c12aaf9 --- /dev/null +++ b/smac_planner/test/test_nodese2.cpp @@ -0,0 +1,214 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeSE2Test, test_node_se2) +{ + smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 0.20; + unsigned int size_x = 10; + unsigned int size_y = 10; + unsigned int size_theta = 72; + + smac_planner::NodeSE2::initMotionModel( + smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( + 10, 10, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(costmapA); + checker.setFootprint(nav2_costmap_2d::Footprint(), true); + + // test construction + smac_planner::NodeSE2 testA(49); + smac_planner::NodeSE2 testB(49); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // test node valid and cost + testA.pose.x = 5; + testA.pose.y = 5; + testA.pose.theta = 0; + EXPECT_EQ(testA.isNodeValid(true, checker), true); + EXPECT_EQ(testA.isNodeValid(false, checker), true); + EXPECT_EQ(testA.getCost(), 0.0f); + + // test reset + testA.reset(); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // Check constants + EXPECT_EQ(testA.neutral_cost, sqrt(2)); + + // check collision checking + EXPECT_EQ(testA.isNodeValid(false, checker), true); + + // check traversal cost computation + // simulated first node, should return neutral cost + EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); + // now with straight motion, cost is 0, so will be sqrt(2) as well + testB.setMotionPrimitiveIndex(1); + testA.setMotionPrimitiveIndex(0); + EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); + // same direction as parent, testB + testA.setMotionPrimitiveIndex(1); + EXPECT_NEAR(testB.getTraversalCost(&testA), 1.9799f, 0.01); + // opposite direction as parent, testB + testA.setMotionPrimitiveIndex(2); + EXPECT_NEAR(testB.getTraversalCost(&testA), 3.67696f, 0.01); + + // will throw because never collision checked testB + EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error); + + // check motion primitives + EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u); + + // check heuristic cost computation + smac_planner::NodeSE2::computeWavefrontHeuristic( + costmapA, + static_cast(10.0), + static_cast(5.0), + 0.0, 0.0); + smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2); + smac_planner::NodeSE2::Coordinates B(10.0, 5.0, 54.1); + EXPECT_NEAR(testB.getHeuristicCost(B, A), 16.723, 0.01); + + // check operator== works on index + smac_planner::NodeSE2 testC(49); + EXPECT_TRUE(testA == testC); + + // check accumulated costs are set + testC.setAccumulatedCost(100); + EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); + + // check visiting state + EXPECT_EQ(testC.wasVisited(), false); + testC.queued(); + EXPECT_EQ(testC.isQueued(), true); + testC.visited(); + EXPECT_EQ(testC.wasVisited(), true); + EXPECT_EQ(testC.isQueued(), false); + + // check index + EXPECT_EQ(testC.getIndex(), 49u); + + // check set pose and pose + testC.setPose(smac_planner::NodeSE2::Coordinates(10.0, 5.0, 4)); + EXPECT_EQ(testC.pose.x, 10.0); + EXPECT_EQ(testC.pose.y, 5.0); + EXPECT_EQ(testC.pose.theta, 4); + + // check static index functions + EXPECT_EQ(smac_planner::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u); + + delete costmapA; +} + +TEST(NodeSE2Test, test_node_2d_neighbors) +{ + smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 4; // 0.2 in grid coordinates + unsigned int size_x = 100; + unsigned int size_y = 100; + unsigned int size_theta = 72; + smac_planner::NodeSE2::initMotionModel( + smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + + // test neighborhood computation + EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 3u); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); + + smac_planner::NodeSE2::initMotionModel( + smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); + + EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 6u); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._theta, 5, 0.01); + + nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(&costmapA); + smac_planner::NodeSE2 * node = new smac_planner::NodeSE2(49); + std::function neighborGetter = + [&, this](const unsigned int & index, smac_planner::NodeSE2 * & neighbor_rtn) -> bool + { + // because we don't return a real object + return false; + }; + + smac_planner::NodeSE2::NodeVector neighbors; + smac_planner::NodeSE2::getNeighbors(node, neighborGetter, checker, false, neighbors); + delete node; + + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); +} diff --git a/smac_planner/test/test_smac_2d.cpp b/smac_planner/test/test_smac_2d.cpp new file mode 100644 index 00000000000..63b4ba99876 --- /dev/null +++ b/smac_planner/test/test_smac_2d.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" +#include "smac_planner/smac_planner.hpp" +#include "smac_planner/smac_planner_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_2d) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("Smac2DTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + node2D->declare_parameter("test.smooth_path", true); + node2D->set_parameter(rclcpp::Parameter("test.smooth_path", true)); + node2D->declare_parameter("test.downsample_costmap", true); + node2D->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); + node2D->declare_parameter("test.downsampling_factor", 2); + node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + goal = start; + auto planner_2d = std::make_unique(); + planner_2d->configure(node2D, "test", nullptr, costmap_ros); + planner_2d->activate(); + try { + planner_2d->createPlan(start, goal); + } catch (...) { + } + + planner_2d->deactivate(); + planner_2d->cleanup(); + + planner_2d.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + node2D.reset(); + costmap_ros.reset(); +} diff --git a/smac_planner/test/test_smac_se2.cpp b/smac_planner/test/test_smac_se2.cpp new file mode 100644 index 00000000000..caf7d3381e9 --- /dev/null +++ b/smac_planner/test/test_smac_se2.cpp @@ -0,0 +1,92 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" +#include "smac_planner/smac_planner.hpp" +#include "smac_planner/smac_planner_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_se2) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = + std::make_shared("SmacSE2Test"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + nodeSE2->declare_parameter("test.smooth_path", true); + nodeSE2->set_parameter(rclcpp::Parameter("test.smooth_path", true)); + nodeSE2->declare_parameter("test.downsample_costmap", true); + nodeSE2->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); + nodeSE2->declare_parameter("test.downsampling_factor", 2); + nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + goal = start; + auto planner = std::make_unique(); + planner->configure(nodeSE2, "test", nullptr, costmap_ros); + planner->activate(); + + try { + planner->createPlan(start, goal); + } catch (...) { + } + + planner->deactivate(); + planner->cleanup(); + + planner.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + costmap_ros.reset(); + nodeSE2.reset(); +} + +TEST(SmacTestSE2, test_dist) +{ + Eigen::Vector2d p1; + p1[0] = 0.0; + p1[1] = 0.0; + Eigen::Vector2d p2; + p2[0] = 0.0; + p2[1] = 1.0; + EXPECT_NEAR(smac_planner::squaredDistance(p1, p2), 1.0, 0.001); +} diff --git a/smac_planner/test/test_smoother.cpp b/smac_planner/test/test_smoother.cpp new file mode 100644 index 00000000000..ab05a93959f --- /dev/null +++ b/smac_planner/test/test_smoother.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2020, 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/smoother.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(SmootherTest, test_smoother) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("SmootherTest"); + + // create and populate costmap + nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0, 0, 0); + + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + + // compute path to use + smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 4.0; // in grid coordinates + unsigned int size_theta = 72; + smac_planner::AStarAlgorithm a_star( + smac_planner::MotionModel::DUBIN, info); + int max_iterations = 1000000; + float tolerance = 0.0; + int it_on_approach = 1000000000; + int num_it = 0; + a_star.initialize(false, max_iterations, it_on_approach); + a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + a_star.createGraph(costmap->getSizeInCellsX(), costmap->getSizeInCellsY(), size_theta, costmap); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + smac_planner::NodeSE2::CoordinateVector plan; + EXPECT_TRUE(a_star.createPath(plan, num_it, tolerance)); + + // populate our smoothing paths + std::vector path; + std::vector initial_path; + for (unsigned int i = 0; i != plan.size(); i++) { + path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); + initial_path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); + } + + smac_planner::OptimizerParams params; + params.debug = true; + params.get(node2D.get(), "test"); + + smac_planner::SmootherParams smoother_params; + smoother_params.get(node2D.get(), "test"); + smoother_params.max_curvature = 5.0; + smoother_params.curvature_weight = 30.0; + smoother_params.distance_weight = 0.0; + smoother_params.smooth_weight = 00.0; + smoother_params.costmap_weight = 0.025; + + smac_planner::Smoother smoother; + smoother.initialize(params); + smoother.smooth(path, costmap, smoother_params); + + // kept at the right size + EXPECT_EQ(path.size(), 73u); + + for (unsigned int i = 1; i != path.size() - 1; i++) { + // check distance between points is in a good range + EXPECT_NEAR( + hypot(path[i][0] - path[i + 1][0], path[i][1] - path[i + 1][1]), 1.407170, 0.5); + } + + delete costmap; +} diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos index b6645197b0f..73d6b1f1e28 100644 --- a/tools/ros2_dependencies.repos +++ b/tools/ros2_dependencies.repos @@ -19,3 +19,11 @@ repositories: type: git url: https://github.com/ros-perception/vision_opencv.git version: ros2 + ros/bond_core: + type: git + url: https://github.com/ros/bond_core.git + version: ros2 + ompl/ompl: + type: git + url: https://github.com/ompl/ompl.git + version: 1.5.0 From 96b47139d3c10f3847c66fe6bf130152dfd1a038 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 4 Nov 2020 15:54:15 -0800 Subject: [PATCH 07/23] Foxy sync 4 - SMAC planner, amongst others (#2072) * initialize variables in inflation layer (#1970) * Fix zero waypoints crash (#1978) * return if the number of waypoints is zero. * terminate the action. * Succeed action instead of terminating. * Add IsBatteryLow condition node (#1974) * Add IsBatteryLow condition node * Update default battery topic and switch to battery % * Fix test * Switch to sensor_msgs/BatteryState * Add option to use voltage by default or switch to percentage * Add sensor_msgs dependency in package.xml * Make percentage default over voltage * Update parameter list * Initialize inflate_cone_ variable. (#1988) * Initialize inflate_cone_ variable. * initialize inflate_cone_ based on parameter. * Increase the sleep time in the tests makes the costmap test always succeed on my machine. * Add timeouts to all spin_until_future_complete calls (#1998) * Add timeouts to all spin_until_future_complete calls Signed-off-by: Sarthak Mittal * Update default timeout value Signed-off-by: Sarthak Mittal * Controllers should not be influenced by time jumps or slew (#2012) * Controllers should not be influenced by time jumps Therefore use rclcpp::GenericRate instead of rclcpp::Rate Signed-off-by: Martijn Buijs * Change to using `rclcpp::WallRate` for better readability Signed-off-by: Martijn Buijs * Fix max path cycles for case where map has larger Y dimension than X dimension (#2017) * Fix max path cycles for case where map has larger Y dimension than X dimension * Improve readability * fix ament_cpplint and ament_uncrustify issues * fix minor cherry pick conflict mistake * bump version to 0.4.4 Co-authored-by: Michael Ferguson Co-authored-by: Wilco Bonestroo Co-authored-by: Sarthak Mittal Co-authored-by: Martijn Buijs Co-authored-by: justinIRBT <69175069+justinIRBT@users.noreply.github.com> --- doc/parameters/param_list.md | 24 ++- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/CMakeLists.txt | 6 +- nav2_behavior_tree/README.md | 1 + .../nav2_behavior_tree/bt_action_node.hpp | 13 +- .../condition/is_battery_low_condition.hpp | 66 +++++++ nav2_behavior_tree/package.xml | 5 +- .../condition/is_battery_low_condition.cpp | 67 ++++++++ .../test/plugins/condition/CMakeLists.txt | 4 + .../plugins/condition/test_is_battery_low.cpp | 162 ++++++++++++++++++ nav2_bringup/bringup/package.xml | 2 +- .../nav2_gazebo_spawner.py | 4 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_controller/src/nav2_controller.cpp | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 1 + .../plugins/range_sensor_layer.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- .../test_costmap_topic_collision_checker.cpp | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- .../lifecycle_manager_client.hpp | 14 +- nav2_lifecycle_manager/package.xml | 2 +- .../src/lifecycle_manager_client.cpp | 30 ++-- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_navfn_planner/src/navfn_planner.cpp | 5 +- nav2_planner/package.xml | 2 +- .../include/nav2_recoveries/recovery.hpp | 2 +- nav2_recoveries/package.xml | 2 +- .../include/nav2_rviz_plugins/nav2_panel.hpp | 3 + nav2_rviz_plugins/package.xml | 2 +- nav2_rviz_plugins/src/nav2_panel.cpp | 28 +-- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- .../src/waypoint_follower.cpp | 7 +- navigation2/package.xml | 2 +- smac_planner/package.xml | 2 +- 51 files changed, 434 insertions(+), 74 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index a59fce17364..ecc954dedcd 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -707,6 +707,14 @@ When `recovery_plugins` parameter is not overridden, the following default plugi ## Conditions +### BT Node DistanceTraveled + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| distance | 1.0 | Distance in meters after which the node should return success | +| global_frame | "map" | Reference frame | +| robot_base_frame | "base_link" | Robot base frame | + ### BT Node GoalReached | Input Port | Default | Description | @@ -715,7 +723,21 @@ When `recovery_plugins` parameter is not overridden, the following default plugi | global_frame | "map" | Reference frame | | robot_base_frame | "base_link" | Robot base frame | -### BT Node TransformAvailable (condition) +### BT Node IsBatteryLow + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| min_battery | N/A | Minimum battery percentage/voltage | +| battery_topic | "/battery_status" | Battery topic | +| is_voltage | false | If true voltage will be used to check for low battery | + +### BT Node TimeExpired + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| seconds | 1.0 | Number of seconds after which node returns success | + +### BT Node TransformAvailable | Input Port | Default | Description | | ---------- | ------- | ----------- | diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 4eb1fced5c9..793230685a9 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.3 + 0.4.4

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index b062c777973..c92b06228fb 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -6,9 +6,9 @@ find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) @@ -31,6 +31,7 @@ set(dependencies rclcpp_action rclcpp_lifecycle geometry_msgs + sensor_msgs nav2_msgs nav_msgs behaviortree_cpp_v3 @@ -89,6 +90,9 @@ list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node) add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp) list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node) +add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp) +list(APPEND plugin_libs nav2_is_battery_low_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) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 2917bed7639..2a9cfac472b 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -72,6 +72,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. | | TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. | | GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. | +| IsBatteryLow | Condition | Checks if battery is low by subscribing to a sensor_msgs/BatteryState topic and checking if battery voltage/percentage is below a specified minimum value. | | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. | | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | | DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 8a8aa9a7462..082a830076c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -38,6 +38,11 @@ class BtActionNode : public BT::ActionNodeBase { node_ = config().blackboard->get("node"); + // Get the required items from the blackboard + server_timeout_ = + config().blackboard->get("server_timeout"); + getInput("server_timeout", server_timeout_); + // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); @@ -179,7 +184,7 @@ class BtActionNode : public BT::ActionNodeBase { if (should_cancel_goal()) { auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (rclcpp::spin_until_future_complete(node_, future_cancel) != + if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( @@ -225,7 +230,7 @@ class BtActionNode : public BT::ActionNodeBase auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(node_, future_goal_handle) != + if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != rclcpp::executor::FutureReturnCode::SUCCESS) { throw std::runtime_error("send_goal failed"); @@ -257,6 +262,10 @@ class BtActionNode : public BT::ActionNodeBase // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; + + // The timeout value while waiting for response from a server when a + // new action goal is sent or canceled + std::chrono::milliseconds server_timeout_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp new file mode 100644 index 00000000000..0bc30cd5635 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// 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__IS_BATTERY_LOW_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "behaviortree_cpp_v3/condition_node.h" + +namespace nav2_behavior_tree +{ + +class IsBatteryLowCondition : public BT::ConditionNode +{ +public: + IsBatteryLowCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsBatteryLowCondition() = delete; + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("min_battery", "Minimum battery percentage/voltage"), + BT::InputPort( + "battery_topic", std::string("/battery_status"), "Battery topic"), + BT::InputPort( + "is_voltage", false, "If true voltage will be used to check for low battery"), + }; + } + +private: + void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg); + + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr battery_sub_; + std::string battery_topic_; + double min_battery_; + bool is_voltage_; + bool is_battery_low_; + std::mutex mutex_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_ diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index a012a86e20e..d46dca49837 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.3 + 0.4.4 TODO Michael Jeronimo Carlos Orduno @@ -17,10 +17,10 @@ rclcpp rclcpp_action rclcpp_lifecycle - std_msgs behaviortree_cpp_v3 builtin_interfaces geometry_msgs + sensor_msgs nav2_msgs nav_msgs tf2 @@ -39,6 +39,7 @@ behaviortree_cpp_v3 builtin_interfaces geometry_msgs + sensor_msgs nav2_msgs nav_msgs tf2 diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp new file mode 100644 index 00000000000..4aad6d16769 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// 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_behavior_tree/plugins/condition/is_battery_low_condition.hpp" + +namespace nav2_behavior_tree +{ + +IsBatteryLowCondition::IsBatteryLowCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + battery_topic_("/battery_status"), + min_battery_(0.0), + is_voltage_(false), + is_battery_low_(false) +{ + getInput("min_battery", min_battery_); + getInput("battery_topic", battery_topic_); + getInput("is_voltage", is_voltage_); + node_ = config().blackboard->get("node"); + battery_sub_ = node_->create_subscription( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1)); +} + +BT::NodeStatus IsBatteryLowCondition::tick() +{ + std::lock_guard lock(mutex_); + if (is_battery_low_) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + if (is_voltage_) { + is_battery_low_ = msg->voltage <= min_battery_; + } else { + is_battery_low_ = msg->percentage <= min_battery_; + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsBatteryLow"); +} diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index e8d9d8d3d51..b1350f74537 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -25,3 +25,7 @@ ament_target_dependencies(test_condition_transform_available ${dependencies}) ament_add_gtest(test_condition_is_stuck test_is_stuck.cpp) target_link_libraries(test_condition_is_stuck nav2_is_stuck_condition_bt_node) ament_target_dependencies(test_condition_is_stuck ${dependencies}) + +ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp) +target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node) +ament_target_dependencies(test_condition_is_battery_low ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp new file mode 100644 index 00000000000..992c50297de --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -0,0 +1,162 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// 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 "sensor_msgs/msg/battery_state.hpp" + +#include "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp" + +class IsBatteryLowConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_is_battery_low"); + 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_); + + factory_->registerNodeType("IsBatteryLow"); + + battery_pub_ = node_->create_publisher( + "/battery_status", + rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + battery_pub_.reset(); + node_.reset(); + factory_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static rclcpp::Publisher::SharedPtr battery_pub_; +}; + +rclcpp::Node::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr; +BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr; +std::shared_ptr IsBatteryLowConditionTestFixture::factory_ = nullptr; +rclcpp::Publisher::SharedPtr +IsBatteryLowConditionTestFixture::battery_pub_ = nullptr; + +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + sensor_msgs::msg::BatteryState battery_msg; + battery_msg.percentage = 1.0; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.percentage = 0.49; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + + battery_msg.percentage = 0.51; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.percentage = 0.0; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); +} + +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + sensor_msgs::msg::BatteryState battery_msg; + battery_msg.voltage = 10.0; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.voltage = 4.9; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + + battery_msg.voltage = 5.1; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.voltage = 0.0; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), 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_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index aa4fa05bb7d..b0ed9593c28 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.4.3 + 0.4.4 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py index 5f4de1d913f..05cc3c6dbc2 100644 --- a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py +++ b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py @@ -36,6 +36,8 @@ def main(): help='the y component of the initial position [meters]') parser.add_argument('-z', type=float, default=0, help='the z component of the initial position [meters]') + parser.add_argument('-k', '--timeout', type=float, default=10.0, + help="Seconds to wait. Block until the future is complete if negative. Don't wait if 0.") group = parser.add_mutually_exclusive_group(required=True) group.add_argument('-t', '--turtlebot_type', type=str, @@ -97,7 +99,7 @@ def main(): node.get_logger().info('Sending service request to `/spawn_entity`') future = client.call_async(request) - rclpy.spin_until_future_complete(node, future) + rclpy.spin_until_future_complete(node, future, args.timeout) if future.result() is not None: print('response: %r' % future.result()) else: diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index 58ca2100273..edc952664c3 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.4.3 + 0.4.4 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 443dfc2f230..6b88c512c82 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.3 + 0.4.4 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 07e75e63088..51b054c9550 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.3 + 0.4.4 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 0dd318d8dc4..5702f9d586f 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.3 + 0.4.4 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 806df12569d..13284395f01 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -274,7 +274,7 @@ void ControllerServer::computeControl() setPlannerPath(action_server_->get_current_goal()->path); progress_checker_->reset(); - rclcpp::Rate loop_rate(controller_frequency_); + rclcpp::WallRate loop_rate(controller_frequency_); while (rclcpp::ok()) { if (action_server_ == nullptr || !action_server_->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 91241953ae4..d073d5d491e 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.3 + 0.4.4 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 18c5da7dc86..12c956e3961 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.3 + 0.4.4 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 4316da9ecb9..13feac3cbc2 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -65,6 +65,7 @@ InflationLayer::InflationLayer() inflate_around_unknown_(false), cell_inflation_radius_(0), cached_cell_inflation_radius_(0), + resolution_(0), cache_length_(0), last_min_x_(std::numeric_limits::lowest()), last_min_y_(std::numeric_limits::lowest()), diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index 8a6124a47a9..c0cb1111555 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -72,7 +72,7 @@ void RangeSensorLayer::onInitialize() declareParameter("phi", rclcpp::ParameterValue(1.2)); node_->get_parameter(name_ + "." + "phi", phi_v_); declareParameter("inflate_cone", rclcpp::ParameterValue(1.0)); - node_->get_parameter(name_ + "." + "phi", phi_v_); + node_->get_parameter(name_ + "." + "inflate_cone", inflate_cone_); declareParameter("no_readings_timeout", rclcpp::ParameterValue(0.0)); node_->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_); declareParameter("clear_threshold", rclcpp::ParameterValue(0.2)); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index b172bd87df2..89c3d28aca8 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -387,7 +387,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) RCLCPP_DEBUG(get_logger(), "Entering loop"); - rclcpp::Rate r(frequency); // 200ms by default + rclcpp::WallRate r(frequency); // 200ms by default while (rclcpp::ok() && !map_update_thread_shutdown_) { nav2_util::ExecutionTimer timer; diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index ad1fb15d3a0..619613c6785 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -183,7 +183,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode setPose(x, y, theta); publishFootprint(); publishCostmap(); - rclcpp::sleep_for(std::chrono::milliseconds(50)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); return collision_checker_->isCollisionFree(pose); } diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 5f075bfca20..f584220a2fd 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.3 + 0.4.4 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 76beb12cc3e..974008dc096 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.3 + 0.4.4 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index a1b190038a1..2145838ac3b 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.3 + 0.4.4 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 7b21d0881ef..ac1f054d328 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.3 + 0.4.4 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 754402d6f9b..c7f90b6460e 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.3 + 0.4.4 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index d63553fc44b..10bd9fdd812 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.3 + 0.4.4 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 4b87fa5093d..6f2fdf99c2b 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.3 + 0.4.4 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index c1d5a25ed14..31bcb449ff9 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.3 + 0.4.4 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index 7e25916b8bf..bb597fbd424 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -52,27 +52,27 @@ class LifecycleManagerClient * @brief Make start up service call * @return true or false */ - bool startup(); + bool startup(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make shutdown service call * @return true or false */ - bool shutdown(); + bool shutdown(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make pause service call * @return true or false */ - bool pause(); + bool pause(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make resume service call * @return true or false */ - bool resume(); + bool resume(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make reset service call * @return true or false */ - bool reset(); + bool reset(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Check if lifecycle node manager server is active * @return ACTIVE or INACTIVE or TIMEOUT @@ -103,7 +103,9 @@ class LifecycleManagerClient * @brief A generic method used to call startup, shutdown, etc. * @param command */ - bool callService(uint8_t command); + bool callService( + uint8_t command, + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); // The node to use for the service call rclcpp::Node::SharedPtr node_; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 97cb21af53a..bf326566c97 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.3 + 0.4.4 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 618d318935c..c7931539f7b 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -40,33 +40,33 @@ LifecycleManagerClient::LifecycleManagerClient(const std::string & name) } bool -LifecycleManagerClient::startup() +LifecycleManagerClient::startup(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::STARTUP); + return callService(ManageLifecycleNodes::Request::STARTUP, timeout); } bool -LifecycleManagerClient::shutdown() +LifecycleManagerClient::shutdown(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::SHUTDOWN); + return callService(ManageLifecycleNodes::Request::SHUTDOWN, timeout); } bool -LifecycleManagerClient::pause() +LifecycleManagerClient::pause(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::PAUSE); + return callService(ManageLifecycleNodes::Request::PAUSE, timeout); } bool -LifecycleManagerClient::resume() +LifecycleManagerClient::resume(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::RESUME); + return callService(ManageLifecycleNodes::Request::RESUME, timeout); } bool -LifecycleManagerClient::reset() +LifecycleManagerClient::reset(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::RESET); + return callService(ManageLifecycleNodes::Request::RESET, timeout); } SystemStatus @@ -101,7 +101,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) } bool -LifecycleManagerClient::callService(uint8_t command) +LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseconds timeout) { auto request = std::make_shared(); request->command = command; @@ -122,7 +122,13 @@ LifecycleManagerClient::callService(uint8_t command) node_->get_logger(), "Sending %s request", manage_service_name_.c_str()); auto future_result = manager_client_->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future_result); + + if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + return false; + } + return future_result.get()->success; } diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index ce2a04b735d..0fa71e84540 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.3 + 0.4.4 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 00265980c7d..a8af2f47e95 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.3 + 0.4.4 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 0776fa8bb04..318b0e26fec 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.3 + 0.4.4 TODO Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 0393fbdaafe..a2b412b776e 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -331,7 +331,10 @@ NavfnPlanner::getPlanFromPotential( planner_->setStart(map_goal); - int path_len = planner_->calcPath(costmap_->getSizeInCellsX() * 4); + const int & max_cycles = (costmap_->getSizeInCellsX() >= costmap_->getSizeInCellsY()) ? + (costmap_->getSizeInCellsX() * 4) : (costmap_->getSizeInCellsY() * 4); + + int path_len = planner_->calcPath(max_cycles); if (path_len == 0) { return false; } diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index ade94a94f43..206edc8c467 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.3 + 0.4.4 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 04aed1b02a1..5ba051b16b4 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -175,7 +175,7 @@ class Recovery : public nav2_core::Recovery // Initialize the ActionT result auto result = std::make_shared(); - rclcpp::Rate loop_rate(cycle_frequency_); + rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { if (action_server_->is_cancel_requested()) { diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 1c8f25d39d8..17bc4f197b1 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.3 + 0.4.4 TODO Carlos Orduno Steve Macenski diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index fabf1e3170a..772759aa47f 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -84,6 +84,9 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + // Timeout value when waiting for action servers to respnd + std::chrono::milliseconds server_timeout_; + // A timer used to check on the completion status of the action QBasicTimer timer_; diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 0a66d3764ae..75eff0e137b 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.3 + 0.4.4 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 5d2b5147fc8..396eafd4fa3 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -34,7 +34,9 @@ using nav2_util::geometry_utils::orientationAroundZAxis; GoalPoseUpdater GoalUpdater; Nav2Panel::Nav2Panel(QWidget * parent) -: Panel(parent), client_nav_("lifecycle_manager_navigation"), +: Panel(parent), + server_timeout_(10), + client_nav_("lifecycle_manager_navigation"), client_loc_("lifecycle_manager_localization") { // Create the control button and its tooltip @@ -316,12 +318,12 @@ Nav2Panel::onPause() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -331,12 +333,12 @@ Nav2Panel::onResume() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -346,12 +348,12 @@ Nav2Panel::onStartup() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -361,12 +363,12 @@ Nav2Panel::onShutdown() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); timer_.stop(); } @@ -409,7 +411,7 @@ Nav2Panel::onCancelButtonPressed() auto future_cancel = waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); @@ -418,7 +420,7 @@ Nav2Panel::onCancelButtonPressed() } else { auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); @@ -522,7 +524,7 @@ Nav2Panel::startWaypointFollowing(std::vector p auto future_goal_handle = waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); @@ -562,7 +564,7 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) auto future_goal_handle = navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index b3ce62b13af..110e9e74c1c 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.3 + 0.4.4 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index bcae5f534df..c08f7db86e8 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.3 + 0.4.4 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 68e8fe652cc..546be9eeeb4 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.3 + 0.4.4 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 250726bc167..3456be149e4 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.3 + 0.4.4 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 55474bd3c89..a86154d9bdd 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -122,7 +122,12 @@ WaypointFollower::followWaypoints() get_logger(), "Received follow waypoint request with %i waypoints.", static_cast(goal->poses.size())); - rclcpp::Rate r(loop_rate_); + if (goal->poses.size() == 0) { + action_server_->succeeded_current(result); + return; + } + + rclcpp::WallRate r(loop_rate_); uint32_t goal_index = 0; bool new_goal = true; diff --git a/navigation2/package.xml b/navigation2/package.xml index bd72a39fc5e..56f0a878c68 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.3 + 0.4.4 ROS2 Navigation Stack diff --git a/smac_planner/package.xml b/smac_planner/package.xml index 8de16907864..1905966cb26 100644 --- a/smac_planner/package.xml +++ b/smac_planner/package.xml @@ -2,7 +2,7 @@ smac_planner - 0.3.0 + 0.4.3 Smac global planning plugin Steve Macenski Apache-2.0 From 0ed83f084d105f3eae667e0b7a28431939a07c96 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 4 Nov 2020 15:57:49 -0800 Subject: [PATCH 08/23] bumping to 0.4.5 for release --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_bringup/bringup/package.xml | 2 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_recoveries/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- smac_planner/package.xml | 2 +- 30 files changed, 30 insertions(+), 30 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 793230685a9..f22e3a42f4e 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.4 + 0.4.5

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index d46dca49837..860a6990343 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.4 + 0.4.5 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index b0ed9593c28..0a16ca2707c 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.4.4 + 0.4.5 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index edc952664c3..0032a5b847b 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.4.4 + 0.4.5 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 6b88c512c82..950ac6d9e6f 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.4 + 0.4.5 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 51b054c9550..28e36f559a4 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.4 + 0.4.5 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 5702f9d586f..93c2993f51b 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.4 + 0.4.5 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index d073d5d491e..2b335e013a1 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.4 + 0.4.5 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 12c956e3961..a4f899f9ee8 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.4 + 0.4.5 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index f584220a2fd..f25f807f442 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.4 + 0.4.5 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 974008dc096..0d77dac1559 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.4 + 0.4.5 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 2145838ac3b..a0e474c7284 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.4 + 0.4.5 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index ac1f054d328..c0ee55637c6 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.4 + 0.4.5 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index c7f90b6460e..348997568ee 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.4 + 0.4.5 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 10bd9fdd812..ba7901850c6 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.4 + 0.4.5 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 6f2fdf99c2b..291e7963fdb 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.4 + 0.4.5 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 31bcb449ff9..b3e6b16f439 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.4 + 0.4.5 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index bf326566c97..3eec000ae73 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.4 + 0.4.5 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 0fa71e84540..c304ae5b5af 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.4 + 0.4.5 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index a8af2f47e95..979223f7adf 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.4 + 0.4.5 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 318b0e26fec..ce103f0a2bc 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.4 + 0.4.5 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 206edc8c467..1e6d33f02b7 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.4 + 0.4.5 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 17bc4f197b1..a6c7a5aa0b1 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.4 + 0.4.5 TODO Carlos Orduno Steve Macenski diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 75eff0e137b..e27a8aaec5d 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.4 + 0.4.5 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 110e9e74c1c..7c07bb08ae6 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.4 + 0.4.5 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index c08f7db86e8..5f443d0e0d8 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.4 + 0.4.5 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 546be9eeeb4..29cdcc7fcbb 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.4 + 0.4.5 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 3456be149e4..676846b4d67 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.4 + 0.4.5 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 56f0a878c68..7d0558bfda1 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.4 + 0.4.5 ROS2 Navigation Stack diff --git a/smac_planner/package.xml b/smac_planner/package.xml index 1905966cb26..14a04772e64 100644 --- a/smac_planner/package.xml +++ b/smac_planner/package.xml @@ -2,7 +2,7 @@ smac_planner - 0.4.3 + 0.4.5 Smac global planning plugin Steve Macenski Apache-2.0 From 3d166b748bd704eca05ef671b0b2ca99968a8c73 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 5 Nov 2020 10:45:19 -0800 Subject: [PATCH 09/23] adding looping fix to foxy (#2075) --- smac_planner/src/a_star.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp index 3464442b70c..e3e3190bffa 100644 --- a/smac_planner/src/a_star.cpp +++ b/smac_planner/src/a_star.cpp @@ -411,10 +411,20 @@ AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( prev = node; for (const auto & node_pose : possible_nodes) { const auto & n = node_pose.first; - n->parent = prev; - prev = n; + if (!n->wasVisited()) { + // Make sure this node has not been visited by the regular algorithm. + // If it has been, there is the (slight) chance that it is in the path we are expanding + // from, so we should skip it. + // Skipping to the next node will still create a kinematically feasible path. + n->parent = prev; + n->visited(); + prev = n; + } + } + if (_goal != prev) { + _goal->parent = prev; + _goal->visited(); } - _goal->parent = prev; return _goal; } From 77c1b742e959dda02acf6830c2a814f2278bb4a4 Mon Sep 17 00:00:00 2001 From: Homalozoa X <21300069+paeunt@users.noreply.github.com> Date: Sat, 2 Jan 2021 04:23:23 +0800 Subject: [PATCH 10/23] Fix deprecated usage of FutureReturnCode::SUCCESS (#2140) Use rclcpp::FutureReturnCode::SUCCESS replace rclcpp::executor::FutureReturnCode::SUCCESS --- .../include/nav2_behavior_tree/bt_action_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 082a830076c..eed3626601e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -231,7 +231,7 @@ class BtActionNode : public BT::ActionNodeBase auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error("send_goal failed"); } From 63b9510ddf16abe2b6cc0f21c4d4824ed4e99331 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 24 Feb 2021 16:22:55 -0800 Subject: [PATCH 11/23] backporting to foxy checking goal index on backchecks (#2202) --- smac_planner/src/a_star.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp index e3e3190bffa..b9668810dfc 100644 --- a/smac_planner/src/a_star.cpp +++ b/smac_planner/src/a_star.cpp @@ -411,7 +411,7 @@ AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( prev = node; for (const auto & node_pose : possible_nodes) { const auto & n = node_pose.first; - if (!n->wasVisited()) { + if (!n->wasVisited() && n->getIndex() != _goal->getIndex()) { // Make sure this node has not been visited by the regular algorithm. // If it has been, there is the (slight) chance that it is in the path we are expanding // from, so we should skip it. From af8a857699c6f9f64cfccb7d98b60230ddc3ee94 Mon Sep 17 00:00:00 2001 From: Albert Yen Date: Thu, 25 Feb 2021 11:25:14 -0800 Subject: [PATCH 12/23] Fix recovery action collision check. (#2193) * Fix recovery action collision check. * Fix linting issue. --- nav2_recoveries/plugins/back_up.cpp | 5 +++-- nav2_recoveries/plugins/spin.cpp | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 585d8a77410..b429c8bdcf3 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -120,11 +120,12 @@ bool BackUp::isCollisionFree( double sim_position_change; const double diff_dist = abs(command_x_) - distance; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_); - pose2d.x += sim_position_change * cos(pose2d.theta); - pose2d.y += sim_position_change * sin(pose2d.theta); + pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); + pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; if (diff_dist - abs(sim_position_change) <= 0.) { diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index e24be17d899..103623b15ca 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -150,10 +150,11 @@ bool Spin::isCollisionFree( int cycle_count = 0; double sim_position_change; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); - pose2d.theta += sim_position_change; + pose2d.theta = init_pose.theta + sim_position_change; cycle_count++; if (abs(relative_yaw) - abs(sim_position_change) <= 0.) { From 52abee596117441311ce8fcb625fc2504e345ea2 Mon Sep 17 00:00:00 2001 From: Albert Yen Date: Thu, 25 Feb 2021 17:33:19 -0800 Subject: [PATCH 13/23] Fix back up action behavior tree node name issue (#2206) --- nav2_bringup/bringup/params/nav2_params.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index d0e1ad4c349..07817d4318a 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -264,10 +264,10 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + recovery_plugins: ["spin", "back_up", "wait"] spin: plugin: "nav2_recoveries/Spin" - backup: + back_up: plugin: "nav2_recoveries/BackUp" wait: plugin: "nav2_recoveries/Wait" From 64bf1463fe9fc92f99f8722624a4920d578b661f Mon Sep 17 00:00:00 2001 From: MarcoLm993 <65171491+MarcoLm993@users.noreply.github.com> Date: Wed, 3 Mar 2021 19:22:48 +0100 Subject: [PATCH 14/23] Export nav2_bt_navigator library and dependencies (#2212) Signed-off-by: Marco Lampacrescia --- nav2_bt_navigator/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index da362c91068..22d8f4884cc 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -82,5 +82,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) - +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) ament_package() From 41996f409fcd7b005cf779e0376273cee3de6d76 Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Wed, 3 Mar 2021 19:27:51 +0100 Subject: [PATCH 15/23] Optional transient map saver (#2215) * Added transient local subscription qos profile parameter to map saver (#1871) * Added transient local subscription qos profile parameter to map saver * Made transient local default true * Fixed linter problem * switched back house world to waffle model * Make transient map subscribe backwards compatible for foxy Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com> --- nav2_bringup/bringup/params/nav2_params.yaml | 1 + nav2_map_server/include/nav2_map_server/map_saver.hpp | 2 ++ nav2_map_server/src/map_saver/map_saver.cpp | 11 ++++++++++- 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 07817d4318a..bbfc35d9690 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -243,6 +243,7 @@ map_saver: save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 + map_subscribe_transient_local: False planner_server: ros__parameters: diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 3aa4e2e2332..9c9b57d2928 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -103,6 +103,8 @@ class MapSaver : public nav2_util::LifecycleNode // Default values for map thresholds double free_thresh_default_; double occupied_thresh_default_; + // param for handling QoS configuration + bool map_subscribe_transient_local_; // The name of the service for saving a map from topic const std::string save_map_service_name_{"save_map"}; diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 7555ebd6326..563e0d52908 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -50,6 +50,8 @@ MapSaver::MapSaver() free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); + // false only of foxy for backwards compatibility + map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", false); } MapSaver::~MapSaver() @@ -173,8 +175,15 @@ bool MapSaver::saveMapTopicToFile( // Add new subscription for incoming map topic. // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode // as a map listener. + rclcpp::QoS map_qos = rclcpp::SystemDefaultsQoS(); // initialize to default + if (map_subscribe_transient_local_) { + map_qos = rclcpp::QoS(10); + map_qos.transient_local(); + map_qos.reliable(); + map_qos.keep_last(1); + } auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback); + map_topic_loc, map_qos, mapCallback); rclcpp::Time start_time = now(); while (rclcpp::ok()) { From 3befe9aa1c2546f2178c7ced6fe55fc3ba56642a Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Thu, 18 Mar 2021 01:07:40 +0100 Subject: [PATCH 16/23] Add has_node_params launch utility (#2257) --- nav2_common/nav2_common/launch/__init__.py | 1 + .../nav2_common/launch/has_node_params.py | 61 +++++++++++++++++++ 2 files changed, 62 insertions(+) create mode 100644 nav2_common/nav2_common/launch/has_node_params.py diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index 1f0638f81a0..c25fadb220c 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -12,5 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. +from .has_node_params import HasNodeParams from .rewritten_yaml import RewrittenYaml from .replace_string import ReplaceString diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py new file mode 100644 index 00000000000..21b8a32f9e2 --- /dev/null +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -0,0 +1,61 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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 typing import List +from typing import Text + +import yaml +import launch + + +class HasNodeParams(launch.Substitution): + """ + Substitution that checks if a param file contains parameters for a node. + + Used in launch system + """ + + def __init__(self, + source_file: launch.SomeSubstitutionsType, + node_name: Text) -> None: + super().__init__() + """ + Construct the substitution + + :param: source_file the parameter YAML file + :param: node_name the name of the node to check + """ + + # import here to avoid loop + from launch.utilities import normalize_to_list_of_substitutions + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__node_name = node_name + + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' + + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions( + context, self.name) + data = yaml.safe_load(open(yaml_filename, 'r')) + + if self.__node_name in data.keys(): + return "True" + return "False" From 9831d83f5dfce01d5842c867a0540dede4dcabf9 Mon Sep 17 00:00:00 2001 From: shonigmann Date: Fri, 2 Apr 2021 17:01:59 -0700 Subject: [PATCH 17/23] corrected backup plugin name for multirobot params (#2288) Co-authored-by: Simon Honigmann --- nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml | 2 +- nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 26b8a54274d..4d1e9643815 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -226,7 +226,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index b5d07000749..39d1ff0e452 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -226,7 +226,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: From 5c9cb96e0c887a6a51506342b8fc50d2ab34643c Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 5 Apr 2021 16:11:11 -0700 Subject: [PATCH 18/23] foxy Sync 5.1 (#2291) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * merge conflict * Add groot monitoring behavior tree visualization (#1958) * include ZMQ publisher for Groot very plain integration, should be made optionally through a launch parameter * fix Groot crashing finding custom nodes in monitor mode straight forward working fix. The manifest was missing, so Groot searched custom node IDs that it did not have. This is implemented correctly directly in BT.CPP V3 and should be used instead of an implementation in nav2_bt_engine * refactor buildTreeFromText to createTreeFromText as in BT.CPP v3 * forward XML to createTreeFromText from BT.CPP v3 factory function * Add createTreeFromFile forware to BT-factory function * fix createTreeFromFile args.. * add personal copyright I think this is okay for finding a nasty bug.. :) * move creating ZMQ Publisher from run to dedicated function this way the ZMQ Publisher ca be added to individual trees within the same factory. Should be important for switching trees (XML files) * Add parameter for Groot Monitoring - default true. Also cleanup ZMQ * Move haltAllActions() Implementation from .hpp to .cpp * update Copyright in hpp of BT-engine * make linters happy.. :) * Update Groot parameter naming and chg default=0 * rename resetZMQGrootMonitor -> resetGrootMonitor * add parameter to nav2_params.yaml - default = false * add ZMQ params and logic for server/pub ports * Fix RewrittenYaml ignoring Integers Integers where converted as floats before which crashes get_parameter.. fun thing.... * add launch based tests for params and ZMQ * Activate Dijkstra and A* switching tests, thanks to RewrittenYaml * add pyzmq==19.0.2 via pip3 to CI test_workspace * make flake8 linter happy * make cpp linters happy * add personal copyright * add GoalUpdated BT node description in order to view the full default BT only affects editor mode of Groot and not live monitoring * make linter happy (unused import) * remove unused groot-port replacement functions in test_system_launch.py * add groot parameters to params.md * get reloading BTs to work nicely with Groot * pretty space for smac :) * switch from unsinged to uint16_t * fix converting string into float or int * Revert "add pyzmq==19.0.2 via pip3 to CI test_workspace" This reverts commit 7bca08121c88db3763771911e3c6b4c6f4f8ddeb. * Switch to 4 spaces indent and other linter stuff for RewrittenYaml * removed prints in test_system_launch.py * linter stuff * add python-zmq as test_depend in package.xml (instead of .CI_conf) * enable groot monitoring by default * remove ZMQ from naming (function / variable) * remove variable zmq ports from testing scripts * remove default ports in BT_engine, as they are set through (def-)params * Remove complete test for "dynamic" ZMQ ports testing * fix python-zmq depend location * fix style * swap missing Groot to default True * fix rosdep zmq + flake8 fixes in system_tests * remove debug logs + c_str() * remove final debug_log * return failure on plugin failure (#2119) * Move voxel publisher activation into conditional that its on * fix boundary point exclusion in convexFillCells (#2161) * Regulated pure pursuit controller (#2152) * regulated pure pursuit migration commit * adding speed limit API * adding review comments + adding rotate to goal heading * adding test dir * add some initial tests * more tests * remove old comment * improve readme * fix CI * first attempt at changing algos in tests * allowing full path parameter substitutions * adding integration tests * enable SMAC testing too with new changes * swap algos * revert * Update angular velocity after constraining linear velocity (#2165) This ensures the robot moves towards the lookahead point more closely. If the angular velocity is not updated, then the robot tries to take cuts while turning, which could lead to collisions when near obstacles Signed-off-by: Shrijit Singh * Update cost scaling heuristic to vary speed linearly with distance (#2164) * Update cost scaling to vary linearly with distance instead of relying on costmap cost Signed-off-by: Shrijit Singh * Resolve suggested changes Signed-off-by: Shrijit Singh * Add documentation for cost scaling parameters Signed-off-by: Shrijit Singh * Improve parameter descriptions Signed-off-by: Shrijit Singh * Comment cost scaling tests since layered costmap is not initialized A valid layered costmap reference is needed to get the inscribed radius Signed-off-by: Shrijit Singh Co-authored-by: Shrijit Singh * Updating example yaml to include extra params (#2183) * Fixing control_frequency to controller_frequency typo (#2182) * Write doxygen for navfn (#2184) * Write doxygen for navfn * Remove forward slashes * expose dwb's shorten_transformed_plan param * Adding RPP to metapackage.xml * [NavFn] Make the 3 parameters changeable at runtime (#2181) * make the 3 params changeable at runtime * use parameter events callbacks * doxygen * lint * Install test_updown to lib/ (#2208) * Remove optimization check on carrot, incorrect optimization (#2209) * [RPP] Remove dependency on collision checking to carrot location (#2211) * Remove dependency on collision checking to carrot location * Fix i removal * changing API to be consistent with collision updates * fix typo in regulated pure pursuit readme (#2228) * Rviz state machine waypoint follower updates (#2227) * working on canceling state machine for waypoint mode * fixing cancelation logic * fix linting isue * adding cherry pick fixes Co-authored-by: Sarthak Mittal Co-authored-by: Florian Gramß <6034322+gramss@users.noreply.github.com> Co-authored-by: ChristofDubs Co-authored-by: Shrijit Singh Co-authored-by: Phone Thiha Kyaw Co-authored-by: simutisernestas <35775651+simutisernestas@users.noreply.github.com> Co-authored-by: G.Doisy Co-authored-by: Uladzslau <79460842+Uladzslau@users.noreply.github.com> Co-authored-by: Erwin Lejeune --- doc/parameters/param_list.md | 4 + .../behavior_tree_engine.hpp | 32 +- nav2_behavior_tree/nav2_tree_nodes.xml | 2 + .../src/behavior_tree_engine.cpp | 52 +- nav2_bringup/bringup/params/nav2_params.yaml | 3 + nav2_bt_navigator/src/bt_navigator.cpp | 31 +- .../nav2_common/launch/rewritten_yaml.py | 273 +++++---- nav2_controller/src/nav2_controller.cpp | 3 + nav2_costmap_2d/plugins/voxel_layer.cpp | 3 +- nav2_costmap_2d/src/costmap_2d.cpp | 2 +- .../include/dwb_core/dwb_local_planner.hpp | 6 + .../dwb_core/src/dwb_local_planner.cpp | 13 +- .../include/nav2_navfn_planner/navfn.hpp | 10 + .../nav2_navfn_planner/navfn_planner.hpp | 115 +++- nav2_navfn_planner/src/navfn_planner.cpp | 35 ++ nav2_planner/src/planner_server.cpp | 1 + .../nav2_recoveries/recovery_server.hpp | 2 +- nav2_recoveries/src/recovery_server.cpp | 10 +- .../CMakeLists.txt | 70 +++ .../README.md | 147 +++++ .../doc/lookahead_algorithm.png | Bin 0 -> 108322 bytes .../regulated_pure_pursuit_controller.hpp | 251 ++++++++ ...nav2_regulated_pure_pursuit_controller.xml | 10 + .../package.xml | 29 + .../src/regulated_pure_pursuit_controller.cpp | 563 ++++++++++++++++++ .../test/CMakeLists.txt | 10 + .../test/test_regulated_pp.cpp | 324 ++++++++++ nav2_rviz_plugins/src/nav2_panel.cpp | 33 +- nav2_system_tests/package.xml | 1 + nav2_system_tests/src/system/CMakeLists.txt | 27 +- .../src/system/test_system_launch.py | 25 +- nav2_system_tests/src/system/tester_node.py | 97 +++ nav2_system_tests/src/updown/CMakeLists.txt | 2 +- navigation2/package.xml | 1 + .../smac_planner/costmap_downsampler.hpp | 26 +- .../include/smac_planner/smac_planner.hpp | 3 +- .../include/smac_planner/smac_planner_2d.hpp | 3 +- smac_planner/src/costmap_downsampler.cpp | 33 +- smac_planner/src/smac_planner.cpp | 106 ++-- smac_planner/src/smac_planner_2d.cpp | 88 +-- .../test/test_costmap_downsampler.cpp | 13 +- 41 files changed, 2142 insertions(+), 317 deletions(-) create mode 100644 nav2_regulated_pure_pursuit_controller/CMakeLists.txt create mode 100644 nav2_regulated_pure_pursuit_controller/README.md create mode 100644 nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml create mode 100644 nav2_regulated_pure_pursuit_controller/package.xml create mode 100644 nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt create mode 100644 nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index ecc954dedcd..9278a512a51 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -5,6 +5,9 @@ | Parameter | Default | Description | | ----------| --------| ------------| | default_bt_xml_filename | N/A | path to the default behavior tree XML description | +| enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree | +| groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot | +| groot_zmq_server_port | 1667 | change port of the zmq server needed for groot | | plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries | | transform_tolerance | 0.1 | TF transform tolerance | | global_frame | "map" | Reference frame | @@ -237,6 +240,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.critics | N/A | List of critic plugins to use | | ``.default_critic_namespaces | ["dwb_critics"] | Namespaces to load critics in | | ``.prune_plan | true | Whether to prune the path of old, passed points | +| ``.shorten_transformed_plan | true | Determines whether we will pass the full plan on to the critics | | ``.prune_distance | 1.0 | Distance (m) to prune backward until | | ``.debug_trajectory_details | false | Publish debug information | | ``.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name | diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index ca627bbe7e4..61816a99504 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Florian Gramss // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,6 +23,8 @@ #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/xml_parsing.h" +#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" + namespace nav2_behavior_tree { @@ -40,28 +43,29 @@ class BehaviorTreeEngine std::function cancelRequested, std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10)); - BT::Tree buildTreeFromText( + BT::Tree createTreeFromText( const std::string & xml_string, BT::Blackboard::Ptr blackboard); + BT::Tree createTreeFromFile( + const std::string & file_path, + BT::Blackboard::Ptr blackboard); + + void addGrootMonitoring( + BT::Tree * tree, + uint16_t publisher_port, + uint16_t server_port, + uint16_t max_msg_per_second = 25); + + void resetGrootMonitor(); + // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state - void haltAllActions(BT::TreeNode * root_node) - { - // this halt signal should propagate through the entire tree. - root_node->halt(); - - // but, just in case... - auto visitor = [](BT::TreeNode * node) { - if (node->status() == BT::NodeStatus::RUNNING) { - node->halt(); - } - }; - BT::applyRecursiveVisitor(root_node, visitor); - } + void haltAllActions(BT::TreeNode * root_node); protected: // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; + std::unique_ptr groot_monitor_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 94e3c2591b8..81df7b46416 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -54,6 +54,8 @@ Parent frame for transform + + diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 36b852e61c9..93841615d9e 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Florian Gramss // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,8 +22,6 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp_v3/utils/shared_library.h" -using namespace std::chrono_literals; - namespace nav2_behavior_tree { @@ -62,13 +61,54 @@ BehaviorTreeEngine::run( } BT::Tree -BehaviorTreeEngine::buildTreeFromText( +BehaviorTreeEngine::createTreeFromText( const std::string & xml_string, BT::Blackboard::Ptr blackboard) { - BT::XMLParser p(factory_); - p.loadFromText(xml_string); - return p.instantiateTree(blackboard); + return factory_.createTreeFromText(xml_string, blackboard); +} + +BT::Tree +BehaviorTreeEngine::createTreeFromFile( + const std::string & file_path, + BT::Blackboard::Ptr blackboard) +{ + return factory_.createTreeFromFile(file_path, blackboard); +} + +void +BehaviorTreeEngine::addGrootMonitoring( + BT::Tree * tree, + uint16_t publisher_port, + uint16_t server_port, + uint16_t max_msg_per_second) +{ + // This logger publish status changes using ZeroMQ. Used by Groot + groot_monitor_ = std::make_unique( + *tree, max_msg_per_second, publisher_port, + server_port); +} + +void +BehaviorTreeEngine::resetGrootMonitor() +{ + groot_monitor_.reset(); +} + +// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state +void +BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node) +{ + // this halt signal should propagate through the entire tree. + root_node->halt(); + + // but, just in case... + auto visitor = [](BT::TreeNode * node) { + if (node->status() == BT::NodeStatus::RUNNING) { + node->halt(); + } + }; + BT::applyRecursiveVisitor(root_node, visitor); } } // namespace nav2_behavior_tree diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index bbfc35d9690..bae3aa36c03 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -53,6 +53,9 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index fcb761322c6..afd20df0e63 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -68,6 +69,9 @@ BtNavigator::BtNavigator() declare_parameter("global_frame", std::string("map")); declare_parameter("robot_base_frame", std::string("base_link")); declare_parameter("odom_topic", std::string("odom")); + declare_parameter("enable_groot_monitoring", true); + declare_parameter("groot_zmq_publisher_port", 1666); + declare_parameter("groot_zmq_server_port", 1667); } BtNavigator::~BtNavigator() @@ -146,9 +150,13 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) { // Use previous BT if it is the existing one if (current_bt_xml_filename_ == bt_xml_filename) { + RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded"); return true; } + // if a new tree is created, than the ZMQ Publisher must be destroyed + bt_->resetGrootMonitor(); + // Read the input BT XML from the specified file into a string std::ifstream xml_file(bt_xml_filename); @@ -161,13 +169,21 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) std::istreambuf_iterator(xml_file), std::istreambuf_iterator()); - RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str()); - RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string.c_str()); - // Create the Behavior Tree from the XML input - tree_ = bt_->buildTreeFromText(xml_string, blackboard_); + tree_ = bt_->createTreeFromText(xml_string, blackboard_); current_bt_xml_filename_ = bt_xml_filename; + // get parameter for monitoring with Groot via ZMQ Publisher + if (get_parameter("enable_groot_monitoring").as_bool()) { + uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int(); + uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int(); + // optionally add max_msg_per_second = 25 (default) here + try { + bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port); + } catch (const std::logic_error & e) { + RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what()); + } + } return true; } @@ -211,6 +227,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) current_bt_xml_filename_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_.rootNode()); + bt_->resetGrootMonitor(); bt_.reset(); RCLCPP_INFO(get_logger(), "Completed Cleaning up"); @@ -243,15 +260,15 @@ BtNavigator::navigateToPose() return action_server_->is_cancel_requested(); }; - auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree; + std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree; // Empty id in request is default for backward compatibility bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; if (!loadBehaviorTree(bt_xml_filename)) { RCLCPP_ERROR( - get_logger(), "BT file not found: %s. Navigation canceled", - bt_xml_filename.c_str(), current_bt_xml_filename_.c_str()); + get_logger(), "BT file not found: %s. Navigation canceled.", + bt_xml_filename.c_str()); action_server_->terminate_current(); return; } diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 4d9b86373f4..40332a64275 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -21,131 +21,166 @@ import tempfile import launch + class DictItemReference: - def __init__(self, dictionary, key): - self.dictionary = dictionary - self.dictKey = key + def __init__(self, dictionary, key): + self.dictionary = dictionary + self.dictKey = key + + def key(self): + return self.dictKey - def key(self): - return self.dictKey + def setValue(self, value): + self.dictionary[self.dictKey] = value - def setValue(self, value): - self.dictionary[self.dictKey] = value class RewrittenYaml(launch.Substitution): - """ - Substitution that modifies the given YAML file. - - Used in launch system - """ - - def __init__(self, - source_file: launch.SomeSubstitutionsType, - param_rewrites: Dict, - root_key: Optional[launch.SomeSubstitutionsType] = None, - key_rewrites: Optional[Dict] = None, - convert_types = False) -> None: - super().__init__() """ - Construct the substitution + Substitution that modifies the given YAML file. - :param: source_file the original YAML file to modify - :param: param_rewrites mappings to replace - :param: root_key if provided, the contents are placed under this key - :param: key_rewrites keys of mappings to replace - :param: convert_types whether to attempt converting the string to a number or boolean + Used in launch system """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop - self.__source_file = normalize_to_list_of_substitutions(source_file) - self.__param_rewrites = {} - self.__key_rewrites = {} - self.__convert_types = convert_types - self.__root_key = None - for key in param_rewrites: - self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key]) - if key_rewrites is not None: - for key in key_rewrites: - self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) - if root_key is not None: - self.__root_key = normalize_to_list_of_substitutions(root_key) - - @property - def name(self) -> List[launch.Substitution]: - """Getter for name.""" - return self.__source_file - - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return '' - - def perform(self, context: launch.LaunchContext) -> Text: - yaml_filename = launch.utilities.perform_substitutions(context, self.name) - rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) - param_rewrites, keys_rewrites = self.resolve_rewrites(context) - data = yaml.safe_load(open(yaml_filename, 'r')) - self.substitute_params(data, param_rewrites) - self.substitute_keys(data, keys_rewrites) - if self.__root_key is not None: - root_key = launch.utilities.perform_substitutions(context, self.__root_key) - if root_key: - data = {root_key: data} - yaml.dump(data, rewritten_yaml) - rewritten_yaml.close() - return rewritten_yaml.name - - def resolve_rewrites(self, context): - resolved_params = {} - for key in self.__param_rewrites: - resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key]) - resolved_keys = {} - for key in self.__key_rewrites: - resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key]) - return resolved_params, resolved_keys - - def substitute_params(self, yaml, param_rewrites): - for key in self.getYamlLeafKeys(yaml): - if key.key() in param_rewrites: - raw_value = param_rewrites[key.key()] - key.setValue(self.convert(raw_value)) - - def substitute_keys(self, yaml, key_rewrites): - if len(key_rewrites) != 0: - for key, val in yaml.items(): - if isinstance(val, dict) and key in key_rewrites: - new_key = key_rewrites[key] - yaml[new_key] = yaml[key] - del yaml[key] - self.substitute_keys(val, key_rewrites) - - def getYamlLeafKeys(self, yamlData): - try: - for key in yamlData.keys(): - for k in self.getYamlLeafKeys(yamlData[key]): - yield k - yield DictItemReference(yamlData, key) - except AttributeError: - return - - def convert(self, text_value): - if self.__convert_types: - # try converting to float - try: - return float(text_value) - except ValueError: - pass - - # try converting to int - try: - return int(text_value) - except ValueError: - pass - - # try converting to bool - if text_value.lower() == "true": - return True - if text_value.lower() == "false": - return False - - #nothing else worked so fall through and return text - return text_value + def __init__(self, + source_file: launch.SomeSubstitutionsType, + param_rewrites: Dict, + root_key: Optional[launch.SomeSubstitutionsType] = None, + key_rewrites: Optional[Dict] = None, + convert_types = False) -> None: + super().__init__() + """ + Construct the substitution + + :param: source_file the original YAML file to modify + :param: param_rewrites mappings to replace + :param: root_key if provided, the contents are placed under this key + :param: key_rewrites keys of mappings to replace + :param: convert_types whether to attempt converting the string to a number or boolean + """ + + from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__param_rewrites = {} + self.__key_rewrites = {} + self.__convert_types = convert_types + self.__root_key = None + for key in param_rewrites: + self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key]) + if key_rewrites is not None: + for key in key_rewrites: + self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) + if root_key is not None: + self.__root_key = normalize_to_list_of_substitutions(root_key) + + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' + + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) + param_rewrites, keys_rewrites = self.resolve_rewrites(context) + data = yaml.safe_load(open(yaml_filename, 'r')) + self.substitute_params(data, param_rewrites) + self.substitute_keys(data, keys_rewrites) + if self.__root_key is not None: + root_key = launch.utilities.perform_substitutions(context, self.__root_key) + if root_key: + data = {root_key: data} + yaml.dump(data, rewritten_yaml) + rewritten_yaml.close() + return rewritten_yaml.name + + def resolve_rewrites(self, context): + resolved_params = {} + for key in self.__param_rewrites: + resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key]) + resolved_keys = {} + for key in self.__key_rewrites: + resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key]) + return resolved_params, resolved_keys + + def substitute_params(self, yaml, param_rewrites): + # substitute leaf-only parameters + for key in self.getYamlLeafKeys(yaml): + if key.key() in param_rewrites: + raw_value = param_rewrites[key.key()] + key.setValue(self.convert(raw_value)) + + # substitute total path parameters + yaml_paths = self.pathify(yaml) + for path in yaml_paths: + if path in param_rewrites: + # this is an absolute path (ex. 'key.keyA.keyB.val') + rewrite_val = param_rewrites[path] + yaml_keys = path.split('.') + yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val) + + + def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): + for key in yaml_key_list: + if key == yaml_key_list[-1]: + yaml[key] = rewrite_val + break + key = yaml_key_list.pop(0) + yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) + + return yaml + + def substitute_keys(self, yaml, key_rewrites): + if len(key_rewrites) != 0: + for key, val in yaml.items(): + if isinstance(val, dict) and key in key_rewrites: + new_key = key_rewrites[key] + yaml[new_key] = yaml[key] + del yaml[key] + self.substitute_keys(val, key_rewrites) + + def getYamlLeafKeys(self, yamlData): + try: + for key in yamlData.keys(): + for k in self.getYamlLeafKeys(yamlData[key]): + yield k + yield DictItemReference(yamlData, key) + except AttributeError: + return + + def pathify(self, d, p=None, paths=None, joinchar='.'): + if p is None: + paths = {} + self.pathify(d, "", paths, joinchar=joinchar) + return paths + pn = p + if p != "": + pn += '.' + if isinstance(d, dict): + for k in d: + v = d[k] + self.pathify(v, pn + k, paths, joinchar=joinchar) + elif isinstance(d, list): + for idx, e in enumerate(d): + self.pathify(e, pn + str(idx), paths, joinchar=joinchar) + else: + paths[p] = d + + def convert(self, text_value): + if self.__convert_types: + # try converting to int or float + try: + return float(text_value) if '.' in text_value else int(text_value) + except ValueError: + pass + + # try converting to bool + if text_value.lower() == "true": + return True + if text_value.lower() == "false": + return False + + # nothing else worked so fall through and return text + return text_value diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 13284395f01..dd938c4dbce 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -115,6 +115,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } try { goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_); @@ -127,6 +128,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create goal_checker. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } for (size_t i = 0; i != controller_ids_.size(); i++) { @@ -145,6 +147,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create controller. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 5235e236757..9a960dc689d 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -88,10 +88,9 @@ void VoxelLayer::onInitialize() if (publish_voxel_) { voxel_pub_ = node_->create_publisher( "voxel_grid", custom_qos); + voxel_pub_->on_activate(); } - voxel_pub_->on_activate(); - clearing_endpoints_pub_ = node_->create_publisher( "clearing_endpoints", custom_qos); diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 976d09ce263..21a96bcf8b5 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -435,7 +435,7 @@ void Costmap2D::convexFillCells( MapLocation pt; // loop though cells in the column - for (unsigned int y = min_pt.y; y < max_pt.y; ++y) { + for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) { pt.x = x; pt.y = y; polygon_cells.push_back(pt); diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 2da4b44a52c..f4a588611a9 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -160,6 +160,11 @@ class DWBLocalPlanner : public nav2_core::Controller * 3) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan * and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_ * of the robot and erases all poses before that. + * + * Additionally, shorten_transformed_plan_ determines whether we will pass the full plan all + * the way to the nav goal on to the critics or just a subset of the plan near the robot. + * True means pass just a subset. This gives DWB less discretion to decide how it gets to the + * nav goal. Instead it is encouraged to try to get on to the path generated by the global planner. */ virtual nav_2d_msgs::msg::Path2D transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose); @@ -168,6 +173,7 @@ class DWBLocalPlanner : public nav2_core::Controller double prune_distance_; bool debug_trajectory_details_; rclcpp::Duration transform_tolerance_{0, 0}; + bool shorten_transformed_plan_; /** * @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic" diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 245323db510..793e9c90f7f 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -89,6 +89,9 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node_, dwb_plugin_name_ + ".shorten_transformed_plan", + rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", rclcpp::ParameterValue(true)); @@ -107,6 +110,7 @@ void DWBLocalPlanner::configure( node_->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); + node->get_parameter(dwb_plugin_name_ + ".shorten_transformed_plan", shorten_transformed_plan_); pub_ = std::make_unique(node_, dwb_plugin_name_); pub_->on_configure(); @@ -518,18 +522,11 @@ DWBLocalPlanner::transformGlobalPlan( sq_transform_start_threshold = sq_dist_threshold; } - // Determines whether we will pass the full plan all the way to the nav goal on - // to the critics or just a subset of the plan near the robot. True means pass - // just a subset. This gives DWB less discretion to decide how it gets to the - // nav goal. Instead it is encouraged to try to get on to the path generated - // by the global planner. - bool shorten_transformed_plan = true; - // Set the maximum distance we'll include points after the part of the part of // the plan near the robot (the end of the plan). This determines the amount // of the plan passed on to the critics double sq_transform_end_threshold; - if (shorten_transformed_plan) { + if (shorten_transformed_plan_) { sq_transform_end_threshold = std::min(sq_dist_threshold, sq_prune_dist); } else { sq_transform_end_threshold = sq_dist_threshold; diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index c6a4fa9699d..a4ce84404a8 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -219,6 +219,10 @@ class NavFn */ void updateCellAstar(int n); + /** + * @brief Set up navigation potential arrays for new propagation + * @param keepit whether or not use COST_NEUTRAL + */ void setupNavFn(bool keepit = false); /** @@ -253,7 +257,13 @@ class NavFn */ int calcPath(int n, int * st = NULL); + /** + * @brief Calculate gradient at a cell + * @param n Cell number + * @return float norm + */ float gradCell(int n); /**< calculates gradient at cell , returns norm */ + float pathStep; /**< step size for following gradient */ /** display callback */ diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index e9d34279a56..66f57790b78 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -37,52 +37,100 @@ namespace nav2_navfn_planner class NavfnPlanner : public nav2_core::GlobalPlanner { public: + /** + * @brief constructor + */ NavfnPlanner(); + + /** + * @brief destructor + */ ~NavfnPlanner(); - // plugin configure + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ void configure( rclcpp_lifecycle::LifecycleNode::SharedPtr parent, std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) override; - // plugin cleanup + /** + * @brief Cleanup lifecycle node + */ void cleanup() override; - // plugin activate + /** + * @brief Activate lifecycle node + */ void activate() override; - // plugin deactivate + /** + * @brief Deactivate lifecycle node + */ void deactivate() override; - // plugin create path + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav_msgs::Path of the generated path + */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; protected: - // Compute a plan given start and goal poses, provided in global world frame. + /** + * @brief Compute a plan given start and goal poses, provided in global world frame. + * @param start Start pose + * @param goal Goal pose + * @param tolerance Relaxation constraint in x and y + * @param plan Path to be computed + * @return true if can find the path + */ bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, nav_msgs::msg::Path & plan); - // Compute the navigation function given a seed point in the world to start from + /** + * @brief Compute the navigation function given a seed point in the world to start from + * @param world_point Point in world coordinate frame + * @return true if can compute + */ bool computePotential(const geometry_msgs::msg::Point & world_point); - // Compute a plan to a goal from a potential - must call computePotential first + /** + * @brief Compute a plan to a goal from a potential - must call computePotential first + * @param goal Goal pose + * @param plan Path to be computed + * @return true if can compute a plan path + */ bool getPlanFromPotential( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan); - // Remove artifacts at the end of the path - originated from planning on a discretized world + /** + * @brief Remove artifacts at the end of the path - originated from planning on a discretized world + * @param goal Goal pose + * @param plan Computed path + */ void smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan); - // Compute the potential, or navigation cost, at a given point in the world - // - must call computePotential first + /** + * @brief Compute the potential, or navigation cost, at a given point in the world + * must call computePotential first + * @param world_point Point in world coordinate frame + * @return double point potential (navigation cost) + */ double getPointPotential(const geometry_msgs::msg::Point & world_point); // Check for a valid potential value at a given point in the world @@ -91,7 +139,12 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // bool validPointPotential(const geometry_msgs::msg::Point & world_point); // bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); - // Compute the squared distance between two points + /** + * @brief Compute the squared distance between two points + * @param p1 Point 1 + * @param p2 Point 2 + * @return double squared distance between two points + */ inline double squared_distance( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2) @@ -101,16 +154,36 @@ class NavfnPlanner : public nav2_core::GlobalPlanner return dx * dx + dy * dy; } - // Transform a point from world to map frame + /** + * @brief Transform a point from world to map frame + * @param wx double of world X coordinate + * @param wy double of world Y coordinate + * @param mx int of map X coordinate + * @param my int of map Y coordinate + * @return true if can transform + */ bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); - // Transform a point from map to world frame + /** + * @brief Transform a point from map to world frame + * @param mx double of map X coordinate + * @param my double of map Y coordinate + * @param wx double of world X coordinate + * @param wy double of world Y coordinate + */ void mapToWorld(double mx, double my, double & wx, double & wy); - // Set the corresponding cell cost to be free space + /** + * @brief Set the corresponding cell cost to be free space + * @param mx int of map X coordinate + * @param my int of map Y coordinate + */ void clearRobotCell(unsigned int mx, unsigned int my); - // Determine if a new planner object should be made + /** + * @brief Determine if a new planner object should be made + * @return true if planner object is out of date + */ bool isPlannerOutOfDate(); // Planner based on ROS1 NavFn algorithm @@ -137,6 +210,16 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Whether to use the astar planner or default dijkstras bool use_astar_; + + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; + + /** + * @brief Callback executed when a paramter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index a2b412b776e..975e5f82881 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -40,6 +40,8 @@ using namespace std::chrono_literals; using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; namespace nav2_navfn_planner { @@ -85,6 +87,16 @@ NavfnPlanner::configure( planner_ = std::make_unique( costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + + // Setup callback for changes to parameters. + parameters_client_ = std::make_shared( + node_->get_node_base_interface(), + node_->get_node_topics_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface()); + + parameter_event_sub_ = parameters_client_->on_parameter_event( + std::bind(&NavfnPlanner::on_parameter_event_callback, this, _1)); } void @@ -453,6 +465,29 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my) costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); } +void +NavfnPlanner::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".tolerance") { + tolerance_ = value.double_value; + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == name_ + ".use_astar") { + use_astar_ = value.bool_value; + } else if (name == name_ + ".allow_unknown") { + allow_unknown_ = value.bool_value; + } + } + } +} + } // namespace nav2_navfn_planner #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index b6b4615bb0a..48eede36868 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -102,6 +102,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index bb9efa571c1..26c497d4585 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -37,7 +37,7 @@ class RecoveryServer : public nav2_util::LifecycleNode RecoveryServer(); ~RecoveryServer(); - void loadRecoveryPlugins(); + bool loadRecoveryPlugins(); protected: // Implement the lifecycle interface diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index d657a9d0563..bd78ae85f85 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -88,13 +88,15 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) } } recovery_types_.resize(recovery_ids_.size()); - loadRecoveryPlugins(); + if (!loadRecoveryPlugins()) { + return nav2_util::CallbackReturn::FAILURE; + } return nav2_util::CallbackReturn::SUCCESS; } -void +bool RecoveryServer::loadRecoveryPlugins() { auto node = shared_from_this(); @@ -112,9 +114,11 @@ RecoveryServer::loadRecoveryPlugins() get_logger(), "Failed to create recovery %s of type %s." " Exception: %s", recovery_ids_[i].c_str(), recovery_types_[i].c_str(), ex.what()); - exit(-1); + return false; } } + + return true; } nav2_util::CallbackReturn diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt new file mode 100644 index 00000000000..9c87714fad7 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_regulated_pure_pursuit_controller) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_util REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2 REQUIRED) + +nav2_package() +set(CMAKE_CXX_STANDARD 17) + +include_directories( + include +) + +set(dependencies + rclcpp + geometry_msgs + nav2_costmap_2d + pluginlib + nav_msgs + nav2_util + nav2_core + tf2 +) + +set(library_name nav2_regulated_pure_pursuit_controller) + +add_library(${library_name} SHARED + src/regulated_pure_pursuit_controller.cpp) + +# prevent pluginlib from using boost +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +ament_target_dependencies(${library_name} + ${dependencies} +) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +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() + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) + +pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml) + +ament_package() + diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md new file mode 100644 index 00000000000..285797499c9 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -0,0 +1,147 @@ +# Nav2 Regulated Pure Pursuit Controller + +This is a controller (local trajectory planner) that implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. It also implements the basics behind the Adaptive Pure Pursuit algorithm to vary lookahead distances by current speed. It was developed by [Shrijit Singh](https://www.linkedin.com/in/shrijitsingh99/) and [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/) as part of the Nav2 working group. + +Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. + +This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). + +It builds on top of the ordinary pure pursuit algorithm in a number of ways. It also implements all the common variants of the pure pursuit algorithm such as adaptive pure pursuit. This controller is suitable for use on all types of robots, including differential, legged, and ackermann steering vehicles. It may also be used on omni-directional platforms, but won't be able to fully leverage the lateral movements of the base (you may consider DWB instead). + +This controller has been measured to run at well over 1 kHz on a modern intel processor. + +

+ +

+ +## Pure Pursuit Basics + +The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. + +In order to simply book-keeping, the global path is continuously pruned to the closest point to the robot (see the figure below) so that we only have to process useful path points. Then, the section of the path within the local costmap bounds is transformed to the robot frame and a lookahead point is determined using a predefined distance. + +Finally, the lookahead point will be given to the pure pursuit algorithm which finds the curvature of the path required to drive the robot to the lookahead point. This curvature is then applied to the velocity commands to allow the robot to drive. + +![Lookahead algorithm](./doc/lookahead_algorithm.png) + +## Regulated Pure Pursuit Features + +We have created a new variation on the pure pursuit algorithm that we dubb the Regulated Pure Pursuit algorithm. We combine the features of the Adaptive Pure Pursuit algorithm with rules around linear velocity with a focus on consumer, industrial, and service robot's needs. We also implement several common-sense safety mechanisms like collision detection and ensuring that commands are kinematically feasible that are missing from all other variants of pure pursuit (for some remarkable reason). + +The Regulated Pure Pursuit controller implements active collision detection. We use a parameter to set the maximum allowable time before a potential collision on the current velocity command. Using the current linear and angular velocity, we project forward in time that duration and check for collisions. Intuitively, you may think that collision checking between the robot and the lookahead point seems logical. However, if you're maneuvering in tight spaces, it makes alot of sense to only search forward a given amount of time to give the system a little leeway to get itself out. In confined spaces especially, we want to make sure that we're collision checking a reasonable amount of space for the current action being taken (e.g. if moving at 0.1 m/s, it makes no sense to look 10 meters ahead to the carrot, or 100 seconds into the future). This helps look further at higher speeds / angular rotations and closer with fine, slow motions in constrained environments so it doesn't over report collisions from valid motions near obstacles. If you set the maximum allowable to a large number, it will collision check all the way, but not exceeding, the lookahead point. We visualize the collision checking arc on the `lookahead_arc` topic. + +The regulated pure pursuit algorithm also makes use of the common variations on the pure pursuit algorithm. We implement the adaptive pure pursuit's main contribution of having velocity-scaled lookahead point distances. This helps make the controller more stable over a larger range of potential linear velocities. There are parameters for setting the lookahead gain (or lookahead time) and thresholded values for minimum and maximum. + +We also implement kinematic speed limits on the linear velocities in operations and angular velocities during pure rotations. This makes sure that the output commands are smooth, safe, and kinematically feasible. This is especially important at the beginning and end of a path tracking task, where you are ramping up to speed and slowing down to the goal. + +The final minor improvement we make is slowing on approach to the goal. Knowing that the optimal lookahead distance is `X`, we can take the difference in `X` and the actual distance of the lookahead point found to find the lookahead point error. During operations, the variation in this error should be exceptionally small and won't be triggered. However, at the end of the path, there are no more points at a lookahead distance away from the robot, so it uses the last point on the path. So as the robot approaches a target, its error will grow and the robot's velocity will be reduced proportional to this error until a minimum threshold. This is also tracked by the kinematic speed limits to ensure drivability. + +The major improvements that this work implements is the regulations on the linear velocity based on some cost functions. They were selected to remove long-standing bad behavior within the pure pursuit algorithm. Normal Pure Pursuit has an issue with overshoot and poor handling in particularly high curvature (or extremely rapidly changing curvature) environments. It is commonly known that this will cause the robot to overshoot from the path and potentially collide with the environment. These cost functions in the Regulated Pure Pursuit algorithm were also chosen based on common requirements and needs of mobile robots uses in service, commercial, and industrial use-cases; scaling by curvature creates intuitive behavior of slowing the robot when making sharp turns and slowing when its near a potential collision so that small variations don't clip obstacles. This is also really useful when working in partially observable environments (like turning in and out of aisles / hallways often) so that you slow before a sharp turn into an unknown dynamic environment to be more conservative in case something is in the way immediately requiring a stop. + +The cost functions penalize the robot's speed based on its proximity to obstacles and the curvature of the path. This is helpful to slow the robot when moving close to things in narrow spaces and scaling down the linear velocity by curvature helps to stabilize the controller over a larger range of lookahead point distances. This also has the added benefit of removing the sensitive tuning of the lookahead point / range, as the robot will track paths far better. Tuning is still required, but it is substantially easier to get reasonable behavior with minor adjustments. + +An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. + +Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. + +## Configuration + +| Parameter | Description | +|-----|----| +| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `max_linear_accel` | Acceleration for linear velocity | +| `max_linear_decel` | Deceleration for linear velocity | +| `lookahead_dist` | The lookahead distance to use to find the lookahead point | +| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | +| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | +| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | +| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | +| `transform_tolerance` | The TF transform tolerance | +| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | +| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | +| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | +| `max_allowed_time_to_collision` | The time to project a velocity command to check for collisions | +| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | +| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | +| `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | +| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. | +| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values | +| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | +| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | +| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | +| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | +| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | +| `goal_dist_tol` | XY tolerance from goal to rotate to the goal heading, if `use_rotate_to_heading` is enabled. This should match or be smaller than the `GoalChecker`'s translational goal tolerance. | + + +Example fully-described XML with default parameter values: + +``` +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + max_linear_accel: 2.5 + max_linear_decel: 2.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + use_approach_linear_velocity_scaling: true + max_allowed_time_to_collision: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + goal_dist_tol: 0.25 + cost_scaling_dist: 0.3 + cost_scaling_gain: 1.0 + inflation_cost_scaling_factor: 3.0 +``` + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | +| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | + +Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed guage. + +## Notes to users + +By default, the `use_cost_regulated_linear_velocity_scaling` is set to `false` because the generic sandbox environment we have setup is the TB3 world. This is a highly constrained environment so it overly triggers to slow the robot as everywhere has high costs. This is recommended to be set to `true` when not working in constantly high-cost spaces. + +To tune to get Adaptive Pure Pursuit behaviors, set all boolean parameters to false except `use_velocity_scaled_lookahead_dist` and make sure to tune `lookahead_time`, `min_lookahead_dist` and `max_lookahead_dist`. + +To tune to get Pure Pursuit behaviors, set all boolean parameters to false and make sure to tune `lookahead_dist`. + +Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. + +The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. diff --git a/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png b/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png new file mode 100644 index 0000000000000000000000000000000000000000..8507ddd3e03bb6846236dd10fe8f945fe807cf0a GIT binary patch literal 108322 zcmb@u1yodP`#uavcS*MaA}QS4XRl5=rvP`u)5zR&j24qpRb7oRPU zh3lpoHkRw1ur+7C8?_~UsV#x<8%(5Uuhz=1O2fc-h5zS+9FBSQDG$N>^L3B;|EIH^ zK7R`OU&{TzoJE!6{<-?^pZ{(X#@~(k&so%I;O?R~RM}$GSDG|v19kc%xR>DV zr@uR3=2?5~>RbpKW?tUX)h<8{@A5I1$sStwJXvCn>Eig){Z7=-b;8$d-m=+UK6{yl z5gqK2`rOHXhJ+k0=P8-_DizHK+bL~~<9El2`hMJf6EmzHKzwcYt zy!~##RwMr*bS_Ktc1qtZy~SDQy3n?dKkTVvx)TC%aRxz_zTIayy_y|*idHcD)DZ>eXrK!=Qg0JBVzmWt{cE;J-0 zq^hc_xjE3xY%WJJB2sK)s@m?|Z7o6~qS5hj+;9njWGt;~xD0O?nV6)XeNW=l$+FzQ z3TJO4PR!0GGqiZ}gn$;gvGFPfIazswoBM&fIzg&PNDezYJ7RJ1p4Y}>OVaj!tmfwC z`1tr%Y}{DgAdCPX3`d*u9=EeQW0c(Fl%K|LI^aL}(pIARkqd*IoBBa)WLTBuJ#6|r z?ZYpupk^ME3pf$Fh4f)PWx{EU$Cd2Na9gdXp-eNl;;}E3MDuTMeWmEc-?;3W;(ks4V z3cE3CQRkc)qZIVu4Z}@ETP^nw?+~X^RGe%49NQ%AZn9*|lD|044<{_jn~xZ1b;zTB z`HU6m^NoN`H2NS}&fm)?&E{*QRND>NazgB0cVA{KV+Oge^rzvHFkrK*Rf5&Doei^W z!y3Q#irn-JVr7BH)a&1J7G)R zaWQnhD6?%hSG$f%o1Q8rSasE6@L;Okr$rsv? ziEJr+oF11NkFiPwUqBL3kmKJty#kIy+#jg^X}TUdUhdLmqRi}NrProG5dNE3PGb0- zs>yn{8x+!)w~3r5%9s_-5^rg&#nFp8rDkT}ykP9V2USXapC5QjfXkA-)3hZ>MzJj~ zNZ3r+VWe{Tk>x=2G&LR>izhvkEM!tUIP^8W?80=7 zqr^6tJ9eU-;ha$BXuN_Sx7vFNh>PBEd08c;%W-wS`#!^te;e_)d! zbCi8|7wj`W3Mie545$smlTT!KGYC&pDyC(myW*G;ZNq=Zx!^ z&c}j?m8aiflXJEMnisg?rLDD%UYoX`e(8Nuxy2&`uduq-^5b^iS@h4>NTuGR3B8el zdm$QOM(;k3vOWP@KWvW8a}lrm+uIxKE8e(9PHbc~O99*9&;`! z_c0{KmEPFFu=H#Fe2hIl*h*^wp<8T9mQbO4&2tuq(FX$Sxy?J;>!xOJ2O}!I)mm9)cpc%B4m0YkG3*tg7Q`W8I7Bx z)Ubdp;$BbJ$?k-$^o@k4xKqz6fG%jYl%0NUvoSRo&Tm>T;DwM{>a8V_Zd?ohaoEaM z=!H9&DW!_PNoE=PfcBN<6wRDK$~JL$$@WLVnSe&u67Nas^wrON5W8AQ0fREr_51x9 z5<&v0@|QZ2taO?&P}frwvQAI7SbPwJYWYIXnaA}l-P#eJ&=XcUNP>|yCbPni)i&>Q zi+b0#TkTDLozXb;OY8#g`7L(Zr$|p2M4s0xK}_N;8W#-6XkY2dhR`4TOtEwP{mR|V z%#FRF8ei`kTRDSpOHnA$&OF0R=GOl$gtRjETfPo;cpBaA(FDc5L}o7v9t>Buc6(JEGe-Kss@4S_q}7ha}WKyk3ZXQg?@V5FG)k} z-G8P_i5m8Zdz<5jjCF)v;LTMbXQ>9wI;h49*t=B??8^dQ@ZsBMBItZR`Ukz+oH8CO zu|kl!2G-$t`3L5u#v&vAqA#WeLR>VQO2m(E3l?5BdzKe)K{$UOjh+PU%3(?P4tx*+ z`|>!RSxP%qpl#*Hnb!HH>w_gzGK%Ax7s4DyK2K7*b~8~_`{f;pv8C+MEW9X3$|q0( z`#3V<52$zH$b%)QnyJ~#oRu`Ef5aq^@J@m29Opo!zqCr%k&kJ75rC4aQo)PqioU)- z#4)R*jFxp>kiXjG4L|*%%9_swvGbpErrfNG*0KlVq@mQmLAk??U+kAd z)qA1$d$~_lGWcNaM+O00Bbj@dL+0@3qYKON08?>M`#3bqGa4uwx0_7vjGP%ZQEDPK zSd`j)YBgD5#n<<*`OcR18&S4C9gZB{jT4*SC6hrfggP$fJe-v)_Szf=6FzlvF9-oO ztaW;F$#Dk_-I+A#Y!mfwqy z!;rPH0l*%5LMMcrt|`7eE_qVTK?to531XqN^M;ZvYCJeteQZf??M?Z`7?_NhhOYIAYy1F`U=UzPFHU3&r$|Jh%HKiB_ z%Ql5+x1)eTcT8)QZIq%RJ;Jo>3iGxCsxSr%b3+D&aB-b|KsHps&Hi8 zynk^YZevT}?;X2l`dQn)M@9XY=O~M!IuF?XTsYEmp}u611U->61@98FY{j@NS?r2Y zbusyV8*{AKkc=@5FJI+xF5DF%B}Fzv_FB?w11NNxo_J~l~| z0b6z5CeA7CD>5iGeunV!KipW()-N-apzQ$m;>u9=5d7PCQhc#vx5sCBy&&bpv_?rzf1cB78z6NEvNPD z!=o9O(`i{0qpK2i`Cj0=OwSjx^r~VISN#$s2#MT@a0?Ipo$M|a@ ze;PqGuxB)1BjXswynBazo*IT1-gp16mAdD4z&}vB8wiA)=)g^**lM!pMjzE9GsIlX zFNf>hmJ(M|gBc9VOc4>OvVn~Q^?{O1wm@`AR3o@&(r|HQmiT~OlyX*JLpXB@*`BU> z+19m^h?t!ZM9ooT3&PG%QE<;ut&4o%(*=GCLjrwMqmSfE15&__8)BC?cgq94i)b~f zbxP~w{g<>{FXGev7?SxOG)>08J44+nrN6?e<7`i9+xvvB=J{B3%dI<=E|zC|J*;Qs zc}Eu!Uq_%%Cl5sI+I<*CwtwzNQrs}T=}c6?*nN`iAtAWC!wS@SlEw-Y2_qsNXQZ4@ zB5&43B=ee#w@{ZuK*O{?2=9^oNJny1zF#_hYaNahcU=gH5eA~B)zZ$A^Pi?!-+|OU zF#%>)#d?15!Am?A%iGhp6jd~#-@ZDG27w(#{TkW-D23(a9qL=!T?$`_wbWAk)0*&U>$Tmvlm%7$@sc#T)E7a_Z2xp4clZ2P@4Fw>^Pti%Tm-Se!f#Zg zx~;j3N0of9J3V@rITVlV2oGFi2b>>1CZW@pl@6n~@s_rxsyml!8N-_9Z?)fPT+IqN zd)q{ENzTuhnKnJD9UkCm7Le-$4uxwM=g?XM0=*|fdFx%eTqQy=>L<0dC$HS{!Mr*V@{ zbj6Duh`FBLg2D*0wd|^=y@`1UDW5_nNzZSuEe)?h5tu$N#s9V0g1l!|Qm^5W_NVPK z12sez8+3kht6#l$YQ3noVY@Sm>XGnqJg`C{2%Lj8E3LaISCC;9PK=K4dfxMzbQ7$l zQ3F@|YbfwP%|C~q3xd7=HF+%qbH-&*<M_+jE7b5tNLcLF|3kk_? z&gU~OEz63>HqXaP*6-O*fBj;1^l^RuR+Uu;?gsh=e?vlFU{st;2u?zz$*rK%Z86;K z^R|I>F&_bga&u<7y9OooLG3>NHdsn!Rjzxx`P!E!u3fSR!-^Nawunz1sDn`>0G-GL z9Muz(vPk=a6?YnnY}y|Lu9nolIzRs@L3V>m_f(4S7V+5hk?)dQISa7OJOHF7e=%^#5E{1~iul691 zpRoW4q3&Et!8Y=YDl0;r6={}5AY0CU#~?+bTkqj+@_9dLU%U<)4D|7>@2CKTiv3LO zc5q7Pe+&|8SoM?xuxC7OLGVbp9~+)(Ckpf%Bru8%B8$^DcA4`gNf}rsNpZ z#{~<%b}Jg}Z=u<34#CwO69bNl2W5WE=ri};tZblb?W;t7D@ zoqK(ILFEQUsc8tinKgSTvgPL+cEx^e+<)X0u$xJ`d%vU>>gKd>U-o!scJ2X(-14K; z7y=4UO)5(UE<_^nJ(yOm>L^a~Xj5F78Fa)4VKV5!13@u%#M24p0%Pr`303AJ%lkxU zHxi`pOOPcq-|XTO63^O6qTw}8;z<-7Z2RL_Y0-&ph!^83VEOgeXcfT`sIhXvYn$6C>cU>+Teb`|wrMnY{Eu@%{( zmqcO&FmXI%<=bdt+ek~g)J`-sBcsA6{c7fWWIp;a1-lP&2?rp1hO7{!HO7u1OZ+(p z4Lx+s5%xVWy5;PC)>Jm08;Tqs4At|i8h_nXL(bXrlSOCkGJ0Ug)YaduUq$(ap8xDi&?=U@F)+SfXJKfY2)OtdGeyn6d{*VUW-_evZLj9r~$ zJWS9HaP`&OpO%8Rj}-qL{pZxx&E8NmwcGF1onA<0hChYQsmL0a8nCAg*zT1}WNNe1 z6XR0+Q^j6$z@iizfaSrzJUINHFA0zJYs|~5c~0ru0Q!IK-te?$X^#tb4Z!-X$Venc z>oJ712H`sS1QIOGQkTaS{Y}*`3!b>TPcKyoM=HLkH0t#JUe$C9bt;1eF2?}(9n2$K z?OEXK<;ReD0ob*_&sLc70m9&lB=|EopnD&4_I!C@`-}d~;Yxqa(;hS0x+nexMkEGC zbRt)>zl|D?6*X0=f&)pjRqV%yP)FeusOiBa$u1rm!__vHJ^v|eDQEmU1h4gqvatmH z3lS<6+2ia+OTsff95a&{1^}e!tCr;Q&rDep(c08$Z>FR>I1;m#-%tr-d}x%L-gv^s}KVZ zH@C{FDuBqRrKOGi{E1kIDjb}^Z7l(-swm*OBKY#mW3g@oIYn~2I9wK^%Y9jX3(vina*Qw2# z`ctQqOvdj$c;V@0t|r#y{=%WU-|H3;v%_;*k)OgT?;VCNLw|n@U7EF%3Jz=u3Hg#) z4e#DQhYX=Guhtn@uIK$%D-5+hx~imWKxIf*gMUv*=q@++!zd&?5M^dy@V+bY?c2S@ z82*!_j6CPL#ktEDFbNGf zu{|>dQ;Q$ml<$l(5B7)TbogN9PpUryzm2A@EfjB4y7iXEt?8!3M9O3z9JiQ!iTj_NVdM~>yq@6*$Zo5g78fSVB`uhWessWJQ%1zK2KTnt#T!s7$5 zh96P-tqx|^lyOF(W$lOA_JQeA!YkvQ@qWK)x19(HJ9Tz&SO*k?N<5R__qW}^Mm3X{ zf_SqOj0vwe5cChWS_Gf#BliMZLojL9X6ldt*EpaE1BQ$P(J;V)4$WqPuT^lyoRamI zjJuKo3zKuT}ECeM6eZVWgKpTWeXPc(Wt%BUFj02uxK$?IabZXcNu*X#ss15@m- zt}Xxt3b_DRffOucO|W{d7Ig@E$3ZSC#`5%NyODFWX??VSlL6iVDVOl1)vup2MSW__ zoAuu;EhzE41Pj*;JNmbvM}x{m2lE}tcdl<`PS!qI4H$mb5r%Ev8+kMz?B~}2W~3ME zL_ndV8wECLsu%!yXp?0lEwh|wy#S4BM+|DdTD+Yo0FayCm>GmP+wj3D)*go_nwNfnr)in{L67=5*W8}l5sGY6m^ z0iC-B4`Y#y#N*1Jn*|Yl4u`s+`Uuvb$tEx|4TWDx)a|bOE4efuj;B}zt%DgGq65vi zQbCc(X_lQ_LVkuZx`5ohD@<^u`TS^xkKjv}&o;e}oh| zA8f2RH26jDHGlrxt$ck)2LO8Rpt{`}>Tza_#nUN*KWh7GU%s2>D{60V7q|rm^=irO zd)`_H{@+C(y390aYf_MQEJ5zV{w3I&v}<$FIzKY$-H@2j+RfC0nw9B}EKkTpX(`|P zcVbo1eBm!9(JW=?iK3Ho*vWTHHdyux(1WX0M-HEAKe#|uFR*#ZXsN|DP$T8>xJb$lQxz&&31^~(sm)THhT)LI?NE{_vFWPx9Y zm@xrX(|I1C6&ZDY`?f?$DJf9#vx`$C&G3s{^&S*pMlS$8pC;_^5P*EVvNvZ^VcDi{X!h)_gG?iL(~eYW54ucQFzVvn?5yePi^+;Mtn8qZ zFEJf&Z>aL@UP1gX_RN3*Ua;xM%Pn(MRdp}r=3kQWYGt#*0`&FufrlW|u##T}G??^$ zzfj|?8uP_d;8&S4wVUXVRb?8QnZ0-k*vif>1+fz2kA;P6B{4{IEv*57ZU89mx&yg6 zE-6#t5aE9}Ri*;SAOONX|9M3y&=B5m|GsB-szYfe^)4law$_HI#JqXBOX1Xm=VNx? z69)LT0MrieojX6I2!_nGH-JU~OugxY|B+cCXqMqO(A(dEN|*;6E79Y}kMmgIJqfJY zYp;vJsCmrq%t3i8ToLW!L)UpK+?O9LLc>%UhJc%@Ol$TOGq7un_TOEPkTUl?F7iXo zdgw&&%}s36U7Vxf0A!hvp2K0d_<=)fYH1!61dHha;@{;ZvgI-h9rVz*;zzZe&ORV{ z-_4e>|GT)J-ILAo^b}x*_(LYtpfS&YprX+=^?nJq(T4(_okd+EBYX2=gVOfTnh>({ z-3~Iqibi`rzyyZ#--1Sv9Pa4on0a%g%}d#CW6!MCX=*W-n~$%y6?9??OGobftKWxL zz($GuqZ?Nak#KnL6B;-ft1QzhT>wzQ113=K(rbTg0C~xtKSCqL%=?LCCo09)h>06$oDu<#4*K}E3m ze`}(+p;B*TJDA}xzg-76=if7nu>gMb3I`hSufoS~1)-OQ}z?iyXkI%qGgerxkw7AyZ`9fQ$ z|H6vH`5-WROtI5NUu=xz$M`9R6TAK0qK>N;9c@ifnOgu0RwI7~m>+TPR+pDWeh&I< zvscOkGqW3UcxehirVk>H-~0N2g(~b4)W=(AB)@6#)lB9PE$jFDX>)Fx19D&&(Q$|f z+1S_snC-?>>-I1L&@mW}ZGS3)lNZxzE-Rxmk^$HPfu!0>GSH0#OHyCF8(PRQNLcydF2{I6K3k z`ym@N2JG=A>mP+sXR4aQgZpi_iXAB6&wk`8pD?3c7NPco=NrZ4wMGXDaaU$$v@sCh zV0J=;6M#~ZPlWE*E@paeivXc8E4NGFpotW%74(b8~if2FTDsuD>_{zDLU#x8JGT zBYHtJ1c35NZ_gJS^2^N7^mwfeE5O?UTXID#*DdE6LQ;x~iZU{oG6(G8G2<(qJF`z> z+`iuWe6a)#)@2PmcpJ1=Rb5?UWD)$Ryx4;@v#qdi6EBBVD=EfxPtJRbR_P^M$l~rn zJaa(BOm!P~>HzMi%wF%x&?8dsl2?zk7`?5Qn0Sbtnq9ZDPhsU5%uC&e7Klt|72@LB0IRroa?M40{ul!$r9-_V~@T%;;Z~9|U{WbXD{C^Iv zs{2#t)n$LK{m%iY`v1#K^0JxQIg?JJ$3rC(o4TEJtAaUMLw2~S4iG%^a>KPRo04SF z+un@POM9uof8=DR&mJ?7mwU{AIl~}s4v!qJ08zdAzyyX71aB7);oqDkrLPYJlKP!Y zDby)A*@=?Cz?iNrjsEO#RRTF2k!{37le2#e!A(G#@}&$*uvuA_nnT&;F_NH!bzbgr8zT$Jiteh5MS!A150UjeNm&P2Ktg@m-gbt92DSECb& zd~}Ts*73ZCK?kzB=S$4=2Ss7ar;cXzF2T5OB@oNvSo%Kvi54&6lR`p;S1MK%nB4bo zQR53}Vn298i52-W41*5yCWd@&$irwgrR1G!*Cs?f7miw`FXvIsGX0ka{>|-3)Tu%7 z^_Z?LZ>exAl}|sG-Mg0dvS)4RiFns|3RjTIgaI4%OT)R~~ax4i202jSr{rcN1G<;_J=#aF{ za##Pj>?~fV;8FR+l3$h;9dQRSZs>sRl6aYUdHICn;VDyss(PY(L&nhRCkF;)y~LYO z-fP{Y-1ZKrG=hBhmCZIZ6=ooLo${=~h^O826`Q9dv$EbUnB$j*{6VtWs-N_}JSL3j zSitIWdxLeuxsI{XT>u18nm6B#UYt?tUfN4ohO*%k%4KTbO(uMbr$UZ>Yzrq9PKsLO z3c2LO)@8#`7#hZiWW&PZ!dKH1CTGLJ$N6?ZsDMLGp1VfyKq-&DJyK}I=+BI~_3k(G7dkzItYPMIgS&@RVKeRxv5;H-=tFO4V6(aYn`QT?@8-h0 zciunCnYi#j-f86H;A^jKdX4QkQ%iU+Q~S}EBy0tyE}bGc8waS92hMug%#zT<^+ovx zLtL)2fx_^T4|L9m=eGY_HEhP+4#Y=UgW|DYy;VFJObI=jw{Q5TdP?`;8T+!;|3tj! za6zPj;e{+C-y>61rOO_^zOQf~us(@IMMp+1U$Gajr)YX(M%Tj-$zj9KYh>@h?W*a$ zcPOzq?jC6c^T7pM5;-+{1r`nDo|I3d8mNnZcHl_8)7)iEu`0O^v zzScuugs@@S2g}Q=E0Kq&-Dk%jpope^i>;2GqjX1^3!6KKi(IKlmkk}%J=Y_~&@(tY zm3fXONjAQ?0}--GLuCcb?{mztFf(r>k^DS7g)@>Kbd{{9WP5>reoqEg0Ay1cTToC? zTU*OEoH>w59tDDG-FW z>Y4!h!I%|^&A9zWUTR@|QKy~GW^XQ4h3a_}2m_WDhRM?#?wOeoNR+C+T17%Kql6Kk zAVERe&cxX+ua;iPlarW-CB4lpF20MO*RN-(+u8(!{T{n5=JNEZ(ygHTkLTfV8zS9O zhg^T>>#Ma<-*c3j>gyL2hL>k%W-jZ+Brg-G9B~rKK*#TpMwR5WE_{fIiMhFe^D-pU zdeZU}0aD09R8(}Fyf8?k%jfxzoSdBDyn+HdFR#O2zrtW)+uPewQ3TJI*esh9y02PJ zXA>(JtnU;mzbhHcAsieO)QuGo5P)B`abksfXb7=;)G60LQK1gb-4$i#=jFwD8E0y? zm${q4wzaeKc^>0bevZJx+`Rjqn%X0zfxIX%s7{!XmJ zKAY|1vmUI+p~g=+UeTIu%OAkk)(T~jAg21H<|kvlq_}u$riZH&Igx8(&WM~g5pQUm zVD}*K6qE z#@EX@c6^iEPoju)HS*LO&5a% zeTn^Y3uZ%CJ{ZaQ@}`BftgN{Pq$hf8YATKYBd6qNtJGxwkM^gfG;psQMF?&W4-Y}i z3my{n?6kC~8FU~r`K;xD1G zFxjYYUC+m0msh65zh8)vNPZoIfDq%>OALI4o9vXB)L7*51Yv~u{pTIbOU;r}b7r!v zLuA7{{*H>P(gSJqbRXx=5Qv=XaR~{h5$^7@SWQh$o}PKUpK?e##ErYrm=ft@DQjEs>G!K0plL2}3&h+TXWUu8g>ytlGOf z2W>WNo0&JJ)xWmvcd%AsSDH`C#n+@dEi|qDeMr1 zcai!PE)C&NX+Hg3LTIl$`jno^WLwXB*Z{3PrDVmzBOoEMj`CpnsRTk4LHv`NETo)Q z1nZq>amWi3S%{s>is8D)^A|pNv!FFJy_kcr*?J+@(8Q0> z4_{Ug0V+9yGNaCy(Dk@?j~x=0l09Dl4RqDIp`oGDwY|McLKqCj^Ko0<^c`Ah+`VjW zbaY9FVyBK0CMP5FgxR>`^qknb2A2+Id>-@g02CN>r@o?M0VPB>{y*)5!{K$3#9KvJ z(LMW!fQ+{__4Sj^XzhWZv&Dl0&llI_;GyuA#>Njk+zHLrzKoUn{!aP+5QvkPZ$R)( z2z+`vT4>e|{j{=>t=Cxd5aO>$nY zuEWw0kiqc1gnVUXr#YGO$NSC`1kls)!36e7MSw#Mnq@2_dj4f}bTr;zedqV@ zdzbf2h~dYX=77`L+4+6phxn?K-(AXxc`3&gfjszx+Bu8In@Vv%P6r{7Q9hnHSIk?QwC zsw~7!%#gsv#KcNTm*TJH{?9@^&PJR9E@|xe?5u>GA$Fw$m6OBX(pXnFVU1?dXizwC zA_xr))fa+zs;V;dT!@0^5&-bA<&M=Sir{2je0X37(#+G8skFAT(#^}OE8@e|4mg&1 zjTA0-q=X4getsSI`26{I%EHE7RilvepE}`OE#DnQAa+VEpb>g#J`RrGb*B-{ zQX!E}F&!8|KBYA=`dw^a$d$M$g&!6@RK|Sx_$Ce^C1no2x^m6~_WRL#Sh4rRqA2Af z!&tbkk13tO@sLQmm05=&O_$YXB%UjB)-J$x$Y<#IBG?mMhA2PheE;^z(XNb?lu3E6 zi_8-QG>u}DSdcfY^4s_Czh^ZyHP=S9b#!E|eK6`Gi+6xVAg6s20(W}Nd|R5D78nU? zQ~mZ3E|FbD-Iz*A{>1PoI+$ZX7S*qn9YnyRM~_~~RcdbaP;>(wuOkTIsWqSMk{BRA5%jT5dw>qE$ZS=;CvZ@&;@O-MjZSgsUbp5+Oy=;j_)^6FPJR zTA`s4bJG$6F%5ooUs|u5DDUKG+r-6%-<+3^pWjVWcXicqHQ*!+SiPX}k&$#TDdBvl z7cb7uw~61s=p})BxVw*!kN3YVNJVlRtSL^U1FEV^t3Uc=ZB6aW@_J^{F0;&57G}Wk z7AwmZ0{U!n3*f~4{lvC4=V!;GLaizM7T(_8VUe;p9X=f4NZT0y7cWZ1T?ych&!5X0 zONG)32wLv)%)|T7sS1C;qg!WzQBhot6c{R4OUvaC`Wzs8-|x=dTcD1~y%~;t_Uu`z z4>w0-I4|SA4FE6KGH=|tv50$jo9tY$=Jvb{1TYFC5r9LqR%|xPDVwWAg8h441AcOH zlDd7wa6$#fCO66YYDW~M0K7;}4pQfmT}N!%MU+D)P^8Kqcz!aL)zooxBnl#e6#p3& zU7v~@DWN->ho8uyAH6sJV!3c;1su)fdZ)|L|uI+`&Ly5eD zH?Pss(sq&kc0}zm>q$xO9J254?=KbrTTu{VxVgD$#5r>@rqsEc$%R7)D=#bCxJ7J# ze70*=ym}KeE-vomU2UzX;9~=Wjr^k{@4e6l1!DLp*+0V*ucf|d=ixEx#GtU4;fs#X z@@bB~@Zsf1SOxY_d9bC1SJQ5wx3>W?kOsC=egITy%3S#N<*@eED=fG0h_!1G;o+TR zD>aP~NTANM5NPzISjpw#W=a6M%-`Stz=4^W`TkY^A0{A9kB=KJgM|+fQVT|TXhA66 zUL#j{QD#K6En`WXrQ zuS^Ya1|k6pTF2gzO|!K+fS_}4T(S?tcEI} z(7U+$01-!d`uN0TL!nZ68*7?mukQm*kA-`1l-h$;VXNKLs@mbxQ+BL&lVfjgc6Q-u zqg|`uZfYy@C6(Yq5pw*GR2FBQOaVO{23LFW$E6gcC+DjUK3v3lJcs{>{o}KRPg0^{ zA4N#Zv3m*OfW+(~EcNskYW%%R7Ukng-Vw0HU)E+hECc}rycnd1LI#X0SR)|p_9AkZ z+3zl;ca)8TL+`w+H5t6dUfJ&X>kg}^Ypd}CFSHU85?Bf0l^1}E7qNQ@bVnb2D-%=c zsS9m4zjvt_5s?Y^P0A>)>vwY1sM*;lBeB%;??0dp%cbBTP|oAlzm-JE{qA8BWe&$3 zd~76Y>CmiaHcd*T8hXBd_GT$wAivU030!SLs0Z)0WT)aYwzY93!4w(Z(Li6yalksZwo0N}`}$&%CYlx9mzS64voQF#4%h8R*ScljvmQ7h}onnMbv(#lJe505-Mrl+StV8N{AQ`2imYWSTuu1854 zjWjegO$=iGji%<)Q&SELGK*3fnVEr?7iRMU$cdRgpsaamzuqHc{!;e-`ZN$+AP+G% zVq@dE>hS#hPV?IKHg8S7j4N)p4GsJmZMyJi3k)X85}%L&lr{m#R4UHCGE~EIS7d}64KD8C>@BbV$a?eap*5dxHlx!a_FVJh|)b4kJrKP3)qzq(h zTY-Ur_Zk@J>7%Om&1@46FiT2G=*XISzJGt9S^9)(|!h$7m z0^Qvc^}pV~g>n>Vsjs)hGFGvD`t;!7KvPF&Xkfri4$zQ0%iZ1G4KIwmYTY7A2^0E*0h7a}PsNw?Aw z6>o9!)Gg1h3dOXWvebLRN&)W`tjMekR$=8#xuM*dmPs7lbNuDYm-vr>Mkpy@{qlCp zg>7?T&@CD$b0w34xJ+S9Z+;LZT4bD#BOKq3&uwGV=&lFEo_@{;v~f@@VPk=Vc}t?>ov}%W@aKkjEs$|b|kG7J`}XE4$*J` z3t<-N;^dTSWR;O|>cbKGy#)&0>>iCm-hf?O7CgUz5NZpf-+oLcu4dbR^4I!A5O@X! zT|`78y`RU0?*N`j0(N$B9$;^;cJV#l;!1}vN&$=X!jIX+T)p)SRd?!XYx!6$gt5P# zrFmC&QL4fzyHXB}&7U0w_EpklG&;!Fcdb^Jgp_nqBP%H>Opjo2UHCyYsiUXo!SQig z2)fIMNae0k+i+P|q_AO|m9&IJn%>&QMbJ-IMk+vknV6VVUC&HgQ~NZY6#r5l1MUqm zOtt@Pd%CxTSEnQsWjeUo6sVFO&2WR6^reG+_?Neber@gT_c!!UzRwAeN<4k?L>fD^ z?epi)B_%vG{qFle^J8--#>V!(8*HX5d@+9fxU*Srb7WOpKXyP{t^fR7<`JpzI?!W_ zwjFkz>c4f+ZCreOw!|4$uAaf?X;8c=T@S zefT!lJaSLey<>lUE;vV*>jF=a-Oc@DL&pBnEiJF_d#O%^&OWs8p{L9x4dV>B@n12C zRwI2NGb+eH&%ytG`@e6pjD35*bnL>#p)`LuK(47zqM%}2@`{?d`d@^eFGA6z zf73T?uowX9{KZFrpZUPo|IZhuL?QoOqInDw74@f-t73!WCjyWXc6XLFMc_kdRndz=7Y*DCYd;=_#ND4N8pOe)v!XfFoYsfws1`fdO7lPCIMs#PoD1i0EgP z3)1~@4&ruQyMP}AGGTG?i>rV}*aCiDVd1ylUUd)v2P_UC)YPz;fk$lc=#j_ahJGS0 z8~_uD#jA9k{(kHeTI-U3A^>N@Nf-gyrwSR)l=iF2$!QxL)CQIxh;hrzG*eOO{`QSa zN-ALVv(`t~8#ty7?)mxoICN2fl5VwsW@QB)6OfpgSoQU5e_!8BbAXt-vy|^H+b}&a zxq+!C{|SA3OhQZy*wlC?2`oz_=|`uMeYyBcCvpP)F#Md42I)?EFAd(b?r^{1`mW+o zG-rfS@=dM^`&F`Nx_k%+rQ$2hwn$uDY=Pl5)MZA-Y8KpKMHVF}^!Z1Vh20=a@M>e$ z>8!+M|0=3EyBvgj<6kW!c`&^g2&f}qG64-P0t3ktBOsz9Q_5#r4dguS0(Spyh6s$B zD}h1eL9fe3c?$?_?#?Nw={-Ie)$w|7$??Uod>%wMV1{y4s|{V_llpO zkVvXwyu5eUv`+8{;3AL9ewmH#sBpYq0Z(!GiqOgs*Q@?@I~Alc?}5NO(IyZ(3c~GX zus=zhgXFR4T~b!rk|D)ZH4u|j2GZ4Jk0x!>B>#Je81x>GEYj2ho>|H52qaevi#3Ck zd_kM;pJiq<;z>VUX6t|4UhQEGh5)KN_^tTnJsLh!Ix4Da5LXGp6q+wiHgO24(aIND zi-6g;f4kupTi~%eC@S&becTPZ!3+>D!~(_+gtWfrGb`dt0eN{E9qZkCASLvA6L3o+ zi0N}3h2$Oy#c~8CZ-bm|vr!a?CDUz-acer+C>D(cPrP7M1;GX&f7VYX!Fl?#R{Fuc zfP=N1BbFuI^B59ER$bl0xYI9JDS9A4QR>6F;b#2&YY?NPW3#nmQeLNzBO*%dwLll~ z$~UlH0nT!ATMrRy?qqM-0(R3$cyALz>d$h)Xb18I_ydDmD+?g^#kA&mvR3?}1I~E& zeuUhaf&^6SzSeSg4)<`{+Jq9_DGxF#%1*fGSXZ2N1jPoG+&YtP=92xvhjTInID4g!dQ6 zrgM;%FWQM}GlqQFDgu#UpFyswXp=%HE`7p9(nX2ZeJS@jp9jcH5}Vn2x7#;C9wr9j zisZi9lx&a*{3G7FS<1YQo1MJ2U-XOLU+)32CUXv(%>gK5iz0bJ((7~BEf*lO$5DFL zm&NZFg%6F?x{^0YfBS0iSZl|>cgM3S*r4q80f&Eq9fNZpGPE@`$Q79wuM?E+@=0sM3 zq{SsPcp}4DhtJrT2o@^s{0GUgSIM;oOd$5h?XDY#dCuAqgANFsc)0JcOy8_iD+?mA zQCf!2bVFPc`x*Wt6+j&5?p5ex_ooXQ#t@aFIK|=za}4x8OklPD5DjU$I4tje1QKo7 zCd!4MWG?L(|3ssG)4d-bTQh;X4$0smAuc!cZp9DLhZ^KG@*ay~h>;ssxjZTW8+zx$ z=x#hns;ggR5={hQLeJ)Rnc4n?Q(w(b7_!E3LX_ov8uugabup`n*y=+iRaM4_3mryz zkUAR6=`!D>P-cOsfwFsp=W>W@eHrc+2anKLVugvh%u9cx57kkPtF2%dnyj>WfdNuO zg;}=!$4;#uDd327f7)TYODb|imtbBWcT-SMZ|pKn_`hY4s9m^5SYQ&DUc0Y(z_v)8 z;hgxHVR*Ral?c?%mfa3M2Xb5}3v{EavvacbKFP|}0-mXJDgWTB1%bQuFsH;UDBjoe zZQX4K0e_;DaT^iPG46iBc%apSDO!Q#kF^1UQIuHb5@h(*JTd%#h%u zwPO!EcJiN-LIW5~T}#N?+ZN7N=u0!<>-(pl4$E-%ZmE;Y@|5Y$yChG}|9W(zVeyWh z{;Za&dNCR_on}3NoP4>U6-oFbpmUfi0Pgrx2X`YcXrvN4{l0w%C?lup$Dr6B=mUfZ zRjp#DEcyx&yc-?!fYRd?_O;72&$i`9i_s-3$#9&&4BSV$t^yQQ<= zgxJ+Qx*vjiH(~8+4O?7uKdYn%O3?2S;KOGsi(9@tu!E*t0H}!q_8B-u+{E#jkaq6U zk1)$Is?GKX7*XMGOfa90W`t_%QP2KTs`Tq8s@rYi>#B?Y1m zp4<;2;LTYA*iD}+TNlhBknkB3XBso(M5P!j*15Sk&_I9_d_~KlTkK?uKB=2KLcV3t zOY=l=c#=O?|Fh>x#-|=pXkIi@Ne44u-Uwh_TP5X6|A#g z62O>+87}ATj z-Oyc;k|hV|;$Pg4ua6WnWCb;pmtwa(^?>aHqY|uv#c9owN!VZ>^%|#d1+BY3g|7s^ zBp}+)B&TTC=A6CE#oaRXaM(FXFvh|tp$`Qcyg;g9f2-Wj5SpNcy`X)ZG2n<+%q0K4 z&y|3{`vNZEc@r*2SR?%oELUbZV_^CZ^q}YO9@RUg zl^MrMg!kq~9PLTQ6Privv!GkM_c{Pt6O@KJ%@1#{K6>NsXkt?=yA6jz$Io5O4E(zG z)ER0A4RR0xnv~vRXV|Ry?({X;+wCpy15)qrE1iZ`7%!TZ}wgb))Tc;EGSPl?9sJ3%u;Lj&_3d;$Q7%j4J4(9?zjFm1rg&_LEl?Kx^pPFNocmK7p+UN7$=Jn|c3NBy>xPg9&D8)f z62z#fz}67G;tm#6xEMM|W9`SA2ADk-u`3+xFWgAe`JvC}!FyZFROMBUdoefGuFzr0 z=;CnW9s_nKSddSFIuxsm@?#v_;9MuZ-Mwk*uiT);hr|`_U9rsX{a``P*zXP&Y$ml8 zoZHSn`&oo*M0MTft~*qB97?)466?)=i9MvO;@7Xw=OMP&1;&k`LO`8EQ-L|#gx|$M z7Cv*QVz6+b#-&RNNKd$A_)8`4SsfLMc)pEXcTW*ZJE%(4Vk=4R-;Hc5W#)|m!{zaO z$MgLvq)ZFu%31hvAubWVN%s;I$ZEp(06_YASimLTfv2W^20TVp@B-k3E>3kezeqBW zN;k>{#RSe&Ak|M|gUjvoVf#_M+K!Az>H`G8*|(a-Jw@-z#;vs26$D7~Bx+nTXEjS5 z2gU^t(VpzbZ=+SNUkq16r}bU$wDQT+6SnB+pK)gfH zldM`B8Ev~}rGc>o`kBURWO?l5d>IcHop2fOX%v#MW?dWV(#6;gl{dV=>~Kf7=~_X@ z1!xi=h%yC7y6B0!Aok|FlOx99XPb0pCnyCRKqgG}^3~}dh0q|eNK3yDyfp~RnJ&16 zA~8i(NcBG|u`;_Hg1cvr967Rx4tsd?jQRb`yV?oTwwdx5C-2U1_>kPs=4$kS^UNpp zQ|$o$`hz^zLxW1zVd9QI+?hW?*vR=_<|WO!N9{xC=`MOR588YS$L>5!njXc|YpzO> zqKz3k5<}zAfT4I85&K2F%Mee?^=1H-PV`f4nu|vTvpPqAz`?<<+`A>8Z`BLVf#s=2 zGxdZ47^Mi7-D72dAGdx74o+w#`XvCuIF0?8MBt;*t!{LjWeOq;+-{P)Oh>)a^n}N zm}P~tiIlPU0D#!r5lV<=3C6G=^mua}W6-nU>SuKqr z9|QA*S4~1n|SBh|CCx#G3MZa>)B@n@QR?r=+=kx zDIzk)dy5>_a;<&<%KY0|utwy_FahJ#CLKcJorCtJHqDA6DOYdOUz86WPE8(7&+p^b z!xq!HwOr0@b1j=IN|2AStBVu^XHl&lYxS1zc4!ic5l%qjjk_PM zG6KoY@t!+7Y!FWpy#3A90G~-%<4%OmH+645<}GpBj8AU9+OY^7U*SiU+1$V<^ISb4 z4WT*`Bp_DFYl3il0`&YCFur^}v9+x57ce5AQNvVK82pZ{-iO2x;zDeO6}N#;>GNTS z#>I)&w|~EL#RXOY56lhIng1X^1)s32yxar8p+!HgX{ckkEd6u@db zLTli$EoIT&x=yg~Xh0NY;`!ZTVEwh%*EHwO9;Xv`i1j6X23 zKyZ(+7G8Q>b@c4rZH@ww6D0VG{=x1Nrva-=p$3H0e=c;LcphX28+M; ztK+kLkc_hn@J>MM-5U_WU}%kb`2CsKlRp=jZo^ZphK5vID|?f}lS0D#jY9jvR~WDV z?QsY#0Z^_9IS4sGQbQc#IN6pCo*4L{E`Q$1!H6}WmmG2xecCzTb2_iD*lr*0D@Dq` z9!dh$+s9on%k_~+QCkDp5Flg#=yv&7V|;5u#)GA#XqQw_+t5i8;G zq2LMKRMm4!A0zBP*CsOuAw)JDE@LNNl7NQ7b37r`ULKkol%o)@fZh?DGu@yO$dZwu z3(I0NrO<*&I<)m-uc$P{R#;xtDRIet^_^Y>x7=k8i2l}Z1+UtgojFXwYXloQ0JLN{ z@Mf8nIWD`cfQ*DBS;Oi;vyHUFM6jYBG?OItv2B`4RB4?W?5$S}G+3pw-bkNhe%S8m zqH3O@lUNm+iJ1|%3rTUlp^Viu5zNTA!Mssn1^(wmva*UFpM1K|L`D{quT^lNjEiXt zX6M#1>L{dvgM_;ZL4z`(3RV=1L#%r&w|FLaaKGQ5QS-9O2nW6twKN4poxDR>$(F~P zjUl%H)S?vVCUBhUh_y^xX8s&%xT9Gy@xP9=T{yYH4QWXlty3#cFSk3O-TvjlLI!50 zDoh|r-dv=|M)u8-GwhHq0#9ikTtSsjnP%aTAT!Z2g@mP>+d2r$l(qlC3%4%-txNpB z-`;Az8wBw*C+7ri&%R^l86_OQnEe6^p#>Hj`Zh)&v-LT*+X^!c4Hp{4eUMgz1kO1{ z^*k(izd>9|UAN^&nXW2Kq@e2ywme+arUdvSd>Eh!EhU_!6&=8(u*!1u|d z7FYlA;t;r=wx$bv{fqJ{XNi#2d$1N{7QaYs}$b)`&8T)QmPcKzomuw9we zS91+uyeN}!k>(L-L9=_ja5jNrHboCdPRqom8tWF=3_v@Q#Ve|kAzU&V>0DBz`!5Ts z>4r+=o0V{!uvx%Z)4=0wtDan0EZ{K07hESEFj+$8Z9GwC8ZFQSlo@b&?JOtf2oS0*`@j!3CI73P8ZNJ06y5?|jT*#i zpgqw9PaoLHPMuTemfl!YS;A?-!<%W$&re2E6Z@xgccTVKV5g4;L$iMpIJ9RJf{$C` z4F1akW5K#ZvKtE#131q>qJ=Vby#c=K_M)cVtlm*t?;(o5FfyybblMHkZ_v@X@{3Lh zf}+rG8_ZxY{yyvX?AVnN$BDtLbZNH|lltJj5v1b$0gk=W(Zc?V7+>IHtBq&}^#!I@ z5L~R!)9ui}yqP@qg8sedAU;R438H!Aw*>)Q)*GRhhz;!{@r~uSx!v8luLl!CbyM%g z?l2|g0^j)Nq96jtrz?&l{rNCVGTI<^=gU6Dkc4;~zu9(t%Y(LkXcEgeYgNG9lkd)i zPUB4}Mda!$vBiIEV}YFSo_xfSk{PjYSg|{XIzB!gXl8q$6IaWsuQwd2mC*4uiasUALZ>CK7& z1}C<%y2y4aIF))mb-86DW!-reObYl>Q+*|_;ACVQ)tWskgdl6OB?Zh!44AeU33MG$ ze!~o35MB_NXy7D*_AGt(VuR%%Zv>kJ=g|~uii?A`z);rHvWf~|IH<@hp7-HwQ+#;l z)(6#HwXTGceA}TL`!?715S9*WdboK6&q&Ud!mc3BhD715sOY z<(lj*>CQRp>jStdjTGsl7p1F#e;P_$P(92;8o!bG5o^3=0jsyPbNBva;JyHNs8uHd z+*zPCF)wTmf=+8s3egj+P>#kR_(f|y?(jh%Bej7<19ZACf&1{24c#+Wx|7y^qQw-L z|De-c>E|Sr=St?*XV5qj5Fl7SIYLK=gML;ZVuuV(s*M0tRVJESC=)**cAJCBlx1i3 zu+I-7l5uRyfuIU9-(yR<9$k<^?ggU0dguiQXU}tOoQtIcoOf5Kk3x|_0vImJS*&ZI zzx>_F>oYhC8R;xeh>0Z~3-m|^Vw9M^Uywu?w*+Ux97xGPw}R0EBjK@{Q-2u~XAb^E z7~c@^F1LDIkP7jzF9v%`Ue3q4FSTmGNuWcPUbToIiPdk6J{NCTbe4)jkBfGi)89z* zRbbj$zn8P_|7WX~UN^q|m!c(QV)OXM=Gdkktcf|2YEQ|<=|(gRa*=|-2B8LAN3O19 zoUpA`k9Zh--ekG+6&Sp|2SOrBVUZHQUKVPXHf=gRGzJtifnI$vcK}_%aoVkY<^6lK zA|~SI(Qt%`^_2)@*lOQLJcPVZ8Pt?IXMy<=^oSLm@5mz9RI{HFZYy6e)9RlroXo)Z zvrDaY7zP~1QZ$jFUHK7^B*NiGwr>|5?q!&P?eYjEer)nUxsuF?q;L|6X-VIH*X$5dI7>zU69EgyJ*%= zdMeE=0d4%vCDHQcLObIK)-%=R?*xqNz|ICh76vH%VAyJ*)!8yeUSZ`Mi+pN z5Lqv}cpo$~RhMAV~<_=WMm)GzU4t573ZF7afV|?cEd2;;9E&hwdKAq!k(K zmc^guj%<8;;J_3xWw|q?6_Z}DC#>daoUMEn6MA*9H{)2T5kgLFJboD0G{f+ z0fhgrJofbo(_l|AS9;~E#Rrtc<{=H>I;2cb3em-Jx8w@7I0@QJM^hIFj#;uY|I8mq zDd=$$EF*AWy)X0iUrwSlxMx?eScMjqcL%VF8OXYEd?`!%odJZV=0*1720vK@O#S+-KOGBCE(^&T80o-@JL)H*z2qzI0@jMlXP5&9ZK0VMeXrVLvjE>yjl z$UdcPS|Bl#-kaJM%f~zfnSfZoYb2T8MQI4D`j;j4y1&I&F$~=Yk_o6QQMs|a;kI?F zWBf+bYWX{#s^DqCd-n)&xL^WJtkCoZ0sk&F`t_YfcOI|hPc-V`D7P0u$x4e^#L~?q z$gG^aHrstw^NY)A{g{m~+m0GoFYq4ze<`!NJoPi4>c|rIg3JzcfMdyodsfP)XUs#& z=s<_p%&xw2{5?0`fBcv{K@r@$<)=ibwzF%_&n@e>UmLmXJc;{2h;Zm%L<^s`gUmN_ zTA(sNQgh+W*O((`6d3D)+G%7tl||0vaWYOmh?A)Tl-5S3ClF|t2SFh6@gpP4{p9cK zcnTQ}zP}n><{JV~D-At4>~`}k4J{)UJ=M{8BTjk4CSUWVSa~DPlfu8_qp)ST?Nm=; z^Q+iV(D&KGlc~FO?IvAFKrQL{R#w%LK!=^o*(G1Gya9yr++|y?&6U9f4m7J4&W8BM z+#qP;SFJnlHAxiIJInjuu;^Pdh*87?cgL;o!S>)ue36q9C_ij-%oQAm@+?n^N6e(g z;1nOH+O%tOgXEr&uJ&YQS^8;Cul@ndo#XfDu$al)Iw`oDmiIvO`i%@Kq9ErB*8un? z0eh%0DwmRUVaxxqxF>ai_&j~5fXB#X50%U^yys%?o7%SP2+n9zjsAx1pgGb6**iuh zP~z#vK*wDkbV}|ZOHJJMx4woDkPT$$o$8ji`}1@sb=N}}ARTwj@4Ew-S472BUftpj z@jq~<$6mkLq)TqJ?lJ%12G&%m23&2Bz_4FturNj>#sBw87zw;qhUu{EjgF#Q(r3CJ z9Y*q9hp^9Q|3^jLKWiKRFGbyT34Esd|6)=1`TF6_6L4xTNn<946vMmolGDg#@x;k> z0;i=^(VCmVvn9E|EA{`SCkz zUL{a}DQ}LIrDpLJ4(60-5c%I$1@3SAh1vGiJ2{lfSar$2{RfyN?PAy@ghwjpgBbj~ zc>X0w#I>bKmH-+vOO)ApTap$Kqm6&?WvPHPjU`H_$o?YxnY_#GdY^9|MqBCtHLEWI zw7E+50sq=t!RUu_KyB+myt^GK=EYaG`u zfayme8Gs|6M)mXVIh+;~LGJ{JVOjBQY-@wwc5}jIn^7?Y4l4+;he*i5B`?z5AMAV| zu}*Vw5;o~i7+JgyqI!7tdu;glLW<5zQj=za&)fkV>8f;>{CA7f-6ByxiNIOF7{F`@ znN%Q3vAPvt6}*&Sc!csY&FW1pn0~`kjmsK8&q)ZYAlH7c zr{$W;IVby9cF-~ZlajHW+&;A%ncv+au0eXpHwjQ`ZK3{$TGE2*21vke?c)W>aG+Se&SRFI32{XYD;z8d4OuT027gnZbGthn;41S=g0N40hc%!3NSN^#*%lc?Ng#ZNexY0}=bgy0#G=>= z>SB*o$gB9u7Uofw5i<1MzPDul*GtE;|Gjh^Cb^}gNOvmu{78>_%4e8_9Vvp2Js;xJ zj(A8KHnW2GAramGLmk`GkGV~f|#j0+92 zv`hh2gN6nM_l{wCZu4v9eTlH0>czB+x3C!g647p(b%G=rf4$da$cJ+(z z27+lb>LqWpdynejEPx!XcL`Dj0jbJd+Ds*a79BR=(}(ln0a^#SbJa(g8h`Fg@@S#c zrqu%CZ4lym4SqM;*0L94$+GP!G~tkW8N-fywob%XHxn=LVpBF29wl;c!`3ph7r+B1KXjgACCvE&+G!4T6Ua>ii@b%-DzH!8 z14Xrv1FXZ};M$3#irt7YTmRtZ`J{W0kmT<_{{E^+$_yiRx&J-gXh4Vkjei~VFlkQ@ zh_G*8pyyV-D_3}c|HO%_o}qhhC}+PKR&m_8Oh*Jp@NWl)$*~1^?|ZIgwN?jbngq04 zXLW)fW2jG%7eAyJa`hxpLE6;#am z4aqDm#D>jA*qxZ`fux-P+P~^kgsKQpw}?o`w!cH~@W0N`^%EA5;qc?n4Yr?bn_e7m z7Cw@0q{bW#LmrwO@a7-`HC^+cwd=GtqeP)5@>|YDKsiNR_{P2rse-nwryHFBo)YAZ zCJ$gtF6pGL-ag&MnkJ*qqU+}72HwwYC!ixN=&p|+VTzfRT2vgQL_%U&0+`X*nds>s zOHM3gA-S-Akv}(okV8nLWaDbD{YCN)?f0W-B1@dQjQ;tTYNB1rPd@z_a^(|KbwOMK ziC^_aTxU@%*U#8-5`8%F`Nk&%5tk4<3-x2&j9@Wn`N z*^hNS@4VN~etM)mTIqqew!C|2ZCrAc_Mshp+l#rvzV(qs(Fjr?EB5F>dwZfL)b`uBS#>U5%3O^gV5m-ijvB)lv4cneR^zUQ%O+x-YAVGVsV{juKP z3@0Tgu-1$96zzK@xra1g{`(a-vWKY4$;Y(p>iJ7n?U%0%y-$TddcnU#{1It33w3^L z(G%9EBq#bj=iWx3GrW(TggGGZ@6axbo){1y%uG>iRckk_rSME;L;L0U%Xh>?UEk?fVIt<;aydcz2axUzXyg!eoVuT@*$aGw2^l##b}GG~ z!NYw=UV593kj{lD@XT*OujgpMNl($$=T#hjY&X;b55){?OM zV(FS=V)q3jOAqj5{XdD3BW;K$r{Nv|o`6KL!>S+np!<5l1Y2lkJ z>w^rqn`GWU>O6yJn3R$Qc~P7*pjJ)OMQJIhbHkIcs(#HdRaV(Q1)(g1+5FL$QkwR| zB-Y+octtl^W{{;F+A^Fak#62NG$5~2$mePDayKMJTH?uBQPQoq^4q|bP0&n#HKuPz zZ*)u!AK`W}`(mqQeCL~nqV1*`bnd8LQW)K&e(%l6_nzh>zt2hDO;I(ownm@5ECZ5e z;L-0l$2%uTv~xcho0GOzb;Nv2;{o>Mj{ z`knN0G-YaSIt_Y*kUpONH&gLeFrJ*8998b&#&u=PEF?lC#KqsA?ksW5XKM_(m)83J zw5hpybxhw6_u%T`L(~=-KNf#fYSBrYG@bDjTU{b1=|*cZZgu96xOaX6#&PbD<>lq| zOLKD5g%3W+Pp46+vkI#;^c`)drr$)pdqiZQ z_VoBuEbBl;ytnoZCaZSqa>mup%3=*R>B^%>Y1A`36A_WgMa9}_9h z_pe`i#dQ+|3t0-h$cOU_EhleJW^v-fPzJhw>GAcD;7B zdVB5Z(PuxgIQq8guCyQ~PU??L{Uot2$7gjl9C$RYUPTD%1t8(cubGfu0}FTUC%?~> z#}6FI+btW|sIBs&mMV32jRGaUNSBPg3xiAKPkQ;T<7EkN`SQ+#HT)_eVcJG8JQZ6= z`8qMZ`kT(tm`iK3zY3P*H6gh;V-4yo!M?Wbs$mazzvL-!4Y;PrD)pJmi)GLO|NOun zD7Y*?N~)3f@q00K*AICpg)iWy5c7H`1RYjlfWtsun<34<<=eEjDyeJ zCdYaT7hrLdxT4eL!DaY7JoPc+DA7_T=BqJLc~bgLhmc7Ah3-)iIhqg2rlAWZq;`RNjnds_hx!aVJa-iM($lrgM$Y9Hd*+f zudQ;>)yT-Gh)YDIJ#Nj%&rj%aW0UGW)g5esBGmCiv#L(;lC|HJ-8sjd@2L((=-1Dv zCwY{F#Nidtb-wB!q~Uvo+Y&{PUS6b5i!$l=gM{e12;}TyTo?X)N3F z6uFw>ce|>`0vdid$pMdJy-^Axk9^2DN2B)=hpSxqdxX-l2)Jw?e6^~orv859C0-W? z+^jG$`B(nP89#hf)PMeZ|51^U%S(9iUDj|H*$d@)Rl&i2(+Ahl8GYcb<{8PVld%aY z0tV`|d#oKfik78eBc`f*4gdZ9c$r^iWv1?DWDW1q^As;j0`^PnOuoT=NM5SlUC$7c zZCh7z${|%Qag0}2p6jvx`%AUZwjkzu@OrFh&z?WO4z${Qg@4`yVyMs)uc4q&1xQnO zC=*9Jv^}U`wYv@FqKWoQ&mP-~D1b{qGXpc+e0Jwf-HF@)DD+M&MB6vXgSTVe^jRK? zL`&ZR#PZZo7V*#)ck*F@+f3mofQM(uYhI~03G_%wE->KRJ1RL=+iM5# zg+8_8dxwNQnEIyJG2@dCuAxV}|0NsA6T!ZIawV8Q1Qcbfk5h7f}3b?n7xp+We0iG+TEwxVWp&PJx-UE=d9q`12o-7PGeH@ow*zA{eDk<{UAp#=jz#N+;r+R*ZP$ zH+Gbxcf~{XA>tEq|6QAO6kNe4P1di9Q~3iG4Au(|uY4{eLM6WPpLD?eAi&--Dskp7 zbC=4?DlJ-0y98W1Si&l3_xt1y$BYS=uh(k>!(uK=I~%|zw^&~6CZ^{g4i9LLrxS9@ z-A)K5=s|vYy!<8cA`bwTY0By9M!Vj8AlP5vNg>0fAdNjCfG#}9bm^sKXfi(QVQxL4NCPpTEj`Y6MYM@u2)6()Yqx0<{4;x zDV%t&L3vQ5OS-cnR#649m2Zbv8!e>*jnJ?MMD>)6t9Q#gQ8H1jMBv2WLEm;FRtNbD)q9%s`Ebzp;j=IA(AFldwx4dzZ z6FWLOiqU(I{ZWdsba;YyvbX)wW}zu#E{R}DJRVp?U#n#$Wk@x->}8ZTFf`=tDD?3O zdY^0Al^6R6y{)vboGq|3`V0*PPZ_@T0o^TJ0yJQn6qQIfDSq5~=LFnDJGVH~+r5Us zg6HfBeZ9LEYXs`Ascs&>1Lawd8?Ue{wyZ*c0}Y5Z12gaOyM`X$^`YP6BF5-_wn_`M z1^u7;;&0CKbd@vP^7C`&FCqZ@r@C^hch|Cr4z)u8mf=%+rVge}w5mri`Ragv0D+2Z zs-WOg7CT$AgK9Refkk8dsuP<}KG>kD^yC7Q;f&7b?fXw+o^KfrT9!k2q1+zzWH{wb zP5|P*GZ_d)TV^?+IOnb2%N2P@Y)+@0y+<(>iU7Ra+$Y(Vr@J+BQhD!MvA!@qMkk@6 z>=Ds4RAI3a`FBsGYOc7!-B_bM))_>k8GtQLHp;uGcLs>B1Cn2+!9hy4)wV687ci=6 zu!#;AyK$qo#NHs=O6BXoPQVphe=^dR()}v1_pe?&d>Aj}-?KaY)2D}3!RM@t=Wc*> zfEX6Tkp+y-oSejGxbKu|ko27PvWX-DN;~iafi5V|Nr{QAg3lln-So%~fDLkTa^`7f z2gKsKq3k$xh^;FZ#f^u9lQyXvk|pspDh)JCc5?Xg@N`s!VD#VVu)Y4Yk|8l;d1!uq z9^fucH5V&B9{=Yx0gZRhpVPLh6%yVf9>D>`7cd-+B8|3C;VPs%6t3lKqODbzU~bNw zH9j~3MBe!*Zc0<=+*lc-liluvOJwfa8#!tRe0=t4%tQ(~Eupd{kaz;_5vbfTNLra2 zvV5>S)0<{DQ?3%d;?ZrH&r}aBe+JJ#Jzsuz41Y#Kf&@6@C!X)I{*v>Rkjto;#q`-%(27^i(&uAKOgvg5=jt=qn$i$7Ybo7-+qX_u9M3o#V-xZwy~wvY;1VSfV* zv!b?f2X5}W4uJ0`Lk( z&FPr7z7E1QH#LzbU#3ykN2UEn0Y{W~)d_f;PYGZ#K5yRwGYJ{sw|#(}0a(nXL4ZHB z!5wK9^hp=5mdD~A{SiEr@mAl3YVFo67>D$}{8%_Fox8#o-g5=^rb%vVhT~X0p!P?4 zNqPLVvZzdZ3R&~?eC_q@e7@>4Cj%t9#Bk=s^h+2vT~!n zAUl#A?@!}VE_1yM`gth5yWwinxcOgb4QP3U1c~1$0334EB6nltR2`{5g??=k7HD!mo?|(DQPCFXJTK%05y~ES0qY#Jl0n zrrWEK7Y^paCVl?Q=v}_A!wU;&K470|O4kw@YCPdR~Una7rk{+Kr z050mu{0pvE=l?Vq6T$W;7+L0o&}>p63`$&a<3D@Wrg{nRnqN4e=!Tifu3r2|cSrQZ ztU{}9i3?MJLjJ*aE$JJ;DgW7RPKOUU6NUZH_5}}3fB!Qw;u5gvI$r_}gyjox(RXKd z4`8gzku1w~14p8^>vXR0B~-r=gJXt)oy(8p12Hc|F#+9+!ykOFZF$G930L+2)wsPc z<|IIE)7#`^WMn|?EP4R|wMeBG`XfTYtd=3|3!1rRO!a`Ajs|F>I{=UnE$4Ct$^%tQ zc-R_61PFp#dKVe#_Va)Uqak0>*hw}EA_E=|;D({bP$KwtYqwU#hwzzt(nF2;#V$)` zEKK&5Iw`#?{j;u$sId^;CnbKNk`fxHRBp`7=y`I`gG|`hdLD7C&)u{*uu(5AVZp}r ziXKgrUZ0g6o`<`aTK@oD|N5jJg`27M9&~;~3NAx4cY@y#FEpz2b8yLNa=bC#+jfk$ z58@7JtLp__2Dz^cBX@8cnZ@q*A$TLNc&BQALeay!`BpkZ1ta0+i#on6B6kFam{5STQ}Pfn8B2+iA@}`q!*=gq`Nf7#!Gsh$3*M zJ)Ko!M(EHyA+v|y(q~Ww);rcHXF?h0nr`uD*$EvvJrkWNloVStga1Wrvx(SPC7s{Y zxG>if{)8sTpr{;Ud%_`Pruy600sT=v1?k6g0lEdmFH!jbo;WYuK_%Ab1Q6|_{vk}Y z#0&PDaHPEloRqXQ<((Y+MY8Eg4IpQFUK~D) zrMCLcPCpA>l=saYM0=eCgEnw)2}P$Hpt+)Hfq=D-(~GHOzw0uiYafVT1F|O9u~={W zlJCL8$o))ptw0FGVg<;TlbD?ix5%FeyThF_M1Frgjf7%flOM`Y_*x&)-S^m$1Gn0p z&XmUccVPy63G4~80AwBn??pAfF_MYx76E#b!Pr28I&F0X+r17xTots937a%r8T|!- zgc-nfYI2BokgIqyP4=d?eM|KPY|wobZNX9jTiGvolHG>HCcorV`4XO{sX+B@ z@ny=JcIkeo>0klxC)CL)Akgfxh#GqE_2JW{Q$j*ZfcPiCiZ$F+*mTTg|I_*|1)@ zFRs>0(J%AjHnk9vBa(!6$}=ohhK7c#rl35BY1Umfv zNO9gI8ww={tG0WKw{&E;|&VB!wfB191rfW5Hy|bPr-g_{aRzcA_E{oVLP*9j& z;5?7rcO_l@?U3k1lef95utulLqF;Bu2QFrKV^xaC9wl)I)aNFr%i40ScCO}heGWPz z&CSV$5|pk*3hU+Qb^aM0_8XZUGR?0is}F@Jl#Q5WM5Gnlt*o+$c;s4lZp@^_I7WAP zKbGCfu@r0Si3?&z-$k_(c`s0iG6@w?>TFOKT|)Bm$#g!@)Yw8XsX#t17}JL`Xld)T9r+99>zCgg2I3 ztodvYJ8ApC4UFf3WZ7lOtGf#3PbDuHdrU6C=rG24eHhC)=}!0F#7rFe&&%z>eYL1H zvb(sKm-{G9lr#rwZRx7&*`smQXXKxZ?=7xX<9o3$lfx<7nzk>d*S$TfPm=!hb2+>WKAGLpWgnN`f z(sQGf&b@L*#A3aw+dEpqb3psPFfycHXqYq*H^!Y+fE7540=B8kj&>nCb1>{(S$VX> z8!2s;a(4){CEH@Q0zhka-m%jwju$qQKsSkzP{fyc_Y#^Lg^MN#FZLJv;7;LQfM~Hn910#t)ES;j^yU?c_Y1Q+r_j>&ao`;m8Vh&+>nCaZLi;=%c|xo2l<0MP zJt<_>v2t>9dZ>|TnGr1|jq||ogzx0CU{_`ajJBsfwoyq6Nis6a)kNLj1^>4%JS{)5 zR8-q~)$Y$Cw%i~8N2iaYFKfBff@fgeJ_eE)3k#(ucz;j9mLh>wau8eK<8vMRSm^d}HuVGWHQ7V4N9 z2@D>$G3si>xqahDAa#ZD#F7i@ei{6qwL&@c&O4Bwa?#hxbZQ$WhY=Vx|B7BJ$IlA*Wr` z7c4Tc=s#t}9K|BUJ`ote;(@JQL!o%o`e50_D9Uz(jY1=$mawPmzjY0&)a;?%Qo~eq zxxe11W?zh1Zy$N==HE5#?da$LE|4)U)I45$L(6m0`lWeg%VPJWu)7p)%3~-o(^7Q) z-uBa#f1b=CGNoLiOS^L~R8}(GXPq)&bGvhAW>eg^6~XbK=Vh|8vW?G9%V$gJ@9*DN zmu6+>fWp0w3X@)2$%wGyB@kF$_>esc+&jnisnNu~JFDTo zVhf8_)UF=AzH^D!!sX9PZH{8;tjUqE)p11UTv7jpE-f~*FH}f4f~uV9s+pa&X?bj6 zVG$^z!GL?`Hzc~>xBZ~x2Xkl7rRVs)h=?D@8Xv4lbL@Dqt5Uv1H-NAG_Z;k9gRlnN zr7<>U^6is0pZE8#9{7#b$3zxkkH^)G?VHl^njl8Se{o3EN;rb5lG+MeVY{xiQDQBp zGbl=~scqdf7}V2~9Rj1HFW`bDQ{@+U^$Sjzv5QE*XhlJ^T!sfuAumE3~$Sl zZ$a@R{NRrfvl3msVg+1stJzC>hxwX0IXvzJZl1h2BXK86Z1YZ4c)=x&6jigGXg`lcF37*1EK;oGiFK8iGAD|DG&D4Tqz>wYD|Km42d|B(q_>D=gR!j5ESAO* z`vU5s4z^$T`^)fJBYr;##papM9tkrT!QO~8&sBcP@R$ln%GV>p?kftg)KRkao0r)f z!iWBYDqUDjSPk-=SGsWvigU8~5{KFG$ZS%&7IuL=r?@HD;HmPYtMh!}Lyc$&1SXn} zWsWZQ1gSWCI*mFs+fi?C_!adgLB;ovEG*7zH>1uTd#mgSw`6QAHTdU>g%BBtLUmEn zEZe(#>?~me$Bw!}$<<63^^f-j6WzdXTvw0BE|K8j19G}F&f5nsn0UR`NC0I+uQ_%b!Vi)P3?Ar2v81ZSU+*Dp^3#X*y-=Arl1^xdxR@(K1 zoDxT6N-^5MECP%D$GUy476y=n#$WWb;`_51 zO3ty}?PD-JfYRvA4LdFWf&31;qU12)-0~c_RswVC&Mik)k*Y}?+s9>RmBf`1x35X2 zUDYdQu*uFYqy{^9CHkP$wro=e6IoF!PI~J}A5wa)yn8--w~wm!aV6nt3U_SzT`f$* zY*#!pRk<5SHMm1$tdMQHGwfbpqgqM3rT9M8X{Qadm2$bUR>b5Z2gGFjN#9uClSMzt zY-rHMtqzQDoUq=uQQ|>~rUfG?P!gllgmm@E1$NK+afWv(VRis-a4w}FdNPvUP#S`v8&^u4K)tEBTm{e_i=@HC7 zL+JhiZW_wYq5P_Ymn<6+b|a#KJi_>^q>aaZ!mYCIy9x#dOl^eTwe`2BHM}Inlc<)- zrLiEm1i{;vRjfohV7@O};>bLr?%z_YFBg#{R~-ec*fwBOe9nT1+Z#pIkzrVvZ{|gWswYw%1fRqH!3!!?D`&94@=F z1l-b_dZV#xID>}-$qf<0a@Bh@T^tD9xkv^dk9!%INlA)o!bSfd-RtQFNxfqPJ(V*H09*GT5n z?DcuYpbMxsFPi&>{$%}uIKqZHrc@-i@BL}{X2C^^>PC@lUP8X*s@0~#glTxCT?h4W zELb6-57*p_Hjv93-e}{+@7+G`C>tSvCpb+FIbaMes*yuNzpY|W{oL*p_Us1dglK(d zz5H<&eW9I*@PYt(QRh)E{haq`F3)_~x*t)p$bv;%^oHv@iRLB8^C(6Hv8+bWUrJodNGwZ(=XZFsP1@a)GeF*8lxF+88yAFyk2$2M za_hfDUCHVEqMkPJL|~yAtwCG1~Rebs9 zJQ*C#O8PCSdH6_Z;@;V#>JwcODBpm9?e*4P?1{z*oF~*zVFmu~oAIOEQ~pC`HB_jL z&_q+#ipFO!6S(83{5uysNoz$>+uoV6(Mqt|w5SU=D;mhHl(i(L4@gXS{O^mVB6!D_I+LmGEtq7HbiMDrl)4q@mGbt*=MlO~@ZTay>3c-x!NE&S# z)7HsCp(4d7jXS$N!_=HTJqRWGA^#GB&}C^DkEca@(bp;@QH$t;Qt%WXgb-cx6(glQ zIlmZf=~@)Z#$u*{Hv$(W?~Hp~ogWa{$Dl6uSJOGUwbzCP6YK6zLd5taV``?iJk2K*-{cg6{|yKOutC(5?vb`>edb%w@y z=azKBDm7l0e=itOi$Xn5DkPHO?>a%!(NcDje`vM_0%|GwR-yL6ox4$atnN~2&jlcO zJ7K-`<>xAdO%GPopL%+JKZ;c>*Arr7<9mI0@x|MfEL+no1(n+J7-e0DDvg)0uK8Rn zjh!3`6xmkGH{uUFFmZ6u!u$I~R~~T=Z@4N^LP_ixfj`?wCL0@0`S&KEX!pZ2ngUwc z;!9)=GW5B+g<{2nh+)U^Ej1P&gduszq|lbktX3id5cT{x%6i{Z26>~b{&bl z884t_yPj4EoFhoPF2BVQh?G8)tLf_doVVqJHS*k1n#O1MQjYH4kD8J$JQW1TI&mVl zQwr>zX5eZ0v{st!FNc}uYDSH)Usq?DS9}EXDW;z z3YBl`6ZT$b6~eIlw5whR=&dow)wwWHe-^(RsEpOW(pFBZSNEs?wu{pSSClqJP#DV{ zU4qp0HboIK5$z=l!+q7HW(;9Zj0L@v6H@cpRAA*FCSCanXseBZH>-x*M*)TU80jGp z^jrYrZ+&ok&ZyEitIL^s6a&KWMwz=`!{&{c@OBWy{lcgMQo7!fZXcmz?Rlqwz+YL} zX&TNcf9c}I!@KEGx7nQ~;Eh4e)-Nw_uQ(9v`}0k#-w99DFGWFrz6#wWMGsy--ivl|@a=*F+mG(VydBHS_{aZe=2zzT)cH+U921rTOZ1M0?p)4GyY&2(bKIqT zg`ej`PxJ?$_++D95jASVdhg?q#N`>~J7F1LuvPecJmWPSu^j`p=G2d;cIr9%z1X}( z@cE17EAv>B=EzrirFFwkI6SGK{)=Q?Z0f@RcG#I&v$NE97+n!cls6}1DfcbES@%K! z@ONV41AX)w+V8&GsZ)JI69-@Q*cjDjKR{tg3_WiV|bkCj}f+Po`f(Mv(qf{6~`XgJT{c>dNwv82t;c0o`Xg%vf5#3HagJJUZkY%9XlkL3jCfBUOJY9c&PkxoPerik z^^Gp~N*sM9vw(~d7R!hLm%W|1g?iXNkWS<+xp*0i!=TkFKSCg*d1UMNcez8dC0(h# z3e(tq_zC-jYbv|MMgdkT{~igV47OFUW;)z-^PvTNv%oW`20g9AdVL%ZoJljMx@C=2 zxkHfkZ!yc=_9?%;H^}VegOnzMXhcJ!0oA<5pfP0 zb-zI0$jv!Ml2cE$?f&t52PS`qxUIA;UM+y!$%JAl;x~K{zwtWtU==Dw92(tI2fa$QW zYxQ$;?(XSpxf1^uS?>W(_5Z$slbIDsw#=+DBAMA+wvd@wGLw~KMv+Zsc4S2|%Q*HH z3T4ExMP!d-{-4wL^Z#Af|N38_tIJg%=e*bJ{k)#%e(vYK&l|M4aQ}Cou(2L}{#hf* zXfbWUkiwA0|NJC#aCul5QjY&c)*%^*OD;aH9be-XPjjg5Tw# z-^zvkvY>zyOU8~t+T)qh#$osNU7vMnGDc}~%%Tpo>-5U5tQ0DeI6v9J!LsSBSSOyh z-{AE`UdH@y_>%@z2Y6m^c);zp!NFn>MILP34hX-oQF;7V{Pr`R|ISxf4h@KVLSe}gIM?$WH4*gInOc5)LyVqD#edcMcVQ{1w7%sX_)W|8vf4TO4Hp+*!KymM-K-HnU1GWaw@R5? zEPx4vxwnvMGKk-OS9`^Hy5Pg8MNpa)O!eaQTFvXKWP>(mPFEtu({n>T=`izh!^M^1mfH+L8>{ zLBC&0oCT56ecU#g!vkEs?>)NM47y@dqocP$TFk|N=PgDDjxSjPNO{orqfzQQ0CCv7 zzd-#WVms{M_m3Mn1<@X|ca9*)raHBWlCtR!r{s)3Fa~K_nn|G8B{mGgWM})qH$1H7 zExoSyrN>$}K3UQNHzN%&b^M($SXhnq$qGpa;ZM$<|876~_dN@o(vRVLU}i35>oBun5T=MOpYOcJ7%MPs zHB+FvASr##8($Z}SH}hGPyYwXWq(>G@bE4;dXC)YLE>QKIDT>=n?RFh>%W6jz`_02 zz-Kvj1gO}X1~`F8snO1XY3%j`u!eovsLREGOeJs`sJUk(|`dGmV z7q}Hzv8WVfc(AeLu!t6!`j&s4PK-{S;&@N2G^VD;nAe+>jQS7Z`CmaiMjG-^1-ppQ z|N6ZA^os&3MMTHthzm3Ug6pw1*TT~;bWxq_p43F4 z(v!hfWpv}YB-8JXWxNYP<{vNNzu0>YHpv05DVJWgXfYrY&4L8CcL3cI%PR)ObXg0~uqUW$;AIWIVu1GU-g6y>&pQ1H z*T`N$S{&1L?|Om)>%+?N?w9)H5p!)}d5MA2>4c4N&Ou%C6jWe<%&auqE{vcwN($JH z9|I6AT%q4i=t@waDPQfl<4~AV)}VbF!LOCMXX1i6Ek+wZmyWGIWj8PHh%vl$*BL4uvLJY=gPA>1y(`2DD7*k^YKlBs5qxD zrb%iXaK0TmNd0_WSr3u7>j%d_|0I4vNB3QfzHCr3D#Og2&?7@*PF@TUnf7SbS_VPY z6$9kQTDShkHf2B4g`&q>c-aVFi_>(4krAbAz814&e)0OJAgP_=m3}c6u5kgYnG3gd3DV+F(k1?iv~wv^u!k9Hb8r1m)97WeDEO%7%;$g=gE)Aw^*;7*QiDb9=gN~B7*+R(WG%Q4OI{T*aI7B3~cM{|+4p=4PH{v&dlF`e}nHR}RBIg4# z@Tzn))9Ev0%}zuKiS5>=(oEx2Sd|)N{1HKBB(qemMk(57+c5fOHOQD}>wT+(qqh_9 z@vqN;HmSV*K#Ay`Is?^D!Vd%zOUc(n9BBg3VABEmltzXrr$A}}>WnG_ zDZR)pZ=QT`6K0y2p{E&X>OC6@Qd}25Uj4d!z=b8nL31AYlejCjDVtiyY(Sd{eHSK; z^ec!fdU;CHpZ0cmMh0qko+K5Xb`f|EYt0bJ5KzsDdeuIA31$;t z9-#-Ed=93(ykvhjXGc5(LbCCK!~;n$p49ai`33mhG0-rJ7yccSN7Uuo$9%coWf~=4Bn8ys?YiR2F}+!sdpTT^U5i+2f`2t#Ci8 z>t2A^rheRqDngb+2YVt9z6B6x-(>d!9lq@$kO@}L+~kU$DY!8h@#W~&d3lHQVbe)m zvQc1%QA+Km()56u5f!gZH8M>;?AGQ{6*8a8_vgauw6HBAnH=-DMvi9VwQ8CI$=?@5zFfomYb&&nLbOj{q@u~*j%N1 zirEG^rC!k?v^ivA>J* zfk0`?!CjZW>VNf%@aNH?E}1)d>vY_Fo-c1X6i=CfK9`(pL9Z7+LAhOL@@HWh)!08Y zyb7R;Wok#>Me>cS2vOiZKubd|lkvF^Yw^6H@yeYfZ}h;O3jsbrEPABE7X<`dz3oOh zir^5hfSZn$UqyO)UyxhDmM*Ejb+Zr4Rq6XUb^}Ln%*=~J(YtSA>~Jo#s5O7+NKn(+ z=R3^(n5EZ1$b!~eru(S}{V31ETXJ zol_ERw#jBmR|3658q^wLrsCBeNcQ9Zlu7jg#XZ`48zgx{>~7^i`uxw{z?hz7V@XgRtAf?kMX|MgODXXeGFTVodv-sG#6zqs;g!dgWX z$3K0&Ak1c_IS3>b`KeB8MyNmDqw*}av(;Kj**Z@?@}j$VZoBveA0M(0FUt#v6WdU3 zzUR;J>#I-bA7Vz7zuzcoTYM(H*^I-$M_?B-cDep?9qCDuD30!)I?k!QD*o#+3tkj@ zUFL@B-!*NbuOBbcN#Dz0>k*BYtUUzkd2-IF{j2K>BpnB_XWPM(Bai7}}O9Cjuyx&7Ry?jH; z>Miv`t`@j>#VMCyyIN#d7! zu8ppDW@6Tm(q~NKbqpufbj?ZvsIpNyc+ zy*#@zWI_Bal2cA!q+(q%OfRqvhnK>o?7|FobkxIyF@Z0KX>ufKTPeNM>OrM}LeEtQ zdk&trqnERqaw*G}bgd2z20xw_tFtzQ@z>fW-v4M6`cVDtH+z!e5|Mkkz;q{bysCx^ zdd(jAj*6p3<{>JYXO|13rU*6ERHcKa@Io}pNEmYF6F9bWbutm#s`jFwMo>~p%lRqU z9NUCLP19|awjFm0vGdgv5qR@^*Y+_-4_=ML-<4Z16Q2ssfh|U(fiw8 zH0X~}N zZc4AOySksnNX9+ObKfmDr6ek8F8`NtdFdxYbo7|*#+oR-$*8mPCc_{M1A*(Mb#l&LHJLcJ7KNnQGZ{fCd*6G!{;CW#U&Xho_)SvNnQqsE zOg`$2-)jA|one>$$0_$ku|v5;XS; zqxiGJI+DcgfoAsf*}P-ZSx&5-uXbYJg0k%$UgL0zv)#WtF_`~UjGW=hkBD9Su?uLB z@e$el*3C`7qU~fss5rhqD`G!Z)P;((Eba97UljtDB9 z>M0v(3=Sbe0_&8^zvl&+c%j1fR9V;ka6IG_5V=?)Avxu4iQ>>izTGh>tV|%Ik-!RW zMf4nK5a#sLvf$$2yIaRbkpI=s6111X=}A?5WkoJ2iZlGwx&-tj2ULWa@D90uU$aLO zoF_|fUnVZ6A>O$<-92f1vBWC$Lk%ItAYVC~-HElh97*b4Uhj0GBpVM=k5|uu>j`mh zn_WD)azbr0sQo!bu?*s?k`qytZfEa{aT7758mNF$5AluuDqp+>-|lcI;xjY9iGLmu z4E~X;o83$KAUieyX4h&wZ;kfmH0x>!C#1d9W7LSN7W?*XJ3|n6c2Q8x|DjUA9Kp_e z`Meu%hHT8$3`Qll#O13w`h9f13A-#kjePTMd{)%NK7#|%v*hc_#iNdV)h0zE(shx> zQRh3X>Z5hXDs|EPSE6Yu=xD!zXG0nZHPqLGbRMDyrc%_e|8k-D>zLhn4vH4G{yO*{ zTwky6(frL=yxV`Nt)j=vfAFQyoYS!+jDVr7F^S%G#_SbPh`opqZW?#`SiVqS_~?yT z0&yv&+z9K*2>agm`vf7gZ&N6G_0z!AoNpQ3e6GTa_aXhi9Zx`8q}ldGhSkrp7KJco z3%*pH?-gDxKO5uiE120)94Mz6oT;7J(QW;i<@@+LCG@VSksN#4O`B?JZpw$~yFS|sn?x5+ z4~W4Mp!)s7G1Q^<7WXm)vbFC^*gQ59X^6XB(0c$`p{Ti%{i|2dQ{LT7mT*XvTOoS0 zK|*=)&(o?8LIQI+dF#>=Yl@xQ_J^>)ur5l>Gdad4yPc*wr?mr-Xk{?k& zhp}<7pP9^)+|H=lZ9eJ8^1OWI@A*kS?wuq;G}GaG5rNPbjVI&!Jzgw4#ATkyf|cG% zLnL`_zko<=OX-_SpGgFysid(|oWfle^DbBljnRmNle*Jd-Wea4!_f?-5jjrPO>X>C zV$maH{4e(s`6cO54FZ_EeL`B6*^XlRCQg=``sY{s{dKPZjX%QTqI>##zQL1EyzHpd zo$KWyZ&IVaaK^CqV6l;%nv~V87R!0G?1tH-8eJl`E{N^;hN~&e(OAUGPCcFZn+EN? z$EqnZoSZC`La_$(1)g_j_0gJFw8R!AoJN=IaFePy>IRx4lc~5+m8Qp@-|^ACvQ{=Zo+Jy!GU`7^JHPl@ zsU(XvCB*UhQ+uzMZ++i)R9l!Q<|bW9^}-aGX}b{kl7)yW?t&A+sYB zeb4)f7DrEqo09b|1tFK`O7h+H_s#KtKQr9c@C+t0Vq7(B$H6)3vHPjb8+&y17SVkbq+X7r1u1Sj&Zliv{IE~^E-*3Vlf(S<=}v;< z*x^#9lrO%fFz2T*N=7zR<;+P0SGezJY?#_X>J5Gd^lDyn15#4cVT zrd47pS!0N7t9(enp*?X;^c%IQt1JblxH*!tKV$Lcl_h&s*4)Ou4OG?)S!-Gk9_uP@ z*0C~9tW3Naa%Z20)(lrtlG-hHT<^S$x9oE+Yz!Ggw*qhKDKy!F2R_^Z-lf&SPF1oYf`1eRW7 z+@4>lUba~kv{UTGc1Yj!K1>+k@ip9zEELAXA_^Zt&8xTPU$g3VAwY*>Akaw5uMf1d z4ONiVx)+|XB}{b4Iz*YV6m9X5$yu{%J}X-A#prW4*k<|dUnKaQAm5s&Kx@z5%ZIwi z&r^x0?fLes^NNm#FHC&g&qj0+Dt8~mSDi;v7OrSd&Ym4c@6`y?5JT1IYJhUel(2n zUBBjq$K3zMSOZQ6W1JxCnULXuH$(20Eo!QIDvvlB6-aDM7V~*IUWJ*aj_>wj`4@b} zeAt!V*(}4zV7KT~RP>|?3p)C>^<>f8MN~yv(8%WR*PQxhy~xrbFPEi+f>%@WeaCf{(b z5Tg*nv%>A%c-yciy4ekZ3Y?A_bDySu?e2^2rnG5~7no%cBGCT+34wmfIv+8BK*8W{ z?+w(tWMe;e?3Nh}+ih=%uU8GPzCrNI9uJb8UfZuV@qWl#ACa(8Xm6iQYM(yWN(-ZT zNV&;MuxqfN!ozv~6R8$dP%1gYSi5)$klB;@n(}QrtEp zWC`I4n!GjKzU&S&l(D{r@7iT&v_D^P#rmT<7?QF(rOe|?EvJI~NsDG89Ay-HYNLC# zGv?X`iSeyd$BCy6h-(6tff>>$hqO6Sm^C~hQuPfpHCgOF08O?JkJdCfaxNBr2Qi zIN+4v`V!_~Z&`k?{1x$gEmcItnF9cTH?Rr;5s$eQ);f$$f=4O%6;` za$-GEu5F|syxrR{f!%^VTkh&g#O+P?Shs-Uqo$vV?S>pwAL87osRb zCaFUN|Mf20;xR@gk|ct<%U^4&eBoye)Vn`u(E|)GdBtaW++B{E_|59BoGx34pY9G< z4nBI2y6smx^y=s2;?u^6gcg8HZKWJ@?+^B~lIGJddG~wC*mgGHVxUl|A zse0}1ewaBxbTN9xaumP2Lik`2-YX)!Z?4`6bzz5duekqvU>sD;_?Wtp=PrzXU zpY(}_R5Up(Q^P`pG|c(7U8B=7TCh$3Tmfa{*7lTIRg}m?H~&dDTMWCKWxyP%3tq36 z+Js&*#uHPh0ig8JtJgn%>ZIc;-z=U6Xn}z01`6~x9Tx49V#(i!lC1QJGh?p!gm8*V z>Y2n^x%{_nMi5<|vsfg$cgyupY3&0;=Z_b7QBI%LKn!%^aPgAlrp)no>9eg%^n00g zJV%#$`B6@y28V_P7XFC__j4v1-`u(SZ)0F}7S3f{SrMBF#0fj$b zoKP2hc-HlCRQBqHFLSSgW+C1k;OvG)L^G{=)Zq^b3Me+tOY(O`a>sK=62Ma}$T z{UTgs)y`>q^7HIb$33s9*bNyaZErMKMN+F}+x^Nrm#`O5ebJ-BtBoWmPT^+I^izkq zvGvF_Q!5WOq;l1{_h+FG$fMOW_iw*azn8Es)Ze^WK$JJU&4c%|GGI449$bW%dr)*f zQU!3cNTEEkoJ?_#;&rCq;L&7+6dJrrS%rT;jQ=VAlHN^W(iI(L#zfpNtHmV@G%C-z zV-T-keiSdMAf{I-*9^pKIO;y(@-aYQiXaCihMQkASz0&etUuEM_73 zP<6P{!csk`v~M`4mLbRHk6th6l3r)Do+cRlon@m$2-_(?OrAE)kJu)cG9QIdYL<6k z5OtCd>#$Euf+WJpcFIt5>v*N;g_WL@8&)L_rbsrg4;t;CLp@l2Tr{8#3%>^bG%xxK zUWWJ`N2=XXvhDTaNyE7p>`zF)u9E0w&aGjzZmgIn(f8J=CN<;lZF6J*jQ!3&*h}$e z6EA~FT}Go6x=q*msKgD5V|sp+`otN_lfsCgz&KYKL>^*r_;k{-lC-v6rh%&R`eEpn zBm`kXFw{eV1?t|@zHAZiQ-?l4Mdoqnw^c(u;IjY|%egNQZPbPHi)WgjdYe&+Qq(ZWe=tQHt^e+> zICxAft$GXK|M-nJG$!-m!|)giL=q<4n@IL#LP?0HAA&z|)i-SHFA2RcH~Hkr;JVO@ zsPJyv8`@8!e@7;7_#E}n9)^MLCe`x>j9TJpJZHu;-RcN4*A~11R{*tdjoVc}zVu9F z%t}LzmZx$%!^r)tzK^e7K+$QG15@;mf8u87_fD-X5HV>28SwqUG0HgNn_{q;h`kiy zxMhyCK6UK-0+ZbFqA-db`E-w0LDl5kxr28&b|UlQesoh;-@)PO%}_*)_ej3`Scb#o5CcZ=`95!#c zL4Wc|;Z9O>C>z@R4n;6+R@-pzD+;;IMf+=Rl9BcpyA@q4T!K{j_2OpLi_*zqy$kt8 z2OTK~)bD>~K*F(bOmvl86z6ifxYYvTd4A@nJk1}`a|DqrP0C>}m}#*lTLU(}EHk~g zTiozWs|UdpP73{-=mj>ICU(0U=SZ-$&fd*hVELPVM(`cLJpwahw1>=g7Zk>|rH`S) zf?YLMsX#rUZ#13XHAi-BxU4Mr>L152f5Z>d{R?B&mC9GyVfYaakLAsLOIrRPx5wvB`yh*(Gx$ z?u(7@*_?qtIw-=DH-N?Jh|DI@Gb@QV$S zptzXaTeP?S^w8`~2h2rDpVp=Cg&jI87T)i+mPf9cHOR71r9OU8`A8y_DLpj;_Ji>e6=a=d@tG0u50Wiqf4`6 zG`qi6tcSELE%u>o0DCFiAF7lS0Hd2>clUn**?^TlduG)7h-72r@^i96txLO2hzOJ;xg$ zI3FyNT1jmYGF?1bw0^A-8VeuC*xu)1JJzkd=em3}ogx$3aKZn^o3A~&GZzg+*UH;K z0*OGkL1&s!;Vi1L4wAWhj*3yTR@ zrtBMU%KX-0VycREegQJ9AeBxj6S>2f@U$M>v@`Gtdo=u>)GA_Co#b}C(s#i>oN+O6 z!6JE|trjIJji1Z^Yzhcy{VR6+%ZnFN?>bZ8%c(wI6&Le+#+lHdp(I%sRrB=|M@ncp z*+$y>muy@QX!Op6eG`-T~@2QPz-xe#yoP>tP?$pP;$JclQ+dD^*`OLTty4 ziL4eRhJ0kANs~E`6(5l# z_ma_GVjN=36cyl2pP@^}EzUjvUyvXsbo*g!kG31kVC^#rAwMB6_Ou#R&?gZI5tN*Y zEo7K}d;C1or^&^!3s0I(o`rQLuGvCVl81ediUrMwyR-c`+x6nb?ykDfp%l5dtchWl zDPs>8l>lXW*tKPw`bC9MmipOfP!G6{s49fEsLqav;Y(PlTv zlFLeDlGX7}PS=7WenU=sc$LI6NfHjAu%FJ0YAA}+TD=7SzHRpC>h0UUEER1eYZ?^u zBtu}(YBd30=IS6SYp{%-)xVzGN^|~IA}&?P{i>Fx1KBLep67%C=1hnGqVLOYt5Q6H z_gPNhpy>28WrUI)6$<CmDTY?{%U0z7lqZ{qpk#QKf9lnV)IGMk8KEn^^Nq9Zr??@3P zCDR(n)GWMw74b>scg?T_h<~(&$0DG2L!5&el_Ok=a>KV2k@NHUsNh<0f*a4I`Gl3{ z#R{=>_zmAIsN}=7FEzrESlbkQG}M1{T)b86pr+k{mu>0a$OC3+4IJjg~BKbkkDWAoR5K~Zr;22CA0YWXTRMKVUO1*XqruVp{8g>!1N2KRuvXa{w`o% z1?9bxpDxfs(jS&q|FBvzXu`*wM#x1<$8&}?kl@~3$H>lsQ<3#A4alFo#%-zX$9$GE zZa=PEsF&StoQC}I=%FXjA|!A~Xatxkc*W8c?6EY33nYp-+CP6pMGJ0R3i!VGDvSDh zE^a#9&9htJ7NxHHiX0wj5s0$sqRWM2`x+31UXnz)O9H&=W@e+$D~mXRB%2RI2w>04B!g(oENJ#2V6llk==u?h1(OK4(ls3H}zc4 zpmS8*h92_guGfFR`oABA-ugS4_S0asmN0^Y%PBtCRC3lR2^=97IMU(ndy9Z`4nR5o zeffW%uz7=(nJnTC!fKp*_$?~~eOWqZzOlS|Mm{Q z%ga&7@HqEKT2u$$1|+_W3yc4>yWHU?y+X%^T0YArgHJ&q!F&qyBTUT}Ru15Rx?g<_ z^v^ZhvVGWdg*6VBP=ZUR=pMiS-!T}#k300~bm%tfF;h4f=|?SJQ6*%6WPlMXrslzX zE#`;*6?&&Cis6f(!Z{9l-|DM(acPOM z;DC>!`7rkvekd$%Hk}_gAu}MMc&RF-^e-yN1GD1r9l4CPD}I{V*D?!)=y@rGyG{}h zCSr&Jp_q@tk3s<^p9Kok02KB^c}UiFcth7opn61sej>UaKAx>AibMJy|0jRu>B#`x z$#Ji1?EjyGEB@PNJf}g_CeNI;9c&LW9zqv89ZGLA8NxJN(l**hB4FMp_~ct2oIrQT z-@X9j`2TL`|9(()KsK{Ji3T28!t?N0eW#)9AfZUQ+r_I^azg^?L3Vm<8Nya0>D1N> z$D8AW9Ja~`z^nK{bqE%9s?c=`x$VX>jGx*_Po5h1JH-RtHUoR;t#jQ9Xs00oiFNtZ zaTQ2snhsPgv|EP1z%BUAh)!A4+4H8i0mqZ5_y=+)KLbvar76yLq|Xx$R>1@#BWPys z!Bpn65TO%b<0b*CqJmCbIxpeq6*=aeeF?m&D-U!D(_RS;d9xaF`JZ@Lp%{*0H#6jvB=$xSmofc7Fuj!m?o8fm*g1ND7k3WIcYjT z4x0JYQX9a;c^OvUA7Nk}_8pwZK+FGnVhafT_7?>~=(JHp@nR}Wsh$x6^j+|4yMKYi zH~~T3(ernM6u`@p1z#w5`4ntFt=CNcm=v6sV}Mk4U=y}3yMT}6!Zh~TVpK53K7gRH zp0>z5lF`NH`V!q0p0|d1U21+kRqr{-m3@p}YJ|P-y2q8CgI7$?RkqFEn0TrqG5-nPvdd@y;@^*7Gd&|8V@@;A@>CvNv0JxJ$hPFir+i12^co7oo`(O6lB1mq_hSgXh%@L#XM3lx$BH( zr02J7WoGc}Q5^khSJQmg??}M74|FfDO=rN~I)(9?%kX>vTm2o#5v0K!4zPixth3#M z#{G#BP|yM|bD);`T($voT7R7AyGM`Cy{?xwLQ$eKZQ}R|uf*(0Y4sVr!j-UdE41`y zNZe@{NSW`CbE6f79HJ!ay5E-7-8agNN_RgkI_`r!Up)}mV8=QG)(b`{O^^3M8IZn< z8*6|`_!v^0+6jk}+dquj&KNOpdD97$Fmva39}QgXW6BMzr65R0BnH=r?rb1-ab z5#xFaX9^*#1!B*@$~2JjM4f?a_kbnz>&o4oej%X!YekDpe(AaXb`9z&Wk!z?r`jnI zpKY6@o&n%VLMT6%_*wY$Z51T+n9d4tVKndc9DwOXU%p!pM>6iMj7Mm|xh`{D7xv!w zdIx~|!&i)B#RC}*T zW)-^X0+{00jZJoRf}}lFg9rV+WHi z(oF*PHlVbu%0PKtxi)THcX2^Om^Y?~PIo5EOi7B}PQ>8I}%|75TJH%`g2um`#E-EZV z4`}|3CAME%_C}w9;NeydAsaPrkT=NsJ$zpwc?$%lc@GSN`bFV5F&Ekx3s*$WfV3RX{7AV*2vKEd=3#Ju52Sk89}i`?1J|Oiwfu<~TV(bJ zDj@#N2doUb%92TLHiIkqk2N-7<(EweaE`@jOMf=-?V=7lW zX5N#o{Fbh*oT3N&(9g~0?Z$h^wGO=kbM&^Wlx%lU?=iQCra4;h=CW!}&T$ zmSLZa^QY+feC|DS@Sc7aS9f7Zy8Wv_^`IS;&#riui*TA$D7W~H>TpOUkeMsU8m*|? z)pjh8G?9ZaFH1ur4 z=psqQi);jE!my;^d-rdO)Es!txabb9OUwMwxe1KHzPR}@;Iyxd^Gvj>+Y520bJ%tX z8h6S+I7elriY(S%(mQ9r$$YL$fVWUDWnpOU!%=e22`j7*L@2kWK?w84p0-{W-6Ka# z60SJRN3S3DO4$Q(y7yCJy(&TdMezN*$=t4e{3yYgNNLyc?7o)XVQ;kmTsWuy&euD} zd{Pr6)Q@vKzN=^)Xo!;p=lm8G(7?|BbIZN;B4@TSacL>kG5gV1UY<$cH(2?1Ls?L? ztyWI{Hm}Gn+IU3Ana=b z;v*wubYnqf+ArjMOYIxAwEnpZ9`)owX|0s9$si}h*!{(&{;&YH-;n?Ve@xcCvU zcgHwJ+%p5Tig9A>t|hJg)jro;=RB|8&JcPcAqMfCF6)w}b{ljhHG%{>`a*I=af&2> z&}(n-Woo&^p*axy1}58^S1P4kknw}`K2%QK6XoY+&+JTC(3$i8bLP7!6{)ks`Spph zNFd#_<7Kms(UNa{sm^J*nEtRG%f5x6Ja&YF4fVJ)UD>kQ)3h}i|M}fD6X>EQ8Ww1D z;VcK^($vJpPugetyhJ26b}JyD;M;<}T*?<7j_f9hbCAWTW^7`+I08 zW!bM9Q<2U9FwFo11hT>k!LD5C*n$rVy8{*2B%Z${=c&4NfK7*W>HN1LUBL8&NG;>E5N$@ZT@I{441PD6$K3UsaG#x@cIc^#w*i(EKZnDHI#Y zZXY~-Y;uLNr%HPd`qRaRov~y)M_(Fd7nD{GaNsjXuizKf3Fvmxl5^!t42*M{a%7Ct zhF!M+H}+~-DFt>3oU+nYD_o~zyx6=#t~1%hJ4D=G>FAGU(m~(ui#(L(mRIsyDoyI9 z&Rupo)EIaM((q=0x5s+!wf9lvO#| z`9k^L=OfU|6jyocn&?#Xf?lBDlJtpdwYoo$1Zz+c=3FKrOl+vyNoR=wrAMni>fcRV zp@AuZ!&g7?wetC!C#4g`o@94J!3*R;&RR?Agg&`A3z3J%P|JLBUfW=e_=cyGGV`gp zUV&z*$WkG6w+8|ZfmXp3@QzmwIea%`7DF+c%WfKa;Bh2ZY!*4(awp6p{%@@Vb>$tt zvJa-p%%6k|hEG;-`XX40MZ{h?)7Z}_J#b<`{2hC0@bscO^5+F8w7fsmwuw=)sA1jf zyCalln$yv))pX{K8jDtZd$4?6NXdXuhZ_@E6Z&T?^0kGN#pqS6WIcTD={caCIj&m_ zr`#*PCA8G7lSez??1@jNQ=K3zjzi8A#maq!+uB;Su>Qu1+W;$yTdg{ApdkIJnkZAo zglp3#NVw8@>ZV7hHxk)A(eB=0UKho2YNib&EuR~0J$qng?hypuQDd|!u4Dv)lPlQ8 zrOg}>ZO%r__UX4C+H*%B3Yve>(io5%asC+Vv=~ySfR;Or4l`c&kDTGL)_j3(XZ+qP z59poR1|BwNzb@`G;_ZEgR3(4AG*}qcT=J$?uRGn%K%?IzXuC1*PTk1*?Das?=q8yD z*tNVijtZ6!Cpp^cyQDb14 z%Xus&pOecX=>Dxh(01cIQ$m13PM-u*Mv>&!4sdulWt{ON)WJ{yck6*J+S%PMaH6eQ z{M+~=eo1cIk>4F7Do@@|RgNweZD$1cf`bUbeHHW9qklN024GHSqR;sxUCce6SwUD7 zho$2ory{LyCKD+?#(9G3LHzgQhRDb9d_*@jf50)a`N+ z$7Bs*hSrTrM1jh9=gab@@ixgXEMq)pz?k$f^r*oi{=zY!UG1GtoPNQ_VTX3xsw7?y z$Dw!)57n)eT~M$&c7$&k?zHttE4O51nqDBB=OK^DfWE zYhOhR^3*>OSxpcmjf`jM3lr3M^mlWCR6}iz1Xv(yjc(swT>i^jJte6?zu1TwpP100 zGkDu*`}fzED--`)jyXQ}ed7C!D%lj+g$0f&b0<;_I|}*DU&n1!Qfq03kW;;YYEmNj zBVwMIZ9(Eov%yYi8=bJPt}LX|dW*-j_URPnEXc9WxJqn@2*wM?kW(h`AxTwCe4BeF z$Rvt35e1%T{VcLDA&JwXYO^3g+}P!`@R!+tI6l4>BN4wf)X&9UTaf4xdEny*85dESPN$7{2xIUs z8T%Ayr?M*aWC!Krhrd3{n%1xK*^u6Pj|x~;ep-sbm68#c2+9Rm9@_RE$die&hcl+h*YT$An5s^EmKjoS`wIYU&8?73 zf3!#mH^5QX%#;*!O;1%H_U8&MGY(-)ojEfDIQDrC(UVr)3TcfbWhAA`I*A?r1i8pc zW;nKk0#a!2(W=QwsvKqTndifE&+mNGcnK&#Ft5Ol$x8%deuU5P4vPQzur=QWEWsuyrQ_*3 z1}aosm5ULaznE{IHq#QmjFuXp*k-iV3u(z&YZgf?Pmp^LTvH7edh?Wl#7MN<=kjK^ znwH&mcg}H4d-$Xt`s9x-#CM0}Pt^m?k5PJtieK;Q9NWUc42oaAC!kZ;>U~x9JJz|Z z)RmPj?pbU?Q=(V=w#$%9S7C1jy<5J-TuUdR=&mF(Zj(TjLHDMJ`@}?rqUDw$5)sq% z4X@^vQ+YrhIyIcKmlSr{9as#Y9iLYgON$32jnjt5`c5+vPc*MRlyrCla3EC+09F>y zS2)~f zZfMg-&*oZHU52;CI`kueyZM2A|^=>T<{y@^$&B~ z?p@KMnJ^h&IL4zk)s{I4g>G6@z)!%Isrgaki#J>zgEn{6cd}$d_un9@uDB>#8v%LE zNeq7Wgwzex3f`6<&;3xn9*oICj$Xwx{TrsAhiISgt=^Wz@!3(imwGF|b zSN_`*+joYd#j1}jzxJ2OYTe}C{N)ZHzQkXBpPR2z-mh&>X-YAsgys(2?$)3mXUo)B z&c)nHEUgDY8nxmt$e})TeF?w-yR9rC-CLodE2rvnp*>#IPSe*I0@}V?T;-GtBa#Qw zGNU-br zV5mw<+^G44?U$>1k=gP!AID@q-Su~fwy_Mm#2028BuuEHU z`2i7lLfoUeorr$JI2Q6cEPX=!iy zcPKESSQPEN6dA?bWYO~DyPfa~u%46Nb=R30@TH6!5YnrK@9yv3 zWS3oReE#+JPf;APZE#ldhe{(=G3lwLl1qpXs*M@(Wxz$aO=(gva zs27gV!P0*atxttwnYaf`(%sg`-B z_c}^d@t}`cN~jDCepxksl3Uvu!^w*`L)@rehZkfBu?6o{k~DBRw~h)77icZ7W{+fi z_cg@3Zb^vi8p9xVD?EF;*K28bE@g7Z7b@bUR(h;=ysfqMx_t-DpBP0PjaDZ_GvhaM z<+R1h(HF{i2`LDbty>Ef!oE{x7Dd_>S$}tti-B=m;)L5y@}a?H;!vl(6@Hv6%LI+3 zZK-S=uHk_Q=R=qhf1tBHW#>meDR*k=?PFJbqX`isJwUs744}$72`(!qEwK1L3b~RP zilhC3*L9S<4`nSoo&U3As=4Sk;>nc5?d}8UQEJ{L~xG6wOj$#^yP9 z@A$ILjOQ5+Q$wY6ApvyoKag~MgZah$c4RDzS%rLrQaqE)*;LB~2(;l|+(#s86o^ZcK6u4LdEw*RGzdx3&=U_sa@HnMI zr6+HI1^1idKr@M$>P;5huY%Mp<8+q%T+5nVHSRDIX%Y8IPAp^fP~0 z7IpyGms%%T`6NjX8ABu3N?S3efh#r4VsuW!j|vJ&8mV=JTj!64?#=|VYV^JdI=Rkk z1IWD}oIfD$NCF@pT9@(rVmFflaN4Jt{570_vU1h;~up z^lFVss1J35sPmV_t4$)WjZ(SqG@JnZXRe9XtjwF4;#ZRLUUgVWD;DR0ZReeATi;;j z|3lSRKtcL~x+2-1RrboWpODJ9a~T_U0&LxX@c2#7E=QV!kmo$+~| z_rJci6qjppVeY=??z7L{zmS=uzBom=S~gyztiCE$@g&kpdH_qJO?qYpG!aCPN_3-S ze2YNGnu7!2m>0GG;2~i|Bpvj=zm|*|!Fj!uHv|lJlb^sC!94P23OJya)Wp_tgN~`N zs9EH@uGg2a#6#zdNlwKXRY@8QVisQ+0eSKS> z&FK*n+Z0O>f-K;EC3>B!{pK!yfaOEWzl3?(FtzDWm(2D60J7Aa;Curu<^MWr?exXc z2}}gkwQ?jt&-(fROh2l5DB=S)dIJ4Mov0L6Kt!4$_%2xx-_)$&#&?HE8{(CaTyl`olk&-Z;!V?E=Ac8Fp(w zL9xQDCBTHeMR*#!ZTt~>>vm%rWlX&)6;F*EmmI6X<^puSKd8tpe*0cC{A)T+3=qOH ztpsS?eRro2`EoztT>Tn-A^07g}x0l)Oy0`dchUXsCMJ zf;1}}A2bS~d8VD1gTl@~gp@7 zc+Q|<2YBd;Z7i?ZxAP?QDGV~P74(mW-mzs&YQQW+@3ZX%^>#+7JzzQVVxFucmr`Pi z+6_~(K(~D-y`E-MRf0D;6RzNTeoW`_jTyU<>ikbk>UAf;h4zD|TFEN}tWnrS(W+)P$9#iZ$1|D_^`Kw=>}yRRZk~fS+WrfEpQ7@dN7}Lyvcn^#tE>cl_sG2Owj90UL7wo|-+R_7b z`SD6WJj>1TU5OPN_U6W?6R-Q}L>K7a35=2sQaZ*ZyEOP(4ZLOOd=#ZUD0jdJEo%vI zYnn7*Ks{twzJfvDk_}^VK@lEqc?Phm!=|ANf^+db$Afg$J}7|`buuxguC0yNX8pb+ z0BZrF?A%60U*MHTAtod8XQ8I_C)YI~ybl*Y5-9k#Kt~|H7^*kuD-L$k?66saia$Gm zjOIB(?D^(HIq1m8>+*Ar+_TEbl0l?K+^xvF;%y+8hNUPEHsBj;r4iXM_txe|~ayq{ph0*W*Sys99KH#D%B5 z2cRR^UMFr(zi$CU7Rnn1oIT)Rq!yb7)S&=&M4_MALXc9gqsc$VLy^@;4;xJ_MoF$;C>xk#7h4%UV0U+<;P|RZQ)`I~qWfJ$3 z*?lBBnBuaFSe34WxbpGdtck7F_`|K*^z>Zv@t9Y0^B#jd=Tcejz0{z4YwJg(qT)96 z$tHB-HM}%Yw~K&WuoftTD&KAPL)BUM9F{Zki8}Xw|RQ% zycP5=S^?WMR7FW^i&4o|_Q^;8koCjpoUaNmRH&KHYjBK+BTKLa=*bu^_@df>6r1cw z=Yq|Ppn>2Ou~PF|_s7_ykDtnv|2=_<4zyFyM<~$iagVe-wmx_H-nd_}DeSr+t&15q zeq132>CFwp1sSO3Rs)QD-(l}f4qFe*9j0_kvF3qM$e3n}$txSB{#j5Z=ODgg%Rd#T zR$Y36)oB*6S!{URy!+sAJl%8a@#}e?x!BibMUbOLw^J~cDaEgKfb*U+Fw}PpML>%0 z#>(rMQiT&S&prwM`OzLmIiPr7J!(~Iuegh#S-5VG+o|Q^H#l6yKL7|5t*wt}RgvPF zoS4i`>umT_c;r=l()*h{V1py}^pY@=G;lqq&~`Rr0fe6GL_}}fGcrRkif)Q!isuWC zPepuEOqX;qqAJ4dr}a@BdZtuM0^Ld|{E;_OPMz7d~ zw79|pSp3cTKdw}4lr2}Sf&79Qw=>W&39_!{K?FmsS*?9O<`H|GQpLHC6 z=cFRNHt{m+u0Nqz_sPSFPR0_}GbZn++_cYe#(HCZI!0G!5!CGNt7y?pafUnQW5U0Mw$udPzV9ZnP|$uLa4^qr`#lZJVG|k? z;hiT9!9JzH%k6Hg0lvzh1(OP1xWHm^ckZLODK4e^FD; zBNEd3h+#x@&f~NNn0&uJJRbyOw0X@FTHJ`qlIf{zi@~pJpbx%EYzY&OFNNWJiX+eN`q%C~6oPyCAvSi%^_bkOUMwgV(y-zjiDH z_*Leb9h>HxV;cWdL^)UQx|Gh>eV%5>p!ID@(l0(4Zf((8`6JxWcnt5`bK~Yld|U~F zAD)YgL$Y!M;I94*`}CowY_|`jpg3xic&kqcZzbhP?8~$j|CMqD zGLf-HH?Mj_ZeKx%md$lt2L}gREk<%%k(!d=ud|St zSZXvd;wiiLZAJ3|gY( z&iYPLtwxow=!mJbr{11Hh4!yorU&bf|LRPF8wM_424uvAl=Tx!X;~L_bZU)w z5sdI#e&qM4%w&qW!h+zBA3sI~|Gg=lnwk=R_VnrB3nmhz7hkkE5(XPfz;0dkVyK=X zby0m03tEUdV6OxIgD|R{zO?GPy1KIB$A0m~ak{418f=@70_(tc8LXnH&Rt){944;> z-eyWY@D|7+y(G(j9|xce zHGTe^77dQi)wQ*%y9x{rIgXqMZwp&t0b{jh3n2lB!2gA*+xIhIQ**}ANsx>Z`PYzxZ2j?>O0lBk)(ti$NzoEb>RLCjxMiX$fjjvsP~*7ZWJ;zjfjx@_l{YF_+3?k z2RYH#_IBNhp9+|+$~k|Fr$bo|1X8e(h{bCZEEpUXBt?UIV5Yh{a|)trAOQUya`|d4*hxUzK2!}UO$~n7`o@O7 z``O;o3$ky8AP}A($F`fclLrHDpod%bi)v^Nvi3-=Q4oi65RWma8yJrmWQn`8&|njBNRykaMU=imGz zL5ulHp=mBVD{IF~67-7jUW_?cg86hj=1b{$ANZG_fd^`9XTckrwKg%#uz$6I-M~1; z0611WKni?w9HJ6p23J8!S7ZMUe!GB+x87;l} zB2~=QawFX0{LtE&g9zv$Q^JYi`RCoI;l%JBUr29XpT7G_Z)_;$z=YO=9Iuaqa7!7u zCN$*qk24dvQC^tXXfl^kGi^$0YGE*VNE7o3A-drAk;55f0w(9(0MfRMdcSo5aSuiq za|d!p80g#Hp-hG8c6CmM7BPFDDsm`f9kUAa`=rQ+%D4@j+8>LG z@oiQ%MHs>pPABI*Z`x}2q~}g<;!khVeOh>ITP+;9k1m4*o}_Gsysm4u?o@YdAZrsH zTLTacsXMu@z9WaftV!jet}m}+2iFQhP^uqm)O0o#{1~y~PpF)AdYpSku;?bfGqmY7 zE|;!d>LJ1qAB9Mk%U#G^*{2fYTMpD(k%BBjWe4#u9Z~J}>7RP9sTeysC&bIaAM~B7 zGXAMh0?WDGr-fX9a=!z=nrD!a9W-*SKB5V6NlhylU{ECo+TDHSVv-xMu0v$ybOmTMlw$P6%X_xBr zrZyOJi>EL}0%>t4QO0;IEeRHf29s(yLmM11?;@7?Ff?Ah_~oTyL!P5(CQr7u(pUnF zcnBUi8Qx|YnS?lpGAzaPuq3N1fHJdeu1>i@r^DVkbhP#K^7=U78Y-(#hkZmX$Ev3pMZi+60u9FTLAQ{`YIi=_hKJJL1k0kx74iWln z$C4XU1h5M;(!0#;pu`yBt)1T1D?f^8Z3sNe3;ff!lYpkN zB>wnoxZBC>Qn!1WO@u!O0gPY6wI5L$vTb=Gd^()otly;S88}>;oZPBEF`;W7bYWBq zYXFCl>vgfVbl(MkYUVFhX8M?#e|~@?LTaEiC2Ww%4FJW2yxhW~Ycy)6RA`;OfcNs2 ze7EH?rt6orl(W-!n0jn9Ejb}M`a2u%GVS9nbEhXjh+2Z8S4PEr%T*}AcfJ!y4)pJKK|}zO^l$=z zJa2)KEEKfo1=q!)zF*90w24=O>2d*e`qx3o=u%5yY05oGH)%;ppJamFsM9}AG%&gA z4jHZ6nn4hM=mtIQg;~N&mqq}!q)Z^r%6}Yezt>7Zb8wRL*@nO4j3@MAlXKqvyj-hj z>Ni0CWBe>1aG#_^86>^fL&Ea1f#haFqpqs1#ueNI5B^0D2{g63mxhXq9MGx5OmN`& zau@@2!Y>l`y_>s1o1<7RBP2-Z7>vHrAB9ySC0!EC%2?URk@KpLc(85nr#}=}yJ}9e zw?O0WIo8{oxAOh0s&Q99qxEoP6&&~}#Rj`^Lj1uUGx_GnrHP7zelJqf;zzshXSXXa zfXL;76|_wAK-<9ZDa9gSbM&F6)98nvd)J`Px&UTgN~|aY=>h!_h+p&HkHaOc)#IDm z3W`N-Sd>3K8j!EJ9GUc9^1U0Qmj^SOrE6FM7YL17~MN z`mvmH+zfKLp_^%1)0tv`VVzkDH2-4Ob*sAiB@Y(N&6|n|Lmz#9{kF^ZRGY`rYcZqg z!@%z)-x6VbcLf#D`~&bS{8)gc565^a5yMfth(5N z6t!P5a2A^;M~bcd2nL#QWnK$(VRMYg^7Od{aQ0DTO{9|nVr(fLr^pr`I7|@qTW_KE zP6nK53v#b!UlYbE2Ooh(K|UZn1bL!g;THfflrD2B^SZqW8eFX@9v^UlRN^>inwg=w z0ydbQ516$;>xC_q?@&|Fu!PH;3DJwLLN=c%P=v8}4a}FLaz6yWY^$ST%DdRR1}*eE z{TIW&u&ziW!?R&^T>5n9j%VexiD>XAE)RyiUlbwrNP8t;by9juJ;y70>s?QcE50~( z_XcZH@oYFPPbrpJ{7h&>dSW;1#jV{|q?fl<5P#!@wiG!W$v{wIt#{~HY8f;P8c|Qx zV7w2cbvd@}=wL0r4(A1EeT9WKMw_E0BhB^pXE)=jMN>!t{JxQ)$l_%b9BMfL9jJQ_ zSql69ZHu1YN`9YM0K(yvA&@};SNm$H?l++&Ci1JE*7fV7G0x=QcpsudG5B>q(AU?6 zioPovfl7;Ft9NfbWx7(bZeWd+IvB! z5q#Vtb@$T21i(KUn)=LccmqI9M;6OlACbhG;=|i6wz#xWJ8zY_a$ETvhO_SY5(i{3 ztf!c|^y^H}N)WzP6wg8u0Ro!c99UwTCC-L}t!+q@&&ZZ8j)B~uz@M=grZ&x&6oH*p zq1u8`%!W_U$^+)C6}r&nui94I8icMKyH3|@K(-wAfm$V2059`CA^d*amCx=gZd^SA zCF?)_oXDqzPS1Ymd=u_Y2$!fTDACSVzo0Di)e4ZBp#9b3-hs7kd&bKulr(Er!qoXa z0{xueC4x7#%QyAVnqex)<<_%m%|ZF2`a|WSK|%R~J3kxr;ui@s6)B#SX3@!{OFxN` z1JM54yOMX6tprK}#7PE-_DdP*b)GMj1E1}?iaspN7UbB|*fc5TK2H|>Bbgs63=A-B zUoO-1ur8YzY|6H7Fp_45BWgn=yRamIdE&mOrz`^F3p={~8lO4i2kDHJ%0MSPRgDM) z_TbLhm`ls<4>vfr5gspTy}`#1%-eOQP58x-QMId<7E#t}iwRd|wg-})MzTG%W!3!e z%oLTh^c2lg>6HDvyc`J?hXpCVqijm6hRbux@O(DOVjN~pqWHx_vMxr9*jcZhkROqZGfUOKGucq|xZYrlHi8F#Le3reM;q#u&;)3{nhKfT? zfG+f#MJum~u;Q(o!O!tJTP3^fZ*zi&Becx;-XMNC_MpW`pfPlqPh1^`6{q~+9G{ue z3lHJIQ0Py}u>D@kul}2P@pvSJ|BX(T?nIjX4pV`K0mv92-J0``I`52pw?D0K9jQyv zPvld1QNuUGImDJ1r^^DpzsN347v+sdsA^5TeyAT^L41R$|Li?cW1Zp`naBGFMv^AA_Z}$|iJ-uzB=D74(&X=^hdPMp zCXL~(q0iW})g~HZaj@?4in2YQvZlt_OFYNHAWCyd#fPiOGQFO7`_Lvc%XqEYV91#s znz|RSIUW%EB||~^Za<^OR~WUA{Iet~;gIe~Vu~Ct6c`sP7hU?)LU;HpH|K+2vAs^} zJjuVa($jT*GQQ3IAvla}Ya|Mtb_+;+K*2mhe_&C&z%6cpkWImzK})5H*X?Xl-^<3~ zr@ts8vlfoqoQZVZpU>&^b&=6(?7j2-Ti-Og{~N?T{31!m%2U<}yk4uR!9DZt^s4E! zaT?8}-)NOf5CT0r3!&!XqOURnZpqe4w z!Lm|DWc-uIStObbWZ4)&t0F~y;xvx5YIh#i33fFJNs+-> zoaZB=*&c~=i8tSlGwcy-t(Xn;oI`>_2KUZ}Y45q%eARQ0O{)q5WAL}VcY`q!dA~e- zjR0lcRC!@d)C{ME25sl*RoG;(8-LZ)nI5<)9z6YS9i#Ct%M-Miu%2+73?52l>4|L~ zO;x<8`gmKO~_DJ|JYHIKotGl{c{80wFHZ( z?Hif7Kj^Z`Ku-r4xUXlfKUP1GxB*rF8S&+ARcnH!^5kBY&a4_Xw|p2yxI2~LhlBSI z9Oz1TE#<#z$fJ0C)xRtgPACSut8DB3F&$4brO4EcGVL3ZJ^gAUT=KBdyn3m2!r6WNbRyxD2C@Q-Py4ITXjS|$|Lnto!9#-*4Tn*p*_0rR-mJ= zmHSR20d0DtpmpWV#+|g-GI{6m<6)(aD-I5>0;T6TXyqs!>7p(&(7rIy+pwPYcP~gD zH0c~UDpo~J!@bY`co<$ipIPhdmOimtn+#H+{^8LrV3OOgv}E(VkQh_VjRLB(CsC~O z*y*j2(@-hZAUZMi5x$m@S3u~yhHOmumzP_^mc>f6mz@bEOP6A|JNwZTseQD(sizyD zgXic?QCYtH74NJ@&f)7SQdL2PUIOpWOKh99OHySI$E!Of{wmO9z5F9^z;vXBwgEk> zm*2{6pSYr;C3c#LLYt=9Xe`>>Pf2QBF`<3xuqC61=r|AQT}?3H>Fsq*pMVdJ!^-8v z$8HjhvRge6A#b)du4YJT#TT#?eeeyH_xo$68u;%%&56omkJvhYsmR^4o_J8s!EslavpKk0 zaBsfPjw)c{_HrYHruK!qe~8bQuX427^t#9IYmp@3^u22^W1g@N9;abx>Pwno%-DdzW)T@H{K`>Nc}}fUQE>nKj^3c;FVaE z>Vv&(GxiG4?K?P1T3x>0@`|C}H#2T=-Ju9@F1lE>DsvKniZK{d?`3Ce5_*45MwTo$ zf;%3DjIg;M9^B}-ERAMjA4e5&dus6*Wq)Cpl&}0GN+m_m;3$A|pwHC{aN7~HN2ZFX*RM}jqr*_Efb2qKj^i3XM>xz)V8eYddKd8t5T;an!Ia9x{Kj1+_ z0hM40-Q?N}`kp4h5$EYcgx&ApcxKUgJQRv}Joz+a4WI~JsG;}Y^eoVX1$-m@CCv!S z1!p^%>j40S0NH}4iHI2Yr2uo{oh%lAiGtfCLvzEk8{kHjz(;Pd*6yoU@#=T1*uq1x zjmgWV$uI*(y6+4U&{{T7DqO8~ta;2)OBvstiRE#3d^BjFG|4hu*B<#+Y{u#pStc2g z|J48xu9y<~Q!00DucLGb+R?OpCoWR@0-KI)Fqbt@gCX}N>VO(U^xfV8b&o}9-v7$`DI%q)~$3WjDgb#j(QSV(NvG`}nA)v}vQ0`-= zg_Y*oLHQMP7BA2Dt^rY3<2>4SbyQ{`0-=>5sNe@$I~3AKl*U$Jx~f&1%mT}ky< zQlU>c0#GEMle*ySDa;uc@+<}7AQ!bw=z{(nwGS+4v8A~TAnppj9@=)DD(A>A*TJkm z@<69q>hYdy@;Cvt5R-SDD%C(&M5$-7<;2bl=poN_JtBaHfTq_tGbnyJp?v;KAmi=L zuYv9cOU1k1{qsS-ONXbuY*lOg9#OHNzMAjO6$d~c;p`6V5*?{s;cNEdU!>3EZs=*V z3~=ekfY2g^GLVGXVn5Ty`r*-%+~!}t{)}vAC~@#s`Lek0xez z!0>h_g&2tmoDZff%?K6#SSbuewuD|`cdfnWi3b9dk#65#gq*op!hdv~0-E5_$3@O5 zYtKcg^T}3hI3Tl5&7`fdRYxFHF22ebx&hkldi$vu9NGXqvyeJ^N&o|NZS*ne!|zHM zF?46z9djpP2src*-(N1=vP@uD3?{$B)o3pD&kPo9H}AD-%X zp&`N&!yJrz>&u$P07PVTQWFoxYb=6QUcdkR^>kc=Zx?yXqA9WlIy5S^jXzB0DHe-4 zDG|-z2aaA0@JW=AEd2*PeTvOv(is^d0FDq`ubKdH3 z1S+-uhi~LN%g?OaPoFf{&YB{Dzn~aveD>xd`(}wSqznsQKK9ei5r8~gVwM7%Q3e4d zhSub{-oVBw|!v_K$fq!^0_NP(LZY)RPQy%w?O){8g)-*|A__>DQyFbo}4*xS+S)jS7@Y>vJ;73^5 z(*E?Bb3R7-d9ybUgMmZ9=1nwUni`c7e|U8~0l3khRnt4s7r=TzN_NuDf5L<%jpyue za#TK5K8wO4o)Fq>vOC|KH+l3CNJvf=b^&L&7Z=Xs1bf$j>u<|CMq0IWh}xX3??%q^ zb+1#V0>5{e1PzwTpCAeK1v}aoGu|<<0n_$vY~+T(JQpt+0(XdMNzw1$bOYvf^|0ug zzqsBf*~mYDWd6RoK3FKyugsSW)JcCkTsyy=J`I~zIx7`Tkl;@&nGld-XdEivg4Z z*C%tSC~$Avd+xHfpwIXtp52gK?&9XOO(&$`FxGhWr!4ppsh`VxC`_xKUV_d!4=K#G zt^;#lpUJPQacn0U1s>@6e{YA+)hXxzc@se8fP9?w3fM3~Gyp4%w5<_nQ>AV!l@V}a z--k_3x{d)j>)4~cYyv0EG6S90U-tdV59|y=D(C;FUq6n6P2iHLE4)nlcaK5*2m9MY&y;g-i65(>T2^2nc^xCoovI*h% z6T3)}il>)4vggl0*PnH{VEC@LZvZZgkX}-_^-Rc6vW3@&7?ab2*W0O_{bv>~{CUFb z2OmPI2;w4w2kN))7EpY6Jg7CNrzSKMNDQ0k2;gE4AAD*D)Bh5dct3I&kg34QoI!1W z0;GR5)(qtk=o>^aK~Zt1Z;g@VO9Zm7kviC8@FfMvL7fG6U{H4VA|*6#uMr`cL-#o3 zDKt)MB3N6B7{TqCWoV7QURne)T;qQux*BSr^Yx}@MF#lv?0(k*PwqZ_4*$=Ir8(7R zHu+lwXA2ip_F)(lWgLWM1y(k?pii{E2u!`3LqmuwyD>CHOV9%Scw6S-(K@AJLZ*V~yW@TKaV zxAZBrLMmH15ukPFfaTXp02F5~;rIVQP)iADgdcXeyLOyPY-AlW-kwF&SLp|hr$;w% zJ#>%wv3+|)>YPppAL9!-ADlS=vK}jZn<7mPw)E`N{duW+7^xE?AXl$-oq`6gpv?3f z1-DdWnU8Mmy@uzSBIGy4KEo;a@TubYV8d0C!x&L0pAFMc1mfg)OJi>yoO8fFhHW8Y zR|FcZ$4;|flr$ddavUHpX`*@3<2&dNH|cIPV?g4f&qx0mV%>`4{gTHrcMZ_C`Qw+ z`PuMyl&IBxt}3r|ad+!hf2MhH|Bhd6U-rzkSvA-vE6r0sZoQ*9@Q?)zM&~@WQPI2* zP-TkEJ|mSy%0i!wExbg0m)OL4R#f3j$!`!xC7|e842W^X8KQu!le3VcIpQ@xoTMWg zwf3l^P-sc&$P%7uJUe&sbmD0s6dBtz$)jm_fF zIK}lx{iBw_v>V|P>OPn9%vEvWVhap1TF(`7_zw17k2>l!cJ#ep%nUfCs70jKEN^im zlXNVJvIvvF;o3E06MiISRK(d=8zthPnSPn?paxE;11g*M$qmTrb2uU&+82Gq`yKHc zBM@u-(*K1b6!o1Y^9rb;q)fo*EBVI~z6a2hQHqR{++uGu4Lc1>c^OF;Iaa7Dl;BbP z@EX)onH-4nKXllc>)-$67l#u4PWX=Zi&h(u{N#=O_hfD_Kz&n#YE<-a5#yo49E#hK zgSJTUkF5%n1JG*eYo1?U?0EGm(jQfuLx$0kAnpw<=MNR@O!2_7O=Suwy zi-))utu~ufSJa!MCv40hm8s^jMA5$NpZ&a9l6hucCj2XX5|I%E1ac3zX0APV$U}?C{89hwMxIq(EaqN5#3no;xw4m>~UB96mU)2``;P&UKp5NBW1lBum z8w&OB$)p>~-fhKXrSiyLP~>S-nFmcNK>C_N>} z_E8Nf32`H#SIi}zUS&c$Ea=D=h z#Gc*u@d2(22N~v3ACyVyyBL)y)7|7!uRyoDADpB8(zLKSJ|-FSXZ^|J?;YN>2_$|a zV&mK?f=%n1=i!_U=RLBqER@jHdXGa5hFvf2`ZVrgtUb`&DErvy!va;+TzIYWvP~%S zfekHegRlrB@yh_vsQp`=IP<*Z7ZE_Pp9KT`8Vq7;78RTENPt;;DKpXK4uH=z9>l_j zsy4^>+^g&9-n}qoP_>!8(W`dU=XpCdL zR=DecZp2#B`oz&t+|sVym$Wnb+vPf9vw10;mh5BsBsXejoGFR6w-{P)> zyiUC?qOpvN_>KE*O$@LNOE~n5pkY0J_3%egM+IbiC%^RxiO1zBf^jS6cg>8yB7Jca zNLa7!3D3X&8tfn&H2cO@5>(sn8+6H}l=U%3S2gb(U`yR^b|v-Pj8}+0^A(iKP?HZ} zc`gQeLF|Is34`}ujXw0k(MygEC|V67wfnir)Kfuu6-U+wiu*BeX#I9+&I?VIF0HA# z;EbNwQnZ7b3oc2qu+=_c&R<^dEQ_!eg^n;j9E?B&w9>((pol?G@%?d<|2+B#R8xlK zmDbx|7UX~%>T9NEbi{UT+o!?$4<0}E+*Y+LxX8jbr@9^`xeLEtM_rnkajw9qoPQPh zS+q;4@s23B*UkX^>u1Kh3pp;##x#7^6V@v`HXQ5<_kKdjMFdV0CiodP~}J<{bS`=-&J_xSEECY+d< z(jajcl1QvJK!>dcNAfva$XPS$JXi=VN@>#wG`-4D`fNy);`V74r4x-U_cvyg;V%hvUh4iG^cgB1lBxr~ z!Am4jZHO#g)Hy@(JJ>l)qNBc`f8&$oG*k6K(+Kz)kQOMlS(H@vrZ8#)G<_w!$3lZU zU*n`5AP-o)yeu0@Vz{>MPK2V}GvLo0+9K5z(X*X4Dj+}`>0zo$w?IO#PTeEwUW%3e+oU5Fmk?bMIbvVU2G~Q?O!{Pjgij=}xoMD5 zx=!y5^t5VWf72RH67L=2HDV;!!05V-#_}ocI7OcONxuJ{?vJ@6z^H+GPI*Yda7$A~ z1ntH{EgD}I>ehRWVJ<^gFI!X)R1jgKSvc|Dr$CGd(whNbA9S^*#^}A|`vL+I6H<-w z(FAnyad9#&TN%J_NCbta)t!1d?BSM{VQv(t7{Lkd@(IRvaEn=@w?l9Z7cH!*mu+m# zI$xfV>l!SuKOO?*##5{6sjB1L2+)mXh#^a)CmL5S$*k`r&={C`H|yl&9L*{AiD^+F zxx}8fkNrUFTXz4c_O55aHYPEBpwHBUUe$|dXr7|^P?XH9Hs31b0Anjfp6$R1nOVV~ zN_^$v$3#6JYLJr+qf1Vfx1V43uWe1}B`K(^YjdXf;I0w?4A{`BOKUCg}^6rqvU)l0V z9h?gDI2)F3=56m(u>J%Ly63^4Da=k{_7lnRVcQmDw{4g2*mJebuEi|K>^*ywdBDJX z4$y!ti9!* zw6BZH@Cb2sfU6u@B%<3_9zOcKVKzCLoc{iNT`le!u;?|mu%G~l=w+!jiW4$g6wnnZHB!F6J^WEB^q@p_G)=f)WQMC8bDnQ80zY z^;4N7_P=me;ngQx*EPnmj6QUIepy*r*Z|wTPyeD0D(hP1CCMWX+5AnTp=y#bKR=&c z&ypBAszl-!!f>{Yaz`1gBL6j*L<-ZnN}XQDDP*B2L@-k3!2 zC`9}9b*qfT30yIMbZIFc9mF@ZwNx4EDo8~|#s6byC>H%ro|0o#M~8wvN?%`JZ$?%Y z8N6Vu?3}!%rDZ$=z%i?>>#1UTG_`_)f@YmgwzoBCP?=z)@UV@I4Fk@pA3tmzGBPrn zTU(PE(f`KD*4FlIV^h=pFx20pG#U|4Oxim-!s;m~DSMjCcu6q`)Lwkd!L+C>0l)WNt9rVO- zwA$I~zrTApJ(n)QMe*7EX-V1pm4xRX@V}IBb7gxAYq(-O`}|z3$6HtRtJWQKY(ejV4X74@W90XE3JCo=i&B7@5Ot_!A$#fQ7lzFpYyt@}OAn z#Eiu{24AqZW>?y}yFZFSw{j;ZEpo2x4KDq8BM*Pu(EePx!! zFJB}uwC>3lp+F$)a1;pFv+d4q3Gc1<6gch6oN+zIua;EhE)vDg>1b$a zr-VdB!Ar)JqsuNVAyMw%`)O*276o}ZMEB#G6eO>y$tcS;Xxr4}E+nQ>oWce_yeon* zW@37}wDK9^lLo<$LB~&A*8n1xHp`+Mt2F;|l{>5_N)n>TM>2y^BP45)v~(S@PCM^s`$AoAw~khej% zL$1Mv7=(3sm~M+L=L@H>`@;Zu?tQFR{{C0=Iz@b6kDtyaCMKpZ;V5|bu1(RAaf3j5 z%mDNmAZ5jo7$*wp_LrP-7{+KTV0yhgW9(y|6dir1|Ht@vL6i6DH)8h#u`R6rXL@v( z4HP1OuSb5d;;WBtaGd7l@oP_QyL#@9dQ9<2PT1DL!J)OmkuGq_1X0w`p#K56S=<3j z1Nm!_UlL+sy(#)ywM{=mV#@ru+i9qP)@7SGkTlD$QDaL=bovx&Xq4B|V)`K)EV9Jh z2(Irxut2E)KDE3_+b5u*h-=agE*7US$0xaTjuvkw04Zd7a&ofj`7L6!%$7F zG`395$Tr!3cxI<&t}PL67*tIONA zseJ5w)>8B8IwVr1aJjsyYE>@I5#?_Hz{jZT@mju{q=upX4eaIJ62VuWDX7}5IpTT@ zUu^&rfY0KGhlkCw{m(p<7#ez3-1nWpI<~Ts|GztMHdHvbsMxEtyjIWJ+H%0S^R`Ei zE~XN5N z+5q$IM|oDpOg;5^4opE?`ne=XGsjWXiTQb+S?w(Ge6D{!GUPoX^!lM?7VzaU)xdSz z9KW}3*5acThHCYQEFQxUt^d|}pzY?kR`Di?N=F?m5j&6sK>j*2LPyT>qd~uFgk2ZH z9ldOVNj}Bjgj?BRabx|8c(o$>fIbd4Ie*_K8z2!Qo@~?nK4#UdgTXp z(ACwVm$8EXU7w&47}z2X-fs;8U?ZgFzO}vm_p`WjXVF;QY_Cq&T26O@&6u7bqDSAz zNK8<$z_=Ht9cg1JME38`2(;P{FyT!8aGRZ-J*9^Z_-BzD;Gn2$6=QtWZL_p$#L-LE zW4elqi#zthK>FlBR*q)ni|pS#ihSf9$q(;#13kHTRMYO|`*A&buXbZBuL{IP@0rCY zOEXaFJb5zP-X6*lO1k^s3W2nP2gQ{GgUMouW@G3`Q)u9S0;|mZ25bh7`{H0piE|H| z*L?bvV*cm98-<`haCEE`2eKxHRufP5y6fdVm;jrIXu~z+GLs_tzm=4ihdvAPEAll$ z*J4W|9{J+AK{lgIAtCUUAZz9NciYt(pNUg*OUu2vDAM}|$>xH{+{6CwCp(o#X&*$i z_#z%#kv>`V+r4J9yckdI{^GNnL6L93d2g0#E#&6P-`(k~B}MwbmkqK9Fx@d5KsNZ1 zmA>&zx6LC1!^x?sC?M#Jc;QY%N5}oo<~IS+U`tw_$^GB6hoCDU7db$~nwyy!FpY5A zQiv|RwWt9pC9A$(fXC$jUqJE>_V&d0lhrx?7e?%t<>lpo!4@?!HkL1+^W=MX{n-<2 zaLmHeWmNNG-_+Rn-vp%zijBV+68W-noS#mHE-YB=ADx~$GC(MJ_Ko)@ zU-kB?xbs@Vs)|_{;$upys+^EH>Ax^{yw@%zWRO0)^)yN9FG`WLy1ltxo}8Zkd$~Ke zN3@gW^_EeOKWNr|eEqSpF=}ZH_rH(%UrH(_`0Ta$&M%HpuBJWwgKktGOX)Wsq|C$K zxtEWR`lp&2UX1^i0%Uo}*WJOfQ;<@-|+klQRsrjhTTUmS7Ap?6mEAAd@#w>wVhT&fdCirH#hMLgGLNz{cv zP5-XbK&$#Q-fV3yBFHC<#8kcdCGLmIPiJj;!G^bthmUU`^?wUUA4Co;?L4Te+f13S zdq6+{88*_&R8QI!c3x70W<^XAkD=+=4kWFQL$9V9RB;Ur4VR;oSwOb`U+A%4J`?27 z2Couoht+lJt^|=CupfwtiD_mFebdLXT08n#t0USKxWSmusJr6A&xd`x&w-0T4N6^I9iad8=s$VV zpF{r7nklw~p^1oy?3g{ULSpHczd+AF)KFQ45y$taA<%)EbDFr^dB2V@|MLE4zX4Ee zS#4=cX5Rq5%Q=u)o{OgvOn-~|Z}_lgzTBbmTpJ+Y7Zw$*aXXW;dQ{&Y*|%Ke`|{+( z$2}N4CC3D?g48~N70P3K(x^apDVg7d{@+U6F`I94WpstTs%Jj&si=WiJ$lkL!vYpi zbkf7SFE97A!7}18`Bag;aoU9TZ%y&49&u$rhhm4H=LZi%Dq5vZj z+iehj&YwRn{Xf8q+*AJP(}_>g_W#Ne2t&Yc=KU<-{NM@UW>~J@`yIVR*>?6u9DZ}s zB$5EC#b+9KNp_sD)a(A;RUlSS=vTlUlkV#3YR0qbAd2tl?JVR92N~Dh$*2@_YH`uZ z-~WrBb-nSJv*|IV?Qb5G|AGSnd|(A>!1EPLt_Xe5`I4C-Ce~Uc7#K`MH`t9Z;Me8n z=V#`eKVV5bKR+*+qqcF1uUSX^FE$Xj_3zaC#zNv^Vx?jb3e1?-e%$=ghRuHa1>)Y` z-ZV~yg@yR|_{%tN6!rD>P4Jv_TExo@)Lqg3m**FO!*%QC1A|tSI{wJMf&$}KkPF>m zdvEyMnEpvh|BtTsj>o!v|9~%hWJi$|NeUU+JK3QSib(d3$h<8B| zs3u4hUPuFeLTApG^dX({FYIu=OAJ$>qI5O$?SEng&4^Wg7J>}UVCr4Z{%@lxn8Z$} zvF?o%g`r-dKq4~j0+9|AsjFA7dg1+*EbzxGudlDIg&;l-_kTix@}OR~yR-Y;!)L3N z^#X2Jsj<`vvhd(iytY-K(9QR+t`_O~Hau*&HC?-ZOP~0^<(gPp&T!w^4)Xwqp50{O zOS@U@OOu~FP?mOJOqB1$a4Oh z9Im6+jf{AE`-}AH2LDq_T=R(U4vIsL^NBcg{s6)PMv<4HZeXBCXK2V^rt#A3sHw@_ z>-GVFB)XO&yFYjBEg6Uuy^kl}Q>A|{U6OiO-_Q^cW6(1)))x56y8eub7aE|$e62Gi zqh)NnlMmw@3F_&{{^yMQnerf!7931=pG~8UHf7T;R`f$UXd@RF7lZpI-)$4T`zjg6 zJTW&9NH*8kZ7AqZohmFWq={x8-P^0f<$C+~#r^2x$EQ&gUrRzyzoq$(l05A!S{Ahd3`Kg z2?H7veVdaPSZW<>UE7rqXh@qU@J_B*BkiPo~U!KYno_99J%8ES-Om;5IdD)Q7jMFw7T5pzn)k8 zT=DCPy<2;jhWHY69M5;3{y&C}_Uhl=mXk_SSGq^g1PyB3gT*He!8xF%tF1^T)i6sa z+73udV{x{}k1VAh8XDp-ymzk*mn-bwAz!^3F7_?jngZ3EB7SBNf?z%{r(85{x7;k^ zK9uUNk|F+2d--%xSpPrCBF%XHPe+}kDGB*Lf+&DYP#zVj(%yQNxvUs%xw7ragKxPraKNaxKT#!3j+HZMz85u}Yfv}nrNIoWX z6Lz~C_u=B_Qf*NJpUqlKXy3?4aZxbhRZ+0Y6mOr?)Y0iH%D7ANeGFEfNr7vczC;r19$C1-B10v|J+lK zCtVvG8^9FL?N=2o;QZ<|DdB=doBev?X$TM;u1MSl8#^kreSs#l=i^5^p83jG8o{#{QvT^nr1$lmX?$}v)^#Dg=or=ec;h>`BjnHQ?;i@wO{rcr33~N z7>TA?DP(Ca-_X;`?VFP-@c=WGeq{6p;t&a8tla}H@ovFj3T7|x`Gh{21lud~zDM@- zr=Gt?GW9Hs!u=lU2l+-8?-l`m{tU+KoE*gzkT`-r8~QR32~WSF7f0T##)scT@}ra{ zMBeY)3*D$oLq@z_N*7J;t(W=slRy5e?}`{NBhq}7HL-UpwGN=`7+VB(Co6zPWN|_@_i=b#++Z*jVXN zcuiUecCu+kc446WO8j80`@K7N(o0DeelDOa7$j{aop@C|K+M~>VI?I_B0jQ(E?Y}? zjfxQWbb;67Xe+M!!B+U47Xr3|k!puz9T*6QcONSW)IXAR0z*hx@Cyq=81+LB!nbSbG7pm<6u)dOKU3i!XISoE zHPzn|1T#zIQvmU!yNU-LP6U0n+vm)$8}L!Kj@nnPS0K_%Fp+hNwR`z zJ-JH*j|R#ICf=0{+#QO&5Y`Oh*uD+mi9%2R&!0ahr`-sYXJfgJV+elMpV%P3q=c^1 zz+==JC77gMN;)P5CBOas{ggQ7$IO90Oo*&I^SM~M%mwXETFXEK+%F_p00XaIW~_@c zy!rP_tRZ57$U%7(4~h7a?WiIPN=s#Y_B>F#7Q62@nir?8Qn*>WxrUh{2#N63uR7Ok z(k8%!hy72JniTy@%YdJdlAFu<;T#GG7OKy$0li`@)5h=7UBL53MMvAIzM`~d?UO0~ zVWMnKWYL+!r3tX;=VFL+=t$ZIX%5Jqkdu*d`SBtGgqS9{Vqt?LU6)W^-rlU}rORC< zZ{BPNo-_t4*GFT@+9~t&p-|U{sCEivtsgr(J8j#)-5ZON2^<_8;&`2WGmjhx(DCpM zz_+sIixf#*?$CEG=Y^8$#5`q>QNt&bKd(w6hfgz37>M%sQTwpla0Ba0ht0Irl;k+f@3iBJT7pTH(ZZsl*VNVR9UWz)rMLOA zD|GAPKYjUP%G#&+Dd!V`Gi^Xg3GXpjfOK$-X5Qi~7}`1|7>t2~)Mdtzp0Mm@mc z9!?9A5^Hhy>xC8a@j<#BDkHqdNQJ7Rr@wsBRyXL&C!@=XyOC&d`TbYZDsyEvunH9! zRaO0~A=ke7gXczcy=MYR08U8C%`%|i3&4&}7;lw5OGvO={Q~ueMbo$C<<9r-uVmG9 z*g0urh6<%}YUF5NXsXCkw6QVB$jX>=xqa-^sZ&y{FQH6fU|@TXjFM6kUh2=AoxZc# zDJ&x5C57@pqi-7-MZU-{DClY^fL}FCrXVLT!0fE8Rf-thxZTj$SY{XpxqC-P$KSty z(`Gf+)opQ0f6}SXuW|LG=xdfxJU8!3{vuN-BdF@0_Oij_6ECL4!de#>O9-AldzN-? zV0gHww6t0jB;hv}J6c~$ik>j37Ix=RWxu$XVRFbt?(}q@*x0FfcGNEkIOgXQ$&@D)OnzyrQ_6 zCst+P|7RQf#}A$z4%Fg7mxgTARC+q^1?}=H?|4UuZBz}}m@~=Qn>fkk3{f16cqAex z2}})f<%m$z)l@+hY$TD4Y(hi$?Ypv#jg*-ECcTg%(}cet!1zmxuKusxf_%kO;7{d} zKtULDUefmft-!;}`%Eq8h7+LXAl`>UBFc{<0v;%Z-r~1_WCyXtRP*NjHz@~I`^<+1 zMn;QJ`69r>!~0-)>CeOaCgI`N(xg|IJfzZoxWz^E%_UhBoH!kNS7@^h(?r=dyUY?b+vw*>|qg{ez;=u!93-IZ~ z{X-#>1%VG~P*9MFsOVp|sIB8w1|JsY@1e$_HgsOnd&8c|{6Kl`f9K%Cc=YZs%gbX) z=hwYZO&8HSYv(PVsh!IY%4R9?8KJV$%b%*AJVP z%Pi-(K71lOC&1{g}Q&a*(=lxIT6G z#+)p|o|<_sK!moH z4&fph12}W4aUP0@k^my458>>n+iC?jNDBarEIoqz7p;S9c@dRP4*ckfE9X?>z{7-S-GpLxxz{>iy(zq3}c?d|O`T3oboFgN9og#G!|Ihb=nQ2^ES z3-_5do3G}4!K9Brd^(PpEA9oU%!4Wv!gr!8Y@2!^TZ-}5f zTK+mZs%d!9E%EE3o7su~R$kZ-`BmEy%ZAVT>@fj-5`RIzZToxVej(yA4&?~)f=gDtZS}1XM!g$o z-252F0S80E09NOK@t>)L=F8F;3DAcll3>LwI>)AbME>vj92`G-OE8Wwb6F)-)>p=K zVgC+Ua4Xz@!TI%_Z{PGHo*W4%*lcLuk+-$7I`^9zxZ-ln7NqTaA3^?_MVt6c?S@J|)@8qQE zJ1$z%LrIhWmI|yQ#08+QUc28kGAaSt4XCQB5{m@i;m@%gNi`1Y=AAo~1;E|YCf#l= zq=vbbstKe}>ho^3%ZGn$plTdM5*uFi#Bv8{086v2zdIAH@LK-gIUK!v%&JW%oRgDQ z!41$!)P5;QX8OuJH0(txP_Qy+eZJ zkm~yGexm4+Y!MT+n+jyd$H$vepq8MEI_N8~9MHZA!i#`4fY+ROa^O3J zc{Sw&bFRoIAleN43iT73!C(CjK-k7#FzVQO`|qpTR(b7hIqP_D<1E1tsi3Fl1j<@Q zMsBMxOM3|!lLDKdI+$+(*`w=l`d-l(jt6u+%FhF>Rj7V*PWjU+Y(gwY7r2-2z}`?F zJs)LqVbtZLG}}7lSvt8V+TPx{uz2u>_WvH1^4|;QcOH1vuFOLRS!iuXON+b;UASUR zr<)m2li60qXq9~j?9pF6iE0uOcN&*d*#|92jV(5|`rkPLw&Jh;M)5Pdxw*-Az)*EM z_s*82!NFVbt5twifvt|wx|mSE8UEJ}sfR+oEEt*Rdcr*zL{JcH;?y(nHvS&f2=<>6 z0O~|WZ6ZpR2z^boEQ~n*0ee!yUx2}OJ|zEl55coxdjLxOwOw&p8ITrM8kBfht)HqI zUgR$rUhLTWXQMu7BsAmX_7v#k$=ibRH5O;>d?)OR$(p`; zK$`fu)SGmu3OM0DAOXFypOGO~9hH80VGe!JL4*=)^p z8RiMgFZLo=lrpcY#BtnzavZ#!Hut6#2Gnn8u!-40W}eF~K?kO|eZwQzlEkmr%`2f*@@ZV? zXl+i!DqtVoZhP8tHsXTB_;&iXcZY(JcVF*DZ_liM)v=d}R-3yGAF(HIm3_b3)Y4+` z{vFf~)3;s~7E0YK4F$0mXGe~$jpk^H!23vN=HGl8YJ&|hZu=bti&Wx4&)x6fR&a-z z3Y|K8AesB2_(%eX2>l-s;=KF`oj$GaAh(>7_yu%Mm~{F-8)E++lGX?AW^GbtQ9XRK zf`5ez1>ht;1vkXsWf1GL7GfW+buW8w{NJa(1Gm@lLEzYmrMIgY7RV_q{1ZqZ+dB;7 zyeJhnK$ml9gBKcco{^F9Z@hv+Rb!#UCH5f(poQUqp&WM4_;8SU!_m z?=F!_4~r3F%6*Uf&v<%oEQFRrPrOL%GmBfaijA0Sz!1s(Cs=ZLIqH~Tkz`Behj?`r z@I4&4uZf(fg&!Zumw$7{XKRJF_A*`FoVU2+zzH3>0JE0VstAr&G%nrzf{Wd*%Ao3@ z3zZ*opDMs0is2xSe!$t_FaAa#XLyklWjhC*oZ$;lu_Zz7YW}5VX5fq!uyAp?gmYaM zZe+v)bgHO@gknv$)L#uooDXWm`}zrdS=K?&h)E!%D8J_b*qm?@lisWUY)yYa zaa|PYGBihKRO2F-Z2lUW2+W|mP0cWgpTwKPgWWSQbN;r00CMa{GR*S=ZmZ74gG5w* zB#e7LQvurS0l_sc!)8QlL*S=a)zH9lON%z>BkF6XhUh4*8El|xQ8v%}cR*q3B&Pl+ zB5`VSe~Eqoz$|KTHlMls8N31VFw+HSPuJkiDlCK>IM{xrdl~VLVO2b~Gq7HGrEQ=K z(|6+Sv=$nwxrbSMRt(iDWGfdCOX!DK=?aAKuq4Q>mRYB};?J}GJWt0MCY8Q3hIX(@ z+_$5p!sE!Nke{B<;Q2BSg4L>Izp8f*1mpO>|FuvI8B@uM38%#PAobBh38;7@?m04-Yy$Wkx#q?`|9?|}XM8?D_W7?Z;vu(*R> zRy2=>WNuiBu%Z?u6*-YJg`J}-U<0;HN2I?wuZz1;R!XHZ%q$i_6xj@%) zjiMW9IQ*9*NAJ6~sg!m1@Iq6ak#jqpfEy4{p7WcI0`UvMnyl<*H-h7b--L$3_r(hE zR`~u%CO7yp`}O^{@Ur+#Ofa*OvjQqJK>n!`jKgXb$_ zr(E2SlYpPSE56uuFPl6ZV3|HMJT#Rw0*jVh~uI z6K0%S+VCi)V*WlMQRGzQ^ADDJeX6LQPN3jnH9OgOt6^o#UPQX~?@7EApfu4(d|rs+ zt18F6>eE&zj-G&O^0l|K=;r4%Q*P$280+f( z7i*8Y!!(O`2(WCL8M!UU&p`>)P(&-BGG=*M-=9`dD`AD=pW@ivSPUuJa5L3?m zdJOj5(Mc1znGZFQ@rmOu!AKtkHyA&Y7brZ3wYc?H5{TSvd+%j^{)EA?B>ol)BRRQv zUw^iK`giRE&E(!Yl1*e|UcXA-f$JBK+-&$sq{6Jb`wcS5`bbqkA}r7MkFJ21+-LR! zHGZxF34V-lkwX9p-|+@Tc8Ko>qw6nr0C(nmW!yMa$s-n^_ODSp4Zx`gdr&V%G?U)y z+t8of_ZttMA?KUU^0c;`elRH!;QcTIr6*$90I<4i<4EYtgl5pep! z7%&!NPzb^(b{`(UerS@uwpl6d1R29Fi`Q@4H(@N4!&4(5P8T@7{IJ^Y+N;9?vOk~n zne#n|+CswBS_o<4?`PAG!1%LQotryX(CJ+;u7DG+{iGy+?bU0^IO7l`M=roGU#oHj zsP1%X(*9B_r2uCGpTM7Qo!qValR}Xz`yRdkZuQmmJ_!FnU=oDW=z(ODdHLa+Q0%2G z7^*TOmP!3bCT>E;U}0hK)jA(mSNCk<+)MzFNQYyOTlJ<;gUBp1n;4@GI>(shzET7_ zqYew{405zzD01J@Vo+1$I{TDHl|();jOuJZsVWz5EDB!^ML;b_Do16bN^?WunRe>t z=FE?kArZ%V(XmZD@d~$Z?j8HSn_^I4fE3-Dp{;FU2o00W;K0DUF7LH=5fyOG11f83 zMEUuzYiU6$CzDG}QA zj1Zdv!YJN?nU!^y^&Deufb48VdHJJIvlHc0PCh=G0`Q7~Wxsye?f8cXNK0#cSy`!w z>Fw=>g3ITJ3KlQ&p|DAM>=-26MMXtfSy>gWqo%;+1wTFO!u2E%L9VYWD=UD&kt$D4 zW~3ha^^4ohW^H5RRL~>S)|~q;E?ONBMZF%zl41nxNGD&_67${VS&ffBkwP8!?Ai2~ zIe2Hv%0%C$y1GK01Ul%=fQ}XUTR?{1KO4$5`GMGwA7Y--4N!E61GwL)RAoUyXkF%j zdE!qJ@s;jw(l6>O&*I_)Wn>P3kiULHa#Ay5WOVd_yZe(G0A4`uzmci|yRdNjSN7?5 zzRSt%GEnv4Sp5Cl^vBZD%l!NhDGpA~sHmtH8k7B(PLo_Nk$jy0JUg8N6HEIXWrBIfCSc>=aA`cIbl(cl{X=P>QkcBS11JV;GSw>~aFy+F9 zRaMW>XE`|5*4CiNOo}YWd8?P+ES+@~-z;K30eooH3ZAP9WMAAweML2Ng0gw;b8>R3 z$P}`QxZ? zg`Pc*WQ#X9&^~_sbAAEJ{ENEsNO&CAVJX!u;GgiulI|`nBl1z8>a>Mhi(BE`~_-iY60{)cKJ>2 zjp*4Kp|jLl$s!JI%XqJ54G*H!B3H#d^KkG82WG5gFGS(Yvt0=~{mQV;V;SPG*!O4R zj%TSoyAWb3%<#1?Q*sHCa1Y2*h&vG+VzBgN}ak z>Qg;X2WbzZ3uza~i1h7(dVTxM)H}@bg}EeoZJu6WJ-yx$cPI^$6dzllo-U5RGKD6! z*PldmgOyo46`qtwFG3X1tV2cgjBjAXk8ujV{q^MJ+V4@u50~zrN;_HMF_PpVaj$Nf zklhATGgTvOP0A?hsGt!^PfyYr*hIGtuw4BTgTPva%7)E

JXPjJ~b2hf{SaGyzGmZ+{;(Z1j2f#ew0b zC=Jm^$F8`u&wBz$jxX%AL8Kzgz+({dh%PGo!P2&&I@QjUx8;wfek3&Lc~k_w$~F3L zZWL(WH2Y)@Jk2kgEI4$?CjpB}&Yq2lh+ORbc`qh#R8MTw-iEb6@c=R4h@D zBF(zv1dg-Y?~#HD{O{}I51OoB<@#MWZiygdd8Xi@bikyK1=#=xqtN3Ko0vVO zVN_}O<^^I$oEEPuP2qS0@^_2um#mLd)-2^2<3;g(`-;A+nKB_M;x_QjN2t-xcHqkC z4saxetp47tmV2jg2}R!7Ii6;h*OHT;JWeNoF1uOI&Tghb{wS9|(M6x1|5*f%r)#@O zF8v3UQ^W+qZX=RjTP(LPNHaF@LLw=Q=*Q=Z~$#fsUZVWcyHq6sHxX~ ze!h~T`h@em*4ol&bbYXhx$MFnpaa-IY!m>JkFa=?)i@wW=88x|;72%{m+(@FoOR?A~ zT=lu}lwZXI=R)10+__4}QHkEAeQ<5)@IGh~cr^@&OuP>afj<+H)YFa!KJ9e+A&>6( zR2(?Tva?I3zk9OGr87}`FNfHV)7jaHsz}MNRlmKN%PY5Kv-=shodmBzB^6PqgJr9G zXi^TL_wl@~;Y`=I#$MjNqCv!s|N8eN8KVf#X|(dAEpal;AHqJO9O=ms zpn|uRgZ`WLxlft?L4Mym-P((d>xbNShFjdFZfFo?Z8`&8@N*$*BHwRRpWglOTK&NL z@2+io_ZRB6zlJ$;uO1wvh7eoe&Q`~7LQL7-Sse0|?~aU}j6Flcl$Zv2reTdEjv~{y zw-0_PMbJOuVq)fd_esFpI}#_+?W4!mZ%PXC4VcE;M%9otQ)@q1VTwua}y!p7(;z0NEh33i0nxxYi1UWD$$?szY;>;U{>% zxbnYYwc3i(Um$Z_A*H(zvbr@D&HeX=;bzOO|N8zhP>Y%U1&q8?O1bP$*pAt3uP7@f z!~E(m%40LxyTw%8kUjt8@>9ME3n8?!sYhA>*6)(dL-A0nYp)jvU(G^F%olPspdPm} zK2a6wm$5jxMUi7>dy-rzTHRe86v;5owQ557RjNZOcp-DbsZdIQTQh}kRO)~wd*{bg;8gxU}`gU z@Ps6jv=?o-xk$UFE6g&RxXTLJH2(?7YG&HCvP$TzJB{v4OO|JDm{N?lUhwJ|s>{5Kvk*l)L&HgKz2Xc}JQymf%#^}a^!MhzTWL(PI?K-PN|nH+C7%K)=Jx95cL^fq z(;{brus6Scrdl{3b@pof4ZJk;imJdTR;c37LV$V*Q3L(1VwS7ZtoKGBb`0#+Ntm=* z=v;hLg;cDSuuK>3(n({F%#6F-#_@hj`;gG#{yHIPFm?9Suvc_teTJ$;{9U+Gi=K_p zp;Q;71K3DsNOF|D@jtq^aBgg6)Oq)(yUUqNCKwsfY9T*vydrLOudJb~bR&E)VQWA7 zwx8wlt7X9P!q{2x)8xDw7RpypHq@<#bQRa zZjn~XS_jL8W8|VEeqzS2j@KoShF;kC1qz2l|l(yw0G$tTtK*H0Gq270}ODV^}-YK-#^NQm*F2bUOob3 zgN@kDOULtk4-cM8!j{m3vY@7pbuda7v^VYGz0I%;QC)F&WDraQ9u2n#Tq=z$=i_nDrc_pYPY%N`$D*N6Nqm#@G?~t zB7l~!;O?(b&TDBY)sOKmRllKXFjFwNk|^`=tr!?a{k0`#NaSZevg}RS#`Gt^djSg< zTSpijw-8mea@)y$HlzK|owvC~I&*<$9GO33eM0S>w<8qIW zpVPE{MDhYC>* z>9;kroD3wMkhCl+522|DC2(1O9SAa=2}+Y-hj<^VKe2_I3xh7rj|l+GppEC$zNq@f zu-cxa-1YhP60YUMT3bl~9Sq9Ajubw6ZgSmkZh=$dd5FL5mS4yTX6CKu@bFNW33gY*36Rra{!v@< zLfekJo=%naTLQMM&%tBit6iu_m2|fyMK~cvSO;=5z-F6hw0BGw6ly}#)bBXy`cE?9VhbuoJ8z$?CO?d^b=m~?03NVtS#-K z&IP1(Uwf}li3NvD)HY@eNW@fn??ck7_-6SFF?g$Ozz>67m02XGSc5&A-FwjXT-a{% zatA|0mNEg}!N%LI`Rq2v{;?Y;ja*~*XJ0r7(q@4MNMyZ+ZnzoV-N=5EqD$p%b;x{YYs))TwsiM@tW3WT|6F|QRW>tog0WY@2kzbVU%>&?8q1)o zu2e@k8?nlm_UP$`pX9sP3l*s?u+D6>2au!ghx~0c{rY_Wl{Q%@;6N~#LGlAlfq^>s z%vH@jl6;TL#*jBJHY^5!)nO|5rz|Dp&6$A7B8_czPpJug!nPD#S62tHVFUpoCVRa6 z0ES8~@$WsW5Wf7m)%~{2ZFhnB?`6?;k>P;?s~`q1GMJ7@Z8wWf+AFkz%6w_l$GJO# zV~2WO5YKb#@pmG3$%|?*N!4DB0{97T7p^b~afUwXr>I+jaWBoXVvz0}$h<;yIF+lO zq@AnI;Cx*n%ZjutkKV)EG!+j=;y}{0jaXP_`^Rw}*TL;g(AkYZJGL!~=e6zpV2N~Z zw$|B<>+jXLg!n((Z_ep`qSCtih|t)X|6CD6OSDW77FA+c1G=q-k_az@K(fE%7{_}W zc^JC4E~HQ%m*KfOQ$W#G8AQqIBNL2$3B&`e7lQ-NJywg42*7$s5d##M36(RE_^?-H zi^3;N78O)1aR0aD{uCJoN4aFDJB$D2m#hNNz#l(G5Xq57^ECi5Sz9x1MB4eafgfeX z94)1|!Y0d;?GO?_G0EM*q}E;qjfsRz>;W2Ef$4NY3O8k$T5Ne9qDeAlhOme31O5M%b2yzj#M6w%UW zZDe)gTqk*~PbfWw9BX3@ilhuv6+`@4>eRNqjvA;}aT%q?{k_`hT_^OQ#K9b$?-m8L z*Dij}B4RNgdceu?{W*m(16qctSV(HQ?Ks!T7X5GeT349tFMlG2^e=Bjc`ykE$M7cF z;l>4za}Zfm5$GVEG_oSW{s|LqYt(UDH3ip$%j#z|=f30!9|um~GZYCd3=LucfpMm76IG1*FVQ)I$6ub0f-RgMmcwg;m{Y zV)M^R_o4chr9$K&_4CIzIF9{cObyX2(nBXxKc}x~tX$S`(vU42*4x(Hi0={UzD%$jjAe;`4sR4Z=0=M@(a=&7MdB=_SuD$T zQTc56C>0giHPWhs0hz}q8T3!oNH?&J;d+GM#S4(&^1bH$+Svn|b32AF92y1DD6XBF zc!PiAybrwK&yO&u8|Qh{r-B?t~V~R4odR`ewz}$`PB z?7?9=ap*bYnPH0Lm$|K1q^@1E^QLb4(PFFe<@VC!?uwoY9{c#&ZI%!$^53&RrLCtH z44bF?DoaIsAZOm%f<1LE_~NX)6D6VlYhO0V%^%gjcuW9&$?@bt{jqz4&l?>EY^wmJ zHIj!U_KzwYC-{BUSHMeO{z#`tJp&kx*O>qF>2YYMfmGVDE=XRT94@A_{Q7I1bRfPrT?h9QMNqC)+x0 z}@}=>jM;zf`w_z3rHhMql6LGMB=to`3b8GfVJm)F!iaa7$rR*#=k6VmzdNuY1YL zwk1g^`#oen+O?hi1HLm$9h#3)Aq9{%$Y(#9Qo+L*siY#2s);{SxHCxWG`AgErmo>i%y zTLuPd!8iZZ!K)+TSxB9a1L^SgoE7f@0H30kf}cO*(UQJYgkT2Wku5=CBZf)B^(RD+ z&V~K9fd$jk7T%U{(#Q#&hPoU-zz?e%dr6&O3T({nIu8NP)`=xdYavpdh99|_Ve(`E z0`unS;Q$u)K2SWzAYkhj78lS^F(WX6l?4(=5071fLp(FNdvAAV-7aw%c!H23mmlv( z4$E62n6D@nSs$i2@GR?m4`m!l{*;iA9e`h{;1P`%{s??e{>@E?L<7VZ!K{*ZRoQ)? z+>nFepY1S<62Yha+}s5yRS(p+m^u*#<~g=1Q)MTGF;{0#+8_ZF$3sClslXnofowz^!VAOZtJI{KrSU*SIY7kxHqW z=Qu#3kQ)ZYpQN?e*R|u|Q`{}YU=Qy=&Fdsa)Q1cph-UqfR_Mb6>4S%5#`UB+@LG-} zT(hYv{WSzuJs zMBfuS)!XjgGwXt!5EK*BqjxOn3C*xRc;Cj>7D~CFsmZ^>U~_8y-cPgJBLg^tm!+R{ ziJioJcC2}b=n3TX_jJZ^Slr%MAU(RK^c{{hjM1GfdKVLNe)&z`S4D410a?_&n&g+_ z)R@?hv$DwS&_5F4XY*X%DmWVZ3xB<9v&)>_h-l4AdQx(Ebaku2poA$BTZ~VH`v~Tk z{<#WGI8Yv{-;I!J^55gG{pIXZJ4*Ut8y)X-?839Kt?&biFH=z1OsMXg`#R?u)^5sk z;uxX7ZMyQs1ekkSnA70n{nBr&BHM4ynQ;1RW0iOy5`ItOB^UaAFa)r?bZaK z%GQa~ciifY^ZMpadb{H=et#C|ycf@C8Dy%h^5#M1UCXHIp>Z_}OUnm30N{o{GeO-W zN}0SiS+9K0uk?Q&zsU=&+^L;?n4muZcB)UM?jVQBPw?*jB|2g^=sgLp+T29NeP1;(xHO&2bs-szc|C-b?{v!86W%HvMx)s8e zS(qaCB-)T|?BS+Sni~v^f)0xDC({q7E{(##lymBdPQBu@=#4?E_;~dhO0ceA{e&@x#sQ_B<2(u^O%aU zZSunbJkeX-BJeC40sX+nkU0-gQZJ3XZj1ZS4R||N)BTu2n0Z~?ajOt&$B~JNbWt&3 zr;#a5Miu6vIqXHF770dQ(xLiedBY6m>tf^QA;e(;T=KhMr$eBp`LWM)d8+|F+aAmc zL)XziAR$jL;cBfSVn6Vb;Mj?qGJupEOB67ZzhK#Oi#bX3p7G}iNjrn(Z$JUm5yaKo z730Tq03b1)t&8AKkYW@SNR#7Bu+()S4Jk>4C0pyefI%yTaRZqnFlNafS#=(J=#NyJ z%^1pao@%6fK)4hwByQfCvH}?wlm39R?wAjtk3Cb{1F_j(AOmo0uoz1D0eR=~u;s5L zk8xJOK`38E*ikM`uvBb!${sXNRO8zp*HB^=Ocu20hEU#C0xb*Bsj1e@G4Ffdc(qR$ zuy=I5p>2vAh4ysw5jMuh_s3EuTQ+OPytv2Swo*tg#d4RvX5Z+@ehpx)a1vOBg2F^? zNQl>qW{a=*o}BFK7+_eZ?iRNB5}-$V{5;#Fishq;yj~JtU#GoKHIx{@IoS^>ZsAn} z@1eNM41c{35|OwKS^(tP9t72iLgHsegzUmkGjRY6oJVCg1-K@j%ckm(Wt=$E+Nfv2 z!}3Gj99!9^nH_&4i+{e?ORg6!VD?op0IR;=IU9O@0}>LuBQ77QyDz=_D2n4#JWJNT zid!rQC>06{jWbLHZ@qs0xU2SjB?Y*olgwgWSc}NW^(%MqgS8wle(+qZ`tETT0$g^! zxF%6Cc*#Y$NxC*qJiyV}|FjRGZAfSr+EZlSRm}Lu_ze<*b5_&-m&k9>wMX-d9Qe0t z=g|EG^zj<`$}&wPZ_P-Y4WZx~0F2f;81Hc;d=ytQbg0s8uZkZ~`-mHQhbIXT;gDha z$aZtPVL8-=T&Gt^0_#RSA-TG~tQY1i*2W{VY&CRWOuy{y{Dr737<%ylW@T*7^_>%o zJSDsXVB-=dQB zyJ)aVs+)X`d#1WefDMVE-c1pWda#?`LhC>@3uzo)F0|EXOF~LaiZ=VbX9%o%e}|~t zqn&jbs)Iov(K>pk#n@#&_yVS$%*lpl`+gnX-O|QrWmRdu;d3*3%?ws4)Yv92X@*Mi z13rtoAwE&Py@yWFH=5r;t{)A&*NuaOhZ@Zkfm?J}160xhTEob6-?qq4A-O%%gEgM% zrqRo8s_-X%yAO`_#>=;E0x25ij*R>H3{i|uB*gbrd zNz)GdD@8(x1(?p{aC2vd{qkqOwx&qlGi)HuMv5`R?@5b^_j45!qcc;R^kB6R^wgkJ z?Y`CuuH$S^=E+DtV_C-E{-0Akp6*|ok{&d8m9dm6zde5Ye%IM}dvGCTO3wy~hyl>( z#hsj&H|)3O^nd9%o{hFN9*z^Z&5Bjey{75lbTg=HELGTfvW=?WY0C!~VFS1vYoGf~ zGx>@L+BniV;RLfTSqYBDE%Qo?;bQ`Wu($W9$3GQ*JZMrKKKvp`i|taRAqW87$^HVU zpA}TDpz*P1XsFNU0Jg_5*`c~%N1y#nPJ@NVV1X@aqUpS#=sV+d&F`yRJW6N}p^It*2I-Ct2&5LwW7ETnH(6<&c$k^H1Ws`W-g z%jRTgNC+4DB()ZP*_{-%Ypx8W!p%OuUqdJjCK6Z4q35aEOOdvruRg7T?=0B1$$eEq z@~ATN(7wSrn}%z$s`pB`AnkO7ZLb0WQ$iZkjETFbR4G@a4~(0QFfnJXULbo23) z%XUur7X>Kgnr{I{8uB?507aOVj zn~z%?e3A3mgxwoiVUYDf*Bt@`%btSKVCUPt4dyB`E~{utahPXO%muvzc6 z*7eJqW|^ktT%ap4B>ajY3N~^pqG`Z@0gn&6#4mPx|9W04yfd}dAJZiNl7%LQyq%o> zn4(=kbWw2wnC45CJulJuoYxBHmXcLXq2d>h)B3*Qa42lDa~3KVd#6Y-zKJKePQOq1 zUa*wfd*Yin536S37FB5vzZC@1%bW)(%Vf*J5tp}MgB7+3EcPo0zI*&};0H=@)&m^bjf^Rp@+MRZucyLHwjZTQ~`Oi`T}Uxs|@4Ma8ZGC4}w^HVg~UoTEx0>HKTdL(r3scW=tZ%z3zA~Jdt24tw2YO<#* zP^65tvBy}v0{!2q4E~j)E(yl-$*=4G?Rne1(G$K;zMigAp*Aeh!(KclfC-I_QMqyq zGv`gB+tKrYuEx*B7WogifhQ9o$*@etzqF z&)z_X3j;r8*EeHt2kkAGDwMxAAsL+UDY(*M_#2bg#*~ID=`E3myBm?wlP-52Ah+vC zQuiiL(X?ImpeFd0bI1s!acPoQE_`Wf*@XPG*bN3|L0vKoNrfF3npM>Cc3r66iT5-4 zSy5rp^8@8sXOkh=HB<&Li=+(|i@k^8vLW!1EXw!4L|b=R2Uz*|BGa!)zTGq<7yM1f zt@l(A&3egFJu{pY0Ga%qSjwI;ym%Pq@^CT>^6b@Wi1T`+fD72^7#d37PM$`S-BLJ3 zdun*FD25NGFsZl_{Pq{)h^x%`WvOK|+41f9F$FY}U9;Uy$HgA-MQ&Z0kYxO_Bkua% z5;Ba@(UTcB31QsvF!)oK?Tu{4>cRcm1J873t2WhRGE~nD2>_s&&;NUI%Ewm*UU7Cl zN1pHMWTar8%WUs5XWC%JplwmfWY*S#UNd4G6lfd5Lx`{ZR@GUm?0l|rMIO5Z{Btg6 zqG?wAV}L+IB$kk&K_`qRCFg2_fhv!2otRi#EOJ2r`?#F@L1eh6VjGHx%&hG2h6};}ZCX|X9nr*gMyL=98VFe(uJRgIlf{O6sBH^AM#|R7D*j{`&@!vz zp9sxzzCf%ayJK`^V)vF+>U3>yLEgs(v03hx@Fhq*17vRd_x(w%n)_ow{1iT;x<{Q{ z_DyYT?1kd7_a+d(EVIGXZT?`2bdSE5cUyVzCLi1(b?rKvfY_I#AcWkS%I8bAYIWgp z2T3&a6lwDvkJGwlpHl(;(9AcvaG`fgsc8(2^m^SCLaZZP_lb7=)P>DQ%;YnNIob#rWiwv9| zI}AA!#UGqo;c;A1;#OE30`MKV^vAdUWBY8xB9|D3Mdq=tb;QkHDlg%NKG}SBzg0Ib zVSL1?m7Md-^;Y)?7HJ8q?$<(50M5T&3K$nCtK{X_LPDkP5bR6};MOi!j7yJs4cX+~ zlHsdcl6NwOhB>w1f+>3M4W=Zk>iY;V!U_6L9f~M;u9`25l$9){j$pJO0yw<+t|6|= z>+9#m>LTMmA`|XB4q@6jz(TB+GUb|_+)Ho6f#4o^iQc@&`Sdw8b z-&3!vQjTWvOt#AJSdo0O8-u_g&PI#@Zo@Mx|9uS^c+gLU+#4a=-;oUN?K6vO`Umq6 zwyr||N~Vyk6_L$bJK~-1x2%|dTW(($eJX&qx*9Q0jsZ3)>w=4~#~*vl3V*wK=3L#( z(gR>F9lM*C>efB1_G7a;=L6=tRm&zM=8|A_h=jyS2#eij)$yR_9ML~B<4g&?p1wlqon6Q{S-puKLK#s zuMP%?FYC@lcaLpH!E{OqF&J4_ebyUP4>5Ww6%6VP7YGQw4*e5HzQyd=FCQR2UGGiA z>t{_NV(~cru=Xo*4$HkMcr!7VJCjvrdDAcTik&%&#z2TYrTm)zg;%Tl2p|B?KiT!s zNy)qtoO|a|bF=B)jyUJn{;TsvW#7EEd>{&LHw5fn&z!?QHD;@?eM^MZDnj(2luO%- z;hHS&=FKdv@V(#R_))S4TUnuB8OR8N@9*z!o_of9c`FJs64GMoRp9<@>M>ZB8bSpP ztIA~0V6OhwdwqT#FxHGIfSlN>S6v=3i>9M(FaxlmOsU4(!f~i@uY|j(tIxJgAgDTabMlBloBqR2aNaHX&6r|z*wh4gU3Xy=$IbS zvuJp!sVQZsA9ip(d=QA`#?*uVfsp3RwACc+-&p+}RPQ>cjq7!oIx>s(`7i@~H=Bkf z_t_b2CDFcMP9GzEV2S^Zd1x`vO6OZl7;PH!ZH??3;OEM8Gl(sl8ffv^gFed~f!{j#fVS=!FU>Rc$#Xx-9>b?5qCqtL)g``PxF>jLQ?Xds_s=(RE&Q*SOyiP zYql!+&A_e%OmT%uhb0*l7tfb9TkU61w_i8e>J>BuXs64VVca0t%p{HL+gQ+~LFuo2 zTy8v{OI48gO~XdqciM>Is5*F0#9jQu*um^Uxa$xhN- z?UglIk{i2tkbD)Vkf!04!Eyb>jwt5$7v4cjLkrvZrD^>4Ntfs^Sw@^4wUsCxg_Fil(v*B!>4O}&RZP(Y;l>)nojh2LBG2*2NcefVY2 zh2)bl+!ZpLBY2rD$1|>rs=t&ysD zge|%!t4De|le3>Y=Fk_kdC~n-R(ZSQ3h&#RDRu7b!vh&B&fAiiew)8%AM#lwx~*G& z^9RG)&eg6nJ+p$1JLxx2pur?;WYk=V6P$#@ehJ{aq;%>Q!EtrClno~92?Z`%JlDN((& z^WBZPGv+2)ESU7~U2^B?o3qj8_}PEPzhBU8kO;T9ap^VRE|V3R*OO^)XSDKGnHd>H zmk)I1hV}q~Mg#f>%L|&zUs*J_aiXEfP_Uq1o6!=uG$k=J{NeUH3CqJJ^D;xNvR7I$ z&CFt7U2QPX$v>o#>Am~KyG_!oEs~$jQ1SiJ_vuDqk@feRDW~5*6LRS+8EPHqk^Bri zikDX>bW&;@J}egPzt{ddJfx~Mmy^s%!tIcD_{!U%BbxFh%2l1WFIu!4TfNouuGrQc z+PyZbwzE~Xb4yRvtW!wL<)D?M@RLa~`sa39iH7L8-U+*}wW-2Q>-_4PH=5o3erc`3 zvITF8E;X*3_hMHbKhZaq&$?E4mYW20EMtYeOLpcbM2dV1?fbp2wBw*3-+;Vk=dr;< zMccS1larE83CdOMuU(UP+3U5+38%bP9OV=c#bMAI|vi?Bt7JaJafWF_0+zUP4O zXNd@7uj)fZV?DWpNY`s0@+;PDT#0v5bj7ONA^!lrRl5Tw#%1h~7@@?v z4C|9&?9S*YT<5%b)BetR)^*>!lG|}vg*#1|h3^_Sn10rlJzWv;IDP!qJ^IC0iki5u zvc~XP_l5P&o;e%4eUk&nhL@SY8PU2^m-3H{W=pq3C%~x?gOJvTuq+Iob=TD7d#Jhm z^582M(Tcu{HNDDJxpm(%_W0Ru-q`q7`HV-~2gB)Nv%_^$^a5{Aj*75;J z-COrbq~-7c-|)(pSN0?ph#Kb%6_}B^muKkE z(9hj20zAj2sIL7hnl8QZTx)|uKs5bIMBBna`_MSPXSx0#d{zM&ru()@Uc+@tfbLFf z?Eak+y9~%64ToJWy~tIT)<~(@Aj~}P`!hz*Mq3AgFhWvD`2>iD7gYb0;LeU#3j!7$_9$jfIfs3>^u!|ce}W!^*$7vaO9L{S0)0>DeL zm7>V*9UIdD6i_r!jMzlD+_saSBPHeM>l@B)vZtp9P*ZfISds7S%A92D#{sJgnkwC#a`dhA!n zMn}yoERH+x+O=Tn;fh1v-iB*{bijTQ<#~{RIO8c$Sdaxfoi%%&$fXK)c600c`Ez5; z531v)FJI`QTeogCB239?Ci#pdiEO(lK+ji5Q%#l)5Br%QDGRx2^C+%ZzVGhdtEjE5 z9hSjCjjgn_6xhu2xbvUlQ)Ny;fdoI|2b5=0tn=~l0lL^kLk?FY&oMxIuT$ie=e%_E zmVUg+v_+|Nc)5k8OzE7}`i}aRHZ;Ceo|)X74;i8crb42@g=G~ScRB74XxJ;Fn$9}n z3ucHE)^&g<2|*rVc6N6BOagO8!)O4PXZU$^boDMSeek~W&)BRpMfnkfkRwMvAb)9@ zU1#X%?0gA`5^mJ9eFqMR2qR~Z{VTOqtLE|p4W0euR#DMhVK8gyTmlHGWDnqD@FU_JgU9v(85 zg?j7;v<3P7K0cCDg}xG!lOy3>w=ceXH=ZT3QV`Kz8mJcYFEofON|WKUmQ zx@=iLVsBWjTUMcU#EB?xxuY!Q!7MQ+Sx>Y=YU=O%9?)!SZ)dfxzXGiTOhfV%3RwMl z@daLl|IWDOQMkzFDLI3led-Au_;~`X_|-xW3dzxrr!V<9nCu)%w%>m zD0|`J_C9|RyBS2fzk(@cIMs6*_SH%zd$RFNdZIPH_ejcChKb2z51%Bp(aO__Iyka*)MwJyiO0pkdqAm^~ zrs6sWDed4XLt=@&Q4jJ-C#m5gmSN-(pAfh$%pm2Aqy{b{vEuW~Jr)qlCaFo%IjrS} zSR`}8l@tU#)%G9U_v@xZX)^Ja+(hy876Qw@LQ~Ra^@WG2yAXN$3N<`TsL4_sMDQ(= zh|+TqnEuo`nzcpaRm)z8DDWIzu5T;)4WAPqC8ykgH$466-Hk0G)a<&tGfQNOk!H4l zlHIptpq3wIV(P7Bxc>rvrKz5EH)_o z5ltvakeiRr$ERVK8XP7dZ^ox4WF{2WJiwjARgWALiG6_~ zavvr*_469KdU|H_0j3Jj2gx&8+5AaKNe8{WLW1Ec=AG0;3Fka$F|PqTIgbpvBcDb+ z09G!E`;=Ap4LdOuP||4Fm%S32k}#BfB%V9*5H!dHUMK^Ou5QX~zL=O8K)EjEqJExg zcjI_I$_=KZq|~cY51U$AE@E%ZN4cMP4yrqxl^QEoN(n|F{2grSfPYz8X<{-MXuoZn zF+~RL7(N(96ppS>Oy++EPO2$D5)_)Wl8DMCMEL=poI2+c1FIIsHy z1LZjMA^BhwLjaKPz#%qxh>U#l_?WbV2z4kd^1ylf4-EhSgw#f?8M<{#x#Iz|)Fr+s}_&;fl56@-`C zFZV1v{~R{x@d*?VfO)}5?qjsQ80T0(;E^LbbQM)q5@L`1IU^tYoV1J#s(nV~0|-?1 z^bRG8@O@rhxAXHwqkjyILTt>qsZHQ`c{8-ScxZ8uw*UMbrw_EkXza6qhIV2%6By{A z6#Fn3g*>4iP7(QLWOVd;-8bY_@UDjf3Zw@ivIF9Wju_!m&n7Hu6C^RXRE z3h+Q)9SsV%2Ke}du?CE0qD-@D6M@gT9(9_bC$sp5vqE19bR&+`(cs1=1pn^$qkgX@ zbqdlQf9u}8_uu*zSTltYT=Lw%|4db@!Ryk%2@*?$)SmWj@7aJ%?waC|&+BPaF8iLn%!RWDs` zAZ*irUxQa+*@>eejtZuV+ycj<6r*`A@fa;SDj8xB!aXBRSdpJ=zL1z@1TBQ`p}WGF zXG3yLCnr{y4d{@DInc+IV)+$-cZ|VEicnK^aRIO@pbIIskBs z1sR6o6o0me|GebuyID~*XcurQ+}+(D#Pgx1reGGE0#@K2Lu?n%n4>ZLOMD+uzK0O{2CuG$<2+nxN*#<4wObnKTfH{wYE}16LK>NrRh~3+h02!Y-aRxpWxX z$tfv-*j;&HR^l8P87Q^99OQgrP_)R<&`_8R@7}#jO@J-rdRyuV2etUYW*}Wd5@sf% z7Neey&de5;nt=&npeGo5ka2F*SxvW9ws&v{-Oo}>FjZx+HPS436H!tU`wZ+QM`@N; z17Qb5GI~6=3$;&7Gpvj4XS18cadPzN(bNP$lmK)I8`+rluWzm1|Qo)-~a#{B~FM#Z%RI z!>92n&=rIqSc)EQ3q2cDM<>qVCmE?g+Sckje0mamdLucGOj9UOYTf1gz<${%8y z;py!yK}LELsX|Z=u$=;o8E7>j4ksRe1_tcd*pX@EammmlU2lME0UC9GeKE?@?mhw! z3QO<6v0awpdiy5N95kV>e_|hDT?H%mwz#INKPbw-Kqt%NHo zuoGLTww9K-I_h@e%zgK*pBfGEC9BpIOB70*GYKlqWWtkmn$VvG?vq>sKrd9Pl9H0B zG)u+N-958tNuw>=k$OTQ;kP(L#4X?Jy9qbR2mNg z$LExx85Sl`x}Fq>Tp{dFf$OayjoFxQlB`lcfvNQb5wwSnK`RJZy4?rPYs z#yg#p(z1~Hkk_Cr9+7FIWcM9u5eP^h|JAWkTaY!-KU@SHbBng!q6-#i#^MSVyx8w4 zB^l_eP;Eu|Uzx)0_CYCu(Ajd(#LELHvh_=ufy2K#fW4C*o7>R!z2SQwG>Lky|8D>p zz+)-)ANm2`u%q78M6JAVr?J~Q^B_ofDTITcd3Qh1sV4wt-US^8yu>;Ju<@?E&8U%_ zX_Nl}mQT;eA~MktTiGqS|1**fVEDt>>^is-L+|3C+JH9=(H@03Y4}52GLRLe=lkOe z`ipq1z)D~s`#Fqk0l(q^+`%H^*=TX=&mkC;{`3 zz13Sa=xS<^(HUCR2}McKHA)7&;jSDP&qVRs95oWQ_yS9 z=`ZH_{63=B!bJ>PL1iF<31ajr{!JJVM1T$XAMymDr^L%KUITpYzP|lRv7KMP@+#n5 zPB3eO_Xw`#Y2?p_X7zSsO+!C|> zyY)=w1UN=Z{PgJ)kjf0w1Uc~w9DajYw#q0#VqtEM66Mp+0DxfO?vvjh|#Htcxf1OY7Y{yiH}V_^C%>!XjQ;ufJY@XJvi#?~{*T5GwC1 z&#ABQN)+BHCc0`d$N9|oGM;e$e2tAa6c;M(IU_Z{VZgiReZWN4J+YdKiN=wMcHP5A z)~*HlDC5V;$?3LtudxQ{SZ{7dEOa=k$Yo|Oa-2mn5g#m@x32w2;);%rMq)J1Ui^6P z-tPAHxaer>En8NtUaefc=Fe|&MChZ)4t5Kt9Xpo0f4IA^+2EBKDj^}aV*%>NFJAl& z%NjeJXd5b_lOG(++nt=!QSSk|q^9QE2M?MN&n5gEmEMvgBC4<$A-RYB{DKRy^^+g$ z7?N^wfNE1#R5UR$sYfap4rG_(2)^&^?pBGz{8pvt99~ep=+Ey#QW@k37a)fY{f2)8 zjGx%%ma<~is-m*8GHe5ynwlpgBl!=IHHq=T+42&l3aza6BjFDidJ7j`32cAA+Z{3G zkT58)@D<9)$UwQ9xv2sf$0}sat<|^kaPs@9q{x-^%f|J*gaUE_*}>*M+@Q#L4^O2 zBftg2lAJ%R1Wz(RWl?FnYX zVFJzD*w`2zU75LaMRrY2Iy9~()Sgp{1r{J#G)7hR1+3Ty4;%sYknVxyu*cC+KGqak zwiv&0K)^VX>67&WRVO=#NS2P6Rs-1qm6}dOL_juhd?>~|{qW(#BPV3NM=aCSG-4i0 zNFY&RaMW*AB@5JS+z8-qabh&`Pcs<8;GhpC7z!%j4w0wkqe_Q^!^D@TB9n8Cm0Bu<<%u$1P&ZMm_)A-xlIV%b%1U_N*VCin zu3ln$OaIpn$vJaYtXMHLG9stzuMPRe)Wh!!_k(x<%n;TG#2}~_MGs+k{p;!q@J4HE zYtS<~HikMeIQ_6Zzpi^-S67gihb+t3r_R`mai#!L5=S#sbfgWlK232addeO>deqdE zYMS8+vr$E*80RWVULiFrC@2U!3XUW+n3mQYXC5#2tgK95S={5}y=AzUlb4sDH?O6$ zHVf*MRaPxReub%KuT*&7BCuf`gq8&WNXYTx*k>s2g)}^*Md6fiM^*z4A2_TaE596G z;XqSYA8x-{RCL(Zw*Jh#b-8(Y*eamO3C=VzNa%oB*Vxw*NOmDSp{ z#|_fL!o$xzb;h_IWUtMgGiT4g|3-`-AS1Z2;xR#Sadm+Fi+{H4kcq#5pKy0~fByWry_Cic>_2qU)pX&;9B%iD|wlKPBus!I10M5ORXaE2J literal 0 HcmV?d00001 diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp new file mode 100644 index 00000000000..eb2026a429c --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -0,0 +1,251 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 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_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "nav2_core/controller.hpp" +#include "rclcpp/rclcpp.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + * @brief Regulated pure pursuit controller plugin + */ +class RegulatedPurePursuitController : public nav2_core::Controller +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + RegulatedPurePursuitController() = default; + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + ~RegulatedPurePursuitController() override = default; + + /** + * @brief Configure controller state machine + * @param parent WeakPtr to node + * @param name Name of plugin + * @param tf TF buffer + * @param costmap_ros Costmap2DROS object of environment + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) override; + + /** + * @brief Cleanup controller state machine + */ + void cleanup() override; + + /** + * @brief Activate controller state machine + */ + void activate() override; + + /** + * @brief Deactivate controller state machine + */ + void deactivate() override; + + /** + * @brief Compute the best command given the current pose and velocity, with possible debug information + * + * Same as above computeVelocityCommands, but with debug results. + * If the results pointer is not null, additional information about the twists + * evaluated will be in results after the call. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @param results Output param, if not NULL, will be filled in with full evaluation results + * @return Best command + */ + geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity) override; + + /** + * @brief nav2_core setPlan - Sets the global plan + * @param path The global plan + */ + void setPlan(const nav_msgs::msg::Path & path) override; + +protected: + /** + * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses + * @param pose pose to transform + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Transform a pose to another frame. + * @param frame Frame ID to transform to + * @param in_pose Pose input to transform + * @param out_pose transformed output + * @return bool if successful + */ + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + /** + * @brief Get lookahead distance + * @param cmd the current speed to use to compute lookahead point + * @return lookahead distance + */ + double getLookAheadDistance(const geometry_msgs::msg::Twist &); + + /** + * @brief Creates a PointStamped message for visualization + * @param carrot_pose Input carrot point as a PoseStamped + * @return CarrotMsg a carrot point marker, PointStamped + */ + std::unique_ptr createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Whether robot should rotate to rough path heading + * @param carrot_pose current lookahead point + * @param angle_to_path Angle of robot output relatie to carrot marker + * @return Whether should rotate to path heading + */ + bool shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + + /** + * @brief Whether robot should rotate to final goal orientation + * @param carrot_pose current lookahead point + * @return Whether should rotate to goal heading + */ + bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Create a smooth and kinematically smoothed rotation command + * @param linear_vel linear velocity + * @param angular_vel angular velocity + * @param angle_to_path Angle of robot output relatie to carrot marker + * @param curr_speed the current robot speed + */ + void rotateToHeading(double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); + + /** + * @brief Whether collision is imminent + * @param robot_pose Pose of robot + * @param carrot_pose Pose of carrot + * @param linear_vel linear velocity to forward project + * @param angular_vel angular velocity to forward project + * @return Whether collision is imminent + */ + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const double &, const double &); + + /** + * @brief Whether point is in collision + * @param x Pose of pose x + * @param y Pose of pose y + * @return Whether in collision + */ + bool inCollision(const double & x, const double & y); + + /** + * @brief Cost at a point + * @param x Pose of pose x + * @param y Pose of pose y + * @return Cost of pose in costmap + */ + double costAtPose(const double & x, const double & y); + + /** + * @brief apply regulation constraints to the system + * @param linear_vel robot command linear velocity input + * @param dist_error error in the carrot distance and lookahead distance + * @param lookahead_dist optimal lookahead distance + * @param curvature curvature of path + * @param speed Speed of robot + * @param pose_cost cost at this pose + */ + void applyConstraints( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & speed, + const double & pose_cost, double & linear_vel); + + /** + * @brief Get lookahead point + * @param lookahead_dist Optimal lookahead distance + * @param path Current global path + * @return Lookahead point + */ + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + + std::shared_ptr tf_; + std::string plugin_name_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; + + double desired_linear_vel_; + double lookahead_dist_; + double rotate_to_heading_angular_vel_; + double max_lookahead_dist_; + double min_lookahead_dist_; + double lookahead_time_; + double max_linear_accel_; + double max_linear_decel_; + bool use_velocity_scaled_lookahead_dist_; + tf2::Duration transform_tolerance_; + bool use_approach_vel_scaling_; + double min_approach_linear_velocity_; + double control_duration_; + double max_allowed_time_to_collision_; + bool use_regulated_linear_velocity_scaling_; + bool use_cost_regulated_linear_velocity_scaling_; + double cost_scaling_dist_; + double cost_scaling_gain_; + double inflation_cost_scaling_factor_; + double regulated_linear_scaling_min_radius_; + double regulated_linear_scaling_min_speed_; + bool use_rotate_to_heading_; + double max_angular_accel_; + double rotate_to_heading_min_angle_; + double goal_dist_tol_; + + nav_msgs::msg::Path global_plan_; + std::shared_ptr> global_path_pub_; + std::shared_ptr> carrot_pub_; + std::shared_ptr> carrot_arc_pub_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml new file mode 100644 index 00000000000..d695bcec7f8 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml @@ -0,0 +1,10 @@ + + + + + nav2_regulated_pure_pursuit_controller + + + + + diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml new file mode 100644 index 00000000000..39a894477aa --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -0,0 +1,29 @@ + + + + nav2_regulated_pure_pursuit_controller + 1.0.0 + Regulated Pure Pursuit Controller + Steve Macenski + Shrijit Singh + Apache-2.0 + + ament_cmake + + nav2_common + nav2_core + nav2_util + nav2_costmap_2d + rclcpp + geometry_msgs + nav2_msgs + pluginlib + tf2 + + ament_cmake_gtest + + + ament_cmake + + + diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp new file mode 100644 index 00000000000..f5017ca058c --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -0,0 +1,563 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 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. + +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using std::hypot; +using std::min; +using std::max; +using std::abs; +using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; +using namespace nav2_costmap_2d; // NOLINT + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * Find element in iterator with the minimum calculated value + */ +template +Iter min_by(Iter begin, Iter end, Getter getCompareVal) +{ + if (begin == end) { + return end; + } + auto lowest = getCompareVal(*begin); + Iter lowest_it = begin; + for (Iter it = ++begin; it != end; ++it) { + auto comp = getCompareVal(*it); + if (comp < lowest) { + lowest = comp; + lowest_it = it; + } + } + return lowest_it; +} + +void RegulatedPurePursuitController::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) +{ + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + tf_ = tf; + plugin_name_ = name; + logger_ = node->get_logger(); + + double transform_tolerance = 0.1; + double control_frequency = 20.0; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_approach_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".goal_dist_tol", rclcpp::ParameterValue(0.25)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); + node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); + node->get_parameter(plugin_name_ + ".max_linear_decel", max_linear_decel_); + node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); + node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_); + node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter(plugin_name_ + ".use_velocity_scaled_lookahead_dist", + use_velocity_scaled_lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_approach_linear_velocity", min_approach_linear_velocity_); + node->get_parameter(plugin_name_ + ".use_approach_linear_velocity_scaling", use_approach_vel_scaling_); + node->get_parameter(plugin_name_ + ".max_allowed_time_to_collision", max_allowed_time_to_collision_); + node->get_parameter(plugin_name_ + ".use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_); + node->get_parameter(plugin_name_ + ".inflation_cost_scaling_factor", inflation_cost_scaling_factor_); + node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_); + node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); + node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter(plugin_name_ + ".goal_dist_tol", goal_dist_tol_); + node->get_parameter("controller_frequency", control_frequency); + + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + control_duration_ = 1.0 / control_frequency; + + if (inflation_cost_scaling_factor_ <= 0.0) { + RCLCPP_WARN(logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + use_cost_regulated_linear_velocity_scaling_ = false; + } + + global_path_pub_ = node->create_publisher("received_global_plan", 1); + carrot_pub_ = node->create_publisher("lookahead_point", 1); + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); +} + +void RegulatedPurePursuitController::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up controller: %s of type" + " regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_.reset(); + carrot_pub_.reset(); + carrot_arc_pub_.reset(); +} + +void RegulatedPurePursuitController::activate() +{ + RCLCPP_INFO( + logger_, + "Activating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_activate(); + carrot_pub_->on_activate(); + carrot_arc_pub_->on_activate(); +} + +void RegulatedPurePursuitController::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_deactivate(); + carrot_pub_->on_deactivate(); + carrot_arc_pub_->on_deactivate(); +} + +std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + auto carrot_msg = std::make_unique(); + carrot_msg->header = carrot_pose.header; + carrot_msg->point.x = carrot_pose.pose.position.x; + carrot_msg->point.y = carrot_pose.pose.position.y; + carrot_msg->point.z = 0.01; // publish right over map to stand out + return carrot_msg; +} + +double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) { + lookahead_dist = speed.linear.x * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & speed) +{ + // Transform path to robot base frame + auto transformed_plan = transformGlobalPlan(pose); + + // Find look ahead distance and point on path and publish + const double lookahead_dist = getLookAheadDistance(speed); + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + carrot_pub_->publish(std::move(createCarrotMsg(carrot_pose))); + + double linear_vel, angular_vel; + + // Find distance^2 to look ahead point (carrot) in robot base frame + // This is the chord length of the circle + const double carrot_dist2 = + (carrot_pose.pose.position.x * carrot_pose.pose.position.x) + + (carrot_pose.pose.position.y * carrot_pose.pose.position.y); + + // Find curvature of circle (k = 1 / R) + double curvature = 0.0; + if (carrot_dist2 > 0.001) { + curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; + } + + linear_vel = desired_linear_vel_; + + // Make sure we're in compliance with basic constraints + double angle_to_heading; + if (shouldRotateToGoalHeading(carrot_pose)) { + double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); + } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); + } else { + applyConstraints( + fabs(lookahead_dist - sqrt(carrot_dist2)), + lookahead_dist, curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel); + + // Apply curvature to angular velocity after constraining linear velocity + angular_vel = linear_vel * curvature; + } + + // Collision checking on this velocity heading + if (isCollisionImminent(pose, linear_vel, angular_vel)) { + throw std::runtime_error("RegulatedPurePursuitController detected collision ahead!"); + } + + // populate and return message + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + cmd_vel.twist.linear.x = linear_vel; + cmd_vel.twist.angular.z = angular_vel; + return cmd_vel; +} + +bool RegulatedPurePursuitController::shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) +{ + // Whether we should rotate robot to rough path heading + angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; +} + +bool RegulatedPurePursuitController::shouldRotateToGoalHeading( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + // Whether we should rotate robot to goal heading + double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; +} + +void RegulatedPurePursuitController::rotateToHeading( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) +{ + // Rotate in place using max angular velocity / acceleration possible + linear_vel = 0.0; + const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; + angular_vel = sign * rotate_to_heading_angular_vel_; + + const double & dt = control_duration_; + const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); +} + +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( + const double & lookahead_dist, + const nav_msgs::msg::Path & transformed_plan) +{ + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { + return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist; + }); + + // If the no pose is not far enough, take the last pose + if (goal_pose_it == transformed_plan.poses.end()) { + goal_pose_it = std::prev(transformed_plan.poses.end()); + } + + return *goal_pose_it; +} + +bool RegulatedPurePursuitController::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const double & linear_vel, const double & angular_vel) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. + + // check current point is OK + if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) { + return true; + } + + // visualization messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + const double projection_time = costmap_->getResolution() / linear_vel; + + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + int i = 1; + while (true) { + // only forward simulate within time requested + if (i * projection_time > max_allowed_time_to_collision_) { + break; + } + + i++; + + // apply velocity at curr_pose over distance + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at this point + if (inCollision(curr_pose.x, curr_pose.y)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + + if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { + return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION; + } else { + return cost >= INSCRIBED_INFLATED_OBSTACLE; + } +} + +double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +void RegulatedPurePursuitController::applyConstraints( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, double & linear_vel) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + // limit the linear velocity by curvature + const double radius = fabs(1.0 / curvature); + const double & min_rad = regulated_linear_scaling_min_radius_; + if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + } + + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(NO_INFORMATION) && + pose_cost != static_cast(FREE_SPACE)) + { + const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + } + + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + linear_vel = std::min(cost_vel, curvature_vel); + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop + if (use_approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { + double velocity_scaling = 1.0 - (dist_error / lookahead_dist); + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) { + approach_vel = min_approach_linear_velocity_; + } else { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + linear_vel = std::min(linear_vel, approach_vel); + } + + // Limit linear velocities to be valid and kinematically feasible, v = v0 + a * dt + double & dt = control_duration_; + const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt; + const double min_feasible_linear_speed = curr_speed.linear.x - max_linear_decel_ * dt; + linear_vel = std::clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed); + linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); +} + +void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; +} + +nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::PlannerException("Received plan with zero length"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) + { + throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + } + + // We'll discard points on the plan that are outside the local costmap + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()); + const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0; + + // First find the closest pose on the path to the robot + auto transformation_begin = + min_by( + global_plan_.poses.begin(), global_plan_.poses.end(), + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // Find points definitely outside of the costmap so we won't transform them. + auto transformation_end = std::find_if( + transformation_begin, end(global_plan_.poses), + [&](const auto & global_plan_pose) { + return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + global_path_pub_->publish(transformed_plan); + + if (transformed_plan.poses.empty()) { + throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +bool RegulatedPurePursuitController::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +} // namespace nav2_pure_pursuit_controller + +// Register this controller as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController, + nav2_core::Controller) diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt new file mode 100644 index 00000000000..aee6297fcbb --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# tests for regulated PP +ament_add_gtest(test_regulated_pp + test_regulated_pp.cpp +) +ament_target_dependencies(test_regulated_pp + ${dependencies} +) +target_link_libraries(test_regulated_pp + ${library_name} +) diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp new file mode 100644 index 00000000000..9389b43bff0 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -0,0 +1,324 @@ +// Copyright (c) 2021 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. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController +{ +public: + BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {}; + + nav_msgs::msg::Path getPlan() {return global_plan_;}; + + double getSpeed() {return desired_linear_vel_;}; + + std::unique_ptr createCarrotMsgWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return createCarrotMsg(carrot_pose); + }; + + void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;}; + void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;}; + void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;}; + void resetVelocityApproachScaling() {use_approach_vel_scaling_ = false;}; + + double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) + { + return getLookAheadDistance(twist); + }; + + geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( + const double & dist, const nav_msgs::msg::Path & path) + { + return getLookAheadPoint(dist, path); + }; + + bool shouldRotateToPathWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) + { + return shouldRotateToPath(carrot_pose, angle_to_path); + } + + bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return shouldRotateToGoalHeading(carrot_pose); + } + + void rotateToHeadingWrapper(double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) + { + return rotateToHeading(linear_vel, angular_vel, angle_to_path, curr_speed); + } + + void applyConstraintsWrapper( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, double & linear_vel) + { + return applyConstraints(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + } + +}; + +TEST(RegulatedPurePursuitTest, basicAPI) +{ + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + + // instantiate + auto ctrl = std::make_shared(); + ctrl->configure(node, name, tf, costmap); + ctrl->activate(); + ctrl->deactivate(); + ctrl->cleanup(); + + // setPlan and get plan + nav_msgs::msg::Path path; + path.poses.resize(2); + path.poses[0].header.frame_id = "fake_frame"; + ctrl->setPlan(path); + EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); + EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); +} + +TEST(RegulatedPurePursuitTest, createCarrotMsg) +{ + auto ctrl = std::make_shared(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "Hi!"; + pose.pose.position.x = 1.0; + pose.pose.position.y = 12.0; + pose.pose.orientation.w = 0.5; + + auto rtn = ctrl->createCarrotMsgWrapper(pose); + EXPECT_EQ(rtn->header.frame_id, std::string("Hi!")); + EXPECT_EQ(rtn->point.x, 1.0); + EXPECT_EQ(rtn->point.y, 12.0); + EXPECT_EQ(rtn->point.z, 0.01); +} + +TEST(RegulatedPurePursuitTest, lookaheadAPI) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + geometry_msgs::msg::Twist twist; + + // test getLookAheadDistance + double rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); // default lookahead_dist + + // shouldn't be a function of speed + twist.linear.x = 10.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); + + // now it should be a function of velocity, max out + ctrl->setVelocityScaledLookAhead(); + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.9); // 10 speed maxes out at max_lookahead_dist + + // check normal range + twist.linear.x = 0.35; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_NEAR(rtn, 0.525, 0.0001); // 1.5 * 0.35 + + // check minimum range + twist.linear.x = 0.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.3); + + // test getLookAheadPoint + double dist = 1.0; + nav_msgs::msg::Path path; + path.poses.resize(10); + for (uint i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = static_cast(i); + } + + // test exact hits + auto pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 1.0); + + // test getting next closest point + dist = 3.8; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 4.0); + + // test end of path + dist = 100.0; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 9.0); +} + +TEST(RegulatedPurePursuitTest, rotateTests) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + // shouldRotateToPath + geometry_msgs::msg::PoseStamped carrot; + double angle_to_path_rtn; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 0.25; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 1.0; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), true); + + // shouldRotateToGoalHeading + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.0; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.24; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + + // rotateToHeading + double lin_v = 10.0; + double ang_v = 0.5; + double angle_to_path = 0.4; + geometry_msgs::msg::Twist curr_speed; + curr_speed.angular.z = 1.75; + + // basic full speed at a speed + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(lin_v, 0.0); + EXPECT_EQ(ang_v, 1.8); + + // negative direction + angle_to_path = -0.4; + curr_speed.angular.z = -1.75; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(ang_v, -1.8); + + // kinematic clamping, no speed, some speed accelerating, some speed decelerating + angle_to_path = 0.4; + curr_speed.angular.z = 0.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.16, 0.01); + + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 1.16, 0.01); + + angle_to_path = -0.4; + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.84, 0.01); +} + +TEST(RegulatedPurePursuitTest, applyConstraints) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + double dist_error = 0.0; + double lookahead_dist = 0.6; + double curvature = 0.5; + geometry_msgs::msg::Twist curr_speed; + double pose_cost = 0.0; + double linear_vel = 0.0; + + // since costmaps here are bogus, we can't access them + ctrl->resetVelocityApproachScaling(); + + // test curvature regulation (default) + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_EQ(linear_vel, 0.25); // min set speed + + linear_vel = 1.0; + curvature = 0.7407; + curr_speed.linear.x = 0.5; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature + + linear_vel = 1.0; + curvature = 1000.0; + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature + + + // now try with cost regulation (turn off velocity and only cost) + // ctrl->setCostRegulationScaling(); + // ctrl->resetVelocityRegulationScaling(); + // curvature = 0.0; + + // min changable cost + // pose_cost = 1; + // linear_vel = 0.5; + // curr_speed.linear.x = 0.5; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.498, 0.01); + + // max changing cost + // pose_cost = 127; + // curr_speed.linear.x = 0.255; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.255, 0.01); + + // over max cost thresh + // pose_cost = 200; + // curr_speed.linear.x = 0.25; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.25, 0.01); + + // test kinematic clamping + // pose_cost = 200; + // curr_speed.linear.x = 1.0; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.5, 0.01); +} diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 396eafd4fa3..e227489171a 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -117,7 +117,7 @@ Nav2Panel::Nav2Panel(QWidget * parent) // State entered when navigate_to_pose action is not active accumulating_ = new QState(); accumulating_->setObjectName("accumulating"); - accumulating_->assignProperty(start_reset_button_, "text", "Reset"); + accumulating_->assignProperty(start_reset_button_, "text", "Cancel Waypoint Mode"); accumulating_->assignProperty(start_reset_button_, "toolTip", cancel_waypoint_msg); accumulating_->assignProperty(start_reset_button_, "enabled", true); @@ -130,6 +130,18 @@ Nav2Panel::Nav2Panel(QWidget * parent) accumulating_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); accumulated_ = new QState(); + accumulated_->setObjectName("accumulated"); + accumulated_->assignProperty(start_reset_button_, "text", "Cancel"); + accumulated_->assignProperty(start_reset_button_, "toolTip", cancel_msg); + accumulated_->assignProperty(start_reset_button_, "enabled", true); + + accumulated_->assignProperty(pause_resume_button_, "text", "Pause"); + accumulated_->assignProperty(pause_resume_button_, "enabled", false); + accumulated_->assignProperty(pause_resume_button_, "toolTip", pause_msg); + + accumulated_->assignProperty(navigation_mode_button_, "text", "Start Navigation"); + accumulated_->assignProperty(navigation_mode_button_, "enabled", false); + accumulated_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); // State entered to cancel the navigate_to_pose action canceled_ = new QState(); @@ -185,12 +197,12 @@ Nav2Panel::Nav2Panel(QWidget * parent) idle_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulating_); accumulating_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulated_); accumulating_->addTransition(start_reset_button_, SIGNAL(clicked()), idle_); + accumulated_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_); // Internal state transitions canceled_->addTransition(canceled_, SIGNAL(entered()), idle_); reset_->addTransition(reset_, SIGNAL(entered()), initial_); resumed_->addTransition(resumed_, SIGNAL(entered()), idle_); - accumulated_->addTransition(accumulated_, SIGNAL(entered()), idle_); // Pause/Resume button click transitions idle_->addTransition(pause_resume_button_, SIGNAL(clicked()), paused_); @@ -205,6 +217,15 @@ Nav2Panel::Nav2Panel(QWidget * parent) runningTransition->setTargetState(idle_); running_->addTransition(runningTransition); + ROSActionQTransition * idleAccumulatedTransition = + new ROSActionQTransition(QActionState::INACTIVE); + idleAccumulatedTransition->setTargetState(accumulated_); + idle_->addTransition(idleAccumulatedTransition); + + ROSActionQTransition * accumulatedTransition = new ROSActionQTransition(QActionState::ACTIVE); + accumulatedTransition->setTargetState(idle_); + accumulated_->addTransition(accumulatedTransition); + initial_thread_ = new InitialThread(client_nav_, client_loc_); connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater); @@ -407,7 +428,7 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) void Nav2Panel::onCancelButtonPressed() { - if (state_machine_.configuration().contains(accumulating_)) { + if (waypoint_follower_goal_handle_) { auto future_cancel = waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); @@ -417,7 +438,9 @@ Nav2Panel::onCancelButtonPressed() RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); return; } - } else { + } + + if (navigation_goal_handle_) { auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != @@ -448,7 +471,7 @@ Nav2Panel::onAccumulating() void Nav2Panel::timerEvent(QTimerEvent * event) { - if (state_machine_.configuration().contains(accumulating_)) { + if (state_machine_.configuration().contains(accumulated_)) { if (event->timerId() == timer_.timerId()) { if (!waypoint_follower_goal_handle_) { RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 7c07bb08ae6..27e1412add2 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -59,6 +59,7 @@ launch launch_ros launch_testing + python3-zmq ament_cmake diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index a7ae2aa43fa..b176fcddfc3 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -12,7 +12,9 @@ ament_add_test(test_bt_navigator TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=False + ASTAR=True + CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_bt_navigator_with_dijkstra @@ -27,6 +29,25 @@ ament_add_test(test_bt_navigator_with_dijkstra GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner +) + +ament_add_test(test_bt_navigator_with_groot_monitoring + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + ASTAR=False + GROOT_MONITORING=True + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_dynamic_obstacle @@ -41,6 +62,8 @@ ament_add_test(test_dynamic_obstacle GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) # ament_add_test(test_multi_robot @@ -55,4 +78,6 @@ ament_add_test(test_dynamic_obstacle # TEST_URDF=${PROJECT_SOURCE_DIR}/urdf/turtlebot3_waffle.urdf # TEST_SDF=${PROJECT_SOURCE_DIR}/models/turtlebot3_waffle/model.sdf # BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +# CONTROLLER=dwb_core::DWBLocalPlanner +# PLANNER=nav2_navfn_planner/NavfnPlanner # ) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 92ea252eba0..8a15b9a63c7 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 # Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Florian Gramss # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -22,6 +23,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import 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 @@ -40,15 +42,30 @@ def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') - # Replace the `use_astar` setting on the params file - param_substitutions = { - 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR')} + # 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'}) + + if (os.getenv('GROOT_MONITORING') == 'True'): + param_substitutions.update({'enable_groot_monitoring': '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')}) + 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_CONSOLE_STDOUT_LINE_BUFFERED', '1'), @@ -79,7 +96,7 @@ def generate_launch_description(): 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', - 'params_file': configured_params, + 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py index 83e9df9dc9f..d84724ea5c8 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/tester_node.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 # Copyright 2018 Intel Corporation. +# Copyright 2020 Florian Gramss # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -15,8 +16,10 @@ import argparse import math +import os import sys import time + from typing import Optional from action_msgs.msg import GoalStatus @@ -34,6 +37,8 @@ from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile +import zmq + class NavTester(Node): @@ -94,6 +99,13 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'NavigateToPose' action server not available, waiting...") + if (os.getenv('GROOT_MONITORING') == 'True'): + if self.grootMonitoringGetStatus(): + self.error_msg('Behavior Tree must not be running already!') + self.error_msg('Are you running multiple goals/bts..?') + return False + self.info_msg('This Error above MUST Fail and is o.k.!') + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) @@ -111,6 +123,19 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.info_msg('Goal accepted') get_result_future = goal_handle.get_result_async() + future_return = True + if (os.getenv('GROOT_MONITORING') == 'True'): + try: + if not self.grootMonitoringReloadTree(): + self.error_msg('Failed GROOT_BT - Reload Tree from ZMQ Server') + future_return = False + if not self.grootMonitoringGetStatus(): + self.error_msg('Failed GROOT_BT - Get Status from ZMQ Publisher') + future_return = False + except Exception as e: + self.error_msg('Failed GROOT_BT - ZMQ Tests: ' + e.__doc__ + e.message) + future_return = False + self.info_msg("Waiting for 'NavigateToPose' action to complete") rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status @@ -118,9 +143,81 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.info_msg('Goal failed with status code: {0}'.format(status)) return False + if not future_return: + return False + self.info_msg('Goal succeeded!') return True + def grootMonitoringReloadTree(self): + # ZeroMQ Context + context = zmq.Context() + + sock = context.socket(zmq.REQ) + port = 1667 # default server port for groot monitoring + # # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 1000) + # sock.setsockopt(zmq.LINGER, 0) + + sock.connect('tcp://localhost:' + str(port)) + self.info_msg('ZMQ Server Port: ' + str(port)) + + # this should fail + try: + sock.recv() + self.error_msg('ZMQ Reload Tree Test 1/3 - This should have failed!') + # Only works when ZMQ server receives a request first + sock.close() + return False + except zmq.error.ZMQError: + self.info_msg('ZMQ Reload Tree Test 1/3: Check') + try: + # request tree from server + sock.send_string('') + # receive tree from server as flat_buffer + sock.recv() + self.info_msg('ZMQ Reload Tree Test 2/3: Check') + except zmq.error.Again: + self.info_msg('ZMQ Reload Tree Test 2/3 - Failed to load tree') + sock.close() + return False + + # this should fail + try: + sock.recv() + self.error_msg('ZMQ Reload Tree Test 3/3 - This should have failed!') + # Tree should only be loadable ONCE after ZMQ server received a request + sock.close() + return False + except zmq.error.ZMQError: + self.info_msg('ZMQ Reload Tree Test 3/3: Check') + + return True + + def grootMonitoringGetStatus(self): + # ZeroMQ Context + context = zmq.Context() + # Define the socket using the 'Context' + sock = context.socket(zmq.SUB) + # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 2000) + # sock.setsockopt(zmq.LINGER, 0) + + # Define subscription and messages with prefix to accept. + sock.setsockopt_string(zmq.SUBSCRIBE, '') + port = 1666 # default publishing port for groot monitoring + sock.connect('tcp://127.0.0.1:' + str(port)) + + for request in range(3): + try: + sock.recv() + except zmq.error.Again: + self.error_msg('ZMQ - Did not receive any status') + sock.close() + return False + self.info_msg('ZMQ - Did receive status') + return True + def poseCallback(self, msg): self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose diff --git a/nav2_system_tests/src/updown/CMakeLists.txt b/nav2_system_tests/src/updown/CMakeLists.txt index a56bff34b23..273e1828ea5 100644 --- a/nav2_system_tests/src/updown/CMakeLists.txt +++ b/nav2_system_tests/src/updown/CMakeLists.txt @@ -6,6 +6,6 @@ ament_target_dependencies(test_updown ${dependencies} ) -install(TARGETS test_updown RUNTIME DESTINATION share/${PROJECT_NAME}) +install(TARGETS test_updown RUNTIME DESTINATION lib/${PROJECT_NAME}) install(FILES test_updown_launch.py DESTINATION share/${PROJECT_NAME}) diff --git a/navigation2/package.xml b/navigation2/package.xml index 7d0558bfda1..5169c4471f3 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -31,6 +31,7 @@ nav2_controller nav2_waypoint_follower smac_planner + nav2_regulated_pure_pursuit_controller ament_cmake diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/smac_planner/include/smac_planner/costmap_downsampler.hpp index 6f23c696e21..bdfb5cdf33c 100644 --- a/smac_planner/include/smac_planner/costmap_downsampler.hpp +++ b/smac_planner/include/smac_planner/costmap_downsampler.hpp @@ -34,9 +34,8 @@ class CostmapDownsampler public: /** * @brief A constructor for CostmapDownsampler - * @param node Lifecycle node pointer */ - explicit CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node); + CostmapDownsampler(); /** * @brief A destructor for CostmapDownsampler @@ -44,13 +43,15 @@ class CostmapDownsampler ~CostmapDownsampler(); /** - * @brief Initialize the downsampled costmap object and the ROS publisher + * @brief Configure the downsampled costmap object and the ROS publisher + * @param node Lifecycle node pointer * @param global_frame The ID of the global frame used by the costmap * @param topic_name The name of the topic to publish the downsampled costmap * @param costmap The costmap we want to downsample * @param downsampling_factor Multiplier for the costmap resolution */ - void initialize( + void on_configure( + const nav2_util::LifecycleNode::SharedPtr & node, const std::string & global_frame, const std::string & topic_name, nav2_costmap_2d::Costmap2D * const costmap, @@ -59,18 +60,17 @@ class CostmapDownsampler /** * @brief Activate the publisher of the downsampled costmap */ - void activatePublisher() - { - _downsampled_costmap_pub->on_activate(); - } + void on_activate(); /** * @brief Deactivate the publisher of the downsampled costmap */ - void deactivatePublisher() - { - _downsampled_costmap_pub->on_deactivate(); - } + void on_deactivate(); + + /** + * @brief Cleanup the publisher of the downsampled costmap + */ + void on_cleanup(); /** * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap @@ -105,8 +105,6 @@ class CostmapDownsampler unsigned int _downsampled_size_y; unsigned int _downsampling_factor; float _downsampled_resolution; - std::string _topic_name; - nav2_util::LifecycleNode::SharedPtr _node; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _downsampled_costmap; std::unique_ptr _downsampled_costmap_pub; diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp index 379f0c92a5b..347bd562c92 100644 --- a/smac_planner/include/smac_planner/smac_planner.hpp +++ b/smac_planner/include/smac_planner/smac_planner.hpp @@ -111,7 +111,8 @@ class SmacPlanner : public nav2_core::GlobalPlanner protected: std::unique_ptr> _a_star; std::unique_ptr _smoother; - nav2_util::LifecycleNode::SharedPtr _node; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner")}; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; std::string _global_frame, _name; diff --git a/smac_planner/include/smac_planner/smac_planner_2d.hpp b/smac_planner/include/smac_planner/smac_planner_2d.hpp index 6d89887c3d3..7cf18a673c4 100644 --- a/smac_planner/include/smac_planner/smac_planner_2d.hpp +++ b/smac_planner/include/smac_planner/smac_planner_2d.hpp @@ -106,7 +106,8 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner std::unique_ptr _smoother; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; - nav2_util::LifecycleNode::SharedPtr _node; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner2D")}; std::string _global_frame, _name; float _tolerance; int _downsampling_factor; diff --git a/smac_planner/src/costmap_downsampler.cpp b/smac_planner/src/costmap_downsampler.cpp index 6955469831a..025a30b351a 100644 --- a/smac_planner/src/costmap_downsampler.cpp +++ b/smac_planner/src/costmap_downsampler.cpp @@ -22,9 +22,8 @@ namespace smac_planner { -CostmapDownsampler::CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node) -: _node(node), - _costmap(nullptr), +CostmapDownsampler::CostmapDownsampler() +: _costmap(nullptr), _downsampled_costmap(nullptr), _downsampled_costmap_pub(nullptr) { @@ -34,13 +33,13 @@ CostmapDownsampler::~CostmapDownsampler() { } -void CostmapDownsampler::initialize( +void CostmapDownsampler::on_configure( + const nav2_util::LifecycleNode::SharedPtr & node, const std::string & global_frame, const std::string & topic_name, nav2_costmap_2d::Costmap2D * const costmap, const unsigned int & downsampling_factor) { - _topic_name = topic_name; _costmap = costmap; _downsampling_factor = downsampling_factor; updateCostmapSize(); @@ -50,7 +49,24 @@ void CostmapDownsampler::initialize( _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); _downsampled_costmap_pub = std::make_unique( - _node, _downsampled_costmap.get(), global_frame, _topic_name, false); + node, _downsampled_costmap.get(), global_frame, topic_name, false); +} + +void CostmapDownsampler::on_activate() +{ + _downsampled_costmap_pub->on_activate(); +} + +void CostmapDownsampler::on_deactivate() +{ + _downsampled_costmap_pub->on_deactivate(); +} + +void CostmapDownsampler::on_cleanup() +{ + _costmap = nullptr; + _downsampled_costmap.reset(); + _downsampled_costmap_pub.reset(); } nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( @@ -74,10 +90,7 @@ nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( } } - if (_node->count_subscribers(_topic_name) > 0) { - _downsampled_costmap_pub->publishCostmap(); - } - + _downsampled_costmap_pub->publishCostmap(); return _downsampled_costmap.get(); } diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp index f3db10830ad..71ba1d3333a 100644 --- a/smac_planner/src/smac_planner.cpp +++ b/smac_planner/src/smac_planner.cpp @@ -31,7 +31,6 @@ using namespace std::chrono; // NOLINT SmacPlanner::SmacPlanner() : _a_star(nullptr), _smoother(nullptr), - _node(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) { @@ -40,16 +39,17 @@ SmacPlanner::SmacPlanner() SmacPlanner::~SmacPlanner() { RCLCPP_INFO( - _node->get_logger(), "Destroying plugin %s of type SmacPlanner", + _logger, "Destroying plugin %s of type SmacPlanner", _name.c_str()); } void SmacPlanner::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_ros) { - _node = parent; + _logger = node->get_logger(); + _clock = node->get_clock(); _costmap = costmap_ros->getCostmap(); _name = name; _global_frame = costmap_ros->getGlobalFrameID(); @@ -64,62 +64,62 @@ void SmacPlanner::configure( // General planner params nav2_util::declare_parameter_if_not_declared( - _node, name + ".tolerance", rclcpp::ParameterValue(0.125)); - _tolerance = static_cast(_node->get_parameter(name + ".tolerance").as_double()); + node, name + ".tolerance", rclcpp::ParameterValue(0.125)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( - _node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); - _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + node, name + ".downsample_costmap", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".downsample_costmap", _downsample_costmap); nav2_util::declare_parameter_if_not_declared( - _node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); - _node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".downsampling_factor", _downsampling_factor); nav2_util::declare_parameter_if_not_declared( - _node, name + ".angle_quantization_bins", rclcpp::ParameterValue(1)); - _node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); + node, name + ".angle_quantization_bins", rclcpp::ParameterValue(72)); + node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); nav2_util::declare_parameter_if_not_declared( - _node, name + ".allow_unknown", rclcpp::ParameterValue(true)); - _node->get_parameter(name + ".allow_unknown", allow_unknown); + node, name + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".allow_unknown", allow_unknown); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_iterations", rclcpp::ParameterValue(-1)); - _node->get_parameter(name + ".max_iterations", max_iterations); + node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + node->get_parameter(name + ".max_iterations", max_iterations); nav2_util::declare_parameter_if_not_declared( - _node, name + ".smooth_path", rclcpp::ParameterValue(false)); - _node->get_parameter(name + ".smooth_path", smooth_path); + node, name + ".smooth_path", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".smooth_path", smooth_path); nav2_util::declare_parameter_if_not_declared( - _node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); - _node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); nav2_util::declare_parameter_if_not_declared( - _node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); - _node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); + node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); nav2_util::declare_parameter_if_not_declared( - _node, name + ".change_penalty", rclcpp::ParameterValue(0.5)); - _node->get_parameter(name + ".change_penalty", search_info.change_penalty); + node, name + ".change_penalty", rclcpp::ParameterValue(0.5)); + node->get_parameter(name + ".change_penalty", search_info.change_penalty); nav2_util::declare_parameter_if_not_declared( - _node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05)); - _node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); + node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05)); + node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); nav2_util::declare_parameter_if_not_declared( - _node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); - _node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); + node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); + node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); nav2_util::declare_parameter_if_not_declared( - _node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); - _node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); + node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0)); - _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0)); + node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); nav2_util::declare_parameter_if_not_declared( - _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); - _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); + node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); MotionModel motion_model = fromString(motion_model_for_search); if (motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( - _node->get_logger(), + _logger, "Unable to get MotionModel search type. Given '%s', " "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", motion_model_for_search.c_str()); @@ -127,14 +127,14 @@ void SmacPlanner::configure( if (max_on_approach_iterations <= 0) { RCLCPP_INFO( - _node->get_logger(), "On approach iteration selected as <= 0, " + _logger, "On approach iteration selected as <= 0, " "disabling tolerance and on approach iterations."); max_on_approach_iterations = std::numeric_limits::max(); } if (max_iterations <= 0) { RCLCPP_INFO( - _node->get_logger(), "maximum iteration selected as <= 0, " + _logger, "maximum iteration selected as <= 0, " "disabling maximum iterations."); max_iterations = std::numeric_limits::max(); } @@ -153,22 +153,23 @@ void SmacPlanner::configure( if (smooth_path) { _smoother = std::make_unique(); - _optimizer_params.get(_node.get(), name); - _smoother_params.get(_node.get(), name); + _optimizer_params.get(node.get(), name); + _smoother_params.get(node.get(), name); _smoother_params.max_curvature = 1.0f / minimum_turning_radius_global_coords; _smoother->initialize(_optimizer_params); } if (_downsample_costmap && _downsampling_factor > 1) { std::string topic_name = "downsampled_costmap"; - _costmap_downsampler = std::make_unique(_node); - _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); } - _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( - _node->get_logger(), "Configured plugin %s of type SmacPlanner with " + _logger, "Configured plugin %s of type SmacPlanner with " "tolerance %.2f, maximum iterations %i, " "max on approach iterations %i, and %s. Using motion model: %s.", _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, @@ -179,32 +180,33 @@ void SmacPlanner::configure( void SmacPlanner::activate() { RCLCPP_INFO( - _node->get_logger(), "Activating plugin %s of type SmacPlanner", + _logger, "Activating plugin %s of type SmacPlanner", _name.c_str()); _raw_plan_publisher->on_activate(); if (_costmap_downsampler) { - _costmap_downsampler->activatePublisher(); + _costmap_downsampler->on_activate(); } } void SmacPlanner::deactivate() { RCLCPP_INFO( - _node->get_logger(), "Deactivating plugin %s of type SmacPlanner", + _logger, "Deactivating plugin %s of type SmacPlanner", _name.c_str()); _raw_plan_publisher->on_deactivate(); if (_costmap_downsampler) { - _costmap_downsampler->deactivatePublisher(); + _costmap_downsampler->on_deactivate(); } } void SmacPlanner::cleanup() { RCLCPP_INFO( - _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner", + _logger, "Cleaning up plugin %s of type SmacPlanner", _name.c_str()); _a_star.reset(); _smoother.reset(); + _costmap_downsampler->on_cleanup(); _costmap_downsampler.reset(); _raw_plan_publisher.reset(); } @@ -251,7 +253,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( // Setup message nav_msgs::msg::Path plan; - plan.header.stamp = _node->now(); + plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; pose.header = plan.header; @@ -282,7 +284,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( if (!error.empty()) { RCLCPP_WARN( - _node->get_logger(), + _logger, "%s: failed to create plan, %s.", _name.c_str(), error.c_str()); return plan; @@ -304,7 +306,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( } // Publish raw path for debug - if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) { + if (_raw_plan_publisher->get_subscription_count() > 0) { _raw_plan_publisher->publish(plan); } @@ -328,7 +330,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( // Smooth plan if (!_smoother->smooth(path_world, costmap, _smoother_params)) { RCLCPP_WARN( - _node->get_logger(), + _logger, "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", _name.c_str()); return plan; diff --git a/smac_planner/src/smac_planner_2d.cpp b/smac_planner/src/smac_planner_2d.cpp index 3c4b8ca83a1..643ffe41a69 100644 --- a/smac_planner/src/smac_planner_2d.cpp +++ b/smac_planner/src/smac_planner_2d.cpp @@ -30,24 +30,24 @@ SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), _smoother(nullptr), _costmap(nullptr), - _costmap_downsampler(nullptr), - _node(nullptr) + _costmap_downsampler(nullptr) { } SmacPlanner2D::~SmacPlanner2D() { RCLCPP_INFO( - _node->get_logger(), "Destroying plugin %s of type SmacPlanner2D", + _logger, "Destroying plugin %s of type SmacPlanner2D", _name.c_str()); } void SmacPlanner2D::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_ros) { - _node = parent; + _logger = node->get_logger(); + _clock = node->get_clock(); _costmap = costmap_ros->getCostmap(); _name = name; _global_frame = costmap_ros->getGlobalFrameID(); @@ -61,42 +61,42 @@ void SmacPlanner2D::configure( // General planner params nav2_util::declare_parameter_if_not_declared( - _node, name + ".tolerance", rclcpp::ParameterValue(0.125)); - _tolerance = static_cast(_node->get_parameter(name + ".tolerance").as_double()); + node, name + ".tolerance", rclcpp::ParameterValue(0.125)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( - _node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); - _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".downsample_costmap", _downsample_costmap); nav2_util::declare_parameter_if_not_declared( - _node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); - _node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".downsampling_factor", _downsampling_factor); nav2_util::declare_parameter_if_not_declared( - _node, name + ".allow_unknown", rclcpp::ParameterValue(true)); - _node->get_parameter(name + ".allow_unknown", allow_unknown); + node, name + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".allow_unknown", allow_unknown); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_iterations", rclcpp::ParameterValue(-1)); - _node->get_parameter(name + ".max_iterations", max_iterations); + node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + node->get_parameter(name + ".max_iterations", max_iterations); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); - _node->get_parameter(name + ".max_on_approach_iterations", max_on_approach_iterations); + node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(name + ".max_on_approach_iterations", max_on_approach_iterations); nav2_util::declare_parameter_if_not_declared( - _node, name + ".smooth_path", rclcpp::ParameterValue(false)); - _node->get_parameter(name + ".smooth_path", smooth_path); + node, name + ".smooth_path", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".smooth_path", smooth_path); nav2_util::declare_parameter_if_not_declared( - _node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); - _node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(1000.0)); - _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + node, name + ".max_planning_time_ms", rclcpp::ParameterValue(1000.0)); + node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); nav2_util::declare_parameter_if_not_declared( - _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); - _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); + node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); MotionModel motion_model = fromString(motion_model_for_search); if (motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( - _node->get_logger(), + _logger, "Unable to get MotionModel search type. Given '%s', " "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", motion_model_for_search.c_str()); @@ -104,14 +104,14 @@ void SmacPlanner2D::configure( if (max_on_approach_iterations <= 0) { RCLCPP_INFO( - _node->get_logger(), "On approach iteration selected as <= 0, " + _logger, "On approach iteration selected as <= 0, " "disabling tolerance and on approach iterations."); max_on_approach_iterations = std::numeric_limits::max(); } if (max_iterations <= 0) { RCLCPP_INFO( - _node->get_logger(), "maximum iteration selected as <= 0, " + _logger, "maximum iteration selected as <= 0, " "disabling maximum iterations."); max_iterations = std::numeric_limits::max(); } @@ -124,22 +124,23 @@ void SmacPlanner2D::configure( if (smooth_path) { _smoother = std::make_unique(); - _optimizer_params.get(_node.get(), name); - _smoother_params.get(_node.get(), name); + _optimizer_params.get(node.get(), name); + _smoother_params.get(node.get(), name); _smoother_params.max_curvature = 1.0f / minimum_turning_radius; _smoother->initialize(_optimizer_params); } if (_downsample_costmap && _downsampling_factor > 1) { std::string topic_name = "downsampled_costmap"; - _costmap_downsampler = std::make_unique(_node); - _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); } - _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( - _node->get_logger(), "Configured plugin %s of type SmacPlanner2D with " + _logger, "Configured plugin %s of type SmacPlanner2D with " "tolerance %.2f, maximum iterations %i, " "max on approach iterations %i, and %s. Using motion model: %s.", _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, @@ -150,32 +151,33 @@ void SmacPlanner2D::configure( void SmacPlanner2D::activate() { RCLCPP_INFO( - _node->get_logger(), "Activating plugin %s of type SmacPlanner2D", + _logger, "Activating plugin %s of type SmacPlanner2D", _name.c_str()); _raw_plan_publisher->on_activate(); if (_costmap_downsampler) { - _costmap_downsampler->activatePublisher(); + _costmap_downsampler->on_activate(); } } void SmacPlanner2D::deactivate() { RCLCPP_INFO( - _node->get_logger(), "Deactivating plugin %s of type SmacPlanner2D", + _logger, "Deactivating plugin %s of type SmacPlanner2D", _name.c_str()); _raw_plan_publisher->on_deactivate(); if (_costmap_downsampler) { - _costmap_downsampler->deactivatePublisher(); + _costmap_downsampler->on_deactivate(); } } void SmacPlanner2D::cleanup() { RCLCPP_INFO( - _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner2D", + _logger, "Cleaning up plugin %s of type SmacPlanner2D", _name.c_str()); _a_star.reset(); _smoother.reset(); + _costmap_downsampler->on_cleanup(); _costmap_downsampler.reset(); _raw_plan_publisher.reset(); } @@ -212,7 +214,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Setup message nav_msgs::msg::Path plan; - plan.header.stamp = _node->now(); + plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; pose.header = plan.header; @@ -243,7 +245,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( if (!error.empty()) { RCLCPP_WARN( - _node->get_logger(), + _logger, "%s: failed to create plan, %s.", _name.c_str(), error.c_str()); return plan; @@ -268,7 +270,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( } // Publish raw path for debug - if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) { + if (_raw_plan_publisher->get_subscription_count() > 0) { _raw_plan_publisher->publish(plan); } @@ -292,7 +294,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Smooth plan if (!_smoother->smooth(path_world, costmap, _smoother_params)) { RCLCPP_WARN( - _node->get_logger(), + _logger, "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", _name.c_str()); return plan; diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/smac_planner/test/test_costmap_downsampler.cpp index 040a2eb760c..5353dcb3871 100644 --- a/smac_planner/test/test_costmap_downsampler.cpp +++ b/smac_planner/test/test_costmap_downsampler.cpp @@ -13,13 +13,11 @@ // limitations under the License. Reserved. #include -#include #include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "smac_planner/costmap_downsampler.hpp" @@ -35,8 +33,7 @@ TEST(CostmapDownsampler, costmap_downsample_test) { nav2_util::LifecycleNode::SharedPtr node = std::make_shared( "CostmapDownsamplerTest"); - smac_planner::CostmapDownsampler downsampler(node); - auto map_sub = nav2_costmap_2d::CostmapSubscriber(node, "unused_topic"); + smac_planner::CostmapDownsampler downsampler; // create basic costmap nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); @@ -44,7 +41,7 @@ TEST(CostmapDownsampler, costmap_downsample_test) costmapA.setCost(5, 5, 50); // downsample it - downsampler.initialize("map", "unused_topic", &costmapA, 2); + downsampler.on_configure(node, "map", "unused_topic", &costmapA, 2); nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(2); // validate it @@ -57,10 +54,10 @@ TEST(CostmapDownsampler, costmap_downsample_test) nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0); // downsample it - downsampler.initialize("map", "unused_topic", &costmapB, 4); - downsampler.activatePublisher(); + downsampler.on_configure(node, "map", "unused_topic", &costmapB, 4); + downsampler.on_activate(); nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(4); - downsampler.deactivatePublisher(); + downsampler.on_deactivate(); // validate size EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u); From fcdc1d83fadc6ea19589f464486a27c6a70820e7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 5 Apr 2021 16:13:04 -0700 Subject: [PATCH 19/23] bumping to 0.4.6 --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_bringup/bringup/package.xml | 2 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_recoveries/package.xml | 2 +- nav2_regulated_pure_pursuit_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- smac_planner/package.xml | 2 +- 31 files changed, 31 insertions(+), 31 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index f22e3a42f4e..1b64b6a6943 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.5 + 0.4.6

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 860a6990343..5ab4e0387b9 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.5 + 0.4.6 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index 0a16ca2707c..65e442c5b7f 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.4.5 + 0.4.6 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index 0032a5b847b..8b14fba65eb 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.4.5 + 0.4.6 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 950ac6d9e6f..e8fa15c88ee 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.5 + 0.4.6 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 28e36f559a4..5c98734e64a 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.5 + 0.4.6 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 93c2993f51b..ebf32bd70f3 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.5 + 0.4.6 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 2b335e013a1..7746a68f021 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.5 + 0.4.6 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index a4f899f9ee8..55418b7f17e 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.5 + 0.4.6 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index f25f807f442..86b1e01d141 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.5 + 0.4.6 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 0d77dac1559..46a8771375c 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.5 + 0.4.6 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index a0e474c7284..bf3ab3e3961 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.5 + 0.4.6 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index c0ee55637c6..48ab8143102 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.5 + 0.4.6 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 348997568ee..befb079cf26 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.5 + 0.4.6 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index ba7901850c6..20efeab7cab 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.5 + 0.4.6 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 291e7963fdb..3f222dacc90 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.5 + 0.4.6 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index b3e6b16f439..65091db8950 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.5 + 0.4.6 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 3eec000ae73..dcfe2e86e63 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.5 + 0.4.6 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index c304ae5b5af..c22d79c42ff 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.5 + 0.4.6 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 979223f7adf..e5404314fda 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.5 + 0.4.6 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index ce103f0a2bc..5b65d408663 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.5 + 0.4.6 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 1e6d33f02b7..dcb2065e313 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.5 + 0.4.6 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index a6c7a5aa0b1..722659e98e3 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.5 + 0.4.6 TODO Carlos Orduno Steve Macenski diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 39a894477aa..5357c9dda85 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.0.0 + 0.4.6 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index e27a8aaec5d..9352a853a8d 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.5 + 0.4.6 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 27e1412add2..5ee6f815be8 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.5 + 0.4.6 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 5f443d0e0d8..19a6d18c6e4 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.5 + 0.4.6 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 29cdcc7fcbb..4f2fd329b1d 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.5 + 0.4.6 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 676846b4d67..3b07a61a1af 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.5 + 0.4.6 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 5169c4471f3..05eb0e0c95b 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.5 + 0.4.6 ROS2 Navigation Stack diff --git a/smac_planner/package.xml b/smac_planner/package.xml index 14a04772e64..ca96ed5cfa3 100644 --- a/smac_planner/package.xml +++ b/smac_planner/package.xml @@ -2,7 +2,7 @@ smac_planner - 0.4.5 + 0.4.6 Smac global planning plugin Steve Macenski Apache-2.0 From a947ab5656e46ae3aa1afdf95e1706a16e478974 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 5 Apr 2021 18:15:13 -0700 Subject: [PATCH 20/23] bump to 0.4.7 and add missing dep to RPP --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_bringup/bringup/package.xml | 2 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_recoveries/package.xml | 2 +- nav2_regulated_pure_pursuit_controller/package.xml | 4 +++- nav2_rviz_plugins/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- smac_planner/package.xml | 2 +- 31 files changed, 33 insertions(+), 31 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 1b64b6a6943..ff27677865c 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.6 + 0.4.7

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 5ab4e0387b9..604c3b5c288 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.6 + 0.4.7 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index 65e442c5b7f..d34bd54d0af 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.4.6 + 0.4.7 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index 8b14fba65eb..c69941fb2a9 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.4.6 + 0.4.7 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index e8fa15c88ee..35280452472 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.6 + 0.4.7 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 5c98734e64a..753275f3bda 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.6 + 0.4.7 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index ebf32bd70f3..0e38b5994da 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.6 + 0.4.7 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 7746a68f021..abe8c478702 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.6 + 0.4.7 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 55418b7f17e..56881189234 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.6 + 0.4.7 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 86b1e01d141..23a2029c47d 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.6 + 0.4.7 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 46a8771375c..fbb064d0705 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.6 + 0.4.7 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index bf3ab3e3961..6b121ecd507 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.6 + 0.4.7 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 48ab8143102..057b2933359 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.6 + 0.4.7 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index befb079cf26..f425fc26aa7 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.6 + 0.4.7 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 20efeab7cab..1ae96ea4028 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.6 + 0.4.7 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 3f222dacc90..3579746eaf0 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.6 + 0.4.7 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 65091db8950..75d5cda8bbb 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.6 + 0.4.7 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index dcfe2e86e63..d56290206e6 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.6 + 0.4.7 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index c22d79c42ff..c66428fb35d 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.6 + 0.4.7 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index e5404314fda..6d2a6f2f356 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.6 + 0.4.7 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 5b65d408663..bf52cb0e117 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.6 + 0.4.7 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index dcb2065e313..e762c67d0e7 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.6 + 0.4.7 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 722659e98e3..62f2f79540c 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.6 + 0.4.7 TODO Carlos Orduno Steve Macenski diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 5357c9dda85..14c4e1c1d91 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 0.4.6 + 0.4.7 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh @@ -21,6 +21,8 @@ tf2 ament_cmake_gtest + ament_lint_common + ament_lint_auto ament_cmake diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 9352a853a8d..388cd4345cf 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.6 + 0.4.7 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 5ee6f815be8..01a41e39d07 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.6 + 0.4.7 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 19a6d18c6e4..9a645cbd706 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.6 + 0.4.7 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 4f2fd329b1d..7eaccca6e41 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.6 + 0.4.7 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 3b07a61a1af..7da90b5ed9d 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.6 + 0.4.7 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 05eb0e0c95b..8b6811af54b 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.6 + 0.4.7 ROS2 Navigation Stack diff --git a/smac_planner/package.xml b/smac_planner/package.xml index ca96ed5cfa3..34d0e42ec97 100644 --- a/smac_planner/package.xml +++ b/smac_planner/package.xml @@ -2,7 +2,7 @@ smac_planner - 0.4.6 + 0.4.7 Smac global planning plugin Steve Macenski Apache-2.0 From 2590238de036513c96c6fd2c76884c7599b72a01 Mon Sep 17 00:00:00 2001 From: Jovian Dsouza Date: Tue, 6 Apr 2021 23:03:10 +0530 Subject: [PATCH 21/23] [nav2_bringup] Update waffle.model (#2296) Changed to the latest tire mesh file names for waffle as per the latest `turtlebot3_gazebo` package. This results in faster loading and resolves the errors that come in `gazebo --verbose` --- nav2_bringup/bringup/worlds/waffle.model | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/bringup/worlds/waffle.model b/nav2_bringup/bringup/worlds/waffle.model index 74af1c7670e..bc984be6c45 100644 --- a/nav2_bringup/bringup/worlds/waffle.model +++ b/nav2_bringup/bringup/worlds/waffle.model @@ -266,7 +266,7 @@ 0.0 0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/left_tire.dae + model://turtlebot3_waffle/meshes/tire.dae 0.001 0.001 0.001 @@ -324,7 +324,7 @@ 0.0 -0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/right_tire.dae + model://turtlebot3_waffle/meshes/tire.dae 0.001 0.001 0.001 From 2e18dd1cae31807f68746eb1a2c702526a34f4cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pau=20Carr=C3=A9=20Cardona?= Date: Mon, 3 May 2021 19:24:31 +0200 Subject: [PATCH 22/23] Update list of behavioural tree nodes (#2329) * Update list of nodes with nodes compiled in the branch and excluding unexistant to prevent runtime exceptions. * Updated documentation Co-authored-by: Pau Carre --- doc/parameters/param_list.md | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 9278a512a51..ada0e743f9f 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -8,7 +8,7 @@ | enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree | | groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot | | groot_zmq_server_port | 1667 | change port of the zmq server needed for groot | -| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries | +| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", "nav2_truncate_path_action_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_time_expired_condition_bt_node", "nav2_distance_traveled_condition_bt_node", "nav2_rotate_action_bt_node", "nav2_translate_action_bt_node", "nav2_is_battery_low_condition_bt_node", "nav2_goal_updater_node_bt_node", "nav2_navigate_to_pose_action_bt_node"] | list of behavior tree node shared libraries | | transform_tolerance | 0.1 | TF transform tolerance | | global_frame | "map" | Reference frame | | robot_base_frame | "base_link" | Robot base frame | diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index afd20df0e63..e44e64887e7 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -53,13 +53,17 @@ BtNavigator::BtNavigator() "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", "nav2_truncate_path_action_bt_node", - "nav2_change_goal_node_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_time_expired_condition_bt_node", - "nav2_distance_traveled_condition_bt_node" + "nav2_distance_traveled_condition_bt_node", + "nav2_rotate_action_bt_node", + "nav2_translate_action_bt_node", + "nav2_is_battery_low_condition_bt_node", + "nav2_goal_updater_node_bt_node", + "nav2_navigate_to_pose_action_bt_node", }; // Declare this node's parameters From 1160100d9689bea4c149031bb82bf7d182ec0645 Mon Sep 17 00:00:00 2001 From: Nisala Kalupahana Date: Tue, 8 Jun 2021 10:37:20 -0700 Subject: [PATCH 23/23] Moves changes from PR #2083 to foxy (#2375) * fix basename not defined issue * macos fixes * removed observationbuffer change * Update regulated_pure_pursuit_controller.cpp Fixes error: moving a temporary object prevents copy elision --- nav2_amcl/src/amcl_node.cpp | 4 ++-- .../include/nav2_behavior_tree/bt_action_node.hpp | 8 ++++---- .../include/nav2_behavior_tree/bt_service_node.hpp | 8 ++++---- nav2_bt_navigator/src/ros_topic_logger.cpp | 5 ----- nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp | 5 +++++ nav2_recoveries/include/nav2_recoveries/recovery.hpp | 2 +- .../src/regulated_pure_pursuit_controller.cpp | 2 +- nav2_util/src/dump_params.cpp | 1 + nav2_waypoint_follower/package.xml | 3 +-- smac_planner/src/a_star.cpp | 2 +- 10 files changed, 20 insertions(+), 20 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 9ea4a9b80c7..37da4a82532 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -401,7 +401,7 @@ AmclNode::checkLaserReceived() RCLCPP_WARN( get_logger(), "Laser scan has not been received" " (and thus no pose updates have been published)." - " Verify that data is being published on the %s topic.", scan_topic_); + " Verify that data is being published on the %s topic.", scan_topic_.c_str()); return; } @@ -410,7 +410,7 @@ AmclNode::checkLaserReceived() RCLCPP_WARN( get_logger(), "No laser scan received (and thus no pose updates have been published) for %f" " seconds. Verify that data is being published on the %s topic.", - d.nanoseconds() * 1e-9, scan_topic_); + d.nanoseconds() * 1e-9, scan_topic_.c_str()); } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index eed3626601e..77aa8836d44 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -36,11 +36,11 @@ class BtActionNode : public BT::ActionNodeBase const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { - node_ = config().blackboard->get("node"); + node_ = config().blackboard->template get("node"); // Get the required items from the blackboard server_timeout_ = - config().blackboard->get("server_timeout"); + config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); // Initialize the input and output messages @@ -245,9 +245,9 @@ class BtActionNode : public BT::ActionNodeBase void increment_recovery_count() { int recovery_count = 0; - config().blackboard->get("number_recoveries", recovery_count); // NOLINT + config().blackboard->template get("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->set("number_recoveries", recovery_count); // NOLINT + config().blackboard->template set("number_recoveries", recovery_count); // NOLINT } std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 2b7b16bfb9c..8931783df6c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -35,11 +35,11 @@ class BtServiceNode : public BT::SyncActionNode const BT::NodeConfiguration & conf) : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name) { - node_ = config().blackboard->get("node"); + node_ = config().blackboard->template get("node"); // Get the required items from the blackboard server_timeout_ = - config().blackboard->get("server_timeout"); + config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); // Now that we have node_ to use, create the service client for this BT service @@ -126,9 +126,9 @@ class BtServiceNode : public BT::SyncActionNode void increment_recovery_count() { int recovery_count = 0; - config().blackboard->get("number_recoveries", recovery_count); // NOLINT + config().blackboard->template get("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->set("number_recoveries", recovery_count); // NOLINT + config().blackboard->template set("number_recoveries", recovery_count); // NOLINT } std::string service_name_, service_node_name_; diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index 48bffd20c85..5b248014e81 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -39,12 +39,7 @@ void RosTopicLogger::callback( // BT timestamps are a duration since the epoch. Need to convert to a time_point // before converting to a msg. -#ifndef _WIN32 - event.timestamp = - tf2_ros::toMsg(std::chrono::time_point(timestamp)); -#else event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); -#endif event.node_name = node.name(); event.previous_status = toStr(prev_status, false); event.current_status = toStr(status, false); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp index dbbfb2caa93..e3195020780 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp @@ -59,6 +59,11 @@ class Observation delete cloud_; } + /** + * @brief Explicitly define copy assignment operator for Observation as it has a user-declared destructor + */ + Observation & operator=(const Observation &) = default; + /** * @brief Creates an observation from an origin point and a point cloud * @param origin The origin point of the observation diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 5ba051b16b4..7edb732d00c 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -107,7 +107,7 @@ class Recovery : public nav2_core::Recovery collision_checker_ = collision_checker; - vel_pub_ = node_->create_publisher("cmd_vel", 1); + vel_pub_ = node_->template create_publisher("cmd_vel", 1); onConfigure(); } diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index f5017ca058c..b8130f37e7f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -229,7 +229,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Find look ahead distance and point on path and publish const double lookahead_dist = getLookAheadDistance(speed); auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); - carrot_pub_->publish(std::move(createCarrotMsg(carrot_pose))); + carrot_pub_->publish(createCarrotMsg(carrot_pose)); double linear_vel, angular_vel; diff --git a/nav2_util/src/dump_params.cpp b/nav2_util/src/dump_params.cpp index a29a21c37b0..259479f1f60 100644 --- a/nav2_util/src/dump_params.cpp +++ b/nav2_util/src/dump_params.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 7da90b5ed9d..a8aa4bb9351 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -7,8 +7,6 @@ Steve Macenski Apache-2.0 - tf2_ros - ament_cmake nav2_common rclcpp @@ -17,6 +15,7 @@ nav_msgs nav2_msgs nav2_util + tf2_ros ament_lint_common ament_lint_auto diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp index b9668810dfc..a8432b6c470 100644 --- a/smac_planner/src/a_star.cpp +++ b/smac_planner/src/a_star.cpp @@ -352,7 +352,7 @@ AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( float d = node->motion_table.state_space->distance(from(), to()); NodePtr prev(node); // A move of sqrt(2) is guaranteed to be in a new cell - constexpr float sqrt_2 = std::sqrt(2.); + static const float sqrt_2 = std::sqrt(2.); unsigned int num_intervals = std::floor(d / sqrt_2); using PossibleNode = std::pair;