From 4048c9a45ba49f8191e25858cd8177eee634c06f Mon Sep 17 00:00:00 2001
From: Jakubach <jakubach@gmail.com>
Date: Wed, 14 Aug 2024 16:23:08 +0200
Subject: [PATCH 01/51] Added promixity BT node and BT tree

---
 nav2_behavior_tree/CMakeLists.txt             |  3 +
 .../condition/is_goal_nearby_condition.hpp    | 81 +++++++++++++++++++
 .../condition/is_goal_nearby_condition.cpp    | 42 ++++++++++
 ...planning_path_if_not_in_goal_proximity.xml | 46 +++++++++++
 4 files changed, 172 insertions(+)
 create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp
 create mode 100644 nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp
 create mode 100644 nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml

diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index 8f45cd318f3..a726cefc6fc 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -209,6 +209,9 @@ list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
 add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
 list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
 
+add_library(nav2_is_goal_nearby_condition_bt_node SHARED plugins/condition/is_goal_nearby_condition.cpp)
+list(APPEND plugin_libs nav2_is_goal_nearby_condition_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
   target_include_directories(${bt_plugin}
     PRIVATE
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp
new file mode 100644
index 00000000000..b434e75f1d3
--- /dev/null
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp
@@ -0,0 +1,81 @@
+// Copyright (c) 2024 Jakub Chudziński
+//
+// 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_GOAL_NEARBY_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_
+
+#include <string>
+#include "nav_msgs/msg/path.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp/condition_node.h"
+
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A BT::ConditionNode that returns SUCCESS when the IsGoalNearby
+ * service returns true and FAILURE otherwise
+ */
+class IsGoalNearbyCondition : public BT::ConditionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::IsGoalNearbyCondition
+   * @param condition_name Name for the XML tag for this node
+   * @param conf BT node configuration
+   */
+  IsGoalNearbyCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  IsGoalNearbyCondition() = delete;
+
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<nav_msgs::msg::Path>("path", "Planned Path"),     
+      BT::InputPort<double>(
+        "proximity_threshold", 3.0,
+        "Proximity length (m) of the remaining path considered as a nearby"),
+    };
+  }
+
+private:
+        
+    /**
+     * @brief Checks if the robot is in the goal proximity
+     * @param goal_path current planned path to the goal
+     * @param prox_thr proximity length (m) of the remaining path considered as a nearby
+     * @return whether the robot is in the goal proximity
+     */
+    bool isRobotInGoalProximity(
+        const nav_msgs::msg::Path& goal_path,
+        const double& prox_thr);
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_
\ No newline at end of file
diff --git a/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp b/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp
new file mode 100644
index 00000000000..5b2855ebefd
--- /dev/null
+++ b/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp
@@ -0,0 +1,42 @@
+#include "nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsGoalNearbyCondition::IsGoalNearbyCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf)
+{
+}
+
+bool IsGoalNearbyCondition::isRobotInGoalProximity(
+  const nav_msgs::msg::Path& goal_path,
+  const double& prox_thr)
+{
+  return nav2_util::geometry_utils::calculate_path_length(goal_path, 0) < prox_thr;
+}
+
+
+BT::NodeStatus IsGoalNearbyCondition::tick()
+{
+    nav_msgs::msg::Path path;
+    double prox_thr;
+    getInput("path", path);
+    getInput("proximity_threshold", prox_thr);
+    if (!path.poses.empty()){
+        if(isRobotInGoalProximity(path,prox_thr)){
+            return BT::NodeStatus::SUCCESS;
+        }
+    }
+    return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsGoalNearbyCondition>("IsGoalNearby");
+}
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml
new file mode 100644
index 00000000000..42030c00b0d
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml
@@ -0,0 +1,46 @@
+
+<!--
+  This Behavior Tree replans the global path until desired proximity to the goal is achived or if the path becomes invalid. It also has
+  recovery actions specific to planning / control as well as general system issues.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence>
+        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
+        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
+        <RateController hz="1.0" name="RateControllerComputePathToPose">
+          <RecoveryNode number_of_retries="1" name="RecoveryComputePathToPose">
+            <Fallback name="FallbackComputePathToPose">
+              <ReactiveSequence name="CheckIfNewPathNeeded">
+                <Inverter>
+                  <GlobalUpdatedGoal/>
+                </Inverter>
+                <IsGoalNearby path="{path}" proximity_threshold="2.0"/>
+                <IsPathValid path="{path}"/>
+              </ReactiveSequence>
+              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
+            </Fallback>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </RecoveryNode>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+      </PipelineSequence>
+      <ReactiveFallback name="FallbackRecoveries">
+        <GoalUpdated/>
+        <RoundRobin name="RecoveryActions">
+          <Sequence name="ClearingActions">
+            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </Sequence>
+          <Spin name="SpinRecovery" spin_dist="1.57"/>
+          <Wait name="WaitRecovery" wait_duration="5"/>
+          <BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>
\ No newline at end of file

From a411a508981783d5dac5fad3567d482480b7b0ab Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Tue, 29 Jul 2025 17:18:50 -0700
Subject: [PATCH 02/51] CI Limits: Moving MPPI, Smac to secondary job (#5401)

* Moving MPPI, Smac to secondary job

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* Use xlarge resource class

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* Bump the cache

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* back to large

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* fix

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* Add route server

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* Shift more packages over

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

---------

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>
---
 .circleci/config.yml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/.circleci/config.yml b/.circleci/config.yml
index d6766347816..295c319a739 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v36\
+              - "<< parameters.key >>-v38\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v36\
+              - "<< parameters.key >>-v38\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v36\
+            key: "<< parameters.key >>-v38\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\
@@ -532,7 +532,7 @@ jobs:
 
 _parameters:
   release_parameters: &release_parameters
-    packages_skip_regex: "nav2_system_tests"
+    packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_bringup|navigation2)'"
 
 workflows:
   version: 2

From f07bfc617f54bb329366eceb339635f1546aacef Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <doisyg@users.noreply.github.com>
Date: Thu, 31 Jul 2025 20:40:07 +0200
Subject: [PATCH 03/51] [DEX] Enforce 3 digits precision (#5398)

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
---
 .../include/nav2_map_server/map_io.hpp        |  8 +++++++
 nav2_map_server/src/map_io.cpp                | 24 ++++++++++++++-----
 2 files changed, 26 insertions(+), 6 deletions(-)

diff --git a/nav2_map_server/include/nav2_map_server/map_io.hpp b/nav2_map_server/include/nav2_map_server/map_io.hpp
index 97dc81821c9..5fc34937ee1 100644
--- a/nav2_map_server/include/nav2_map_server/map_io.hpp
+++ b/nav2_map_server/include/nav2_map_server/map_io.hpp
@@ -98,6 +98,14 @@ bool saveMapToFile(
   const nav_msgs::msg::OccupancyGrid & map,
   const SaveParameters & save_parameters);
 
+/**
+ * @brief to_string_with_precision
+ * @param value
+ * @param precision
+ * @return
+ */
+std::string to_string_with_precision(double value, int precision);
+
 /**
  * @brief Expand ~/ to home user dir.
  * @param yaml_filename Name of input YAML file.
diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp
index e95d192a9af..989f38f1211 100644
--- a/nav2_map_server/src/map_io.cpp
+++ b/nav2_map_server/src/map_io.cpp
@@ -594,13 +594,15 @@ void tryWriteMapToFile(
     std::string image_name = mapdatafile.substr(file_name_index + 1);
 
     YAML::Emitter e;
-    e << YAML::Precision(3);
+    e << YAML::Precision(7);
     e << YAML::BeginMap;
     e << YAML::Key << "image" << YAML::Value << image_name;
     e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(save_parameters.mode);
-    e << YAML::Key << "resolution" << YAML::Value << map.info.resolution;
-    e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x <<
-      map.info.origin.position.y << yaw << YAML::EndSeq;
+    e << YAML::Key << "resolution" << YAML::Value << to_string_with_precision(map.info.resolution,
+        3);
+    e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq <<
+      to_string_with_precision(map.info.origin.position.x, 3) <<
+      to_string_with_precision(map.info.origin.position.y, 3) << yaw << YAML::EndSeq;
     e << YAML::Key << "negate" << YAML::Value << 0;
 
     if (save_parameters.mode == MapMode::Trinary) {
@@ -611,8 +613,10 @@ void tryWriteMapToFile(
       e << YAML::Key << "occupied_thresh" << YAML::Value << 0.65;
       e << YAML::Key << "free_thresh" << YAML::Value << 0.196;
     } else {
-      e << YAML::Key << "occupied_thresh" << YAML::Value << save_parameters.occupied_thresh;
-      e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh;
+      e << YAML::Key << "occupied_thresh" << YAML::Value <<
+        to_string_with_precision(save_parameters.occupied_thresh, 3);
+      e << YAML::Key << "free_thresh" << YAML::Value <<
+        to_string_with_precision(save_parameters.free_thresh, 3);
     }
 
     if (!e.good()) {
@@ -648,4 +652,12 @@ bool saveMapToFile(
   return true;
 }
 
+std::string to_string_with_precision(double value, int precision)
+{
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(precision) << value;
+
+  return out.str();
+}
+
 }  // namespace nav2_map_server

From fba0358ab241a286286e659bc4afd9a0cbdbab98 Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <doisyg@users.noreply.github.com>
Date: Thu, 31 Jul 2025 20:40:20 +0200
Subject: [PATCH 04/51] [static_layer] limit comparison precision (#5405)

* [DEX] limit comparison precision

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* EPSILON 1e-5

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
---
 .../include/nav2_costmap_2d/static_layer.hpp  |  9 +++++++
 nav2_costmap_2d/plugins/static_layer.cpp      | 26 ++++++++++++++-----
 2 files changed, 29 insertions(+), 6 deletions(-)

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 7da443ebd25..0333f03577b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
@@ -153,6 +153,15 @@ class StaticLayer : public CostmapLayer
    */
   unsigned char interpretValue(unsigned char value);
 
+  /**
+   * @brief Check if two double values are equal within a given epsilon
+   * @param a First double value
+   * @param b Second double value
+   * @param epsilon The tolerance for equality check
+   * @return True if the values are equal within the tolerance, false otherwise
+   */
+  bool isEqual(double a, double b, double epsilon);
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index e4dbe277723..94aa69f87f8 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -47,6 +47,8 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav2_ros_common/validate_messages.hpp"
 
+#define EPSILON 1e-5
+
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
 
 using nav2_costmap_2d::NO_INFORMATION;
@@ -190,9 +192,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
   Costmap2D * master = layered_costmap_->getCostmap();
   if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
     master->getSizeInCellsY() != size_y ||
-    master->getResolution() != new_map.info.resolution ||
-    master->getOriginX() != new_map.info.origin.position.x ||
-    master->getOriginY() != new_map.info.origin.position.y ||
+    !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) ||
+    !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) ||
+    !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) ||
     !layered_costmap_->isSizeLocked()))
   {
     // Update the size of the layered costmap (and all layers, including this one)
@@ -206,9 +208,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
       new_map.info.origin.position.y,
       true);
   } else if (size_x_ != size_x || size_y_ != size_y ||  // NOLINT
-    resolution_ != new_map.info.resolution ||
-    origin_x_ != new_map.info.origin.position.x ||
-    origin_y_ != new_map.info.origin.position.y)
+    !isEqual(resolution_, new_map.info.resolution, EPSILON) ||
+    !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) ||
+    !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON))
   {
     // only update the size of the costmap stored locally in this layer
     RCLCPP_INFO(
@@ -469,6 +471,18 @@ StaticLayer::updateCosts(
   current_ = true;
 }
 
+/**
+  * @brief Check if two floating point numbers are equal within a given epsilon
+  * @param a First number
+  * @param b Second number
+  * @param epsilon Tolerance for equality check
+  * @return True if numbers are equal within the tolerance, false otherwise
+  */
+bool StaticLayer::isEqual(double a, double b, double epsilon)
+{
+  return std::abs(a - b) < epsilon;
+}
+
 /**
   * @brief Callback executed when a parameter change is detected
   * @param event ParameterEvent message

From c5531cb74ce5b1e06aba34e44fc21875f5cd0371 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Thu, 31 Jul 2025 12:29:24 -0700
Subject: [PATCH 05/51] Moving Nav2 package builds to the system build job to
 balance CI times for CircleCI limits (#5409)

* Moving Nav2 behaviors to the system build job

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* Adding Docking

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* moving more over to balance jobs

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

---------

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>
---
 .circleci/config.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.circleci/config.yml b/.circleci/config.yml
index 295c319a739..edb996eb1f7 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -532,7 +532,7 @@ jobs:
 
 _parameters:
   release_parameters: &release_parameters
-    packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_bringup|navigation2)'"
+    packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'"
 
 workflows:
   version: 2

From 8e3aee6845f2f1ce29ff49dff78266d2b47e4835 Mon Sep 17 00:00:00 2001
From: Vince Reda <60265874+redvinaa@users.noreply.github.com>
Date: Mon, 4 Aug 2025 20:11:32 +0200
Subject: [PATCH 06/51] Add Pause and SequenceWithBlackboardMemory BT nodes
 (#5247)

* Add pause and seq with bb memory BT nodes

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Requested changes

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Lint

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Restructure pause, rename unpaused state to resumed

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Add PauseResumeController test

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Implement tests using xml_txt trees and dummy nodes

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Update include

Signed-off-by: redvinaa <redvinaa@gmail.com>

* One more fix because of sync

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix build

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Add tests

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Remove unreachable code

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix tests

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Update copyrights

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix size calc

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Rename to PersistentSequence

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix docstring

Signed-off-by: redvinaa <redvinaa@gmail.com>

---------

Signed-off-by: redvinaa <redvinaa@gmail.com>
---
 nav2_behavior_tree/CMakeLists.txt             |   6 +
 .../control/pause_resume_controller.hpp       | 152 ++++++++++
 .../plugins/control/persistent_sequence.hpp   |  61 ++++
 nav2_behavior_tree/nav2_tree_nodes.xml        |  10 +
 .../control/pause_resume_controller.cpp       | 190 ++++++++++++
 .../plugins/control/persistent_sequence.cpp   |  84 +++++
 .../test/plugins/control/CMakeLists.txt       |   5 +
 .../control/test_pause_resume_controller.cpp  | 286 ++++++++++++++++++
 .../control/test_persistent_sequence.cpp      | 157 ++++++++++
 .../test/utils/get_node_from_tree.hpp         |  66 ++++
 .../test/utils/test_dummy_tree_node.hpp       |  24 +-
 11 files changed, 1038 insertions(+), 3 deletions(-)
 create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
 create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp
 create mode 100644 nav2_behavior_tree/plugins/control/pause_resume_controller.cpp
 create mode 100644 nav2_behavior_tree/plugins/control/persistent_sequence.cpp
 create mode 100644 nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp
 create mode 100644 nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp
 create mode 100644 nav2_behavior_tree/test/utils/get_node_from_tree.hpp

diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index 03339b19cdc..4059e6da15e 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -218,6 +218,12 @@ list(APPEND plugin_libs nav2_nonblocking_sequence_bt_node)
 add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp)
 list(APPEND plugin_libs nav2_round_robin_node_bt_node)
 
+add_library(nav2_pause_resume_controller_bt_node SHARED plugins/control/pause_resume_controller.cpp)
+list(APPEND plugin_libs nav2_pause_resume_controller_bt_node)
+
+add_library(nav2_persistent_sequence_bt_node SHARED plugins/control/persistent_sequence.cpp)
+list(APPEND plugin_libs nav2_persistent_sequence_bt_node)
+
 add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp)
 list(APPEND plugin_libs nav2_single_trigger_bt_node)
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
new file mode 100644
index 00000000000..a3f5333b19b
--- /dev/null
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
@@ -0,0 +1,152 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// 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__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_
+
+// Other includes
+#include <string>
+#include <memory>
+#include <map>
+
+// ROS includes
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "behaviortree_cpp/control_node.h"
+#include "nav2_ros_common/service_server.hpp"
+
+// Interface definitions
+#include "std_srvs/srv/trigger.hpp"
+
+
+namespace nav2_behavior_tree
+{
+
+using Trigger = std_srvs::srv::Trigger;
+
+enum state_t {RESUMED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME};
+const std::map<state_t, std::string> state_names = {
+  {RESUMED, "RESUMED"},
+  {PAUSED, "PAUSED"},
+  {PAUSE_REQUESTED, "PAUSE_REQUESTED"},
+  {ON_PAUSE, "ON_PAUSE"},
+  {RESUME_REQUESTED, "RESUME_REQUESTED"},
+  {ON_RESUME, "ON_RESUME"}
+};
+const std::map<state_t, uint16_t> child_indices = {
+  {RESUMED, 0},
+  {PAUSED, 1},
+  {ON_PAUSE, 2},
+  {ON_RESUME, 3}
+};
+
+/* @brief Controlled through service calls to pause and resume the execution of the tree
+ * It has one mandatory child for the RESUMED, and three optional for the PAUSED state,
+ * the ON_PAUSE event and the ON_RESUME event.
+ * It has two input ports:
+ * - pause_service_name: name of the service to pause
+ * - resume_service_name: name of the service to resume
+ *
+ * Usage:
+ * <PauseResumeController pause_service_name="/pause" resume_service_name="/resume">
+ *     <!-- RESUMED branch -->
+ *
+ *     <!-- PAUSED branch (optional) -->
+ *
+ *     <!-- ON_PAUSE branch (optional) -->
+ *
+ *     <!-- ON_RESUME branch (optional) -->
+ * </PauseResumeController>
+ *
+ * The controller starts in RESUMED state, and ticks it until it returns success.
+ * When the pause service is called, ON_PAUSE is ticked until completion,
+ * then the controller switches to PAUSED state.
+ * When the resume service is called, ON_RESUME is ticked until completion,
+ * then the controller switches back to RESUMED state.
+ *
+ * The controller only returns success when the RESUMED child returns success.
+ * The controller returns failure if any child returns failure.
+ * In any other case, it returns running.
+ */
+
+
+class PauseResumeController : public BT::ControlNode
+{
+public:
+  //! @brief Constructor
+  PauseResumeController(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  //! @brief Reset state and go to Idle
+  void halt() override;
+
+  //! @brief Handle transitions if requested and tick child related to the actual state
+  BT::NodeStatus tick() override;
+
+  //! @brief Declare ports
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::string>(
+        "pause_service_name",
+        "Name of the service to pause"),
+      BT::InputPort<std::string>(
+        "resume_service_name",
+        "Name of the service to resume"),
+    };
+  }
+
+  [[nodiscard]] inline state_t getState() const
+  {
+    return state_;
+  }
+
+private:
+  //! @brief Set state to PAUSE_REQUESTED
+  void pauseServiceCallback(
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<Trigger::Request> request,
+    std::shared_ptr<Trigger::Response> response);
+
+  //! @brief Set state to RESUME_REQUESTED
+  void resumeServiceCallback(
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<Trigger::Request> request,
+    std::shared_ptr<Trigger::Response> response);
+
+  /** @brief Switch to the next state based on the current state
+   *
+   * PAUSE_REQUESTED -> ON_PAUSE
+   * ON_PAUSE -> PAUSED
+   *
+   * RESUME_REQUESTED -> ON_RESUME
+   * ON_RESUME -> RESUMED
+   *
+   * Do nothing if in end state
+   */
+  void switchToNextState();
+
+  rclcpp::Logger logger_{rclcpp::get_logger("PauseResumeController")};
+  rclcpp::CallbackGroup::SharedPtr cb_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  nav2::ServiceServer<Trigger>::SharedPtr pause_srv_;
+  nav2::ServiceServer<Trigger>::SharedPtr resume_srv_;
+  state_t state_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp
new file mode 100644
index 00000000000..79d494eb8e6
--- /dev/null
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/persistent_sequence.hpp
@@ -0,0 +1,61 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// 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__CONTROL__PERSISTENT_SEQUENCE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_
+
+#include <string>
+#include "behaviortree_cpp/control_node.h"
+#include "behaviortree_cpp/bt_factory.h"
+
+namespace nav2_behavior_tree
+{
+/**
+ * @brief The PersistentSequenceNode is similar to the SequenceNode, but it
+ * stores the index of the last running child in the blackboard (key: "current_child_idx"),
+ * and it does not reset the index when it got halted.
+ * It used to tick children in an ordered sequence. If any child returns RUNNING, previous
+ * children will NOT be ticked again.
+ *
+ * - If all the children return SUCCESS, this node returns SUCCESS.
+ *
+ * - If a child returns RUNNING, this node returns RUNNING.
+ *   Loop is NOT restarted, the same running child will be ticked again.
+ *
+ * - If a child returns FAILURE, stop the loop and return FAILURE.
+ *   Restart the loop only if (reset_on_failure == true)
+ *
+ */
+class PersistentSequenceNode : public BT::ControlNode
+{
+public:
+  PersistentSequenceNode(const std::string & name, const BT::NodeConfiguration & conf);
+
+  ~PersistentSequenceNode() override = default;
+
+  //! @brief Declare ports
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::BidirectionalPort<int>("current_child_idx", "The index of the current child"),
+    };
+  }
+
+private:
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PERSISTENT_SEQUENCE_HPP_
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index a9798c0c64c..230ce02f70c 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -404,6 +404,16 @@
     <Control ID="RoundRobin"/>
 
     <Control ID="NonblockingSequence"/>
+
+    <Control ID="PauseResumeController">
+      <input_port name="pause_service_name">Service to call to pause</input_port>
+      <input_port name="resume_service_name">Service to call to resume</input_port>
+    </Control>
+
+    <Control ID="PersistentSequence">
+      <bidirectional_port name="current_child_idx">Index of the child to execute</bidirectional_port>
+    </Control>
+
     <!-- ############################### DECORATOR NODES ############################## -->
     <Decorator ID="RateController">
       <input_port name="hz">Rate</input_port>
diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp
new file mode 100644
index 00000000000..0f039aad8e6
--- /dev/null
+++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp
@@ -0,0 +1,190 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// 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.
+
+// Other includes
+#include <functional>
+
+// ROS includes
+#include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp"
+
+// Other includes
+#include "behaviortree_cpp/bt_factory.h"
+
+namespace nav2_behavior_tree
+{
+
+using namespace std::placeholders;
+
+PauseResumeController::PauseResumeController(
+  const std::string & xml_tag_name,
+  const BT::NodeConfiguration & conf)
+: BT::ControlNode(xml_tag_name, conf)
+{
+  auto node = this->config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  state_ = RESUMED;
+
+  // Create a separate cb group with a separate executor to spin
+  cb_group_ = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+
+  executor_ =
+    std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  executor_->add_callback_group(cb_group_, node->get_node_base_interface());
+
+  std::string pause_service_name;
+  getInput("pause_service_name", pause_service_name);
+  pause_srv_ = std::make_shared<nav2::ServiceServer<Trigger>>(
+    pause_service_name,
+    node,
+    std::bind(&PauseResumeController::pauseServiceCallback, this, _1, _2, _3),
+    cb_group_);
+
+  std::string resume_service_name;
+  getInput("resume_service_name", resume_service_name);
+  resume_srv_ = std::make_shared<nav2::ServiceServer<Trigger>>(
+    resume_service_name,
+    node,
+    std::bind(&PauseResumeController::resumeServiceCallback, this, _1, _2, _3),
+    cb_group_);
+}
+
+BT::NodeStatus PauseResumeController::tick()
+{
+  unsigned int children_count = children_nodes_.size();
+  if (children_count < 1 || children_count > 4) {
+    throw std::runtime_error(
+            "PauseNode must have at least one and at most four children "
+            "(currently has " + std::to_string(children_count) + ")");
+  }
+
+  if (status() == BT::NodeStatus::IDLE) {
+    state_ = RESUMED;
+  }
+  setStatus(BT::NodeStatus::RUNNING);
+
+  executor_->spin_some();
+
+  // If pause / resume requested, reset children and switch to transient child
+  if (state_ == PAUSE_REQUESTED || state_ == RESUME_REQUESTED) {
+    resetChildren();
+    switchToNextState();
+  }
+
+  // Return RUNNING and do nothing if specific child is not used
+  const uint16_t child_idx = child_indices.at(state_);
+  if (children_nodes_.size() <= child_idx) {
+    switchToNextState();
+    return BT::NodeStatus::RUNNING;
+  }
+
+  // If child is used, tick it
+  const BT::NodeStatus child_status =
+    children_nodes_[child_indices.at(state_)]->executeTick();
+
+  switch (child_status) {
+    case BT::NodeStatus::RUNNING:
+      return BT::NodeStatus::RUNNING;
+    case BT::NodeStatus::SUCCESS:
+    case BT::NodeStatus::SKIPPED:
+      if (state_ == RESUMED) {
+        // Resumed child returned SUCCESS, we're done
+        return BT::NodeStatus::SUCCESS;
+      }
+      switchToNextState();
+      // If any branch other than RESUMED returned SUCCESS, keep ticking
+      return BT::NodeStatus::RUNNING;
+    case BT::NodeStatus::FAILURE:
+      RCLCPP_ERROR(
+        logger_, "%s child returned FAILURE", state_names.at(state_).c_str());
+      return BT::NodeStatus::FAILURE;
+    default:
+      throw std::runtime_error("A child node must never return IDLE");
+  }
+}
+
+void PauseResumeController::switchToNextState()
+{
+  static const std::map<state_t, state_t> next_states = {
+    {PAUSE_REQUESTED, ON_PAUSE},
+    {ON_PAUSE, PAUSED},
+    {RESUME_REQUESTED, ON_RESUME},
+    {ON_RESUME, RESUMED}
+  };
+
+  if (state_ == PAUSED || state_ == RESUMED) {
+    // No next state, do nothing
+    return;
+  }
+
+  state_ = next_states.at(state_);
+  RCLCPP_INFO(logger_, "Switched to state: %s", state_names.at(state_).c_str());
+}
+
+void PauseResumeController::pauseServiceCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<Trigger::Request>/*request*/,
+  std::shared_ptr<Trigger::Response> response)
+{
+  if (state_ != PAUSED) {
+    RCLCPP_INFO(logger_, "Received pause request");
+    response->success = true;
+    state_ = PAUSE_REQUESTED;
+    return;
+  }
+
+  std::string warn_message = "PauseResumeController BT node already in state PAUSED";
+  RCLCPP_WARN(logger_, "%s", warn_message.c_str());
+  response->success = false;
+  response->message = warn_message;
+}
+
+void PauseResumeController::resumeServiceCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<Trigger::Request>/*request*/,
+  std::shared_ptr<Trigger::Response> response)
+{
+  if (state_ == PAUSED) {
+    RCLCPP_INFO(logger_, "Received resume request");
+    response->success = true;
+    state_ = RESUME_REQUESTED;
+    return;
+  }
+
+  std::string warn_message = "PauseResumeController BT node not in state PAUSED";
+  RCLCPP_WARN(logger_, "%s", warn_message.c_str());
+  response->success = false;
+  response->message = warn_message;
+}
+
+void PauseResumeController::halt()
+{
+  state_ = RESUMED;
+  ControlNode::halt();
+}
+
+}  // namespace nav2_behavior_tree
+
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::PauseResumeController>(
+        name, config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::PauseResumeController>(
+    "PauseResumeController", builder);
+}
diff --git a/nav2_behavior_tree/plugins/control/persistent_sequence.cpp b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp
new file mode 100644
index 00000000000..1886deb3d5c
--- /dev/null
+++ b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp
@@ -0,0 +1,84 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_behavior_tree/plugins/control/persistent_sequence.hpp"
+#include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/bt_factory.h"
+
+namespace nav2_behavior_tree
+{
+
+PersistentSequenceNode::PersistentSequenceNode(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ControlNode::ControlNode(name, conf) {}
+
+BT::NodeStatus PersistentSequenceNode::tick()
+{
+  const int children_count = children_nodes_.size();
+
+  int current_child_idx;
+  getInput("current_child_idx", current_child_idx);
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  while (current_child_idx < children_count) {
+    TreeNode * current_child_node = children_nodes_[current_child_idx];
+    const BT::NodeStatus child_status = current_child_node->executeTick();
+
+    switch (child_status) {
+      case BT::NodeStatus::RUNNING:
+        return child_status;
+
+      case BT::NodeStatus::FAILURE:
+        // Reset on failure
+        resetChildren();
+        current_child_idx = 0;
+        setOutput("current_child_idx", 0);
+        return child_status;
+
+      case BT::NodeStatus::SUCCESS:
+      case BT::NodeStatus::SKIPPED:
+        // Skip the child node
+        current_child_idx++;
+        setOutput("current_child_idx", current_child_idx);
+        break;
+
+      case BT::NodeStatus::IDLE:
+        throw std::runtime_error("A child node must never return IDLE");
+    }  // end switch
+  }  // end while loop
+
+  // The entire while loop completed. This means that all the children returned SUCCESS.
+  if (current_child_idx >= children_count) {
+    resetChildren();
+    setOutput("current_child_idx", 0);
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::PersistentSequenceNode>(
+        name, config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::PersistentSequenceNode>(
+    "PersistentSequence", builder);
+}
diff --git a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt
index 1a8b0cf781a..a625bdad792 100644
--- a/nav2_behavior_tree/test/plugins/control/CMakeLists.txt
+++ b/nav2_behavior_tree/test/plugins/control/CMakeLists.txt
@@ -5,3 +5,8 @@ plugin_add_test(test_control_pipeline_sequence test_pipeline_sequence.cpp nav2_p
 plugin_add_test(test_control_round_robin_node test_round_robin_node.cpp nav2_round_robin_node_bt_node)
 
 plugin_add_test(test_control_nonblocking_sequence test_nonblocking_sequence.cpp nav2_nonblocking_sequence_bt_node)
+
+plugin_add_test(test_control_pause_resume_controller
+  test_pause_resume_controller.cpp nav2_pause_resume_controller_bt_node)
+
+plugin_add_test(test_persistent_sequence test_persistent_sequence.cpp nav2_persistent_sequence_bt_node)
diff --git a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp
new file mode 100644
index 00000000000..8a78c37d89b
--- /dev/null
+++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp
@@ -0,0 +1,286 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// 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 <gtest/gtest.h>
+#include <memory>
+
+#include "utils/test_behavior_tree_fixture.hpp"
+#include "utils/test_dummy_tree_node.hpp"
+#include "utils/get_node_from_tree.hpp"
+#include "nav2_behavior_tree/plugins/control/pause_resume_controller.hpp"
+#include "std_srvs/srv/trigger.hpp"
+
+class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  static void SetUpTestCase()
+  {
+    auto node = std::make_shared<rclcpp::Node>("pause_resume_controller_test_fixture");
+    executor_ =
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    cb_group_ = node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    executor_->add_callback_group(cb_group_, node->get_node_base_interface());
+    pause_client_ = node->create_client<std_srvs::srv::Trigger>(
+      "pause", rclcpp::ServicesQoS(), cb_group_);
+    resume_client_ = node->create_client<std_srvs::srv::Trigger>(
+      "resume", rclcpp::ServicesQoS(), cb_group_);
+
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+    config_ = new BT::NodeConfiguration();
+    config_->blackboard = BT::Blackboard::create();
+    config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node);
+
+    factory_->registerNodeType<nav2_behavior_tree::PauseResumeController>("PauseResumeController");
+
+    // Register dummy node for testing
+    factory_->registerNodeType<nav2_behavior_tree::DummyNode>("DummyNode");
+  }
+
+  static void TearDownTestCase()
+  {
+    if (config_) {
+      delete config_;
+      config_ = nullptr;
+    }
+    tree_.reset();
+  }
+
+protected:
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+  static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  static rclcpp::CallbackGroup::SharedPtr cb_group_;
+  static rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr pause_client_;
+  static rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resume_client_;
+};
+
+rclcpp::executors::SingleThreadedExecutor::SharedPtr
+PauseResumeControllerTestFixture::executor_ = nullptr;
+rclcpp::CallbackGroup::SharedPtr
+PauseResumeControllerTestFixture::cb_group_ = nullptr;
+rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr
+PauseResumeControllerTestFixture::pause_client_ = nullptr;
+rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr
+PauseResumeControllerTestFixture::resume_client_ = nullptr;
+BT::NodeConfiguration * PauseResumeControllerTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> PauseResumeControllerTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> PauseResumeControllerTestFixture::tree_ = nullptr;
+
+TEST_F(PauseResumeControllerTestFixture, test_incorrect_num_children)
+{
+  // create tree with incorrect number of children
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <PauseResumeController
+            pause_service_name="pause"
+            resume_service_name="resume">
+          </PauseResumeController>
+        </BehaviorTree>
+      </root>)";
+  EXPECT_THROW(
+    auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard),
+    BT::RuntimeError);
+}
+
+TEST_F(PauseResumeControllerTestFixture, test_unused_children)
+{
+  // create tree with only RESUMED child
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <PauseResumeController
+            pause_service_name="pause"
+            resume_service_name="resume">
+            <DummyNode/>  <!-- RESUMED -->
+          </PauseResumeController>
+        </BehaviorTree>
+      </root>)";
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // get pause_resume_controller so we can check state
+  auto pause_bt_node =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::PauseResumeController>(tree_);
+  ASSERT_NE(pause_bt_node, nullptr);
+  using state_t = nav2_behavior_tree::state_t;
+
+  // get dummy nodes so we can change their status
+  auto resumed_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 0);
+  ASSERT_NE(resumed_child, nullptr);
+  resumed_child->changeStatus(BT::NodeStatus::RUNNING);
+
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+
+  const auto & check_request_succeeded = [](
+    rclcpp::Client<std_srvs::srv::Trigger>::FutureAndRequestId & future)
+    {
+      executor_->spin_until_future_complete(future, std::chrono::seconds(1));
+      ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready);
+      EXPECT_EQ(future.get()->success, true);
+    };
+
+  // Call pause service, expect RUNNING and PAUSED
+  auto future = pause_client_->async_send_request(
+    std::make_shared<std_srvs::srv::Trigger::Request>());
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+  check_request_succeeded(future);
+
+  // Tick again, expect RUNNING and PAUSED
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+
+  // Call resume service, expect RUNNING and ON_RESUME
+  future = resume_client_->async_send_request(
+    std::make_shared<std_srvs::srv::Trigger::Request>());
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+  check_request_succeeded(future);
+
+  // Tick again, expect RUNNING and RESUMED
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+}
+
+TEST_F(PauseResumeControllerTestFixture, test_behavior)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <PauseResumeController
+            pause_service_name="pause"
+            resume_service_name="resume">
+            <DummyNode/>  <!-- RESUMED -->
+            <DummyNode/>  <!-- PAUSED -->
+            <DummyNode/>  <!-- ON_PAUSE -->
+            <DummyNode/>  <!-- ON_RESUME -->
+          </PauseResumeController>
+        </BehaviorTree>
+      </root>)";
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // get pause_resume_controller so we can check state
+  auto pause_bt_node =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::PauseResumeController>(tree_);
+  ASSERT_NE(pause_bt_node, nullptr);
+  using state_t = nav2_behavior_tree::state_t;
+
+  // get dummy nodes so we can change their status
+  auto resumed_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 0);
+  auto paused_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 1);
+  auto on_pause_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 2);
+  auto on_resume_child =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 3);
+  ASSERT_NE(resumed_child, nullptr);
+  ASSERT_NE(paused_child, nullptr);
+  ASSERT_NE(on_pause_child, nullptr);
+  ASSERT_NE(on_resume_child, nullptr);
+
+  resumed_child->changeStatus(BT::NodeStatus::RUNNING);
+  paused_child->changeStatus(BT::NodeStatus::RUNNING);
+  on_pause_child->changeStatus(BT::NodeStatus::RUNNING);
+  on_resume_child->changeStatus(BT::NodeStatus::RUNNING);
+
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+
+  const auto & check_future_result = [](
+    rclcpp::Client<std_srvs::srv::Trigger>::FutureAndRequestId & future, bool success = true)
+    -> void
+    {
+      executor_->spin_until_future_complete(future, std::chrono::seconds(1));
+      ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready);
+      EXPECT_EQ(future.get()->success, success);
+    };
+
+  // Call pause service, set ON_PAUSE child to RUNNING, expect RUNNING and ON_PAUSE
+  auto future = pause_client_->async_send_request(
+    std::make_shared<std_srvs::srv::Trigger::Request>());
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::ON_PAUSE);
+  check_future_result(future);
+
+  // Change ON_PAUSE child to SUCCESS, expect RUNNING and PAUSED
+  on_pause_child->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+
+  // Set PAUSED child to SUCCESS, expect RUNNING and PAUSED
+  // (should keep ticking unless RESUMED branch succeeds)
+  paused_child->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+
+  // Set PAUSED child to SKIPPED, expect RUNNING and PAUSED
+  paused_child->changeStatus(BT::NodeStatus::SKIPPED);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+
+  // Call pause service again, expect RUNNING and PAUSED
+  future = pause_client_->async_send_request(
+    std::make_shared<std_srvs::srv::Trigger::Request>());
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
+  check_future_result(future, false);
+
+  // Call resume service, change ON_RESUME child to FAILURE, expect FAILURE
+  future = resume_client_->async_send_request(
+    std::make_shared<std_srvs::srv::Trigger::Request>());
+  on_resume_child->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE);
+  check_future_result(future);
+
+  // Halt the tree, expect RUNNING and RESUMED
+  tree_->haltTree();
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+
+  // Set resumed child to SUCCESS, expect SUCCESS and RESUMED
+  resumed_child->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+
+  // Call resume service again, expect RUNNING and RESUMED
+  future = resume_client_->async_send_request(
+    std::make_shared<std_srvs::srv::Trigger::Request>());
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
+  check_future_result(future, false);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}
diff --git a/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp
new file mode 100644
index 00000000000..ebf6aa0d6b8
--- /dev/null
+++ b/nav2_behavior_tree/test/plugins/control/test_persistent_sequence.cpp
@@ -0,0 +1,157 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// 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 <gtest/gtest.h>
+#include <memory>
+
+#include "utils/test_behavior_tree_fixture.hpp"
+#include "utils/test_dummy_tree_node.hpp"
+#include "utils/get_node_from_tree.hpp"
+#include "nav2_behavior_tree/plugins/control/persistent_sequence.hpp"
+
+class PersistentSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  static void SetUpTestCase()
+  {
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+    config_ = new BT::NodeConfiguration();
+    config_->blackboard = BT::Blackboard::create();
+    config_->blackboard->set("seq_child_idx", 0);
+
+    factory_->registerNodeType<nav2_behavior_tree::PersistentSequenceNode>(
+      "PersistentSequence");
+
+    // Register dummy node for testing
+    factory_->registerNodeType<nav2_behavior_tree::DummyNode>("DummyNode");
+  }
+
+  static void TearDownTestCase()
+  {
+    if (config_) {
+      delete config_;
+      config_ = nullptr;
+    }
+    tree_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+BT::NodeConfiguration * PersistentSequenceTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+PersistentSequenceTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> PersistentSequenceTestFixture::tree_ = nullptr;
+
+TEST_F(PersistentSequenceTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <PersistentSequence current_child_idx="{seq_child_idx}">
+            <AlwaysSuccess/>
+          </PersistentSequence>
+        </BehaviorTree>
+      </root>)";
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  config_->blackboard->set("seq_child_idx", 0);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+}
+
+TEST_F(PersistentSequenceTestFixture, test_behavior)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <PersistentSequence current_child_idx="{seq_child_idx}">
+            <DummyNode/>
+            <DummyNode/>
+            <DummyNode/>
+          </PersistentSequence>
+        </BehaviorTree>
+      </root>)";
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // get dummy nodes so we can change their status
+  auto child0 =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 0);
+  auto child1 =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 1);
+  auto child2 =
+    nav2_behavior_tree::get_node_from_tree<nav2_behavior_tree::DummyNode>(tree_, 2);
+  ASSERT_NE(child0, nullptr);
+  ASSERT_NE(child1, nullptr);
+  ASSERT_NE(child2, nullptr);
+  child0->changeStatus(BT::NodeStatus::RUNNING);
+  child1->changeStatus(BT::NodeStatus::RUNNING);
+  child2->changeStatus(BT::NodeStatus::RUNNING);
+
+  // Set child index to 0 and child0 to RUNNING, expect RUNNING and idx = 0
+  config_->blackboard->set("seq_child_idx", 0);
+  child0->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(config_->blackboard->get<int>("seq_child_idx"), 0);
+
+  // Set child0 to SUCCESS, expect SUCCESS and idx = 1
+  child0->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(config_->blackboard->get<int>("seq_child_idx"), 1);
+
+  // Set child1 to FAILURE, expect FAILURE and idx = 0
+  child1->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(config_->blackboard->get<int>("seq_child_idx"), 0);
+
+  // Set idx to 1, child0 to FAILURE, child1 to SKIPPED and child2 to RUNNING,
+  // expect RUNNING and idx = 2
+  config_->blackboard->set("seq_child_idx", 1);
+  child0->changeStatus(BT::NodeStatus::FAILURE);
+  child1->changeStatus(BT::NodeStatus::SKIPPED);
+  child2->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(config_->blackboard->get<int>("seq_child_idx"), 2);
+
+  // Set child2 to SUCCESS, expect SUCCESS and idx = 0
+  child2->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("seq_child_idx"), 0);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}
diff --git a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp
new file mode 100644
index 00000000000..068fc924005
--- /dev/null
+++ b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp
@@ -0,0 +1,66 @@
+// Copyright (c) 2025 Enjoy Robotics
+//
+// 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 UTILS__GET_NODE_FROM_TREE_HPP_
+#define UTILS__GET_NODE_FROM_TREE_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "behaviortree_cpp/bt_factory.h"
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief Get node from tree by type and index, casted to NodeT type, const casted away
+ * Returns true if the node was found, false otherwise
+ */
+template<typename NodeT>
+NodeT * get_node_from_tree(
+  const std::shared_ptr<BT::Tree> tree,
+  const size_t index = 0)
+{
+  static rclcpp::Logger logger = rclcpp::get_logger("nav2_behavior_tree::get_node_from_tree");
+
+  std::vector<const BT::TreeNode *> nodes = tree->getNodesByPath<NodeT>("*");
+  if (nodes.empty()) {
+    RCLCPP_ERROR(logger, "No nodes of given type found");
+    return nullptr;
+  }
+  if (nodes.size() <= index) {
+    RCLCPP_ERROR(logger, "Out of bounds (found %zu < %zu nodes)", nodes.size(), index);
+    return nullptr;
+  }
+
+  const NodeT * const_bt_node =
+    dynamic_cast<const NodeT *>(nodes[index]);
+  if (const_bt_node == nullptr) {
+    RCLCPP_ERROR(logger, "Failed to cast node to given type");
+    return nullptr;
+  }
+
+  NodeT * bt_node = const_cast<NodeT *>(const_bt_node);
+  if (bt_node == nullptr) {
+    RCLCPP_ERROR(logger, "Failed to cast away const from node");
+    return nullptr;
+  }
+
+  return bt_node;
+}
+
+}  // namespace nav2_behavior_tree
+
+#endif  // UTILS__GET_NODE_FROM_TREE_HPP_
diff --git a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp
index dcfa52927ac..b4a64d7c78f 100644
--- a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp
+++ b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp
@@ -18,6 +18,7 @@
 
 #include <behaviortree_cpp/basic_types.h>
 #include <behaviortree_cpp/action_node.h>
+#include <string>
 
 namespace nav2_behavior_tree
 {
@@ -29,17 +30,20 @@ namespace nav2_behavior_tree
 class DummyNode : public BT::ActionNodeBase
 {
 public:
-  DummyNode()
+  DummyNode(
+    const std::string & /*xml_tag_name*/ = "dummy",
+    const BT::NodeConfiguration & /*conf*/ = BT::NodeConfiguration())
   : BT::ActionNodeBase("dummy", {})
   {
   }
 
   void changeStatus(BT::NodeStatus status)
   {
-    if (status == BT::NodeStatus::IDLE) {
+    requested_status = status;
+    if (requested_status == BT::NodeStatus::IDLE) {
       resetStatus();
     } else {
-      setStatus(status);
+      setStatus(requested_status);
     }
   }
 
@@ -50,12 +54,26 @@ class DummyNode : public BT::ActionNodeBase
 
   BT::NodeStatus tick() override
   {
+    if (requested_status == BT::NodeStatus::IDLE) {
+      resetStatus();
+    } else {
+      setStatus(requested_status);
+    }
     return status();
   }
 
   void halt() override
   {
+    resetStatus();
   }
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+
+protected:
+  BT::NodeStatus requested_status = BT::NodeStatus::IDLE;
 };
 
 }  // namespace nav2_behavior_tree

From e8b18110a95ef1c94aac1de36ff62f75c7e72578 Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <doisyg@users.noreply.github.com>
Date: Mon, 4 Aug 2025 19:15:51 +0100
Subject: [PATCH 07/51] corner case bin check (#5413)

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
---
 nav2_smac_planner/src/smac_planner_hybrid.cpp | 31 +++++++++++--------
 .../src/smac_planner_lattice.cpp              | 14 +++++----
 2 files changed, 26 insertions(+), 19 deletions(-)

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
index ad1ae0647c9..1f374af35d0 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -414,15 +414,17 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
             std::to_string(start.pose.position.y) + ") was outside bounds");
   }
 
-  double orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size);
-  while (orientation_bin < 0.0) {
-    orientation_bin += static_cast<float>(_angle_quantizations);
+  double start_orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size);
+  while (start_orientation_bin < 0.0) {
+    start_orientation_bin += static_cast<float>(_angle_quantizations);
   }
   // This is needed to handle precision issues
-  if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
-    orientation_bin -= static_cast<float>(_angle_quantizations);
+  if (start_orientation_bin >= static_cast<float>(_angle_quantizations)) {
+    start_orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  _a_star->setStart(mx_start, my_start, static_cast<unsigned int>(orientation_bin));
+  unsigned int start_orientation_bin_int =
+    static_cast<unsigned int>(start_orientation_bin);
+  _a_star->setStart(mx_start, my_start, start_orientation_bin_int);
 
   // Set goal point, in A* bin search coordinates
   if (!costmap->worldToMapContinuous(
@@ -435,15 +437,17 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
             std::to_string(goal.pose.position.y) + ") was outside bounds");
   }
-  orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size);
-  while (orientation_bin < 0.0) {
-    orientation_bin += static_cast<float>(_angle_quantizations);
+  double goal_orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size);
+  while (goal_orientation_bin < 0.0) {
+    goal_orientation_bin += static_cast<float>(_angle_quantizations);
   }
   // This is needed to handle precision issues
-  if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
-    orientation_bin -= static_cast<float>(_angle_quantizations);
+  if (goal_orientation_bin >= static_cast<float>(_angle_quantizations)) {
+    goal_orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  _a_star->setGoal(mx_goal, my_goal, static_cast<unsigned int>(orientation_bin),
+  unsigned int goal_orientation_bin_int =
+    static_cast<unsigned int>(goal_orientation_bin);
+  _a_star->setGoal(mx_goal, my_goal, static_cast<unsigned int>(goal_orientation_bin_int),
     _goal_heading_mode, _coarse_search_resolution);
 
   // Setup message
@@ -460,7 +464,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
 
   // Corner case of start and goal being on the same cell
   if (std::floor(mx_start) == std::floor(mx_goal) &&
-    std::floor(my_start) == std::floor(my_goal))
+    std::floor(my_start) == std::floor(my_goal) &&
+    start_orientation_bin_int == goal_orientation_bin_int)
   {
     pose.pose = start.pose;
     pose.pose.orientation = goal.pose.orientation;
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index d689507e8f4..fa89e90636c 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -338,9 +338,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
             std::to_string(start.pose.position.y) + ") was outside bounds");
   }
-  _a_star->setStart(
-    mx_start, my_start,
-    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation)));
+  unsigned int start_bin =
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation));
+  _a_star->setStart(mx_start, my_start, start_bin);
 
   // Set goal point, in A* bin search coordinates
   if (!_costmap->worldToMapContinuous(
@@ -353,9 +353,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
             std::to_string(goal.pose.position.y) + ") was outside bounds");
   }
+  unsigned int goal_bin =
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation));
   _a_star->setGoal(
-    mx_goal, my_goal,
-    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)),
+    mx_goal, my_goal, goal_bin,
       _goal_heading_mode, _coarse_search_resolution);
 
   // Setup message
@@ -372,7 +373,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
 
   // Corner case of start and goal being on the same cell
   if (std::floor(mx_start) == std::floor(mx_goal) &&
-    std::floor(my_start) == std::floor(my_goal))
+    std::floor(my_start) == std::floor(my_goal) &&
+    start_bin == goal_bin)
   {
     pose.pose = start.pose;
     pose.pose.orientation = goal.pose.orientation;

From d93c12974678d4b799c68e6a2a1a311895a4a5bd Mon Sep 17 00:00:00 2001
From: Sushant Chavan <sushant.chavan@idealworks.com>
Date: Wed, 6 Aug 2025 18:05:34 +0200
Subject: [PATCH 08/51] Add backward_ros dependency (#5404)

Signed-off-by: Sushant Chavan <gitecsvc@gmail.com>
---
 nav2_amcl/CMakeLists.txt                               | 1 +
 nav2_amcl/package.xml                                  | 1 +
 nav2_behavior_tree/CMakeLists.txt                      | 1 +
 nav2_behavior_tree/package.xml                         | 1 +
 nav2_behaviors/CMakeLists.txt                          | 1 +
 nav2_behaviors/package.xml                             | 1 +
 nav2_bringup/CMakeLists.txt                            | 1 +
 nav2_bringup/package.xml                               | 2 ++
 nav2_bt_navigator/CMakeLists.txt                       | 1 +
 nav2_bt_navigator/package.xml                          | 1 +
 nav2_collision_monitor/CMakeLists.txt                  | 1 +
 nav2_collision_monitor/package.xml                     | 1 +
 nav2_common/CMakeLists.txt                             | 1 +
 nav2_common/package.xml                                | 1 +
 nav2_constrained_smoother/CMakeLists.txt               | 1 +
 nav2_constrained_smoother/package.xml                  | 1 +
 nav2_controller/CMakeLists.txt                         | 1 +
 nav2_controller/package.xml                            | 1 +
 nav2_core/CMakeLists.txt                               | 1 +
 nav2_core/package.xml                                  | 1 +
 nav2_costmap_2d/CMakeLists.txt                         | 1 +
 nav2_costmap_2d/package.xml                            | 1 +
 nav2_docking/opennav_docking/CMakeLists.txt            | 1 +
 nav2_docking/opennav_docking/package.xml               | 1 +
 nav2_docking/opennav_docking_bt/CMakeLists.txt         | 1 +
 nav2_docking/opennav_docking_bt/package.xml            | 1 +
 nav2_docking/opennav_docking_core/CMakeLists.txt       | 1 +
 nav2_docking/opennav_docking_core/package.xml          | 1 +
 nav2_dwb_controller/costmap_queue/CMakeLists.txt       | 1 +
 nav2_dwb_controller/costmap_queue/package.xml          | 1 +
 nav2_dwb_controller/dwb_core/CMakeLists.txt            | 1 +
 nav2_dwb_controller/dwb_core/package.xml               | 1 +
 nav2_dwb_controller/dwb_critics/CMakeLists.txt         | 1 +
 nav2_dwb_controller/dwb_critics/package.xml            | 1 +
 nav2_dwb_controller/dwb_msgs/CMakeLists.txt            | 1 +
 nav2_dwb_controller/dwb_msgs/package.xml               | 1 +
 nav2_dwb_controller/dwb_plugins/CMakeLists.txt         | 1 +
 nav2_dwb_controller/dwb_plugins/package.xml            | 1 +
 nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt | 1 +
 nav2_dwb_controller/nav2_dwb_controller/package.xml    | 2 ++
 nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt         | 1 +
 nav2_dwb_controller/nav_2d_msgs/package.xml            | 1 +
 nav2_dwb_controller/nav_2d_utils/CMakeLists.txt        | 1 +
 nav2_dwb_controller/nav_2d_utils/package.xml           | 1 +
 nav2_graceful_controller/CMakeLists.txt                | 1 +
 nav2_graceful_controller/package.xml                   | 1 +
 nav2_lifecycle_manager/CMakeLists.txt                  | 1 +
 nav2_lifecycle_manager/package.xml                     | 1 +
 nav2_map_server/CMakeLists.txt                         | 1 +
 nav2_map_server/package.xml                            | 1 +
 nav2_mppi_controller/CMakeLists.txt                    | 1 +
 nav2_mppi_controller/package.xml                       | 1 +
 nav2_msgs/CMakeLists.txt                               | 1 +
 nav2_msgs/package.xml                                  | 1 +
 nav2_navfn_planner/CMakeLists.txt                      | 1 +
 nav2_navfn_planner/package.xml                         | 1 +
 nav2_planner/CMakeLists.txt                            | 1 +
 nav2_planner/package.xml                               | 1 +
 nav2_regulated_pure_pursuit_controller/CMakeLists.txt  | 1 +
 nav2_regulated_pure_pursuit_controller/package.xml     | 1 +
 nav2_ros_common/CMakeLists.txt                         | 1 +
 nav2_ros_common/package.xml                            | 1 +
 nav2_rotation_shim_controller/CMakeLists.txt           | 1 +
 nav2_rotation_shim_controller/package.xml              | 1 +
 nav2_route/CMakeLists.txt                              | 1 +
 nav2_route/package.xml                                 | 1 +
 nav2_rviz_plugins/CMakeLists.txt                       | 1 +
 nav2_rviz_plugins/package.xml                          | 1 +
 nav2_smac_planner/CMakeLists.txt                       | 1 +
 nav2_smac_planner/package.xml                          | 1 +
 nav2_smoother/CMakeLists.txt                           | 1 +
 nav2_smoother/package.xml                              | 1 +
 nav2_theta_star_planner/CMakeLists.txt                 | 1 +
 nav2_theta_star_planner/package.xml                    | 1 +
 nav2_util/CMakeLists.txt                               | 1 +
 nav2_util/package.xml                                  | 1 +
 nav2_velocity_smoother/CMakeLists.txt                  | 1 +
 nav2_velocity_smoother/package.xml                     | 1 +
 nav2_voxel_grid/CMakeLists.txt                         | 1 +
 nav2_voxel_grid/package.xml                            | 1 +
 nav2_waypoint_follower/CMakeLists.txt                  | 1 +
 nav2_waypoint_follower/package.xml                     | 1 +
 82 files changed, 84 insertions(+)

diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt
index aa6cf1a4ed7..4813b3cdd9b 100644
--- a/nav2_amcl/CMakeLists.txt
+++ b/nav2_amcl/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_amcl)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(nav_msgs REQUIRED)
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
index c1a2cbc2cea..69ac2d324b5 100644
--- a/nav2_amcl/package.xml
+++ b/nav2_amcl/package.xml
@@ -24,6 +24,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>message_filters</depend>
   <depend>nav_msgs</depend>
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index 4059e6da15e..de7d0f6237f 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_behavior_tree CXX)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(action_msgs REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
index e931df421e2..69bcb1466f7 100644
--- a/nav2_behavior_tree/package.xml
+++ b/nav2_behavior_tree/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>action_msgs</depend>
+  <depend>backward_ros</depend>
   <depend>behaviortree_cpp</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_common</depend>
diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt
index c67f9af628b..bd37a097c2f 100644
--- a/nav2_behaviors/CMakeLists.txt
+++ b/nav2_behaviors/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_behaviors)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml
index c3739a3461e..04f20f80782 100644
--- a/nav2_behaviors/package.xml
+++ b/nav2_behaviors/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt
index 5537046d246..b0999ee4eb8 100644
--- a/nav2_bringup/CMakeLists.txt
+++ b/nav2_bringup/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_bringup)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(navigation2 REQUIRED)
 
diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
index dc5ebc7bb19..d17f705cd1e 100644
--- a/nav2_bringup/package.xml
+++ b/nav2_bringup/package.xml
@@ -12,9 +12,11 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <build_depend>backward_ros</build_depend>
   <build_depend>navigation2</build_depend>
   <build_depend>launch_ros</build_depend>
 
+  <exec_depend>backward_ros</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>launch_ros</exec_depend>
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
index 2fa895dd8b8..83fa44fab7f 100644
--- a/nav2_bt_navigator/CMakeLists.txt
+++ b/nav2_bt_navigator/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_bt_navigator)
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_behavior_tree REQUIRED)
 find_package(nav2_common REQUIRED)
diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
index 825e8f9eec3..62e5b7307ea 100644
--- a/nav2_bt_navigator/package.xml
+++ b/nav2_bt_navigator/package.xml
@@ -11,6 +11,7 @@
   <build_depend>nav2_common</build_depend>
 
   <depend>ament_index_cpp</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_behavior_tree</depend>
   <depend>nav2_core</depend>
diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
index a7c22e59a09..e67c8ea7719 100644
--- a/nav2_collision_monitor/CMakeLists.txt
+++ b/nav2_collision_monitor/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_collision_monitor)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
index 47ffe9a6d60..6a8e5ace75b 100644
--- a/nav2_collision_monitor/package.xml
+++ b/nav2_collision_monitor/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_util</depend>
   <depend>nav2_costmap_2d</depend>
diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt
index 8504b8b23de..f0f1e22c32c 100644
--- a/nav2_common/CMakeLists.txt
+++ b/nav2_common/CMakeLists.txt
@@ -4,6 +4,7 @@ project(nav2_common NONE)
 
 find_package(ament_cmake_core REQUIRED)
 find_package(ament_cmake_python REQUIRED)
+find_package(backward_ros REQUIRED)
 
 ament_python_install_package(nav2_common)
 
diff --git a/nav2_common/package.xml b/nav2_common/package.xml
index 7aa6a3925eb..12bbf129ccd 100644
--- a/nav2_common/package.xml
+++ b/nav2_common/package.xml
@@ -7,6 +7,7 @@
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>
 
+  <depend>backward_ros</depend>
   <depend>launch</depend>
   <depend>launch_ros</depend>
   <depend>osrf_pycommon</depend>
diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt
index 31e83e77fe5..c4613f52496 100644
--- a/nav2_constrained_smoother/CMakeLists.txt
+++ b/nav2_constrained_smoother/CMakeLists.txt
@@ -4,6 +4,7 @@ project(nav2_constrained_smoother)
 set(CMAKE_BUILD_TYPE Release) # significant Ceres optimization speedup
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
 find_package(Eigen3 REQUIRED)
 find_package(geometry_msgs REQUIRED)
diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml
index 11fb47b1b06..eb5b60ae9b4 100644
--- a/nav2_constrained_smoother/package.xml
+++ b/nav2_constrained_smoother/package.xml
@@ -12,6 +12,7 @@
   <build_depend>eigen</build_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>libceres-dev</depend>
   <depend>nav2_core</depend>
diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt
index a100f96a214..56ea5d7e794 100644
--- a/nav2_controller/CMakeLists.txt
+++ b/nav2_controller/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_controller)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(nav2_core REQUIRED)
diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml
index 16875164c0d..b5be342451c 100644
--- a/nav2_controller/package.xml
+++ b/nav2_controller/package.xml
@@ -11,6 +11,7 @@
   <build_depend>nav2_common</build_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>lifecycle_msgs</depend>
   <depend>nav2_core</depend>
diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt
index 1d0e6b724f5..586410c3488 100644
--- a/nav2_core/CMakeLists.txt
+++ b/nav2_core/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_core)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_behavior_tree REQUIRED)
 find_package(nav2_common REQUIRED)
diff --git a/nav2_core/package.xml b/nav2_core/package.xml
index 044eb215676..aa95ad053fd 100644
--- a/nav2_core/package.xml
+++ b/nav2_core/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_behavior_tree</depend>
   <depend>nav2_costmap_2d</depend>
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index 221c5595515..851f2428b42 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_costmap_2d)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(laser_geometry REQUIRED)
 find_package(map_msgs REQUIRED)
diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml
index c01e7bb2dd6..c69d467b908 100644
--- a/nav2_costmap_2d/package.xml
+++ b/nav2_costmap_2d/package.xml
@@ -20,6 +20,7 @@
   <build_depend>nav2_common</build_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>laser_geometry</depend>
   <depend>map_msgs</depend>
diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt
index 83680e3eeae..a68bd4677ab 100644
--- a/nav2_docking/opennav_docking/CMakeLists.txt
+++ b/nav2_docking/opennav_docking/CMakeLists.txt
@@ -3,6 +3,7 @@ project(opennav_docking)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_graceful_controller REQUIRED)
diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml
index ecfa8082379..b65cb91ee56 100644
--- a/nav2_docking/opennav_docking/package.xml
+++ b/nav2_docking/opennav_docking/package.xml
@@ -11,6 +11,7 @@
   <build_depend>nav2_common</build_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_graceful_controller</depend>
   <depend>nav2_msgs</depend>
diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt
index 015265a3753..05e67fb713f 100644
--- a/nav2_docking/opennav_docking_bt/CMakeLists.txt
+++ b/nav2_docking/opennav_docking_bt/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(opennav_docking_bt)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_behavior_tree REQUIRED)
diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml
index 3b946c8c296..9fc0d3c0692 100644
--- a/nav2_docking/opennav_docking_bt/package.xml
+++ b/nav2_docking/opennav_docking_bt/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>behaviortree_cpp</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_behavior_tree</depend>
diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt
index 9575861645a..057bebb0b6c 100644
--- a/nav2_docking/opennav_docking_core/CMakeLists.txt
+++ b/nav2_docking/opennav_docking_core/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(opennav_docking_core)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml
index 735f89cf24d..285bf274cce 100644
--- a/nav2_docking/opennav_docking_core/package.xml
+++ b/nav2_docking/opennav_docking_core/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>tf2_ros</depend>
diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt
index 28d47ed3dd5..68dbfa64ef9 100644
--- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt
+++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(costmap_queue)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 
diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml
index 6d96d276a12..a74b57e69a6 100644
--- a/nav2_dwb_controller/costmap_queue/package.xml
+++ b/nav2_dwb_controller/costmap_queue/package.xml
@@ -9,6 +9,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>nav2_costmap_2d</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt
index ba787fb831c..b57c2291a51 100644
--- a/nav2_dwb_controller/dwb_core/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(dwb_core)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(dwb_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml
index ce879804bbc..a76dd8c474c 100644
--- a/nav2_dwb_controller/dwb_core/package.xml
+++ b/nav2_dwb_controller/dwb_core/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>builtin_interfaces</depend>
   <depend>dwb_msgs</depend>
   <depend>geometry_msgs</depend>
diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
index b1cb464cd5c..ddda0a9087b 100644
--- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
@@ -3,6 +3,7 @@ project(dwb_critics)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(costmap_queue REQUIRED)
 find_package(dwb_core REQUIRED)
 find_package(dwb_msgs REQUIRED)
diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
index 79daf191b20..eb923ca051e 100644
--- a/nav2_dwb_controller/dwb_critics/package.xml
+++ b/nav2_dwb_controller/dwb_critics/package.xml
@@ -9,6 +9,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>angles</depend>
   <depend>costmap_queue</depend>
   <depend>dwb_core</depend>
diff --git a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
index 2f34fcdf5ea..574a48c4c6a 100644
--- a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
@@ -1,6 +1,7 @@
 cmake_minimum_required(VERSION 3.5)
 project(dwb_msgs)
 
+find_package(backward_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_2d_msgs REQUIRED)
diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml
index 7c17869c2f3..9d1b3f47e6b 100644
--- a/nav2_dwb_controller/dwb_msgs/package.xml
+++ b/nav2_dwb_controller/dwb_msgs/package.xml
@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>backward_ros</depend>
   <depend>builtin_interfaces</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_2d_msgs</depend>
diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
index f725aa2cf94..7d4228ae427 100644
--- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(dwb_plugins)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(dwb_core REQUIRED)
 find_package(dwb_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
index d07a7b79f08..f25f724d95a 100644
--- a/nav2_dwb_controller/dwb_plugins/package.xml
+++ b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>dwb_core</depend>
   <depend>dwb_msgs</depend>
   <depend>geometry_msgs</depend>
diff --git a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
index e900fd292ca..5c1c8fce07a 100644
--- a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
+++ b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_dwb_controller)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(nav2_common REQUIRED)
 
 nav2_package()
diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml
index 7d8363bcfc2..8eea7edacb8 100644
--- a/nav2_dwb_controller/nav2_dwb_controller/package.xml
+++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml
@@ -11,8 +11,10 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>backward_ros</build_depend>
   <build_depend>nav2_common</build_depend>
 
+  <exec_depend>backward_ros</exec_depend>
   <exec_depend>costmap_queue</exec_depend>
   <exec_depend>dwb_core</exec_depend>
   <exec_depend>dwb_critics</exec_depend>
diff --git a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
index c2838963c9b..155c13cdd65 100644
--- a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
@@ -1,6 +1,7 @@
 cmake_minimum_required(VERSION 3.5)
 project(nav_2d_msgs)
 
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml
index 129c4d3367a..bd343580140 100644
--- a/nav2_dwb_controller/nav_2d_msgs/package.xml
+++ b/nav2_dwb_controller/nav_2d_msgs/package.xml
@@ -11,6 +11,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
   <depend>rosidl_default_generators</depend>
diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
index 8a729af46a9..c3005dffd6c 100644
--- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav_2d_utils)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_2d_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml
index ea8e213c77d..9968469eab4 100644
--- a/nav2_dwb_controller/nav_2d_utils/package.xml
+++ b/nav2_dwb_controller/nav_2d_utils/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_2d_msgs</depend>
   <depend>nav_msgs</depend>
diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt
index 1a72a698085..fc8859860b3 100644
--- a/nav2_graceful_controller/CMakeLists.txt
+++ b/nav2_graceful_controller/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_graceful_controller)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml
index a195e466bec..f162f3f0ed3 100644
--- a/nav2_graceful_controller/package.xml
+++ b/nav2_graceful_controller/package.xml
@@ -11,6 +11,7 @@
   <build_depend>nav2_common</build_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt
index 654b7153d06..64b1f4ad88d 100644
--- a/nav2_lifecycle_manager/CMakeLists.txt
+++ b/nav2_lifecycle_manager/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_lifecycle_manager)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(bondcpp REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 find_package(geometry_msgs REQUIRED)
diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml
index 99b0b2f9250..0b6790935fc 100644
--- a/nav2_lifecycle_manager/package.xml
+++ b/nav2_lifecycle_manager/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>bondcpp</depend>
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>
diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
index 438e1aa12fb..55160b2f7de 100644
--- a/nav2_map_server/CMakeLists.txt
+++ b/nav2_map_server/CMakeLists.txt
@@ -4,6 +4,7 @@ project(nav2_map_server)
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(GRAPHICSMAGICKCPP REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml
index f6d5f75a278..ffb3d3d1fce 100644
--- a/nav2_map_server/package.xml
+++ b/nav2_map_server/package.xml
@@ -14,6 +14,7 @@
 
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>nav_msgs</depend>
   <depend>std_msgs</depend>
diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index 5197a8859de..9dabc27507f 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_mppi_controller)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml
index adde8cf93dc..a27716ab997 100644
--- a/nav2_mppi_controller/package.xml
+++ b/nav2_mppi_controller/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>libomp-dev</depend>
   <depend>nav2_common</depend>
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index 6252ab32e4e..f01fbdba6a5 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_msgs)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(nav_msgs REQUIRED)
diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml
index 12a085fbf88..61e84addc37 100644
--- a/nav2_msgs/package.xml
+++ b/nav2_msgs/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>builtin_interfaces</depend>
diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt
index df0a924a40a..0da443e5e35 100644
--- a/nav2_navfn_planner/CMakeLists.txt
+++ b/nav2_navfn_planner/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_navfn_planner)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml
index e00bcefaf29..fe4c11b373a 100644
--- a/nav2_navfn_planner/package.xml
+++ b/nav2_navfn_planner/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>builtin_interfaces</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_costmap_2d</depend>
diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt
index 9c2e44e69b2..d3037ddf839 100644
--- a/nav2_planner/CMakeLists.txt
+++ b/nav2_planner/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_planner)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml
index b0cdf1c8846..50ad1add559 100644
--- a/nav2_planner/package.xml
+++ b/nav2_planner/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>lifecycle_msgs</depend>
   <depend>nav2_core</depend>
diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
index 2cd43c87154..493418fda67 100644
--- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
+++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_regulated_pure_pursuit_controller)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml
index 454b9542c7d..70df60839d7 100644
--- a/nav2_regulated_pure_pursuit_controller/package.xml
+++ b/nav2_regulated_pure_pursuit_controller/package.xml
@@ -12,6 +12,7 @@
   <build_depend>nav2_common</build_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
diff --git a/nav2_ros_common/CMakeLists.txt b/nav2_ros_common/CMakeLists.txt
index fc529730ea8..13dfd418833 100644
--- a/nav2_ros_common/CMakeLists.txt
+++ b/nav2_ros_common/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_ros_common)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(bond REQUIRED)
 find_package(bondcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
diff --git a/nav2_ros_common/package.xml b/nav2_ros_common/package.xml
index 33731f20e15..e2e2eb2695d 100644
--- a/nav2_ros_common/package.xml
+++ b/nav2_ros_common/package.xml
@@ -11,6 +11,7 @@
 
   <depend>nav2_common</depend>
 
+  <depend>backward_ros</depend>
   <depend>bond</depend>
   <depend>bondcpp</depend>
   <depend>builtin_interfaces</depend>
diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt
index c8140e661a6..0adcf17d8f1 100644
--- a/nav2_rotation_shim_controller/CMakeLists.txt
+++ b/nav2_rotation_shim_controller/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_rotation_shim_controller)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_controller REQUIRED)
diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml
index 2505599a071..02fbffd55f0 100644
--- a/nav2_rotation_shim_controller/package.xml
+++ b/nav2_rotation_shim_controller/package.xml
@@ -11,6 +11,7 @@
   <build_depend>nav2_common</build_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
   <depend>nav2_controller</depend>
diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt
index 3ceb96f6753..fcc53119651 100644
--- a/nav2_route/CMakeLists.txt
+++ b/nav2_route/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_route CXX)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
diff --git a/nav2_route/package.xml b/nav2_route/package.xml
index 855356eefc2..93fffbf8e1a 100644
--- a/nav2_route/package.xml
+++ b/nav2_route/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>std_msgs</depend>
diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
index 958047a22c2..62e19b2f7a2 100644
--- a/nav2_rviz_plugins/CMakeLists.txt
+++ b/nav2_rviz_plugins/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_rviz_plugins)
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_lifecycle_manager REQUIRED)
diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml
index 91de46fa84c..3a25157d1ce 100644
--- a/nav2_rviz_plugins/package.xml
+++ b/nav2_rviz_plugins/package.xml
@@ -12,6 +12,7 @@
   <build_depend>qtbase5-dev</build_depend>
 
   <depend>ament_index_cpp</depend>
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_lifecycle_manager</depend>
   <depend>nav2_msgs</depend>
diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
index a344a41aa0f..051818f7c9a 100644
--- a/nav2_smac_planner/CMakeLists.txt
+++ b/nav2_smac_planner/CMakeLists.txt
@@ -4,6 +4,7 @@ project(nav2_smac_planner)
 find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml
index f8acf8bd6a6..9400ea3b37b 100644
--- a/nav2_smac_planner/package.xml
+++ b/nav2_smac_planner/package.xml
@@ -12,6 +12,7 @@
 
   <depend>ament_index_cpp</depend>
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt
index b2e3555ab2a..465cadd670d 100644
--- a/nav2_smoother/CMakeLists.txt
+++ b/nav2_smoother/CMakeLists.txt
@@ -3,6 +3,7 @@ project(nav2_smoother)
 
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml
index e7d4708d790..c2983d916ce 100644
--- a/nav2_smoother/package.xml
+++ b/nav2_smoother/package.xml
@@ -12,6 +12,7 @@
   <build_depend>nav2_common</build_depend>
 
   <depend>angles</depend>
+  <depend>backward_ros</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_msgs</depend>
diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt
index 87da1fa9cbc..0a478f2fb9b 100644
--- a/nav2_theta_star_planner/CMakeLists.txt
+++ b/nav2_theta_star_planner/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_theta_star_planner)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml
index 0279b52fe7e..414de9f3066 100644
--- a/nav2_theta_star_planner/package.xml
+++ b/nav2_theta_star_planner/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt
index 08a7908f3b3..2a2f4061818 100644
--- a/nav2_util/CMakeLists.txt
+++ b/nav2_util/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_util)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(bond REQUIRED)
 find_package(bondcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
diff --git a/nav2_util/package.xml b/nav2_util/package.xml
index 0acd0f1a37b..334879c3981 100644
--- a/nav2_util/package.xml
+++ b/nav2_util/package.xml
@@ -13,6 +13,7 @@
 
   <depend>nav2_common</depend>
 
+  <depend>backward_ros</depend>
   <depend>bond</depend>
   <depend>bondcpp</depend>
   <depend>builtin_interfaces</depend>
diff --git a/nav2_velocity_smoother/CMakeLists.txt b/nav2_velocity_smoother/CMakeLists.txt
index 75c59b8a89c..916ee93ed2c 100644
--- a/nav2_velocity_smoother/CMakeLists.txt
+++ b/nav2_velocity_smoother/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_velocity_smoother)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_util REQUIRED)
diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml
index b0ed66dd3e6..52cfb61d9d7 100644
--- a/nav2_velocity_smoother/package.xml
+++ b/nav2_velocity_smoother/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_util</depend>
   <depend>rclcpp</depend>
diff --git a/nav2_voxel_grid/CMakeLists.txt b/nav2_voxel_grid/CMakeLists.txt
index e204367ada8..093fee48a84 100644
--- a/nav2_voxel_grid/CMakeLists.txt
+++ b/nav2_voxel_grid/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_voxel_grid)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 
diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml
index ef9eedd9b42..36d5b14ba6b 100644
--- a/nav2_voxel_grid/package.xml
+++ b/nav2_voxel_grid/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>rclcpp</depend>
 
   <test_depend>ament_lint_common</test_depend>
diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index f41c6e5f730..17d406adef9 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_waypoint_follower)
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(geographic_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml
index 4aa90590587..980881aa7a9 100644
--- a/nav2_waypoint_follower/package.xml
+++ b/nav2_waypoint_follower/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
+  <depend>backward_ros</depend>
   <depend>cv_bridge</depend>
   <depend>geographic_msgs</depend>
   <depend>geometry_msgs</depend>

From 6330ee464a82556b2388c683960972cc0937c3dd Mon Sep 17 00:00:00 2001
From: YLFeng <ericfengx@foxmail.com>
Date: Thu, 7 Aug 2025 01:02:41 +0800
Subject: [PATCH 09/51] Add IndexType definition for Nanoflann KDTree in
 `node_spatial_tree`. (#5420)

* fix: Add KDTree type definition to include unsigned int for IndexType

Signed-off-by: Ericsii <ericfengx@foxmail.com>

* code format

Signed-off-by: Ericsii <ericfengx@foxmail.com>

---------

Signed-off-by: Ericsii <ericfengx@foxmail.com>
---
 nav2_route/include/nav2_route/node_spatial_tree.hpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp
index 15d9e2a5528..744d30791c7 100644
--- a/nav2_route/include/nav2_route/node_spatial_tree.hpp
+++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp
@@ -55,7 +55,9 @@ struct GraphAdaptor
 };
 
 typedef nanoflann::KDTreeSingleIndexAdaptor<
-    nanoflann::L2_Simple_Adaptor<double, GraphAdaptor>, GraphAdaptor, DIMENSION> kd_tree_t;
+    nanoflann::L2_Simple_Adaptor<double, GraphAdaptor>, GraphAdaptor, DIMENSION,
+    unsigned int>
+  kd_tree_t;
 
 /**
  * @class nav2_route::NodeSpatialTree

From c7884f583ef72f758b6cf370db19df5a69df9c92 Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Thu, 7 Aug 2025 01:03:01 +0800
Subject: [PATCH 10/51] Update tf2 ros headers (#5381)

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 nav2_amcl/include/nav2_amcl/amcl_node.hpp              |  6 +++---
 nav2_amcl/src/amcl_node.cpp                            | 10 +++++-----
 .../plugins/action/truncate_path_local_action.hpp      |  2 +-
 .../plugins/condition/are_poses_near_condition.hpp     |  2 +-
 .../plugins/condition/distance_traveled_condition.hpp  |  2 +-
 .../plugins/condition/goal_reached_condition.hpp       |  2 +-
 .../condition/transform_available_condition.hpp        |  2 +-
 .../plugins/decorator/distance_controller.hpp          |  2 +-
 .../include/nav2_behavior_tree/ros_topic_logger.hpp    |  2 +-
 .../plugins/action/truncate_path_local_action.cpp      |  4 ++--
 .../condition/transform_available_condition.cpp        |  2 +-
 .../plugins/decorator/distance_controller.cpp          |  2 +-
 .../test/utils/test_transform_handler.hpp              |  6 +++---
 .../include/nav2_behaviors/behavior_server.hpp         |  4 ++--
 .../include/nav2_behaviors/timed_behavior.hpp          |  4 ++--
 .../include/nav2_bt_navigator/bt_navigator.hpp         |  6 +++---
 .../nav2_collision_monitor/collision_detector_node.hpp |  4 ++--
 .../nav2_collision_monitor/collision_monitor_node.hpp  |  4 ++--
 .../include/nav2_collision_monitor/polygon.hpp         |  2 +-
 .../include/nav2_collision_monitor/source.hpp          |  2 +-
 .../nav2_collision_monitor/velocity_polygon.hpp        |  2 +-
 nav2_collision_monitor/src/collision_detector_node.cpp |  2 +-
 nav2_collision_monitor/src/collision_monitor_node.cpp  |  2 +-
 .../test/collision_detector_node_test.cpp              |  2 +-
 .../test/collision_monitor_node_test.cpp               |  2 +-
 nav2_collision_monitor/test/polygons_test.cpp          |  6 +++---
 nav2_collision_monitor/test/sources_test.cpp           |  6 +++---
 nav2_collision_monitor/test/velocity_polygons_test.cpp |  6 +++---
 .../test/test_constrained_smoother.cpp                 |  2 +-
 .../include/nav2_controller/controller_server.hpp      |  2 +-
 nav2_core/include/nav2_core/behavior.hpp               |  2 +-
 .../include/nav2_core/behavior_tree_navigator.hpp      |  2 +-
 nav2_core/include/nav2_core/controller.hpp             |  2 +-
 nav2_core/include/nav2_core/global_planner.hpp         |  2 +-
 nav2_core/include/nav2_core/smoother.hpp               |  4 ++--
 .../include/nav2_costmap_2d/costmap_2d_ros.hpp         |  4 ++--
 nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp      |  2 +-
 .../include/nav2_costmap_2d/observation_buffer.hpp     |  2 +-
 .../include/nav2_costmap_2d/obstacle_layer.hpp         |  2 +-
 .../include/nav2_costmap_2d/plugin_container_layer.hpp |  2 +-
 nav2_costmap_2d/src/costmap_2d_ros.cpp                 |  2 +-
 nav2_costmap_2d/test/integration/costmap_tester.cpp    |  2 +-
 nav2_costmap_2d/test/integration/dyn_params_tests.cpp  |  2 +-
 nav2_costmap_2d/test/integration/footprint_tests.cpp   |  2 +-
 .../test/integration/test_costmap_2d_publisher.cpp     |  2 +-
 .../test_costmap_topic_collision_checker.cpp           |  8 ++++----
 nav2_costmap_2d/test/unit/binary_filter_test.cpp       |  6 +++---
 nav2_costmap_2d/test/unit/keepout_filter_test.cpp      |  6 +++---
 nav2_costmap_2d/test/unit/speed_filter_test.cpp        |  6 +++---
 .../include/opennav_docking/docking_server.hpp         |  2 +-
 nav2_docking/opennav_docking/test/test_controller.cpp  |  2 +-
 .../include/opennav_docking_core/charging_dock.hpp     |  2 +-
 .../nav_2d_utils/include/nav_2d_utils/tf_help.hpp      |  2 +-
 nav2_mppi_controller/benchmark/optimizer_benchmark.cpp |  2 +-
 .../optimal_trajectory_validator.hpp                   |  2 +-
 .../include/nav2_mppi_controller/optimizer.hpp         |  2 +-
 .../nav2_mppi_controller/tools/path_handler.hpp        |  2 +-
 nav2_mppi_controller/test/optimizer_smoke_test.cpp     |  2 +-
 nav2_mppi_controller/test/optimizer_unit_tests.cpp     |  2 +-
 nav2_mppi_controller/test/path_handler_test.cpp        |  2 +-
 nav2_mppi_controller/test/utils/utils.hpp              |  2 +-
 nav2_planner/include/nav2_planner/planner_server.hpp   |  4 ++--
 .../test/test_shim_controller.cpp                      |  2 +-
 nav2_route/include/nav2_route/edge_scorer.hpp          |  2 +-
 .../include/nav2_route/goal_intent_extractor.hpp       |  2 +-
 nav2_route/include/nav2_route/graph_loader.hpp         |  4 ++--
 nav2_route/include/nav2_route/graph_saver.hpp          |  4 ++--
 .../nav2_route/interfaces/edge_cost_function.hpp       |  2 +-
 nav2_route/include/nav2_route/route_planner.hpp        |  4 ++--
 nav2_route/include/nav2_route/route_server.hpp         |  4 ++--
 nav2_route/include/nav2_route/route_tracker.hpp        |  2 +-
 nav2_route/test/test_edge_scorers.cpp                  |  4 ++--
 nav2_route/test/test_goal_intent_extractor.cpp         |  6 +++---
 nav2_route/test/test_goal_intent_search.cpp            |  6 +++---
 nav2_route/test/test_graph_loader.cpp                  |  2 +-
 nav2_route/test/test_graph_saver.cpp                   |  2 +-
 nav2_route/test/test_route_server.cpp                  |  6 +++---
 nav2_route/test/test_route_tracker.cpp                 |  6 +++---
 .../include/nav2_rviz_plugins/nav2_panel.hpp           |  6 +++---
 nav2_smoother/src/nav2_smoother.cpp                    |  2 +-
 .../src/behavior_tree/test_behavior_tree_node.cpp      |  6 +++---
 .../assisted_teleop_behavior_tester.hpp                |  4 ++--
 .../src/behaviors/wait/wait_behavior_tester.hpp        |  4 ++--
 nav2_system_tests/src/planning/planner_tester.hpp      |  2 +-
 nav2_util/include/nav2_util/robot_utils.hpp            |  2 +-
 nav2_util/src/base_footprint_publisher.hpp             |  8 ++++----
 nav2_util/test/test_robot_utils.cpp                    |  6 +++---
 .../nav2_waypoint_follower/waypoint_follower.hpp       |  4 ++--
 88 files changed, 146 insertions(+), 146 deletions(-)

diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
index 0be08c39db6..65787e82b00 100644
--- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp
+++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -44,9 +44,9 @@
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "nav2_ros_common/service_server.hpp"
 #include "std_srvs/srv/empty.hpp"
-#include "tf2_ros/message_filter.h"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/message_filter.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/transform_listener.hpp"
 
 #define NEW_UNIFORM_SAMPLING 1
 
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index e7910c583b1..93b8f599a8d 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -38,11 +38,11 @@
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/LinearMath/Transform.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/message_filter.h"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/message_filter.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 
 #include "nav2_amcl/portable_utils.hpp"
 #include "nav2_ros_common/validate_messages.hpp"
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
index 6ce8a76fd64..9ffc5aa773f 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
@@ -25,7 +25,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
 #include "nav2_behavior_tree/json_utils.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 
 
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp
index db615c06181..eb099f329b0 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp
@@ -20,7 +20,7 @@
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "behaviortree_cpp/condition_node.h"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
index abcf722b872..c75f8b78d98 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
@@ -23,7 +23,7 @@
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
index aeebfcb3634..db3c56f24b7 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
@@ -23,7 +23,7 @@
 #include "behaviortree_cpp/json_export.h"
 #include "nav2_behavior_tree/bt_utils.hpp"
 #include "nav2_behavior_tree/json_utils.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 
 namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp
index 41a86ee813d..be1bcd6ee18 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp
@@ -21,7 +21,7 @@
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "behaviortree_cpp/condition_node.h"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 namespace nav2_behavior_tree
 {
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
index 64e4a7c4a75..782e89a8f89 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
@@ -20,7 +20,7 @@
 #include <string>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 #include "behaviortree_cpp/decorator_node.h"
 #include "nav2_behavior_tree/bt_utils.hpp"
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp
index a8d27ecd96f..57ca3d4ffc3 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp
@@ -24,7 +24,7 @@
 #include "nav2_msgs/msg/behavior_tree_log.hpp"
 #include "nav2_msgs/msg/behavior_tree_status_change.hpp"
 #include "tf2/time.hpp"
-#include "tf2_ros/buffer_interface.h"
+#include "tf2_ros/buffer_interface.hpp"
 
 namespace nav2_behavior_tree
 {
diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp
index bf8e58b9184..7cae783bb9f 100644
--- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp
+++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp
@@ -25,8 +25,8 @@
 #include "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/LinearMath/Quaternion.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 
 #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
 
diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp
index b377d47c585..914a2be547a 100644
--- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp
@@ -18,7 +18,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/time.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 #include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp"
 
diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp
index 0fdd47e7ffb..b560cddf2e7 100644
--- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp
+++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp
@@ -21,7 +21,7 @@
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 #include "behaviortree_cpp/decorator_node.h"
 
diff --git a/nav2_behavior_tree/test/utils/test_transform_handler.hpp b/nav2_behavior_tree/test/utils/test_transform_handler.hpp
index 19cfcdfa222..cf272d132cd 100644
--- a/nav2_behavior_tree/test/utils/test_transform_handler.hpp
+++ b/nav2_behavior_tree/test/utils/test_transform_handler.hpp
@@ -29,9 +29,9 @@
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/buffer.hpp"
 
 using namespace std::chrono_literals; // NOLINT
 using namespace std::chrono;  // NOLINT
diff --git a/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp
index 7301cef3836..82b55c0786f 100644
--- a/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp
@@ -19,8 +19,8 @@
 #include <vector>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav2_core/behavior.hpp"
diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
index 70d0f30fa30..22db8701700 100644
--- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
+++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
@@ -26,8 +26,8 @@
 #include <utility>
 
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/twist_publisher.hpp"
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 8a25e5a2c8d..adea1aba20f 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -23,9 +23,9 @@
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_util/odometry_utils.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 #include "nav2_core/behavior_tree_navigator.hpp"
 #include "pluginlib/class_loader.hpp"
 
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp
index 59b807f562e..450e97c8078 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp
@@ -23,8 +23,8 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "tf2/time.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_msgs/msg/collision_detector_state.hpp"
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
index d73804cb827..c53540089d7 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
@@ -26,8 +26,8 @@
 #include "geometry_msgs/msg/twist_stamped.hpp"
 
 #include "tf2/time.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_util/twist_publisher.hpp"
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
index 12d64259993..3191de1727e 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -24,7 +24,7 @@
 #include "geometry_msgs/msg/polygon_stamped.hpp"
 
 #include "tf2/time.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
index 312c13ea577..4e91dab13c7 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
@@ -22,7 +22,7 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "tf2/time.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp
index 55f894d87c5..8056973ddf9 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp
@@ -24,7 +24,7 @@
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 namespace nav2_collision_monitor
 {
diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp
index 739d0d12207..a45d35dc3cd 100644
--- a/nav2_collision_monitor/src/collision_detector_node.cpp
+++ b/nav2_collision_monitor/src/collision_detector_node.cpp
@@ -19,7 +19,7 @@
 #include <utility>
 #include <functional>
 
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/create_timer_ros.hpp"
 
 #include "nav2_ros_common/node_utils.hpp"
 
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index f85bdf4a866..7bff3a8a4d1 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -18,7 +18,7 @@
 #include <utility>
 #include <functional>
 
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/create_timer_ros.hpp"
 
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp
index 1bd2eb0ca0c..4bbd37bc4e9 100644
--- a/nav2_collision_monitor/test/collision_detector_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp
@@ -34,7 +34,7 @@
 #include "geometry_msgs/msg/polygon_stamped.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/collision_detector_node.hpp"
diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
index d64f0bd71e8..ea0213f468c 100644
--- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -34,7 +34,7 @@
 #include "geometry_msgs/msg/polygon_stamped.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/collision_monitor_node.hpp"
diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp
index 52eff3c650b..86f393febfa 100644
--- a/nav2_collision_monitor/test/polygons_test.cpp
+++ b/nav2_collision_monitor/test/polygons_test.cpp
@@ -27,9 +27,9 @@
 #include "geometry_msgs/msg/point32.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
 
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp
index 04929bd339c..034cdd40d96 100644
--- a/nav2_collision_monitor/test/sources_test.cpp
+++ b/nav2_collision_monitor/test/sources_test.cpp
@@ -30,9 +30,9 @@
 #include "sensor_msgs/msg/range.hpp"
 #include "sensor_msgs/point_cloud2_iterator.hpp"
 
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/scan.hpp"
diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp
index d13fc873340..88024dd723b 100644
--- a/nav2_collision_monitor/test/velocity_polygons_test.cpp
+++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp
@@ -27,9 +27,9 @@
 #include "geometry_msgs/msg/point32.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
 
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
index b9285fa9240..6e4e3d4bc90 100644
--- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp
+++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
@@ -23,7 +23,7 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/create_timer_ros.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp
index a112a3c9d66..a838854a52b 100644
--- a/nav2_controller/include/nav2_controller/controller_server.hpp
+++ b/nav2_controller/include/nav2_controller/controller_server.hpp
@@ -26,7 +26,7 @@
 #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 "tf2_ros/transform_listener.hpp"
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_msgs/msg/speed_limit.hpp"
 #include "nav_2d_utils/odom_subscriber.hpp"
diff --git a/nav2_core/include/nav2_core/behavior.hpp b/nav2_core/include/nav2_core/behavior.hpp
index b2e00e8cf53..7a58d3aa2a0 100644
--- a/nav2_core/include/nav2_core/behavior.hpp
+++ b/nav2_core/include/nav2_core/behavior.hpp
@@ -20,7 +20,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 
 namespace nav2_core
diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
index 87824c7fd31..fda64cec974 100644
--- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
+++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
@@ -21,7 +21,7 @@
 #include <mutex>
 
 #include "nav2_util/odometry_utils.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "pluginlib/class_loader.hpp"
diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp
index 261eec22436..d24e84d6680 100644
--- a/nav2_core/include/nav2_core/controller.hpp
+++ b/nav2_core/include/nav2_core/controller.hpp
@@ -41,7 +41,7 @@
 
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_listener.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp
index bcf65da6d4f..fa63d2e519e 100644
--- a/nav2_core/include/nav2_core/global_planner.hpp
+++ b/nav2_core/include/nav2_core/global_planner.hpp
@@ -18,7 +18,7 @@
 #include <memory>
 #include <string>
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
diff --git a/nav2_core/include/nav2_core/smoother.hpp b/nav2_core/include/nav2_core/smoother.hpp
index 7705106177d..deb4a6107ec 100644
--- a/nav2_core/include/nav2_core/smoother.hpp
+++ b/nav2_core/include/nav2_core/smoother.hpp
@@ -20,8 +20,8 @@
 
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "nav_msgs/msg/path.hpp"
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 b18938352a3..5b1fa01591b 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
@@ -56,8 +56,8 @@
 #include "pluginlib/class_loader.hpp"
 #include "tf2/convert.hpp"
 #include "tf2/LinearMath/Transform.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "tf2/time.hpp"
 #include "tf2/transform_datatypes.hpp"
 #include "nav2_ros_common/service_server.hpp"
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
index 2136db7c856..571df68c9d3 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
@@ -41,7 +41,7 @@
 #include <vector>
 #include <unordered_set>
 
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/layered_costmap.hpp"
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp
index c54d586b729..09f0e79c114 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp
@@ -43,7 +43,7 @@
 
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "rclcpp/time.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "nav2_costmap_2d/observation.hpp"
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 16113d61b16..5e66e24795a 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -47,7 +47,7 @@
 #include "laser_geometry/laser_geometry.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wreorder"
-#include "tf2_ros/message_filter.h"
+#include "tf2_ros/message_filter.hpp"
 #pragma GCC diagnostic pop
 
 #include "message_filters/subscriber.hpp"
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp
index 2e00a412637..8a9aeb29623 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp
@@ -27,7 +27,7 @@
 #include "nav2_costmap_2d/costmap_layer.hpp"
 #include "nav2_costmap_2d/observation_buffer.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
-#include "tf2_ros/message_filter.h"
+#include "tf2_ros/message_filter.hpp"
 #include "pluginlib/class_loader.hpp"
 
 using nav2_costmap_2d::LETHAL_OBSTACLE;
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 49dd1885fea..f43373d286b 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -48,7 +48,7 @@
 #include "nav2_util/execution_timer.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/create_timer_ros.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
 
diff --git a/nav2_costmap_2d/test/integration/costmap_tester.cpp b/nav2_costmap_2d/test/integration/costmap_tester.cpp
index c7d44e6e70c..8569cae54e3 100644
--- a/nav2_costmap_2d/test/integration/costmap_tester.cpp
+++ b/nav2_costmap_2d/test/integration/costmap_tester.cpp
@@ -38,7 +38,7 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 
diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp
index a543dda02ae..94e0f72ea77 100644
--- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp
+++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp
@@ -19,7 +19,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 class DynParamTestNode
 {
diff --git a/nav2_costmap_2d/test/integration/footprint_tests.cpp b/nav2_costmap_2d/test/integration/footprint_tests.cpp
index 2f527b5faac..0bde175ce0a 100644
--- a/nav2_costmap_2d/test/integration/footprint_tests.cpp
+++ b/nav2_costmap_2d/test/integration/footprint_tests.cpp
@@ -41,7 +41,7 @@
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_costmap_2d/footprint.hpp"
 
 class FootprintTestNode
diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp
index c0616176647..a365cdc9a7d 100644
--- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp
@@ -19,7 +19,7 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 
 class CostmapRosLifecycleNode : public nav2::LifecycleNode
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 1543060ccca..e5b73e4b5cc 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
@@ -29,10 +29,10 @@
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.hpp"
diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp
index befb8d00e1e..e0061e91e88 100644
--- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp
@@ -24,9 +24,9 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/occ_grid_values.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
index c75acf2b7dc..37a5a683299 100644
--- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
@@ -22,9 +22,9 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
 #include "nav2_util/occ_grid_values.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp
index 76e96187924..5d3eebb6db5 100644
--- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp
@@ -24,9 +24,9 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
 #include "nav2_util/occ_grid_values.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
index 06c6a7252e0..eb1497f21db 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
@@ -34,7 +34,7 @@
 #include "opennav_docking/dock_database.hpp"
 #include "opennav_docking/navigator.hpp"
 #include "opennav_docking_core/charging_dock.hpp"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_listener.hpp"
 
 namespace opennav_docking
 {
diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp
index f0e982a919b..8708b31428d 100644
--- a/nav2_docking/opennav_docking/test/test_controller.cpp
+++ b/nav2_docking/opennav_docking/test/test_controller.cpp
@@ -21,7 +21,7 @@
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_ros_common/node_utils.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
 // Testing the controller at high level; the nav2_graceful_controller
diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp
index a923cc98d26..67e599405d7 100644
--- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp
+++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp
@@ -20,7 +20,7 @@
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 
 namespace opennav_docking_core
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 9bed6cd6810..3f5fadcd4aa 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,7 +37,7 @@
 
 #include <string>
 #include <memory>
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp
index c59f28e2fad..afe8e83eea9 100644
--- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp
+++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp
@@ -28,7 +28,7 @@
 #include <nav2_costmap_2d/costmap_2d.hpp>
 #include <nav2_costmap_2d/costmap_2d_ros.hpp>
 #include <nav2_core/goal_checker.hpp>
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 #include "nav2_mppi_controller/optimizer.hpp"
 #include "nav2_mppi_controller/motion_models.hpp"
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp
index b5d88008ab1..b3f7b03faab 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimal_trajectory_validator.hpp
@@ -21,7 +21,7 @@
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
index 5eba18b9d5d..e70cfcab786 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
@@ -26,7 +26,7 @@
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_core/goal_checker.hpp"
 #include "nav2_core/controller_exceptions.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "pluginlib/class_loader.hpp"
 
 #include "geometry_msgs/msg/twist.hpp"
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
index cfa87f5ace4..544a98467d9 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
@@ -23,7 +23,7 @@
 #include <memory>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "builtin_interfaces/msg/time.hpp"
diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp
index d201e20eeb8..733229b77df 100644
--- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp
+++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp
@@ -21,7 +21,7 @@
 #include <nav2_costmap_2d/costmap_2d.hpp>
 #include <nav2_costmap_2d/costmap_2d_ros.hpp>
 #include <nav2_core/goal_checker.hpp>
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 #include "nav2_mppi_controller/optimizer.hpp"
 #include "nav2_mppi_controller/tools/parameters_handler.hpp"
diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp
index 510d9ef300b..eec9ea0ec57 100644
--- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp
+++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp
@@ -18,7 +18,7 @@
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_mppi_controller/optimizer.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 
 // Tests main optimizer functions
 
diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp
index 375978e98da..eb0bd140c43 100644
--- a/nav2_mppi_controller/test/path_handler_test.cpp
+++ b/nav2_mppi_controller/test/path_handler_test.cpp
@@ -18,7 +18,7 @@
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_mppi_controller/tools/path_handler.hpp"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 // Tests path handling
 
diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp
index b0185ce7cfe..2772ea9a858 100644
--- a/nav2_mppi_controller/test/utils/utils.hpp
+++ b/nav2_mppi_controller/test/utils/utils.hpp
@@ -22,7 +22,7 @@
 
 #include <rclcpp/executors.hpp>
 
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
index c9fc2a46dd6..372f4c7c2f9 100644
--- a/nav2_planner/include/nav2_planner/planner_server.hpp
+++ b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -32,8 +32,8 @@
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_ros_common/service_server.hpp"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp
index 0b97876182f..ee87908d827 100644
--- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp
+++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp
@@ -24,7 +24,7 @@
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_controller/plugins/simple_goal_checker.hpp"
 #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 class RotationShimShim : public nav2_rotation_shim_controller::RotationShimController
 {
diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp
index c294ac47e43..7b6534b50c4 100644
--- a/nav2_route/include/nav2_route/edge_scorer.hpp
+++ b/nav2_route/include/nav2_route/edge_scorer.hpp
@@ -19,7 +19,7 @@
 #include <memory>
 #include <vector>
 
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav2_ros_common/node_utils.hpp"
diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp
index db26121fcef..8e407d66c91 100644
--- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp
+++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp
@@ -19,7 +19,7 @@
 #include <memory>
 #include <vector>
 
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_core/route_exceptions.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_ros_common/node_utils.hpp"
diff --git a/nav2_route/include/nav2_route/graph_loader.hpp b/nav2_route/include/nav2_route/graph_loader.hpp
index 2a39da1bee1..c2235e5e914 100644
--- a/nav2_route/include/nav2_route/graph_loader.hpp
+++ b/nav2_route/include/nav2_route/graph_loader.hpp
@@ -23,8 +23,8 @@
 #include <pluginlib/class_loader.hpp>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_route/types.hpp"
diff --git a/nav2_route/include/nav2_route/graph_saver.hpp b/nav2_route/include/nav2_route/graph_saver.hpp
index 7cc25d83c04..8155689df55 100644
--- a/nav2_route/include/nav2_route/graph_saver.hpp
+++ b/nav2_route/include/nav2_route/graph_saver.hpp
@@ -23,8 +23,8 @@
 #include <pluginlib/class_loader.hpp>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_route/types.hpp"
diff --git a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp
index e20bade1070..e45980f3a45 100644
--- a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp
+++ b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp
@@ -18,7 +18,7 @@
 #include <memory>
 #include <string>
 
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "nav2_route/types.hpp"
diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp
index bdadc6c9dd0..cd0c978bc90 100644
--- a/nav2_route/include/nav2_route/route_planner.hpp
+++ b/nav2_route/include/nav2_route/route_planner.hpp
@@ -22,8 +22,8 @@
 #include <mutex>
 #include <algorithm>
 
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_route/types.hpp"
 #include "nav2_route/utils.hpp"
 #include "nav2_route/edge_scorer.hpp"
diff --git a/nav2_route/include/nav2_route/route_server.hpp b/nav2_route/include/nav2_route/route_server.hpp
index 3d384e93c0b..ce52c07e750 100644
--- a/nav2_route/include/nav2_route/route_server.hpp
+++ b/nav2_route/include/nav2_route/route_server.hpp
@@ -23,8 +23,8 @@
 #include <mutex>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
diff --git a/nav2_route/include/nav2_route/route_tracker.hpp b/nav2_route/include/nav2_route/route_tracker.hpp
index 9c2e7058aec..a2a3d4e897c 100644
--- a/nav2_route/include/nav2_route/route_tracker.hpp
+++ b/nav2_route/include/nav2_route/route_tracker.hpp
@@ -16,7 +16,7 @@
 #include <string>
 #include <memory>
 
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_msgs/action/compute_and_track_route.hpp"
diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp
index 07af4293d0b..8c16ced8306 100644
--- a/nav2_route/test/test_edge_scorers.cpp
+++ b/nav2_route/test/test_edge_scorers.cpp
@@ -27,8 +27,8 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
 #include "nav2_core/route_exceptions.hpp"
-#include "tf2_ros/static_transform_broadcaster.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/static_transform_broadcaster.hpp"
+#include "tf2_ros/transform_listener.hpp"
 
 class RclCppFixture
 {
diff --git a/nav2_route/test/test_goal_intent_extractor.cpp b/nav2_route/test/test_goal_intent_extractor.cpp
index c3ac8d176cc..21c49708193 100644
--- a/nav2_route/test/test_goal_intent_extractor.cpp
+++ b/nav2_route/test/test_goal_intent_extractor.cpp
@@ -19,9 +19,9 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/service_client.hpp"
 #include "nav2_ros_common/node_thread.hpp"
diff --git a/nav2_route/test/test_goal_intent_search.cpp b/nav2_route/test/test_goal_intent_search.cpp
index a829af0370f..1f3e8fd4b1e 100644
--- a/nav2_route/test/test_goal_intent_search.cpp
+++ b/nav2_route/test/test_goal_intent_search.cpp
@@ -19,9 +19,9 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/service_client.hpp"
 #include "nav2_ros_common/node_thread.hpp"
diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp
index 99341852f78..e76fe863314 100644
--- a/nav2_route/test/test_graph_loader.cpp
+++ b/nav2_route/test/test_graph_loader.cpp
@@ -20,7 +20,7 @@
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_route/graph_loader.hpp"
-#include "tf2_ros/static_transform_broadcaster.h"
+#include "tf2_ros/static_transform_broadcaster.hpp"
 
 class RclCppFixture
 {
diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp
index a6897f11602..4abdc0b74d3 100644
--- a/nav2_route/test/test_graph_saver.cpp
+++ b/nav2_route/test/test_graph_saver.cpp
@@ -22,7 +22,7 @@
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_route/graph_loader.hpp"
 #include "nav2_route/graph_saver.hpp"
-#include "tf2_ros/static_transform_broadcaster.h"
+#include "tf2_ros/static_transform_broadcaster.hpp"
 
 class RclCppFixture
 {
diff --git a/nav2_route/test/test_route_server.cpp b/nav2_route/test/test_route_server.cpp
index f8906529d31..5fb73b4ce0f 100644
--- a/nav2_route/test/test_route_server.cpp
+++ b/nav2_route/test/test_route_server.cpp
@@ -19,9 +19,9 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "std_srvs/srv/trigger.hpp"
 #include "nav2_ros_common/service_client.hpp"
diff --git a/nav2_route/test/test_route_tracker.cpp b/nav2_route/test/test_route_tracker.cpp
index 9d874a1dc5a..07ca24a4e91 100644
--- a/nav2_route/test/test_route_tracker.cpp
+++ b/nav2_route/test/test_route_tracker.cpp
@@ -19,9 +19,9 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
+#include "tf2_ros/transform_listener.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_core/route_exceptions.hpp"
 #include "nav2_route/route_tracker.hpp"
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 3a4cdd9e50f..44ad28d279e 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
@@ -35,9 +35,9 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 #include "nav2_util/geometry_utils.hpp"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
+#include "tf2_ros/buffer.hpp"
 
 class QPushButton;
 
diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
index 4408b5a43e0..eb852c1eb52 100644
--- a/nav2_smoother/src/nav2_smoother.cpp
+++ b/nav2_smoother/src/nav2_smoother.cpp
@@ -25,7 +25,7 @@
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav_2d_utils/conversions.hpp"
 #include "nav_2d_utils/tf_help.hpp"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/create_timer_ros.hpp"
 
 using namespace std::chrono_literals;
 
diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
index 2f62894abb7..d36535924e0 100644
--- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
+++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
@@ -26,9 +26,9 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "behaviortree_cpp/utils/shared_library.h"
 
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/create_timer_ros.hpp"
 
 #include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/string_utils.hpp"
diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp
index bb3f3b18159..40e2aa9a754 100644
--- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp
+++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp
@@ -35,8 +35,8 @@
 #include "std_msgs/msg/empty.hpp"
 
 #include "tf2/utils.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 
 namespace nav2_system_tests
 {
diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp
index 5a7313c0cd5..bea4506e68e 100644
--- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp
+++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp
@@ -33,8 +33,8 @@
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 
 #include "tf2/utils.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
 
 namespace nav2_system_tests
 {
diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp
index d878bc49f22..3c14f795d4f 100644
--- a/nav2_system_tests/src/planning/planner_tester.hpp
+++ b/nav2_system_tests/src/planning/planner_tester.hpp
@@ -32,7 +32,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "nav2_planner/planner_server.hpp"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.hpp"
 
 namespace nav2_system_tests
 {
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
index cffbe80a26b..faca6d301dc 100644
--- a/nav2_util/include/nav2_util/robot_utils.hpp
+++ b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -26,7 +26,7 @@
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "tf2/time.hpp"
 #include "tf2/transform_datatypes.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "rclcpp/rclcpp.hpp"
 
diff --git a/nav2_util/src/base_footprint_publisher.hpp b/nav2_util/src/base_footprint_publisher.hpp
index 67f50625101..15b16037bed 100644
--- a/nav2_util/src/base_footprint_publisher.hpp
+++ b/nav2_util/src/base_footprint_publisher.hpp
@@ -21,10 +21,10 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
-#include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/create_timer_ros.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/buffer.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/utils.hpp"
 
diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp
index 119addc9355..33c55aa2a3b 100644
--- a/nav2_util/test/test_robot_utils.cpp
+++ b/nav2_util/test/test_robot_utils.cpp
@@ -16,11 +16,11 @@
 #include <cmath>
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_util/robot_utils.hpp"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "gtest/gtest.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/create_timer_ros.hpp"
 
 TEST(RobotUtils, LookupExceptionError)
 {
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 83edf012880..816feb9eca3 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
@@ -37,9 +37,9 @@
 #include "nav2_core/waypoint_task_executor.hpp"
 
 #include "robot_localization/srv/from_ll.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_listener.hpp"
 
 namespace nav2_waypoint_follower
 {

From 9cd0f9fc977242a30f4921c8fdb99ada2ec61093 Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Thu, 7 Aug 2025 04:37:29 +0800
Subject: [PATCH 11/51] Move nav_2d_utils to nav2_util (#5414)

* Move nav_2d_util to nav2_util

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Rename frame

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Replace OdomSubscriber with OdomSmoother

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Use transformPoseInTargetFrame in all controllers

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 nav2_controller/CMakeLists.txt                |   9 --
 .../nav2_controller/controller_server.hpp     |  14 +--
 nav2_controller/package.xml                   |   2 -
 .../plugins/pose_progress_checker.cpp         |   1 -
 .../plugins/simple_progress_checker.cpp       |   1 -
 nav2_controller/plugins/test/CMakeLists.txt   |   4 +-
 nav2_controller/plugins/test/goal_checker.cpp |   1 -
 .../plugins/test/progress_checker.cpp         |   1 -
 nav2_controller/src/controller_server.cpp     |  33 +++---
 .../opennav_docking/docking_server.hpp        |   4 +-
 .../opennav_docking/src/controller.cpp        |   1 -
 .../opennav_docking/src/docking_server.cpp    |   7 +-
 nav2_dwb_controller/dwb_core/CMakeLists.txt   |   1 -
 .../include/dwb_core/dwb_local_planner.hpp    |   2 +-
 .../dwb_core/src/dwb_local_planner.cpp        |  22 ++--
 .../dwb_critics/src/goal_align.cpp            |   6 +-
 .../dwb_critics/src/oscillation.cpp           |  12 +--
 .../dwb_critics/src/path_align.cpp            |   4 +-
 .../dwb_critics/src/rotate_to_goal.cpp        |  13 +--
 .../dwb_plugins/src/kinematic_parameters.cpp  |   1 -
 .../src/limited_accel_generator.cpp           |   4 +-
 .../src/standard_traj_generator.cpp           |   1 -
 .../dwb_plugins/src/xy_theta_iterator.cpp     |   1 -
 .../nav_2d_utils/CMakeLists.txt               |  25 +----
 .../include/nav_2d_utils/conversions.hpp      |   4 -
 .../include/nav_2d_utils/odom_subscriber.hpp  | 100 ------------------
 .../include/nav_2d_utils/parameters.hpp       |  76 -------------
 .../include/nav_2d_utils/tf_help.hpp          |  85 ---------------
 .../nav_2d_utils/src/conversions.cpp          |  28 -----
 .../nav_2d_utils/src/tf_help.cpp              |  98 -----------------
 .../nav_2d_utils/test/2d_utils_test.cpp       |  60 +++++------
 .../nav_2d_utils/test/CMakeLists.txt          |   3 -
 .../nav_2d_utils/test/tf_help_test.cpp        |  98 -----------------
 nav2_graceful_controller/CMakeLists.txt       |   3 -
 .../nav2_graceful_controller/path_handler.hpp |   4 +-
 nav2_graceful_controller/package.xml          |   1 -
 .../src/graceful_controller.cpp               |   2 +-
 nav2_graceful_controller/src/path_handler.cpp |  14 +--
 .../tools/path_handler.hpp                    |  11 --
 nav2_mppi_controller/src/path_handler.cpp     |  33 ++----
 .../test/path_handler_test.cpp                |  11 +-
 .../path_handler.hpp                          |  16 +--
 .../src/path_handler.cpp                      |  31 ++----
 .../src/regulated_pure_pursuit_controller.cpp |   2 +-
 nav2_smoother/CMakeLists.txt                  |   3 -
 nav2_smoother/package.xml                     |   1 -
 nav2_smoother/src/nav2_smoother.cpp           |   2 -
 nav2_util/src/robot_utils.cpp                 |   5 +
 48 files changed, 122 insertions(+), 739 deletions(-)
 delete mode 100644 nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
 delete mode 100644 nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp
 delete mode 100644 nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp
 delete mode 100644 nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp
 delete mode 100644 nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp

diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt
index 56ea5d7e794..93885ef7efc 100644
--- a/nav2_controller/CMakeLists.txt
+++ b/nav2_controller/CMakeLists.txt
@@ -11,8 +11,6 @@ find_package(nav2_common REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
-find_package(nav_2d_msgs REQUIRED)
-find_package(nav_2d_utils REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -42,9 +40,6 @@ target_link_libraries(${library_name} PUBLIC
   ${nav2_msgs_TARGETS}
   nav2_util::nav2_util_core
   nav2_ros_common::nav2_ros_common
-  ${nav_2d_msgs_TARGETS}
-  nav_2d_utils::conversions
-  nav_2d_utils::tf_help
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
@@ -86,7 +81,6 @@ target_link_libraries(simple_progress_checker PUBLIC
 )
 target_link_libraries(simple_progress_checker PRIVATE
   nav2_util::nav2_util_core
-  nav_2d_utils::conversions
   pluginlib::pluginlib
 )
 
@@ -107,7 +101,6 @@ target_link_libraries(pose_progress_checker PUBLIC
 target_link_libraries(pose_progress_checker PRIVATE
   angles::angles
   nav2_util::nav2_util_core
-  nav_2d_utils::conversions
   pluginlib::pluginlib
 )
 
@@ -218,8 +211,6 @@ ament_export_dependencies(
   nav2_costmap_2d
   nav2_msgs
   nav2_util
-  nav_2d_msgs
-  nav_2d_utils
   pluginlib
   rclcpp
   rclcpp_lifecycle
diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp
index a838854a52b..b68c8e5bb94 100644
--- a/nav2_controller/include/nav2_controller/controller_server.hpp
+++ b/nav2_controller/include/nav2_controller/controller_server.hpp
@@ -29,10 +29,10 @@
 #include "tf2_ros/transform_listener.hpp"
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_msgs/msg/speed_limit.hpp"
-#include "nav_2d_utils/odom_subscriber.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/twist_publisher.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
@@ -208,12 +208,12 @@ class ControllerServer : public nav2::LifecycleNode
    * @param Twist The current Twist from odometry
    * @return Twist Twist after thresholds applied
    */
-  nav_2d_msgs::msg::Twist2D getThresholdedTwist(const nav_2d_msgs::msg::Twist2D & twist)
+  geometry_msgs::msg::Twist getThresholdedTwist(const geometry_msgs::msg::Twist & twist)
   {
-    nav_2d_msgs::msg::Twist2D twist_thresh;
-    twist_thresh.x = getThresholdedVelocity(twist.x, min_x_velocity_threshold_);
-    twist_thresh.y = getThresholdedVelocity(twist.y, min_y_velocity_threshold_);
-    twist_thresh.theta = getThresholdedVelocity(twist.theta, min_theta_velocity_threshold_);
+    geometry_msgs::msg::Twist twist_thresh;
+    twist_thresh.linear.x = getThresholdedVelocity(twist.linear.x, min_x_velocity_threshold_);
+    twist_thresh.linear.y = getThresholdedVelocity(twist.linear.y, min_y_velocity_threshold_);
+    twist_thresh.angular.z = getThresholdedVelocity(twist.angular.z, min_theta_velocity_threshold_);
     return twist_thresh;
   }
 
@@ -233,7 +233,7 @@ class ControllerServer : public nav2::LifecycleNode
   std::unique_ptr<nav2::NodeThread> costmap_thread_;
 
   // Publishers and subscribers
-  std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
+  std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
   nav2::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
 
diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml
index b5be342451c..9360c10d05f 100644
--- a/nav2_controller/package.xml
+++ b/nav2_controller/package.xml
@@ -18,8 +18,6 @@
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_msgs</depend>
   <depend>nav2_util</depend>
-  <depend>nav_2d_msgs</depend>
-  <depend>nav_2d_utils</depend>
   <depend>pluginlib</depend>
   <depend>rcl_interfaces</depend>
   <depend>rclcpp</depend>
diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp
index 453d20992e8..a891104cac0 100644
--- a/nav2_controller/plugins/pose_progress_checker.cpp
+++ b/nav2_controller/plugins/pose_progress_checker.cpp
@@ -18,7 +18,6 @@
 #include <memory>
 #include <vector>
 #include "angles/angles.h"
-#include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "nav2_ros_common/node_utils.hpp"
diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp
index 2e219cdeee8..df3cb77df69 100644
--- a/nav2_controller/plugins/simple_progress_checker.cpp
+++ b/nav2_controller/plugins/simple_progress_checker.cpp
@@ -17,7 +17,6 @@
 #include <string>
 #include <memory>
 #include <vector>
-#include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "nav2_ros_common/node_utils.hpp"
diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt
index cd8048513d4..ba0ce9eb2d5 100644
--- a/nav2_controller/plugins/test/CMakeLists.txt
+++ b/nav2_controller/plugins/test/CMakeLists.txt
@@ -1,5 +1,5 @@
 ament_add_gtest(pctest progress_checker.cpp)
-target_link_libraries(pctest simple_progress_checker pose_progress_checker nav_2d_utils::conversions)
+target_link_libraries(pctest simple_progress_checker pose_progress_checker)
 
 ament_add_gtest(gctest goal_checker.cpp)
-target_link_libraries(gctest simple_goal_checker stopped_goal_checker nav_2d_utils::conversions)
+target_link_libraries(gctest simple_goal_checker stopped_goal_checker)
diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp
index 4730aeb3a04..1aba1ba80bb 100644
--- a/nav2_controller/plugins/test/goal_checker.cpp
+++ b/nav2_controller/plugins/test/goal_checker.cpp
@@ -38,7 +38,6 @@
 #include "gtest/gtest.h"
 #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/geometry_utils.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "eigen3/Eigen/Geometry"
diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp
index 0791b9e9c27..293daf13a25 100644
--- a/nav2_controller/plugins/test/progress_checker.cpp
+++ b/nav2_controller/plugins/test/progress_checker.cpp
@@ -38,7 +38,6 @@
 #include "gtest/gtest.h"
 #include "nav2_controller/plugins/simple_progress_checker.hpp"
 #include "nav2_controller/plugins/pose_progress_checker.hpp"
-#include "nav_2d_utils/conversions.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_util/geometry_utils.hpp"
 
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index 26187087ec1..66b62e667e2 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -21,8 +21,6 @@
 
 #include "lifecycle_msgs/msg/state.hpp"
 #include "nav2_core/controller_exceptions.hpp"
-#include "nav_2d_utils/conversions.hpp"
-#include "nav_2d_utils/tf_help.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_controller/controller_server.hpp"
@@ -65,6 +63,9 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true));
   declare_parameter("costmap_update_timeout", 0.30);  // 300ms
 
+  declare_parameter("odom_topic", rclcpp::ParameterValue("odom"));
+  declare_parameter("odom_duration", rclcpp::ParameterValue(0.3));
+
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()},
@@ -125,8 +126,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
-  std::string speed_limit_topic;
+  std::string speed_limit_topic, odom_topic;
   get_parameter("speed_limit_topic", speed_limit_topic);
+  get_parameter("odom_topic", odom_topic);
+  double odom_duration;
+  get_parameter("odom_duration", odom_duration);
   get_parameter("failure_tolerance", failure_tolerance_);
   get_parameter("use_realtime_priority", use_realtime_priority_);
   get_parameter("publish_zero_velocity", publish_zero_velocity_);
@@ -219,7 +223,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
     get_logger(),
     "Controller Server has %s controllers available.", controller_ids_concat_.c_str());
 
-  odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
+  odom_sub_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration, odom_topic);
   vel_publisher_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel");
 
   double costmap_update_timeout_dbl;
@@ -624,7 +628,7 @@ void ControllerServer::computeAndPublishVelocity()
     throw nav2_core::FailedToMakeProgress("Failed to make progress");
   }
 
-  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
+  geometry_msgs::msg::Twist twist = getThresholdedTwist(odom_sub_->getRawTwist());
 
   geometry_msgs::msg::TwistStamped cmd_vel_2d;
 
@@ -632,7 +636,7 @@ void ControllerServer::computeAndPublishVelocity()
     cmd_vel_2d =
       controllers_[current_controller_]->computeVelocityCommands(
       pose,
-      nav_2d_utils::twist2Dto3D(twist),
+      twist,
       goal_checkers_[current_goal_checker_].get());
     last_valid_cmd_time_ = now();
     cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
@@ -665,10 +669,9 @@ void ControllerServer::computeAndPublishVelocity()
 
   // Find the closest pose to current pose on global path
   geometry_msgs::msg::PoseStamped robot_pose_in_path_frame;
-  rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
-  if (!nav_2d_utils::transformPose(
-          costmap_ros_->getTfBuffer(), current_path_.header.frame_id, pose,
-          robot_pose_in_path_frame, tolerance))
+  if (!nav2_util::transformPoseInTargetFrame(
+          pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(),
+          current_path_.header.frame_id, costmap_ros_->getTransformTolerance()))
   {
     throw nav2_core::ControllerTFError("Failed to transform robot pose to path frame");
   }
@@ -793,14 +796,12 @@ bool ControllerServer::isGoalReached()
     return false;
   }
 
-  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
-  geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist);
+  geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist());
 
   geometry_msgs::msg::PoseStamped transformed_end_pose;
-  rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
-  nav_2d_utils::transformPose(
-    costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(),
-    end_pose_, transformed_end_pose, tolerance);
+  nav2_util::transformPoseInTargetFrame(
+    end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(),
+    costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance());
 
   return goal_checkers_[current_goal_checker_]->isGoalReached(
     pose.pose, transformed_end_pose.pose,
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
index eb1497f21db..4538b2e447d 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp
@@ -27,7 +27,7 @@
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_ros_common/simple_action_server.hpp"
 #include "nav2_util/twist_publisher.hpp"
-#include "nav_2d_utils/odom_subscriber.hpp"
+#include "nav2_util/odometry_utils.hpp"
 #include "opennav_docking/controller.hpp"
 #include "opennav_docking/utils.hpp"
 #include "opennav_docking/types.hpp"
@@ -264,7 +264,7 @@ class DockingServer : public nav2::LifecycleNode
   rclcpp::Time action_start_time_;
 
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
-  std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
+  std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
   typename DockingActionServer::SharedPtr docking_action_server_;
   typename UndockingActionServer::SharedPtr undocking_action_server_;
 
diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
index 9048c70a29a..7838158f8af 100644
--- a/nav2_docking/opennav_docking/src/controller.cpp
+++ b/nav2_docking/opennav_docking/src/controller.cpp
@@ -19,7 +19,6 @@
 #include "opennav_docking/controller.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_ros_common/node_utils.hpp"
-#include "nav_2d_utils/conversions.hpp"
 #include "tf2/utils.hpp"
 
 namespace opennav_docking
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
index 3584af4e9d3..17dc1c28da3 100644
--- a/nav2_docking/opennav_docking/src/docking_server.cpp
+++ b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -42,6 +42,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options)
   declare_parameter("dock_backwards", rclcpp::PARAMETER_BOOL);
   declare_parameter("dock_prestaging_tolerance", 0.5);
   declare_parameter("odom_topic", "odom");
+  declare_parameter("odom_duration", 0.3);
   declare_parameter("rotation_angular_tolerance", 0.05);
 }
 
@@ -83,7 +84,9 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state)
   // Create odom subscriber for backward blind docking
   std::string odom_topic;
   get_parameter("odom_topic", odom_topic);
-  odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node, odom_topic);
+  double odom_duration;
+  get_parameter("odom_duration", odom_duration);
+  odom_sub_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration, odom_topic);
 
   // Create the action servers for dock / undock
   docking_action_server_ = node->create_action_server<DockRobot>(
@@ -464,7 +467,7 @@ void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_po
     }
 
     auto current_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
-    current_vel->twist.angular.z = odom_sub_->getTwist().theta;
+    current_vel->twist.angular.z = odom_sub_->getRawTwist().angular.z;
 
     auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
     command->header = robot_pose.header;
diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt
index b57c2291a51..00789a46e3d 100644
--- a/nav2_dwb_controller/dwb_core/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt
@@ -45,7 +45,6 @@ target_link_libraries(dwb_core PUBLIC
   nav2_util::nav2_util_core
   ${nav_2d_msgs_TARGETS}
   nav_2d_utils::conversions
-  nav_2d_utils::tf_help
   ${nav_msgs_TARGETS}
   pluginlib::pluginlib
   rclcpp::rclcpp
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 41120a7c48a..63013bd091e 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
@@ -199,7 +199,7 @@ class DWBLocalPlanner : public nav2_core::Controller
   bool prune_plan_;
   double prune_distance_;
   bool debug_trajectory_details_;
-  rclcpp::Duration transform_tolerance_{0, 0};
+  double transform_tolerance_{0};
   bool shorten_transformed_plan_;
   double forward_prune_distance_;
 
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 e539c98819a..5c1b47322a6 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -44,7 +44,6 @@
 #include "dwb_msgs/msg/critic_score.hpp"
 #include "nav_2d_msgs/msg/twist2_d.hpp"
 #include "nav_2d_utils/conversions.hpp"
-#include "nav_2d_utils/tf_help.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_ros_common/node_utils.hpp"
@@ -112,8 +111,7 @@ void DWBLocalPlanner::configure(
   std::string traj_generator_name;
 
   double transform_tolerance;
-  node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance);
-  transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance);
+  node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance_);
   RCLCPP_INFO(logger_, "Setting transform_tolerance to %f", transform_tolerance);
 
   node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_);
@@ -293,9 +291,9 @@ DWBLocalPlanner::prepareGlobalPlan(
 
   goal_pose.header.frame_id = global_plan_.header.frame_id;
   goal_pose.pose = global_plan_.poses.back().pose;
-  nav_2d_utils::transformPose(
-    tf_, costmap_ros_->getGlobalFrameID(), goal_pose,
-    goal_pose, transform_tolerance_);
+  nav2_util::transformPoseInTargetFrame(
+    goal_pose, goal_pose, *tf_,
+    costmap_ros_->getGlobalFrameID(), transform_tolerance_);
 }
 
 nav_2d_msgs::msg::Twist2DStamped
@@ -470,9 +468,9 @@ DWBLocalPlanner::transformGlobalPlan(
 
   // let's get the pose of the robot in the frame of the plan
   geometry_msgs::msg::PoseStamped robot_pose;
-  if (!nav_2d_utils::transformPose(
-      tf_, global_plan_.header.frame_id, pose,
-      robot_pose, transform_tolerance_))
+  if (!nav2_util::transformPoseInTargetFrame(
+      pose, robot_pose, *tf_,
+      global_plan_.header.frame_id, transform_tolerance_))
   {
     throw nav2_core::
           ControllerTFError("Unable to transform robot pose into global plan's frame");
@@ -538,9 +536,9 @@ DWBLocalPlanner::transformGlobalPlan(
   // frame to local
   auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {
       geometry_msgs::msg::PoseStamped transformed_pose;
-      nav_2d_utils::transformPose(
-        tf_, transformed_plan.header.frame_id,
-        global_plan_pose, transformed_pose, transform_tolerance_);
+      nav2_util::transformPoseInTargetFrame(
+        global_plan_pose, transformed_pose, *tf_,
+        transformed_plan.header.frame_id, transform_tolerance_);
       return transformed_pose;
     };
 
diff --git a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp
index 1ae85c2e1dd..f8b713c032f 100644
--- a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp
@@ -37,7 +37,6 @@
 #include <string>
 #include "dwb_critics/alignment_util.hpp"
 #include "pluginlib/class_list_macros.hpp"
-#include "nav_2d_utils/parameters.hpp"
 
 namespace dwb_critics
 {
@@ -52,9 +51,8 @@ void GoalAlignCritic::onInit()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  forward_point_distance_ = nav_2d_utils::searchAndGetParam(
-    node,
-    dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325);
+  forward_point_distance_ = node->declare_or_get_parameter(dwb_plugin_name_ + "." + name_ +
+      ".forward_point_distance", 0.325);
 }
 
 bool GoalAlignCritic::prepare(
diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
index bad8e40e3ff..170a9764c1f 100644
--- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
@@ -37,7 +37,6 @@
 #include <cmath>
 #include <string>
 #include <vector>
-#include "nav_2d_utils/parameters.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "dwb_core/exceptions.hpp"
 #include "pluginlib/class_list_macros.hpp"
@@ -99,16 +98,13 @@ void OscillationCritic::onInit()
 
   clock_ = node->get_clock();
 
-  oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam(
-    node,
-    dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist", 0.05);
+  oscillation_reset_dist_ = node->declare_or_get_parameter(dwb_plugin_name_ + "." + name_ +
+      ".oscillation_reset_dist", 0.05);
   oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_;
-  oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam(
-    node,
+  oscillation_reset_angle_ = node->declare_or_get_parameter(
     dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle", 0.2);
   oscillation_reset_time_ = rclcpp::Duration::from_seconds(
-    nav_2d_utils::searchAndGetParam(
-      node,
+    node->declare_or_get_parameter(
       dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time", -1.0));
 
   nav2::declare_parameter_if_not_declared(
diff --git a/nav2_dwb_controller/dwb_critics/src/path_align.cpp b/nav2_dwb_controller/dwb_critics/src/path_align.cpp
index 6c4b28ea6f5..d91414586dd 100644
--- a/nav2_dwb_controller/dwb_critics/src/path_align.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/path_align.cpp
@@ -37,7 +37,6 @@
 #include <string>
 #include "dwb_critics/alignment_util.hpp"
 #include "pluginlib/class_list_macros.hpp"
-#include "nav_2d_utils/parameters.hpp"
 
 namespace dwb_critics
 {
@@ -52,8 +51,7 @@ void PathAlignCritic::onInit()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  forward_point_distance_ = nav_2d_utils::searchAndGetParam(
-    node,
+  forward_point_distance_ = node->declare_or_get_parameter(
     dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325);
 }
 
diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
index f4323188b18..34f2ae95335 100644
--- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp
@@ -35,7 +35,6 @@
 #include "dwb_critics/rotate_to_goal.hpp"
 #include <string>
 #include <vector>
-#include "nav_2d_utils/parameters.hpp"
 #include "dwb_core/exceptions.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "dwb_core/trajectory_utils.hpp"
@@ -58,19 +57,15 @@ void RotateToGoalCritic::onInit()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(
-    node,
+  xy_goal_tolerance_ = node->declare_or_get_parameter(
     dwb_plugin_name_ + ".xy_goal_tolerance", 0.25);
   xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
-  double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(
-    node,
+  double stopped_xy_velocity = node->declare_or_get_parameter(
     dwb_plugin_name_ + ".trans_stopped_velocity", 0.25);
   stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
-  slowing_factor_ = nav_2d_utils::searchAndGetParam(
-    node,
+  slowing_factor_ = node->declare_or_get_parameter(
     dwb_plugin_name_ + "." + name_ + ".slowing_factor", 5.0);
-  lookahead_time_ = nav_2d_utils::searchAndGetParam(
-    node,
+  lookahead_time_ = node->declare_or_get_parameter(
     dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0);
   reset();
 }
diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
index dde49133011..c1152bc3f5b 100644
--- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
@@ -38,7 +38,6 @@
 #include <string>
 #include <vector>
 
-#include "nav_2d_utils/parameters.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
index 24d1c255973..4c096f7e1f1 100644
--- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
@@ -36,7 +36,6 @@
 #include <vector>
 #include <memory>
 #include <string>
-#include "nav_2d_utils/parameters.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "dwb_core/exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
@@ -65,8 +64,7 @@ void LimitedAccelGenerator::initialize(
     RCLCPP_WARN(
       rclcpp::get_logger("LimitedAccelGenerator"),
       "'sim_period' parameter is not set for %s", plugin_name.c_str());
-    double controller_frequency = nav_2d_utils::searchAndGetParam(
-      nh, "controller_frequency", 20.0);
+    double controller_frequency = nh->declare_or_get_parameter("controller_frequency", 20.0);
     if (controller_frequency > 0) {
       acceleration_time_ = 1.0 / controller_frequency;
     } else {
diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
index 8e68a399f08..a11dd8ece0b 100644
--- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
@@ -38,7 +38,6 @@
 #include <algorithm>
 #include <memory>
 #include "dwb_plugins/xy_theta_iterator.hpp"
-#include "nav_2d_utils/parameters.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "dwb_core/exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
index c319bef4a72..01dab35cb7b 100644
--- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
@@ -38,7 +38,6 @@
 #include <memory>
 #include <string>
 
-#include "nav_2d_utils/parameters.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 
 namespace dwb_plugins
diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
index c3005dffd6c..7aad657a142 100644
--- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
@@ -56,28 +56,7 @@ target_link_libraries(path_ops PRIVATE
   ${geometry_msgs_TARGETS}
 )
 
-add_library(tf_help SHARED
-  src/tf_help.cpp
-)
-target_include_directories(tf_help
-  PUBLIC
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
-)
-target_link_libraries(tf_help PUBLIC
-  conversions
-  ${geometry_msgs_TARGETS}
-  ${nav_2d_msgs_TARGETS}
-  rclcpp::rclcpp
-  nav2_ros_common::nav2_ros_common
-  tf2::tf2
-  tf2_geometry_msgs::tf2_geometry_msgs
-  tf2_ros::tf2_ros
-  rclcpp_lifecycle::rclcpp_lifecycle
-  rcl_lifecycle::rcl_lifecycle
-)
-
-install(TARGETS conversions path_ops tf_help
+install(TARGETS conversions path_ops
   EXPORT ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -101,7 +80,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include/${PROJECT_NAME})
-ament_export_libraries(conversions path_ops tf_help)
+ament_export_libraries(conversions path_ops)
 ament_export_dependencies(
   geometry_msgs
   nav_2d_msgs
diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp
index 35f3ec2018f..3a30fd03050 100644
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp
+++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/conversions.hpp
@@ -49,10 +49,6 @@ namespace nav_2d_utils
 {
 geometry_msgs::msg::Twist twist2Dto3D(const nav_2d_msgs::msg::Twist2D & cmd_vel_2d);
 nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel);
-geometry_msgs::msg::PoseStamped poseToPoseStamped(
-  const geometry_msgs::msg::Pose & pose,
-  const std::string & frame, const rclcpp::Time & stamp);
-nav_msgs::msg::Path posesToPath(const std::vector<geometry_msgs::msg::PoseStamped> & poses);
 nav_msgs::msg::Path posesToPath(
   const std::vector<geometry_msgs::msg::Pose> & poses,
   const std::string & frame, const rclcpp::Time & stamp);
diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
deleted file mode 100644
index 2f989e71f2f..00000000000
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * 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.
- */
-
-#ifndef NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
-#define NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
-
-#include <cmath>
-#include <memory>
-#include <mutex>
-#include <string>
-#include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "nav2_ros_common/lifecycle_node.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "nav2_ros_common/node_utils.hpp"
-
-namespace nav_2d_utils
-{
-
-/**
- * @class OdomSubscriber
- * Wrapper for some common odometry operations. Subscribes to the topic with a mutex.
- */
-class OdomSubscriber
-{
-public:
-  /**
-   * @brief Constructor that subscribes to an Odometry topic
-   *
-   * @param nh NodeHandle for creating subscriber
-   * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
-   */
-  explicit OdomSubscriber(
-    nav2::LifecycleNode::SharedPtr nh,
-    std::string default_topic = "odom")
-  {
-    nav2::declare_parameter_if_not_declared(
-      nh, "odom_topic", rclcpp::ParameterValue(default_topic));
-
-    std::string odom_topic;
-    nh->get_parameter_or("odom_topic", odom_topic, default_topic);
-    odom_sub_ =
-      nh->create_subscription<nav_msgs::msg::Odometry>(
-      odom_topic,
-      std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1));
-  }
-
-  inline nav_2d_msgs::msg::Twist2D getTwist() {return odom_vel_.velocity;}
-  inline nav_2d_msgs::msg::Twist2DStamped getTwistStamped() {return odom_vel_;}
-
-protected:
-  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
-  {
-    // ROS_INFO_ONCE("odom received!");
-    std::lock_guard<std::mutex> lock(odom_mutex_);
-    odom_vel_.header = msg->header;
-    odom_vel_.velocity.x = msg->twist.twist.linear.x;
-    odom_vel_.velocity.y = msg->twist.twist.linear.y;
-    odom_vel_.velocity.theta = msg->twist.twist.angular.z;
-  }
-
-  nav2::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
-  nav_2d_msgs::msg::Twist2DStamped odom_vel_;
-  std::mutex odom_mutex_;
-};
-
-}  // namespace nav_2d_utils
-
-#endif  // NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
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
deleted file mode 100644
index 09b039487e0..00000000000
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * 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.
- */
-
-#ifndef NAV_2D_UTILS__PARAMETERS_HPP_
-#define NAV_2D_UTILS__PARAMETERS_HPP_
-
-#include <string>
-#include <memory>
-
-#include "rclcpp/rclcpp.hpp"
-#include "nav2_ros_common/lifecycle_node.hpp"
-#include "nav2_ros_common/node_utils.hpp"
-
-// TODO(crdelsey): Remove when code is re-enabled
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-parameter"
-namespace nav_2d_utils
-{
-
-/**
- * @brief Search for a parameter and load it, or use the default value
- *
- * This templated function shortens a commonly used ROS pattern in which you
- * search for a parameter and get its value if it exists, otherwise returning a default value.
- *
- * @param nh NodeHandle to start the parameter search from
- * @param param_name Name of the parameter to search for
- * @param default_value Value to return if not found
- * @return Value of parameter if found, otherwise the default_value
- */
-template<class param_t>
-param_t searchAndGetParam(
-  const nav2::LifecycleNode::SharedPtr & nh, const std::string & param_name,
-  const param_t & default_value)
-{
-  nav2::declare_parameter_if_not_declared(
-    nh, param_name,
-    rclcpp::ParameterValue(default_value));
-  return nh->get_parameter(param_name).get_value<param_t>();
-}
-
-}  // namespace nav_2d_utils
-#pragma GCC diagnostic pop
-
-#endif  // NAV_2D_UTILS__PARAMETERS_HPP_
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
deleted file mode 100644
index 3f5fadcd4aa..00000000000
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * 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.
- */
-
-#ifndef NAV_2D_UTILS__TF_HELP_HPP_
-#define NAV_2D_UTILS__TF_HELP_HPP_
-
-#include <string>
-#include <memory>
-#include "tf2_ros/buffer.hpp"
-#include "nav_2d_utils/conversions.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-
-namespace nav_2d_utils
-{
-/**
- * @brief Transform a PoseStamped from one frame to another while catching exceptions
- *
- * Also returns immediately if the frames are equal.
- * @param tf Smart pointer to TFListener
- * @param frame Frame to transform the pose into
- * @param in_pose Pose to transform
- * @param out_pose Place to store the resulting transformed pose
- * @return True if successful transform
- */
-bool transformPose(
-  const std::shared_ptr<tf2_ros::Buffer> 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 PoseStamped from one frame to another while catching exceptions
- *
- * Also returns immediately if the frames are equal.
- * @param tf Smart pointer to TFListener
- * @param frame Frame to transform the pose into
- * @param in_pose Pose to transform
- * @param out_pose Place to store the resulting transformed pose
- * @return True if successful transform
- */
-bool transformPose(
-  const std::shared_ptr<tf2_ros::Buffer> tf,
-  const std::string frame,
-  const geometry_msgs::msg::PoseStamped & in_pose,
-  geometry_msgs::msg::PoseStamped & out_pose,
-  rclcpp::Duration & transform_tolerance
-);
-
-}  // namespace nav_2d_utils
-
-#endif  // NAV_2D_UTILS__TF_HELP_HPP_
diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
index 4ec9a73fb99..3629e8bb989 100644
--- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
+++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp
@@ -72,34 +72,6 @@ nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel)
   return cmd_vel_2d;
 }
 
-
-geometry_msgs::msg::PoseStamped poseToPoseStamped(
-  const geometry_msgs::msg::Pose & pose_in,
-  const std::string & frame, const rclcpp::Time & stamp)
-{
-  geometry_msgs::msg::PoseStamped pose;
-  pose.header.frame_id = frame;
-  pose.header.stamp = stamp;
-  pose.pose = pose_in;
-  return pose;
-}
-
-nav_msgs::msg::Path posesToPath(const std::vector<geometry_msgs::msg::PoseStamped> & poses)
-{
-  nav_msgs::msg::Path path;
-  if (poses.empty()) {
-    return path;
-  }
-  path.poses.resize(poses.size());
-  path.header.frame_id = poses[0].header.frame_id;
-  path.header.stamp = poses[0].header.stamp;
-  for (unsigned int i = 0; i < poses.size(); i++) {
-    path.poses[i] = poses[i];
-  }
-  return path;
-}
-
-
 nav_msgs::msg::Path posesToPath(
   const std::vector<geometry_msgs::msg::Pose> & poses,
   const std::string & frame, const rclcpp::Time & stamp)
diff --git a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp
deleted file mode 100644
index 0916cc6a402..00000000000
--- a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * 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 <memory>
-#include <string>
-#include "nav_2d_utils/tf_help.hpp"
-
-namespace nav_2d_utils
-{
-
-bool transformPose(
-  const std::shared_ptr<tf2_ros::Buffer> 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;
-}
-
-}  // namespace nav_2d_utils
diff --git a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp
index c7f5c502f76..fd7d209d3d7 100644
--- a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp
+++ b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp
@@ -48,59 +48,55 @@ using nav_2d_utils::posesToPath;
 
 TEST(nav_2d_utils, PosesToPathEmpty)
 {
-  std::vector<geometry_msgs::msg::PoseStamped> poses;
-  nav_msgs::msg::Path path = posesToPath(poses);
+  std::vector<geometry_msgs::msg::Pose> poses;
+  auto node = std::make_shared<nav2::LifecycleNode>("twod_utils_test_node");
+  rclcpp::Time time = node->now();
+  std::string frame = "test_frame";
+
+  nav_msgs::msg::Path path = posesToPath(poses, frame, time);
 
   EXPECT_EQ(path.poses.size(), 0ul);
+  EXPECT_EQ(path.header.frame_id, frame);
+  EXPECT_EQ(path.header.stamp, time);
 }
 
 TEST(nav_2d_utils, PosesToPathNonEmpty)
 {
-  std::vector<geometry_msgs::msg::PoseStamped> poses;
-  geometry_msgs::msg::PoseStamped pose1;
-  rclcpp::Time time1, time2;
   auto node = std::make_shared<nav2::LifecycleNode>("twod_utils_test_node");
-  time1 = node->now();
+  rclcpp::Time time = node->now();
+  std::string frame = "map";
 
   tf2::Quaternion quat1, quat2;
   quat1.setRPY(0, 0, 0.123);
-  pose1.pose.position.x = 1.0;
-  pose1.pose.position.y = 2.0;
-  pose1.pose.orientation.w = quat1.w();
-  pose1.pose.orientation.x = quat1.x();
-  pose1.pose.orientation.y = quat1.y();
-  pose1.pose.orientation.z = quat1.z();
-  pose1.header.stamp = time1;
-  pose1.header.frame_id = "frame1_id";
-
-  geometry_msgs::msg::PoseStamped pose2;
-  pose2.pose.position.x = 4.0;
-  pose2.pose.position.y = 5.0;
   quat2.setRPY(0, 0, 0.987);
-  pose2.pose.orientation.w = quat2.w();
-  pose2.pose.orientation.x = quat2.x();
-  pose2.pose.orientation.y = quat2.y();
-  pose2.pose.orientation.z = quat2.z();
 
-  time2 = node->now();
-  pose2.header.stamp = time2;
-  pose2.header.frame_id = "frame2_id";
+  geometry_msgs::msg::Pose pose1;
+  pose1.position.x = 1.0;
+  pose1.position.y = 2.0;
+  pose1.orientation = tf2::toMsg(quat1);
 
-  poses.push_back(pose1);
-  poses.push_back(pose2);
+  geometry_msgs::msg::Pose pose2;
+  pose2.position.x = 4.0;
+  pose2.position.y = 5.0;
+  pose2.orientation = tf2::toMsg(quat2);
 
-  nav_msgs::msg::Path path = posesToPath(poses);
+  std::vector<geometry_msgs::msg::Pose> poses = {pose1, pose2};
+
+  nav_msgs::msg::Path path = posesToPath(poses, frame, time);
 
   EXPECT_EQ(path.poses.size(), 2ul);
   EXPECT_EQ(path.poses[0].pose.position.x, 1.0);
   EXPECT_EQ(path.poses[0].pose.position.y, 2.0);
-  EXPECT_EQ(path.poses[0].header.stamp, time1);
-  EXPECT_EQ(path.poses[0].header.frame_id, "frame1_id");
   EXPECT_EQ(path.poses[1].pose.position.x, 4.0);
   EXPECT_EQ(path.poses[1].pose.position.y, 5.0);
-  EXPECT_EQ(path.poses[1].header.frame_id, "frame2_id");
 
-  EXPECT_EQ(path.header.stamp, time1);
+  for (const auto & stamped_pose : path.poses) {
+    EXPECT_EQ(stamped_pose.header.frame_id, frame);
+    EXPECT_EQ(stamped_pose.header.stamp, time);
+  }
+
+  EXPECT_EQ(path.header.frame_id, frame);
+  EXPECT_EQ(path.header.stamp, time);
 }
 
 
diff --git a/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt
index f0cf339a322..fd861291fca 100644
--- a/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt
@@ -11,6 +11,3 @@ target_include_directories(2d_utils_tests
 
 ament_add_gtest(path_ops_tests path_ops_test.cpp)
 target_link_libraries(path_ops_tests path_ops)
-
-ament_add_gtest(tf_help_tests tf_help_test.cpp)
-target_link_libraries(tf_help_tests tf_help conversions)
diff --git a/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp
deleted file mode 100644
index 4095567473a..00000000000
--- a/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2018, Wilco Bonestroo
- *  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 <memory>
-#include <string>
-
-#include "gtest/gtest.h"
-#include "nav_2d_utils/tf_help.hpp"
-
-TEST(TF_Help, TransformToSelf) {
-  bool result;
-
-  std::shared_ptr<tf2_ros::Buffer> tf;
-  std::string frame = "frame_id";
-  geometry_msgs::msg::PoseStamped in_pose;
-  in_pose.header.frame_id = "frame_id";
-  in_pose.pose.position.x = 1.0;
-  in_pose.pose.position.y = 2.0;
-  in_pose.pose.position.z = 3.0;
-  tf2::Quaternion qt;
-  qt.setRPY(0.5, 1.0, 1.5);
-  in_pose.pose.orientation.w = qt.w();
-  in_pose.pose.orientation.x = qt.x();
-  in_pose.pose.orientation.y = qt.y();
-  in_pose.pose.orientation.z = qt.z();
-
-  geometry_msgs::msg::PoseStamped out_pose;
-  rclcpp::Duration transform_tolerance(0, 500);
-
-  result = nav_2d_utils::transformPose(tf, frame, in_pose, out_pose, transform_tolerance);
-
-  EXPECT_TRUE(result);
-  EXPECT_EQ(out_pose.header.frame_id, "frame_id");
-  EXPECT_EQ(out_pose.pose.position.x, 1.0);
-  EXPECT_EQ(out_pose.pose.position.y, 2.0);
-  EXPECT_EQ(out_pose.pose.position.z, 3.0);
-  EXPECT_EQ(out_pose.pose.orientation.w, qt.w());
-  EXPECT_EQ(out_pose.pose.orientation.x, qt.x());
-  EXPECT_EQ(out_pose.pose.orientation.y, qt.y());
-  EXPECT_EQ(out_pose.pose.orientation.z, qt.z());
-}
-
-TEST(TF_Help, EmptyBuffer) {
-  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
-  auto buffer = std::make_shared<tf2_ros::Buffer>(clock);
-
-  std::string frame = "frame_id";
-  geometry_msgs::msg::PoseStamped in_pose;
-  in_pose.header.frame_id = "other_frame_id";
-  in_pose.pose.position.x = 1.0;
-  in_pose.pose.position.y = 2.0;
-  in_pose.pose.position.z = 3.0;
-  tf2::Quaternion qt;
-  qt.setRPY(0.5, 1.0, 1.5);
-  in_pose.pose.orientation.w = qt.w();
-  in_pose.pose.orientation.x = qt.x();
-  in_pose.pose.orientation.y = qt.y();
-  in_pose.pose.orientation.z = qt.z();
-
-  geometry_msgs::msg::PoseStamped out_pose;
-  rclcpp::Duration transform_tolerance(0, 500);
-
-  bool result;
-  result = nav_2d_utils::transformPose(buffer, frame, in_pose, out_pose, transform_tolerance);
-
-  EXPECT_FALSE(result);
-}
diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt
index fc8859860b3..d9720ca6401 100644
--- a/nav2_graceful_controller/CMakeLists.txt
+++ b/nav2_graceful_controller/CMakeLists.txt
@@ -9,7 +9,6 @@ find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_util REQUIRED)
-find_package(nav_2d_utils REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -58,7 +57,6 @@ target_link_libraries(${library_name} PUBLIC
   nav2_ros_common::nav2_ros_common
   nav2_util::nav2_util_core
   nav2_costmap_2d::nav2_costmap_2d_core
-  nav_2d_utils::tf_help
   ${nav_msgs_TARGETS}
   pluginlib::pluginlib
   rclcpp::rclcpp
@@ -104,7 +102,6 @@ ament_export_dependencies(
   nav2_core
   nav2_util
   nav2_costmap_2d
-  nav_2d_utils
   nav_msgs
   pluginlib
   rclcpp
diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp
index aaa64f1bb62..a24a84c2228 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp
@@ -35,7 +35,7 @@ class PathHandler
    * @brief Constructor for nav2_graceful_controller::PathHandler
    */
   PathHandler(
-    tf2::Duration transform_tolerance,
+    double transform_tolerance,
     std::shared_ptr<tf2_ros::Buffer> tf,
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
 
@@ -71,7 +71,7 @@ class PathHandler
   nav_msgs::msg::Path getPlan() {return global_plan_;}
 
 protected:
-  rclcpp::Duration transform_tolerance_{0, 0};
+  double transform_tolerance_{0};
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   nav_msgs::msg::Path global_plan_;
diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml
index f162f3f0ed3..98b6e1e257a 100644
--- a/nav2_graceful_controller/package.xml
+++ b/nav2_graceful_controller/package.xml
@@ -16,7 +16,6 @@
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_util</depend>
-  <depend>nav_2d_utils</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp
index fb0010698f4..0706c86378d 100644
--- a/nav2_graceful_controller/src/graceful_controller.cpp
+++ b/nav2_graceful_controller/src/graceful_controller.cpp
@@ -48,7 +48,7 @@ void GracefulController::configure(
 
   // Handles global path transformations
   path_handler_ = std::make_unique<PathHandler>(
-    tf2::durationFromSec(params_->transform_tolerance), tf_buffer_, costmap_ros_);
+    params_->transform_tolerance, tf_buffer_, costmap_ros_);
 
   // Handles the control law to generate the velocity commands
   control_law_ = std::make_unique<SmoothControlLaw>(
diff --git a/nav2_graceful_controller/src/path_handler.cpp b/nav2_graceful_controller/src/path_handler.cpp
index 9dbfc291f8d..b60b4a874f0 100644
--- a/nav2_graceful_controller/src/path_handler.cpp
+++ b/nav2_graceful_controller/src/path_handler.cpp
@@ -22,7 +22,7 @@
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
-#include "nav_2d_utils/tf_help.hpp"
+#include "nav2_util/robot_utils.hpp"
 #include "nav2_graceful_controller/path_handler.hpp"
 
 namespace nav2_graceful_controller
@@ -31,7 +31,7 @@ namespace nav2_graceful_controller
 using nav2_util::geometry_utils::euclidean_distance;
 
 PathHandler::PathHandler(
-  tf2::Duration transform_tolerance,
+  double transform_tolerance,
   std::shared_ptr<tf2_ros::Buffer> tf,
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
 : transform_tolerance_(transform_tolerance), tf_buffer_(tf), costmap_ros_(costmap_ros)
@@ -49,8 +49,8 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
 
   // Let's get the pose of the robot in the frame of the plan
   geometry_msgs::msg::PoseStamped robot_pose;
-  if (!nav_2d_utils::transformPose(
-      tf_buffer_, global_plan_.header.frame_id, pose, robot_pose,
+  if (!nav2_util::transformPoseInTargetFrame(
+      pose, robot_pose, *tf_buffer_, global_plan_.header.frame_id,
       transform_tolerance_))
   {
     throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame");
@@ -88,9 +88,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
       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;
-      if (!nav_2d_utils::transformPose(
-          tf_buffer_, costmap_ros_->getBaseFrameID(), stamped_pose,
-          transformed_pose, transform_tolerance_))
+      if (!nav2_util::transformPoseInTargetFrame(
+          stamped_pose, transformed_pose, *tf_buffer_,
+          costmap_ros_->getBaseFrameID(), transform_tolerance_))
       {
         throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame");
       }
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
index 544a98467d9..4233c91ef28 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
@@ -98,17 +98,6 @@ class PathHandler
   geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
 
 protected:
-  /**
-    * @brief Transform a pose to another frame
-    * @param frame Frame to transform to
-    * @param in_pose Input pose
-    * @param out_pose Output pose
-    * @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 largest dimension of costmap (radially)
     * @return Max distance from center of costmap to edge
diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp
index 93593982c5c..55931f6bc62 100644
--- a/nav2_mppi_controller/src/path_handler.cpp
+++ b/nav2_mppi_controller/src/path_handler.cpp
@@ -17,6 +17,7 @@
 #include "nav2_mppi_controller/tools/path_handler.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace mppi
 {
@@ -85,7 +86,8 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
     geometry_msgs::msg::PoseStamped costmap_plan_pose;
     global_plan_pose->header.stamp = global_pose.header.stamp;
     global_plan_pose->header.frame_id = global_plan_.header.frame_id;
-    transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose);
+    nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_,
+        costmap_->getGlobalFrameID(), transform_tolerance_);
 
     // Check if pose is inside the costmap
     if (!costmap_->getCostmap()->worldToMap(
@@ -109,7 +111,9 @@ geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
   }
 
   geometry_msgs::msg::PoseStamped robot_pose;
-  if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) {
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_,
+      global_plan_up_to_inversion_.header.frame_id, transform_tolerance_))
+  {
     throw nav2_core::ControllerTFError(
             "Unable to transform robot pose into global plan's frame");
   }
@@ -142,27 +146,6 @@ nav_msgs::msg::Path PathHandler::transformPath(
   return transformed_plan;
 }
 
-bool PathHandler::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_buffer_->transform(
-      in_pose, out_pose, frame,
-      tf2::durationFromSec(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;
-}
-
 double PathHandler::getMaxCostmapDist()
 {
   const auto & costmap = costmap_->getCostmap();
@@ -196,7 +179,9 @@ geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
     throw nav2_core::ControllerTFError("Goal pose has an empty frame_id");
   }
   geometry_msgs::msg::PoseStamped transformed_goal;
-  if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) {
+  if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_,
+      costmap_->getGlobalFrameID(), transform_tolerance_))
+  {
     throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame");
   }
   return transformed_goal;
diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp
index eb0bd140c43..e0f17e48411 100644
--- a/nav2_mppi_controller/test/path_handler_test.cpp
+++ b/nav2_mppi_controller/test/path_handler_test.cpp
@@ -46,13 +46,6 @@ class PathHandlerWrapper : public PathHandler
     return getGlobalPlanConsideringBoundsInCostmapFrame(pose);
   }
 
-  bool transformPoseWrapper(
-    const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
-    geometry_msgs::msg::PoseStamped & out_pose) const
-  {
-    return transformPose(frame, in_pose, out_pose);
-  }
-
   geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper(
     const geometry_msgs::msg::PoseStamped & pose)
   {
@@ -180,12 +173,10 @@ TEST(PathHandlerTests, TestTransforms)
     path.poses[i].header.frame_id = "map";
   }
 
-  geometry_msgs::msg::PoseStamped robot_pose, output_pose;
+  geometry_msgs::msg::PoseStamped robot_pose;
   robot_pose.header.frame_id = "odom";
   robot_pose.pose.position.x = 2.5;
 
-  EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose));
-  EXPECT_EQ(output_pose.pose.position.x, 2.5);
 
   EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error);
   handler.setPath(path);
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
index cc8b7915330..135a304dc26 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp
@@ -43,7 +43,7 @@ class PathHandler
    * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler
    */
   PathHandler(
-    tf2::Duration transform_tolerance,
+    double transform_tolerance,
     std::shared_ptr<tf2_ros::Buffer> tf,
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
 
@@ -65,18 +65,6 @@ class PathHandler
     const geometry_msgs::msg::PoseStamped & pose,
     double max_robot_pose_search_dist, bool reject_unit_path = false);
 
-  /**
-   * @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;
-
   void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;}
 
   nav_msgs::msg::Path getPlan() {return global_plan_;}
@@ -89,7 +77,7 @@ class PathHandler
   double getCostmapMaxExtent() const;
 
   rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")};
-  tf2::Duration transform_tolerance_;
+  double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   nav_msgs::msg::Path global_plan_;
diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp
index 1fcc39b7e37..c91bb8dc8f7 100644
--- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp
@@ -23,6 +23,7 @@
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_regulated_pure_pursuit_controller
 {
@@ -30,7 +31,7 @@ namespace nav2_regulated_pure_pursuit_controller
 using nav2_util::geometry_utils::euclidean_distance;
 
 PathHandler::PathHandler(
-  tf2::Duration transform_tolerance,
+  double transform_tolerance,
   std::shared_ptr<tf2_ros::Buffer> tf,
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
 : transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros)
@@ -60,7 +61,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
 
   // 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)) {
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_.header.frame_id,
+      transform_tolerance_))
+  {
     throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame");
   }
 
@@ -101,7 +104,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
       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;
-      if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) {
+      if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_,
+        costmap_ros_->getBaseFrameID(), transform_tolerance_))
+      {
         throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame");
       }
       transformed_pose.pose.position.z = 0.0;
@@ -128,24 +133,4 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
   return transformed_plan;
 }
 
-bool PathHandler::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_regulated_pure_pursuit_controller
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 d8153ea16bd..1bd12ae3a06 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
@@ -61,7 +61,7 @@ void RegulatedPurePursuitController::configure(
 
   // Handles global path transformations
   path_handler_ = std::make_unique<PathHandler>(
-    tf2::durationFromSec(params_->transform_tolerance), tf_, costmap_ros_);
+    params_->transform_tolerance, tf_, costmap_ros_);
 
   // Checks for imminent collisions
   collision_checker_ = std::make_unique<CollisionChecker>(node, costmap_ros_, params_);
diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt
index 465cadd670d..5831d10189c 100644
--- a/nav2_smoother/CMakeLists.txt
+++ b/nav2_smoother/CMakeLists.txt
@@ -9,7 +9,6 @@ find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
-find_package(nav_2d_utils REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
@@ -45,8 +44,6 @@ target_link_libraries(${library_name} PUBLIC
   nav2_ros_common::nav2_ros_common
 )
 target_link_libraries(${library_name} PRIVATE
-  nav_2d_utils::conversions
-  nav_2d_utils::tf_help
   rclcpp_components::component
   tf2::tf2
 )
diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml
index c2983d916ce..339bdeb5586 100644
--- a/nav2_smoother/package.xml
+++ b/nav2_smoother/package.xml
@@ -17,7 +17,6 @@
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_msgs</depend>
   <depend>nav2_util</depend>
-  <depend>nav_2d_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
index eb852c1eb52..66e3fd926ff 100644
--- a/nav2_smoother/src/nav2_smoother.cpp
+++ b/nav2_smoother/src/nav2_smoother.cpp
@@ -23,8 +23,6 @@
 #include "nav2_core/smoother_exceptions.hpp"
 #include "nav2_smoother/nav2_smoother.hpp"
 #include "nav2_ros_common/node_utils.hpp"
-#include "nav_2d_utils/conversions.hpp"
-#include "nav_2d_utils/tf_help.hpp"
 #include "tf2_ros/create_timer_ros.hpp"
 
 using namespace std::chrono_literals;
diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
index 912d73d0859..d333df9cf14 100644
--- a/nav2_util/src/robot_utils.cpp
+++ b/nav2_util/src/robot_utils.cpp
@@ -50,6 +50,11 @@ bool transformPoseInTargetFrame(
 {
   static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame");
 
+  if(input_pose.header.frame_id == target_frame) {
+    transformed_pose = input_pose;
+    return true;
+  }
+
   try {
     transformed_pose = tf_buffer.transform(
       input_pose, target_frame,

From 1468484f1f96fe0f06241819315c8371311d0f52 Mon Sep 17 00:00:00 2001
From: Patrick Roncagliolo <ronca.pat@gmail.com>
Date: Thu, 7 Aug 2025 18:10:59 +0200
Subject: [PATCH 12/51] Construct TF listeners passing nodes, spinning on
 separate thread (#5406)

* Construct TF listeners passing nodes, spinning on separate thread

Signed-off-by: Patrick Roncagliolo <ronca.pat@gmail.com>

* (tentative) pin down of the impacting change

Signed-off-by: Patrick Roncagliolo <ronca.pat@gmail.com>

---------

Signed-off-by: Patrick Roncagliolo <ronca.pat@gmail.com>
---
 nav2_amcl/src/amcl_node.cpp                            | 2 +-
 nav2_behaviors/src/behavior_server.cpp                 | 2 +-
 nav2_bt_navigator/src/bt_navigator.cpp                 | 2 +-
 nav2_collision_monitor/src/collision_detector_node.cpp | 2 +-
 nav2_collision_monitor/src/collision_monitor_node.cpp  | 2 +-
 nav2_docking/opennav_docking/src/docking_server.cpp    | 2 +-
 nav2_route/src/route_server.cpp                        | 2 +-
 nav2_smoother/src/nav2_smoother.cpp                    | 2 +-
 8 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index 93b8f599a8d..b3b21631a59 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -1339,7 +1339,7 @@ AmclNode::initTransforms()
     get_node_timers_interface(),
     callback_group_);
   tf_buffer_->setCreateTimerInterface(timer_interface);
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this, true);
   tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(shared_from_this());
 
   sent_first_transform_ = false;
diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp
index 54ace7bac0a..0f4db4b3bf6 100644
--- a/nav2_behaviors/src/behavior_server.cpp
+++ b/nav2_behaviors/src/behavior_server.cpp
@@ -87,7 +87,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & state)
     get_node_base_interface(),
     get_node_timers_interface());
   tf_->setCreateTimerInterface(timer_interface);
-  transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+  transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, true);
 
   behavior_types_.resize(behavior_ids_.size());
   if (!loadBehaviorPlugins()) {
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 5f99c1452db..95d3c7dcaa7 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -54,7 +54,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & state)
     get_node_base_interface(), get_node_timers_interface());
   tf_->setCreateTimerInterface(timer_interface);
   tf_->setUsingDedicatedThread(true);
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, false);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, true);
 
   global_frame_ = this->declare_or_get_parameter("global_frame", std::string("map"));
   robot_frame_ = this->declare_or_get_parameter("robot_base_frame", std::string("base_link"));
diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp
index a45d35dc3cd..91023afae4f 100644
--- a/nav2_collision_monitor/src/collision_detector_node.cpp
+++ b/nav2_collision_monitor/src/collision_detector_node.cpp
@@ -50,7 +50,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & state)
     this->get_node_base_interface(),
     this->get_node_timers_interface());
   tf_buffer_->setCreateTimerInterface(timer_interface);
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this, true);
 
   state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionDetectorState>(
     "collision_detector_state");
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index 7bff3a8a4d1..d7ec6131220 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -52,7 +52,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
     this->get_node_base_interface(),
     this->get_node_timers_interface());
   tf_buffer_->setCreateTimerInterface(timer_interface);
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this, true);
 
   std::string cmd_vel_in_topic;
   std::string cmd_vel_out_topic;
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
index 17dc1c28da3..1208ade5521 100644
--- a/nav2_docking/opennav_docking/src/docking_server.cpp
+++ b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -121,7 +121,7 @@ DockingServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   auto node = shared_from_this();
 
-  tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_buffer_);
+  tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_buffer_, this, true);
   dock_db_->activate();
   navigator_->activate();
   vel_publisher_->on_activate();
diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp
index 2a44005111b..fee6bf7b671 100644
--- a/nav2_route/src/route_server.cpp
+++ b/nav2_route/src/route_server.cpp
@@ -35,7 +35,7 @@ RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     get_node_base_interface(),
     get_node_timers_interface());
   tf_->setCreateTimerInterface(timer_interface);
-  transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+  transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, true);
 
   auto node = shared_from_this();
   graph_vis_publisher_ =
diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp
index 66e3fd926ff..74714bafb81 100644
--- a/nav2_smoother/src/nav2_smoother.cpp
+++ b/nav2_smoother/src/nav2_smoother.cpp
@@ -78,7 +78,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state)
   auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
     get_node_base_interface(), get_node_timers_interface());
   tf_->setCreateTimerInterface(timer_interface);
-  transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+  transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, true);
 
   std::string costmap_topic, footprint_topic, robot_base_frame;
   double transform_tolerance = 0.1;

From ec2885d0849429ba8a08aa149d8acbf7d61ff0a5 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Thu, 7 Aug 2025 11:29:53 -0700
Subject: [PATCH 13/51] Adding new API docs website

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
---
 README.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/README.md b/README.md
index 4ef84c7c30f..06448b37c73 100644
--- a/README.md
+++ b/README.md
@@ -12,6 +12,7 @@ For detailed instructions on how to:
 - [Concepts](https://docs.nav2.org/concepts/index.html)
 - [Build](https://docs.nav2.org/development_guides/build_docs/index.html#build)
 - [Install](https://docs.nav2.org/development_guides/build_docs/index.html#install)
+- [API Docs](https://api.nav2.org/)
 - [General Tutorials](https://docs.nav2.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://docs.nav2.org/plugin_tutorials/index.html)
 - [Configure](https://docs.nav2.org/configuration/index.html)
 - [Navigation Plugins](https://docs.nav2.org/plugins/index.html)

From d590533fb9bdd181dc28d3daed58bd4a3007c600 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Cihat=20Kurtulu=C5=9F=20Alt=C4=B1parmak?=
 <cihataltiparmak1@gmail.com>
Date: Fri, 8 Aug 2025 00:41:30 +0300
Subject: [PATCH 14/51] Smooth path even if goal pose is so much near to the
 robot (#5423)

* Smooth path even if goal pose is so much near to the robot

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Apply suggestions

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Remove unnecessary diff

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

---------

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
---
 nav2_smoother/src/simple_smoother.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp
index db9f323e8a1..5179784f83a 100644
--- a/nav2_smoother/src/simple_smoother.cpp
+++ b/nav2_smoother/src/simple_smoother.cpp
@@ -81,7 +81,7 @@ bool SimpleSmoother::smooth(
   std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
 
   for (unsigned int i = 0; i != path_segments.size(); i++) {
-    if (path_segments[i].end - path_segments[i].start > 9) {
+    if (path_segments[i].end - path_segments[i].start > 3) {
       // Populate path segment
       curr_path_segment.poses.clear();
       std::copy(

From df3effd125da78da1bd655bcceb5f36488abf9b9 Mon Sep 17 00:00:00 2001
From: Vince Reda <60265874+redvinaa@users.noreply.github.com>
Date: Fri, 8 Aug 2025 17:35:08 +0200
Subject: [PATCH 15/51] Make pause resume controller use nav2::LifecycleNode
 (#5427)

* Make pause resume controller use nav2::LifecycleNode

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix tests

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Use new nav2 create_service

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Use attribues from parent, fix future wait, update docstring

Signed-off-by: redvinaa <redvinaa@gmail.com>

---------

Signed-off-by: redvinaa <redvinaa@gmail.com>
---
 .../control/pause_resume_controller.hpp       |   1 +
 .../control/pause_resume_controller.cpp       |   8 +-
 .../control/test_pause_resume_controller.cpp  | 109 ++++++------------
 .../test/utils/get_node_from_tree.hpp         |   2 +-
 4 files changed, 43 insertions(+), 77 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
index a3f5333b19b..fa5bfa35f2f 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp
@@ -26,6 +26,7 @@
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "behaviortree_cpp/control_node.h"
 #include "nav2_ros_common/service_server.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
 
 // Interface definitions
 #include "std_srvs/srv/trigger.hpp"
diff --git a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp
index 0f039aad8e6..60b269fa2e0 100644
--- a/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp
+++ b/nav2_behavior_tree/plugins/control/pause_resume_controller.cpp
@@ -31,7 +31,7 @@ PauseResumeController::PauseResumeController(
   const BT::NodeConfiguration & conf)
 : BT::ControlNode(xml_tag_name, conf)
 {
-  auto node = this->config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  auto node = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
   state_ = RESUMED;
 
   // Create a separate cb group with a separate executor to spin
@@ -45,17 +45,15 @@ PauseResumeController::PauseResumeController(
 
   std::string pause_service_name;
   getInput("pause_service_name", pause_service_name);
-  pause_srv_ = std::make_shared<nav2::ServiceServer<Trigger>>(
+  pause_srv_ = node->create_service<Trigger>(
     pause_service_name,
-    node,
     std::bind(&PauseResumeController::pauseServiceCallback, this, _1, _2, _3),
     cb_group_);
 
   std::string resume_service_name;
   getInput("resume_service_name", resume_service_name);
-  resume_srv_ = std::make_shared<nav2::ServiceServer<Trigger>>(
+  resume_srv_ = node->create_service<Trigger>(
     resume_service_name,
-    node,
     std::bind(&PauseResumeController::resumeServiceCallback, this, _1, _2, _3),
     cb_group_);
 }
diff --git a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp
index 8a78c37d89b..a546bcfb565 100644
--- a/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp
+++ b/nav2_behavior_tree/test/plugins/control/test_pause_resume_controller.cpp
@@ -26,58 +26,46 @@ class PauseResumeControllerTestFixture : public nav2_behavior_tree::BehaviorTree
 public:
   static void SetUpTestCase()
   {
-    auto node = std::make_shared<rclcpp::Node>("pause_resume_controller_test_fixture");
-    executor_ =
-      std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    cb_group_ = node->create_callback_group(
-      rclcpp::CallbackGroupType::MutuallyExclusive, false);
-    executor_->add_callback_group(cb_group_, node->get_node_base_interface());
-    pause_client_ = node->create_client<std_srvs::srv::Trigger>(
-      "pause", rclcpp::ServicesQoS(), cb_group_);
-    resume_client_ = node->create_client<std_srvs::srv::Trigger>(
-      "resume", rclcpp::ServicesQoS(), cb_group_);
-
-    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
-    config_ = new BT::NodeConfiguration();
-    config_->blackboard = BT::Blackboard::create();
-    config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node);
+    nav2_behavior_tree::BehaviorTreeTestFixture::SetUpTestCase();
 
-    factory_->registerNodeType<nav2_behavior_tree::PauseResumeController>("PauseResumeController");
+    ASSERT_TRUE(node_);
+    pause_client_ = node_->create_client<std_srvs::srv::Trigger>("pause", true);
+    resume_client_ = node_->create_client<std_srvs::srv::Trigger>("resume", true);
 
-    // Register dummy node for testing
+    ASSERT_TRUE(factory_);
+    factory_->registerNodeType<nav2_behavior_tree::PauseResumeController>("PauseResumeController");
     factory_->registerNodeType<nav2_behavior_tree::DummyNode>("DummyNode");
   }
 
   static void TearDownTestCase()
   {
-    if (config_) {
-      delete config_;
-      config_ = nullptr;
-    }
     tree_.reset();
+    pause_client_.reset();
+    resume_client_.reset();
+  }
+
+  static void CheckServiceResult(
+    const nav2::ServiceClient<std_srvs::srv::Trigger>::SharedPtr & client,
+    std::shared_future<std::shared_ptr<std_srvs::srv::Trigger::Response>> future,
+    bool success = true)
+  {
+    const auto res = client->spin_until_complete(future, std::chrono::seconds(1));
+    ASSERT_EQ(res, rclcpp::FutureReturnCode::SUCCESS);
+    ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready);
+    EXPECT_EQ(future.get()->success, success);
   }
 
 protected:
-  static BT::NodeConfiguration * config_;
-  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
   static std::shared_ptr<BT::Tree> tree_;
-  static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
-  static rclcpp::CallbackGroup::SharedPtr cb_group_;
-  static rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr pause_client_;
-  static rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resume_client_;
+  static nav2::ServiceClient<std_srvs::srv::Trigger>::SharedPtr pause_client_;
+  static nav2::ServiceClient<std_srvs::srv::Trigger>::SharedPtr resume_client_;
 };
 
-rclcpp::executors::SingleThreadedExecutor::SharedPtr
-PauseResumeControllerTestFixture::executor_ = nullptr;
-rclcpp::CallbackGroup::SharedPtr
-PauseResumeControllerTestFixture::cb_group_ = nullptr;
-rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr
+std::shared_ptr<BT::Tree> PauseResumeControllerTestFixture::tree_ = nullptr;
+nav2::ServiceClient<std_srvs::srv::Trigger>::SharedPtr
 PauseResumeControllerTestFixture::pause_client_ = nullptr;
-rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr
+nav2::ServiceClient<std_srvs::srv::Trigger>::SharedPtr
 PauseResumeControllerTestFixture::resume_client_ = nullptr;
-BT::NodeConfiguration * PauseResumeControllerTestFixture::config_ = nullptr;
-std::shared_ptr<BT::BehaviorTreeFactory> PauseResumeControllerTestFixture::factory_ = nullptr;
-std::shared_ptr<BT::Tree> PauseResumeControllerTestFixture::tree_ = nullptr;
 
 TEST_F(PauseResumeControllerTestFixture, test_incorrect_num_children)
 {
@@ -128,31 +116,22 @@ TEST_F(PauseResumeControllerTestFixture, test_unused_children)
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
 
-  const auto & check_request_succeeded = [](
-    rclcpp::Client<std_srvs::srv::Trigger>::FutureAndRequestId & future)
-    {
-      executor_->spin_until_future_complete(future, std::chrono::seconds(1));
-      ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready);
-      EXPECT_EQ(future.get()->success, true);
-    };
-
   // Call pause service, expect RUNNING and PAUSED
-  auto future = pause_client_->async_send_request(
-    std::make_shared<std_srvs::srv::Trigger::Request>());
+  auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
+  auto future = pause_client_->async_call(req);
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
-  check_request_succeeded(future);
+  CheckServiceResult(pause_client_, future);
 
   // Tick again, expect RUNNING and PAUSED
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
 
   // Call resume service, expect RUNNING and ON_RESUME
-  future = resume_client_->async_send_request(
-    std::make_shared<std_srvs::srv::Trigger::Request>());
+  future = resume_client_->async_call(req);
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
-  check_request_succeeded(future);
+  CheckServiceResult(resume_client_, future);
 
   // Tick again, expect RUNNING and RESUMED
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
@@ -206,21 +185,12 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior)
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
 
-  const auto & check_future_result = [](
-    rclcpp::Client<std_srvs::srv::Trigger>::FutureAndRequestId & future, bool success = true)
-    -> void
-    {
-      executor_->spin_until_future_complete(future, std::chrono::seconds(1));
-      ASSERT_EQ(future.wait_for(std::chrono::seconds(0)), std::future_status::ready);
-      EXPECT_EQ(future.get()->success, success);
-    };
-
   // Call pause service, set ON_PAUSE child to RUNNING, expect RUNNING and ON_PAUSE
-  auto future = pause_client_->async_send_request(
-    std::make_shared<std_srvs::srv::Trigger::Request>());
+  auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
+  auto future = pause_client_->async_call(req);
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(pause_bt_node->getState(), state_t::ON_PAUSE);
-  check_future_result(future);
+  CheckServiceResult(pause_client_, future);
 
   // Change ON_PAUSE child to SUCCESS, expect RUNNING and PAUSED
   on_pause_child->changeStatus(BT::NodeStatus::SUCCESS);
@@ -239,18 +209,16 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior)
   EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
 
   // Call pause service again, expect RUNNING and PAUSED
-  future = pause_client_->async_send_request(
-    std::make_shared<std_srvs::srv::Trigger::Request>());
+  future = pause_client_->async_call(req);
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(pause_bt_node->getState(), state_t::PAUSED);
-  check_future_result(future, false);
+  CheckServiceResult(pause_client_, future, false);
 
   // Call resume service, change ON_RESUME child to FAILURE, expect FAILURE
-  future = resume_client_->async_send_request(
-    std::make_shared<std_srvs::srv::Trigger::Request>());
+  future = resume_client_->async_call(req);
   on_resume_child->changeStatus(BT::NodeStatus::FAILURE);
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::FAILURE);
-  check_future_result(future);
+  CheckServiceResult(resume_client_, future);
 
   // Halt the tree, expect RUNNING and RESUMED
   tree_->haltTree();
@@ -263,11 +231,10 @@ TEST_F(PauseResumeControllerTestFixture, test_behavior)
   EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
 
   // Call resume service again, expect RUNNING and RESUMED
-  future = resume_client_->async_send_request(
-    std::make_shared<std_srvs::srv::Trigger::Request>());
+  future = resume_client_->async_call(req);
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
   EXPECT_EQ(pause_bt_node->getState(), state_t::RESUMED);
-  check_future_result(future, false);
+  CheckServiceResult(resume_client_, future, false);
 }
 
 int main(int argc, char ** argv)
diff --git a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp
index 068fc924005..5ea720872d4 100644
--- a/nav2_behavior_tree/test/utils/get_node_from_tree.hpp
+++ b/nav2_behavior_tree/test/utils/get_node_from_tree.hpp
@@ -26,7 +26,7 @@ namespace nav2_behavior_tree
 
 /**
  * @brief Get node from tree by type and index, casted to NodeT type, const casted away
- * Returns true if the node was found, false otherwise
+ * Returns node if it was found, nullptr otherwise.
  */
 template<typename NodeT>
 NodeT * get_node_from_tree(

From 773a8aa39bb86a4f4f3df777c3f71a1def9ebf47 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Fri, 8 Aug 2025 09:14:44 -0700
Subject: [PATCH 16/51] Updating readme table for docs links (#5430)

* Updating readme info

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* Adding configuration guide

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* Conciseness

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

---------

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
---
 README.md | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/README.md b/README.md
index 06448b37c73..48a40d00e21 100644
--- a/README.md
+++ b/README.md
@@ -8,17 +8,16 @@
 </p>
 
 For detailed instructions on how to:
-- [Getting Started](https://docs.nav2.org/getting_started/index.html)
-- [Concepts](https://docs.nav2.org/concepts/index.html)
-- [Build](https://docs.nav2.org/development_guides/build_docs/index.html#build)
-- [Install](https://docs.nav2.org/development_guides/build_docs/index.html#install)
-- [API Docs](https://api.nav2.org/)
+- [Concepts](https://docs.nav2.org/concepts/index.html) and [Getting Started](https://docs.nav2.org/getting_started/index.html)
+- [First Time Setup Guide](https://docs.nav2.org/setup_guides/index.html)
+- [ROS Distribution Statuses](https://docs.nav2.org/#distributions)
+- [Build & Install](https://docs.nav2.org/development_guides/build_docs/index.html#build) and [Docker Containers](https://github.com/orgs/ros-navigation/packages/container/package/navigation2)
 - [General Tutorials](https://docs.nav2.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://docs.nav2.org/plugin_tutorials/index.html)
-- [Configure](https://docs.nav2.org/configuration/index.html)
+- [Configuration Guide](https://docs.nav2.org/configuration/index.html)
 - [Navigation Plugins](https://docs.nav2.org/plugins/index.html)
-- [ROSCon Talks](https://docs.nav2.org/about/roscon.html)
+- [API Docs](https://api.nav2.org/)
+- [ROSCon Talks](https://docs.nav2.org/about/roscon.html) and [Citations](https://docs.nav2.org/citations.html)
 - [Migration Guides](https://docs.nav2.org/migration/index.html)
-- [Container Images for Building Nav2](https://github.com/orgs/ros-navigation/packages/container/package/navigation2)
 - [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html)
 
 Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-uj428p0x-jKx8U7OzK1IOWp5TnDS2rA) (if this link does not work, please contact maintainers to reactivate).

From e690ef0ed2391bbf93e856fe4a6cd4b371335318 Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Sat, 9 Aug 2025 02:07:48 +0800
Subject: [PATCH 17/51] Add support for dynamically changing keepout zone
 (#5429)

* Add support for dynamically changing keepout zone

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Linting

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Revert binary and speed changes

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 .../costmap_filters/costmap_filter.hpp        |  2 +-
 .../costmap_filters/keepout_filter.hpp        | 20 ++++++++++++
 .../costmap_filters/keepout_filter.cpp        | 31 +++++++++++++++++++
 3 files changed, 52 insertions(+), 1 deletion(-)

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
index e51809df696..25238cf8a35 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
@@ -99,7 +99,7 @@ class CostmapFilter : public Layer
    */
   void updateBounds(
     double robot_x, double robot_y, double robot_yaw,
-    double * min_x, double * min_y, double * max_x, double * max_y) final;
+    double * min_x, double * min_y, double * max_x, double * max_y) override;
 
   /**
    * @brief Update the costs in the master costmap in the window
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
index e615ba465a0..b23e03a5e49 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
@@ -68,6 +68,20 @@ class KeepoutFilter : public CostmapFilter
   void initializeFilter(
     const std::string & filter_info_topic);
 
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
+  void updateBounds(
+    double robot_x, double robot_y, double robot_yaw,
+    double * min_x, double * min_y, double * max_x, double * max_y) override;
+
   /**
    * @brief Process the keepout layer at the current pose / bounds / grid
    */
@@ -108,6 +122,12 @@ class KeepoutFilter : public CostmapFilter
   bool last_pose_lethal_{false};  // If true, last pose was lethal
   unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u};
   unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u};
+
+  unsigned int x_{0};
+  unsigned int y_{0};
+  unsigned int width_{0};
+  unsigned int height_{0};
+  bool has_updated_data_{false};
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
index e727d00634e..5fa544dec1c 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
@@ -155,6 +155,37 @@ void KeepoutFilter::maskCallback(
 
   // Store filter_mask_
   filter_mask_ = msg;
+  has_updated_data_ = true;
+  x_ = y_ = 0;
+  width_ = msg->info.width;
+  height_ = msg->info.height;
+}
+
+void KeepoutFilter::updateBounds(
+  double robot_x, double robot_y, double robot_yaw,
+  double * min_x, double * min_y, double * max_x, double * max_y)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  CostmapFilter::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+
+  if(!has_updated_data_) {
+    return;
+  }
+
+  double wx, wy;
+
+  layered_costmap_->getCostmap()->mapToWorld(x_, y_, wx, wy);
+  *min_x = std::min(wx, *min_x);
+  *min_y = std::min(wy, *min_y);
+
+  layered_costmap_->getCostmap()->mapToWorld(x_ + width_, y_ + height_, wx, wy);
+  *max_x = std::max(wx, *max_x);
+  *max_y = std::max(wy, *max_y);
+
+  has_updated_data_ = false;
 }
 
 void KeepoutFilter::process(

From 2a947bf8f16615207a0cbf844536774277bd2bb6 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <gitecsvc@gmail.com>
Date: Mon, 11 Aug 2025 19:17:54 +0200
Subject: [PATCH 18/51] Fix KeepoutFilter on the ARM architecture (#5436)

Signed-off-by: Sushant Chavan <gitecsvc@gmail.com>
---
 nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
index bc4e89f878e..06d7ed2d025 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
@@ -222,7 +222,7 @@ unsigned char CostmapFilter::getMaskCost(
 {
   const unsigned int index = my * filter_mask->info.width + mx;
 
-  const char data = filter_mask->data[index];
+  const signed char data = filter_mask->data[index];
   if (data == nav2_util::OCC_GRID_UNKNOWN) {
     return NO_INFORMATION;
   } else {

From bab2a867514512c2745ba21c62af0cb5d67a8c74 Mon Sep 17 00:00:00 2001
From: Sakshay Mahna <sakshum19@gmail.com>
Date: Mon, 11 Aug 2025 23:13:07 +0530
Subject: [PATCH 19/51] Calculate next path point using lookahead for Graceful
 Controller (#5003)

* Add controller utils

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

* Fix linting issues

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

* Update regulated pure pursuit

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

* Update graceful controller

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

* Remove interpolate after goal & Use orientationAroundZAxis

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

* Update nav2_util/src/controller_utils.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_util/src/controller_utils.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_util/src/controller_utils.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Remove interpolate_after_goal parameter from test

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

---------

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
---
 .../graceful_controller.hpp                   |  18 ++
 .../src/graceful_controller.cpp               | 104 ++++---
 .../regulated_pure_pursuit_controller.hpp     |  26 --
 .../src/regulated_pure_pursuit_controller.cpp |  99 +------
 .../test/test_regulated_pp.cpp                | 273 ------------------
 .../include/nav2_util/controller_utils.hpp    |  54 ++++
 nav2_util/src/CMakeLists.txt                  |   1 +
 nav2_util/src/controller_utils.cpp            | 142 +++++++++
 nav2_util/test/CMakeLists.txt                 |   3 +
 nav2_util/test/test_controller_utils.cpp      | 265 +++++++++++++++++
 10 files changed, 556 insertions(+), 429 deletions(-)
 create mode 100644 nav2_util/include/nav2_util/controller_utils.hpp
 create mode 100644 nav2_util/src/controller_utils.cpp
 create mode 100644 nav2_util/test/test_controller_utils.cpp

diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
index 96358ae9607..28bec7eddd8 100644
--- a/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
+++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp
@@ -107,6 +107,24 @@ class GracefulController : public nav2_core::Controller
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
 protected:
+  /**
+   * @brief Validate a given target pose for calculating command velocity
+   * @param target_pose Target pose to validate
+   * @param dist_to_target Distance to target pose
+   * @param dist_to_goal Distance to navigation goal
+   * @param trajectory Trajectory to validate in simulation
+   * @param costmap_transform Transform between global and local costmap
+   * @param cmd_vel Initial command velocity to validate in simulation
+   * @return true if target pose is valid, false otherwise
+   */
+  bool validateTargetPose(
+    geometry_msgs::msg::PoseStamped & target_pose,
+    double dist_to_target,
+    double dist_to_goal,
+    nav_msgs::msg::Path & trajectory,
+    geometry_msgs::msg::TransformStamped & costmap_transform,
+    geometry_msgs::msg::TwistStamped & cmd_vel);
+
   /**
    * @brief Simulate trajectory calculating in every step the new velocity command based on
    * a new curvature value and checking for collisions.
diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp
index 0706c86378d..b8cce0b0aed 100644
--- a/nav2_graceful_controller/src/graceful_controller.cpp
+++ b/nav2_graceful_controller/src/graceful_controller.cpp
@@ -18,6 +18,7 @@
 #include "angles/angles.h"
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/controller_utils.hpp"
 #include "nav2_graceful_controller/graceful_controller.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
@@ -195,46 +196,38 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     // Else, fall through and see if we should follow control law longer
   }
 
-  // Precompute distance to candidate poses
+  // Find a valid target pose and its trajectory
+  nav_msgs::msg::Path local_plan;
+  geometry_msgs::msg::PoseStamped target_pose;
+
+  double dist_to_target;
   std::vector<double> target_distances;
   computeDistanceAlongPath(transformed_plan.poses, target_distances);
 
-  // Work back from the end of plan to find valid target pose
+  bool is_first_iteration = true;
   for (int i = transformed_plan.poses.size() - 1; i >= 0; --i) {
-    // Underlying control law needs a single target pose, which should:
-    //  * Be as far away as possible from the robot (for smoothness)
-    //  * But no further than the max_lookahed_ distance
-    //  * Be feasible to reach in a collision free manner
-    geometry_msgs::msg::PoseStamped target_pose = transformed_plan.poses[i];
-    double dist_to_target = target_distances[i];
-
-    // Continue if target_pose is too far away from robot
-    if (dist_to_target > params_->max_lookahead) {continue;}
-
-    if (dist_to_goal < params_->max_lookahead) {
-      if (params_->prefer_final_rotation) {
-        // Avoid instability and big sweeping turns at the end of paths by
-        // ignoring final heading
-        double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x);
-        target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
-      }
-    } else if (dist_to_target < params_->min_lookahead) {
-      // Make sure target is far enough away to avoid instability
-      break;
-    }
-
-    // Flip the orientation of the motion target if the robot is moving backwards
-    bool reversing = false;
-    if (params_->allow_backward && target_pose.pose.position.x < 0.0) {
-      reversing = true;
-      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
-        tf2::getYaw(target_pose.pose.orientation) + M_PI);
+    if (is_first_iteration) {
+      // Calculate target pose through lookahead interpolation to get most accurate
+      // lookahead point, if possible
+      dist_to_target = params_->max_lookahead;
+      // Interpolate after goal false for graceful controller
+      // Requires interpolating the orientation which is not yet implemented
+      // Updates dist_to_target for target_pose returned if using the point on the path
+      target_pose = nav2_util::getLookAheadPoint(dist_to_target, transformed_plan, false);
+      is_first_iteration = false;
+    } else {
+      // Underlying control law needs a single target pose, which should:
+      //  * Be as far away as possible from the robot (for smoothness)
+      //  * But no further than the max_lookahed_ distance
+      //  * Be feasible to reach in a collision free manner
+      dist_to_target = target_distances[i];
+      target_pose = transformed_plan.poses[i];
     }
 
-    // Actually simulate our path
-    nav_msgs::msg::Path local_plan;
-    if (simulateTrajectory(target_pose, costmap_transform, local_plan, cmd_vel, reversing)) {
-      // Successfully simulated to target_pose - compute velocity at this moment
+    // Compute velocity at this moment if valid target pose is found
+    if (validateTargetPose(
+        target_pose, dist_to_target, dist_to_goal, local_plan, costmap_transform, cmd_vel))
+    {
       // Publish the selected target_pose
       motion_target_pub_->publish(target_pose);
       // Publish marker for slowdown radius around motion target for debugging / visualization
@@ -283,6 +276,49 @@ void GracefulController::setSpeedLimit(
   }
 }
 
+bool GracefulController::validateTargetPose(
+  geometry_msgs::msg::PoseStamped & target_pose,
+  double dist_to_target,
+  double dist_to_goal,
+  nav_msgs::msg::Path & trajectory,
+  geometry_msgs::msg::TransformStamped & costmap_transform,
+  geometry_msgs::msg::TwistStamped & cmd_vel)
+{
+  // Continue if target_pose is too far away from robot
+  if (dist_to_target > params_->max_lookahead) {
+    return false;
+  }
+
+  if (dist_to_goal < params_->max_lookahead) {
+    if (params_->prefer_final_rotation) {
+      // Avoid instability and big sweeping turns at the end of paths by
+      // ignoring final heading
+      double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x);
+      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+    }
+  } else if (dist_to_target < params_->min_lookahead) {
+    // Make sure target is far enough away to avoid instability
+    return false;
+  }
+
+  // Flip the orientation of the motion target if the robot is moving backwards
+  bool reversing = false;
+  if (params_->allow_backward && target_pose.pose.position.x < 0.0) {
+    reversing = true;
+    target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+      tf2::getYaw(target_pose.pose.orientation) + M_PI);
+  }
+
+  // Actually simulate the path
+  if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) {
+    // Successfully simulated to target_pose
+    return true;
+  }
+
+  // Validation not successful
+  return false;
+}
+
 bool GracefulController::simulateTrajectory(
   const geometry_msgs::msg::PoseStamped & motion_target,
   const geometry_msgs::msg::TransformStamped & costmap_transform,
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
index 2eb901dc11f..fd79208d291 100644
--- 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
@@ -174,32 +174,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller
     const double & pose_cost, const nav_msgs::msg::Path & path,
     double & linear_vel, double & sign);
 
-  /**
-   * @brief Find the intersection a circle and a line segment.
-   * This assumes the circle is centered at the origin.
-   * If no intersection is found, a floating point error will occur.
-   * @param p1 first endpoint of line segment
-   * @param p2 second endpoint of line segment
-   * @param r radius of circle
-   * @return point of intersection
-   */
-  static geometry_msgs::msg::Point circleSegmentIntersection(
-    const geometry_msgs::msg::Point & p1,
-    const geometry_msgs::msg::Point & p2,
-    double r);
-
-  /**
-   * @brief Get lookahead point
-   * @param lookahead_dist Optimal lookahead distance
-   * @param path Current global path
-   * @param interpolate_after_goal If true, interpolate the lookahead point after the goal based
-   * on the orientation given by the position of the last two pose of the path
-   * @return Lookahead point
-   */
-  geometry_msgs::msg::PoseStamped getLookAheadPoint(
-    const double &, const nav_msgs::msg::Path &,
-    bool interpolate_after_goal = false);
-
   /**
    * @brief checks for the cusp position
    * @param pose Pose input to determine the cusp position
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 1bd12ae3a06..fced1dff734 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
@@ -25,6 +25,7 @@
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/controller_utils.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
 using std::hypot;
@@ -204,7 +205,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   }
 
   // Get the particular point on the path at the lookahead distance
-  auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);
+  auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_plan);
   auto rotate_to_path_carrot_pose = carrot_pose;
   carrot_pub_->publish(createCarrotMsg(carrot_pose));
 
@@ -214,7 +215,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   double regulation_curvature = lookahead_curvature;
   if (params_->use_fixed_curvature_lookahead) {
-    auto curvature_lookahead_pose = getLookAheadPoint(
+    auto curvature_lookahead_pose = nav2_util::getLookAheadPoint(
       curv_lookahead_dist,
       transformed_plan, params_->interpolate_curvature_after_goal);
     rotate_to_path_carrot_pose = curvature_lookahead_pose;
@@ -358,100 +359,6 @@ void RegulatedPurePursuitController::rotateToHeading(
   }
 }
 
-geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection(
-  const geometry_msgs::msg::Point & p1,
-  const geometry_msgs::msg::Point & p2,
-  double r)
-{
-  // Formula for intersection of a line with a circle centered at the origin,
-  // modified to always return the point that is on the segment between the two points.
-  // https://mathworld.wolfram.com/Circle-LineIntersection.html
-  // This works because the poses are transformed into the robot frame.
-  // This can be derived from solving the system of equations of a line and a circle
-  // which results in something that is just a reformulation of the quadratic formula.
-  // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
-  // https://www.desmos.com/calculator/td5cwbuocd
-  double x1 = p1.x;
-  double x2 = p2.x;
-  double y1 = p1.y;
-  double y2 = p2.y;
-
-  double dx = x2 - x1;
-  double dy = y2 - y1;
-  double dr2 = dx * dx + dy * dy;
-  double D = x1 * y2 - x2 * y1;
-
-  // Augmentation to only return point within segment
-  double d1 = x1 * x1 + y1 * y1;
-  double d2 = x2 * x2 + y2 * y2;
-  double dd = d2 - d1;
-
-  geometry_msgs::msg::Point p;
-  double sqrt_term = std::sqrt(r * r * dr2 - D * D);
-  p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2;
-  p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2;
-  return p;
-}
-
-geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(
-  const double & lookahead_dist,
-  const nav_msgs::msg::Path & transformed_plan,
-  bool interpolate_after_goal)
-{
-  // 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()) {
-    if (interpolate_after_goal) {
-      auto last_pose_it = std::prev(transformed_plan.poses.end());
-      auto prev_last_pose_it = std::prev(last_pose_it);
-
-      double end_path_orientation = atan2(
-        last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y,
-        last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x);
-
-      // Project the last segment out to guarantee it is beyond the look ahead
-      // distance
-      auto projected_position = last_pose_it->pose.position;
-      projected_position.x += cos(end_path_orientation) * lookahead_dist;
-      projected_position.y += sin(end_path_orientation) * lookahead_dist;
-
-      // Use the circle intersection to find the position at the correct look
-      // ahead distance
-      const auto interpolated_position = circleSegmentIntersection(
-        last_pose_it->pose.position, projected_position, lookahead_dist);
-
-      geometry_msgs::msg::PoseStamped interpolated_pose;
-      interpolated_pose.header = last_pose_it->header;
-      interpolated_pose.pose.position = interpolated_position;
-      return interpolated_pose;
-    } else {
-      goal_pose_it = std::prev(transformed_plan.poses.end());
-    }
-  } else if (goal_pose_it != transformed_plan.poses.begin()) {
-    // Find the point on the line segment between the two poses
-    // that is exactly the lookahead distance away from the robot pose (the origin)
-    // This can be found with a closed form for the intersection of a segment and a circle
-    // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
-    // and goal_pose is guaranteed to be outside the circle.
-    auto prev_pose_it = std::prev(goal_pose_it);
-    auto point = circleSegmentIntersection(
-      prev_pose_it->pose.position,
-      goal_pose_it->pose.position, lookahead_dist);
-    geometry_msgs::msg::PoseStamped pose;
-    pose.header.frame_id = prev_pose_it->header.frame_id;
-    pose.header.stamp = goal_pose_it->header.stamp;
-    pose.pose.position = point;
-    return pose;
-  }
-
-  return *goal_pose_it;
-}
-
 void RegulatedPurePursuitController::applyConstraints(
   const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/,
   const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign)
diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
index 197cae4c5a0..4b3dbb905f8 100644
--- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
+++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
@@ -52,28 +52,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
     return getLookAheadDistance(twist);
   }
 
-  static geometry_msgs::msg::Point circleSegmentIntersectionWrapper(
-    const geometry_msgs::msg::Point & p1,
-    const geometry_msgs::msg::Point & p2,
-    double r)
-  {
-    return circleSegmentIntersection(p1, p2, r);
-  }
-
-  geometry_msgs::msg::PoseStamped
-  projectCarrotPastGoalWrapper(
-    const double & dist,
-    const nav_msgs::msg::Path & path)
-  {
-    return getLookAheadPoint(dist, path, true);
-  }
-
-  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)
   {
@@ -207,239 +185,6 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange)
   EXPECT_EQ(rtn, std::numeric_limits<double>::max());
 }
 
-using CircleSegmentIntersectionParam = std::tuple<
-  std::pair<double, double>,
-  std::pair<double, double>,
-  double,
-  std::pair<double, double>
->;
-
-class CircleSegmentIntersectionTest
-  : public ::testing::TestWithParam<CircleSegmentIntersectionParam>
-{};
-
-TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection)
-{
-  auto pair1 = std::get<0>(GetParam());
-  auto pair2 = std::get<1>(GetParam());
-  auto r = std::get<2>(GetParam());
-  auto expected_pair = std::get<3>(GetParam());
-  auto pair_to_point = [](std::pair<double, double> p) -> geometry_msgs::msg::Point {
-      geometry_msgs::msg::Point point;
-      point.x = p.first;
-      point.y = p.second;
-      point.z = 0.0;
-      return point;
-    };
-  auto p1 = pair_to_point(pair1);
-  auto p2 = pair_to_point(pair2);
-  auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper(p1, p2, r);
-  auto expected_point = pair_to_point(expected_pair);
-  EXPECT_DOUBLE_EQ(actual.x, expected_point.x);
-  EXPECT_DOUBLE_EQ(actual.y, expected_point.y);
-  // Expect that the intersection point is actually r away from the origin
-  EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y));
-}
-
-INSTANTIATE_TEST_SUITE_P(
-  InterpolationTest,
-  CircleSegmentIntersectionTest,
-  testing::Values(
-    // Origin to the positive X axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {2.0, 0.0},
-  1.0,
-  {1.0, 0.0}
-},
-    // Origin to the negative X axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-2.0, 0.0},
-  1.0,
-  {-1.0, 0.0}
-},
-    // Origin to the positive Y axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, 2.0},
-  1.0,
-  {0.0, 1.0}
-},
-    // Origin to the negative Y axis
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, -2.0},
-  1.0,
-  {0.0, -1.0}
-},
-    // non-origin to the X axis with non-unit circle, with the second point inside
-    CircleSegmentIntersectionParam{
-  {4.0, 0.0},
-  {-1.0, 0.0},
-  2.0,
-  {2.0, 0.0}
-},
-    // non-origin to the Y axis with non-unit circle, with the second point inside
-    CircleSegmentIntersectionParam{
-  {0.0, 4.0},
-  {0.0, -0.5},
-  2.0,
-  {0.0, 2.0}
-},
-    // origin to the positive X axis, on the circle
-    CircleSegmentIntersectionParam{
-  {2.0, 0.0},
-  {0.0, 0.0},
-  2.0,
-  {2.0, 0.0}
-},
-    // origin to the positive Y axis, on the circle
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {0.0, 2.0},
-  2.0,
-  {0.0, 2.0}
-},
-    // origin to the upper-right quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {6.0, 8.0},
-  5.0,
-  {3.0, 4.0}
-},
-    // origin to the lower-left quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-6.0, -8.0},
-  5.0,
-  {-3.0, -4.0}
-},
-    // origin to the upper-left quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {-6.0, 8.0},
-  5.0,
-  {-3.0, 4.0}
-},
-    // origin to the lower-right quadrant (3-4-5 triangle)
-    CircleSegmentIntersectionParam{
-  {0.0, 0.0},
-  {6.0, -8.0},
-  5.0,
-  {3.0, -4.0}
-}
-));
-
-TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
-  auto ctrl = std::make_shared<BasicAPIRPP>();
-  auto node = std::make_shared<nav2::LifecycleNode>("testRPP");
-  std::string name = "PathFollower";
-  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
-  auto costmap =
-    std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
-  rclcpp_lifecycle::State state;
-  costmap->on_configure(state);
-  ctrl->configure(node, name, tf, costmap);
-
-  double EPSILON = std::numeric_limits<float>::epsilon();
-
-  nav_msgs::msg::Path path;
-  // More than 2 poses
-  path.poses.resize(4);
-  path.poses[0].pose.position.x = 0.0;
-  path.poses[1].pose.position.x = 1.0;
-  path.poses[2].pose.position.x = 2.0;
-  path.poses[3].pose.position.x = 3.0;
-  auto pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
-
-  // 2 poses fwd
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 2.0;
-  path.poses[1].pose.position.x = 3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
-
-  // 2 poses at 45°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 2.0;
-  path.poses[0].pose.position.y = 2.0;
-  path.poses[1].pose.position.x = 3.0;
-  path.poses[1].pose.position.y = 3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses at 90°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 0.0;
-  path.poses[0].pose.position.y = 2.0;
-  path.poses[1].pose.position.x = 0.0;
-  path.poses[1].pose.position.y = 3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses at 135°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = -2.0;
-  path.poses[0].pose.position.y = 2.0;
-  path.poses[1].pose.position.x = -3.0;
-  path.poses[1].pose.position.y = 3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses back
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = -2.0;
-  path.poses[1].pose.position.x = -3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
-
-  // 2 poses at -135°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = -2.0;
-  path.poses[0].pose.position.y = -2.0;
-  path.poses[1].pose.position.x = -3.0;
-  path.poses[1].pose.position.y = -3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses at -90°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 0.0;
-  path.poses[0].pose.position.y = -2.0;
-  path.poses[1].pose.position.x = 0.0;
-  path.poses[1].pose.position.y = -3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON);
-
-  // 2 poses at -45°
-  path.poses.clear();
-  path.poses.resize(2);
-  path.poses[0].pose.position.x = 2.0;
-  path.poses[0].pose.position.y = -2.0;
-  path.poses[1].pose.position.x = 3.0;
-  path.poses[1].pose.position.y = -3.0;
-  pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
-  EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON);
-  EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON);
-}
-
 TEST(RegulatedPurePursuitTest, lookaheadAPI)
 {
   auto ctrl = std::make_shared<BasicAPIRPP>();
@@ -476,24 +221,6 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI)
   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<double>(i);
-  }
-
-  // test exact hits
-  auto pt = ctrl->getLookAheadPointWrapper(dist, path);
-  EXPECT_EQ(pt.pose.position.x, 1.0);
-
-  // test interpolation
-  ctrl->configure(node, name, tf, costmap);
-  dist = 3.8;
-  pt = ctrl->getLookAheadPointWrapper(dist, path);
-  EXPECT_EQ(pt.pose.position.x, 3.8);
 }
 
 TEST(RegulatedPurePursuitTest, rotateTests)
diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp
new file mode 100644
index 00000000000..dbe04b1f2f3
--- /dev/null
+++ b/nav2_util/include/nav2_util/controller_utils.hpp
@@ -0,0 +1,54 @@
+// 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_UTIL__CONTROLLER_UTILS_HPP_
+#define NAV2_UTIL__CONTROLLER_UTILS_HPP_
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+namespace nav2_util
+{
+/**
+* @brief Find the intersection a circle and a line segment.
+* This assumes the circle is centered at the origin.
+* If no intersection is found, a floating point error will occur.
+* @param p1 first endpoint of line segment
+* @param p2 second endpoint of line segment
+* @param r radius of circle
+* @return point of intersection
+*/
+geometry_msgs::msg::Point circleSegmentIntersection(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  double r);
+
+/**
+* @brief Get lookahead point
+* @param lookahead_dist Optimal lookahead distance
+* @param path Current global path
+* @param interpolate_after_goal If true, interpolate the lookahead point after the goal based
+* on the orientation given by the position of the last two pose of the path
+* @return Lookahead point
+*/
+geometry_msgs::msg::PoseStamped getLookAheadPoint(
+  double &, const nav_msgs::msg::Path &,
+  const bool interpolate_after_goal = false);
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__CONTROLLER_UTILS_HPP_
diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt
index 563742e8b72..99d24ee6348 100644
--- a/nav2_util/src/CMakeLists.txt
+++ b/nav2_util/src/CMakeLists.txt
@@ -3,6 +3,7 @@ add_library(${library_name} SHARED
   lifecycle_service_client.cpp
   string_utils.cpp
   robot_utils.cpp
+  controller_utils.cpp
   odometry_utils.cpp
   array_parser.cpp
 )
diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp
new file mode 100644
index 00000000000..18f2ae42dd3
--- /dev/null
+++ b/nav2_util/src/controller_utils.cpp
@@ -0,0 +1,142 @@
+// 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 <cmath>
+#include "nav2_util/controller_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+namespace nav2_util
+{
+geometry_msgs::msg::Point circleSegmentIntersection(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  double r)
+{
+  // Formula for intersection of a line with a circle centered at the origin,
+  // modified to always return the point that is on the segment between the two points.
+  // https://mathworld.wolfram.com/Circle-LineIntersection.html
+  // This works because the poses are transformed into the robot frame.
+  // This can be derived from solving the system of equations of a line and a circle
+  // which results in something that is just a reformulation of the quadratic formula.
+  // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
+  // https://www.desmos.com/calculator/td5cwbuocd
+  double x1 = p1.x;
+  double x2 = p2.x;
+  double y1 = p1.y;
+  double y2 = p2.y;
+
+  double dx = x2 - x1;
+  double dy = y2 - y1;
+  double dr2 = dx * dx + dy * dy;
+  double D = x1 * y2 - x2 * y1;
+
+  // Augmentation to only return point within segment
+  double d1 = x1 * x1 + y1 * y1;
+  double d2 = x2 * x2 + y2 * y2;
+  double dd = d2 - d1;
+
+  geometry_msgs::msg::Point p;
+  double sqrt_term = std::sqrt(r * r * dr2 - D * D);
+  p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2;
+  p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2;
+  return p;
+}
+
+geometry_msgs::msg::PoseStamped getLookAheadPoint(
+  double & lookahead_dist,
+  const nav_msgs::msg::Path & transformed_plan,
+  const bool interpolate_after_goal)
+{
+  // Find the first pose which is at a distance greater than the lookahead distance
+  // Using distance along the path
+  const auto & poses = transformed_plan.poses;
+  auto goal_pose_it = poses.begin();
+  double d = 0.0;
+
+  bool pose_found = false;
+  for (size_t i = 1; i < poses.size(); i++) {
+    const auto & prev_pose = poses[i - 1].pose.position;
+    const auto & curr_pose = poses[i].pose.position;
+
+    d += std::hypot(curr_pose.x - prev_pose.x, curr_pose.y - prev_pose.y);
+    if (d >= lookahead_dist) {
+      goal_pose_it = poses.begin() + i;
+      pose_found = true;
+      break;
+    }
+  }
+
+  if (!pose_found) {
+    goal_pose_it = poses.end();
+  }
+
+  // If the no pose is not far enough, take the last pose
+  if (goal_pose_it == transformed_plan.poses.end()) {
+    if (interpolate_after_goal) {
+      auto last_pose_it = std::prev(transformed_plan.poses.end());
+      auto prev_last_pose_it = std::prev(last_pose_it);
+
+      double end_path_orientation = atan2(
+        last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y,
+        last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x);
+
+      // Project the last segment out to guarantee it is beyond the look ahead
+      // distance
+      auto projected_position = last_pose_it->pose.position;
+      projected_position.x += cos(end_path_orientation) * lookahead_dist;
+      projected_position.y += sin(end_path_orientation) * lookahead_dist;
+
+      // Use the circle intersection to find the position at the correct look
+      // ahead distance
+      const auto interpolated_position = circleSegmentIntersection(
+        last_pose_it->pose.position, projected_position, lookahead_dist);
+
+      geometry_msgs::msg::PoseStamped interpolated_pose;
+      interpolated_pose.header = last_pose_it->header;
+      interpolated_pose.pose.position = interpolated_position;
+
+      return interpolated_pose;
+    } else {
+      lookahead_dist = d;  // Updating lookahead distance since using the final point
+      goal_pose_it = std::prev(transformed_plan.poses.end());
+    }
+  } else if (goal_pose_it != transformed_plan.poses.begin()) {
+    // Find the point on the line segment between the two poses
+    // that is exactly the lookahead distance away from the robot pose (the origin)
+    // This can be found with a closed form for the intersection of a segment and a circle
+    // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
+    // and goal_pose is guaranteed to be outside the circle.
+    auto prev_pose_it = std::prev(goal_pose_it);
+    auto point = circleSegmentIntersection(
+      prev_pose_it->pose.position,
+      goal_pose_it->pose.position, lookahead_dist);
+
+    // Calculate orientation towards interpolated position
+    // Convert yaw to quaternion
+    double yaw = atan2(
+      point.y - prev_pose_it->pose.position.y,
+      point.x - prev_pose_it->pose.position.x);
+
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.frame_id = prev_pose_it->header.frame_id;
+    pose.header.stamp = goal_pose_it->header.stamp;
+    pose.pose.position = point;
+    pose.pose.orientation = geometry_utils::orientationAroundZAxis(yaw);
+    return pose;
+  }
+
+  return *goal_pose_it;
+}
+}  // namespace nav2_util
diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt
index 4527fd8373d..09d7d4a38c0 100644
--- a/nav2_util/test/CMakeLists.txt
+++ b/nav2_util/test/CMakeLists.txt
@@ -16,6 +16,9 @@ target_link_libraries(test_odometry_utils ${library_name} ${nav_msgs_TARGETS} ${
 ament_add_gtest(test_robot_utils test_robot_utils.cpp)
 target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS})
 
+ament_add_gtest(test_controller_utils test_controller_utils.cpp)
+target_link_libraries(test_controller_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS})
+
 ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp)
 target_include_directories(test_base_footprint_publisher PRIVATE "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/src>")
 
diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp
new file mode 100644
index 00000000000..3c13d8e9bbc
--- /dev/null
+++ b/nav2_util/test/test_controller_utils.cpp
@@ -0,0 +1,265 @@
+// 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 <cmath>
+#include "nav2_util/controller_utils.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "gtest/gtest.h"
+
+
+using CircleSegmentIntersectionParam = std::tuple<
+  std::pair<double, double>,
+  std::pair<double, double>,
+  double,
+  std::pair<double, double>
+>;
+
+class CircleSegmentIntersectionTest
+  : public ::testing::TestWithParam<CircleSegmentIntersectionParam>
+{};
+
+TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection)
+{
+  auto pair1 = std::get<0>(GetParam());
+  auto pair2 = std::get<1>(GetParam());
+  auto r = std::get<2>(GetParam());
+  auto expected_pair = std::get<3>(GetParam());
+  auto pair_to_point = [](std::pair<double, double> p) -> geometry_msgs::msg::Point {
+      geometry_msgs::msg::Point point;
+      point.x = p.first;
+      point.y = p.second;
+      point.z = 0.0;
+      return point;
+    };
+  auto p1 = pair_to_point(pair1);
+  auto p2 = pair_to_point(pair2);
+  auto actual = nav2_util::circleSegmentIntersection(p1, p2, r);
+  auto expected_point = pair_to_point(expected_pair);
+  EXPECT_DOUBLE_EQ(actual.x, expected_point.x);
+  EXPECT_DOUBLE_EQ(actual.y, expected_point.y);
+  // Expect that the intersection point is actually r away from the origin
+  EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  InterpolationTest,
+  CircleSegmentIntersectionTest,
+  testing::Values(
+    // Origin to the positive X axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {2.0, 0.0},
+  1.0,
+  {1.0, 0.0}
+},
+    // Origin to the negative X axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-2.0, 0.0},
+  1.0,
+  {-1.0, 0.0}
+},
+    // Origin to the positive Y axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, 2.0},
+  1.0,
+  {0.0, 1.0}
+},
+    // Origin to the negative Y axis
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, -2.0},
+  1.0,
+  {0.0, -1.0}
+},
+    // non-origin to the X axis with non-unit circle, with the second point inside
+    CircleSegmentIntersectionParam{
+  {4.0, 0.0},
+  {-1.0, 0.0},
+  2.0,
+  {2.0, 0.0}
+},
+    // non-origin to the Y axis with non-unit circle, with the second point inside
+    CircleSegmentIntersectionParam{
+  {0.0, 4.0},
+  {0.0, -0.5},
+  2.0,
+  {0.0, 2.0}
+},
+    // origin to the positive X axis, on the circle
+    CircleSegmentIntersectionParam{
+  {2.0, 0.0},
+  {0.0, 0.0},
+  2.0,
+  {2.0, 0.0}
+},
+    // origin to the positive Y axis, on the circle
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {0.0, 2.0},
+  2.0,
+  {0.0, 2.0}
+},
+    // origin to the upper-right quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {6.0, 8.0},
+  5.0,
+  {3.0, 4.0}
+},
+    // origin to the lower-left quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-6.0, -8.0},
+  5.0,
+  {-3.0, -4.0}
+},
+    // origin to the upper-left quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {-6.0, 8.0},
+  5.0,
+  {-3.0, 4.0}
+},
+    // origin to the lower-right quadrant (3-4-5 triangle)
+    CircleSegmentIntersectionParam{
+  {0.0, 0.0},
+  {6.0, -8.0},
+  5.0,
+  {3.0, -4.0}
+}
+));
+
+TEST(InterpolationUtils, lookaheadInterpolation)
+{
+  // test Lookahead Point Interpolation
+  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<double>(i);
+  }
+
+  // test exact hits
+  auto pt = nav2_util::getLookAheadPoint(dist, path);
+  EXPECT_EQ(pt.pose.position.x, 1.0);
+
+  // test interpolation
+  dist = 3.8;
+  pt = nav2_util::getLookAheadPoint(dist, path);
+  EXPECT_EQ(pt.pose.position.x, 3.8);
+}
+
+TEST(InterpolationUtils, lookaheadExtrapolation)
+{
+  // Test extrapolation beyond goal
+  double EPSILON = std::numeric_limits<float>::epsilon();
+
+  nav_msgs::msg::Path path;
+  double lookahead_dist = 10.0;
+  // More than 2 poses
+  path.poses.resize(4);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[1].pose.position.x = 1.0;
+  path.poses[2].pose.position.x = 2.0;
+  path.poses[3].pose.position.x = 3.0;
+  auto pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses fwd
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[1].pose.position.x = 3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses at 45°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = 3.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at 90°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = 0.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at 135°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[0].pose.position.y = 2.0;
+  path.poses[1].pose.position.x = -3.0;
+  path.poses[1].pose.position.y = 3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses back
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[1].pose.position.x = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
+
+  // 2 poses at -135°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = -2.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = -3.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at -90°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = 0.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON);
+
+  // 2 poses at -45°
+  path.poses.clear();
+  path.poses.resize(2);
+  path.poses[0].pose.position.x = 2.0;
+  path.poses[0].pose.position.y = -2.0;
+  path.poses[1].pose.position.x = 3.0;
+  path.poses[1].pose.position.y = -3.0;
+  pt = nav2_util::getLookAheadPoint(lookahead_dist, path, true);
+  EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON);
+  EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON);
+}

From 935ffb9925275eec72d6daae964bf605db0a6081 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Mon, 11 Aug 2025 11:37:39 -0700
Subject: [PATCH 20/51] Bump the cache for file movements to upload codecov
 results

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
---
 .circleci/config.yml | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/.circleci/config.yml b/.circleci/config.yml
index edb996eb1f7..841d60042d1 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v38\
+              - "<< parameters.key >>-v39\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v38\
+              - "<< parameters.key >>-v39\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v38\
+            key: "<< parameters.key >>-v39\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

From f7b665406c46494b0f6fbfe80bd16d4f0d85e1df Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Mon, 11 Aug 2025 14:09:58 -0700
Subject: [PATCH 21/51] Fix lifecycle manager deadlock during shutdown (#5438)

* Fix lifecycle manager deadlock during shutdown

Add stop() method to ServiceClient that cancels internal executor operations
and call it in LifecycleServiceClient destructor to prevent deadlock when
CTRL+C is pressed during lifecycle node bringup.

This addresses issue #5437 where spin_until_future_complete can hang
indefinitely during shutdown when bringup and shutdown sequences run
concurrently.

Co-authored-by: Steve Macenski <SteveMacenski@users.noreply.github.com>

* Update service_client.hpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com>
---
 .../include/nav2_ros_common/service_client.hpp         | 10 ++++++++++
 .../include/nav2_util/lifecycle_service_client.hpp     |  6 ++++++
 2 files changed, 16 insertions(+)

diff --git a/nav2_ros_common/include/nav2_ros_common/service_client.hpp b/nav2_ros_common/include/nav2_ros_common/service_client.hpp
index c1bc2bd8435..1cf32f25fe0 100644
--- a/nav2_ros_common/include/nav2_ros_common/service_client.hpp
+++ b/nav2_ros_common/include/nav2_ros_common/service_client.hpp
@@ -218,6 +218,16 @@ class ServiceClient
     return service_name_;
   }
 
+  /**
+  * @brief Stop any running spin operations on the internal executor
+  */
+  void stop()
+  {
+    if (client_) {
+      callback_group_executor_->cancel();
+    }
+  }
+
 protected:
   std::string service_name_;
   rclcpp::Clock::SharedPtr clock_;
diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp
index 64860f47e87..56eea6b5ccd 100644
--- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp
+++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp
@@ -57,6 +57,12 @@ class LifecycleServiceClient
     }
   }
 
+  ~LifecycleServiceClient()
+  {
+    change_state_.stop();
+    get_state_.stop();
+  }
+
   /// Trigger a state change
   /**
    * Throws std::runtime_error on failure

From 2a377c36fa4b9eb5ac39e38714447a40e18ebe0e Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 12 Aug 2025 07:27:28 -0700
Subject: [PATCH 22/51] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20actions/ch?=
 =?UTF-8?q?eckout=20from=204=20to=205=20(#5445)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Bumps [actions/checkout](https://github.com/actions/checkout) from 4 to 5.
- [Release notes](https://github.com/actions/checkout/releases)
- [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md)
- [Commits](https://github.com/actions/checkout/compare/v4...v5)

---
updated-dependencies:
- dependency-name: actions/checkout
  dependency-version: '5'
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 .github/workflows/build_main_against_distros.yml | 2 +-
 .github/workflows/claude.yml                     | 2 +-
 .github/workflows/lint.yml                       | 4 ++--
 .github/workflows/update_ci_image.yaml           | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/.github/workflows/build_main_against_distros.yml b/.github/workflows/build_main_against_distros.yml
index 5b4aa08f1e1..95941856d3e 100644
--- a/.github/workflows/build_main_against_distros.yml
+++ b/.github/workflows/build_main_against_distros.yml
@@ -17,7 +17,7 @@ jobs:
 
     steps:
     - name: Checkout repository
-      uses: actions/checkout@v4
+      uses: actions/checkout@v5
 
     - name: Set up Docker build context
       run: |
diff --git a/.github/workflows/claude.yml b/.github/workflows/claude.yml
index 3ffbfa392fd..73467b184cc 100644
--- a/.github/workflows/claude.yml
+++ b/.github/workflows/claude.yml
@@ -34,7 +34,7 @@ jobs:
       id-token: write
     steps:
       - name: Checkout repository
-        uses: actions/checkout@v4
+        uses: actions/checkout@v5
         with:
           fetch-depth: 1
 
diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
index 62645e662aa..337b0966f0d 100644
--- a/.github/workflows/lint.yml
+++ b/.github/workflows/lint.yml
@@ -13,7 +13,7 @@ jobs:
       matrix:
           linter: [xmllint, cpplint, uncrustify, pep257, flake8, mypy]
     steps:
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v5
 
     - name: Install typeshed for mypy
       if: matrix.linter == 'mypy'
@@ -30,7 +30,7 @@ jobs:
     name: pre-commit
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - uses: actions/setup-python@v5
       - uses: pre-commit/action@v3.0.1
         env:
diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml
index ef39facf09c..3c7743f1ad3 100644
--- a/.github/workflows/update_ci_image.yaml
+++ b/.github/workflows/update_ci_image.yaml
@@ -66,7 +66,7 @@ jobs:
       - check_ci_image
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v3
       - name: Login to Docker Hub

From b066a1872bcc3b5baa6b1db981918b202bd378a4 Mon Sep 17 00:00:00 2001
From: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>
Date: Tue, 12 Aug 2025 19:41:55 +0300
Subject: [PATCH 23/51] Include option to use PointCloud Transport (#5264)

* Rebase

Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>

* Make linters happy

Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>

* Rename point_cloud_transport parameter

Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>

* Rebase

Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>

* Feedback

Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>

* Feedback

Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>

* Fix

Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>

---------

Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>
---
 nav2_collision_monitor/CMakeLists.txt           |  4 ++++
 .../nav2_collision_monitor/pointcloud.hpp       |  9 +++++++++
 nav2_collision_monitor/package.xml              |  2 ++
 .../params/collision_detector_params.yaml       |  1 +
 .../params/collision_monitor_params.yaml        |  1 +
 nav2_collision_monitor/src/pointcloud.cpp       | 17 +++++++++++++++++
 nav2_costmap_2d/CMakeLists.txt                  |  3 +++
 .../include/nav2_costmap_2d/obstacle_layer.hpp  |  1 +
 nav2_costmap_2d/package.xml                     |  2 ++
 nav2_costmap_2d/plugins/obstacle_layer.cpp      | 15 ++++++++++++---
 10 files changed, 52 insertions(+), 3 deletions(-)

diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
index e67c8ea7719..040e3f51a37 100644
--- a/nav2_collision_monitor/CMakeLists.txt
+++ b/nav2_collision_monitor/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(nav2_ros_common REQUIRED)
+find_package(point_cloud_transport REQUIRED)
 
 nav2_package()
 
@@ -54,6 +55,7 @@ target_link_libraries(${monitor_library_name} PUBLIC
   tf2::tf2
   tf2_ros::tf2_ros
   ${visualization_msgs_TARGETS}
+  point_cloud_transport::point_cloud_transport
 )
 target_link_libraries(${monitor_library_name} PRIVATE
   rclcpp_components::component
@@ -88,6 +90,7 @@ target_link_libraries(${detector_library_name} PUBLIC
   tf2_ros::tf2_ros
   tf2::tf2
   ${visualization_msgs_TARGETS}
+  point_cloud_transport::point_cloud_transport
 )
 target_link_libraries(${detector_library_name} PRIVATE
   rclcpp_components::component
@@ -169,6 +172,7 @@ ament_export_dependencies(
   tf2_ros
   nav2_ros_common
   visualization_msgs
+  point_cloud_transport
 )
 ament_export_targets(export_${PROJECT_NAME})
 
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
index 8a46d2b65c5..c94180f94cf 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
@@ -20,6 +20,7 @@
 #include <string>
 
 #include "sensor_msgs/msg/point_cloud2.hpp"
+#include "point_cloud_transport/point_cloud_transport.hpp"
 
 #include "nav2_collision_monitor/source.hpp"
 
@@ -98,7 +99,15 @@ class PointCloud : public Source
   // ----- Variables -----
 
   /// @brief PointCloud data subscriber
+  #if RCLCPP_VERSION_GTE(30, 0, 0)
+  std::shared_ptr<point_cloud_transport::PointCloudTransport> pct_;
+  point_cloud_transport::Subscriber data_sub_;
+  #else
   nav2::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr data_sub_;
+  #endif
+
+  // Transport type used for PointCloud messages (e.g., raw or compressed)
+  std::string transport_type_;
 
   // Minimum and maximum height of PointCloud projected to 2D space
   double min_height_, max_height_;
diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
index 6a8e5ace75b..6b009f1634b 100644
--- a/nav2_collision_monitor/package.xml
+++ b/nav2_collision_monitor/package.xml
@@ -25,6 +25,8 @@
   <depend>tf2_ros</depend>
   <depend>visualization_msgs</depend>
   <depend>nav2_ros_common</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>point_cloud_transport_plugins</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml
index eb3ee0a8739..bbeaadc5154 100644
--- a/nav2_collision_monitor/params/collision_detector_params.yaml
+++ b/nav2_collision_monitor/params/collision_detector_params.yaml
@@ -23,6 +23,7 @@ collision_detector:
     pointcloud:
       type: "pointcloud"
       topic: "/intel_realsense_r200_depth/points"
+      transport_type: "raw" # Options: raw, zlib, draco, zstd
       min_height: 0.1
       max_height: 0.5
       enabled: True
diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
index eac7486913c..28224362ea9 100644
--- a/nav2_collision_monitor/params/collision_monitor_params.yaml
+++ b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -96,6 +96,7 @@ collision_monitor:
     pointcloud:
       type: "pointcloud"
       topic: "/intel_realsense_r200_depth/points"
+      transport_type: "raw" # Options: raw, zlib, draco, zstd
       min_height: 0.1
       max_height: 0.5
       enabled: True
diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp
index 590eacb3cb8..72ef467b9a5 100644
--- a/nav2_collision_monitor/src/pointcloud.cpp
+++ b/nav2_collision_monitor/src/pointcloud.cpp
@@ -45,7 +45,11 @@ PointCloud::PointCloud(
 PointCloud::~PointCloud()
 {
   RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str());
+  #if RCLCPP_VERSION_GTE(30, 0, 0)
+  data_sub_.shutdown();
+  #else
   data_sub_.reset();
+  #endif
 }
 
 void PointCloud::configure()
@@ -60,10 +64,20 @@ void PointCloud::configure()
 
   getParameters(source_topic);
 
+  #if RCLCPP_VERSION_GTE(30, 0, 0)
+  const point_cloud_transport::TransportHints hint(transport_type_);
+  pct_ = std::make_shared<point_cloud_transport::PointCloudTransport>(*node);
+  data_sub_ = pct_->subscribe(
+    source_topic, nav2::qos::SensorDataQoS(),
+    std::bind(&PointCloud::dataCallback, this, std::placeholders::_1),
+    {}, &hint
+  );
+  #else
   data_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
     source_topic,
     std::bind(&PointCloud::dataCallback, this, std::placeholders::_1),
     nav2::qos::SensorDataQoS());
+  #endif
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -131,6 +145,9 @@ void PointCloud::getParameters(std::string & source_topic)
   nav2::declare_parameter_if_not_declared(
     node, source_name_ + ".min_range", rclcpp::ParameterValue(0.0));
   min_range_ = node->get_parameter(source_name_ + ".min_range").as_double();
+  nav2::declare_parameter_if_not_declared(
+    node, source_name_ + ".transport_type", rclcpp::ParameterValue(std::string("raw")));
+  transport_type_ = node->get_parameter(source_name_ + ".transport_type").as_string();
 }
 
 void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index 851f2428b42..d7d74038949 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -26,6 +26,7 @@ find_package(tf2_ros REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(tf2_sensor_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(point_cloud_transport REQUIRED)
 
 nav2_package()
 
@@ -88,6 +89,7 @@ target_link_libraries(layers PUBLIC
   nav2_ros_common::nav2_ros_common
   tf2::tf2
   nav2_ros_common::nav2_ros_common
+  point_cloud_transport::point_cloud_transport
 )
 target_link_libraries(layers PRIVATE
   pluginlib::pluginlib
@@ -222,6 +224,7 @@ ament_export_dependencies(
   tf2_ros
   tf2_sensor_msgs
   nav2_ros_common
+  point_cloud_transport
 )
 pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
 ament_package()
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 5e66e24795a..32b17ca5d5b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -51,6 +51,7 @@
 #pragma GCC diagnostic pop
 
 #include "message_filters/subscriber.hpp"
+#include "point_cloud_transport/subscriber_filter.hpp"
 
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml
index c69d467b908..4e4940a59af 100644
--- a/nav2_costmap_2d/package.xml
+++ b/nav2_costmap_2d/package.xml
@@ -42,6 +42,8 @@
   <depend>tf2_sensor_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>nav2_ros_common</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>point_cloud_transport_plugins</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index bac51b92f92..b897ac515be 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -141,7 +141,7 @@ void ObstacleLayer::onInitialize()
   while (ss >> source) {
     // get the parameters for the specific topic
     double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
-    std::string topic, sensor_frame, data_type;
+    std::string topic, sensor_frame, data_type, transport_type;
     bool inf_is_valid, clearing, marking;
 
     declareParameter(source + "." + "topic", rclcpp::ParameterValue(source));
@@ -158,6 +158,7 @@ void ObstacleLayer::onInitialize()
     declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0));
     declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0));
     declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0));
+    declareParameter(source + "." + "transport_type", rclcpp::ParameterValue(std::string("raw")));
 
     node->get_parameter(name_ + "." + source + "." + "topic", topic);
     node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame);
@@ -173,6 +174,7 @@ void ObstacleLayer::onInitialize()
     node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
     node->get_parameter(name_ + "." + source + "." + "marking", marking);
     node->get_parameter(name_ + "." + source + "." + "clearing", clearing);
+    node->get_parameter(name_ + "." + source + "." + "transport_type", transport_type);
 
     if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
       RCLCPP_FATAL(
@@ -287,16 +289,23 @@ void ObstacleLayer::onInitialize()
           tf_filter_tolerance));
 
     } else {
+      // For Rolling and Newer Support from PointCloudTransport API change
+      #if RCLCPP_VERSION_GTE(30, 0, 0)
+      std::shared_ptr<point_cloud_transport::SubscriberFilter> sub;
       // For Kilted and Older Support from Message Filters API change
-      #if RCLCPP_VERSION_GTE(29, 6, 0)
+      #elif RCLCPP_VERSION_GTE(29, 6, 0)
       std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> sub;
       #else
       std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
         rclcpp_lifecycle::LifecycleNode>> sub;
       #endif
 
+      // For Rolling compatibility in PointCloudTransport API change
+      #if RCLCPP_VERSION_GTE(30, 0, 0)
+      sub = std::make_shared<point_cloud_transport::SubscriberFilter>(
+        *node, topic, transport_type, custom_qos_profile, sub_opt);
       // For Kilted compatibility in Message Filters API change
-      #if RCLCPP_VERSION_GTE(29, 6, 0)
+      #elif RCLCPP_VERSION_GTE(29, 6, 0)
       sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>(
         node, topic, custom_qos_profile, sub_opt);
       // For Jazzy compatibility in Message Filters API change

From 920dff0fa07bfa27aff7b9ec94972e82fa0e8eba Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Wed, 13 Aug 2025 03:07:56 +0800
Subject: [PATCH 24/51] Add option to override lethal cost in keepout zone
 (#5433)

* Add option to override lethal cost in keepout zone

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Linting

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Base class call

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 .../costmap_filters/keepout_filter.hpp        |   5 +-
 .../costmap_filters/keepout_filter.cpp        | 121 +++++++++---------
 2 files changed, 67 insertions(+), 59 deletions(-)

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
index b23e03a5e49..118fc3ef295 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
@@ -120,8 +120,9 @@ class KeepoutFilter : public CostmapFilter
   bool override_lethal_cost_{false};  // If true, lethal cost will be overridden
   unsigned char lethal_override_cost_{252};  // Value to override lethal cost with
   bool last_pose_lethal_{false};  // If true, last pose was lethal
-  unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u};
-  unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u};
+  bool is_pose_lethal_{false};
+  double lethal_state_update_min_x_, lethal_state_update_min_y_;
+  double lethal_state_update_max_x_, lethal_state_update_max_y_;
 
   unsigned int x_{0};
   unsigned int y_{0};
diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
index 5fa544dec1c..48cd6c74e42 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
@@ -43,6 +43,7 @@
 
 #include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -84,6 +85,8 @@ void KeepoutFilter::initializeFilter(
   // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given
   lethal_override_cost_ = \
     std::clamp<unsigned int>(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE);
+  lethal_state_update_max_x_ = lethal_state_update_max_y_ = std::numeric_limits<double>::lowest();
+  lethal_state_update_min_x_ = lethal_state_update_min_y_ = std::numeric_limits<double>::max();
 }
 
 void KeepoutFilter::filterInfoCallback(
@@ -171,27 +174,70 @@ void KeepoutFilter::updateBounds(
 
   CostmapFilter::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
 
-  if(!has_updated_data_) {
-    return;
-  }
+  // If new keepout zone received
+  if(has_updated_data_) {
+    double wx, wy;
+    layered_costmap_->getCostmap()->mapToWorld(x_, y_, wx, wy);
+    *min_x = std::min(wx, *min_x);
+    *min_y = std::min(wy, *min_y);
 
-  double wx, wy;
+    layered_costmap_->getCostmap()->mapToWorld(x_ + width_, y_ + height_, wx, wy);
+    *max_x = std::max(wx, *max_x);
+    *max_y = std::max(wy, *max_y);
 
-  layered_costmap_->getCostmap()->mapToWorld(x_, y_, wx, wy);
-  *min_x = std::min(wx, *min_x);
-  *min_y = std::min(wy, *min_y);
+    has_updated_data_ = false;
+    return;
+  }
 
-  layered_costmap_->getCostmap()->mapToWorld(x_ + width_, y_ + height_, wx, wy);
-  *max_x = std::max(wx, *max_x);
-  *max_y = std::max(wy, *max_y);
+  // Let's find the pose's cost if we are allowed to override the lethal cost
+  is_pose_lethal_ = false;
+  if (override_lethal_cost_) {
+    geometry_msgs::msg::Pose pose;
+    pose.position.x = robot_x;
+    pose.position.y = robot_y;
+    pose.position.z = 0.0;
+    pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(robot_yaw);
+    geometry_msgs::msg::Pose mask_pose;
+    if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
+      unsigned int mask_robot_i, mask_robot_j;
+      if (worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
+        mask_robot_j))
+      {
+        auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
+        is_pose_lethal_ = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
+        if (is_pose_lethal_) {
+          RCLCPP_WARN_THROTTLE(
+            logger_, *(clock_), 2000,
+            "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out.");
+        }
+      }
+    }
 
-  has_updated_data_ = false;
+    // If in lethal space or just exited lethal space,
+    // we need to update all possible spaces touched during this state
+    if (is_pose_lethal_ || (last_pose_lethal_ && !is_pose_lethal_)) {
+      lethal_state_update_min_x_ = std::min(*min_x, lethal_state_update_min_x_);
+      *min_x = lethal_state_update_min_x_;
+      lethal_state_update_min_y_ = std::min(*min_y, lethal_state_update_min_y_);
+      *min_y = lethal_state_update_min_y_;
+      lethal_state_update_max_x_ = std::max(*max_x, lethal_state_update_max_x_);
+      *max_x = lethal_state_update_max_x_;
+      lethal_state_update_max_y_ = std::max(*max_y, lethal_state_update_max_y_);
+      *max_y = lethal_state_update_max_y_;
+    } else {
+      // If out of lethal space, reset managed lethal state sizes
+      lethal_state_update_min_x_ = std::numeric_limits<double>::max();
+      lethal_state_update_min_y_ = std::numeric_limits<double>::max();
+      lethal_state_update_max_x_ = std::numeric_limits<double>::lowest();
+      lethal_state_update_max_y_ = std::numeric_limits<double>::lowest();
+    }
+  }
 }
 
 void KeepoutFilter::process(
   nav2_costmap_2d::Costmap2D & master_grid,
   int min_i, int min_j, int max_i, int max_j,
-  const geometry_msgs::msg::Pose & pose)
+  const geometry_msgs::msg::Pose & /*pose*/)
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
@@ -286,49 +332,10 @@ void KeepoutFilter::process(
   }
 
   // unsigned<-signed conversions.
-  unsigned int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
-  unsigned int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
-  unsigned int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
-  unsigned int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
-
-  // Let's find the pose's cost if we are allowed to override the lethal cost
-  bool is_pose_lethal = false;
-  if (override_lethal_cost_) {
-    geometry_msgs::msg::Pose mask_pose;
-    if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
-      unsigned int mask_robot_i, mask_robot_j;
-      if (worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
-        mask_robot_j))
-      {
-        auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
-        is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
-        if (is_pose_lethal) {
-          RCLCPP_WARN_THROTTLE(
-            logger_, *(clock_), 2000,
-            "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out.");
-        }
-      }
-    }
-
-    // If in lethal space or just exited lethal space,
-    // we need to update all possible spaces touched during this state
-    if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) {
-      lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_);
-      mg_min_x_u = lethal_state_update_min_x_;
-      lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_);
-      mg_min_y_u = lethal_state_update_min_y_;
-      lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_);
-      mg_max_x_u = lethal_state_update_max_x_;
-      lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_);
-      mg_max_y_u = lethal_state_update_max_y_;
-    } else {
-      // If out of lethal space, reset managed lethal state sizes
-      lethal_state_update_min_x_ = master_grid.getSizeInCellsX();
-      lethal_state_update_min_y_ = master_grid.getSizeInCellsY();
-      lethal_state_update_max_x_ = 0u;
-      lethal_state_update_max_y_ = 0u;
-    }
-  }
+  const unsigned int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
+  const unsigned int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
+  const unsigned int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
+  const unsigned int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
 
   unsigned int i, j;  // master_grid iterators
   unsigned int index;  // corresponding index of master_grid
@@ -367,7 +374,7 @@ void KeepoutFilter::process(
         }
 
         if (data > old_data || old_data == NO_INFORMATION) {
-          if (override_lethal_cost_ && is_pose_lethal) {
+          if (override_lethal_cost_ && is_pose_lethal_) {
             master_array[index] = lethal_override_cost_;
           } else {
             master_array[index] = data;
@@ -377,7 +384,7 @@ void KeepoutFilter::process(
     }
   }
 
-  last_pose_lethal_ = is_pose_lethal;
+  last_pose_lethal_ = is_pose_lethal_;
 }
 
 void KeepoutFilter::resetFilter()

From 1ecf27cdde7d2afae5eb3e14c5049a6a6427a933 Mon Sep 17 00:00:00 2001
From: Dhruv Patel <dhruvpatel2991998@gmail.com>
Date: Thu, 14 Aug 2025 20:39:36 +0200
Subject: [PATCH 25/51] Make MPPI Controller and Critics Apple Clang Friendly
 (#5455)

* fix: adjust C++ standard and compiler flags for Apple Clang

Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com>

* Update: Only mppi_critics target will require C++20

Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com>

* cmake_minimum_required(VERSION 3.8) update

Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com>

---------

Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com>
---
 nav2_amcl/CMakeLists.txt                               |  2 +-
 nav2_behavior_tree/CMakeLists.txt                      |  2 +-
 nav2_behaviors/CMakeLists.txt                          |  2 +-
 nav2_bringup/CMakeLists.txt                            |  2 +-
 nav2_bt_navigator/CMakeLists.txt                       |  2 +-
 nav2_collision_monitor/CMakeLists.txt                  |  2 +-
 nav2_common/CMakeLists.txt                             |  2 +-
 nav2_constrained_smoother/CMakeLists.txt               |  2 +-
 nav2_controller/CMakeLists.txt                         |  2 +-
 nav2_core/CMakeLists.txt                               |  2 +-
 nav2_costmap_2d/CMakeLists.txt                         |  2 +-
 nav2_docking/opennav_docking/CMakeLists.txt            |  2 +-
 nav2_docking/opennav_docking_bt/CMakeLists.txt         |  2 +-
 nav2_docking/opennav_docking_core/CMakeLists.txt       |  2 +-
 nav2_dwb_controller/costmap_queue/CMakeLists.txt       |  2 +-
 nav2_dwb_controller/dwb_core/CMakeLists.txt            |  2 +-
 nav2_dwb_controller/dwb_critics/CMakeLists.txt         |  2 +-
 nav2_dwb_controller/dwb_msgs/CMakeLists.txt            |  2 +-
 nav2_dwb_controller/dwb_plugins/CMakeLists.txt         |  2 +-
 nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt |  2 +-
 nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt         |  2 +-
 nav2_dwb_controller/nav_2d_utils/CMakeLists.txt        |  2 +-
 nav2_graceful_controller/CMakeLists.txt                |  2 +-
 nav2_lifecycle_manager/CMakeLists.txt                  |  2 +-
 nav2_map_server/CMakeLists.txt                         |  2 +-
 nav2_mppi_controller/CMakeLists.txt                    | 10 ++++++++--
 nav2_msgs/CMakeLists.txt                               |  2 +-
 nav2_navfn_planner/CMakeLists.txt                      |  2 +-
 nav2_planner/CMakeLists.txt                            |  2 +-
 nav2_regulated_pure_pursuit_controller/CMakeLists.txt  |  2 +-
 nav2_ros_common/CMakeLists.txt                         |  2 +-
 nav2_rotation_shim_controller/CMakeLists.txt           |  2 +-
 nav2_route/CMakeLists.txt                              |  2 +-
 nav2_rviz_plugins/CMakeLists.txt                       |  2 +-
 nav2_smac_planner/CMakeLists.txt                       |  2 +-
 nav2_smoother/CMakeLists.txt                           |  2 +-
 nav2_system_tests/CMakeLists.txt                       |  2 +-
 nav2_velocity_smoother/CMakeLists.txt                  |  2 +-
 nav2_voxel_grid/CMakeLists.txt                         |  2 +-
 nav2_waypoint_follower/CMakeLists.txt                  |  2 +-
 navigation2/CMakeLists.txt                             |  2 +-
 41 files changed, 48 insertions(+), 42 deletions(-)

diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt
index 4813b3cdd9b..e29f641cb84 100644
--- a/nav2_amcl/CMakeLists.txt
+++ b/nav2_amcl/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_amcl)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index de7d0f6237f..de9cde5f2ab 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_behavior_tree CXX)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt
index bd37a097c2f..292191ea013 100644
--- a/nav2_behaviors/CMakeLists.txt
+++ b/nav2_behaviors/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_behaviors)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt
index b0999ee4eb8..af08ad0265b 100644
--- a/nav2_bringup/CMakeLists.txt
+++ b/nav2_bringup/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_bringup)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
index 83fa44fab7f..156ec4afff6 100644
--- a/nav2_bt_navigator/CMakeLists.txt
+++ b/nav2_bt_navigator/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_bt_navigator)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
index 040e3f51a37..eb91f295d47 100644
--- a/nav2_collision_monitor/CMakeLists.txt
+++ b/nav2_collision_monitor/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_collision_monitor)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt
index f0f1e22c32c..71a771b29c4 100644
--- a/nav2_common/CMakeLists.txt
+++ b/nav2_common/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 
 project(nav2_common NONE)
 
diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt
index c4613f52496..6e179687f94 100644
--- a/nav2_constrained_smoother/CMakeLists.txt
+++ b/nav2_constrained_smoother/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_constrained_smoother)
 
 set(CMAKE_BUILD_TYPE Release) # significant Ceres optimization speedup
diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt
index 93885ef7efc..c8a655f4340 100644
--- a/nav2_controller/CMakeLists.txt
+++ b/nav2_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt
index 586410c3488..1f7ae52aa05 100644
--- a/nav2_core/CMakeLists.txt
+++ b/nav2_core/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_core)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index d7d74038949..00cb720aa15 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_costmap_2d)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt
index a68bd4677ab..8801b18d5a8 100644
--- a/nav2_docking/opennav_docking/CMakeLists.txt
+++ b/nav2_docking/opennav_docking/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(opennav_docking)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt
index 05e67fb713f..2d0306397aa 100644
--- a/nav2_docking/opennav_docking_bt/CMakeLists.txt
+++ b/nav2_docking/opennav_docking_bt/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(opennav_docking_bt)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt
index 057bebb0b6c..e8a4f821884 100644
--- a/nav2_docking/opennav_docking_core/CMakeLists.txt
+++ b/nav2_docking/opennav_docking_core/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(opennav_docking_core)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt
index 68dbfa64ef9..c330e96a9f0 100644
--- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt
+++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(costmap_queue)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt
index 00789a46e3d..f69d546f551 100644
--- a/nav2_dwb_controller/dwb_core/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(dwb_core)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
index ddda0a9087b..be02bcc98f8 100644
--- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(dwb_critics)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
index 574a48c4c6a..9ff83591d2e 100644
--- a/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_msgs/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(dwb_msgs)
 
 find_package(backward_ros REQUIRED)
diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
index 7d4228ae427..b462e7b3aab 100644
--- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(dwb_plugins)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
index 5c1c8fce07a..3fd5af5cc2a 100644
--- a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
+++ b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_dwb_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
index 155c13cdd65..f597712deda 100644
--- a/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav_2d_msgs)
 
 find_package(backward_ros REQUIRED)
diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
index 7aad657a142..5378f2e4b83 100644
--- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
+++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav_2d_utils)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt
index d9720ca6401..95d682a8cad 100644
--- a/nav2_graceful_controller/CMakeLists.txt
+++ b/nav2_graceful_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_graceful_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt
index 64b1f4ad88d..50874482167 100644
--- a/nav2_lifecycle_manager/CMakeLists.txt
+++ b/nav2_lifecycle_manager/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_lifecycle_manager)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
index 55160b2f7de..a003a8fb087 100644
--- a/nav2_map_server/CMakeLists.txt
+++ b/nav2_map_server/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_map_server)
 
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules)
diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index 9dabc27507f..931af234887 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_mppi_controller)
 
 find_package(ament_cmake REQUIRED)
@@ -83,7 +83,13 @@ add_library(mppi_critics SHARED
   src/critics/twirling_critic.cpp
   src/critics/velocity_deadband_critic.cpp
 )
-target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE)
+    # Apple Clang: use C++20 and optimization, omit -fconcepts
+    target_compile_features(mppi_critics PUBLIC cxx_std_20)
+    target_compile_options(mppi_critics PUBLIC -O3)
+else()
+    target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
+endif()
 target_include_directories(mppi_critics
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index f01fbdba6a5..93f7998c56a 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_msgs)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt
index 0da443e5e35..1b50472013f 100644
--- a/nav2_navfn_planner/CMakeLists.txt
+++ b/nav2_navfn_planner/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_navfn_planner)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt
index d3037ddf839..0285fbe9c1a 100644
--- a/nav2_planner/CMakeLists.txt
+++ b/nav2_planner/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_planner)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
index 493418fda67..bec081d3987 100644
--- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
+++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_regulated_pure_pursuit_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_ros_common/CMakeLists.txt b/nav2_ros_common/CMakeLists.txt
index 13dfd418833..4765454321d 100644
--- a/nav2_ros_common/CMakeLists.txt
+++ b/nav2_ros_common/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_ros_common)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt
index 0adcf17d8f1..68b64fdc0a8 100644
--- a/nav2_rotation_shim_controller/CMakeLists.txt
+++ b/nav2_rotation_shim_controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_rotation_shim_controller)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt
index fcc53119651..db9c139c6f1 100644
--- a/nav2_route/CMakeLists.txt
+++ b/nav2_route/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_route CXX)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
index 62e19b2f7a2..b917cbffc33 100644
--- a/nav2_rviz_plugins/CMakeLists.txt
+++ b/nav2_rviz_plugins/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_rviz_plugins)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
index 051818f7c9a..08563b3df41 100644
--- a/nav2_smac_planner/CMakeLists.txt
+++ b/nav2_smac_planner/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_smac_planner)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt
index 5831d10189c..f65d18fa2f9 100644
--- a/nav2_smoother/CMakeLists.txt
+++ b/nav2_smoother/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_smoother)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index 88922fc900f..9817f2f3563 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_system_tests)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_velocity_smoother/CMakeLists.txt b/nav2_velocity_smoother/CMakeLists.txt
index 916ee93ed2c..f7437297d42 100644
--- a/nav2_velocity_smoother/CMakeLists.txt
+++ b/nav2_velocity_smoother/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_velocity_smoother)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_voxel_grid/CMakeLists.txt b/nav2_voxel_grid/CMakeLists.txt
index 093fee48a84..e4d6d1866a8 100644
--- a/nav2_voxel_grid/CMakeLists.txt
+++ b/nav2_voxel_grid/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_voxel_grid)
 
 find_package(ament_cmake REQUIRED)
diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index 17d406adef9..aafd4548a5e 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(nav2_waypoint_follower)
 
 find_package(ament_cmake REQUIRED)
diff --git a/navigation2/CMakeLists.txt b/navigation2/CMakeLists.txt
index 3b82e6c0027..ac8cac17ffa 100644
--- a/navigation2/CMakeLists.txt
+++ b/navigation2/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
 project(navigation2)
 
 find_package(ament_cmake REQUIRED)

From 1856b67b0a3f4efaa1caeeaf46bf546ef8154556 Mon Sep 17 00:00:00 2001
From: Tim Clephas <tim.clephas@nobleo.nl>
Date: Fri, 15 Aug 2025 18:47:03 +0200
Subject: [PATCH 26/51] Fix missing dependency (#5460)

---
 nav2_loopback_sim/package.xml | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml
index 9d55a6d4837..1df96f1a27e 100644
--- a/nav2_loopback_sim/package.xml
+++ b/nav2_loopback_sim/package.xml
@@ -7,12 +7,13 @@
   <maintainer email="steve@opennav.org">steve macenski</maintainer>
   <license>Apache-2.0</license>
 
-  <exec_depend>rclpy</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav2_simple_commander</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>tf_transformations</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
   <exec_depend>python3-transforms3d</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>tf_transformations</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

From 57a639ec9b3a9def3f793918e91f7f40df3d1c7e Mon Sep 17 00:00:00 2001
From: Jad EL HAJJ <jad.elhajj@inmind.ai>
Date: Tue, 19 Aug 2025 20:09:52 +0300
Subject: [PATCH 27/51] Support loading multiple behavior tree files as
 subtrees (#5426)

* Support loading multiple behavior tree files as subtrees

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Fix code style

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Added default param value

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Added recursive check to loadBehaviorTree and adapted unit test accordingly

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Removed nested loadBehaviorTree check in navigators

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Removed whitespace cpplint

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Fixed goalReceived

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Let loadbehaviorTree use its own search_directories var

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* PR fixes-format-lint and test

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* fix pointer

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Added unit test for BT xml validity

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* CPPLint

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Check non existent search directory for bt

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* CPPLint

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Fixed BT tests

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

* Fixed BT tests

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>

---------

Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>
---
 .../behavior_tree_engine.hpp                  |   6 +
 .../nav2_behavior_tree/bt_action_server.hpp   |   5 +-
 .../bt_action_server_impl.hpp                 |  37 ++-
 .../src/behavior_tree_engine.cpp              |   7 +
 nav2_bringup/params/nav2_params.yaml          |   2 +
 .../src/navigators/navigate_through_poses.cpp |   1 -
 .../src/navigators/navigate_to_pose.cpp       |   1 -
 .../nav2_core/behavior_tree_navigator.hpp     |   7 +
 .../behavior_tree/test_behavior_tree_node.cpp | 247 +++++++++++++-----
 9 files changed, 232 insertions(+), 81 deletions(-)

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 96fb5df0dce..b17a914a541 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
@@ -98,6 +98,12 @@ class BehaviorTreeEngine
    */
   void resetGrootMonitor();
 
+  /**
+   * @brief Function to register a BT from an XML file
+   * @param file_path Path to BT XML file
+   */
+  void registerTreeFromFile(const std::string & file_path);
+
   /**
    * @brief Function to explicitly reset all BT nodes to initial state
    * @param tree Tree to halt
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
index d5d1fa4c3a4..8e345abe0d7 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -53,6 +53,7 @@ class BtActionServer
     const std::string & action_name,
     const std::vector<std::string> & plugin_lib_names,
     const std::string & default_bt_xml_filename,
+    const std::vector<std::string> & search_directories,
     OnGoalReceivedCallback on_goal_received_callback,
     OnLoopCallback on_loop_callback,
     OnPreemptCallback on_preempt_callback,
@@ -102,7 +103,8 @@ class BtActionServer
    * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false
    * if something went wrong, and previous BT is maintained
    */
-  bool loadBehaviorTree(const std::string & bt_xml_filename = "");
+  bool loadBehaviorTree(
+    const std::string & bt_xml_filename = "");
 
   /**
    * @brief Getter function for BT Blackboard
@@ -245,6 +247,7 @@ class BtActionServer
   // The XML file that contains the Behavior Tree to create
   std::string current_bt_xml_filename_;
   std::string default_bt_xml_filename_;
+  std::vector<std::string> search_directories_;
 
   // The wrapper class for the BT functionality
   std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index 184f3d94aca..b8fb983f91c 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -39,12 +39,14 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
   const std::string & action_name,
   const std::vector<std::string> & plugin_lib_names,
   const std::string & default_bt_xml_filename,
+  const std::vector<std::string> & search_directories,
   OnGoalReceivedCallback on_goal_received_callback,
   OnLoopCallback on_loop_callback,
   OnPreemptCallback on_preempt_callback,
   OnCompletionCallback on_completion_callback)
 : action_name_(action_name),
   default_bt_xml_filename_(default_bt_xml_filename),
+  search_directories_(search_directories),
   plugin_lib_names_(plugin_lib_names),
   node_(parent),
   on_goal_received_callback_(on_goal_received_callback),
@@ -246,6 +248,8 @@ void BtActionServer<ActionT, NodeT>::setGrootMonitoring(
 template<class ActionT, class NodeT>
 bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml_filename)
 {
+  namespace fs = std::filesystem;
+
   // Empty filename is default for backward compatibility
   auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
 
@@ -255,19 +259,38 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
     return true;
   }
 
-  // if a new tree is created, than the Groot2 Publisher must be destroyed
+  // Reset any existing Groot2 monitoring
   bt_->resetGrootMonitor();
 
-  // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(filename);
-
   if (!xml_file.good()) {
     setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
-      "Couldn't open input XML file: " + filename);
+      "Couldn't open BT XML file: " + filename);
     return false;
   }
 
-  // Create the Behavior Tree from the XML input
+  const auto canonical_main_bt = fs::canonical(filename);
+
+  // Register all XML behavior Subtrees found in the given directories
+  for (const auto & directory : search_directories_) {
+    try {
+      for (const auto & entry : fs::directory_iterator(directory)) {
+        if (entry.path().extension() == ".xml") {
+          // Skip registering the main tree file
+          if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) {
+            continue;
+          }
+          bt_->registerTreeFromFile(entry.path().string());
+        }
+      }
+    } catch (const std::exception & e) {
+      setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+        "Exception reading behavior tree directory: " + std::string(e.what()));
+      return false;
+    }
+  }
+
+  // Try to load the main BT tree
   try {
     tree_ = bt_->createTreeFromFile(filename, blackboard_);
     for (auto & subtree : tree_.subtrees) {
@@ -281,15 +304,15 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
     }
   } catch (const std::exception & e) {
     setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
-      std::string("Exception when loading BT: ") + e.what());
+      std::string("Exception when creating BT tree from file: ") + e.what());
     return false;
   }
 
+  // Optional logging and monitoring
   topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
 
   current_bt_xml_filename_ = filename;
 
-  // Enable monitoring with Groot2
   if (enable_groot_monitoring_) {
     bt_->addGrootMonitoring(&tree_, groot_server_port_);
     RCLCPP_DEBUG(
diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
index 21131be010a..a2b93f7d086 100644
--- a/nav2_behavior_tree/src/behavior_tree_engine.cpp
+++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -102,6 +102,13 @@ BehaviorTreeEngine::createTreeFromFile(
   return factory_.createTreeFromFile(file_path, blackboard);
 }
 
+/// @brief Register a tree from an XML file and return the tree
+void BehaviorTreeEngine::registerTreeFromFile(
+  const std::string & file_path)
+{
+  factory_.registerBehaviorTreeFromFile(file_path);
+}
+
 void
 BehaviorTreeEngine::addGrootMonitoring(
   BT::Tree * tree,
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 23e509760cc..e66d409478f 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -58,6 +58,8 @@ bt_navigator:
       plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
       enable_groot_monitoring: false
       groot_server_port: 1669
+    bt_search_directories:
+      - $(find-pkg-share nav2_bt_navigator)/behavior_trees
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
index 1127c3ab2cd..5b8bbbc5b30 100644
--- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
+++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
@@ -73,7 +73,6 @@ bool
 NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 {
   auto bt_xml_filename = goal->behavior_tree;
-
   if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
     bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       "Error loading XML file: " + bt_xml_filename + ". Navigation canceled.");
diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
index 03d77451e16..a3b64ec2cc2 100644
--- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
+++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
@@ -83,7 +83,6 @@ bool
 NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 {
   auto bt_xml_filename = goal->behavior_tree;
-
   if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
     bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled.");
diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
index fda64cec974..9e984bc6550 100644
--- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
+++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
@@ -201,6 +201,12 @@ class BehaviorTreeNavigator : public NavigatorBase
     // get the default behavior tree for this navigator
     std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node);
 
+    auto search_directories = node->declare_or_get_parameter(
+      "bt_search_directories",
+      std::vector<std::string>{ament_index_cpp::get_package_share_directory(
+      "nav2_bt_navigator") + "/behavior_trees"}
+    );
+
     // Create the Behavior Tree Action Server for this navigator
     bt_action_server_ =
       std::make_unique<nav2_behavior_tree::BtActionServer<ActionT, nav2::LifecycleNode>>(
@@ -208,6 +214,7 @@ class BehaviorTreeNavigator : public NavigatorBase
       getName(),
       plugin_lib_names,
       default_bt_xml_filename,
+      search_directories,
       std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1),
       std::bind(&BehaviorTreeNavigator::onLoop, this),
       std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1),
diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
index d36535924e0..0c23468dc45 100644
--- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
+++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
@@ -71,55 +71,80 @@ class BehaviorTreeHandler
     }
   }
 
-  bool loadBehaviorTree(const std::string & filename)
+  BT::Blackboard::Ptr setBlackboardVariables()
   {
-    // Read the input BT XML from the specified file into a string
-    std::ifstream xml_file(filename);
+     // Create and populate the blackboard
+    blackboard = BT::Blackboard::create();
+    blackboard->set("node", node_);
+    blackboard->set<std::chrono::milliseconds>("server_timeout", std::chrono::milliseconds(20));
+    blackboard->set<std::chrono::milliseconds>("bt_loop_duration", std::chrono::milliseconds(10));
+    blackboard->set<std::chrono::milliseconds>("wait_for_service_timeout",
+             std::chrono::milliseconds(1000));
+    blackboard->set("tf_buffer", tf_);
+    blackboard->set("initial_pose_received", false);
+    blackboard->set("number_recoveries", 0);
+    blackboard->set("odom_smoother", odom_smoother_);
+
+    // Create dummy goal
+    geometry_msgs::msg::PoseStamped goal;
+    goal.header.stamp = node_->now();
+    goal.header.frame_id = "map";
+    blackboard->set("goal", goal);
+    return blackboard;
+  }
 
+  bool behaviorTreeFileValidation(
+    const std::string & filename)
+  {
+    std::ifstream xml_file(filename);
     if (!xml_file.good()) {
-      RCLCPP_ERROR(node_->get_logger(), "Couldn't open input XML file: %s", filename.c_str());
+      RCLCPP_ERROR(node_->get_logger(),
+        "Couldn't open BT XML file: %s", filename.c_str());
       return false;
     }
+    return true;
+  }
 
-    std::stringstream buffer;
-    buffer << xml_file.rdbuf();
-    xml_file.close();
-    std::string xml_string = buffer.str();
-    // Create the blackboard that will be shared by all of the nodes in the tree
-    blackboard = BT::Blackboard::create();
 
-    // Put items on the blackboard
-    blackboard->set("node", node_);  // NOLINT
-    blackboard->set<std::chrono::milliseconds>(
-      "server_timeout", std::chrono::milliseconds(20));  // NOLINT
-    blackboard->set<std::chrono::milliseconds>(
-      "bt_loop_duration", std::chrono::milliseconds(10));  // NOLINT
-    blackboard->set<std::chrono::milliseconds>(
-      "wait_for_service_timeout", std::chrono::milliseconds(1000));  // NOLINT
-    blackboard->set("tf_buffer", tf_);  // NOLINT
-    blackboard->set("initial_pose_received", false);  // NOLINT
-    blackboard->set("number_recoveries", 0);  // NOLINT
-    blackboard->set("odom_smoother", odom_smoother_);  // NOLINT
-
-    // set dummy goal on blackboard
-    geometry_msgs::msg::PoseStamped goal;
-    goal.header.stamp = node_->now();
-    goal.header.frame_id = "map";
-    goal.pose.position.x = 0.0;
-    goal.pose.position.y = 0.0;
-    goal.pose.position.z = 0.0;
-    goal.pose.orientation.x = 0.0;
-    goal.pose.orientation.y = 0.0;
-    goal.pose.orientation.z = 0.0;
-    goal.pose.orientation.w = 1.0;
+  bool loadBehaviorTree(
+    const std::string & filename,
+    const std::vector<std::string> & search_directories)
+  {
+    if (!behaviorTreeFileValidation(filename)) {
+      return false;
+    }
+
+    namespace fs = std::filesystem;
+    const auto canonical_main_bt = fs::canonical(filename);
+
+    // Register all XML behavior Subtrees found in the given directories
+    for (const auto & directory : search_directories) {
+      try {
+        for (const auto & entry : fs::directory_iterator(directory)) {
+          if (entry.path().extension() == ".xml") {
+          // Skip registering the main tree file
+            if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) {
+              continue;
+            }
+            factory_.registerBehaviorTreeFromFile(entry.path().string());
+          }
+        }
+      } catch (const std::exception & e) {
+        RCLCPP_ERROR(node_->get_logger(),
+          "Exception reading behavior tree directory: %s", e.what());
+        return false;
+      }
+    }
 
-    blackboard->set("goal", goal);  // NOLINT
+    // Create and populate the blackboard
+    blackboard = setBlackboardVariables();
 
-    // Create the Behavior Tree from the XML input
+    // Build the tree from the XML string
     try {
-      tree = factory_.createTreeFromText(xml_string, blackboard);
+      tree = factory_.createTreeFromFile(filename, blackboard);
     } catch (BT::RuntimeError & exp) {
-      RCLCPP_ERROR(node_->get_logger(), "%s: %s", filename.c_str(), exp.what());
+      RCLCPP_ERROR(node_->get_logger(), "Failed to create BT from %s: %s", filename.c_str(),
+          exp.what());
       return false;
     }
 
@@ -196,19 +221,75 @@ std::shared_ptr<BehaviorTreeHandler> BehaviorTreeTestFixture::bt_handler = nullp
 
 TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles)
 {
-  std::filesystem::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  root /= "behavior_trees/";
-
-  if (std::filesystem::exists(root) && std::filesystem::is_directory(root)) {
-    for (auto const & entry : std::filesystem::recursive_directory_iterator(root)) {
-      if (std::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") {
-        std::cout << entry.path().string() << std::endl;
-        EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true);
-      }
+  // Get the BT root directory
+  const auto root_dir = std::filesystem::path(
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator")
+    ) / "behavior_trees";
+
+  ASSERT_TRUE(std::filesystem::exists(root_dir));
+  ASSERT_TRUE(std::filesystem::is_directory(root_dir));
+
+  std::vector<std::string> search_directories = {root_dir.string()};
+
+  for (auto const & entry : std::filesystem::recursive_directory_iterator(root_dir)) {
+    if (entry.is_regular_file() && entry.path().extension() == ".xml") {
+      std::string main_bt = entry.path().string();
+      std::cout << "Testing BT file: " << main_bt << std::endl;
+
+      EXPECT_TRUE(bt_handler->loadBehaviorTree(main_bt, search_directories))
+        << "Failed to load: " << main_bt;
     }
   }
 }
 
+TEST_F(BehaviorTreeTestFixture, TestWrongBTFormatXML)
+{
+  auto write_file = [](const std::string & path, const std::string & content) {
+      std::ofstream ofs(path);
+      ofs << content;
+    };
+
+  // File paths
+  std::string valid_subtree = "/tmp/valid_subtree.xml";
+  std::string invalid_subtree = "/tmp/invalid_subtree.xml";
+  std::string main_file = "/tmp/test_main_tree.xml";
+  std::string malformed_main = "/tmp/malformed_main.xml";
+
+  // Valid subtree
+  write_file(valid_subtree,
+    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+    "<root BTCPP_format=\"4\">\n"
+    "    <BehaviorTree ID=\"NoopTree\">\n"
+    "        <AlwaysSuccess />\n"
+    "    </BehaviorTree>\n"
+    "</root>\n");
+
+  // Invalid subtree (malformed XML)
+  write_file(invalid_subtree, "<root><invalid></root>");
+
+  // Main tree referencing the valid subtree
+  write_file(main_file,
+    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+    "<root BTCPP_format=\"4\" main_tree_to_execute=\"MainTree\">\n"
+    "  <BehaviorTree ID=\"MainTree\">\n"
+    "    <Subtree ID=\"NoopTree\"/>\n"
+    "  </BehaviorTree>\n"
+    "</root>\n");
+
+  // Malformed main tree
+  write_file(malformed_main, "<root><invalid></root>");
+
+  std::vector<std::string> search_directories = {"/tmp"};
+
+  EXPECT_FALSE(bt_handler->loadBehaviorTree(main_file, search_directories));
+  EXPECT_FALSE(bt_handler->loadBehaviorTree(malformed_main, search_directories));
+
+  std::remove(valid_subtree.c_str());
+  std::remove(main_file.c_str());
+  std::remove(invalid_subtree.c_str());
+  std::remove(malformed_main.c_str());
+}
+
 /**
  * Test scenario:
  *
@@ -218,10 +299,14 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles)
 TEST_F(BehaviorTreeTestFixture, TestAllSuccess)
 {
   // Load behavior tree from file
-  std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  bt_file /= "behavior_trees/";
-  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
-  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+  const auto root_dir = std::filesystem::path(
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator")
+    ) / "behavior_trees";
+  auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml";
+
+  std::vector<std::string> search_directories = {root_dir.string()};
+
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true);
 
   BT::NodeStatus result = BT::NodeStatus::RUNNING;
 
@@ -265,10 +350,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess)
 TEST_F(BehaviorTreeTestFixture, TestAllFailure)
 {
   // Load behavior tree from file
-  std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  bt_file /= "behavior_trees/";
-  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
-  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+  const auto root_dir = std::filesystem::path(
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator")
+    ) / "behavior_trees";
+  auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml";
+
+  std::vector<std::string> search_directories = {root_dir.string()};
+
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true);
 
   // Set all action server to fail the first 100 times
   Ranges failureRange;
@@ -321,10 +410,14 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure)
 TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries)
 {
   // Load behavior tree from file
-  std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  bt_file /= "behavior_trees/";
-  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
-  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+  const auto root_dir = std::filesystem::path(
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator")
+    ) / "behavior_trees";
+  auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml";
+
+  std::vector<std::string> search_directories = {root_dir.string()};
+
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true);
 
   // Set ComputePathToPose and FollowPath action servers to fail for the first action
   Ranges failureRange;
@@ -380,10 +473,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries)
 TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple)
 {
   // Load behavior tree from file
-  std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  bt_file /= "behavior_trees/";
-  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
-  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+  const auto root_dir = std::filesystem::path(
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator")
+    ) / "behavior_trees";
+  auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml";
+
+  std::vector<std::string> search_directories = {root_dir.string()};
+
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true);
 
   // Set ComputePathToPose action server to fail for the first action
   Ranges plannerFailureRange;
@@ -478,10 +575,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple)
 TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex)
 {
   // Load behavior tree from file
-  std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  bt_file /= "behavior_trees/";
-  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
-  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+  const auto root_dir = std::filesystem::path(
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator")
+    ) / "behavior_trees";
+  auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml";
+
+  std::vector<std::string> search_directories = {root_dir.string()};
+
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true);
 
   // Set FollowPath action server to fail for the first 2 actions
   Ranges controllerFailureRange;
@@ -546,10 +647,14 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex)
 TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated)
 {
   // Load behavior tree from file
-  std::filesystem::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  bt_file /= "behavior_trees/";
-  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
-  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+  const auto root_dir = std::filesystem::path(
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator")
+    ) / "behavior_trees";
+  auto bt_file = root_dir / "navigate_to_pose_w_replanning_and_recovery.xml";
+
+  std::vector<std::string> search_directories = {root_dir.string()};
+
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string(), search_directories), true);
 
   // Set FollowPath action server to fail for the first 2 actions
   Ranges controllerFailureRange;

From 16acda895280e8b1e11db575d964bd717fb35b93 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Tue, 19 Aug 2025 13:45:51 -0700
Subject: [PATCH 28/51] Revert "Unclamp noise velocity. (#5266)" (#5467)

This reverts commit fb25b2fcc4fa41d00754a048753acb3ef6ee52d3.
---
 .../include/nav2_mppi_controller/motion_models.hpp     | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
index c10684b0c79..3115cc4e0a1 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
@@ -89,13 +89,15 @@ class MotionModel
         0).select(state.vx.col(i - 1) + max_delta_vx,
         state.vx.col(i - 1) - min_delta_vx);
 
-      state.vx.col(i) = state.cvx.col(i - 1)
+      state.cvx.col(i - 1) = state.cvx.col(i - 1)
         .cwiseMax(lower_bound_vx)
         .cwiseMin(upper_bound_vx);
+      state.vx.col(i) = state.cvx.col(i - 1);
 
-      state.wz.col(i) = state.cwz.col(i - 1)
+      state.cwz.col(i - 1) = state.cwz.col(i - 1)
         .cwiseMax(state.wz.col(i - 1) - max_delta_wz)
         .cwiseMin(state.wz.col(i - 1) + max_delta_wz);
+      state.wz.col(i) = state.cwz.col(i - 1);
 
       if (is_holo) {
         auto lower_bound_vy = (state.vy.col(i - 1) >
@@ -104,10 +106,10 @@ class MotionModel
         auto upper_bound_vy = (state.vy.col(i - 1) >
           0).select(state.vy.col(i - 1) + max_delta_vy,
           state.vy.col(i - 1) - min_delta_vy);
-
-        state.vy.col(i) = state.cvy.col(i - 1)
+        state.cvy.col(i - 1) = state.cvy.col(i - 1)
           .cwiseMax(lower_bound_vy)
           .cwiseMin(upper_bound_vy);
+        state.vy.col(i) = state.cvy.col(i - 1);
       }
     }
   }

From 8aedf20896997953239ebcc3214f04fcd0ebe9dd Mon Sep 17 00:00:00 2001
From: DavidG-Develop <147402604+DavidG-Develop@users.noreply.github.com>
Date: Thu, 21 Aug 2025 17:58:25 +0200
Subject: [PATCH 29/51] fix naming for
 nav2_path_expiring_timer_condition_bt_node (#5471)

Signed-off-by: David G <david.randommail1@gmail.com>
---
 nav2_behavior_tree/CMakeLists.txt                        | 4 ++--
 nav2_behavior_tree/test/plugins/condition/CMakeLists.txt | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index de9cde5f2ab..2004b5c62b3 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -123,8 +123,8 @@ list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node)
 add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
 list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
 
-add_library(nav2_path_expiring_timer_condition SHARED plugins/condition/path_expiring_timer_condition.cpp)
-list(APPEND plugin_libs nav2_path_expiring_timer_condition)
+add_library(nav2_path_expiring_timer_condition_bt_node SHARED plugins/condition/path_expiring_timer_condition.cpp)
+list(APPEND plugin_libs nav2_path_expiring_timer_condition_bt_node)
 
 add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp)
 list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
index d7d32012733..51f7a908801 100644
--- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
+++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
@@ -2,7 +2,7 @@ plugin_add_test(test_condition_distance_traveled test_distance_traveled.cpp nav2
 
 plugin_add_test(test_condition_time_expired test_time_expired.cpp nav2_time_expired_condition_bt_node)
 
-plugin_add_test(test_condition_path_expiring_timer test_path_expiring_timer.cpp nav2_path_expiring_timer_condition)
+plugin_add_test(test_condition_path_expiring_timer test_path_expiring_timer.cpp nav2_path_expiring_timer_condition_bt_node)
 
 plugin_add_test(test_condition_goal_reached test_goal_reached.cpp nav2_goal_reached_condition_bt_node)
 

From 69a60dfacb80eb3c9c2ad47f7590eea8defbdaec Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Thu, 21 Aug 2025 12:20:37 -0700
Subject: [PATCH 30/51] fix: Move SmootherParams declaration outside
 smooth_path conditional (#5473)

Fixes crash when dynamically changing smooth_path parameter from false to true.
The issue occurred because SmootherParams were only declared when smooth_path
was initially true, causing ParameterModifiedInCallbackException when trying
to declare parameters within the dynamic parameter callback.

Now SmootherParams are always declared, making them available for dynamic
reconfiguration regardless of the initial smooth_path value.

Fixes #5472

Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com>
---
 nav2_smac_planner/src/smac_planner_hybrid.cpp  | 4 ++--
 nav2_smac_planner/src/smac_planner_lattice.cpp | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
index 1f374af35d0..07a30839374 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -273,9 +273,9 @@ void SmacPlannerHybrid::configure(
     _angle_quantizations);
 
   // Initialize path smoother
+  SmootherParams params;
+  params.get(node, name);
   if (smooth_path) {
-    SmootherParams params;
-    params.get(node, name);
     _smoother = std::make_unique<Smoother>(params);
     _smoother->initialize(_minimum_turning_radius_global_coords);
   }
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index fa89e90636c..c6e110cbfd4 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -230,9 +230,9 @@ void SmacPlannerLattice::configure(
     _metadata.number_of_headings);
 
   // Initialize path smoother
+  SmootherParams params;
+  params.get(node, name);
   if (smooth_path) {
-    SmootherParams params;
-    params.get(node, name);
     _smoother = std::make_unique<Smoother>(params);
     _smoother->initialize(_metadata.min_turning_radius);
   }

From 7941300c916bff276c17abc1201fdcc09c553856 Mon Sep 17 00:00:00 2001
From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com>
Date: Tue, 26 Aug 2025 12:18:41 -0700
Subject: [PATCH 31/51] Return early from edge interpolation for zero-length
 edges (#5453)

* Return early from edge interpolation for zero-length edges

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>

* Move the check and add a test

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>

---------

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>
---
 nav2_route/src/path_converter.cpp       |  5 +++++
 nav2_route/test/test_path_converter.cpp | 12 ++++++++++++
 2 files changed, 17 insertions(+)

diff --git a/nav2_route/src/path_converter.cpp b/nav2_route/src/path_converter.cpp
index ff48773dcea..46776c5f0e9 100644
--- a/nav2_route/src/path_converter.cpp
+++ b/nav2_route/src/path_converter.cpp
@@ -141,6 +141,11 @@ void PathConverter::interpolateEdge(
   // Find number of points to populate by given density
   const float mag = hypotf(x1 - x0, y1 - y0);
   const unsigned int num_pts = ceil(mag / density_);
+  // For zero-length edges, we can just push the start point and return
+  if (num_pts < 1) {
+    return;
+  }
+
   const float iterpolated_dist = mag / num_pts;
 
   // Find unit vector direction
diff --git a/nav2_route/test/test_path_converter.cpp b/nav2_route/test/test_path_converter.cpp
index 91c8ec749a1..d8af7736cc8 100644
--- a/nav2_route/test/test_path_converter.cpp
+++ b/nav2_route/test/test_path_converter.cpp
@@ -163,3 +163,15 @@ TEST(PathConverterTest, test_path_converter_interpolation)
         poses[i].pose.position.y - poses[i + 1].pose.position.y), 0.05);
   }
 }
+
+TEST(PathConverterTest, test_path_converter_zero_length_edge)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("edge_scorer_test");
+  PathConverter converter;
+  converter.configure(node);
+
+  float x0 = 10.0, y0 = 10.0, x1 = 10.0, y1 = 10.0;
+  std::vector<geometry_msgs::msg::PoseStamped> poses;
+  converter.interpolateEdge(x0, y0, x1, y1, poses);
+  ASSERT_TRUE(poses.empty());
+}

From 0acacb031f53ff1a9b512766a2d464c1c2793e83 Mon Sep 17 00:00:00 2001
From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com>
Date: Tue, 26 Aug 2025 12:23:43 -0700
Subject: [PATCH 32/51] nav2_route vizualization marker rendering performance
 improvement (#5452)

* nav2_route vizualization marker use sphere_list and line_list for rendering performance

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>

* Fix unit test and break out magic constants into named values

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>

---------

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>
---
 nav2_route/include/nav2_route/utils.hpp  | 158 +++++++++++------------
 nav2_route/test/test_utils_and_types.cpp |  10 +-
 2 files changed, 82 insertions(+), 86 deletions(-)

diff --git a/nav2_route/include/nav2_route/utils.hpp b/nav2_route/include/nav2_route/utils.hpp
index 9ccf65eb41e..04deda14bec 100644
--- a/nav2_route/include/nav2_route/utils.hpp
+++ b/nav2_route/include/nav2_route/utils.hpp
@@ -59,102 +59,90 @@ inline visualization_msgs::msg::MarkerArray toMsg(
   const nav2_route::Graph & graph, const std::string & frame, const rclcpp::Time & now)
 {
   visualization_msgs::msg::MarkerArray msg;
-  visualization_msgs::msg::Marker curr_marker;
-  curr_marker.header.frame_id = frame;
-  curr_marker.header.stamp = now;
-  curr_marker.action = 0;
 
-  auto getSphereSize = []() {
-      geometry_msgs::msg::Vector3 v_msg;
-      v_msg.x = 0.05;
-      v_msg.y = 0.05;
-      v_msg.z = 0.05;
-      return v_msg;
-    };
-
-  auto getSphereColor = []() {
-      std_msgs::msg::ColorRGBA c_msg;
-      c_msg.r = 1.0;
-      c_msg.g = 0.0;
-      c_msg.b = 0.0;
-      c_msg.a = 1.0;
-      return c_msg;
-    };
-
-  auto getLineColor = []() {
-      std_msgs::msg::ColorRGBA c_msg;
-      c_msg.r = 0.0;
-      c_msg.g = 1.0;
-      c_msg.b = 0.0;
-      c_msg.a = 0.5;  // So bi-directional connections stand out overlapping
-      return c_msg;
-    };
-
-  unsigned int marker_idx = 1;
-  for (unsigned int i = 0; i != graph.size(); i++) {
-    if (graph[i].nodeid == std::numeric_limits<int>::max()) {
-      continue;  // Skip "deleted" nodes
-    }
-    curr_marker.ns = "route_graph";
-    curr_marker.id = marker_idx++;
-    curr_marker.type = visualization_msgs::msg::Marker::SPHERE;
-    curr_marker.pose.position.x = graph[i].coords.x;
-    curr_marker.pose.position.y = graph[i].coords.y;
-    curr_marker.scale = getSphereSize();
-    curr_marker.color = getSphereColor();
-    msg.markers.push_back(curr_marker);
-
-    // Add text
-    curr_marker.ns = "route_graph_ids";
-    curr_marker.id = marker_idx++;
-    curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-    curr_marker.pose.position.x = graph[i].coords.x + 0.07;
-    curr_marker.pose.position.y = graph[i].coords.y;
-    curr_marker.text = std::to_string(graph[i].nodeid);
-    curr_marker.scale.z = 0.1;
-    msg.markers.push_back(curr_marker);
-
-    for (unsigned int j = 0; j != graph[i].neighbors.size(); j++) {
-      curr_marker.ns = "route_graph";
-      curr_marker.id = marker_idx++;
-      curr_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
-      curr_marker.pose.position.x = 0;  // Set to 0 since points are relative to this frame
-      curr_marker.pose.position.y = 0;  // Set to 0 since points are relative to this frame
-      curr_marker.points.resize(2);
-      curr_marker.points[0].x = graph[i].coords.x;
-      curr_marker.points[0].y = graph[i].coords.y;
-      curr_marker.points[1].x = graph[i].neighbors[j].end->coords.x;
-      curr_marker.points[1].y = graph[i].neighbors[j].end->coords.y;
-      curr_marker.scale.x = 0.03;
-      curr_marker.color = getLineColor();
-      msg.markers.push_back(curr_marker);
-      curr_marker.points.clear();  // Reset for next node marker
-
-      // Add text
-      curr_marker.ns = "route_graph_ids";
-      curr_marker.id = marker_idx++;
-      curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-      curr_marker.pose.position.x =
-        graph[i].coords.x + ((graph[i].neighbors[j].end->coords.x - graph[i].coords.x) / 2.0) +
-        0.07;
+  visualization_msgs::msg::Marker nodes_marker;
+  nodes_marker.header.frame_id = frame;
+  nodes_marker.header.stamp = now;
+  nodes_marker.action = 0;
+  nodes_marker.ns = "route_graph_nodes";
+  nodes_marker.type = visualization_msgs::msg::Marker::POINTS;
+  nodes_marker.scale.x = 0.5;
+  nodes_marker.scale.y = 0.5;
+  nodes_marker.scale.z = 0.5;
+  nodes_marker.color.r = 1.0;
+  nodes_marker.color.a = 1.0;
+  nodes_marker.points.reserve(graph.size());
+
+  visualization_msgs::msg::Marker edges_marker;
+  edges_marker.header.frame_id = frame;
+  edges_marker.header.stamp = now;
+  edges_marker.action = 0;
+  edges_marker.ns = "route_graph_edges";
+  edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+  edges_marker.scale.x = 0.1;  // Line width
+  edges_marker.color.g = 1.0;
+  edges_marker.color.a = 0.5;  // Semi-transparent green so bidirectional connections stand out
+  constexpr size_t points_per_edge = 2;
+  // This probably under-reserves but saves some initial reallocations
+  constexpr size_t likely_min_edges_per_node = 2;
+  edges_marker.points.reserve(graph.size() * points_per_edge * likely_min_edges_per_node);
+
+  geometry_msgs::msg::Point node_pos;
+  geometry_msgs::msg::Point edge_start;
+  geometry_msgs::msg::Point edge_end;
+
+  visualization_msgs::msg::Marker node_id_marker;
+  node_id_marker.ns = "route_graph_node_ids";
+  node_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  node_id_marker.scale.z = 0.1;
+
+  visualization_msgs::msg::Marker edge_id_marker;
+  edge_id_marker.ns = "route_graph_edge_ids";
+  edge_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  edge_id_marker.scale.z = 0.1;
+
+  for (const auto & node : graph) {
+    node_pos.x = node.coords.x;
+    node_pos.y = node.coords.y;
+    nodes_marker.points.push_back(node_pos);
+
+    // Add text for Node ID
+    node_id_marker.id++;
+    node_id_marker.pose.position.x = node.coords.x + 0.07;
+    node_id_marker.pose.position.y = node.coords.y;
+    node_id_marker.text = std::to_string(node.nodeid);
+    msg.markers.push_back(node_id_marker);
+
+    for (const auto & neighbor : node.neighbors) {
+      edge_start.x = node.coords.x;
+      edge_start.y = node.coords.y;
+      edge_end.x = neighbor.end->coords.x;
+      edge_end.y = neighbor.end->coords.y;
+      edges_marker.points.push_back(edge_start);
+      edges_marker.points.push_back(edge_end);
 
       // Deal with overlapping bi-directional text markers by offsetting locations
       float y_offset = 0.0;
-      if (graph[i].nodeid > graph[i].neighbors[j].end->nodeid) {
+      if (node.nodeid > neighbor.end->nodeid) {
         y_offset = 0.05;
       } else {
         y_offset = -0.05;
       }
-
-      curr_marker.pose.position.y =
-        graph[i].coords.y + ((graph[i].neighbors[j].end->coords.y - graph[i].coords.y) / 2.0) +
-        y_offset;
-      curr_marker.text = std::to_string(graph[i].neighbors[j].edgeid);
-      curr_marker.scale.z = 0.1;
-      msg.markers.push_back(curr_marker);
+      const float x_offset = 0.07;
+
+      // Add text for Edge ID
+      edge_id_marker.id++;
+      edge_id_marker.pose.position.x =
+        node.coords.x + ((neighbor.end->coords.x - node.coords.x) / 2.0) + x_offset;
+      edge_id_marker.pose.position.y =
+        node.coords.y + ((neighbor.end->coords.y - node.coords.y) / 2.0) + y_offset;
+      edge_id_marker.text = std::to_string(neighbor.edgeid);
+      msg.markers.push_back(edge_id_marker);
     }
   }
 
+  msg.markers.push_back(nodes_marker);
+  msg.markers.push_back(edges_marker);
   return msg;
 }
 
diff --git a/nav2_route/test/test_utils_and_types.cpp b/nav2_route/test/test_utils_and_types.cpp
index a6b9a7e37e9..25703f95ff3 100644
--- a/nav2_route/test/test_utils_and_types.cpp
+++ b/nav2_route/test/test_utils_and_types.cpp
@@ -159,7 +159,15 @@ TEST(UtilsTest, test_to_visualization_msg_conversion)
   graph[3].addEdge(default_cost, &graph[6], ids++);
 
   auto graph_msg = utils::toMsg(graph, frame, time);
-  EXPECT_EQ(graph_msg.markers.size(), 34u);  // 9 nodes and 8 edges (with text markers)
+  constexpr size_t expected_edge_markers = 1;
+  constexpr size_t expected_node_markers = 1;
+  constexpr size_t expected_edge_id_text_markers = 8;
+  constexpr size_t expected_node_id_text_markers = 9;
+  constexpr size_t expected_total_markers =
+    expected_edge_markers + expected_node_markers +
+    expected_edge_id_text_markers + expected_node_id_text_markers;
+
+  EXPECT_EQ(graph_msg.markers.size(), expected_total_markers);
   for (auto & marker : graph_msg.markers) {
     if (marker.ns == "route_graph_ids") {
       EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::TEXT_VIEW_FACING);

From f01ee785441f73e0a713bf35e704e0eefacedc53 Mon Sep 17 00:00:00 2001
From: Adi Vardi <57910756+adivardi@users.noreply.github.com>
Date: Tue, 26 Aug 2025 21:42:30 +0200
Subject: [PATCH 33/51] make Blackboard variable ID private to the navigators
 (#5466)

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
---
 .../src/navigators/navigate_through_poses.cpp             | 6 +++---
 nav2_bt_navigator/src/navigators/navigate_to_pose.cpp     | 8 ++++----
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
index 5b8bbbc5b30..9b8435ac4a3 100644
--- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
+++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
@@ -31,11 +31,11 @@ NavigateThroughPosesNavigator::configure(
   auto node = parent_node.lock();
 
   goals_blackboard_id_ =
-    node->declare_or_get_parameter("goals_blackboard_id", std::string("goals"));
+    node->declare_or_get_parameter(getName() + ".goals_blackboard_id", std::string("goals"));
   path_blackboard_id_ =
-    node->declare_or_get_parameter("path_blackboard_id", std::string("path"));
+    node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path"));
   waypoint_statuses_blackboard_id_ =
-    node->declare_or_get_parameter("waypoint_statuses_blackboard_id",
+    node->declare_or_get_parameter(getName() + ".waypoint_statuses_blackboard_id",
       std::string("waypoint_statuses"));
 
   // Odometry smoother object for getting current speed
diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
index a3b64ec2cc2..a9495645858 100644
--- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
+++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
@@ -29,10 +29,10 @@ NavigateToPoseNavigator::configure(
   start_time_ = rclcpp::Time(0);
   auto node = parent_node.lock();
 
-  goal_blackboard_id_ =
-    node->declare_or_get_parameter("goal_blackboard_id", std::string("goal"));
-  path_blackboard_id_ =
-    node->declare_or_get_parameter("path_blackboard_id", std::string("path"));
+  goal_blackboard_id_ = node->declare_or_get_parameter(getName() + ".goal_blackboard_id",
+      std::string("goal"));
+  path_blackboard_id_ = node->declare_or_get_parameter(getName() + ".path_blackboard_id",
+      std::string("path"));
 
   // Odometry smoother object for getting current speed
   odom_smoother_ = odom_smoother;

From 76039388943b5fc35b5b6199cfdd002bcb796e9d Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Tue, 26 Aug 2025 22:03:17 +0200
Subject: [PATCH 34/51] Fix dynamic param SmacPlannerLattice  (#5478)

* Fix SmacPlannerLattice dynamic parameter early exit

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove comment

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
---
 nav2_smac_planner/src/node_lattice.cpp | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp
index d9a7c8ebfa5..7922d353d19 100644
--- a/nav2_smac_planner/src/node_lattice.cpp
+++ b/nav2_smac_planner/src/node_lattice.cpp
@@ -50,23 +50,21 @@ void LatticeMotionTable::initMotionModel(
   unsigned int & size_x_in,
   SearchInfo & search_info)
 {
-  size_x = size_x_in;
-
-  if (current_lattice_filepath == search_info.lattice_filepath) {
-    return;
-  }
-
   size_x = size_x_in;
   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;
   travel_distance_reward = 1.0f - search_info.retrospective_penalty;
-  current_lattice_filepath = search_info.lattice_filepath;
   allow_reverse_expansion = search_info.allow_reverse_expansion;
   rotation_penalty = search_info.rotation_penalty;
   min_turning_radius = search_info.minimum_turning_radius;
 
+  if (current_lattice_filepath == search_info.lattice_filepath) {
+    return;
+  }
+  current_lattice_filepath = search_info.lattice_filepath;
+
   // Get the metadata about this minimum control set
   lattice_metadata = getLatticeMetadata(current_lattice_filepath);
   std::ifstream latticeFile(current_lattice_filepath);

From ff80727eee2bc5f3d70a849d9e821bc01eee757c Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Thu, 28 Aug 2025 02:47:03 +0800
Subject: [PATCH 35/51] Replace last pose if only orientation differs in Navfn
 (#5490)

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 nav2_navfn_planner/src/navfn_planner.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index fab5d3f0fcf..dd5ac3f3e5f 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -353,11 +353,11 @@ NavfnPlanner::smoothApproachToGoal(
   const geometry_msgs::msg::Pose & goal,
   nav_msgs::msg::Path & plan)
 {
-  // Replace the last pose of the computed path if it's actually further away
-  // to the second to last pose than the goal pose.
   if (plan.poses.size() >= 2) {
     auto second_to_last_pose = plan.poses.end()[-2];
     auto last_pose = plan.poses.back();
+    // Replace the last pose of the computed path if it's actually further away
+    // to the second to last pose than the goal pose.
     if (
       squared_distance(last_pose.pose, second_to_last_pose.pose) >
       squared_distance(goal, second_to_last_pose.pose))
@@ -365,6 +365,11 @@ NavfnPlanner::smoothApproachToGoal(
       plan.poses.back().pose = goal;
       return;
     }
+    // Replace the last pose of the computed path if its position matches but orientation differs
+    if (squared_distance(last_pose.pose, goal) < 1e-6) {
+      plan.poses.back().pose = goal;
+      return;
+    }
   }
   geometry_msgs::msg::PoseStamped goal_copy;
   goal_copy.pose = goal;

From c057c954a343af2a337b61d5da464c53e9fc9f58 Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Wed, 27 Aug 2025 23:05:41 +0200
Subject: [PATCH 36/51] Fix duplicate poses with computePlanThroughPoses
 (#5488)

* fix-duplicate-poses

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Update nav2_planner/src/planner_server.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
---
 nav2_planner/src/planner_server.cpp | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 81752e25644..998220d6533 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -428,9 +428,17 @@ void PlannerServer::computePlanThroughPoses()
         throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path");
       }
 
-      // Concatenate paths together
-      concat_path.poses.insert(
-        concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end());
+      // Concatenate paths together, but skip the first pose of subsequent paths
+      // to avoid duplicating the connection point
+      if (i == 0) {
+        // First path: add all poses
+        concat_path.poses.insert(
+          concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end());
+      } else if (curr_path.poses.size() > 1) {
+        // Subsequent paths: skip the first pose to avoid duplication
+        concat_path.poses.insert(
+          concat_path.poses.end(), curr_path.poses.begin() + 1, curr_path.poses.end());
+      }
       concat_path.header = curr_path.header;
     }
 

From 2b8cc4d140ce12469f0828d9647002c88e831e7e Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Thu, 28 Aug 2025 05:08:17 +0800
Subject: [PATCH 37/51] Revert double spin (#5477)

* Revert "Add double spin_some in some BT nodes (#5055)"

This reverts commit 4e8469e5c9b33c8cf987ee8e83e70aefcbb34eba.

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Update spin some to use spin all

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Replace 50 ms with bt loop duration

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 .../plugins/action/controller_selector_node.hpp            | 2 ++
 .../plugins/action/goal_checker_selector_node.hpp          | 2 ++
 .../plugins/action/planner_selector_node.hpp               | 2 ++
 .../plugins/action/progress_checker_selector_node.hpp      | 2 ++
 .../plugins/action/smoother_selector_node.hpp              | 2 ++
 .../plugins/condition/is_battery_charging_condition.hpp    | 2 ++
 .../plugins/condition/is_battery_low_condition.hpp         | 2 ++
 .../plugins/decorator/goal_updater_node.hpp                | 2 ++
 .../plugins/action/controller_selector_node.cpp            | 7 +++----
 .../plugins/action/goal_checker_selector_node.cpp          | 4 +++-
 .../plugins/action/planner_selector_node.cpp               | 7 +++----
 .../plugins/action/progress_checker_selector_node.cpp      | 4 +++-
 .../plugins/action/smoother_selector_node.cpp              | 7 +++----
 .../plugins/condition/is_battery_charging_condition.cpp    | 7 +++----
 .../plugins/condition/is_battery_low_condition.cpp         | 7 +++----
 nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp | 7 +++----
 .../test/plugins/action/test_controller_selector_node.cpp  | 3 +++
 .../plugins/action/test_goal_checker_selector_node.cpp     | 3 +++
 .../test/plugins/action/test_planner_selector_node.cpp     | 3 +++
 .../plugins/action/test_progress_checker_selector_node.cpp | 3 +++
 .../test/plugins/action/test_smoother_selector_node.cpp    | 3 +++
 .../test/plugins/condition/test_is_battery_charging.cpp    | 3 +++
 .../test/plugins/condition/test_is_battery_low.cpp         | 3 +++
 .../test/plugins/decorator/test_goal_updater_node.cpp      | 3 +++
 nav2_route/test/test_graph_loader.cpp                      | 1 -
 nav2_route/test/test_graph_saver.cpp                       | 1 -
 26 files changed, 64 insertions(+), 28 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
index ebc29fa9721..cd52705f197 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -102,6 +103,7 @@ class ControllerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
index 820a1a7f71f..0b9d9f6b43c 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -102,6 +103,7 @@ class GoalCheckerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
index 07026ac64f9..269ad96b13b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -103,6 +104,7 @@ class PlannerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
index c58f615dbf7..3ff8e397313 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -101,6 +102,7 @@ class ProgressCheckerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
index f752fafb913..6d438262a58 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -102,6 +103,7 @@ class SmootherSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
index 2c95e1e880e..d456d8092c1 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
@@ -18,6 +18,7 @@
 #include <string>
 #include <memory>
 #include <mutex>
+#include <chrono>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
@@ -85,6 +86,7 @@ class IsBatteryChargingCondition : public BT::ConditionNode
   nav2::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
   std::string battery_topic_;
   bool is_battery_charging_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // 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
index d66eb5eea72..de31aa1c4e0 100644
--- 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
@@ -19,6 +19,7 @@
 #include <string>
 #include <memory>
 #include <mutex>
+#include <chrono>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
@@ -93,6 +94,7 @@ class IsBatteryLowCondition : public BT::ConditionNode
   double min_battery_;
   bool is_voltage_;
   bool is_battery_low_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp
index 2c6d69895e6..61a909c53c6 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp
@@ -19,6 +19,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "behaviortree_cpp/decorator_node.h"
 #include "behaviortree_cpp/json_export.h"
@@ -110,6 +111,7 @@ class GoalUpdater : public BT::DecoratorNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
   std::string goal_updater_topic_;
   std::string goals_updater_topic_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp
index 860f1ed12de..eee98bd7861 100644
--- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp
@@ -33,9 +33,8 @@ ControllerSelector::ControllerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
-
-  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
-  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void ControllerSelector::initialize()
@@ -69,7 +68,7 @@ BT::NodeStatus ControllerSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected controller received from the topic input.
   // When no input is specified it uses the default controller.
diff --git a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp
index 20df1bcca9c..0b1fbae15c8 100644
--- a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp
@@ -33,6 +33,8 @@ GoalCheckerSelector::GoalCheckerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void GoalCheckerSelector::initialize()
@@ -66,7 +68,7 @@ BT::NodeStatus GoalCheckerSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected goal checker received from the topic input.
   // When no input is specified it uses the default goal checker.
diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp
index 6dd1806d67a..8dc248d032e 100644
--- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp
@@ -33,9 +33,8 @@ PlannerSelector::PlannerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
-
-  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
-  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void PlannerSelector::initialize()
@@ -69,7 +68,7 @@ BT::NodeStatus PlannerSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected planner received from the topic input.
   // When no input is specified it uses the default planner.
diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp
index 71d0b308e2a..458c0253e41 100644
--- a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp
@@ -32,6 +32,8 @@ ProgressCheckerSelector::ProgressCheckerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void ProgressCheckerSelector::initialize()
@@ -65,7 +67,7 @@ BT::NodeStatus ProgressCheckerSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected progress checker received from the topic input.
   // When no input is specified it uses the default goaprogressl checker.
diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
index 1e7768e7020..7c17a97561f 100644
--- a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
+++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
@@ -34,9 +34,8 @@ SmootherSelector::SmootherSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
-
-  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
-  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void SmootherSelector::initialize()
@@ -70,7 +69,7 @@ BT::NodeStatus SmootherSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected smoother received from the topic input.
   // When no input is specified it uses the default smoother.
diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
index 571cf10d64f..dab7e019dd0 100644
--- a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
@@ -27,9 +27,8 @@ IsBatteryChargingCondition::IsBatteryChargingCondition(
   is_battery_charging_(false)
 {
   initialize();
-
-  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
-  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void IsBatteryChargingCondition::initialize()
@@ -65,7 +64,7 @@ BT::NodeStatus IsBatteryChargingCondition::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
   if (is_battery_charging_) {
     return BT::NodeStatus::SUCCESS;
   }
diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
index 628b31145b4..57898da658b 100644
--- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
+++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
@@ -30,9 +30,8 @@ IsBatteryLowCondition::IsBatteryLowCondition(
   is_battery_low_(false)
 {
   initialize();
-
-  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
-  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void IsBatteryLowCondition::initialize()
@@ -71,7 +70,7 @@ BT::NodeStatus IsBatteryLowCondition::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
   if (is_battery_low_) {
     return BT::NodeStatus::SUCCESS;
   }
diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
index 2cf6031ce19..d5b67155d30 100644
--- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
+++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
@@ -36,9 +36,8 @@ GoalUpdater::GoalUpdater(
   goals_updater_topic_("goals_update")
 {
   initialize();
-
-    // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
-  callback_group_executor_.spin_all(std::chrono::milliseconds(1));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void GoalUpdater::initialize()
@@ -91,7 +90,7 @@ inline BT::NodeStatus GoalUpdater::tick()
   getInput("input_goal", goal);
   getInput("input_goals", goals);
 
-  callback_group_executor_.spin_all(std::chrono::milliseconds(49));
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   if (last_goal_received_set_) {
     if (last_goal_received_.header.stamp == rclcpp::Time(0)) {
diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp
index ec1abd18e59..9f686ad2ed4 100644
--- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp
@@ -44,6 +44,9 @@ class ControllerSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::ControllerSelector>(name, config);
diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp
index fb4a40c0ffc..0067a5bd7ed 100644
--- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp
@@ -44,6 +44,9 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::GoalCheckerSelector>(name, config);
diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp
index 3308abe5efa..7043d193789 100644
--- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp
@@ -44,6 +44,9 @@ class PlannerSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::PlannerSelector>(name, config);
diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp
index c7c70e742e9..eebb6c193b4 100644
--- a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp
@@ -43,6 +43,9 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::ProgressCheckerSelector>(name, config);
diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp
index 002610472f4..50238346741 100644
--- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp
@@ -44,6 +44,9 @@ class SmootherSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::SmootherSelector>(name, config);
diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp
index 18aaf99f8ec..fb27568c275 100644
--- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp
+++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp
@@ -39,6 +39,9 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test
     config_->blackboard->set(
       "node",
       node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     factory_->registerNodeType<nav2_behavior_tree::IsBatteryChargingCondition>("IsBatteryCharging");
 
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
index 0af03b9afaa..a5080593117 100644
--- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp
+++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp
@@ -40,6 +40,9 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test
     config_->blackboard->set(
       "node",
       node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     factory_->registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>("IsBatteryLow");
 
diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp
index 1766dfbd418..33cfeffc5e7 100644
--- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp
+++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp
@@ -43,6 +43,9 @@ class GoalUpdaterTestFixture : public ::testing::Test
     config_->blackboard->set(
       "node",
       node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder =
       [](const std::string & name, const BT::NodeConfiguration & config)
diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp
index e76fe863314..84b544a1be5 100644
--- a/nav2_route/test/test_graph_loader.cpp
+++ b/nav2_route/test/test_graph_loader.cpp
@@ -107,7 +107,6 @@ TEST(GraphLoader, test_transformation_api)
   tf_broadcaster->sendTransform(transform);
   rclcpp::Rate(1).sleep();
   tf_broadcaster->sendTransform(transform);
-  rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1));
   rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50));
 
   graph[0].coords.frame_id = "map_test";
diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp
index 4abdc0b74d3..bd69a9bc112 100644
--- a/nav2_route/test/test_graph_saver.cpp
+++ b/nav2_route/test/test_graph_saver.cpp
@@ -143,7 +143,6 @@ TEST(GraphSaver, test_transformation_api)
   tf_broadcaster->sendTransform(transform);
   rclcpp::Rate(1).sleep();
   tf_broadcaster->sendTransform(transform);
-  rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1));
   rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50));
 
   GraphSaver graph_saver(node, tf, frame);

From 3c081db0cf7508451cf10651be79b4566729a40d Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Fri, 29 Aug 2025 01:27:34 +0800
Subject: [PATCH 38/51] Add custom window size and poly order for SG filter
 (#5489)

* Add custom window size and poly order in SG filter

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Fix linting and benchmark tool

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Cmakelist

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Adding comments

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Add blog

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
---
 nav2_smoother/CMakeLists.txt                  |  3 +
 .../nav2_smoother/savitzky_golay_smoother.hpp |  9 +-
 nav2_smoother/package.xml                     |  2 +
 nav2_smoother/src/savitzky_golay_smoother.cpp | 95 ++++++++++---------
 .../smoother_benchmark_bringup.py             |  9 ++
 5 files changed, 72 insertions(+), 46 deletions(-)

diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt
index f65d18fa2f9..a48a7166d45 100644
--- a/nav2_smoother/CMakeLists.txt
+++ b/nav2_smoother/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_ros_common REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 nav2_package()
 
@@ -42,6 +43,7 @@ target_link_libraries(${library_name} PUBLIC
   rclcpp_lifecycle::rclcpp_lifecycle
   tf2_ros::tf2_ros
   nav2_ros_common::nav2_ros_common
+  Eigen3::Eigen
 )
 target_link_libraries(${library_name} PRIVATE
   rclcpp_components::component
@@ -157,6 +159,7 @@ ament_export_dependencies(
   rclcpp_lifecycle
   tf2
   tf2_ros
+  Eigen3
 )
 ament_export_targets(${library_name})
 ament_package()
diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp
index e18c0b823e5..1f56e8440e5 100644
--- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp
+++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp
@@ -15,6 +15,7 @@
 #ifndef NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_
 #define NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_
 
+#include <Eigen/Dense>
 #include <cmath>
 #include <vector>
 #include <string>
@@ -85,6 +86,11 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother
     nav_msgs::msg::Path & path,
     const rclcpp::Duration & max_time) override;
 
+  /**
+   * @brief Method to calculate SavitzkyGolay Coefficients
+   */
+  void calculateCoefficients();
+
 protected:
   /**
    * @brief Smoother method - does the smoothing on a segment
@@ -99,7 +105,8 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother
     bool & reversing_segment);
 
   bool do_refinement_, enforce_path_inversion_;
-  int refinement_num_;
+  int refinement_num_, window_size_, half_window_size_, poly_order_;
+  Eigen::VectorXd sg_coeffs_;
   rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")};
 };
 
diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml
index 339bdeb5586..f193809becc 100644
--- a/nav2_smoother/package.xml
+++ b/nav2_smoother/package.xml
@@ -24,6 +24,8 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>nav2_ros_common</depend>
+  <depend>eigen3_cmake_module</depend>
+  <depend>eigen</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_index_cpp</test_depend>
diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp
index af23cf30995..90180734e5c 100644
--- a/nav2_smoother/src/savitzky_golay_smoother.cpp
+++ b/nav2_smoother/src/savitzky_golay_smoother.cpp
@@ -40,9 +40,39 @@ void SavitzkyGolaySmoother::configure(
     node, name + ".refinement_num", rclcpp::ParameterValue(2));
   declare_parameter_if_not_declared(
     node, name + ".enforce_path_inversion", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(
+    node, name + ".window_size", rclcpp::ParameterValue(7));
+  declare_parameter_if_not_declared(
+    node, name + ".poly_order", rclcpp::ParameterValue(3));
   node->get_parameter(name + ".do_refinement", do_refinement_);
   node->get_parameter(name + ".refinement_num", refinement_num_);
   node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_);
+  node->get_parameter(name + ".window_size", window_size_);
+  node->get_parameter(name + ".poly_order", poly_order_);
+  if (window_size_ % 2 == 0 || window_size_ <= 2) {
+    throw nav2_core::SmootherException(
+      "Savitzky-Golay Smoother requires an odd window size of 3 or greater");
+  }
+  half_window_size_ = (window_size_ - 1) / 2;
+  calculateCoefficients();
+}
+
+// For more details on calculating Savitzky–Golay filter coefficients,
+// see: https://www.colmryan.org/posts/savitsky_golay/
+void SavitzkyGolaySmoother::calculateCoefficients()
+{
+  // We construct the Vandermonde matrix here
+  Eigen::VectorXd v = Eigen::VectorXd::LinSpaced(window_size_, -half_window_size_,
+      half_window_size_);
+  Eigen::MatrixXd x = Eigen::MatrixXd::Ones(window_size_, poly_order_ + 1);
+  for(int i = 1; i <= poly_order_; i++) {
+    x.col(i) = (x.col(i - 1).array() * v.array()).matrix();
+  }
+  // Compute the pseudoinverse of X, (X^T * X)^-1 * X^T
+  Eigen::MatrixXd coeff_mat = (x.transpose() * x).inverse() * x.transpose();
+
+  // Extract the smoothing coefficients
+  sg_coeffs_ = coeff_mat.row(0).transpose();
 }
 
 bool SavitzkyGolaySmoother::smooth(
@@ -62,8 +92,10 @@ bool SavitzkyGolaySmoother::smooth(
     path_segments = findDirectionalPathSegments(path);
   }
 
+  // Minimum point size to smooth is SG filter size + start + end
+  unsigned int minimum_points = window_size_ + 2;
   for (unsigned int i = 0; i != path_segments.size(); i++) {
-    if (path_segments[i].end - path_segments[i].start > 9) {
+    if (path_segments[i].end - path_segments[i].start > minimum_points) {
       // Populate path segment
       curr_path_segment.poses.clear();
       std::copy(
@@ -100,68 +132,41 @@ bool SavitzkyGolaySmoother::smoothImpl(
   nav_msgs::msg::Path & path,
   bool & reversing_segment)
 {
-  // Must be at least 10 in length to enter function
   const unsigned int & path_size = path.poses.size();
 
-  // 7-point SG filter
-  const std::array<double, 7> filter = {
-    -2.0 / 21.0,
-    3.0 / 21.0,
-    6.0 / 21.0,
-    7.0 / 21.0,
-    6.0 / 21.0,
-    3.0 / 21.0,
-    -2.0 / 21.0};
-
-  auto applyFilter = [&](const std::vector<geometry_msgs::msg::Point> & data)
-    -> geometry_msgs::msg::Point
-    {
-      geometry_msgs::msg::Point val;
-      for (unsigned int i = 0; i != filter.size(); i++) {
-        val.x += filter[i] * data[i].x;
-        val.y += filter[i] * data[i].y;
-      }
-      return val;
+  // Convert PoseStamped to Eigen
+  auto toEigenVec = [](const geometry_msgs::msg::PoseStamped & pose) -> Eigen::Vector2d {
+      return {pose.pose.position.x, pose.pose.position.y};
     };
 
   auto applyFilterOverAxes =
     [&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts,
-    const std::vector<geometry_msgs::msg::PoseStamped> & init_plan_pts) -> void
+    const std::vector<Eigen::Vector2d> & init_plan_pts) -> void
     {
-      auto pt_m3 = init_plan_pts[0].pose.position;
-      auto pt_m2 = init_plan_pts[0].pose.position;
-      auto pt_m1 = init_plan_pts[0].pose.position;
-      auto pt = init_plan_pts[1].pose.position;
-      auto pt_p1 = init_plan_pts[2].pose.position;
-      auto pt_p2 = init_plan_pts[3].pose.position;
-      auto pt_p3 = init_plan_pts[4].pose.position;
-
       // First point is fixed
       for (unsigned int idx = 1; idx != path_size - 1; idx++) {
-        plan_pts[idx].pose.position = applyFilter({pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3});
-        pt_m3 = pt_m2;
-        pt_m2 = pt_m1;
-        pt_m1 = pt;
-        pt = pt_p1;
-        pt_p1 = pt_p2;
-        pt_p2 = pt_p3;
-
-        if (idx + 4 < path_size - 1) {
-          pt_p3 = init_plan_pts[idx + 4].pose.position;
-        } else {
-          // Return the last point
-          pt_p3 = init_plan_pts[path_size - 1].pose.position;
+        Eigen::Vector2d accum(0.0, 0.0);
+
+        for(int j = -half_window_size_; j <= half_window_size_; j++) {
+          int path_idx = std::clamp<int>(idx + j, 0, path_size - 1);
+          accum += sg_coeffs_(j + half_window_size_) * init_plan_pts[path_idx];
         }
+        plan_pts[idx].pose.position.x = accum.x();
+        plan_pts[idx].pose.position.y = accum.y();
       }
     };
 
-  const auto initial_path_poses = path.poses;
+  std::vector<Eigen::Vector2d> initial_path_poses(path.poses.size());
+  std::transform(path.poses.begin(), path.poses.end(),
+               initial_path_poses.begin(), toEigenVec);
   applyFilterOverAxes(path.poses, initial_path_poses);
 
   // Let's do additional refinement, it shouldn't take more than a couple milliseconds
   if (do_refinement_) {
     for (int i = 0; i < refinement_num_; i++) {
-      const auto reined_initial_path_poses = path.poses;
+      std::vector<Eigen::Vector2d> reined_initial_path_poses(path.poses.size());
+      std::transform(path.poses.begin(), path.poses.end(),
+                       reined_initial_path_poses.begin(), toEigenVec);
       applyFilterOverAxes(path.poses, reined_initial_path_poses);
     }
   }
diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py
index 86baea19ce5..5506b8a1f74 100644
--- a/tools/smoother_benchmarking/smoother_benchmark_bringup.py
+++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py
@@ -19,6 +19,7 @@
 from launch.actions import ExecuteProcess, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
+from nav2_common.launch import RewrittenYaml
 
 
 def generate_launch_description() -> LaunchDescription:
@@ -30,6 +31,14 @@ def generate_launch_description() -> LaunchDescription:
     )
     map_file = os.path.join(benchmark_dir, 'maps', 'smoothers_world.yaml')
     lifecycle_nodes = ['map_server', 'planner_server', 'smoother_server']
+    config = RewrittenYaml(
+        source_file=config, root_key='', param_rewrites={},
+        value_rewrites={
+            'KEEPOUT_ZONE_ENABLED': 'False',
+            'SPEED_ZONE_ENABLED': 'False',
+        },
+        convert_types=True
+    )
 
     static_transform_one = Node(
         package='tf2_ros',

From 6c3005ef89fac9c3c06b382eff22d11e03a47dba Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Tue, 2 Sep 2025 15:13:50 -0700
Subject: [PATCH 39/51] Adding 3Laws Sponsors Sept 2025 (#5505)

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>
---
 README.md                  |   2 +-
 doc/sponsors_sept_2025.png | Bin 0 -> 192041 bytes
 2 files changed, 1 insertion(+), 1 deletion(-)
 create mode 100644 doc/sponsors_sept_2025.png

diff --git a/README.md b/README.md
index 48a40d00e21..7e85a8225c8 100644
--- a/README.md
+++ b/README.md
@@ -29,7 +29,7 @@ Please visit our [documentation site](https://docs.nav2.org/). [Please visit our
 Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.
 
 <p align="center">
-  <img src="doc/sponsors_july_2025.png" />
+  <img src="doc/sponsors_sept_2025.png" />
 </p>
 
 ### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights.
diff --git a/doc/sponsors_sept_2025.png b/doc/sponsors_sept_2025.png
new file mode 100644
index 0000000000000000000000000000000000000000..655ad1ce3cdc4bf2850e1d514ea37e93ab28e450
GIT binary patch
literal 192041
zcmeFZ_dlC$_&%;fJ*DdDqiCz>vPTq+iq(bMReJ@sE2)YtLRD2&?G<~~D7Avb45jvn
zQ6pBWjZM@{h48)gd4FEd`}O$)K0kf^Cduu-?&~_w<2=seJR<Cgjv6cT6=nto23GaQ
zkDf9xFrgV3PKW(<=J*wb<|qb+R}AWp?i=_(SEtYV^Vl~YZNv>ckS0j0o0$!n#^&d}
zGvl};#MNkgvMSkEC#Brtrn>11l@s0<Wv9ZsRSX_AN=%80NC@R{O<mw>=GX>hy<--^
zFk?U%j)~19`urPFt@F`X)BfbW2CK65VrF=kuS0*Y^cFHvgArK4|Nry<@4)}_9msm4
zs;0)q&CM<E&9a$y$7R(2(cwXM=v74L?p_nuf6I9yX=`iSq0T$e{n@J0p)XWbRdr%B
zQ2D<PWL;~|&u>0>8PMp^m$J8abevQF^OMEUo{w$*ET?ujk$4cu^nYGvU>G2`=i{3{
z`??)Ao%-)pCxmU3rS;6sBcz&|8glHjX=R*U;2za+WKWuH;m1Md|H>7a+CE{)g^0*V
zUh@Tlz1(6}@au#1|8)bIiV|VTGILz1VE~E4^KkW{%jjqI%_hG8ZZ_~0o8SDIOhwi8
zXO4WB3k3c5R#uqy{h&Q*&%@92SqJO?`>quwVv@Md^?loP<^6#ZOjo~eHeLSjx<7wX
z0Ro|QLjonj59%rZ*OJ7AC3~98ukY2^0oNDQ-a2yp?^*_JiW_sU?BmkjxgtoD>w{6?
z>A^>&6aRfernaI(LekZ_Kc)%G|9`*pNnFy(qw{(wEc3|qze^aHtzplZdsp~7*jQP8
z_x|_eKl$6%{E0ObIMI>K6<2=8&Lbs+o!zbOqsIR(fA!io?c^5k9*O0@*Mx2}_x|_W
zEA{N=bN~H{zWD#-&C5JIiZB>#b;|$z-S4j~H=DS$KjQ*3_||ZZrjRL>EAn0)bT6l<
zI7SxEn%xkVP<jYU<~qpin`?dD87=;+n<5*JQ+Wy9?_=PiD3rTsLJXYz7mmy(C+R{v
zpE)@weOEc~1^d(aMa3G-6g=suu2-Zs>X&6nn-}Y97IjL)-qHo1{N&_N=JM(^PC5`8
ze}^Tp-V4uyev>%i{`e?uwf51v8|_05L4?+u6SKgDdOPTt-0}V}LoCX_f3QjOsGpet
zaeB03R!xX*@{0YTy1&8Jl<B)&pFBN@SxHbhLX@O14pFvAhsk?AMqVE>Lxs{0<@^qI
zP>90@A=_;pwGzIw4CosxW^n5FVJ!>AAytaW(4so&W)r^e4TV)eBMNh$c7@x(YCdF1
zQ!9<vE|;tlu=titT3ZV}MUd9R^fY9Av7d>(HNZtSb@QuriP~Y*4m?U@W|KqPq~G@p
zOUn(_QNW!Qc57J{UHf6DlIK?1B@T}Xhod9A3D_4c1as`0rF!zA{4>Ae@M`#GM92;E
zv1O39cjDIfIrjeW&ACRS{Vc^1!)*Lrrft9RF<D~*6Ur@_)nvwIU7`$WSV-X8^xxaw
zt*J}3OxsP(O7XMt%v?3Qa?6E$dG2p^0nlzRS<-wzU_^cAFJ0P)O^YqQ>V-6qN3`gD
zEXYgekPZ!qr$`(fT;BArY!JV?2o5g_r@wT{=DdhGsSAD?{^f79X*g%ZxR)HMBXe?V
zOz&yKV&ewS&vi$rV=Fl!0NwEIE9D-y@zHd6`c~_<)sER6*RJNLC{XU$r@bv>udimW
z^1sW%w=>AzY-^$&3)7yF=3TLPwe{MqM581FpG4v07&t~Lc{@*x#Mmc)aCe#@;T66q
z&d4zS?B=9XhcV=9ldKKxZ|5Rd?382c_W8XX)g?&_{-b@VJrY;9_Ljxo5h?NLh@ZQF
zZ<zbj!4YnJpi#qWpsFWWXSc<%>%0EEC#%><(me59JIq6zM@K7j^@O~Ka*?|d5SSL<
zIBy-z+yf<>ZO>SrqQ2O}L*dV<sRsm${Le}IOS+zakL7UC!6XwQ37#}TYKd&O+)H@+
zR*ps35EFTfWrvZ2asKJ^wY9a(f?&hRA0v8dxXHn<qM4LX%HAf=<=tElIQZ8c?!Qub
z7ebag$YY@H%8q0N{UDpng;1_FIVAGUQ7x`73|7ak%w>{9nifjy!lu@+yC2-AZ)Ug$
zxV$Ex+;dGA4J6?{AIj!z-`XSbefx6H?E7F&l@e*f$r{9Nn7Q}zgqf9qLx|)1ezYph
zLYx%NmU3-cau%ZTK7)zhyQg~5VQ)vHD-XdVZxx`ZS*lmi?YWU_N`h!a#eN?7A_bKk
zC>RqeL*DJ3>G&pa!kXDGI3s6y<zq?NZH1o2g`-BU`c@a|WW9gHC#V}a+kfrxTHIJ@
zZig(WCS$tZc4bzT6`n$EMjRj5MYQJMYg#j)T0=xc*wi{XdqFU^tS4gUVpk|RlTZBU
z;O{*W5}A!OLN@pbT0(A_zmI$^o&PtZ!-M7gLFQf}1_6=RqHtIxX0B5p%I-u&YFI3x
z+Pi@8NxbKM&ak~i<pqijO}~}p@{(rV3VH13{7!x&2kk9?d8wh}Glyc+)4{sVQA}Q2
zxj`dfji`YKrQ}?Mj=x?84?efemb*^5bI+~hz_0#bYsa%cd#V4-p4?%!ic4<Dq7Xgd
zK#Y`>IVZziqdR`hx9n_@+Jw)`qt%&F#a~sCr9pW_sjvu-b(P`VW-VU9iKH4eM6gBb
z;kB_sK4i>(QADin<C*MrTUgre%8oiyn<Uz$xO%bIja@Kv^n|8oq#7}YI$sI%-eEjf
zB@#CGLY`efw7N{7;}8#7ctk{mZgVKlg`Uza{UvsaK|k!IdsuFWBab~Y>5<iyLntpI
zAf->{-ISo#>I^J*IUM!`6|{E6#|-=CG5oD`d7F?V`%~EnV{1+#@9X=X5r_Vs0rMF9
z+_OVX-HMy(W-q&N9Pjg8OV11Z%H7_A#9{jPo)kIF>LA`mO!Kaq8d*2ex3+2XAzhW)
zcMCzcj%@w0)55Bb-x-(6KC)2-b1UDwoQg`xeudQwm`C!<BC9WlaE&q(mXgTs>DLGf
zpd><q&wbCaNB{|0ym@y;8$7-mWi@MorOiANT?{t8_jafVOUT%wh)3PF7djHFMzQi}
zdk53?E7eZb4&SZ|NvkTr2cjds=td-_9(1Cf;KX}?6|0r;ASjtm&*LIC7akBX(6TaU
zni2O}It#-^21bX?LYS0Zj^?!)ezrE*&a$MQ;64JsRcz#<x#zkxs&<^04OAw6yFl~q
zZQ6x6wV>qaI_J)ll}>~F`!Mv?o60}6Jp(8R{>Oar2SaZuuO?<PgbG(1sGgolX7`I$
zdqk9ksB2a$y&^mwv94k=d5LC6je(K<-$xo_D=$mjN0eV(p{nEgJLnHUT1U^HYc?)_
zsK3F{Q<aq~?fXKyu1+6)*uMC=RC3a2ajwp)N}{A>Uwy1}huE0ha$X?hq%^VngH>j%
zIT??v<$MG)q1SK;fU3i%dFw*7;R$l3nsK*Kb%qlKNL#GpDHKGpNg`o?TJ*W7PgbFa
zu!pi&Xio)xD*mv++i%87f@+`Ge&*HE{h(w8n{9`NnZ<J%*UY^r57>=#N4X>e(5rEM
z2ouOuUe97hsC33(f09j)rI&-A+(20_-E366V;<n~g8;=&x#nzpOzY&_j^e^j^tT3p
zvm_>^vD=!qD>FS~n#cyVaDl!22l~!~&z4@#m0Bi2;o(C-opo#`YY!D1r3@y!R|h>Z
z(;DT7?B13__!}0Gm1@VPUw9^Zk@1G9jp<jP<$h6I5Z6IgqC}DV3+m=lHJnjlC#92H
z7OT|>H+lYcue8Vw5J$yDIqvr|jc`?kJX9pgqhSNeypwfCJdf>zx0|%#)_VB#9?r(G
z^l4uCj#tFj@6{$SUl542Jiapacbg0Ml5@reQ$4aGEmt$lve}pzhJ%3Z&RK-|Q@hLR
zEEbdN><Yt*m>faH<ogAc-XtAfk0)$@_9Td<x4O}(Qk?w;m4!Y2@VcRUa8+ByPfdI2
z!lEam)ys%@#D^y5_jhdFHQl2)`CO1uxxza<N;pLw&c#ha9j<^&Ftp1J)ih|I<Fe1;
za$7ENN?n{AqnLNoJBZv0`PuVTz0R`1uK>1^Wmb`lWZ(tjAbEb#*E7{3H9%l&<EOR>
z&+v&Otcx|%`;e%Dz{rSL#Y!X}UA|8np}X${2bme&5=%UktE1LL48@&8M!8T*OM3M!
z+rY{Ul5io(IN>fj7!eqwvn+cciE2_X;G5tSB+wTM&7k<Kw<o2qdkzmUIt&hRQijFZ
zid<!@`Sp7gi}jPbBZsU~gyim~4!&&};4p=)MpiUL$80J1HlL=P<gsXBNzNb6=}GqX
z0Lo}U($&@^bw2t{gasjSWt4>O!>A0t{@OPbsl0QNs?Kg0CH2Mn##+5QJXsGqkPF;3
z`O9MOyz;WhLqF|Ud=C^p+nu4&fHXgBc$Akj_MH1;+3o6*3!89LEUc(xGzKrhEg6#I
zw2^o#=;~Om$rbaT;FM0f&i)t3xD?5R;g#^ipkyuQs#ESb4~Ih`xrc{Tx6KtNa1nT=
zM(I&@E%{$0gj$yx&}j3^VyeoBg7IsOmHm!F=9Hm4%uY%^JkC`@CX<oHqJGzuTyt2|
zzae2;y3?|4In#2h6UT97q|Ru~)qDHgZgN&#JKGRj@-=#7+E`$z_>sjfSA>2%jGxUr
zJz}aB@~|^%+6HbpGL115zw|-R?;^VELjksKpgOkFQ<DXxQ%r>RRU2~t`m63yY1LpL
zw`KYbQsi9ETi8H+8t7cC$h()FTMuM!5u|8%I4a#1NI#K<-yPT$FZuaYW(LVT;6R<j
zC2GCHJzhvu*dX%5rdH_Izew|;4;6nR@8rJu_ff5^IV~+u0Sl$KQ}g=*-(pm7?`tO7
zyKWO<7mSvjUWlq<k8uU@B5?Yf!|AywZz2N6#c#Fskm`?Ba6So;_65cf{yO9GT^u|<
z;9=x!UaY(p(Krn~V|U6WqW>hzP$i5!BTApLQ7%#n%X{7v4jgIQqMUawSH7~Wp5JR|
z6l8AZNp=)X7XeRPcep`s<hz1>H!zir9QNAHZv+5it3t7hnY~`O$IV{S$H;n&e}l;w
zOq1ti5Mp5NwVTKVS#8fBdQZ3Zs!|?t=iP+($(##q*x*W1*p5H_CozSYd%L4{XQ232
z*Y47#-nYYCzFf;si3z1A;#@^#A`&A@v-9f8uvZD(%U2hUF}>V|Ce7V*C(Itqjo!}W
z+A+7xky|lV>k3mXQ?v2Q4a1m<+BYc?JgT0!rJjvbe!L$zdYaPQKcH9)07Ab8zxmv2
z$MjgBgk8DD{n6Dg^!zq8cl$JV8O1f+Sna1MC7x~;FMeH&+Ao_jfmOC|GJdh?k-*NA
zW@Ii>zq+7S`FxQ7yJwes*N$hTJ5cvYq@K#@o&|!VyeI?u#BrQB=)(a<C%M6)tOXTg
zQ>;(rd0XF}^fKq6`OAywv!7D)g(XGVjWb`|pkD4-bc*OduTZQ_vF}41{Bo-1lA`>r
zFWR3#SIM2fqU_=NEO6uPx17qd1Aw&S?xG^OjI5r&-e!sAZl@Z~AEeV4Z~Hu<MB#qN
z>_=LdLtSifp<|6ApTFLOSO+_%zXsUM6(K*xI$a7|(;x@uwwYc1887QQwHo1;m`!1@
z$C159TO)=f?{bPI@cGX#^p16XCom9jEO97u+F`{YDs-!(#)5cjVS7iwi*FHV<8Tj}
zSQs2mGCD{hV<J~e0?--uKen9!x-;1l^kQ1};{(C0@;|YwZD>7u;y%>C5}vzl(R!$J
zW%~S&H{j`$IZtiaDjjk(ZPi|Y<lZ`_PB>8yD#Xnx*lc-iM<|dSm9W}L9mg!95K-T;
z@G&2adnmK6r2qCU){AmFM+02=%@qhp<4mPEhtH_640G?t3qQN6A5(Z1l7)Ox$TS6G
zrm+M+()8h>oY_m+nEleKu=4{c0}lK>;+q!Uv0=lGGJRV@?Gdm^>|Nhd|0d-!R3xMW
zYY+pj(d?f)8d;YOc4n|{e_}1b8ZFdlxOtPi#v3{K{d+^3^ZM@(4j0e;iP>7_-WIXx
zdQGdRU8klpS&{xPByA8Cd;%ai&`j@PAGAJSTD)SithN-2j>xocWSu4eoljYEsY(d$
znVmU6#=r6FGzy{WEnM&*oAB53J(d{~ihQ@$6RbjaCUQH#fi;5%07AUgf$vR$y2@NU
z!$YK++y|BFw~<i$o8E12yhN<P>U=*z0$1;vCH+kRkj<!AXXw)o+px>>)iD4jL~EbW
zx<I9Hy^{>+>le}Lt&frj_%@c|%<es(1}@gm8C(WQzhWy*{yi&W>ruLL<A9=Oz;4`}
zJPMgkFu+cUQp;U6ck`VFu0rIgYw=1Qn&fdyvjv|-@l@FEv)ibjzj^)9o|_*=@017k
zms4}dJo(_B48@jId5F+=BlcRBgK<>zn-N2xAFiZV>0JU=!A^z(ol4+ddFhP>wSBmi
z@KO<TfB`C&bvSB6EN<>T`z@~VYtvSaJ`0n~5srg70mbWEo?-3|KU_RSRlWVOQUVOt
z0(!vNaHXI1E>>7QFbhBhVm)FggHr|}Tne1by>Gs0vvZ<V8jz_Hd(N+6pH?wVk4Nr5
zg@}^rJc_|RBK#=5F&V$He(4}?$@_@JtCjmJR|!ws-Y)S3MElRM#5u}Ndlvebaf-cM
z<GJr?yu{L^9@!Vw?kKkM`NP?cNEv&~gUZrN(QDyX46L&DpC|d%PMpkc)=IhM$5W@B
z(bEJP%TB_9G!l(zv~FD5Dj=OycfRoW+c-KAe-CI96*pIe&3wv!i48BBHCkus;#&*4
zZ`aEQifN+vJ<D;VxWi(sHok&^Vfg(?Y3FfRyv|`dSc`zmMiwX(DmQ_P1-P}<9dYWw
z9(;U}QrRp9LGH`XU7<hUgId{}U<{ca(??mRY_^h0R*%bF$Tq5;DN|r!E%5TRGV+I;
zn0ZxmK%~T^`Fx&~BdKnbvB$DLtXP+7>y5V1Z*qRVAQ&?D!y))Q<&ss(;o`23HEyCY
zIN7%E1F)E)(@UoithDg{AJuvoM|ipBmSPl)71d#YOjHL+!}f2w86#tmsn8yg25%Pw
zaX)`DvcCV#*~<0mva4OvJ~;WRf6n{vwV!0rk_WO@7=eqJ@*Q&aN#`=~E?INy>Pd2V
z^oPe!oU9yDoFZSLpG{0c-tCI2%)`{`-pS#t^r-3iI&u@fD3R&2s*$7()&jY*O&8CD
zxNV+%U3Fq@y<VmWZHx;W4u-`Cs6_Tect&yvdwGnG6OUDlM<b5?nPnjk4l=Sgf!nLQ
zM{P3&-0)RFriqn_c~8y})_(XEsyME8d+(msuRlseHp8+s?F*V@D&$|zw%90>%+M%5
z3oUO?t;jJI_DHL}oZttg2@7?$?Mx~R3=X`E4nvQ+68Su>b{}{b_A3o3s)>9kf{FZ#
z_Y#t0iAlw!v4jmpb^+?ho<Dzhy`^JEzj@lOQH|Q2M4yj+;YG#p_zYY0<sy2KJPaM6
z!4yVLWtP5InqJ4`icB+_Gg%CW`|`T?W{9|gzZso!E9j&ms&XgSOiFL)-Ui@GJ1qjT
zs6|mbT2d?a=Cg_@xJoyv<(qVRsaty(pg9rbczKC0rH~e~>$BsoS?l3<GcF(quOGL*
zyoA6n2Q0HkStZ9R_S`Din{D0_;pjKavWbX@Q1tci*sk2`R6e2}tgA@p0D&$+UD_+E
zoI9Wdp>R>gY94;6O#eb6OK(fm?j)4@Qra}=<2K09bG|hXT!jCO{c+|>Yn@U3T~nKf
zROQ(E<*PW13ROgYEPPl*n%^-)B6ONFuRisz;>6;s(03fy9*aZapf;AgrUNDJdd27F
zr|-BPWx)V66o}fjagAOfe1g+5BvUV#L@K@BCBox*ZCbaZJSXyCgInJ<oWs4VLrzU6
zV9v=n`K*{t{+ylLq$|w4=_0LM#Uc1Lc%u)u7}qww-=8S!$8o+oBL#s>D1*VfPn0$Z
zQGG{Hnd9IPu2iR9i8;Tn3n$Nv&ZH|jHaxuFf&21r&f%NWb_DCrWCD_5$GQE9=N^*Z
zh|txm`xD?ZJg045FUKHYhX6<>h(k1&q}Xf6p4XEnnTHF@Tq&<#W|>ERL4OhtMU@=T
z?xov4>gGY!2Il&!H95Be`r$LG0foO0=x2oQZPVNfrc(Qd`77mnDsSk9OR!0dO|v;l
z`v_t+Yo+i8*wVr;^n%-qMJLx{<R4|+2FaXT9JMIke}dGEy+$%(xN?qhJ_jiNDG)Y|
zg(C=eUcn09lZg0;3SEKCODQni;s1k_CzyK^Hu5lW31BU}V5T!FonVToNrfBGeT*&%
zb*)Cd<v8yPl_`g$3U?2U(|-IGI95+GvN_UHr2%bs_nBlV#o?y+XA3@w+tn!Vt&Y&>
z&|K2U{+!$b0nvYe3yb>(5W9?Zm@Tjr=J@@}gr5KtCmLNDA`mjyLxushllC5&3Y+B1
zu)js^+BkY27He5U?ZqC-1>F6uz+ES;wL6{m@^d%m8Xw)YBbH1Yx|^1IFE_W@!UHHK
z!;bn`nfLu)&rWYlEG;iyP9trHq`U@V^zrn?+-9-5x-%cbhN_9HofAD92ae+U+gnx+
z!OJcHld{2=NInEw2){4)GN+JA;x?C`i9bI>sTX+6zU!TSL42ZrNY>IJrwGd3biL!d
zQ7g2^ZZifMRzLQ(i%?1IRiu3KE9>hQm$?cAi}fIK#1pKcl)Y}sDRFgIl(A`d@hK4+
z&wD*{|7ok3UV4{*BWo-byEN*-sJzSi*&gaT;c~+}XO~IfYVM{A6N{DIGw&?UnXL%M
ztP^_H5)j~T-mS3lOY*40uY`_p?d$ERC-_jY%<13XJ~#$QVXYujPY+h{o#5@$dltBn
zz0Y%+-z9!Mlr?yt%okDM8F2TTZSM9OLaa39C9FG%bx61FYt-~CBo)v((a&#AE`~o>
zzUq^4{`GdwHt21TtX)rTNQMj9cR?Q-e{OVFRa~_x`!SGi4+Jz^6VfQ^fnNSD5ht5$
z2T?DK*KTF*(>E3IUtW>|2Htd)c{1Eabr1)M^{xfy{mNtZ_0xaqW1XxPHP{Q(#*92q
z3|thF<l<p;cv`dgR5fWiT;VvQE0YI>{?LvP5y>$cr*l)-l=mh3^f@Sd4PkF~AWk$S
z;QE-4M}9;^EEiFT9%ADR3#&H`pg#uG*V`l`^fREX@g!-aTV7uv8N<;tG8;aBV;Ij4
z$ybBcLb^mN?Ol*#7+Hr?Kv`x^tG`7WR`m7!Wsuj2nwUBC1z#EOdD}I{I%(=e*~}rk
zfD7@jPE<;3^|!%@yF&Ec-`x>=w)&V`byr3l0YX%KcU|YzKUx0<-1SO52(RqKK>8=7
zG>~Yo`qgZKE&_<Q&&?Lx&hVDy@VQEQdwD}zY54{hd!E{!jk2aa0qt4xQ`0t6q~kl~
z;I^=hM-z6P*tur-(M9_vGf=Z`YGtJNr9}-{UFNZqI)0M5vzVmbK$-h~uPs-1(M_;N
zjtLyN@!?0nkv!{|7Nug{wZ5N^cOm3x1eM7{^zP$(Lc381v0-aZ>K4T)D_ZCVS192-
zMk6Q+X}gqZi;U$s=J}X4LxHPF82d)9y3A^o5iY~$<{=DDoEOpIkpSQzrW%RijQ|Ai
zo+}v`4BT^NaNzud>Mt1`#3@TgjH*@`DR<q<8Pz4<fW_U2pBJX_Cx&}hC8y?8C~(C2
z!^Zv%A@iyMw@(pMJ*G1+{haqx%ImuAkO2i_mmK<mR(I0r^|mT^P1(|imWf#6irLn}
zt>arfeLW>QmX>KZ85{Bt@)A?n)}Gj(Rrd%_;49A_CQj~o37LOX{wXn<hY_aqz2mH(
z630{^6Gl$h`w=pIw7M_ykbfCFhssWt2JrwIbryipe-h}|AQy5BJ&rO<N;EB>s@~eP
z-vvtK#}~{d)|{wN!bk}-Y!M&B`aAh|ttN+a=;T3X`VYon6y$~?$pQ@t=iD*pHncKG
zRqSR-hOvFQMt_1-XlQsk#r369v(%gA1{Y3kBzP`uA73KQZ?=o#?%Gx(R3O{SJY9EN
zg9KMLSNDe5r9**zRj1(e{P3i;^VRatD+&tNjoWebv%{6!(qqn{07=i?cR-R4^Ij54
zA{=Tb`?n2@lkd`bHEkUSM+%izLR;mHeG*Tku>FhW8gctt<bZFy^{2SroSLTHkUWAn
zZv(&z#9w9eXAic@yMqDkW!hNOmp24xzSH&Z8lac1JDkg-tO{3k!#QqF(q_MJk<;Y+
z%4M(;ghMv&yrUJS;G_2Dx6S}DtK&+G81@{Rr)jl)wwD?n-0#r_Ki1vkEFhSczdBJ}
z38-1ZdgQCR61%qEJ~2r>(3-7s*BjpN!iGqrN?UKdgbY9G`lfD+D@!R~UH)`&8%@A_
zmVht$zpvUV_<-;D?0!`v;To#>EydsR?B^CaCK*BIUKK#NjTfP9&OKFr>3YYFb}q3u
z!}kwk`oW0~F*UFhW9wXxCHQKl1M(&88<QQk!BJz&Q{u3-U25bDTrTTW#n5PA@$jL2
zI41-oW3%~Vf>{FnOx&!qa`1b`P`_ZRko*F=TFXHiN4!QavO8M)Iijbx&k+I8Sz%M~
z(zT&oA-Qp&MB$(I{?IL^$cts})WB3OQMMq~zi|QI&#wRtZ{^coTcu);C0}Jk8IFh_
zW1nb(5x<8lWojEJK0m}<!0w$$0mekf)mDQGHQv<ymeS+_CbWdK1{|6PEdh!a0NEZv
zrfN30wQ@i2T1tkEhcF>5v&qhls$rfsdb!R~{ikFrjCm<sLRn)hMTpL-X<GpKapdP5
zN?qc8y@zcXjdd&p0^9=}5-}{@p7b@t=iFb(vpzO{xU?u`*%&0eBQ_|H1A0gh0&0iH
z*;poW%GU5M#p83iii2g0u*Pl`ATZ6Es^*$Y`wzbf7EP|Y-S#5}42W1m?F*M2dR<;r
z#O#0UUo8DKbxsEKKj(6rsJXz+Z#Bpjyyk|vcGTCr{V&!vG+TUHwC3GQvlT#aT}q4^
zu{f?a)u<K8JF3`?Tn}&Vd`NWA9N8oAt0dhIX)#>_NQ}Z@Odrrem+{rK=0#NN>eKSZ
z)r0vTdp5QA0Q%EttpO9dT;Wg)K<w_z-enf90k=?-F~Cfh6Om}+=o$!qH|P+2NmDu8
z)ICljVNGpB(D41;LTm8$PG29^qARi83peJx1%R7BsedcjL)qx?WXjcl&!n3hknn9z
zdS^)Re5!mR;_;yXC`Z6N>KEhudkN53>h45m<K&~rIeg3Z{aU7U*N5KsjK^Me$#R0t
zHO=o1z&-XJKs%klozzut=-uD{O!M6(nT%gKW|kxJzCc%2wZZ@P%ejwkxu>2VJ#Sfd
z6EJIJ0XaIJ0if;2l<kl0&%5Kb8tW4``k%v1uc5T9Xeo*KeAV^aNu680nj)W>?{b(;
z{1}M|n@*kc-D5U-*4<y$GY6$XETt3xMKtAqc$`M&4&u=2b5CbAAetv=kfsiBr}Y7V
zx$dd|_N2Rt<CCRx`haDs0@CdGOg>(WBH<i5yi?cN=(u9H|NYF;g;-Gmtou?<<@3_Y
z7<r>`3J_GBE}uSMB95|!W{8od*&cI~?4E;~HrI3?pHi`UQs30$-oKz_VwpGAeGnG$
zNd-t$1uAhMeJM1UAnHK!vMgpT0!6zlduzwfSS@TNiPwpY5W!x^@~7=a%U2|!CevyY
zTY_)%D)?g8$9Oiyw!(mfwG9cocW>)o_I7qK2?Ty=JT8QaT2nVNNUUGmBwf^E{X<LD
z0a^-UlLl#pHV)8xfBpKrml_dkyD+lbIXCO8dAw=~=ro9$=&FHCJhZohgRH44uIkth
z*@bW4oVuU~j3btf*A%->FQxB!34W~jNl9>NWrIgkl%aO>6O!5h_lPZ7-LPjb@yp*G
z+$6>1bFI%fv<98hB1?c3dYrER3gG$r==w{S53rxW)GQ*v6?0b7bE4M56twWoSD-Y1
zFY)BS$A5{<1Kt>ykHP-0$1JB4?8*86wn7Sw4mMo%DgruD{SJoOuyBlu<2|U*n%fh5
z$Hb&b|C^JYD0@eC0ls*zh6<hiL{UB3D{_)R^j6p$bAg7YeXR@5Z2|=M$B^gGuIW6M
zzV+C{YTg~DP!k9=tA*vV`&Gx8$T^)!{V7nY)c8s|*?gh}RR>><j3M|2+bBEG50WMa
z8tabLPJi3kv(-MDvCVJ0<i$~15i|0jxUD6_1eN2dH$%$Bf0N{~gzR`H9mjW`Ufgfg
z6=+-T`G=q7iAak2Mh9RmTs-4<r#&ZfVYRa6I+7aRV|mm~ct}S+5<$G{n-CVOJmNei
zHGhRWe;w%Dfp^5E0}s2;11jB|pXU|O;o}pCrnH`g>W-!RW`E6PNbek<fPHSnkSG~n
z`{J0av{<?T5#aGX*&i?cfug9m0YntUg^tXiH1VwWKZ@w9C?s`z+kbtuRJL5@a@jq1
zTVHAQ7zWT6*e){tpND{)+)}liSA&3&hAr1YM<rhp|CGo0(&nj=JJzr?ifik(Bk^-j
z82l-Av30wmn(Gjqb|~jnG1>a4%bP##q3Ns;{rwuRzZpKS+#V1XmVW&l+3&iuEMJ}1
zAy_4My$`)l_CKuUt8eXK%Dq_pOS%Vm5~&Q>$Q$xAZo{ZWpa;o3DwU}RWECXXM9ug{
zQORSO3RVpc?|SlZwdo2Y_Y+AtZ651w=v|pDKO(tk7RPEJ`Ng^g8*=Z<lRwnu&9^QA
z_B>olp<msS*Qs8+49K5?IK+V#t!uDofNVabI&jyDlIx!Aa!J!}&>pb!Gl49A`zL|}
zo{1kTTpn-gS%(9^ITJ7{giMm1W(k$!g>W}<AV>4Bp4vOlbGNUyuHFJ%9=yO9Foa@Q
zC*v4$V=Ep&-p<^du@pXZd7TE+nfWGAU#U2vg0*NphT!w3{w_X>fI0uXab5_0rX)Rl
z7;5R;xoCtEtHyshP4OP$E&G0Og8AJ^_d%dbCcEX_T<n)}yR)_oF$wP#8^k31i6?t8
zNn;O>`Y@yQ8<|IT+yVe-Cp`r;$@)B={%`!$OV#um%#X||-5ICW?R<avL=fa|tK}Nw
z2yW$Yey_d7!DS?GZ1Se}yELn&XmG&GXQ}K@-z`@EjKu=M4vwpZp`wq+q|s+<tsI>+
z_~t$_ib>%XIb{*z^!$K+NM<99D1<7=MBi0ssiE%157CPzbII2FsVA&<e_-ZS_jplH
zj*;=?6NZ0T@%1P^+{`0N{n~VJm_)Eac4vd(pU2=Amkd8vY|b#{vG&^}+iGumv*+;@
zt;x4-B0TLEv^=Xn^@-*E=Bk^7qXW7nF8gbQD0PoM-^it7W$*7;I}dMZ!ySCm!Ga$p
zi9!UqBy^1*xgo3W<EysJ%g3W^$l0Nlxv`&+$`!3IG#o%zOB(gH18o;&oj$r!@T;Lq
z{UP$B`t!ePX;3X%3dy>-Pz-YS=Okj?KLDS{aPjoN>Ckx#feV|$P5i9Vi1A(d!+yKX
ztxFd@vjelwPPydql*;7seOUu9Z(0LP$I-XkkG6moBDSe6sSg?9v|&xL{2F6O8`?0o
zv$6bGxf^Y9a2_Py0&74&!p%}ILks%O3PUk-m^nX$!7%}`UklgbIL7aY^M5rAu*D@U
zi^wEAYt(h_O6(rZi_HBT4c&B17Bfy6Z|stAv`>ead1QrbOOpf_dP_VfN@0V@xxCF)
zB>Mb~ep${}umA12&L(TY;%Bb&G)p)5G>cCq({JN0{;A*J7#-|^)<(cwM82_DXFpz?
zdOXUDI777~ALUm0Ot)t9)yb-J)2nsr6U1pJx5^Nu4Q)WblHe)^OHAy>B=A7*mY1E4
zTX&ho>mb>mk~`(rC0;}gjX7uEI%51nJ{Efdo|}KMlLk{Iri>(kNn78H`=g~LRYAdT
zdk1sx&0TMLXYhU8zs;~YI8U%5&Z;e$vDcEcVYnfCeYoc->X+k+muGTHmqB63mIIBG
z0i2b!kRwtv43H=~9mo>CLNveKCtow9%-{T}<g}CS*i|>cen#`-)eKR;0f0tp!(!kY
zplb~P#wZV|wyi_sS5QHF#0kXmxCl!q3UtLpd~I)S!>bQ>=eqqn&SVfqZ^!<p^tlQ~
zAW}EcM7tKUDmT9%X9c>-Nw2Be1|mYZ-6SQJK##6!;|Yil_`*_DpGdn*jWPNFa|vE%
z!Fa!4C6)}{7qr4{EC*OVS0Y(3Fu(aXtL?UaP!iIXLnx*b_j?0#fBpWG3>VR&37qL6
z&N<tjV2?nlIuQA*mx%s0{}f+Cq2Jd7dr|q#2ty^nfft1=I$JE*qe#wxtQlezoNjmr
zMKDo8d!Du{;>$QddEpjtyz;*1<LcQVCaq|T0bYKlDA(a9g(nBJfO6OQQ+1!(M1AJ%
zljqV*Zc^@(z!`WQRwjuPHRPe>nPqYH*Z$@Sgii3<#&SAYZr;Z^iF>)qm>4Uby56>=
z31IoGDVGqStsM6p(R~qI`A3p^J1kId&#@Nqb(y~Zc^2svGx|)npeEA)5uy^9Q|;d(
zNxR({A=dAWip2a;px;u`!OuwcU8>HZqFQ92b_JK*B8ghQhtl{-cBZfghal?8P(D_k
zNBleV(=gCmU)Q~~M5<r!p_2C!G>*~^ybHUk7ayHos?2aeXN$OT&zb$!tRHVIAF9WG
zBS;OM35TKaEpNN{1hD!`IfUP16W35|#hxel?+4W`0;|N>yU8N;c5VUAFw-U5+SRT<
zde;-{fN=ya{cLgZN9}&82K3<6Dj-jKMlBCz@By~YKnb};G_iEy(RZcU(1*IEPks{-
zfGY^{dbxBcqX0c&9qDg=?Bj$TRyLH^MNK75ZdvgufPrMz5}-INjinE8p5HNeKbo|(
zp82z5DPZ$5ND8);Q%edTDsx!x>!yj_MCn+Ht4;i1dh7n<x+TZZ8A>tF7i%m0Cx(mP
zQtr59$brFLl#BDCfa}?T9Y|p8-I)6Ruar{(Xo{Fs{^$XYh5e7bDkQ1=x*v_v+mv!n
z>d{(qyj>q*G<m(0cPaEC%NE?w2S`JBcyaww(RHwgw>B(uNQEU^`B%>J^0DgxUn{9D
zY6Nr1F(kyhFE^3*J#Evy1m3(^;(?O!4mWXJlP9$Sq5gqD0#jbpz6(2l=;FPW18O1B
zce?mNtruQfX`woQn;_-9JJZ`vxpeHA!x^5fs_H2A+HBd*9a?B`e=r#j;?{~kZ2ua}
zWGJHb?Zh!qX>}Ss;^Qo>(xZ9pCL{x*;G1u(jPq%}KsDAPE5T0je$@uYO`=F|y1HZ2
zZ<eGtI=f<%EYTKlpZmD3M@w^cy1v3p+**KB@}Q9RaTlP(RZ9uI?*+*lu~QOzRlv+L
ze)X{O*as%H<ivLr4vp%6CN{p4SLXTxkPlR7<yPQoj_aF1f%6>*AKHTB<g?x(U6nHc
z%hnzk0ZMY81^v;-9g~}q%l)Q)FEcW{diH184!Qn;ht`16bHcYbu=SVIG1uT`?)3!}
zw(6|s9>NvZe6w^?6GWisgI1t00Cj&xV=(a_|8vJqFl}#$k@=NWt{njc4xqFcMP4XW
zxJyow)y!k}Lz=!CQIeiJ+i=59s`fsO(s67sDKQwWcs9&_d~3V7k|FonZC9xI*C6*}
zTk!M`6$IYK)Gp$@&~Jo!JO!GPT-@lg;9KFodCog>_NvcgJd3wFJ|`H&Tyq3rh`)Rs
zMG$~GD_pX)e!ZP_>;N|K+1N+_Pb#nc%5B7rT_6c)9a;;WR-YP)m0kJg#~xNBBL?0;
z*_J~DDq1S-)573-n8(Tw@hr+i^sZVRSVdz0%%P5R&r$UHVB1?4=Ie2N0=0)0vuY!_
z4<J{Km_0Y3l-fz>tn0Oq_r2UOjwe`jh*@A|ov1{D-w2?*;1@L>jdjz><A`%;9%B2~
zk%GxWnmT*!<Z_i-?LT=daKPC0p>q60*6HK(RH-#aV7cnDyK@3kkMgts_;?-$)#4L<
zEl+9Yp(3S;=Za!MF;{P~jStYr*_c8_fxHC}^(xST9=DA{M^!eosIzNN@UZBghk{+j
z^*+}t^USJT=T%M>cR0q7C((az)^sJtHhp_LVm#6ILT@+yfhBOv#MLQS3mN60=jnF1
zZ@?g4*Y;}LSs9eMf&W94>~46wVX0mfq~z*FPCtPnUi+IJL*p><zZm`isIX@IOg<Z1
zHNTYxW|iRZe=tZ4h?|`{U><X>)@$)zhW})O$k2J5I@14n0G7{T*1^xtqyDV&XS@le
zps6qL@E=vH&ME5~-p>HPXaTcIy7=&<a#@MmJ|7YKj{42n@j)m1!hP<#Eq<np4U*-@
z<O`5_rx@4tq)a*;v+=D9wCNXCzL9%@Z|!66P<ys|We<RX?=z*I#dFixyVE4qB~Uw)
z)7)g1!C+p+TrGt~K*Os`mV+Vd@8hj+>)I7g1*YKSCirnc+P*ot^>e@;-YDt&sfrzy
zuyNsgoxvyk*M6MgFyKz81Z*~7t$vy6g!`MBO|V)uN-{}m_PYi=)!*GlincIb`lSE}
z_`<3MZO;kx-%?98$hrFM9qh~RzT2kDKp)b{sX|?#zr)!bJ?MY&%`~+-593*%8e7(B
z&=V0$?P>fdPcl-m<`NVoZ=ptiiRBWrPCW*|n2mmhK}hTviQT|}7+%R*RJL^Nray`h
z1;(g<fJX4UXBjg*{;Eq3ITEsFTxz#JQp<<yaIQAFc{Emy?daNsC4aqU{txPhBgnYw
zuG=TpVdKTm6n__+r_cBK-j2vVCw(xz^+rqmg;dPG^}-NLQoLA%9ADV%pE*@lmZ;Rl
zpQnvJoKdCdZm!&kHSaK<m{qE@Cl*Tk?@ZfO(6DSPnZp+hyc4$Ow_r!HDouh-F}uH`
za@IPEPvvS`C_Yspk(T`_oBWT<a5jh!Ygfjv0~4xOEytTKL%t9A1U$~l?P~g0{lkIw
zw8KaQRcAyCYDFJn#7(OC80R}ihIoy=hxtdyw4$}dqkdqpR=Fc>7CnOPk~NZ!C1CDR
zE`V;+T0R+B^aajiRKo8HdU>!)6kjA?BHUdHCJONkCbPeFe-6$;T}Opklqq{OZO5%+
zO!Wj(yF4N+hD)zR_5Z57e>g3UIG|htO(@U^j;G)l+itD$%Tbd;9TY^ZQX?Rfa2nMp
zDQfsK_nO-5ECSw<7pAs=*=EH4Wj&MCtQsX4(MWr5k=mc&G6wnxmx#5v1J(Mr!Z}tf
z*xxr1$<l)IFmjnK6$QNZCfv1L-Ete!wMvVMVn=&DiS={xTTKDc2dNS_GhE9YF1~%3
z0Zcx+9P0;up~mtm*5h4%a5VtljFf^qBq*?PLWV)6@;i0wDfq&gMice*U(eY<RCPx~
zs~_=r>0yRiHJ0m}e^+qzm>uc^%cXe99oFeiC%;k9BiErG!1+XY8+iZ7lW<okRlb$i
zA;nzVM6n;3-71lc`U+f20TLZg@W5vOT?hysd$<FqYA6>oHP%xVP+qX&9)zUBjWJf~
zB*z<UY@fLUM>SlftKqhX*A_N?H&<lm0=(a{%6ytH-aI<$V4L12*B^l=^%YyzOYwI#
zSI-ZdF2w%)2->;5hbo9XN}k9Z3sSYAo!XNJC0Yp9!Vi2GCG<tfzpl#A2UoU31T@C_
zt+1GzM?0dGn-|f;q$)<pq)fe9!d=MSs=&w@cM+gMEPY2<w)L*__+bE@Jz-^}Knpvf
zV)U|3oucl|O)HN|&_*=XjGrF4lK>Fugp=Kg-RFm8yLV>GhzsfCXMjok{fwYFxf<Fn
z%Pt`jv@b7E2YktlDFsl;9U80J*dTS&_ih{WGcYJT_{7|cd)%L>L+|+bkQ#GCMQOOh
z(+=&AUr(NJB8tvsTDd;g)S8h|LL|0q^)9ITKBkrPomWUs4!dJ7EV-Y(ABd+&4};R3
zA7ltsuLR=@Ti6HkFpZs!ecuvqitQ`HAA+M*lGs-*C%=Mzj8+3tES5mny0FwUWwqd*
zD9&9av=9;unhohqj9o~z{v`~J&#=STTL@O&PEJ|8^EmUihwF+9f78f)edu>3o7dz=
zd*17q>EK*!hhp{SIm<z{bx$G-S-g=n@?H+8A|52wX@P5#QA#fb9SDzX{NA(1W%iUo
zSZM2}gK|0F6(R;#c1(xbk;WozXcU-3!^FD%r;8~d`;!KxYo6jokA$gtWg3%~&sxZw
ztzx4PH`&R~^l@Mm=!(A{-m<%R*g%o~NhLtwPf8!lALvEy^`s(^wVow_9f&T33h$y_
zV^?<U%9%U^on;#0)ud9oahwcUFEtFTM=$H@Azr#(P<yz`qh!qIFj+smu(%PgGa=gh
zgS>;VAYDtZrfO{8pTChRZl547Ur@>HGb=?O+-=lKinb-hq+l+hwc`)Fql${RGWnW~
zTs9JG9J@Tp2KuSOvK#xxrHfi$^dT)|V)Ykh?SeL705GFEl4ROwrE}nK-y={PXaQQF
zGR=!$HcN38)6xj<|69t9rs}Ng@Rv5K%E`+SOD*DzKSed{s@Hy^_!Qp(8Zt#zRwJS;
zS2>ek$Yv4l2RuQA118>E%411s`Pg%~^Wg9P=!vP1l>|sIOxeLeNb;xKW91USy&*JK
zs5gD`VJ26^`GM?#gTl@``&@tz-qSPHpPjP>a1S*Ouo64dT}`UhsEY|(q;&YtswPuG
z`$Y|2peVzh$DtM-ABB~QxMM8SJ_C*)hOB5nn6&5t9Ng1iTL}X?b(X78r{L{(uy*(8
z49C~7WW998e9)4q(o4t<9tYo7zmxguP~jL{?`T%@IcpbtYIP^i4N^7f3h$4dEi0GY
zZN;tD1jPjL%*%J>ftdlj;3uG#x=6EbWtJHXuIa$g8Q1C6iXWr!aD45rube<D2UyZ?
zE)2`1>FaZ%RBP(Nk4NZc6FV1c=daEJoJ2cK>EWb8;8h1pE7BCe*>)FK&Dd`-^=uK4
zTG%1|O{VngjwVFs?wsIYkLZEdjHQZ!m%87blorVK%J|qS@ELDas^9c^O_p%SSwVJE
zNY04q+WcgRQ1AIxO9aTT(ncj;48d1BqDpCn_x%>jJ;lvcK-joJy+cbk1$XYOUY({1
zk)8k`n$v6n>6!Mb^A8UzB2-O}a4jNahQI|8QCW=$|4+Q{`}9+hc_?Mqq;pDnL;gJT
z!YV<Rn+*mGuajqY);3YY559a|x30z59L$j)yRVrFq6XS1pNOOU#DF1oC1YX^V_EpY
z0FOqtgXRul<MkK(XF}Uhoc;yz=q2whS)x*>-p*gjSN);K!bzW3$wq5-2p0Lv8JNId
znWoBV1wQl8%$~S5In*>oDtfeZD%RZ-sN#GRvANA3x>ho6IqesEqRG<v*eZ#A>9I}4
zvidp~eA|0lJ8t%p85N}k$E7km;;prpwWaqkFK5d$T}xE+=`5ftKB-eozKNga%#^jF
z=1tEqXoQ?}@2WQVF+J&_F1K_^(R5M`=<PHJd{!F!5^>^rYeIKj;WyH4=4bNZ>D5TU
zc6LTm#f%(K-Shh?adk3PoW86xOnmbbnFg*)ghk&&Hw-yQGizFXC)%B~yi)Hsb@1}B
zqpLN`;?OwQtHT7k33uK`{?6+EC|iBMKzZY#cf+yc4S$b^w9?qMUru*FR#FCe%Vs_)
zB0@VI4=`Q354BxyS0m376wDJR$w(>*;#8<kxjzgrWw1GE)_pREZp-sq84ttEw7w5G
zRM2}d67*5Kj^6WeahVU?iN)=QZO<m}nN7C;5L;p?ODQP<gC}z?0F83z&fyRwvj9f4
zduVKaMc%}e7Iuf>iPS}O46f7@Flnp+U>mdbV;G7)>|NYUtxnRm4;&z-D4_fPVVep{
zP_@u_`SuC0AND|6OmDgSca09MTM4r5;Os%w$l`F(h{Gp*>ofWLgb3<w0sq9!wrQ7}
zR6||rP<vv`VPn-SOP;vrQ9T3>eBUTlD|+$HHaNF!REXYSS5oQ|?>gTSv!z;^N=PD<
z^nA;ZR%%Wx2?i@e_E$ke0j>jI9ww+ZF#!gG-tlME3G;_4IO(k$1;~!1+QpjtrNUOr
zCQ?;0BrTO#@dO_JvLxF>M*&Z}S``{1vyj#3`7b(pZvT01P~*q-U+`9I$G%3y`9MwM
zu5Vxb_vTMBT)e{Qpa8baXkXVk)aHFTBeGXim75`m>pmr|DGN8Hd?R|n-8^v`d1dkY
zxAlP?2r+92Y0b8pY+cpRr@elKM{7vDNW%B$sN7^#h<3ItUUSn;2p~G1Lnb)$#$#*7
zzmD9o&)mb!(H5NCX1+P(ayAx9SASyduQjFZHevMqV%I+5JH>c_g79)S<RNtO2e}rZ
z>>G!_mf*7Flby_U7^ee%e0?trP~V)9^QkprLXx&FeJfW?L>36<A0t3Arh^U-CX=q$
z<+RU17tf3PRcM5N0&5+1eA}Jv$30wnRz1#}rxsCmFqWl?b&djDE!2je{DFkPxBsZF
zdp7}QOV1dp4ikfd0CO+;bUH_D<2fBbhf(bM#_Q#gcq(MRFS6c@H-3i5xy~_{T6<1N
z@)X*P?hdIcHxM;SJZ$?=3eJ(X@l=HdQ$mP2pbun|#{O@+SJHl6*5E`1`ISTrQLEaD
zHHjefNpZ;V=v&Gqxg*!l!|fjfz>!wcAgTMb3(8u%pZZKBsMkUzuWn8N^$&elR3=b=
zV!^Dn_hVOqmrgn?_`W4?WjyfcA|KZK{0p~T1D#wZ6BFD-FQkwE$_|+>Zt>s0*bS&c
zc-iQH52efc+biwtf{*#4kzKWG748v`ntS1e0juXLiJaW$9!W#C6tt8nfVd1O;0*DA
z!5emF>7d{IbB|*^O523~ZZPR-^#kejqOOJeOaAm5+|yRhok>T>Hq}?3MuG8)hqFZ^
z_6oEY7P~_51RR%Txh^kFTRFm8t`m}!-js+(Snk^94JK&oY~^}TRZ(H1zqZq2l}3Kc
zwKaDL_6y`{VnTriqe*uVe&%?Z#T_8rojme%vY@sum9GXDxgPatpLw%EI;@lan^~?2
zn%742Z%sVG=%5t?bnGa3I%Qc2aHqbl!kwQHGLA82iO7CoDOs*Dd6RTc<p-c1%_(i2
z(dYhVQ2NtC5K{QS*l>_|u!l_nI@YxxRU}~kV;Uxm^5IXbbkVdOPCET6HmCW6c=^9>
zZCF$>U~CD^vkVl3`^)B11-J_|Em7D%g2(UHr+}u!Fnm3ggImBgcAZ2L@aOB{LVZGI
zTLh0vn0%kug5N>X7JLA&+1XeEQF2c$J+uD!8>X=M4A)kVs-Zj9n<XJ#mx!vgtop}I
zKcpeZzz5uM{y?e><y~!BW1{>5JexXo8hUuc4N1$B(x%r)BaNO8kyVrS`B)41lGUN|
z{cT<kEbAGKTDF_Oj_~t55?*+EgPt!9&k7zAxEh+XAf-z&<~Q-OGXCvb#5#TSfnY+w
zJ-qCmz&Y*NY>cmP&;y3u-)<_8ZGgFNL1rB#Os7<R6Zb05i->;lzS!(>iJKc)K<<X@
z`R3U%#?7k+T{h1RjKnweGxuf7SC7_qLVxKw1+>%@AH1N_x-oB;C3S)Blp$Yz_gVX!
zkvMJe_fP#)jK$!E9a4W{_lpmMWbwMwRa2)T!PRukma;2QFN=y{@eQr!v8RDa5BUQK
z7Z5+2=9lA?y>fmj&=Pi&N}1SfBZ@#2Ds{U%!49Fb62q1}m2Naoc7Z_d%!)Wy$4$c2
z4Z^fwKTDCO2){f5NQ`cA@5!!eBuL*JYqej)e}(*`YTWPr?#5Q%QyDjfq5O3=^5N!9
zhxU9ng7r~YssFcY!Lt}c#s(q97S$yyfrEy*9s%xrSxu#(ijNl@Q%*Vg(CKvd_V)He
z3Ex5}@LwUQ{YRH&L;#TZN1Pa*<g*jB`D^QrK622T^v3oc?`4$J-Fhrp-#4*4AK+FK
zi_2FAzguI|gh39xnCyD}&B2XEl;aNLQH~0IRJALdTyr3G<eqrD`ioE3Cjp(z7*|m(
zqob>G1-pQn8M?R2AJvTlYm`M>H7!yNj6r6%pKXbUb(xKcpy~yQlD-913n4!%lP(#U
zD|36?3+AeOD9TzL+YVTSJ}eC?cK}BG%|=>4m(l9@EqWfGrV+UFRhM%@qkhkse)~z2
zmw}()9o~qNCWW33WnSm4A5|hGWmHmIq8MN^scIVovLeZ~`X2^;YYz{PCVxG-q%!i+
zxhot%xMd%I-_<yi(y=r+MEw2f)0(l-cIZk;R|EnY@De%}9r-(TB<V;Ra1gy(vhx}@
zx9V-sq1+Dy12Xl4`E~j@4>-`sxW{B{SzISmT8SZoOKRfGeOeFp+H}*?Smcq%KoI*-
zBA?_rC;!ij=Wz9~#en(H5!s0hyATRD+EopvlrHC5Kdi@{XLYBVE>&AP64kG8@YFm_
z0jCAZ6qdBD6El-}HdiMd%lc7luu+%lDeJb=dB$ACx2|eY<R%nuenz}h$0_Qcc@5<v
zyuE*0rs=XQl~7?BtQ+@p^Y)(ai;^wrMDe=0?zXyV4K>2|1Amtj%^yERM4;+*^!8#*
z1Al#CT8}<*cY1UYeS7D?X?-nk>yFsMNI{3yCP{hL=kLB?C|RPMe<CpEcnj`eA|equ
zz;H9m`07@zZmAk+G>^mctv9~f;k9X<caj7n+_UT3S!Jswj5;OAxTo=`B;an9Bw&2J
zwbV0~VKv0EQg!<ihk|SL)~X96`abnSzwCAF)4Sy3Z&Js&tnOnR|39kUJRS=6dmo<?
zDqAJ{R*8tl*msKTL-y=sCrkEyOYs!hV^Cz@_kAZJj3xW7vW{)SNJe9R_tf)zzVF}P
zUN3XzzR$VNb*^*HJ;-Y`0V3?_jx`&LOaq>6zylm|=eu|l(34l^&hn+gjWJk%cQD=Z
z-Zf4x;jH=6La2%SuyCFcC62$Q>q0*<nAebdKbqiL&M<08t4B9D@+?<s(1i22c8tm*
zdEqNiJ6IN7l$<jI!_Re6OZ@00TB)k4qVRe>5IX?G{-t;Lnm=cu?pg5x)TzNqtQ;&=
znX_|TKVS8|T;N{r7@64%D^Mw^TTidDCnQhrZ*c|7vL(}uKl>M~pJPa5l^@y1K1S!(
zygD2bE?YIIMUBm5oqGMOf&V^HrP(Fx_wy^JBmX&s)ecdAYFbjBEGY9&6@2i826R?<
z#1x7@#qCOX2(da0Hr&Wp#WUaY{Z%Cy7^s-F_eNqLktEq{+H(vxxl`8Cl)@OspPFvB
z=1p?A5Ba4Om3@elg(^h<WE)8i-tjmr)6$hNp1&|Tt&+Kq2J;WmO??;kJet)N+RO4y
zUGY+U-op>~BBdVN8oFk$8iSeT*6M}nbMWvlU%oVKMD-ut>OhasC{_!BIctQ*wvlAO
z@vak9g04hgLBF4O&6EC}K#_i;?`tBAzD;{tsjgkw;E@mu&dq3P@FWHMavu-H%b_>!
z>dl`NC)sU18EOcM=Ys=Rv&~^4<X&Rq(HI>T>>QnL4i*Cq(jCM6^Sn<ZSa_pvpO1~M
z?~zut^HM@>I|{b>?u!!r*=PaEDHrZU5bnH2p5euM6?EWFvm}@PC_lam55ZZuf&zl0
z1yP7K6sJ6%5*JZG3Xetl>*XD2IxO*TcWO3O1itI?#HTPhCAGKJG<4Jmc+^GE%EEn;
z@p~%yQ+)$?xXoa%(`g<osGoO;lT`D{y_}q!kRiH64yvgWJBcVin=`)qIa!_2sg)p3
ze+DGHSqR9JKTy)yez&jvBlAoBuX;Mpk#eMoou58|T<dtwLYp=MlW2sho#M8~#G6;m
z^4x#Hce>~1SnmwtD*an)@o&6>GZK27YqL%hF$(6AISOYVQJFcoi{b_C(2bhm=+g12
zylCjOZ&Yo+&+pOQLA4IU(P`0!ng%S~Z%SM%rpG*WElDluOnZ;?6gi5s)7P@-MH&>~
zN+*DT4Cf}PYdqpC&d@AAO%K#>^4`0biRZ;i8DgY5M#0k<rZ>5lO~yYckn)~>`^w~-
z+@Fo+b&d1bm-XuZ5oiT)0zAF5_nz-o?1=u9;ZXVk-o)Mi4E^T_7{<TfGokShlZ40g
z#X4=eh+a=+OHI#JUut@`0Txc4Dae766KOc@2==PiPJa=ZA*}}m@$!3u_;H}XlnM-m
zr+Rva)t-caO6L3T5v{QW?vv8LG63fYXmxYo{>fQuZ7$bfN)g6>)SF+imNjaVtVy=h
zE1`5cU_aJ375+Gp968KZq0QNXe~40H0X+wv@j!g4GXOH3+FyA@)2sd-Z@9YxXqqAd
z^Q`*g%;Z(s=RG8q%>~<!hy(Cb^(m*>V1H-w0Z312s$f-Bk#x5ow_T8bJzOjU=F?xd
zj;ZHb3~HWdz261aNrIX*uAaoyJ)Is-G!N0*ST0NPT*I>pT?64^NLX1Km4@f$csFN#
z{{2}B%uChpmbY8<9)EuK;5y<Q^7i|Cj_k+oCuU~0zWj%A!`YJ@Bn3XS&Uj`pR31It
z_<7HnIwVG0!A<}SDRU|(s&<j_jH_iDVp`qpwxfSs+BF&8BpWGNWjy{iT!=-2^5sX3
zeER*UX=&zxCaj~*d_czTIY)*}zhqV+dBG7|3SwL2yLe|$Vj)>ona2q>NqhtPD6SOR
zMIQt7q2lAasWY4ev8ci9-iSaSVVQkS=7c1j5p>S}xrZ;)F}S35&3Z(jLnvM{x4N@#
z!<Y=h>1=In-8C~a6Xf;!bLLnGUk58V1eCz6>oDHb{x&#t&<NZ8{p^RIyNO2p!l$T@
z`wvk^W?*Aml5!``wK|(1(b%fU7W7F^l8wg>_#E1Hf8cztb<EDPDQr4OK1>zq4<1;k
zyv-+>$(8Q3S`Kq_x3<yh7;2#5e01zur**U|qKYmYAGV36tvCZ~q_VhYU;B|JF1&=T
z^on_wyM3E?#?bc$S&L#bl}TwIKdO(4^yG+i02D_C+>wfeudFdY=YW1RH&*iPwx>u%
z;Lb<N-ine6*YZn=Z^tUmvbfLqGvJsx|9DzSuVlCOru_SA#;f0*gcC+hO*cBs>zxvw
zBJ0NR!LjvfV9U*-CORgjozq;3b%Yg*o-Q|?7uWBA;8Ik>*lwh;f%8uQW6Sj*tyP7c
z=1PT{yBc`siwMBrA-d^nx=0&4Lhf*Djx;nd<(SrguN8oQfqr`89tB?OnBfS2lzKoT
z2;}ylFAm+SR9<H-d4s=7BXyjnzmDIPqH-oS-CNqVkw0DuWJ(dD8Zim>F>ZY1^U|Z_
zs!f3CfUw2H)@2f;^^}uPv$LG0bfbW@LRiH8Fa|-jy;!jS;V%0tmpL(;s<T-SThZsJ
zCB1xEp}`{TY0A@$c+hb)D=-}ZvtmpB&0yFm!L_7}28jA>`o~qX5Pp5jgasaFTnC|s
z<_>YWpeY014NIBeW;NII`$V)05~xPWVSp#?;Xuflx;-!OigeIk0(o>yD>(CuA-(5U
zj8xlHFRG=LLnY3&>=wZ(M0icacEGUCwUgkpFR&=T#JAp+Spjuyl=n*EdTy%N$DGfI
z4vs@&r7LReAE2C^sZp-3>8?<?nFy1R>e9_!KjZh_P~+8>HR!S2@o@dK(C_Zy+glPd
zzxKCqaOY<ih~CNh#I8oIoPMD@Sjg*)X1IgnLyfb*+K<8OD2+y&5~h10BLkuj7?5D4
zZGivmikb-C9848U>XUlt<?jSnm+M7M`DY5muNW$l3Mryo=d65g%vVBY9KleGpG&)a
zDdDNApYa{&P$J4_C(D8v<7_9Vqsm+`e8|t?3&@yk6TBTKnYihs9p}w%_~RpZ1icCU
zLmPt&(3JSw1tnN(#jpNwl5<-8jbS`4pKX3ey$<Dt{_~H&n|v3|{@v$H&$D-4UmR#h
zzu>sZdkJ4L%;26itb({*<N3I)W5X{~DprGY;<35U<4NNq8iYUcd@A<?`S7*vr#>`u
zK^v*(v!KP9mfo&cNW&Q*Fum(N*{uFDv1=cbP)-L6*t|MkRcO(ojVLs)x@6qoU3-EW
zPA>^o?m+QI5>MNU>SAHhOX^)>hW9X~>e@&9^2^tyPv+dYzdTsaxK%4pETHyObL5+>
z;5+N!<n}K7MFniKSUK(P{U!*xEsSPj4@H9&>Qg(Al}CS(C2k+CIz6@fVSw(>m7rr1
ze=5Lai@(~7-L++}VkuYu(Bot;lrP*v$MkUvbdGZO2m9QHs2~<52K7#}kJAyoihLnw
zBYGv14w#O`$<>XFTTiaNQ?Y=BJ84FzC~xIZX`7o08r^NKqH2q8&m{54{ASwR9iy`p
z)<q6MsLE+AmDR(-6Rk(m6h6bo><|k5A8q%3i{8m<QeGZ<*<8NAysu5=UZj!N*lJOJ
zK_o3%JbPhC;XvgzYEk|BPGUy+$n;qE5Mq|{`e!jq7?&z8;zPUEN1sGLiXfh0r|lwS
zL-*==7@{j_v?DO0Df<O>t)4Sz?KKj$Q!+G3M7+HBgtaHSAxe%-b>*Dkk~fY;khW}9
z4nI@M@4QFju`aV=!~x0TdwPKWRj9;GyG-%0TE1(1g8~AJVSd(m&dJyJ*VTEUCD#tz
znIpb5?Cf}DCzAzt5<4SHRt?TNj=>1|+)!ofc;DaePQ{>2`so~!QcqIZkCa3ZVxfxS
z6{O`Lt^B9=Lfz?&bg2&3S4vxoulr8t0*+l8<b`dKEYohqjhY=c20H@ikQ$Y})-SGN
zwALb#?OSlp1O?8FjK__J-M74`MQy6`Vs>F+ffh?O6S1f1)V|0=b&d52LvH=k^ty|Z
z8iWr-+<op+M;*j7^28;jzx`EDe}E*mRa$G-=rx@ywW@#LMKANjU%v_^Q%C!1y(nFr
zzgv+y!AE^-xbC*_7j-=9aJ5Mc=@ykXWGZ>sA55%kc^(o?+TGplS~X;uV>6h{Vv8?*
zf*Q-4pOuv?X1bj2CvhHr0fu;xZ!b({H(?$r82>x!U~Il8gmF~*p~S#Kku@pzNT&q}
z1kPW!{s}6bb7!>1U)Cewstd1w1R2hbwJ1omO4nPhj5lY(JZt&LH*p!A4sU7?(UDq0
z%^e5ag(*@<m$2nDE`7N@m7&b!aRN>2duQ-E#mTDg;%ZcV#()zeS;w0t>c@f=qT(pM
zb=8;mSft)ig)_iu)}eFvH}RAUtg(x(?U6P9dKHVou7#QU=AWxh_QX>!^F7wy>+l*r
zZ(7|gOky<Ge57Vj8e;?7eS=*1Ws6m03)?n;KvsIIZ3dx^mb9{7o##lMZ16iCqxcZR
z#15<4nKMJ!B=~NrIdvkP)L4322OY{U(>)<Io0t;KVpB3o=ixv8?~Y`#T||YrX9YXb
zHZf(Gk34kqebK9I2TN^l)})_ByvN_G*?2q;cW}Z=H_UWQ%YX4zkY!f(+M;!<?xduh
zQ6?eDpH`iVnseZtev@|8A037+Su8OOkXe-6MX~o<+qz-Rw&v%RPG|OfsH{b<ZkHG)
z?QIyDbz6|=^qy~f$<3x7v~n<)!9Oxm8bXb;8D=&WNM|q%`n`+tStS<m(azL)J#KoL
z$?x^-r-b(Y(<*-lC$tsA#Upy$N!ypPnc-6T^N{?wJhhZ|OJ84KXX>0^UBDHBJc6?E
zc(nRi%$M1p9p~~H>5(@QCmub#gd^T+p+U}vWr*DIrWEBc-G!{t&?6zCyjS`rEpZuT
ztE%fq+|_AQ?nN#at#D83y_^fMGqlv7EUsJ>{-kg~x_(ken<Kz+G+dO@yO;SSlFW&@
z#*WW%Ma!T5&2o@*n^SK)&!+SdP7^viDNH@=^W0D6p~sJ_`m*=dP*Q!hrfgQ0Px|f<
zyiP}%Cu;KNgy^zWd5p8o!S$h<5UnI^o%c#mj@b!#MI!Z0w)Z=!>u*w6*W}(K>&?V|
zHo>1*_`h<0wJsq0<jlo3ustKob=v%Oel<<+dr0U$`2N3sadPgjZ*pH#<+Z*HzH*6f
z0gZxR!~No#rv)XyO#%)3^Uj`dYj{JW{o>v^Tfm4Q)z1wPSw1@jhHbjuD;_(U<u@pp
zR%T-K*l~i-xfbJJVbXJ!?N5D9IISwpSzoW-TfhB+<zD6f1Z{twM2SkNv{qU3M?dFy
zXgaZ`q|6QUD06E|HA~8t?!aLrBIWR%(aCW~MEqP|9&=<Dd49r&fT|BZ19pfOtD3rr
z_GORM*7C2Pv7hq=VL|#nHdLHPBizuE^iKmyrgj?M>FTe&Ytu%xudlgaPZBo+R<Wo4
zyaFq6^9D)nY)m%ifcqb;4cd`YQ7zAZy1ir)G;!9@se3-0!5T3vK&F88hbHmg4&t>^
z4R2kYgN?bOJy)*_lG{S=Ctqxy)Ckj*wKYK8L`~Z2=(LPha69>aDbJ(li^urLs5Uq4
zANv+6r}9Y0Jl*DXbtWZqI!}h76>^b{v-H=IFU4u@$~<6HYi029lZ2bxBXKj-DaZ32
zdSqC68s9j&+FH}Pv^{-50y5oO#I@fhg+8eRLh4u~=z9B>{@mRz8Lpj2Gv#*-8C{LS
zEHh|)Wt4t^<)D76@bdZ6$C&9u{$t_s7Hk>j5btyl5SUcNe4@T&I9p!%&F=;T68-+$
zw{LhyOUl?t7v8TnZ*LM39reIV<A_`0zO~UsAsk|P!HB5UTFt_g*{Xha7~&ix^<&aN
za<L<A38G}}f~ba%dunj@^*bIrn$nS1!ov(_R^)K6p70Oa3-^4tQz-IY{4818<!_JH
zZo59t_oZQCp~$Xp-YvDoVe*~{r&6=N1ig@UORQl&saB@>aA07j>e2O*#dO1xsnt)X
z)BxqRvBiRhx+-9t{D?zCU-7G)`G~@rY445NbTk`#*}9|n>p?g8?PHkIX&}27A8Pf>
zQO)`?jlYIHRPg@t2M^^LYp~Ca8UC6tBr^17_t`l}C#Rs4)bByg^&h?tRXB85(RmWV
z&r)@X+9q4<g>0^_n~#re_Ig{zV77TgEuK$+TMH--mU6vUO5Dov(=|po1ZLn83=z%G
z$?A~rZq<b=)J7*m&!+4znV+Hq^3xc8#opg+D8FN7k;8y+>6H?4>97`RSO30O%bL4^
ze!4EsQ|kKUx)`zf2T6lcSvmBiWcq6OVYc6Jhq{W+<6_}15X|Z~5?G#do!ngeke+^s
zGG(~vbsas%;yvH?36WQQAn|&TkN&OzgS6FD@Q_cB)A#Cb8@L~<p4kJdW4^JGG@iRV
z9q3e~fiXU+AA`$Uuv3#Xf<P+!bxS5Qz;)ZD(RYjT8rM5nlxAnjmi41y67y%GUApJO
z8CJvMrz(AZteI?%8?^1HTj3?{1<6vQ>}IFrn>xB#L6W}_06<{HlMXsbwRvJX`$Ouq
zpby3JGu(3bp`q8@;rU6%*IiA#`fPc4wqyP2p<k!+MKZ1;AEo&;ECgW%-YCQ07kZ|W
zSQ@PM8m|fV;%G`1TNYAI)n8g5YY~b1CR-K$&2AeGX*rqvN=YrdovT;rkru_@r2UPt
z8G+KUm8RZcj|Ws=#4s14+s<N}JR(2B^j%j3PUyP1*S~DhW2Z5ui-S%kfx)Hlmk*)i
z^1eY73r^NT`v(TtC)h*JCBd1;_v;KHoxTFV1(wy}-|SNE8G9$;YM3lRs^`n<XqY;a
z&Mi=G5>;J*0JosHC!}feIS>0jgoAph{3B+eK0~#GC8-sh6(mjti?vO**8V1ki$C#Y
zk<Z!tfZi>zjq1vfbFO+~(cHwI*guet3h-oza<ow&`rb6+FBP|?#8G7alwsc@u}Fg~
z$C?Tokzs7k?DSbJ<%oPpr`&R}-}o{M(LxOQv-e>r=Ub+5^y0EM>#yWf#gxcfer+so
zr2r(sg>82fafgNpCF$qb3Vhazda(2~(bqhwE<uk9xJA~XrjoX-sukHj`=WE}ZyTVc
zf*K{`uZ3I+PBLb71KCjLl;1yEE?Fh+zW{-x4}Sivy#3?Hk0Edxb-l3qkh7i-9mg%4
z9fO-WKS!Py5srasw=JMyJoMzs)<}o|es7@lz0wvefYf#=6tLdUcZG@cHmSx11C_c3
zcQcVOKY385Ki9=QJo}U2f;KfOvXpu4@oGVIx#h+0*Z0(0c1+HvD+ZwN2V6ll)L_L%
zhE=A$*r@yW6{;?ht!HSzP844^)ABzy8~L5SA=Q8D=;S%kp(nu)ipo+i9zkh2M1@hJ
zbhQ%Kw?gJ5p7!4SC2RCLyP7FTe)x%*KJMxDH<PXwfG8ZoC*&t&yGxbczBsd{FB*}%
zt;O(B5nO&_jVEf%omy~a)AH1=?;~J6l=cTL(Js6ne#?=7Z*=fUxrF<s67^5&!bfPE
zqiX1zpxyk8^2Pp?LxEWW0vbm2pm<W@9YfC|Q?K^i$4g!J_o<)!r~%b`>vKm*-fI$R
z6Ay(VSuXENolk#A79?uUc=3pX5iK=$n{DxJ5t4nfDT@njOG@mk;GEH&C-Z8uyw4$l
zXK#RhYh(2Ic+y+*58SU|jz<d1_7$*bb?gwawEW5o!TI_zrXa|Gm+gr8gre1W!>H)0
zBXt31)gc_KQu9;ybsZGL`ohZs0zvRbL`H^;ej*oHt6F}7-!Y(PW+vZ6S5$^97t+ql
zQgJeYgl@+>YK&+>6+Q~-mlA|DPJ}>vimK%k?hsPBfV#OCXInu;qizVuY<|hpX!<kq
z5EVZjYNju;^!L4b-0BRtcr&#+BWCx*4hHFe)|F_ct!8%%q;ph|S>e3tG+dt#<sN@9
zd{8<zXV4T(6ZW2O_--7w_kOYbL2$xl;8o{&Rj;=xL9JX|KAR8P9CT~9S~ImQTI5cC
zdTHs&OWiNcsjvQh<E=gP@CF@L2qdlHPkk};;C+2JTR_gB4qnvkzxo~PB>K)ys^miO
z6Pz96n~}g}`$@n*XUj5a<cOVoGEtGglPDW*TeO!gOC`$L!jjkuAq1}%;8NG8GzyB1
z5s#J;>jJxYV{TQd&=X?LYdY<5{TLz>`<fe$8(D)!T?+1L;%%m#w#4B5^0lreA^o!}
z{SDiUtd{XIRa{<9eYYP00%jQc>cEngBLx;OCJ{3!E2<ZH`In2x*yH#8D;z}RdDTjJ
z)1xy764>_Ekw9}o8iLT<CEqjFXDDAz{e6D7VL7;OR<?zo*R<J_^wcM2YkIPH7Dts&
z39c!n_6KX*J#|F8#(~3VYXV8%^0!6XV;dO)<^C))=@2$r@g!a*ob1S;`*1f?{|o&V
zGwZfA*~>7LcP)gTD!NN`dDvATQ^ZU8=z7YQXsI#By4ih6&Q4<^-^XK0=tV(zjL_<v
z7J=z=rk}Ho>hc%T6hWOs`@yGA&OZZcIlQz{9%2f3^RruZ;_u(v^kxs738geeoCuX0
zb!2>#!{7WTCD%`;l<>Z|^>tuxL+y)Oe$q#~leUj7<wIJE?BGQ_OL@h9_t*&HKpYvo
zX-Sy_BbE8czxo@)F&Os<*nLsIHy7m7WcrIVHIHMb{B1toIB~&4&$jEB7MjTZ=B5ql
zg<r#+o=}W7biymA$#NsMq!}8Galc7IBzep49Ko4Pw%Rsv`%3HOU4iGM?OM7m%w)S-
zgV+bca~yuRXY&V}4InpyVg3e09)xGsEq)UMeUQ1xiV+qPk(%J>9nw-V?~wiMq1oUm
zp^965skv9f5qp$nQq07d;fBtgu2jZLM;{#!3NcB2h!bTmy!dTi%9+#p#TRA@SQV|U
z>A8JKC#*#6s9HHF<DiGl<A!$ZXl6lV_HTDd*1txUGa>8hk8Vd<z+4faaFW;2U?#W*
z`eNRh*t!6)tFsZ{`zoTnJr$lCRpiQNQ8(VEj$M?N)}Ss%^EdsPe964m`hsxrSMTLJ
z@qxIAc*(#{9d4dh9mVyQqBjxG6T7Rfz?hT1x#(_xv)4<@Li$q+Em2m)^JbOReyM_=
zl#QWEYxvxYt7Br%yU43Uw@CyO`#CsLbuUi_>U*X0!i#pallJ-^x^s94(}`$|AjbCd
zuUVek{z$OSmp~HIEn6QR7_ju@HkV|LEKa1&eV9R{;08T>%-INpxSbv`e9JW74ttDo
zl3n~!ddgYEmM{BAgy!Ln^0`Wf=vtC&frleAdK4$?<x`^uo2SMTYQsd8GDdSbo~$YT
zUy}Hah5FeadjaAf*W>{n%?@2X2fT_5+)7Sut#h8a+;ZFqzsKSm79}c$kshmey%u@U
zI%<th!t?(t=YdM$lRSb@pyt|IIYfv?;jy%1O7IhXNk&70N^!&Wh5NT;liTQT+W4pw
zFFIQfE#~v7tj`luETj-M_fvdJDXrq0TW(I9yw&v?^T4#rQSy8VRq54z`kGq7YC{(0
zs=;5<=a=;EnkOHvx7Yze6(NS#G1PFs2h0tJ6ZG}LW+qX|S`~h_wqAD^t>MDa&pQ^5
zQ~voK1Q9~%h)2AVQqPuA#03V(ceE7g<tMEY>facJsXq~Yvus^P89I@o^yr7bi@^3B
z6}rm8gi$f~d=_4a2+2|+=8MM>#XwmflgE%sS^mZbQ%#RWEDISV?J9C!G#fplW%k}O
zd27KtHElc8os?i67yg<cn@voNVUsR^_`5H=q!ej4@A;a{rdaxjA8tz|9631Oz$iSh
zYrc?I4Z=8d2q5UQa9Huq1Ha!-*Za9`p%^weisz7gzUs38Ap<t$R!EG$Bgxu0HHlcn
zD^iif%k<Erx%fy$QXRdFu9PHYlywO8^V8KO_uik5s4_F#r2pzI1~H&2%PF|ZXI-A5
z=rnO}V##N1|CCT-g(lB6=oRv$VbZCKQ}7zJYtHwq?_OHxxEgzOhdY>RZXcs-d)UJl
zZdBx?n?%4F5h8kICxvgy-dAlll^~p^{IVo>5Rdv5O4G{wu21@I4gLw?w?CKK=HKu?
z9Qh@insY}^L@pmUUBwmT%0;p<S9RWk7n#vfc<-O_@TW}^2&-Z4!pyNVN_4rCUEldo
z3N1iGj&zWYdG8;`3^=VyOqsm2?z^=O{;!S($c!odOhvKGW-H?zP!-&ql#5)d>Rh&`
z5)>0_SeEvhMF;<Id6KkoBGUy(cJ)?--+c(>Ckkh1B#LtMNiv-qJ=Rc77nr?+d@p%@
z&h77+`sJ6^v<lO0S+745U#93!!s6<h*d};wfvI165R*Eok5&#78+CcoR#Tn|qzja9
z$aNIJ0<_Y>)tiy)P$n-m4QD9VT4$sP+Ibb3Nm3Hr9pQ-Vl^y6f7K~q*%Ux!Um#MhU
zH=CM97Vc;DVc&nI>^Q$_+Fa{y3+G{3Q<55vBx!)~lN%xo;B;9Gl;#wIf_PE=5obP{
zCWV=4Eo8f5*5FT&&_Kbl0W0DABPS=wL>>l_+gM6zMXZr=L+~pWZiom{`RNrsY?H36
zWw7oA-&<F<-f1TJ)UcoHG(fNqEShXlr;6SgN#OCz{aLpX=LIUxqTcBKM);;_ywpPO
z@;gmumRvR0rwuHvT=x|CXS#E-<fvfD@(f2B6f^5vJT=EO!9~?U)%NTkV*e=Z(&bMt
ze`ZhILft+5J@@3_v+PScT8o!>k#V%0Dtr9IMY$nTJEZak$QJsLQaX={CS|%BWn8(6
zMpL1r!|%hZ3vJ{g4E}x9${nvVQlBE|(giRq>x>bt_kIziB?{26R604n2dIJL(jK6m
z$!bB~+^j|x4J^?ww`#L>TimlW?*_cbuq5dy(J3^wgr0&JfrP#x1~$G<#>SZ9Xa*eE
zH29a*K<_FG23w?js85LXXXk28mltLgMoxo}Q7_LZwiuujFSB_0_?g;v49<;46H?IH
zu#$?*yAOulFWdi7c>5`a2*UJ$2mNJ!Obok?B+?0O=ZQt|a051DP5Q^}b>M2;P_!J*
zBc_HUE-zRhX=~p3{o51ol`eXexlDJd-l)m?$aS6RWj(8_Lu;6~y!xkB>^H!zSAGG`
z?V@E|nY2E9(@RaBva#T;NipAs!#D6XAf1<xAK^aLefECKZz<4_<?BUCn<K7cV%vIt
zlFlUgBp#}tqMtCj_dg(BXSsQ_4|sxY-?`8?T}Th#T*@4~4HC%_<_Sk^{qI|VECc5R
z2!qi6?&yYzSyW5Ir1BW`TYC8iBn{W3Q}qsmw9Jaa_c=z=GFtBBASabJUTRfGHud(z
z{6WzvGIHCm^teI0XNG<(M<;$r+#+H<>8E&)Z~w42ef4FKg+h4{+OwEawud9X^#J=C
z;@(UD2#|PNx#cnerQl>y<B!K?exItl_bZt8HDkiOuRvyeN%9Y&!kxE{)DjHI8$5<M
z%AUlxdtzAE7Xoz%Zu-IpMY)yUt>Pnu61?;2rN2<z)?m(BbeC=C>i5TjPkJrQbnyXN
zZt+sgsz-kHN16pJJm6e`!+~&;R^3l<KCdN7W2K=hy#8F$NPc9`MrwHEBls?mP_^rM
zF(^Mgx%4rWNGs{@@kCzjjsi4VGwb+9bX_M2y#vL=-4~O~>8tTijjZmn{ILJRv%>|c
z<gPu^T_11xd58>0Q_^jbK*xMGP#9T)RD^sz=7({XeucHuOO)|YSa*D5b*S5OuLh%M
z7g04A`Q5LnhGSAxUmHJE%0&0wG|Ib?VCpE~Z=KS}TNe_yh9=N>r_V&kp44>MQL#Hj
z79j8;4fGE8RO@)&@COfX<xMg!lLk9*wBg%(a$fU~yI1XBf)nEeU$0A0qk3MYp8Dm=
zOfI$fw6G9EL@uQb5`wFDe7f5I=%VC?$rz4T`%bM704j1r;9m%d;_=tnt@E3RiElpP
z?Qb#hn$U%@7|4vh(d+L=i?;7RZs%3A45wGARtSt-i9O0_{7LwU9(rmRA}sw!c~5|L
zhxykBCyl3@C%AXw3U+-rq6jHKERvoa`RjQ`kYKWV&;vj6v15-c#mfXzp?S^%#&oV}
zLP=yj#=}T$<f8(w>RbNFT`@>y(RCnj_)ll7oJlEFT+Wq{ywXqUO-^ttnBJ%LGn=fX
z-iPi_6VKe0>b#_nYz5Eitso)*2W}K&JhD3E_k&t*E}5w=Q>%K(acj_OhCX8x1g2I#
z0cDkFs?|X9WgOVPPIZD#SWOJ0wOu*}Vjj@`L4$Y;muqfCG7m=1OvzjD+vrY%T^)lD
z4V6~J_vc#=rZBC6bvIp^b#6>hV>XTfcem>UsX7a2Z|74H>Wr~-ej5mQ|J1TO5JKY;
zi|Ij|NcV}VVx%_P$YOb6O)^=~)op#1tk>gtD~jmOQq97f1MkUF=T8LnQ=1lKeUsXW
zXzYQZp>zb;N~TSlK|cx#36O_2atNOLMUmpqH*PC=(U6D$2I?gFDH4M%qlfsm5B2*6
z{Q+Hk(!<=e&UHF=29OJUxz|&KgYNLTCWw}!XgL#NUsygXXCZ=!(18lM7cVN(FFEZg
z9CehAM8PFV3@?D|zh#8<hrD@!pUJH-NiaMWi_NH7+y&pRyS3$fk*vmkTaL;?P*PLx
zS|I0GPoW&3dg-4*;l3mzB^7x1@Sosg)}I}u`9P)aG)3NyBHP8@9^AYP9Z;iN{hEsq
zC8-%r`SQsJEeA69Y8ZAXmzpyTO~}2;j>nc$(Sp--$$dA1BAdpYF_$BeuA%1>70IF!
zhzxWkcb9LwzKa*fxpT)(wi_TWP(LU#-u;v-(V(&?&Kab1WAY9lA(d8f3Iu$x03DX>
z<F3=xriRw43X?HvjLa{s(f51yeGn&+d%XJdI?)~SO&Hz8nbSg=2f2?@g<YRLrDl_~
z0H<#D0!VXI2^sHi+^fe+!}Ngn?tPRPgVN^Rf%Of(b<a8*V)rs&9XS4F3G>LR*bu-y
zG=5Zl(cN51pU7GI?a9lJ7x(NBm=70=-VV@WSx?)(W@Y4~$1|JYOI*($u^RM&*Qvaf
za+@nuh8uI=VxVFr6j5I-o?l%1c+W41d1$MkbmzB+3eWkh4zXrK?n464xv#cMX<8KW
zIs22dm1Z#wW&<Tv-1?F7JUuBkW#nQ@;&i&BQ)#fm11mOPPR5$EbWFLNUFz(&^AO07
zTjbpqKQDmWuqhVoRt^fqhIq-#H0HH9$eo}*1Srp3{mj@-k1TDU@d3h5SsZD?mLn|(
zdRMR@H4I$~Yg*M4s>iZ+v)C$PaFccidjsB_BHopuAwcSvPYsTCU&-P;`JS-1e(S{A
zOvCg!vz&(!YmvM`?w(6R2(;fxAIsV%Njr1%^DPhp)>ayPq*Z1v(XjPjz5Zz)i>#j8
z>>pFUZ@qf;v9;SV-NvSDEPDP#NJP3q_h+Z4q#3vjkxm%7?ITbjn%G)M&^a>DVa+K{
zz9#EOXyYDnji<~3^H~{?apwM9SNcOXvf$DP^38ebJ9tnMy?%1s@nt19(T(aJ?1EO4
zY~0pP=k8F*)1eu`=!6C;atr=z@ayZwIVASw9uE?kJ>l4C1@T{Va^Gf=uDYNg<4oDA
zwX|ziyON4_Jb0KG?5@(g^P4nk^6ceR-8X4sibenjk3<cE$X$=aeX`Lz@<1~^0P+Cw
z#U=oVRyNT-unK2OR@vFr4YL~yUGF#m>J&Kehu?={e3%Zp?v)AqRDQ{IC;b!4fJ3bo
z0?(zhK<W-efy4&eITIxloKDJ>xnc$Dyk%+|k0qmiH8}E8+|5r0Jr*#xOKKtI0J<1M
zdDv>wV;eJt!=}8K%uX}ltT?Hc43}QJ?-D)8b#WY1Dn~H@Ipl&I#jG1-C7PZb{*JpL
zxNo5&;t8)2lk$-(=Mkw^Dqx$vskoo>t~S)+C#t6#_mcq-E^uy|@fI#WvbL48+<0N!
zT&Z7wXN8OphPbIj9L`{%TQKQ^FBw;f73ybL#=K}^s+`F47O1MqA^v@Hb1d!P+X1Ke
z_c+X5D8|9BNsx9m#fV!cK1Op%dK3|<fvVTmD8B_hgp<ObwXw7{2KzlF$yLT=(6Ze0
z#E)yT+?%WOS2vhCAm2{6adjA@JL`8^P1rgPgWn}ao}Bxm8|;7veAqmKCBmdIr*VQx
zboGUfYWb>wpVbtdnFUNf254LMxi!c}__Fd!Y==P704FNoqjFGtS&cwP+`c1|#YcO7
zGLIyx$3p9olG$z>>_VK9%?-ylkm8*$1deUwpQvz>x)+_<Ze*!_my?BKty$Oi3689F
zJzQELOGYgSsK;k+8kh{@t~{Bl@C<%QS_?V*Z>Ik9U$keT=1&E@6}IAUj1{bXbCiHU
zK2yLDra;7f#>`JYYMrYp9&9MDA$Hqw<&Bia5_@5r_r>Z9<hGx+K=rC#lrk{IW?8L&
zcYYYWc1Am20n4kFlqg<^p?St;74g!|9hQPMS3l@x&^mP?U%`AmFQOfK$u3tIP&!);
zSaT3d_qfSz^PL@DLCN!kM1dej=DSb5ryQq0^k4s3gO@_J3?Z706gb|g<aC56&g77S
zQx+B(oU&Q>j7;}Zlr$j(AE@@BK%*0Tq;@4w67-Vi;;1QKk>ReSpbFM`M>!-RhX48P
z(U{1{LVW7G?_DgZR$mgsMrA3R$62)Q&db^!6`RqPSf!q>1%_C`Wm|9%_ywVkAeteM
zVe9hcMDEFkp-Cn0(;7lsZJ!)23VyO=n<+S{xBQiwmvkcbz9gfC<)t+l<FT3DXa1zi
zJ*~6I$_LVbgLB29t{w8u?E{pnjPJDY2F7*<Ig;^dgC|N^uWSoTX`7Svbego_{?AB?
zYXhE_+D_qy7CUEj{x-Y8MpcD<G1Sk}Z`?he7X8gZ8QT|~fNnT0Hv3~`o|xhNk;(r2
zXiqDs#O<PC8~6T_ou76TKmfO7Kn&*$@NA~y&j&qS2wrFMqC`Vo?UXLS#mc0NFqff|
zMMBF#r79i23Y4Wk-U?F0rN5x40frj>XQ-T3u(<licG`<|#W(qwuhs37Iv<iW)X!%E
znE2zkGdFhIh;~gjk2T8|knfgughck0mz9@GeR}rG6(sdLO`qHE$4uZiW=_5)-8#Cy
z_A`YbMcX#8g}&-sWlX`o51SAbS<R8p-q?zr7;gL5@bB~v!pBh3c$r&Quv45t-^*|5
z5G%7$3A(=C<gL-nt=NdiN_(^!_0D)P1*OC=mX(I_XaS8*C#>sFtesBmbtYkhO4}=!
z5lrRETbVTm&aMrzYMmAl%rxY-KP+n_4#!i*0<gCSQb^NR<IEHKb>PM3FD&=>40ti5
zeB`!SXi5d_7qN27!wZPp#pCvY(hr+_I6MM~#`Su{P9r#h7ib~0du#<S^_(?x;=VQS
z3EQN->38WPhEswDtRB}9DTBzMOo1?%W8&XN&A`CV7^a^ix1|HP7ieMOAM{PZbSaL`
z1tr=Oy?g>>PIp}pu~G6XR37GZZAAF{;wwY%{g_5^>$!J==*;vxheO!xFEvkSz;DaC
zCXnrYMv8)E6gm6>gE!Fbxi*`O9-_x(`6DZyT+D61d1cGld;X8BUd8-m6^a_fynz>&
zEOyvtzsfz624t8xnyw&$b}6p`$cXQ2tFvOIu2-ax<JolTmndlo(!=@iSz?d~WG%8=
zXLevtpUF~gPnxS%sqWU>U7|<E<$%P~`;c2>{RfwH^pj&*6(A_LM+{<E@?SKv@O-5Y
znoP|T(=#a~WAf-?A(k`!)a=!y+0!2g^LwFHO-kK=!t3sLn9!cSE`e^C&kzO~#Vx-F
zG592wmHb8f=kQiX-8QM8cqDDOwD+@4>zY7CuDkh`y?Mw}Kcs~8r@XTeIdT%9RfB%?
zzp?)-jE<y6bNYu%9nvKr862|sUEu#nByE(xJ={I;#>5IJ+(e~^USes`0tMPwx9{UN
z>zmgSQ4fSj8uL|H8*`NLx30)na9_{p;pTxkyZ{<3G@*i+GV^YZYfnz@RY#Y6wbhRn
zBrD2;?}(c}+2jqpJWHHsQWm)#MB4N9VvWi;<rTzviQ;z$`<h$nEe!DW4Nqbq`jEZc
zg8xA&Gr(>LDD~@H>=-_m8P=QMofs5U(v;HwKL+P~-A_0NokWAZ{M@QnWJmPWC{&(r
zzX)-XjAt?^aNIF_?en|L&lHb(@PbMn452DHWd#E1JzF{O+aRyxN%~7gk5R*ocYhMD
z$>N>8@Upsp`LU?p0HS*hZT|3SA+Dq7Z69kFZjd3XC)xwBH)Y(ReWJ{vhAp)F<(Wnc
z+}wF;W;Yb55i^j#dEf^T=z{iHW7Yc}MVDQ?74%~9CofQS|2V87zt5f~y&`tkq%cGf
zP=V&2B$?e!rmx<mp-dMR<!@a_*=;jjX2dREax2nD1oWuCJx{<aUiLD0u4|T5>-4Z%
zWNku5l}^<DHHQWn5TGRKBg#)<e6Qr<nc;5jttvZhV04^ZJ1EMBBgi|c^ZG54@DEB<
z+RML_u_%?6<<=x!AGg%NW-c$2&Im(L-%0*wG$^O<#7=4@kx_}PMo1bB-8xE@i%{$9
z`U?S-Rkg|Ad*)$9GLKF?LK5E{sE;717oD?P8#U<>E~I3unb`&tR>;TeKe<aK)i0UX
zPiht{217U*X;Gr7+STd@V>D&iO}`lc_*yhb|FJu=X{G+RJ0o?_^NfwB>7N#PQr{qe
zY|NkcaaTF4h~s*=hkB8dQ?ltD!S=T7+lfRHl7mQsJP~>X`5SDHHcN)dk<hQoi<5Co
zd-lSpM&5Lje@Tp!R1zQjc9ebKK}YG?S^ePvkDGr?qptZHNaE+c&m{W_yMEiOS9Za7
zTf!9jPv`v4*vB~n!_uE=nGL{&|KV&x{i9DRaB{Qc?TsNQKpM_5OE@<p(XNWenrkH%
zS3exg*LT}LVzb5H?Mlf6CGBUDEVw!2!+vfdD(`xUp<b1WTkrd=4~owdnhr1#3s?)#
zvA<H><5i{nVDik*u7jUTBjdC>Yt$uB|G(%<L`a8d8H1j!4ka#e=}j=-?Wy^YHtx61
z8VWO2=TGu&VM_hn@Y}-~?w}(c-Yf39y?MXL2X=COWwqluR;4;(!Bb9KnnC$U&~alJ
z;2B^YmGQNRfqMX8X+X+_ViYjU71m-`9WghgkMCHw@wP9M!>MfiE**`VGr0d0m)00L
z*f%swB!Yy(&;31PSNp72XJ<&8@TU473Gc2A1l89vWzzhY%dC+f8B(zJ139E=tI4iL
z-3sn*TnW>3Ff)(Kpi?nMW0LRssR05f2CWvuTt0_{6c^><0_BZE-~Q0QK@lCFESDuw
zqi-uV6_6GHm}|%Vr%k%}d_-#*D^Hv$-0&D<0j(AHiv3-tg6H|?$X%ywAO-*~gzR5%
zkjJ?Q^ikNx2i`-ZJOtkvbSM5Wt#k$OFrZ)x)~=2i)BF2CJywUgeds&pEivz1iEcat
zl9EqG5u0#d|L#jiuQY<ArfE0KaVKppM6`M;J@P0D9(m|j_1DJMi)Am*&KR6ZUcMKa
zGE{bxib>tNx3@9a%bmSpdV9HGe_zBK@?hQG0VvUS+L_8h$I9~N*rGd)qq5v8I3tgN
zc}hZ0b>rU?4WSoCxVnC(c&sam=rBu?sGraHzw?y(?ik<W7y`xAA#I3UN)xt*?{l(=
za(vp=3=7l-<Wy=5%7lDMlyzoL9)4a#F_inmb*g(8ZFcwcB}Gl+n_`bf!bZdY-u&nV
z4<kRL2jnDRSkV>mz-|L3tCQjn!98zTj0^QAl|nvI-u}$hMpR?)?k?vM@r2JKQPd+g
zuZk1eS|ya!9|X<-y$wSC{T-%&3Pw7I&nCs3>Kku-o%-K%R#OW|*~&A)@}ibF#n&@)
z+i#aLT*3+NScLuN=TY6*R}RU`%3O@+PaLKK(-<wg5I6~;Yv}KqF>zbe^ot;`cFLH~
z6W5kq0-dAJ^KPi-UqT+~Wfkryj%<)Rum*Cj^~w@uW!PWSDFi>xx%(T7dK5<|*Yr#c
zn&9J+g%_TS^zQKIY>OZ}x+4f4hwC!|B@aQr{MYhV{0P^lgwy6JOABnceur$Q-j4Xc
zjC{@`G8Ib&os6ZYWzAfZlfI^iC9i)w*B(lp)nuoivIP|G$vrze-TBo5d;=J3opmO)
z5ZDCl8;S`LUCkF9d%7v!k`{HK4b1;2A3htHDLtwDbl19?KjE}k@J*_d2KNXML58<@
zU0DD}r9)}exxLp7v0B%4c`^Q;4~o0kvwhfG$r~#5#qzGh=r5f0RtL=++*a0Rde3*y
zj-{pWN)iePWSfYjh73UcnT0o%d)CluIlrul=8(k{z&iT_P_admv9J)mQ*aF7OyJ?J
zm=fw2&a~E$U4Sp|#r_}$!_zdMY08`}pOHex*+fuwk8AIOn)yS)n5!sjXn$4+Nsy$>
z0^u%<tj653l4tLYTw9_?#$!p}1X0<tIHbhheLd}S2qc2_B<G@*1WmK{sP(44FPV0r
zgnD?mXv}5q5d1+uCT&3Mi?~SY#%~|cD85VtUfl@MEvQmXL5VjuPNlrTZKpVr943f@
zDhLuv@h^gh*ynUX)qk9g#W9r^+QLc%9o$a^z*&NR^PjTWjU7aF*o4y9@zx$^(|S7c
z`@~xA6GBf~{{|mAIARvW6-4FX1KsG_dM~Qo&kEQ?&Y+#w`U%1Jm!GrjOcBAvO9c_8
zrW0VC@xk79u>bHQ*tFx!2<>OGNrx=uWw{`Sb^h|;*j`S8_WI-dzbu~Zohag^e;V15
z4(IsedFPSK|0b1a9w9S7nl_$}N6Wl5Zl8xmM{*y#6^#%|qU|-V2WLO}Ev71}jHx<T
zEy!(gH)$OnzZt4Cxjmh1S=X_6Kp+9nMYQ4#2S-CLTE3+%&c}#-<{>m-EWJvP^QsAh
zaGg<v(K((mG;h$*nRuKHpp;;>)513XjDTk~i$6;NBl^Q6#KvXWqbFpNj2{d4i?Vt7
zT-nC|X@#*9AB3Kg2-5Xi!HZpM#Dek`{UMyB8x-qyO}e6^Cf}XC2MLMc;+*wjpSAmM
zX05&YOrAJFFawyXOskcd@#x^#^dgudfP}IUJPs1>w*`v%KkTs8A<O4Pgm=o^t}*IF
z+*F+r3g%}j!-=b7NJ%_!NyDf>vo#v;?Q;lb3z!|?nOAC6{3D`Pzu!+z`HHD#O8RcC
zapOp`$hE?%J$V4USPt6VRD>@W+9h@LZ>0enCWmjBVkvS~1ZC-L7>_Q{Vej;aExtTI
zVW~uJ&2UU%JN6os+6e9cK4Cu_y|Dt{qSh+v<u+0UP2Pnf^`PG$7B|c;0vQzoktN_)
z7g@OBrpMN?t}XR{kslvFW!Y+N2c3AtpuuN62KsL?mc~?Gv=#yz7x~E7`&8@%QrqWT
zq#=pnq$D*k5|5Lz{|?T+%%)1FVxq*{xN~M$*vRkkfrP&gKKaDpF4O<}$gJx{FBGs9
z6iwe==lZ{Xwc27^Q20N$=lew)EIU9GbJWd*`-PPYLlD$X9vgeCb6rG+J)n8Wfg;7+
zo9X)V`TU5<Z$iV~URGm9&<SB)jpG{Q3_>+I+6yG`&lqZ!Afkofj8G!PDI!lM4vfsL
z8|(e?XUK1GR6im?`DbVQ<l`{K;|st6^Hl+xX@muwW!GyKhi?X*iAIbM4%xJOY2&iF
z23k_xN?GIyVk;4BpFUjt1m@v$E~E@R;rJIA2#AwY`}z_N=YJF%uYc-y-sQg4PnWF?
zt4)^nY!%8z9zr1f{a{ksyi+ac!V-vWa2FTU--4=TsGve3(FY~nq#S&0f_8!N0IYA*
zZJFuZZE5rQut>@N`F2^&O4e?Z;X{|P%ouxN=>}P*Hg;x^=v+C6h-Ah9OkD-e>#k-i
zh?IRUChog87@_&L*8yvX5T`sk=1YC_H;y1b2>oj|gWTm$CZdQutiG|G-_!pAmZ7}7
z_7?M?sa2gs=5SuOA{TP@Hs%7v;R9nk8yPV3(Zcp#%Q-O<YooQfxLZ#IV?a=d=1lVl
ztrt*_J++q!5pGjTo^u|#MmUdBRn`wD<8(@*eJgu5eGceFunL&a;1O>!xKLKRFGm18
z$X8OJXvzMvfRW0D<baW;25FvuJ-*TqQPsaGy4B2iLM%Kzo_lDk@K*Y=0RkyilkLeG
zi+P-e$>9;O9dkuqV)-|F-u|wg`8RnAWp=z+zb5}T<6mS9&tByQ#Zb7xwj2!;v2<3C
z6X%EHU;E&lC?KIlffDq;i2DFIT@sm(&}sxv!dCzN!twCEo{ug8|HniH3;)bcaweFI
z_TRH7`YyN3b~$L_9l2}id9T-&T2!$KI*9H9{Y}Yd{M2hb#7@2toz5Kt=aD>#K~SJz
z$mzBo0X^?Ov!W7={c<U{34_#`H@}L2`AjwOXV5lw^|chvrf+e;*8uhs?u*H&6$UQL
zox6Ov>VH_Ku8}S1w=?FvyMD2*TE*o7U-@>E@{%BMVG+s38<OBu$A7JlX>PWio~Ddc
z&Qo<h)Y;OP{9^KTp|{mj@sZ9cAajl)Zq~E}NP6Oy#k!)M`FtoWV=Q2_^v2jAH{D*X
zN4NO@yC=Tr+G;NG&?YAsS)Ig{EiywhYmh(wKwzRhAbINoexxb+Lf+pi_eH{rmxBTx
z40&#%Bdb!h96?QO?kzcT^md}x2Jtks=)**wc4GJ&Kh!^tXn)*OR=XB*X#)6$!X0+z
zj6!F*1jZa?Wjm?h9G3?oOBfHj3De2{y33t^x<aEci{PF_|1HQ)2s>Hp|3_a$SO@5!
zM`r7v3_-(6{opcV3u`w2(kn5FL{mkIbv`ZgGKaB{7-e&&e=uKTV$Qh`G^7YMJ&N4s
zDt<l|t=+pHRu{4-D(dF{!im4aGJbXL{+5repS%>p_s_LJG(gh&^?={(R?|eLt?kvh
zVG1A21qw@99s2$~7tpc%qil#I*ck)t4JGpua8O8m_gusXs}IElT%Oz#lAN>H+V8Sh
z?_G$RI90d#Q_TvkV_N;F=KN4``iaAqRdY*!3;GA!iS3b|fgl3g8A#_xD1VUt!IDrN
zkcWahR}&s<WrT=canzG{5xWJQU^gj#H1jtC38J;rbq^p<0C_A)QdF=2o%<e+5(X{a
zmFiZy0jO<kHkS{Gye|&p9NIpI%0aKaa!tKY&mru&@IUW>>M>>Svr{fkAEekfe>wm2
zv@JL2<SbYsBkQ#XC<-v`>va&qY=nmRTGvbDw?x5QNxvjk+dswZAC(N|T|DUkYR<;d
z5c>qRQQLpi>Yl6NCbfvp7gz~A9BnhuWD82}g~ZO`)?aU?1}7b$d3LoVMqFee)M{Rg
zZ{#WSrCOK&)c1p!L6)}JrA`82;Qdn5IV1u1;i$@_`+A2k8U%q95R%%z<B&qDFXgje
z&C3p<i2rivKg5aOQq_uYHbU)456dY%c{}C9(74g--HudUHbOsYcQd?*J?Ye`-x?dd
zl|B!qf_Ma?Htk*l4TqpM+zt%e`yh_HI7Gw>N;ZvuqE`8_hchwlj`?UY0+HeyWyC_w
z=zxkrM8rP6yFvWx5@@jlY~cnzBWn>4ZE=B(Lw@qvR=WJp>2S*Ni>36kqsd-0zseH&
z`ANC>j!Flag5Q-9>xfNW!KJbkZ8p;{qjoN+R?BzU7q}3*gf&KYv}71J`ywJyqfkFf
zt}q8qbLFq{dqKM3Q?!TcC2RLwKj@Af@h?J~|6d!ZKu`Cpww^=i#;+S9CuRK4K+ycB
z!kIA-#Ln5?iT}-5ka9=gWK&7G;O+MwuD9<QkniNtDmah!MRO?km90l$toCCkM8h#m
zg?*=dr|V^Nr;SeHoR2F|DuJj6)Z#0;DSo!77CbRn3<y(y{PhI*DX`!jVQwZ9^x%HD
z1l^|ivFG@iticnS7-jDKKZ5Elsd+hcM~YjK2E_~iRu#$zD;1J&7`{wDe(!Khf?#|O
z#_ism8+I6b3`MaCZHQHdwh|@&({%ZtcD|7O-^Sy&?<{J9Uy@>PTW|@O0?zP2*yBWN
zEHw|T>xr101Cz(Vjwyb?Bq1n6wdq+|(rR*`KFE~T=!@(H3_rr;VILTeWS;<e{=d!y
z)2F0f$~-JR7z$QDAWh>^Ihy`pOkvZOTg-Ri%XgL{^LHYMCS9t1gAnb1x;hCy+pYX`
zv?v0x9d)FQe>12$=@RtElC4+w3)x5)Q88jo<Y~Zx!Ien-tA5C=>++>+DIrm(&^qy|
zXA*>si~q5}BDd~($pqrPx;tr_L0PWru+I=Kcl~<PqM(I4fDqfGs&!PJSgO?@DLfj-
zy+L_raq2eP`>S+YmYjE^{wm>d8%F9Yi_2q;YO^CGkdgnt0TO`pgSs*Z-$5b%Ev|7i
zHlj+xo3OOU9!DTlguapiA6R2~^I+IQ!_|GDlxrQEd}EAl<NaFMPW!n;vP3mAg_XDH
z8fEby(x%x7`r$|Bjs$Z)1mm8cIo_DC@`g%q6om|1*0n93m4XjHFnBbI*ru`#y6SPO
z672qfOlR<KRlU$lU;d<YsrMz;PF|z&b`jdb&EMA!^mq9`6KDki)P#C?g%0;tm6Lii
z<W!DyZ%nZHnaqmDlqj^scCHetYySA)<lb|~qL@VaZ2k>4ZZs{Se|XD6K=^Iiu%*^_
zOz)dH#j>(U37~HRkBuuZa_%)Mm1G&lM)<%0=LY^-2?n4EGHc_T)K$TFW_juiy}ySu
z9dXM~ZR!wk+CPY*h&{DSvMg)Tq_fQO8#od>>T1xs8}RyS__;(iO6zi0EcKR%|K75V
zC|l^horMI@%!Q^;g!>ooGH6TpDEPZiiH<RXGhTX?@rd&X9Th)kx3{n;f0_#E1cOST
z+VJ|f_Hur{ceCrI#4JjIj=hNQ(hF8z3}5Dxz8gR?{DZQi5cJr*Kv6rxger;1D!EWU
zASZo!G6b*O13Gl=1=Bi(9-f^uH<ecXVli=&CKApx!jxi@Ze(0}!{I29WMFOGvZ;}N
zQa~yaEX#9UI@zsSs;L0;mcgtLd(f`}!LuFuw=pta8<E}0bcC;9Jm?Zf9ja<~)N9f^
z)P?__zxbY0C|ZH(_f5UYj|dFQhhtTtJd+<gJ3U)QI#;iFI;aP^`B^QC<xS?lb0v)L
z0ja4s5sdeOk!+9Q<qqD7A&&TVNh%UrU<`YirGPZgW)a3Nh~m68P`C(D{I6=1!hm2^
z_PTh4CnEI8GxEE(4GFUXG4}~UN1!HO{eYDBaw)x+<*`UVjXjv!{|7@wlw}^SJ*yMm
zlnz$REVrVB)v<jvpvl%Owy9l2FeRRr@o`7=!EQQoccUN7a~v@pREh((z8VhZAnpG@
zs;)XN%57_NMBymqIfyi<bT>$=2#9ovAVZhLAT2o}0!o*3H&W7F0s})U4Ff0)T{6Io
z0|MV3&b{Bg_s`=GJ^S5z#j~FEtcAwKLiUSYJ(|EKQgL7=|9zaztcU)vMq<j}p-(s(
z@PyG7!*2Q@!!3@cF>E|ENJcB+JVSBQ_wZFUk(vw;X1JA%xSrdGq=%7sNW^c6tf8_N
zyv&w=EPUJ@+Fa~R_p|>dzxQOx=+?>Dm39)dFVE%jSb3=2)HN*6C5sL>GTPjjG*rHr
zg`^wg?-e@#N=i{P%?P#q+lu0UPUXebt3dMH-vDL_q!9COpTN?Ao($+;x~YRwNX{zb
z#<F<7qOQA|f&s`RsYJ&h#oJndR4X**Ds}$-^0mZ+l$er}18V*WVPvfIH85S0q9*^~
z?5A0LyNe1oHIlTem9<&{2*__5ga<xZQeR3xO+#g=`lP}qmz?Ignc3FO1r_)Qhg^Uj
zqpVH!c&>?}^Es%UL7$EBUmfBkqYpYcl}`QF*7GaL9=pC$?jvZhj1lt_N>)kIIVaPJ
zSHJ$OpW=@tLy*5*9$9`u39kh-eM?tO8?UjwrcH|_(vV^)4;*v0hQD#zpe{Okd}l%u
zOaRug0O7cqc;dH~9h&J#H9R=l5V)<_H2;t8Ns>E$G$}-<O)`O`q4tpJO(dXK{>$U^
zi3;h*KQ?u|!SRM4u{|`<L3H110*W3z7@2i*4+!<0`k%(ta2ou_vq$957k+BI5$51O
zVi6UW5u%usH|~z;+m!ZBTHuI!$rtHW!F!|amUVU4<4)^Uk3cP$W1Mf4$`aG#llNMr
zBXyTD9)*gMdW6CjGy3_y;l_41mef)F64sRci{*lT0X(f}_Nga8wPa1e*#CTYt|Fx2
zRdxBd2?-eT3qYekOZnt6|3p{__KyT>NVi2D%-qbZd7;E?SulLUctc)vlcIt3nDJpc
zQ*S-fR;?vL=bCl~{4x5r&0a#o;fUlW&xU`MsDRsR5ET`CQr$_S<26%T`Liu3X)dR8
z{y$6`Tjh(r*SA-bV~?xDpN8qiJ^!a%;@A!wX1wjK-wgUAeBwO781X$lvoPS)d#v1(
z8}!7!Y_gi~v8{nZEnnU^UMLw1AX_L01Rk(#F#UbBC&gc!bPA5amSVf{R`4kX=$rIG
za3r_mOR~S_Uj*yUm;2o2wGS$ZKV|@#^50p*@<^kaA{Od|7Y^Ej=~W<2n)PI;{u6|)
zHx9Q}tZaSOM$H?&B$JjO$v)WMrUc`1OfAk`S07p)04}-V^ourRq#&UC?KK|~X;K15
zZgaiaAlGd_3N<y$>@2GHR>Z(xfIKb1c~dN*r6F0}tl<8FA_%S{E<j!OKi-Bl%cw)o
zETOYy>LoM$Gq_?6ik}tDy5KcIe-QAStEu=@GOF&^h0-q)YMEH6z2VttJ6proAy}d0
zKZy#dSv-=7S43+9e+Z?*(rLg*?3^}%S$sD3maMchN&bmM>F6~&^avvx#pfqk6H}6n
zb&MfpK+8T*^{j@oDaNBDDD1!_?pxF3h%*5kRFD4^o%-IF_I`e`4<s}qZUJ8vK^L<*
zMv=R!8-K}!1jb#-v1}z2sC!NDplurC>8%IFS+`f%q|+S4ciM?oG<d>w^*aXga|F^U
zBPRnG!q9`ecYqRf>=(K~=BY*w(x+!cZ2IwG6Ft~Ckx`_ZAefxPV9&R@4GA1GCGGi6
z?qbY$`_+<g@`L|<xLi&THQaB>twUvc*G^g4he1=$p^w^zq7?oSD!)Bn75OF8MlHMF
zVHNB1=?5dhs>X4P+VS*!i(cizg4yrcT;Iy1S2^uA*r2@TG%!Ck(@G$o*1k2QQ>NqN
z$#1deobMmfUYYc*#oHN3K0#JZV1DUVHy9-IO`gl+<-)9?&9!tRqIl4U`mY413Z_j9
z<~|a+t_x$SAwwUMm4T*y4|z@?LOXN~|B&Qk_TrdVIUSt^KE*$9q3E)-jg}LQn!wII
zcft0n3*m}1C6v$eGojxopej@1IqA>{4ry`%M|`kc7M6@lQ77@tY$}>O3izeSd3zt)
zcXtpZ@@j#$5V#b@I1R?)M6PYgajcJsQ!wR!@W}mlG^Cta6nTE_9XS^Z6T_lHGCkx9
zc=ZxNzchYY(k-Q(<!0Tf0DTsWb5uBOe1t;LTcTyD>({qM>9G+@f6mD3P^WJc6$sSi
zs!YYs3r?Bv>O3<_Fck{M$&I&|Pd*C;t=;EoRMQxmhm+C_X~FB_KA?ns+U4}YD3%sE
zz1rpX6Z|_Y?REMd)f;c@Yr)c7H6tazCvUAD#3=&T1JIh_!N|)Wmz`+m6t1P0@~2wo
z`XXWz2C4_kPCFySD=JHBA|d9-wk1`}$Z>xeova)9D(ne?AiLBc@6D6)KrIPTgaDRx
zGD8#wGc{*5<=_FiBqc#l(<P@O&-<5)%#?2;pilo;{oHlye=#j7U+W9Ly!Q5zoc)Ca
zNwUKXQ0mSH{!63}M~)5_$bo9+AY`4%RnZ~&NkxHN!1N9-d~Sz<U`0vw&XGf-o_y|W
zX;`^Rxmx`M^;i~(`uwgL!B2~XWw!ZOIhuJcNAYC1)m$sI7f^eG`2<G%RUMH=zHlbB
zIA*)ODGapxVn*cjOLqmW|K|l89|CdiD}04xGC77Nx36_Xe#OfWc>lRElN2xv)Tl;5
zD=WL!es`@Vw+ETxn<=@fi;p5r1n|&Ynw^~eRa?KNxp#2r#afjVDLr*3``8zjI_I;i
z7M9Q^PWx?&xivDV6ES{lBt-a*h?1D~k#o`hNK$xi>x&nf)}CM#ppC$i){5YN;sY_h
zK71sw`qy!leE9i=nXqg&e%lLU6fc+{A&61kkk*Y;2@Xe5$RYi&m)JTKO|bYYaSjLw
zm_78gcwee05|H=W)TwVto3?v1$)*Z+bd{FC(xM)m*LIcU=u9Zhbf?u-TT?KW!E%aw
z=Q;k$#%?)~C2c7k{uWGW{iOBc^b3~ge-U2L?{1(x8rrnpT34n!Sx`^87VdT>c1Md-
zCaqM_865x3?}wHJpbJyCAEC!Xm+94Sy`Ul0>UJtzQ*$MPn5En}5o-~9k|AJ2+NKQs
zUKJ2s^4?6D@XZ=;)?Z4hn4E({s-d*V+7lc8aWB+YnxHOH^{QuIpC6|iT}w=NdQ1$M
z{{O1gV%(&N?jP?9Zn{2ek*HExl4i$)3Rcsh`LjVH{9#)E0<B;aSfder6giaE>u;y$
zU|%$~ja_?tJ-hrx$2+(fhi;kdY-`T+SUQS<J*jmd>KehY-FDef(f;U%1ZfJ1wfJ}=
zRX5B98R(vnNR}}HvmSV}hth%hX!w`k@fyAP>N3Ip8vzFyj}NT#eabK)f9F${Gs;M%
zPIJ+_f|B)a?WO>5=8kPGQPq@aNaH*2>JQhe!SKZ9s581Ee^i59kla0yHqv2H#ihOU
zE@p(gDHML4`Ln_w4+sk*{K_!LJM^`w?mpUEGvvxl08CpyZ~zg`YoNOUenej7cI!>a
z{}d>9`am)~;-Wq=S{p49u6ceiKlk#J{rMgnxKV<B%DVB7I<VkL>Q?$JkA6ZnhixqF
z7DXsfXJ@dOxzWB9TT-Pf^3^+^uGPKxG*M?|D@{irlcfWGr{+T2&ZyV}8<t%%84ZfT
z6(u7=QX5VdpEx+b{G&S3OGX~lj(tFq-Tvfc?NAI9_lEzHbE3mj1ixnQ@@_2^(Vxs~
zr=VweHQGl&wDOd}PtOrNFtG!qbVl!{?`DPv7$GOe6+DquUs@s=_HASKqKlI5ef<HY
zlhyNEGUuhc#OBX-uq{ZMiC_Y-GC*2*T8Dd-=GBQ)Ocg*KoLl3MrQnnB0rd;B80aaj
z`A728%RG0<6r$q6#(+ly^Ay@0C$7x1#YY`>56+`n6I<}ybN{tM-jXuCO@9yC!@;As
zQ+W@HKOU{8s1;U@lpe$y0?G6DN6cH}9vpdTx6f~d4QMg(e0%dD6wB(Lz_Vn2yrmc^
ziOO#DCVq0vH}3h*yl2AQ@^XW#W1%;MXl9yjbmBKGfZX;9ze^?hXw>YUVb8WwfhNht
zT0SNiOdl}D{xh2F|7kqp!;?|8#HPRX-jb6a{I017N76~ZPWn()4Ur=gP<#(XxX@Yd
zRqqvg4GFpRY`5&bCK&zT#YYrtq9M6#aE+`42*mxB@%UXKj#@@InK>6XvR^Vf(jRAU
zoui1>q*#_$+602GtYFzK`5P=V48+wy%?1vKC&_|-oA34HpUcFid?;TSB@ZV#D{dmG
z1%S^)njJsJN^ocU=Lq`QQ6%n8TPf6hF@Y@@&aT_8Z%P+-bnLnKX4vvs!pMqF#vF}X
zku`ersm~I_3`p;uYHz?~@QV^i_mi`?xMMOOc={HFRe^<t88Mr{Yb(KvNKr>|1uGSN
zYQE<$(e|R!8NIYqawMC-f(IEHl|ZTSAA%9xi%%1F*`7B`DW3RCwK=*=hkwJ~CQ>S#
zcR`Tge{R+W7m>>q=>#V|Zn;CB<t)D%kEnVWnK*FH1XksEV%`j!Me)1jEGmvJlzR?A
zp4|~$jXzfcVL9M#A6W4t|8}qUZQn_}@y6+{pUaCAbNfSTJX!^J@P#D^_sJsRJZa&a
zbnNf2t|YhEfRuB315w;QM!QfC6RB3nEa<@H*-4P>_+QrdArKHm3&HG9Y<06Lpc|Ic
zY?IF2oU`3gPcwmI6Z8ugygm@e`^J{R-=cE!CQnVfHcz|%ieImtO3n`CW$tQm56w_8
z)1^(8<C*f+@f&sc+ilu|V|60*1`z0Z>b-H+vn6^&@l6`5Ih5Cur0hW)9*0dmzAnw}
zeTB%4DBFwcxNGsqq%8v1aM$18Cj@bX)WLq&0Fyueb4+60sSkiEFU?}I-?414COuvm
z`V!BWB!jo!7=8RMFWY-KLD_B!QJL`odp?jhn6?$X!PM)~*_lvyTv6)p^l0INNP(5y
zYSM6}6=iKwjFz!{pG?b@;7JKSV|d;`d+?RL{v#lCkDI7meRS~-9-G<b;6SA?LY51*
zYkUOTH6(d}e?Yc|kCwb&#9w&IEAD^1qf-PhMie6-0Xt}u0#m(Y-X(KthTaxer%&$p
zIPRGC^%k`ACu|J|f2nEW&%*bKV^pMwUZ0%=ql`ja$3{2hZf{!-`G>&$C|bsYIF;l~
zG{5QvYDFJD$#Npic_g;Pu_qivL)yqGQ1HuSM8=n#D~oC6-kA}Y_0xJ#;wT_gP(@f0
zZB<9P8!5Tmn3;|167rUBb9~d4&S;~!DRZ(fBl)umJ?m4jpN;#ayp9(un-Hko{x7S2
z;K5Eda$83}Q@**otqQg<1q9PeMTqZ%JN+m?ff2mfIY1thY?VDI)+ZaS$?|zia$KXe
zv9L{YK3Y$+629n^Wqbd}S$f1DJ`zbM<M4M4H8&h@jMgVpMnu460-pdKid*PMEjc!c
zuOb+s`r|Je(!!lCVAyv9f+nJ4G3}C>H`b1yfUQ*P)OE)xgEsYh0PNpBFF109^n8}w
zlay6y&gL`HVWOc)frgU4LHwt=@NkOurBl*oY01TiV39Q4$t1<XZTME+Wr638-&aBK
zV5&HaPB+UeAd}AF9#Fja^HFa>)==o?2gle^&TrC4no-^_{zG}fN@HSR>agV%nx6ze
zjGYj~Y!M(At@-WHLnvvJUK%f?BYTx(k-qoHvd=9AhUKSE`TJl=j19fph+haZ`z0Z0
z4GecKmQRbl465{bqkNL^b$Git`44sgW&Uj$vxTxx*MXKWm3`AogxL#>7SHRPB_0<C
z2L}cs_l5Bd{{CB{>}Ih6l$DV|tz|J^)hM)hqez-VA9oqOzGWf!XK(;d(g~a$Hfm5V
zj@51WU=_OFcf;`>XdAH)gmV>A3sY#OM?wo_L;xp7t@Mhyh9bF%{1{g?)X~!CaAU*w
z(b9K?OZB`uQ(#xRn39oq(qbZ<uV@T*7{<Q)Q`=#K-j{H7GxQa+Nb-&U#0je;Ez%*f
z$7m>BatdSLBA;;7)<dRJfv>1hBqGTd|8v!kUtpIRHyKYYeV{J)8u)AO1VBr8W>db5
z4<YSmg-z}mmPL`hGnz&1pO!pX7-sQ5vMX^+Cz?}JV@P=-w{DuUlR9KGK%62}>@&_U
zE#ApzU&K=&_3lVhQ$jMuPrlM_ui6W>a6L#e2uus&aSIfeM|>vyOjdnAV@W8MU!1;W
zC*}`9Ab$r8?PW)ypB+eiT5oMiXhTg9tzt>TkK)@Sl<iGxPUoVu!PkSkzEQ@qyR7Xh
z8a|OnHu||pm;-U8gnZVlX+1D-GY3r2mIU0I*hf43l07f6A%#$ag^g~e;<x7R64I+X
za2_op6PTUf*HH@>&O3?PoNSN`Ob|8z@p$fpb2fG(D<!L-YWOuygNY-{eUr%-*hmkJ
zOoFpB`Muy9ycb2238fd~;$YYc?~bhhaYy0zT!wQHmEuu;vv&T*m`1L6&SP4zb1Z`~
zw-wa#1TojZz(K(SR^pnI3_m85jxjX^VZP;STW=m|6LGvyK1a{F!s&_|>@R<{8}E#)
z_lnj4S9=N8MLxUKdTb}XD(TqT^<S#<+*VCdqv$NWMr|k;g!UEK+CEI~pc2i@!LBL7
z?Pk6g7N&!!*E00iDpZ&83f1#W0e|A?qkrg=R-g#~FY_SXVqPr3lIBaLdCTEMyo}=(
zXYq}hP_9b+d#FhsX^64kq24MA94n>EG?lOWhGYRO0o2lX!}(GJc<+q2ZG7Kca5Hi$
z3cmcj=Dia!sUt(<eoOt!u81I*e%!~W3<lwz5f1`#oJSotGEND%mM%sYGddC4Q#j`R
zly@;D>zjy&>>_7+9)CZ&$@-`LJ1S$37(snaP4-THr$P?nW&XwRitoqNv@*9_D<KpU
z)>JnS+C3(z7Or(sS{srp?wz(1UcY{^G%ib+Q}{PKf??s<Z?SS3D|UaJvIwRRnXcUO
zW>OdMigs;4t<?C&v2C8z3eyb@mq{svo)IIw=SDw%kc7H$v2E8?QQyi#{j?t38QBc`
zBR&w%95y`)qE)x@_DD>R>3A~LSHp(o?httH?A{%{rARn9Zf$QZn*4`aO?{Ir;%{7f
zCVLYE*Oo<#=>Ivw=KCA`!V3||XFj7Uiz4#bLSlG8cuuvlZsn@J&LGUVOLs`xuF*gL
zx>p5v?{?qRT)1Q0%7@d04;?z9XT%r`W<<^4dU=tEQC&;fd{)A$uO?g=%NltWm7c{6
z;Y2yK6?P$!$oOU`GvD_2RoZ>Tc;hxdW=UB|eX!Sf!ww40MbJ-0H)MBvlJ;Q<ESP&%
zU;qt%^=ZIY(==27<E$=ZTozLDOeu)AvvzAw7mbs77P|Mv;_M1`X0#I4q>Ju~8dC~(
zYW@5iHZU<cJ|l1YyQSSGC@hYdiW<wDTtQtQxmN}Gl+em0h?$F!I{n*A#xJ_%mwY8M
zDUD0lZ_@l^An?Ny+u-3ztaX~nVms>ViIfx3?E;61EjvMpmLHyy%nJ%pgz|-SnqP~S
zlA>qt3mf&%BLgHLLT0&Nf$`ljuoKLqJp!`lt@S2ch`#0h2U!A*jgH^o0S*BpnI@ig
zU>&j2F4XQZ<5a`L_7U%t5lbj++>mKz6ymN<?Z_~POdp<O?f#U21ncBaQ(MB#(Xp{P
z)eR*tqlfiv-QOYpfZq3A#p<H>8h`yU>*?>aSMjlT9%tBlWWOy)82%RgjtAaB2Uc%q
zIjESFS_Qw-fqaD9(~!hRK~2}nW*o`gVaoka)?KLP!VCD71@*+)>B#`<bqyvGDNIT4
zY?NfKt6I0Cl)rD&<W1{aI<W$AOF9}+Pb!@RUndAb_WDcS{#ER!G#!Z>@}YBz<5`et
z2>ud9x^A5D*AeR}!gIegaOv;^G!d>f+nn%w9LW~C3N@Ac7-nM@=m%Zm+@7i%*r&g_
z;M<}9F^*#JptpvGzopLoap@zfXNwhd!`2<jqkpa(#|ez^H>I(mLy6oRHY%d+ETfrd
zx#!pY2iLsy(gqCl^}IEjO*$Xvg0T9aV$2LKDohv{6=z~7XUh{(Y|pBOgiI_6gp`u^
zuTXO1ZJpSwJBmEsK&682(EC`zi&KKy!=QizQt2cHOWD=acFlKOg6(%H9v{HT{y2>v
z(fM0obk?0(w`^hIJ!jv~r4^N$lSmSYTdIiA<9jUzIjJ2*@`wGA=sSJE7YxZL`&D1J
z*$HRI=1(-UD|y9pu@(AEV`flVC&sn0YDa@5=2!8jHdPH<asCZ0O*{}mv)nOYDw?EZ
zWd5zsN@ZhXjZW%b+!OGB3E!6&x_`fs^~^|(nr<kWy_#Vnci=A6PD4NIMU-?XebscS
z&g!mr#>IOgkvClH7>NRflY|ej{MTPRMJXKO{c2o9#^1?YpA~P=Gvqxz(p_C@mt46R
zoko^CJy2f9u&NqIepBrJ<*sz#C+T@#`{Rxitr&)ne!DLW9`GdTBnx8JW^doM+{oJZ
z&N*(?GX?GUd?!|hcY9Bg`zZ#o5?M=5w)RXXLz}l(AQ6kU>>Xx~Oo$_kGaWoYrnW9#
zeY>d)Qie>`N20^>NqY~ClVd|W!G<f~ONa(bYfwGD57K@m&9Ux9nWqj$%5ZOQQh3yU
zl8APTlf=JKmAFKD8UJiF!uGI2C`(^{1=Ez&gcDdFZOGVs>(oHS<H?<RP8cMJQ|c5%
z<+;?gE76}4t%)M<?lNk(!p^wXs>Cot^!i*)Y@D;2o30m&n-)1-wZjxndJ#J>5<Unp
z)8=rtG58NrHS2j#zT5LnB_&&%S}v^m6ZQIuSjd1zhqCG68*xOF+tZKKw;DfOkADMV
z>1zufGMni;=T`su79(h5K__daJK6mYzHt$2XuB_{#5=Y7$qnv)5(l;S=)kYE(Xq7K
zX`i;UypR!Y-I*CJ^>Wb+oWqVT*2!J0zB9QmYIA2}r%GA1jW+CeiA+T|>g`DXoO_#K
z0Lkq#{;4br@u^6k_=>8~66evqn8UP-4PK5@D)wJB5sbpZxeC^e2esbS<~!&^fjL%+
zTiqyLs%M!zVgsw=gZSqWK`zpk&q0C@LKEmctaeV1g!WE`$j~W_<O89}?!9jq6))eY
zj;IFZg2RO!?dN|rgI*I&7Si{z-C}A<5<b`rRH@K6cJ%i7^16+_LiD*~mO=l4;v@Wr
zjf7=L<>lvgqaO8+<>z@n-f~%Fpp=Kjg>X2%_}pj5BG@Fba<McmldW*N>8%O`h9n^e
zbWj8BRmnNB%Crmk@ar0tt%EmyXBEQ!p+6J+D4&~T=$mRPoN_J95V>0F${y3-I~Ned
zG(H-c)}<Tb>yPZzi8AHQn%%#uRv=6#!Pw2#D=1mm(xO?QO0@GQIMVkIwswN@cjYgJ
zfBu{jQ2W=nTpN0j&2p+!a*>qqVc}+hu_J97z22>?&wkz>&zk+bL4NGiE1`&FWXp;l
zU9q&3%R67Yurl?lb!acz77Lt1QrgeWxDhJsDp#x6yF@y)w&324U+exgWH=$O8Hy46
zBzNx*mhTZglja!{C=NwkN%zHXADbAj70r5h9NSBd8gdP;4E~*2yTm)!L$M`@+u7L*
zgp7L(($fu<Bgp0}9{JXybb9N;lpdWXqkJ>8SDn8dU-7&OBJs~25wT=t^xV<DQxuUK
zPj*)Xf{OROs;ikt1gfZ-fl1r==Foe8B;>tUw_Cw>k_6syu1)voYOh|M#kTxV=?hSj
zjewG#y-qcH{mQG^gtEM}^pk#xFzW>jrp0d4v9I%VYOwwM3ghtw<d(klH?eNePOwN^
zc`tC4-TReV<k^U&>&}4#<LvaP6Dcg3ORoc$lTK<cx#XhokY~i}8!q1QVye-BVsmbl
zjBG7&QhbTQ=vkp_DnkP@?I6H^c%fRZqob(Lvw(!iQQ-1<T*Q<2KRO=VqXM@1Ac!{J
zs`o&;gmbotljG$as{v<7+T7EfRo7)t<6Dd|rz9iz50aWqoDwj8hk%vnv<3{Vj}Oi$
z*H$g<knWNcZCru9&@ypC-T&_NNr?C*d<kW_?7N^fo1xu0G|34Om8m%~9xC|7S+bsW
zOEX1Q7TWw#uxW0X?GyEE+l^7dIU08O?=hw74)1v0prukcj(m7^I9C;M)@&ue29?s7
z=8pyut9mQKqH&7=Cj*mTIXj`79fc>kD<NW$451%@<q*gSjS$rg8g4EDS8AdH*@rbI
zJ{s<$53|R4_9ByBM3BdyB4b<><9D=wezy<-uNq&bj_nz&`!K3ifI1h-Qa7cXQ(chp
zf&Mw|-o>uzp`@J5aPu8P;tFLY{h<VocKUVm!<1iqx-Gn}1x`@S>c>%Ra>dE((mbw1
zPR`@(xKECRmTT6TYTdgJo6v9Xl=-m>Vx&sWLLydw_*+4K>yKIP-daG4?t)LL#|=8#
zYSs6Zz{${?*d1%D(>0}>)Aqe80lr(A{b$EF)Z{K-$;nR8Ic}OM;oKjjT)f#`AAbgX
zvtaPdx0Dn2=F_NHKjim|=!F@Y5kXwbTMlZ1mjKA9HMcmM3)MO&j^?z?jGh<8rg?A$
zn1&iQ6$?&E2rCl17wfp@|0XlwPVBQY9`Azal{OLSH(Sv}FQzDtcNmFf(x^`-iOM-p
zwEX3oP?C-capu=Mv)faTt#48#PumV@nP$gT^(+kaj31>`O>gU<HR@g+-&StJI}%@|
zlLmI6pn9*}RIKo}N{2IZ4Pv1&|IulVWcc=BvF2_St83_G1P4FNpJY%vX}Af?Qvxsu
zyx8O7k<^;8y~0TUHgQC-&VJ}EYNJf9y~n#J7Krw4vB%%2hIzofut)AJLB>@=!T#w*
zft@G`3KiM*IojS+%@-o$k@gnL(>#<*9}lp&R1eo7cTX2YXXZ8j2`F=e$;UXDMa5A0
znu~sQ!@%^#SdM>+uW)qHsAtTME%Mfbim&(jd?dIeu*Wma@jl{NsphE|`SCYIZsbRp
z5%+B;t*7nqKY2Cbka?W0*^Ubd<~c+yiFD>RaE?yO7j}ZC+)ejZPBr_tmtxN)>_wAb
z5RlpIyx)o-A2*mX*U@J`4|Daxg<L;`L<#I|?(N@JzX+6rxaV|{n#I*l3Y^gUMlHX+
zSi{A0>xUHjCXiC;XX=`YGq6P1#4{wB2=r<0>yc1k*1$^n+sVIhQnjWANUiERpf64I
z*v-wY;=rc~0R<{eUat{1J41|!@_;SY7v!Rd32}(?xA$oc$HIpDCSuN;1CpEeYqVr*
zey_I6{idQ5Et6Dbo+qwErV)L&m5QK1Z=PIZXS$4fx9NsD+`7#Nr`0#J5LjjniKv&C
z$YF+m_fu&A+Ao=$CrkTSGHlE3@KuiW@1|UuS=i3rp6=!{`1gT>nOoWIcyUrLiLV`Q
zIPd9b`}u%n6F)E>uB|^^XU{J*T*}6ta2ZxU#aLK#*S!V%%_yzU{pPt&=PmWP&X<Ia
z*)%m*)}qqtKq14+78*@zI5a-A@;ws_WipnjuP#~9dw#S>vP#v%QHYJP#nNt+h%4{!
zvbVO|;OmffX|_bS5zW8lR-=mG?J$9r!mDHO0M{ft{bYU`L6d)8&0aA2_l_#9yj$eT
zzSFrgz=t5|R;kY+WuU#BlExaT9jqQ_<Nd9c-tvK&8;y5W<;U-GlvfmP-do3iAtk?5
zHjAKt=r#7H2V$8OG^Z&?0?`BKxOsf`$bU5aGePCW=R5p}C674NTtvt8G_vx`fMJ)#
zY3n05-lAvYm!Kf(K%V<_!B==oHIvE9txrLxRJCqGEr~~fD>?8EL@z=rfJ5TstvCi!
z9R=YvU3j&Vei*@0eSy=rP0+g$OeI}Vn(8H*e3&m~Ig)!$#lm7OcxJ#-&HO<i$?e48
zn`k!%56ClTjDm2*c~M537ye%GQ3yHx*}rWxqVHA9;_lDjxfwB?K>^hYy-4rGrkIZ5
zD*8H=b7e&r9nK?AKuYc7izKsMXtFVGD1#~))g_drD8G{W1O~!q{~8@zo|L6V3Hr(c
z4IIg+kbePXUP+u(mx|Ej4+-6mtDu3Krz}XuoH?28T-&4J$%9)tA`ea%;^Z5P4pB3X
z6d}?J#vX7a8{%(C6Ii)pWx&1dNhcP~HzFrrez_KjACy4f1-e`NKM0aEMY<%=b16tv
z!15iwJ0Lm9<#pNS4Sw#}c&m|KM$K}u<czMaF4)koCVKAEle{RYPXR*V{{&Dqr@ab)
zY%Lo3YO<ZX+WBiFi5GN3TjG}IqGmD&PnvtOG28Ysy?<Mb8UQ0#?cG=&_MdO+;f5A+
z8-MLD+rP@=*Iyh@$g|?Dk)|HR2pXDn6w>~(VlrU!im;Eifg*RuC-o=yUUECdm5OYs
zR<G1k(dXM>5z|X8PD-DPVPH*$X+_euc?&&Ro`{r{7nQz7%*ru|zU6b{=_lq(!i(<^
zQdDfFQ?du>M>V^!!aso>$65}zSM$$$4f?jr=8?Cr@iW4+xKrC@A)sm?tPjBl)8!Pp
zp<MNIY%p!53+|2Y!$=}G5|Ay1%V$dLh;DOE>+$P{xZBA~>qtnjru)tXlJH6=ws?Tw
zIki{iOC`g2BOVkk*DZ<O#(Mk1N@<!uGdaZ(dnEhSiPQ=>PO*Ol<>Ii1gb~6My>_&;
z?*srSZ;SJ%&uHWoYWs%4bbz7=E#>dd6MC_rX6WL}d&RymX+?&&F>A4#&?Zm_1%E*s
zZc43d?yE`i?{kMB{y;Bk#_g`-(C7Pf-WHM&j_Tt^`r83)>(R=Owd+_QFHOhHM)DR@
zSXe%%^X;Ghh%kDce4_sLqG%8!w%5oHih2iNif=gJU(NW-AT1b4blWj)<%<rR;nX&V
zf5Ps5`Tc+*GZ$lUPlWo>vPK&3<%<<g!()rOkk|DVHKU5SqnqINH!)ah853fB&b;IF
zu%uE*fGP4#Y-6}T#%u0~LLAq7ChnE@QTTuwwQnMU@qbMxx#!wV<@HYdDK+VecE*p8
zZHAO^e@B1B>*Xliz5z69O$VJ~G2<1HF1`EW`RC^33eRu{o__Iak+7uAxOC#AIEqpo
zzbe=6kR%HX$2_%!!1nPRYZ0ikzxYJTDh5ertqIg*a92B6%U)zGpY+>__q(hM1oh9^
zw3Yld{NXZf%8=P8w_^h%rkj3azkfG#hGSm}N&s$#$=>v{nTNK(9?VlKisB>X_l79}
zbaP>~O2zozm$<5EuFPkdV^`U6;oQ9(x4MHC#%2~xE_2VlM*2MD*i5rf^v29+c2<+F
zYwDbb2LP$+%2B;nD4DrW;o-OX=pz%hB%c1Rr(y5AC6m$OuG~}takn#N!5t=_#5sL%
z$ZYifR{8NTh?ywI-&yD>dDcQauS*_d!S>>#xydg<Eenx<e$w-9Y3(?)VqP8zgy?3#
z(d3UMGT+Fz$PG2GN3zomHBC;Llk>G~Ue#N9%>$tjpltM4yame<F>2!l#kuM~Pd;zv
zdB``2tsHz^?!cOra?V7yW}^@!c@Z+d@LBON*NR!fDg{JJ*KD`M2>{KJ0{R6ftpj^1
z*4TG^3K1Glce-`3B5R4<;Ut;lk+NaH?itxn4`MBoma3P0grRod_J>MLk<8HGGqto9
zl?NYpKcE{-;|~H_)y$TOgLKeh62_JL>Z!XT$i20ogwZd39+u}wAu;y(TMhmmZZ}(u
zObWYE($&MVZO`|l%MKFkRm<|KHIDel%r73gI$}Hx1xDOhWB~vxwI?mg#Ik?C^%&D_
zj&|_M+{`4^6d+gvreCua(K)k@kh-Ai9d^lc@n}>^JD2z+^OU}x>@q9ArXL#l{{8z_
z#B{Ucz%OtEh08C3W|^FZ9*i~qnhSHGc2ydet{LMo1Z3eC^cFAm^b>x5+L@Y~^d6}Q
z)AC%DC?g0l@N*sepB$107o~n}3bM0WHYTfF?^_yw>Mq0|)%OQCs(^~LliYs${qmVL
z3?qgs6PmVXNi*(#oM!deC1RZ0$F^BARNty&j7HXa=oBrKMi*I$5}kgdX+XO2d1%iV
z_Gq$4{zu8g^>Ar}SA8|mC#NkQ$;OPyC>W|k-ZWz8z<&C*sU*eMW4CyC)t-CA#Q4;G
zgSd{I7EOfEOpTOvTCw`-$|t!>SJz;a=j46j<=?gzZY_@8q3isc9*3Vb`l_7)iR3o2
zfO@`E&zfXC(KDJ5cN|!CK`A=o-?l{bJq6GFX*#L~&BkW=^Q;`F<a8m`Wb6g$ee=k7
zZNs%Mc%&74-Ak=Co_l);RXcNqU>KVdT9tz_mT|CYb2J$hT%NY6(XXy`ntruoK^Nf_
zO=?gNMO{ggMtEr6d-c$IYuWzXx8y<yx>$HrG^*S#np{@}18j)F$gZU0^}C}9*{ySp
zB+HwlU>HV#zgR}~qt(SU4PDxqVIGkLpq;S&$hLA1^HH*oyZpjpg`2O`vyM&=g`oVq
zgW+ta(K2MU{zyH+>Wt?++mmb&u7W>Y<{7X(pQIaJ4I9ngo~O@W?D%Mz<%TlezM!I>
z{e!C9Zv>f{-wt!ryxC&CoO%(=NRLpickbJtn4BzKND$d$kC-hOSUySLGk5*WyDvdx
z?pl}@Ds6|<^DRmzk-d*@a3vSL?y{ch%72m~8UBpliI*9eV+UN655ZNvYgxSW^&P?=
zn$X(WGb*q%<H^L7eF-e4hz>y{G7LC3gYQRN2M)Qc8zkv%OutQ60Zo?h5+1XU?U-Mj
zf8q-J&YvvGztc!m?MpYC(c}6f|D{exws!`|?t9Ayf#Lif;121tMhB>*YCG<sQgR=i
zGS_V9F}!+c<A98ms_u}km%DfUw8h8x@G*LFWUT$OzA|DXVj1V^hasAkFis1AG1&&)
zSYf`cz7%+^o@736Sj=1Rxp#}B2y~=aAmvxT_j`KR=70*<G{>TgGQgM2%K7V}ogdm=
znBHWoBvo70vOXdIYVH>!&F<da<)u`L8I5q9Mmo4h`<FP}nHG4W?CDhHsF~ouuDt5I
z&P93gZ6W4vlgfFB@0Cog%rE_D@_6R{>eq(;kNGCPnqdOzxupZz%PvfHB5_;ebq7Le
zZ^vYJZgwcv&q6?3AT94-*H^<L4|DtX-RTFV#BizY@P9sQp+Bz?vbw`yN;kAs@`H6j
zQR7j4rVe^O>f1nv!z9rUJubk!>D$2s(V|8tBI}k+1_i$?7%>Jf{1yl6Pg<LD&K&a|
zimmydq)J;sF8X&etyG%LXrh03g34k==`>cVxzMt?-O9D*rPYhJit}48D@3!>)-bPd
zzqV)XUIl(<@u&tVHU5FH-{;P#*C)|&gF%8WnyfAC)oEMnOp`PP2(kOhSsEG_7x9SU
zmAx-bX>o`J)`asTTm(@KGqRWzTD$}I<i-yA_{Hv4IhG1+ypQW%O}Ba0Y}3hkiPUyq
z3SvQbB4PTs&<Yk%`FwOk@6;ZN_g`O!ZDpPZDTNUB{22}Y%u)4xOgro%ljtyUScEV<
z%pCngDbGUftTn<@VO$Aw=F`v~yy;WNFl#^*TZhg(e{q2NVP{OZ9t~c=cM;S(apnxU
z_I>?6t5MQ+1frUB-Hy?V@7Gg%3|}JOt(tUmst+f<&`<E|BnvVzO9(E?>Dq~`hve$#
z(3h);DW$1S_YGb>=4f7VTT?{HZNB{@?=l(H7j^CQJCMgX4*i1A4XK0te3vp)ut_f%
zW4ETq7!Ljugq&6}n>(+%bDw7L>x$AUtz?q_hFT^>{4h2~8ju)x4YnT+qZca>uRBcO
zWLWxl2rnCDOhH6QHDoUA$B&jqT$zWZ-5JO)?;PEb@hhg^3eIiakixggY_jxhBO($G
z6noXx@?82hp|v04Z>ajb{B)P{GDl#<lNr4iY=r}oh}?8@yVUxxDrpq&cs#UQ%JGCO
zK+G0bh3o_=<7nxIUa<CYt+priUq7M9*NC>680mBTW{xh+PA?mo=A?xhV)x4d5lCeu
zaTR*ve@f$(gMms`{F)fiZsmmtfzrV@bdzUL9JQ-6=%8$$S|M`R%FiWF)W4`EeWy=d
zVAf91*9P>w7#bLIAS^E(BmE-jQ<kS0W2GC+r30%j@6j{Ai?>5s)5P}$e1F^wDkYkH
zJtezw4ei8T%ght<(ROw9L-hwG;1)w58i^uJobM;@6zN-z47)GLsQcB3V<xiw-m4gt
zt@71K^Nf4FZ|%#9AdO8sIByGQFztcu31W_eua$-aBarzdg8F4iVOS)`+joc5`e{F=
zS1d_@SE-a-;8m4hxSims@~+RT2J)5o)k7BprHsq0j|aju6;f{w=>aDTGRn>qkWr?Y
zQVvc<lheq_Y#y<5VpZCaf9aoxNP9Fy{PXPT=Aahy2I-dXxJl<=!dx~|vUsJ^{qe!M
zH^emTM^}rTu6G7`d`JFLN8fDrqgk&IMAx-7=>?5$5ouo{O#J9rlg_9IE&kz!JoEfK
zE1t#rmpWt@%0ph?AhTiCw=EW2G_Xf|<I<5?+C0oFQS;8PBp!PLB<bG!UE<QYa&mI&
zf`Wo<3tgX{c)Vb#l$Tvux?l6Gl<!H$?Qzgqx=R_;$GS8viz&d-Rk$yIHDhNL5l!xU
z)yJVyIk6_MZ%Ln4krF+d4Z`GZ2*c2Et7rEvAoSGIP^Q+fgC|Oie#8EAJ+|&jrJnel
z&zhB%%~7(U<JxY~uJ)*&t-(R!X}k)?ETOPGuWP4hZ%=Nuq7>mWrx<oNXt$+c&7)gF
zHS{2+g7!C_`hH`a1B^5ZG!InuHJ1HMe`e7DGeCf5z%7Z%H)-JpX{f*mA8%TxWnbW@
zTKiKCf{@DJn;Dx>!I;&H8h1vS5hX)n{-n`fhc$d_BFx-xjbGBS;s%d0qN;!Y77NQw
z;Y^vbrwBuP^J>kU&lW{Twl_4DgJxei?WsU!H9J)8-5iwKz=Owyz8wo_%j=75B^1Rs
zgz&vX%4&t{yKEWSu2u#H`bF{(ntwYtZG-HjIhwm<P%MmSEdDDyu0HIUTwhXAAKrV5
zbylxVKBs_BbdNXYWWe$jPKE2K=gZZ-uzVBnb`v>W1M7+8o7ILWH#8#^6WTa+a0M(w
zv27FaXo5LQPM^J<SpOQ4ilgyjfHg2Qw5Am$3jx4-*+@=_*&SM$W9<+~Trt4+jc|f>
z1^+qL@BGyoDc_Sa@_5sJ72HXLk$3V5GN}X9T=s^_HU&J1rro=W;|;cEahgvU_h?^G
z3#{-@$gysBC+R=;h6Y+j?}{Rsc+XqnCeW;iI$96*cgZO2Y2(D*pMj!DebGcU>(3ZE
zbZs?}l}`_PX~ho3()8>u3|!rFI^8Yskl?|{m>Ij3^m6K;0qBC~gFtE?NYc9UFBh&T
z9eJfLGTR=Y(exv`ZwfK`4lyiMUG%Bqsb;eU*ib7~Dn#9Imbv^gd)T5<xA5Yz1kw$#
zla)U(<ZgDevifXdZI+Ia*AJv920hpMdOueW{G9k9p3@a!@%ktKrvSqEHoiQl344rF
z82txjz@oTl`V=`3b^PZo8Jm<s4-js*vAa(d?B>By@pc~!nE8hE_ZMzvait5oEwmnJ
zA*?Fj$n^n4K)XqdTi&~VIqalPI1iOVVYE#Uszbjd?p7nhiCE9Jm~&RSUh+bT<xLF7
zVeyS!oYYR`$P1$kT{HR`bvr>z_oaJcC#@CCeUT*@(X%LMa)STvW=#vRC*Nnt0Ue0e
zmwn#5^S6C<h1AZiHD>qLd`0Yh|8=gU*$jGW`W*pUI$u^DVhPcDaZh!CkqcDUKq7A#
z{b=N8yoDZ1mcr3vMaO1+v|D8$LXJUfZ8g%l14|o%xUz{OOoSBWvMy3xrX%TWR47CH
zBiEaHLNc5C5J+1y4O^&#m&o&#!q4t!ei5bJ%n2V%wJ%Fo+TRcV-!B_=pF*{B&WiVF
zpYLtQTS)^myqj{+-c{ziWAf=dU-`G0lK*ct#Nyux-22yW_Z7UwQsaDWo6V^T_~oC;
z^vci~ggS)5GdD8P8X!_kJrg8+7`iF1wdGw%&ewfN>71wTS`f8E@9wqq<;0e79>h?k
z+H0$mO2LSEeY?uh;;c&V=k%xL<d&waV)mNUWC52}23pMQRt&SK)WJX%xgQ}daiha!
zM;zCI6M<CmdciESrWlragXe<RqtWoI3`3;vz>3VmN~XrKTiX-2L$d9$cBN2+sqogS
z?H7~~b^>O$^8g<h+=m6BPQe>*qN2vywYGffCyru;>eyMAyp7XPHw>|wJMW(bU!#+s
zk0cVw{3fj%T#5nI?puqiiC-IYs`ukp&Z9T>x)&mOf8PB6w;jDXz0+7_7dIYezG@gu
z)#vty7xCr$o{J|BKlmP{;BF@nCxtq**m-@}NFE;^X6;L(-aSm#;H7j>ZISHvJPoxM
z(d<oI(i;?aNO$Ny4)jMrTEb}V`ex$TSW)E7ueg)iTL)}&Gt(^`{0sHsXJ0#ieKmuR
z^Hz@B$j!fZx=C_NLOTjBmQqRjWuWvd^9tM)@Sm5Wj(3i1X~^_J^f6(^*G~&=j^}EE
z0C8Et$BEqbhek53<Rx+EaolWJ+lk)3#_(u~jj_x!PuWIkh|oS?<%p|-HRy1^*sA5v
z3y@nY@v!+}Kb68rr7!JH!<<j2z6MxL^(UFjN`t@R!?-y_GmQ(IS^;`uDVNjHM6_$$
zuT}8mZ*MqEm5iM~h=Q2FhZD{LA$}<zU}u#EnXm1c;=yL~#D(LUapq-8h=?Xzs|R|L
zhcsYkwG*GFho}y-xFAkP>r-oI&9ONW>DMh)*7`IJb)kfrwA|h>74P>sr1w%Y{m!+<
zIJuTt{H>rpsW|Q|S6IkRIKCAw_CR1#8?7nRB;x{{wG5!+@<rv8f%VmyV0v!)*p>5N
z;w623;q0?{o$31~|2Z5Bl&QMI&Qg;vHBgr1T1amuCq7|Cz{qSQL_O`IwQ|~?i)Cg%
zo$l!XW$9cG^_F0Ow}C;WMiOaW$o#rHc9;7YT%P>>!l-i_fCMV9!Q}*i>(+>~%okfE
zGu)BXjUs05FP6Alo=(QnZPC%Su%qbay*y^7ln8REzUW8rm;Kw98@)vLQU|T2!c6y}
zOls4lxc_9+aZ3q%%Su4Ff&X2l@j#3^<z?DBsOT!owaPjfn;CMGqQhRrMgk7%YWRvy
zActh;!~G$+aTCUuw(dys*rQ+U{x_;_aR(mW%?w7+bZ|9(O(Y=_gUpv(uRbfMzkZLC
z*}_i>lY9ek=GUC|erd@0ay;FwKol2Mx+qv*XaQA}kVU#YKzRf)Jm7j}dFg8zHlCWy
zC-=pW@KXX00%7D+m$4`ziM!oJR_lJtEd|TywZCS?Gs{~}u^4OF8EpteA41C0Hz0Q!
z7U6S3JAMB~yeYsAO-=8{#kxI<M^6_z8iWr*_59D(LaaIX7^jDD!Q2M+ksL(cuHHcX
z15X5CsTj?`$euL`eVR}ub}W>J{QN^ILVnWx8UIrQQrzSJk8@9$;s`slO8YDLLWuWu
zzHe5kJiLr0<7S=;@-4e%_QrbIP00Qg?g#;XCB%<lDmBDsvbi-N;jqebZ5FO-F}w>g
z5THy(J!E8GdUg6DlPHxFFOX=LT^Ron$D?n#Hz!obk)NUqyCJU@aPysiDUwX;+K31H
zH8LB=NkG+DbBAt>Wl*?ve|v(&b$SdOWjs%ImL6wQIRF3jwO^wWijbX~ai)rtb;N_r
zYy6FBbeAq0O>}r;Iv)hqG?Yy5*i2fli7Dx>w0rdK(<IVS)N8|{op2<wTW2t|Z)$3=
zk)%$7hcZj9MbFUqd$P^jqmcy5o&s*QMZFHQelp>FkL9V!7ON%S3I|83=2JbNU3d@3
zv9_Yls|-%umGN|oG1><5x?$6U;#(b3c~9@HE7W{rQ)vIsIo(=r>~?Gm%GsS)pD@-K
zzt4Dl=^zwjYvR*8-`M5!7xyE>)`sl%yxg>jZNB3qP0!<P_TswNZg=;PeLPdnB=Qli
zs5(%d1QzjV_wlEZS8h|cS$tdlpfDD1-aO1ZQC(y^=GiPZQ`<P<H{b9o0_a>;#tK1C
zxwgu7Lisigm}R0|Dp19YH}~zW+9&YSp@aU7pI&EdTP?sq$UjtC<3>HUiMxcqP0oS>
zAo&%4`DKUuZF+CtOF@b3w`y`3#TQgAc|7UuW}k&u1vC8JyEXI**08bR6^X(T;Jx%2
z1uS7$0D`408X@=sf}2CE<RIFPJjMp-n>v8!SKct#ees_ByRq#J(C2(jBXaXr_W<{7
z=x<{eW0};x0w!4O=;({dFOv(KsdxY1Up^$cXx@huJ~~~?81$5opw>Bw4@6vAB&w|o
zIrUfhoM|X#8*MUe-3nu_QAzm~awkIb8RR3AL7w~9&wNo!TpKEwakgzBqnVyt>1jYr
zkE0H$3wc5qg^+uNda)-J36`lfO;v%S=HduBtd0b!GbrX-xVXPL2(c5B*+l53EtWgy
zfXs{n>kyUv&(tifVzLH>Ai;k<M}t^G@H+QdX$FV)J$$>x6|a`6ys$XILZW2Q{nne)
z7R?~XWwNR5#Z#U$Z-DUkm6W&NMWKl6pSy+%wiMlTf9@UKI!Y|O*pl>>JwOYie33Kg
z@7rRd-)u8OuIgj!U^!hpW|CUb(C2Lolt;JKSDBleKgqDkR{=Rlj;nD^DE};WQ^(aI
zZ_zUGkGs0G608`7YQEne$anwB(Dsl!O)dceam64b$CoH^P!6R#(1W#^n3<MgJ^uA&
zNAXN`oJv6sZ?l?XW*Fdw%j)6?pMW%RM8}BXL_=<xWPc&}(?`Pb^F)Vu4W{5nSj?!b
zrmf7!$WOOSjYTn^zl5!Le{#Iyk*qK%Rl6U5ygL%ne3}}nZ`Z-$;Z3Vas2;LKD$&u(
z&E_0;yytc76AdAHr=}=gBG{xo;vvUjO2ID*<u-SZ-~#f=dAvMS_@hF8ro2As$2M>1
zG`rkn<n1_qS|etBSr4=*pf|!U*CTh&^rAS@-{9-fE<(^{9CXz0rXXGhHF?=oO<Y*u
zhF}JZD|xj0^W!tyY|guYAp->wF`ubiY4;H$=do;3lS@{FZ|=b}#<}R*-2*i@MAO+H
zdMJTTWz$!eE0!Hx+e@;vgMl$B5euB_k_MZC-nRHQI`nbqkx^I)z=iXUU0b;=w7Qsa
z*l=-8<t*@+Ws4@)V(F|WNu=c<t`od|OH?CZCm8R?A$Slp$GY@Y7W4ju!74pl4Ycsz
zS+OUPt&!Kb#*)Wl0T~`{FWU5Nf4*fR6fS-s8CW$M|F@9iCMytctuj!-l~vkc`>Fh!
z*56+m=dFa@sjcJ^d^^ZW3(e{n^7N!Ry@&II=}fhy2`3WXrDT#IZY1A@QK@2HON1#b
zMzI+I(%dmr+>0=oe^%s{gq7UKF*15+M>aJXuO{{Xo|S>4R$_B3pe&DLu|o)Z$1O%3
zRMOzAULdFEXE?%4@i^7O<l9%7iZ3V$be&SXvrgS~C@*<DEY@i0V-<tnuw@+crt2!(
z=3DbGwN#0-84GrjuB0b$_<BYfL%$VVo!G1+J1@Thbmd-Eh@1hy(E?H{-?kdBhsQbs
znK1i)M}F61PA^3I5q*Jb$P_-A%RCI4r4~(A*<04=pV}0}DGI4YrL?dbF0Y9{t;8iW
z;iIbB2;ESd$bw&GTvq>w<C6&Nwxu{CU~}tI7BH`Gmb|PoB`w0KZ;~B(7G|N*J?z#X
zj_6wleUNdg?#Ejho2mY;Lp4Uqm>1~T4$ZHEcPP`ys-w@Aa!b-DXnO~hLV9dEzFh^!
zm9A?_6Im5GyRn<9W4XQS06J8I!F#FJcRzXNhlfnzQ|Z6Y8}&E)HqZrz-&l#J0#XOO
ziq4e-<_7BgCpNB=W}f4J6^{ixS)5j&-Jcw6ysJjv{ks>MERRvu1)XBUP|Tl()B@U~
zSS6jTqT^BSPwC%7{?<$)rTkQGMsx+T1@rtm)A01{BAOoBe--62n}y_;tQ3hj4F`qy
ztBP@Ba8}~lZ6JwYXz}ww!SmgR{fE2b1O}@H-M(E@aJX!VXLs$8&<(Lt&*ne!En?_A
z6a6#ZU@}x|xn#BQF+T633cHM%>E>r7`hA<lSNwtINV%7~rHFP`LcJVt`*!i^_qI_^
zw1d6VW$~%M8*37JDz0Z$(6Y~ccA#-DDHdk!D_Kf9SsM5}YbAa7p57A@ijXF>%<3EW
z!#DFh-|r-S{9y3ZAJ(I;qB)qMlQixD3LtmHn9n6G2H2>#L?SP7W$}a!s@oh41p}(a
zZh4(IUDPjO9LRRwAX;$G$~nblxoYpcY<gJl%OS2b*_nV>i34_AYYHLYKq;SG<>(jT
zn|kW$w8L?{d~QqqIyZp3cG%QLntt2D`guJuEXMk$9lZK3E6Xwc-_XR-9Z9LMi$yWP
zg?hfWxEx84u%foMKG{n~$vw08vsPO0k7Q`+fj&;9ljy6v)G1x38*0z)QZthB0>wdT
z_rTyf@0RHRLg_FM|JG<As@RTrwrfHfA6<N+OJup2SLr<?CZV6O6VFdm7~-&la4jy0
zYkS_lcJ}rH6hPt_?h*vOGIXe04~U(_06<|#9?0QVYwx*PY{lSm8e`hjlt@S^-%(m>
zK;0NyUjjhh<K%@l*Oyvh@@%JJeZuzp4oq6`O!@?F0nX2ijxR5@9-_(qAa~YUG3f(n
zPCj!!3#oOMp_=*@Neg5jfCJ!LGUHDTyax2q@vTXvx71s|I?~aO)NZX=s85sX7PfUW
z7)`O$K0E&Lp^ult^f0+BZQUy|{ys!dtq@e=vak9odpW(+99aU?2?MRz@sXsNXuRuF
z={H@@pA)Wl_0|XNjDqg=B+xft&BJ6C%B6ObRYUx;S`2w0Lbc?0((d0k$qs#@EP<UV
zsBX{X=hzeA1`>)(6-MQxoyn!YJ4hr01$c!i=b!P1v~;Hkz}dQf1Qk}}nr|QnU!mc?
z8ulp5*IuuwL=4U^Ag~fb21HxP^y-E3PAREg(7S6&VZ--E`zZ{*+Ofxxp_^JuY1o%0
zSFJvGml>llRoCRQ_yAgB52H7KobrpR+WA%FeJ?)NHd4PKBE){U`RWYr(8;#h#*Xa2
zE?3llE7BTBACQV;CtVu=%>;irK=v-Z#Z%2GiBLi6;m*vt84;AfOv1rpYlQeg8~XUr
z=W8eWN@U;~zG*0m5!8Pgkk_as4Jd={e0FE%roXP}IstvCmR!%q7*oy{RB2$<^tc^H
zc(3MlgWqk~-T9IRu%mN#eqo{L#*VxZAY|3DJtfW}T({#@_WGX_Wx|{PkG=N}YbxvB
zhMjQ~M@P^BLHZ0xZz@f?qJVU1(u>l&A`m)`sHn6FE!0G%cOi5L$S5^HB$N<Z6cS<x
zMFJs&knkRGo@bt4xxT;O@4DXok0tx;v+G*-y4PC!_}|2Za|kr9v}GK|usoLd`(&+X
zR;C~vLd?7?4hGcbnZ{>;-~$AAQP}t00C<{34a?tAP!yq{fe6<wuOgMf6MF7!PF9M}
ziYCIKggpP!p{wm`?rrqE{>GQ__CSQvqh#e+C&jU|`Etm`u#kdnEoY-3US4)3MCa{M
z5k?Z5jYm9D-v@1hMxHV~)RN+v0tSbUj^YKJO;FyudSnk~?hOzt2bi~{^f&Y)y7@aU
zL1#YXnFz|HQVQNrFIl(x;rh<q{;)<;zF2d}<mUPgo_quZjyiEpDwkQi543MHC`vm~
z*r`G#(BQpCfT0vhF60F=0V*!5RV_^L!dE+!qeCdQdy+p{G?-TfCQ?d3rP1k~K6@Op
z`a5zBPI1z+oT_}fQMq8vOD2a0##%cXCYil<Zwo}0d1PftE&_J5%3&y3gPEX2zZ}F<
zmdzDD7O0a%7?RP~kG_89&`^e6<m1~jYQLgsan_680x5M(BIzdND%s#~%`FtCFpb8n
zSvQ_-cCA0H{DgGgk32%Qjcy5%t+6gkz8b1ZJ6b?43PuyZOrM-*M)O>pX_S(ib`P^i
z+(b4R%3D*HQm-IC^|$gfYNhnETCP~{H2OdK;8j&?lop+56Y@%c$Mtp2li$oM;uk{g
z=&Gs#@!nht?$=iLfum^n=`b4K2@pxQ=Y>b@xVx6`VGZIsS|#a@73Vob8qXb3=nggE
z;q6yeEo<)bl(=??QTE{IZXL9nOF^7m=^5Zs-CyeY37^T_f$0iW>CRQri-bqqRFq7$
zU5_SG?TCeHbV|S~>)<!Er7jOnRz9rPWI{EQ=gU(N<ri!2mw#EFT3+t7v7o5X8tK!7
zufBhIAOQAfF7k#<U{XkR&AVbyv!iWN$!>2tXyEFeDYL?M`CM>sfaSeTm$tr!m#7Cf
z(H(suZx-y9x3VRGU~#p(tHp-_){L|ZjK^aEA(%h$5$g$$hQeDydp~s^?fsK%Y$y}e
z=LUHnmg4s8T;hWOypG4I{EgPfv-`tx<|JGdGU{LtLEk#mvOC%<qwu<CK$}E>&eFX)
zZh6*=R-Cn7Z!iHnCMUN3Byx`lMCUkF*j9c5=$vsIH{mIKr6S;Tt(Ao<UNC<i#Lak-
z54|<@1pzP#jTJg$t5DgE<V7YSf_^J-6_xoi*y<i-dfM}Lo@^0OBi{Pn{ym%KSV1qs
z+b6uIa`q%(fFf^bt+0J{FC=+_#Ua{XNb+2C_u%(en7nu3?;)(F<E*G8@%L)&ybN`Z
zxP%<9n@<NUl3?$B7H7XuwCp3a@>#xNkD(>`=ku57ZSrCp7GkrihlVd;lKCodd_VA$
zZb3@Rt7@<A?Lm`O2Iqs=9i7)_^Gk0e*?EE3O&T1s=j*u4k81&u%Z31bEUIesy29GK
z^~~49%!GlRWomlxiF*{=-sV<1x|P1-B`8h=7)JBlq2F&rHa%25Qw))&0B2Pk^`L{F
zetopeJi{I0sF?k-&c1BLJlvLDiYwLor%rAl7#j#>zIJVQJ=A?#uj^oO+rQ<O%R$oi
zhOr9uD$I`)z)z*(A-`_F&OR<1xQY#VO)Xs%VTgc!T$lgG9f3yjAL$3AQVtLz`z)&k
zN!)EtrhNf`TGtu`5MOhDHk0z+M7EXb+W6pPk>7yu<ds7dH&X}1qaOwej4R(%{8~sV
zO}F?`8Jko}Iqlp4%L=3f6YLnQWP)L-O6}OgEWD;o`j(*DdW~u}KT+cw5a?<mjm`cl
zNtzUAWy>ZQT+L3ZYjfQB<6!6xw&pPVE#kh9&sSyMk$V`oxUeHj5h2cGq<5g}K7<19
zNpa<%ui_xkT3Xn}ZgYc8Tn9aC{)F@TXByzm1UB8TXT97{9!AH{4{*YM8KGI%KKwAn
z{UHHli1}qC$fJDKa$m3a4mO{teL>uNM2cGHv$K|ti}F@^Zc?Kub2<FC%USm>%n17h
z9m`v3J%4VULl>+19EkHMnh!7Wi84TeTESDI!1)=RnLDcwq%z!1q@ENMeWzpztJEL0
zaB=*++**2+L~}(+iOX9l(K&(scH19bZ4Y0zU-u;dYZC_nL;}#+t5T8HQx(S6cJvnl
z)`FrE*V%?S0`bdhc-<1*krw3kxy;eRmb7QvTFPlYs3pfy^(oE*Nb-@>yYI8gjfbKI
z+a50JE-S@c<tM=75>nH@?D;7AgnU>FGQ$CGP7)AqPc~QPXBgT)he*?I?M)Mm#l?f7
z9~R-ADro#bNS2EC=KI>M?An5*yaQ6nPdWg6!}ABhg4yMeSCAwi^TA7&-*u}TUzYNw
zA)j6VP<KG|vVmaO#9t0II}!Q`yDtI9Va9kJ&Z)_vP^^wox4n^qPu?b9DFm!J!PE}_
zDZyb}Sqfrq<c8Xe_4?&l0ssUgKpXB{M@zK|u(Rxn?{4Sgxh5|f<&=r>VI`H@9>1w~
zbjb(e%31)7GH_)Wc7aUuQTh7GMVhf@%l+2RpxwvEC~!SzRxlbRZ^PZ;UVDF#Rn&1s
z^75c^<z}Y#S>jrn|Hby-eK-ExFUzNt0Z<!oV3p<bcU<{p=dvnaWGav;r@vtWx(OCY
zjB`|OHYE|3+~+oD3oSF^`P()#$mXVpzO#tpyH(6fMK?6_>dvyW3tiLPpD>)b?M^>8
z*vnfdJo0||KsX0TcN`4YGrPYSeLTZSj_n%{U=o^y-10_MGM%j=;SyPD<^AJFBExjv
zE!^C_LQ>6=HHwqY<1BhOLXdGvxB)o$WZ!lBKm4%PKG)L1o`D()iT%wzl9AV6EQBMi
z8txd|{$g4;qWl}TtetbgA#knd>b9HOu&wwMr<eLc9<sEXZ&PGHboPQHKt4h0T8!t~
zC~2V=VFKHB<UF3BoS8n;My0GxE+B_XdJBRz18n5MhTVj>g8-K_f0F|UK7P8_7~o@c
zxVEBz4;az3daqU$C7SMU(D3^bF`r2BYf@8y?g54bFL3XnI^~5tw)><wT^LkW&A!HX
zRT|=RXT^2w{ETF!&ir2|xB~=_*7oDs<uBo?BF_RC-K3r4yzw0nF!QrB)ytzTeq-4w
zXxa<#NfcUGf;|~H5|9uVWP6XI0OYM@0`!Gp*mHmVLsu~d9P5j~2Bf28R11834nQ6&
z$MZ%~G)1VpNvm6NQe>8`4HjDdVfQzLq)Tmd0A^so<T?->n4`M}=b^3fJ>87<bR-1v
z87YVYn|O68&P8d^PyeEy+2K$Xn;h0Sc~chG2KSo$F<p|(4fl3{YjV6@5!cU333<S>
zqDPfhiwcK)SiOMUOqzFQtm}%JdIlUZ5~aTU!+@k76JrF?=N57|L5y6>PM2qcbn2UW
zUiE9X%Oj3z2Ko4=hJrdl_#88ZziB!B{l|mLAj6Hh(2-9eGpA+lt{%ZHZxBr(O!d`J
zeVh$skv0_0I@1uy_#+14LRS`U^Z#kVrMvBt$!H~P>teLi*tW1_e@*m6{*Wa+YVEN%
zApWiVBEzOuL-rScGwBjX;=d#l@ZP%7bur9N7Y`c1<c_BuZsb0#hfY<KqeDIygN>)o
zo;_<bexQ=~10aC-7%#%fH|*4m69=R5#k*^NbhKKo$8Q4h8r7l>7uu67hRc!#kD!*4
zcY<1Fcxzumj8{C-9{wda#B^+$lQ&8MGBalyDqX2MGvz58{hiL9sp}Z~rM3RD?aW+U
z8IZh`aN?owp{H_r+gdzM@0|EX<{FEU@Qy;&{}8zvHTKs-23*6Vkd=!sT1N~qVwz6d
z3a;`EtrvJNpjQO)hNc15;>zTlGM`PYfK0z!JU${;Dct>Y5VtH=HAK;8>j*va2?%J`
z-mRL?9@C5~-ACmq`|p?@MEd~w7R=;WJO1e0SJE&@!EbEZ{X;#pnNiF5YJlVGNHRH$
zL;{VU<&}~-&BJ-+_am_@9sr145Rj*<ZqoH8LgYRi_|z1PUNS1_qF*Fe1_&Ux^vU!y
zee&_aaKmRi;({}@_GQO*kAV~wkSGZt{>HVM$ffTs?h)Gls1R5gz3>XG<ub}|HeO1>
z&0L~+R!f-X?M0$W8dz}-tQ|}R6n~MvAv$-K^O12@?>b^C_-zeG$xyhU1`w`Yy6_-6
z_T!s(6eyU)jz$gv{_m)Y{<>G<+Kun2)ul(dOm}PUYCM1bd}@l%T*+0uO{eHh45V{e
zl|uahoMyJtgR@HcE6J&c2@0JGV8PEt#lCR)%Y!#7AO7{ATcEr(Nu73k-CDfMv}*xS
z@#6HTBK%1@G#dS0>mF4@5DQQ8WO%8%Hta9-$hS+7&H?~t7zkN>+3^|;SW(Hmr}#$j
zQ3(kh)GJaRg)!jje4SCd{+~BhOLiAi(Z46ES|qf6&+_Ts<-x)Rv`bi*h|}G(-I~>=
z6xYI%OY&S%-la*;=X@TiVpE1K7!tjEI(dm6`QI4#Ck}?52&e)-I{8%vle4!+!-=O4
znW6UO+*j$bp%s+$y~t-TZNsxOdhWMT6na0g?lgmKlM52(>~vJ6#9Aa((|}Lh-c8xM
z2>O6lzt9&X4NalUm^zv>eV5LPl0S=z9z54zYcJXiPJ<qN#2^o7lX@9WlKpw!t#8&7
zpsfcLTTs>oK`%FOe&P%Qq8oa>e}HpHN$~QU3BX)!FoTxz5!Xp;K5>>tahw;U?e>`N
z?XBXE2qzi>My*oTR;N9eUFSCQ^I4xC0a;cXp}m)5tt-+8Saelhq^x&+_auRJ0|29!
z*?~;ksrT=%|H;yJxUqD7LJRzQuG6tts>qb^s28BU40fE`3p(K!#e&JQwC=llRnMQi
zYpDEiDSr0+43BnhIYkvnpY4ubaRWR&CFzlX&p3^Jt2y$ql+7<PDCxW*LsD45Ihg3|
zuGMnpo~;rp!)lWEFr>rVE;3{Gp!mSWwJ3%8RE3$)`|Yzc`ohhHUs^htS*J5p+s<T=
z!g?@Y-%)Z$KM6U<&v14{Q)2E90=G5PZmvFufK=a2p>FDr8ah2{@mamsu-<6|d_Jqe
zoF3us+z{0D307qp!tx2t+1CZKhV$diUA7eIED(NcR}6bH-V(Bu5!Oagr~TAWz=PQK
zC2DJhtaFMnTFcU%MI+()UTPaXnGZ!D5ALx-Jf&gg!8f<K=&~HDsm^0cfRE{%y+(VX
zn0yur$y)RPjlf?9;i(InprX8hn|<<iF?y+vW?@AovcV>br;c5ZYwKv8e*k*a&K52<
zl6!b*LQC>?Q0y5pFYgfGJe2G$81Ru=7x4fx$`PrYi;w2sg;OOe^9&q&e);K8DX|=5
z%@Wlb=X|P_ktxS=tV3c-IvCvT?(eqSKR<FZJ&n-pN!v_cD-mF{l^!A&(4lLDT7Gzm
z!um@{N7)cGJLF*eprx~KBxyA$C#;)1nlKlBtrAr+*nDxMT)jx~6_FT=`%-JFgsqps
z?;-%HY0v^MKCe2T!2idGNeO^B;!urx%Yk`4AHBM=GI?T33%Ir9|A(a+cNqTL;9*=b
z={HZpVblGZ+9jEceBxocQ4UM+pCMgODxdTN8A7LxYE2K!vH>Nn&)dyt{Dqx8PJtQy
zPrb+77SYQmw>S-iBJmB|8zqC}RKo)oATaRo8vpO*^|MJJ|0BConi=g&$y4y!Kg?MB
zcNs{U+2tE{H9m03>s&5P#vMl5{<}n4@Vh3^*MaX^?`&_Rro%RJH#OW3#qPh0gI!<@
zkYFwlcViD^Fo<%2#Kqn{bYlOjTt8FgshU%V++CPOcWW5M?TjR2(&(!Hv&w-e69K?!
zSV=tQ;Z1KOYQqr0`h5_CJ~;WGz4f`*w0d$YU$LvZ7GNBT<Ksv2lo_7;tTZuKvv?Sw
z?f+dG-zDoe`2W;xNJ-hIi6wgMvu3Zl5W5a-lYbXVar3j2@R-<}dyI4%U;>+u*Ex2C
zublx>WB!@G-(3BGjK#Xc>>1Jq$Jw*YxGh}M)VeG#{t~sFEh;+Nd+NW8rfZ`4lUufM
z(_lO<cMq#S_+$(x$j7HH$?TvYgj4rB{!82DevW{14;HJ%gad<L@#<zQY@@`2dFh8K
zbN?;_88dhJhAGrAULD5j*Nvfg*Gv}u)2B}?(F6JaS<io}8mqxn9CV#WRZgx5&^OLj
zz~&`zuq%`eG~mAbS9z>i5m?gW<TLysCVG!}^X5%sf^yY34iXa&kB9u{OyUg#$pDqH
zRaY9Xt)vv%iC-dZ7%y~UHx5a+e-)rLtw#y;CK3$8XG%n(al(9j4)y=D&dr3#wV9^q
zl6d*Z>kU#=M08?e;;bqI1Y{QdtEpqxp%c(IJ(`rfS#;gt0LZHQ=6_u@m9CRpkt9rU
z#-_%9o6IGj%g6)3YE~u<_(NOh8x#L+Wq@<w0{0q$kJlLzpd&=raKHc3fU6(SKw9T|
z_BL4s*>csz{J*`Ib#kkXgh}xN4D_eTnDPHQ{FLsV{mN~!KG1$%tm6N8cJ$;H*g&Yw
zU@uIx%{Xo@J5KArtXwW&Qf328jFn1S%*NwwasjX%K*g>yI1P2Cx&bSt+r0a3m#uL(
z`@d)D|NZ6vsu{S>0<J?rcszLUEmP1Gz@&yDCU8AgbI~Ql$i)igIwi~yPH0_9l_|w=
z{?qRVXO}bln`StlHeMG>cba`lufp$pCC7B3cmY5U$BM+yCbB68S?8`#v|4!Oe@Fp7
z_4&O-l-{jERH-zB4Nm?3^v{<ZszU+pK$Tg`CH>q$cGaQ3tId`Jk)-EzfjZ}|c0DK>
zR_TNAd*Wk-GPN;39aT{y$e*0(bGat+^m{Gcz6)w~F`zxC@14X?08-|`-r_N!fumL$
z&Z;TL*8Y6-Y{-!Fse#sO*AH7i{4rT(y^pZnCVuqefRt*c{eL`ob)mHaf+pr2{rFoM
zfY{kXc#exp19s>K3U$~ksT1`GAFR=BxMe%oWSW!c$IFxUcf*&mKqV;U?*jR8>4Uk4
z{TJBS*dR)dKqKLhI)rInkzdoy71JCbH}hX}75!JrRiIZdaEIp6_wwH3vwrT{X<(*q
z`prqgwZ7={>W2SDp=+xly8n?q$AU7kUHCnH-U_BMn*4pBe>d=d*&hcmgvJkP#nml3
zaq0FK|1|wc?<ZdC2d=HHWSh=)21V6O{lNuQYp&dT>9xnCE$SGlNQ9*N`d7)%*G$lY
z?|WiDIM3br;f4j5p7pI}`hl6P$eZ?Y(#m!oJq}9LyiwL+<?K99s<hg0{J(76|F1j$
zpEK~dmM?i#NbS<qUwKmsu4O<BpxkZ79z?9he>~N!AR|Ic7T}@B;2B_7y*Qq>7;%Vx
z+<AI9o)b(o*h%^2)IYV~;oAfjG_DHKjqnWv-6FFq_`U%h5gz@~<b9Aw)<5-*8E2P$
z*eNHVBn2yC5S2*$vyc?D#YnJ~gY|zLuk0KGI8m>QtIc7u_aQXm))q_zmbxl&)Xd{f
z@n%2zdHzP}xL<YEvr-(?u2}Fs`w{qUVre6pmKI|=lHveWh);{rY=C@!F`sh1X#yEF
z#q2YR3a_sg{rUI8!Rbb!M|0SAn;osg<2(GtUw)%#du9+**?!3-fW;RX>%sP2m?+&}
zjHwZTO_4LwSvi?uTE7=7zNF2UZo)$!{Oj38nhiL<yj>tjPt$p)1S|o3G#)6g&&%f|
zjYqj#PF(;8KK*mlqqBO%U5sh)_fy{{rnP_H#8k>Zcg*D%aTM+N{*>GAT*FU8CAbdn
zx3qg-Ip8=kLzYMJxTZzkK5F&YnKy3Y#Tsu8Va0X9t{qC?Bx?)_-;`Wd%w)M7Re4Oe
zxXg(zymJVaUZ)bkWtECW5bv$o0oLIx9XnQ@_$2qw^`}n`F>cuE>(HpxfS>0^_Z2Cj
z-$y)g69_W^=^o0&$Lis)Osd*{3i8%;*vtCg9v7hJRvLc}@59{F);ntb81>FSZ)fKj
z?zRX6T5omJzWKlY3@JZsUGh7We*c$b&d(&Xqkr8=JhSY2^lGQe@lOBq*;L0{U8O6(
zwidQ&Q^fz5j^EWYJ9@Jdznbp<^7I8TvGVZEtY6y_zyG}QpA(w^P7tvzZ$A}e5M-?`
z)?#4xeRmG0{WEoErqx0!E0LeWKVn>6WRBW+{Oq5xyZwL^O|w!D3u`_X<f$1&F9T10
zpFg`(Clr65|M|qB8Nz+}T$?980nBgh{7Z{;d|Ib$gUR>a{O2+skC9tu=*PWYx!-zW
zb7C6$oT2i)DW9MIX^OA$riUs1z75aEq?ZPI@oI}3#X@qxS9^i-?(%6b-FD8?Ucle+
z@BHy}aFVq%mntU&>6)aeYm)N47UlIn8;f1&UZ*489vm!LIHT^!r<zwK$}h^1R(<ZM
zz3W2%>`|Y;Qo%PxH<Fy*u25QCoQw`Ghup`n-^YIkAwDPn>0loI{w}+bfnv8RrEoyU
z+F4RVRiAZHO_j1T;4ac~{(H;te>T9o+f^cLl7eF~5%{MA8loF%1p<-A_~`*fQ4VVd
zSp}Nl_eIas|7W*y^Cnj(&q+79;!}+buHno`VfoI|48GCBwm%62NFXSF`d*?|#U06N
zK_MaShh{E9!0nV%)%Ht*!pEF-|J~40U*0DJ=o)eQ*<U|{VS3GSnP4zD3E=vj^}U79
zT#x?eh#ggKjs|QaBv5$_ECUw{Noe!(@W5wra&kf!e!kB3&#w4h**{S*e7aUtR5U}`
zXld?cKOI0&f7S{A^m^p#iLI30+>i0NKQl!<ox9!8vo=>Bxt3Ml5q&zxb+mwoExZr+
z_wn%|>b`m4(k$s>@;ouoe3T`0%#?q++4MN)tWxX4qHMv)hP$nH(CJE>LHijwDe_sc
zabH=O@BTi)B<@!=1<J@t3c|yI*pG6xUUx)$f~yw056-u>+f6BNb{|}5X={$y(rvE{
z74?YzzEuz3>lUqiN0MuFnET@+#YZUw8V3gl?4k~gKQhZesvp(u8JypSvx0a3a$QhS
zjPmiW98B)dr2IzYmHg2BFtK^x&gz@`#!(aX`AuG-M(&38!RDn+_d1I`VjR+VVQA)5
ztQjqaE)n|KqRg+mNTSJhE;ubpEvg6zyr`HVmX!G%6ls@gqy(W6uf*ncC@&)RCDMY%
z9`%<h2}MR*q9sJUCFG08ukM)}I_MrKQtAf6fXF)0*MsRCM`!J503AwKuTu5{u4qbh
zUlbCGGuqLHYu1F%Ef@3A@<%w9ypCfTKYSV)!UM1W@X>3nvAK9wTx>o%z{vcfjK5Ou
z)a)zISn;?xH7PP=$<tX#=YX$G-LR}>f%{<HqE{j{JB++q=ksi6fb{cO_Org&kNP#q
zp7YdJ!?mzglDf})v5>ZQ%^SPaBKJ0&y{vC#PByCB9KA@-^C%64=0<f?7K;|U-aM4N
zpIWl@V(jY#y%rB#YS)Fjzg?|)wfbwDv&Y7OnGq!T!RVU$2>Bz~YkhB}KZ0&6>5Q6>
zDFjZ$W0tC-@(#?8>%%C|A`XOHD9dhL+vB6bMsC7e*qRNp-iCX5P-YYx8TFoPw!BiB
z6(7=l5x}ySrKfl0XHq+MaklN(QekJZ8`k7=_$?xX_?yt<VZzasJB|mD^9oRm;u0Nk
zHG3^NLI@`ao6=Jp4~i~^BR5aL_<u(T9B$FaU0-y^JlC8>{6&_)Abl1{d?U3}no=;(
z19tXa9}kjG4Hk}!!tV||ALtt(io%j&q$z(4%nWdfL{}&xJzdyXwdAnHuq~na#;(09
z8?QQDNdEEG_*?)ItaD*+`u=bzuU?-Aj!*xD<kMIt{tT4XNu*a}pZB_F!P6AC>?FK`
z&jfI2u3(-~+EZq(H1tn5vYUug@kS)|8gmJ;SRE;H!zDq2qm7Glo{MCT|3i|1&dK|2
zL7NSxGi9xMG+<ok>xVa9z=(Y)oH$9+`?NP2a-?#<o{`TMe$(hQ_bdG&9t5ruu|?rG
zz&f|$xYZne>8r+Z%Ax33&d$2f$LP0PjRQ1Mh+b_LvAsRA_T|2Khzq_5Oq3R!pOfz}
zfR;A?$$Wwa2D%ic)By}MKFR6LcGE%FN5%D6<*U4EViboG^3Dx#Y-VaGpzM#`F@AGr
z_eS$MsMIrJA$PR)LzqEdZ2Yfulj4g9jF+2RD%jZ{jHgu22F#UvLf51AP39@Ba_Z#~
zrBF**G8L+>E=3|PK^N2!`VVdFCj+9I+@NU&>d~*(>gC!|)0wF+JmVo}hyx{t4%b|H
zd`ape#e(05>?<S~V^C5Q?0ktO6_QYSW-cHo*@>5KhF=5UoS2cS)H2;VNQ!ar9g9R%
zLhg07284g<_&ucDK_`#C0L~D>CGFcxcE?xro!Ax@)k%xFLixF5b|gcTQQ1_d1B~kv
z4v=5A-bsX9r{bxN;$z0>?lQzeb)ngG^{kjxRDdOVvsbFnL6&TgrQ0^*>Dzr_nD@8=
ztXMD=TRfW_G?mb2q%IB}C*DUemEv4-O~^CFBjmI{y}lnKxYMHc<&a7=xJNz(Ub3jW
z8OwSb`jiZB+)W?rx7QTeyI3NceNeK3!}>VAh*P5klwf#VWa|_-lycBJHnJ!l9y&xu
zYw$B1%$b$?ghuY)Jk)FfhkZ8i%S6uV>+8n^@*UgyHi0pKR$oBP7CUPisY&@>qCktQ
zgSn7<#^^7)ji@ur`Lm^eEZW{j0GT)`WkvbsFNwPd5A`V-PeoGw5mej0<Z4Xo7V*|&
zRjg1h2Wtj4CFw(kk&~K`42fIvmXDr+RD@Du<M6jjPCSHbUf7P8iA{~vw+dF+RDX!w
zZyI0|P1M`rvGM_sid(deB$o?J4pIn^vOI@}VZ%w9BXGjVJXWjSkS1+Lq8mEI#py_q
zDvOhPiRscgik+hdI(5hUB$~vhwiw>Ej?Pl~Nlx(GP_-i@s?}ct>xN}b&NNJ<QRPaC
zV)A(hSJ4bHu(1tuI_YK73apZTeo82Xku#7sk%x}1=oifgVv40HZwCZ#jU58XwUL2=
z0RuRJVj=`V$i36bD`csw8Fxh93al^Ykq|57)`<sa0$?d%<!*LJk0u1U99BkRcNf#r
zfYSO!;(+AnWiscG7aBlVzyP<oASDYDd*x`6%Z}{wDnh_r<RLtSr5`Exz_9YYD~{*W
zJXl_MHv)(_p!U{jl;1p<Wn7}SwUreFGARVa$K$mR-@bh#9xfoe#_GF7k6;bJGd`fW
z81A^`asU3?Tt(kMMu7!;lj5LKZ=n&rM_z1XY2;2-7~1nBmw9@jF67P{_h@21-d*eL
zs=k??{t66t*V;F}D-Z}Ye$4RJ4Qmw&syIn>aBzp`NG3uXj7Xtlww3MY0&H04BkR!T
z2U8x9S|MoiZu_ZGZI`M=3PaQgbE7C`Z5ibA5c6bVVJVE3G>|BomNuu$H3DopyA=Rm
zWViKF3N)3XXWJHWX|)ew?Yw+^_lJJlZhRTYS1|qBhO9ykFA~$Lzg7Yg6N^>Nox+J|
z*-E2EcI+;r!nTWd9CS+hQ%!AZ6PO<cvcTjhW|$~b_<Ik(WXy0_tEYrD{}kY<iN=gO
zMc)b$1?K(BfU6DDu4WV_l6KeK>52BWklae)M0}dTaP(|+AinngOaRb3Sx-P9!-lwf
zMghli%oL&XG}o}5bhEG9lD^zlSXNSg*ukb$jZA6Ly@Nct^eDC2=pIe-i?a?*aq#u4
z4(J&>1>z+!<`~A$Eb768nmyll*~2T;M)ntIaravd(Db7+jSP+{ct8d+D}{>$H&2nw
zW#pQ87H12pSNeZTLK48^BIaqUvOs6tr1*GwBW;^cK?lz?v=mld?@88Hz4E3<wAs&Q
zQ)$hyU}F50XQ6{hnO&E><8RcPk~zVnm{FnGazwv4Z~{=?8R_ybovey|#O2rz!NYab
zx2{jcm1nxcKkBWYFfuT~ELlt@-m0~vx;M%Qf#(L4-_nwtidPsJz5U}^FdntvLmaPo
zssKI7&I52G#3RYAzxUBj^P>xrIb&tw&A#8tQ)VIq;H)A$^vq^MS7tHBrdB$2Pl{`}
ziGJ(JOT#t3I;yD1hFK|f%g*%x=ts@zzxx4d(5Y-+^|xhl(}O38WTkGWNm`({X8Yc=
zO4Kvr9lW{?4%R3RHDZE{4BDe-13awNfNlWjH!*P|U+#%FWob2#{{oHalG0z~rcVnP
ze-D=M?={vu+nl8D>eyE8dI8z4?Z{2_m(1Z&X|#~R1fpa6OSgB5Z;hEa9N7?$p$}Wp
z;9l(8I_HOAhIUC&CJ|nQCZ)rHesr>HSMC*gA%-{ZP-6|rTeN}Y0nUl(%oU&y*EjNW
zaxThporZWL-Ml?KK8{}_==UVMeDO_}Q1K%m%_g;r9F@`|D)9|*Et#(f*gKb1gM4&l
zFr}i?IBxPf&-v0(anr;@yY;nq=iNzyi^ii^e88l_+1RrSTQ;${@IF!R?%KvVDC*U1
z`$?ME+Dn61@yb^P+MWS#e_ZnVj%25*<4Kf<@lJEfYd8HCn^%ObrjY%B`QN3;VuOv%
z`mAiZ=X=*wN4egrUJ{$XY?b3Gs3Vao=imKuWX5-!tzGY7M!4Z9I*8M;i4o9;nvm=7
zu67({9%w!b-Jhcmw7Tv)5iGUi<b`Jd6?|VkJTZ1T8Ue>K(qXec_B6@t?I}A@cXHUS
zLftGMIsebd?<z_WFjVU?qY=g5(lWCLwl=$+t5+D!it;(}d~I3FM8aNe<%aPLPq2au
z5!zLderUEcH{N|<@9=(er}c{CDU?U-&JGnImTLrrgt}h$E#0|XwB{sRH(M^4Hvcvu
zy`eA3Ggc!6Fho}%!<hr}X?00X87AbF&5#nw?n|sSgUzkZ0sGB^SWOnKLz4>H-+wT-
z-ne@pk&)iqhYIdPZ4A$`5;ud1{qYAM542p+AZ9R7TTRGan$?Z^=pN&?ZGtGZLnkfa
zpz3kW>of{#<-NOh#jIv<MtCkDlb}I!1ULq?qx{t%(aeWlpC7gnF}m@B`rux5nm^|&
zFSOf{#UIs2s+6bB@ba|r+zKDtcfz~8J)8%ZT37v`BKlfsqX23F(x?h293VwHP6M;>
z74%H^O9oaq!ptT1>zhP*)PjV6M8q!m;Vf*TRp*4_%#f%>Z0tN<c3u&%e+EN6XWStT
zHlBCV#068Frl?CK$(eY4mH=u=0<5^yjk7J7*rVoChj%yF(oD(km&`CVPTQ<#!GgN1
zUSGr&7V{NfTK9D8MAvU2W|rvaS5{Gx_0?q;P-y3jl$o`!hHIbCqq4?EV>Qu{KDUvn
zDO7&cR>IPFyAr7`@I@$aw1F;bB|K3(d*abX|Ln+^k!xbteBvRrb7J4HmE@`&$o}im
z;^OWJz?6(eA#i{~5?t(3NQq~Jr!asut<iikMOd!S5B+9m7;L1urzGcBW%ODInF5_r
zH#}p)Gt`}Oz?4hVl+WHCTFitlT1=xDufubt0(mBupR{@P8SfeF@4LuuNEJD5zx6QU
zkPAX<3+VyTJ+21VV4;76U+bzi==Xo<Fh*94t*M0S{QkG-+kIV`)EVAqC3J9yTBZ@+
z69cZ?m<1RemZ1m7NSahbhcIN=KtKFfU?87R03H%@1jd5m{-7LUivNr6ARghQedWs7
z9gCB)9?f=UM(rTHAlNv4K|VWJI@35y7hB|x;>}0%>z8eBhvkP#KpGrA<|5}MbE>X7
zx3O<OllSU3iXzo#s7I{v&LBceiGfVjb}Z(d(Ck}XO_;*Rtqy6@T43_OCU|MGQhswL
zmn=dUFd9J-wrJLeg5kuLt%!zC9Q14zAf(erd+Wy2L{Zkvz*Efrx-#!`t&i{v;KW-(
zDj5v@_48O)(kh#P%!y;ijBf&%J6`cbNb_$&>VSU3QEn4gH+~G6E&U-aS<Z|!y5xxf
zY{5krLFW~+1kX5G!qrNa1nKH~xWB=fA*B_*=Gz|V8Aaw!{kHb<3B7dKjLxJ*#CE|<
z$bp-<G*?G-AfkX%KHqV7ZdN*bYr0`4iJ+qnO{}wD<NT>rTaga>`((f~Q>90_BiDwy
zMI<WXZZt|$ez(E36mF`2^I4B#IJqpVR_t{Z!#Dk@DFbhe06#s!a+?YUHLC?Db&6l*
zj=YDjFp8(S7@6mkx0r&GTw5FsgaD`SfJ|nbe2i>}zQ<L{vjHCO#SBXo18qV2b^KXy
zk%)P+KzV5RA5k!=wQmIjjyBAwVV?N#)@!ccLS6wMa%SDKIbcQcS~9Wx4uf^$MOi^`
zEFni><AtVSQ(ez`;Mc6mE|hL$;K%+;Gr*($-L!{bOhpuy+vwM}Ds_|uD8g|NKCjV#
ziyg3l@o?qJxc#^nE%)M8FzPQy$W&h=9VDEVuh4J6zF9asF#GjpRxp3;Bm8EU;^P8f
zo2|P_a>eDzy~-aDUaWWNo0zt1%)KAkSbaR*R}hv-g%_*VrD<raQ7<fcGRv=(Wb_X?
zC^p-(nFUOltrc8t??1g9Anm!e+t@{}7{-{B>g%f+*0u9<pSeEIFB6<v^k~V1I8c2x
z?h>Mn36?*iOJ94>$xt+GAQ5XrHsE`cL<$L8*%hqu$P|BSk{D|x{9D)ENt#6A2o<a-
zN=x2e*@S-(g~&Khh4;-C;cUCp_N7ChXx*I;U|T(?P+O574}T#ZGboxVT+;Xp?Po#a
ztvJ3mUDDRK!Q(<D>mII;4&qrL;s!WL6qj63+Bk8|#Fw;88lMdp@J%(Bc1~R-UvaHX
z*1WkEFDP`8o`<I9IzOjV#7gumC5EP$BK(=PX{#Z_5S%14{|*^_TDxMA=Ws>n`DhWA
z7o=(R^{iAZM}%4fviXkYhkp0TNHzm);A_tHt5+wJ6aqhEg6n5lQ1F2jb%}O!KQ1N-
zeN_z!OOl7w-K8>@k#z8RTwBV16vB>#%U<1pDa70Cb^Z7$Sv2+9{YgM}T>*r%s`9I)
zXO#IvD+CCNh#YctyK?fmiXV2Y%olgxUO^Z(rJCMQJIbv&pKKs4p;!`v&v&<qGJa^3
z=G1Ky-OfdOG@xA({&n+7WRPYhBY3c}%SM(I-#0oDUprqp&s48UIT2Hp9B18fQHE@|
zDIngD+#?T@VX>(m8vnvek_N(;N3+$|UL?JN>2c@Pj7UsGHI55gGcN-FRfY5WLbwX~
zQGT-_?S?umUVou3^iqJrI#0C6I?Ng|SNIAS9B&O|sYBk7vr=C>2MlIFT=~0OI<*)0
z=?&-%2Zyk?&vD)I(^_GB-lF|dg1uymk+nH_?UGRbVR=-{=2ezXQ#`Z7jppOqy@U14
zQIH=pw`L&k#y5BPY#W|vG7~AkfLos!n73w7x0SK^Op~4F-8++UbKzUf-(qbPtVHR5
zpvO>!c=slSSNoQeNcf$YA2A&wEtthtw;k`eolVB?EX|#P4f^Tj78$wq!vn*TCuI1|
z48jK^N>9NgvzW}SUI~-V8PLghr-WjiB)NoyBK`3N1Hy`)4Q1o3hj_SrF^A6nHw{nW
z<34>KRn8u=SoMZl9d=V~OY&mTWy4z`P8<I0{V3toENb>xx=`4J{+gDOMcb5FfA=S<
zMUSSceExwz-9|Q`bvDkgFfUhPk*eAyll5~FA634M4>T!5`LSk44uwyOPT`v7q#r)7
z{_)q{el~MlSzm1-IUWC@GA=p%BU_wKWnZb|j--h&U=y7rnX#%v_iTa{5T&yxw&r$c
zh!9DwT)a<7(QS<@6h^a=0XFcC`r@dbS5fbM#m%kINUfUCnLzUk>*>XU0KcKav+8Q2
z{`PW*z*es(QFLO#Vp#qnVuAH$1*hX0LqR>tgkHXbynos&t=Wkm>s%OSTw?WCbpC+6
zl_*KSXBCxfl^L85<9D5UUsTKUXcYno@I&j<p`S26mNC<Gt;GA1XI4gftx?n)J1C15
z_RLw@^&+zAUZ{~djT)J#q7r)NP0e1T8#D#8rVJKNGhM0ak8|PB^eYO7erw8$o*t9%
zO<eSv*U4sxf_215JK)$LkC%S~+=zoOj|bW+weL(1wOY4JQjRJ25ip{nkcz!}2f?(=
zxE#Kvge97V!<46RVyJ}BS<>QLa{tAvQ5C@J&P<=lG8<vQ{9NfPSYE9Q&E}Gu1NY6D
zGo$y__#tZ0sW2lFSKXTAM0A_mjy1ZguO4$FTx~hYAhYAxR=XctUA(z>q4!VKXDDfs
zg?lkw_d~9nfVT|F8JXY8+1Q-z!Mxs4S{~_=Xt%h`pqzuo>2%1Z^v!mA^OT1PuIXD`
z#vM}C*Utd9#UcHci@ph*%LvOUf}sLexNrR`xEZ);Yx`Mo@%;vS+K%<$T4!m9ORURf
z)JRxwgM9cOIxgs?Lv$?>)#|gO@-7a(BT$Ima>sV7Y1hP$1}I{Qdf7hT72~Tj9BEhg
zlPWp5o27;7cTOs+7)4}r++w`;hgw8LDr$szL(%=H9=1CgS61?61%z91)!XVH5RJlf
zNhh`foRas{N4mtn#faX_ZwkhHf@_QnWWD1-5=%d#F!9uhIgY#jXQ(dnIcsr7<^mMM
z6Ijn&1^F7MOU)r#Bkhmyp#(>79=+3*IyZZOUDPjek$%PRLj2n2ZvQ%~E~#NSDYOT0
zqog?OgPxiKT$$$S<^01_V3+xasQJ{ZrO~My015tL|Egh~uagZv^qML+vOO-w0m5y(
z_kNmNYt$+n<z(ANlzsC=%~YyT8z3Nj2koO4jm<GN(iq=$3r>D_W?4}#P8hi#T|3gr
z{H~<Rb}zZP_U~v7fgZ!hB0LHVUJ91SIB_pH`&wkh4&VLA7K5^g3Pg2kuFSF_^9BFV
zci`vAt+Q4Sf!l6}^sKf<Y)cwJpw!cyn(J>GZ*59WYW8|{ELHr;<i!(L8mccG7mh->
z?MPB=rEs$6Sk>Ww5RKKE?+cDlR)2&Jv2>MLxg->kLyz5(>1iL(v&xxcmNI8WjW@5-
zlnlw{1}$2bo$*qDmle|d)19+5+isJ@)be6E?)<%L9%%~h%#4T5E@r06W^b1iS9g0O
z^Ub_VFLtWuPMM3;@iPmAzIB>z8o(3vWiSo+)5~fviBw30vx)(<xwcqKJSy}S+(0o6
zTI(1cZDzC=B&&e&T44V!eICW!uS5c*&fFgx0sD41OZE9MMaCs<d-~5tbsTxItGyq^
z66<~@S7hbe%j1Y|z`<2}^@gBD%kd)XXu?l47lj<bZk_Jx&{w;?CRf(ob!fIxHJ)k1
z0gbm-J)EF%T9o|(A%{+&+Q>K^Ru$uUpa=G@2$K+;Mu>Pz<ElA$^rj@`hRBE0vYXB(
zBsIT|=}Uu2B5Fg!c`3yiC*TmQ1e;U!3SoesryOeP)|7X%C&ub<tE&Z$Uk}L+=a(Z{
z^rmZPWm3z6(sm2z%!3yMy5A*UIkJ#}>!%$B4>@?|nPu!p|82-FJ-5CAI)T*N0)!PH
zlQ>=xm=RY10<b+WW9Rvi6)h3D;%#2FvUth-{AE|CuQ5`TOLDcHlpCXO22`7N&s(P&
zE9*?XqnKg~2_jpAMh12*4Ve-ny(P;AnW=pn&obs2Z`{J2wBsB|x5vCvd3Loq(qitC
zZhvu+g?KNa(uAetOr7q^d1;UmpI~7f{W7j^NMYN91I~@DR7E7LJ`&OcD=RCnw^wig
z1PQIMIy8?NPDvJk(nZX_xXKjD=4j<HIfrcHrjpFBMZeAK5LZ93WqL>qDXkA|H7>R`
ztfyygmlm^?eqe#>ou)AQ64VArtP9)riqbSSfaAng_mVHNjQVCwN-zr0S=B-U&g)~t
z2C&lDej|15Y`V80q~#I1>pP_=E4-~`M+(IXO-`EF;~7qG8Xrd{=gHJG#JnCC&DAOE
zb;eAJrPUo@IX;&ph47Bl**y<V)2p?nZip79v}nMRKfS_)*YB0Mh3|LS31gk<yRA>m
zjnr<E!lvM9vsLt$Xz*~0^_G~Q4%hH<1<ZKJ)`jUp+S{Z);URZ{6(n6NR<Nh`wOxF+
z8=Bs_g~y$b%;N^3Ewos(EB+Y=4wKy)!Rj`b8Hg{O^)Z(E;k<b%=DM2`Yb};>T*Io*
zfc1QLnI{OiY%~y<g};ic6~vvP?9rny2z3{LSsBxo%k~bmJps2uQ#S!L9$*=)6`sKk
zoxVeF__R7|J^N9mNjjUhS&s;O3A44);y}~>CjaH1a=hrgMQSXSALNtDi{%$~+Rp6g
z*ITby)|VRTPa|9<D+6v~!QTr3?74UzE2Ks^4ofU=LK3K9swG9+UFD6phgfmw+z9@H
z<khP64f2jv&uWZ0X;3V3fgf~NU9Q=<XmyDi(pgwg*7$ASeuM;hg6$4gqVU@&jK@6W
zT?RKV2ys7p@@L)4b(9m&6c%^Ib-#@j@3*i1sYW`-c^#og@;1<@zN>z<^8!2}v=Y#0
zlnv`WkXFYpBYhiM@pa!a_agtLlbnx}Rs8y?j@$3<8^TQ5bp*pp0F!jj?4jjgyJ@?X
z4i6IE+yUtMeVOJm;Y;J{&9>&hUIf@2U|tIi%C>8#*X51dJD3^V-?cQ6{=N(O2b&~<
zwRo}w!_P_x(iUvw$cvgN=ZFXfywo~Iq=EP0-$LlW!RrcF-(uM+<7;P%mSWybvE7T!
ze>t$^y*?{1#H7EtZ9!^(a1Z07X70el6DbeUaK?KoTU}=G^*}3W^JkZ1jmqk@j7jAv
z%@cYm@;gqq)=T>$qT}Ic${U;6R&R-Kx6zNXxC`Qf%JfwXx+=HZJ8LAr3dE-taz_4{
z$KN!gZ~{JWLpQh*JsaD`4MMCOSV!ePb%3F^dh5C|8OR#_#!6t{s*b{g8uu>dn$KL*
zq_{UNBhVF3=wFO*Ni}xMZJVJag*&WVam}|2MelXGUj$gRrx4nzpW%o))_ZE1v7P!$
zC^)fVr#$9R`IB4i_+jXkdMSiR?iAh7{$MCuQ4kqgGSSg&hgJ}&186j@*qXW3tF)@M
zRyXK~jgXiVz{<K%exA3~x)vB75ZuvsRa=LGcq#>lm>enlz2~&^;g(TyCOnPW*V|o4
zToCOSUv-Drc45D^LD0wF7f#oGD_4h13!NZTSY!11M1l)m8ZR_~y+`NzO0f6!7nO%l
z^P(bJa%&kP3fAqZF6Seih5JNnHSZ5wWki1@3TJKn7SYn6+>oXYmm4k1eS)2>H8_E*
z1#Vky)Qs`*NI}!^Sfeuk;8HHi>Tx!|FVy89N3A3~rnd~mvHgRbkRe=3%x@t9K-|R5
zEVG)`+#h1*9Z*i6Ic@bbN4Y+S(0-Kb3fN&HVsSUl=n$*pq;r^2ZL(c<lSpOPPG$jr
z<ZDWbAB=!<f<69>N?vtG@dO)dKfK?>hZzF8^7f#)?VEqetn_>ra~F%lny6b`?l?vL
z*&RTHGzArT5VNbJh_V^p=|CdhfNy%2)NK?J5#+$#vqs&<9>R5FyIaFBOw)ofz(YOe
zo-X~P$=inp0QPhf^6+K@D=!o@u}lRUCyB+ntrZI`o}a|(t%JCR4@OG{GZ(a*^=PDC
z&QoUE<}y>k=puK=0+V)4Q%zsf?<<p+<gg(e^%Q=f5qxEw-Q_tJjU|;3LONjt3Iexy
z9#@rRCc}C<kR%dprG9@0<Pg&~UE8X8(UNsOq-MgTXO%KRM%c?!HT@>^nvfqgi?#*K
z!`3)Oxz*)A$Eu~7j99(!SPG%>gE2B%Hwec{>M*(+j<9bb93_NT#u|_$OvT<Ne+rTl
zA{kkyGjDzZ9y#QjRy=Gq)NxMh_Yi*q5&Rx!-Gw@Z(%<wTMB|zp+`<95^fX}dXTt?3
zjE|n0r$H*Z3rr|OD?E>zwonP#Fo4$1JoDF}Z!I+qfkq+@YNG2MT1-7G0Q}G0Z^b2S
zJLTMmjCzDuHMCdtIFrl1+~?sCp9IV3ZGbML9nZw~YycCjnq)8(f5m0%gG)?%kk*y4
zW(70TO-i!qO6q15LL+;N!w_WN^;^;}NAU@tAWkPEl%QuWx)w1wjmqRktwh44<qe#Y
z!^jc(s;8Ijg%Db`jUJ<0GrP?~J@&z`qSHRn3{S=b#yEe<nS2f@IZo$VC9A8EN<{{a
z9d5E!bn-{w;)+U&dDeXJ2{;C0EDA)2zuj^)Dr2GGL7mKqOB7dYX2e!+FRep_Ap^V1
zGaOf_DXPD~$-calp}p`X^zI+48-6N48~~ywqWM=y4i=~7syU>JBCk<b-+~mvxKBJ<
z0ye;FA^=V{WoA4BjBXklI%XVN^j8E&o4^~%W`1ixoB_sP%)N>roulQvJLho_`w|F+
z!=;$66!x2~e0C8S=>%ZgeWtK0z?}VT<wXlT+Eb6ah)g~6d#fUgibiJ=G<$72mJ$}_
z`y67mCiTPMZYmIN`6pZ4j`GV|%$*panr(-MNCN}Za+y)t0(D{`(g;b95Py~^BUha-
zhp4+7WlGZ&4P(@}shAtaJg3ODU{cE17Hm^Ce-_k!_#X2bwH5g}aEONNh;wkhtkE?U
zRF!J0=|q~?ik#}z^#uVa)|zFW0(mEYL8oFH{ZopzIc^klBL0PX^sndso&<=Ez8tJ3
z0f?rM+T`k;7X*_?@8X5&Zkw`N?|71WRW5$pDv-)K$Sfwbm$~kQy{rS^ZT}@fsc3ur
zuW7Qd{M~~%jjdpaLG3oP54fQzH!Skz6~UhuMlg*eu5ofuycCp}cgRUct=&3qjuArc
z4%>CreWbW#ILDy@9CpYfXa6Fs)0&1Rn;hI3;Yq7WHCSqQ3$(eHZ`*RlYm1tsr5p?e
zzM_X|-><RRYaI>)&kp<C<1x~)bU4*M5J-4<QYX7EdfjrE+&Px+9~Aetan*18>C-sb
z9Oj(M>-}fdNSuDDSqmb|%z2#zU>ZeN%_uA;x*I|9H|W`C44lbIopSx9+tsO=AoZ2c
zu;a{fzf575HY1vtq~ks%8Cx5vy(Mp@Ub#>bTn4yyQUS;kHSxZ^&bJ>BLl&~+xnd_N
zv8vNK?8~rv0aso*UyphHwJ!vu`~z2jZ{;Zl6AS`ARpMpW!eb0uoLdY*uZa8Q5v;lw
z#DnEsH6a~XcU%Va;i{L`p-X3D*LR+{+pu0=Tda{1Jh{@Q7M{dzWYo^swO5abwdZh*
zE`?sZi#`J^S5#+p#u9Ecxu0Q?&OIjox~?X3bY2VmTd=%L_BOkYIqV$RxoIAc+0H?c
zesH%^*UBEZYNw4y&95K%Z;r{T$?M0?yQdi;tvZ_&<<G{hVZ$#Rr*7-^tP+`K1Q|vg
zAlm(SYUJIV1C*>j@9njBJqIC3Ej%Ck8)D|CIizvc2>9mDKf4^XSi0}kyEQX2N3VSb
z4`MwF6fpWM)eyEx;##1c@9%}){JC-_TTaoDcQsA}UVSJFC*dQze38V=G{ZrOKc(rT
zN1tF%tR9qaVvoh7EYoC|1z}g1RKAv61s9a?%Q^ePn>};a0QLM;tt)j^t=-)cO%{Q8
z7(>|mMFX{dq^%Ws&MysQI9^q@Xt5dcNHcNiu<5ANF=Yt1=y6<BAio3Dd3dN1ES1RU
zwWI0OY0}bbU&U*zx5D`s{_vT2|LN)7E<9&D0SLDdD4dADAkm}}M>GIJrW7}QHc3OF
zUoo^ENh0}`SfQYfu<-0lRF~5*gq*z<mg|l+dqv%4<o>ABv0u#g_q-a)iwSe9+0fZP
zx5#L_ZTwZmU*e*`HM|pAa>8&2G{OTJOS25a5<>{4ezlzlk@RvV=HrgL@UOX+T@;Xt
z>#I4(lnaJThs)7|TB;#`R6^=q<X>qjHWV!reH(R3V?^heLa{|F=13QFU^<C^4ZK*<
z!A0$>r|HKPt@OJD%-CNR7Ra?;k~lS-UFTx19obWa+3HjnwcYFT!E0dv|8`1|+sVf+
zmbs~sNkWCip3W_0R79=alx@G!UOZ<;Rv{!jt|z$cFp6LqJSY?^NZwfhgYO##rx16a
zy{H>!V0I6J%In$D6|a%%4;u@}>Kdl}eCP&heiJdmJ~7#pe4^Jk)~B<0D<kx$AO*+>
zl)~TvoHvyEl$q$+=7kv-eSisY=XE_#rs^vArw}BxvV|mb5N8##wv$#vT4b2NbqufF
z7Q!-`MU%2(J6(o$L>Qfpg>hN2LWS(dwnc=Wf($p$mssJvIIe@#g>A^jv-;acJu#4h
zIvZvOzz0=L<*{Q4Uu-i|?S>c1T%4=}??f?i;TM!1$t7tT`~I+WJ{?P_;Bl>+DS2yN
z>k8b2512OPi2X>TX6#rgTct|8f|@SrHdP;K*l*RKb9clwO~brr75vdm6iHm0D{4l}
zt9%$AW<)KgWIlI+>O+vewmqwu_s{M_QAwnMgjnwqiIQCw&s6Jt%;xPqjNXo=_rFBP
zUn*w>;D#-L>!`iB+SM~DTON-ZY*cBTBQ^P^@h?i%srnIsIJf8qJsKcsafXH)Fkapm
zz4Rit)zF>-!e5bqZ)WW)x0Z5q+Ch@%o?*HR+e{I%D!YYkv7*b|ffhNB!031OH+TBY
zS!F`DnU@gI@NldlM@jyPEum;13;{`fy^AW_wt$|C^uT`_I4<G>I;aovo@X|*`5Y92
zYFCrjO1Jqes@T<vDXi^L^E$3`Eh%%xipcrpT@ZB@*F0I(<d-NYJ)Y<Z>Bv%xC@Z|L
zCD8sY>P4Vu*?f4FNf<HVJ+|N1@NVtId?<2g2Rq#S_4rdpRSL4@g+ONr&%EbGhYz)e
z9lhqYkO-xWp~84d*c75RnK?<_RjaLuJ1O2@#x${EpVB|7grJ<m$Dn?=8~NECjQOUJ
z$;$Phe3`pnwKdkT)6s%<i~HD>gxArx*zc(8t<Pts7SGs_e%R<@>^uM1IT~v^m$DK!
z81qKmHfVHG<^;UEsP)ewt7dIe62q_7`<DFOC9&xMP181-r@DRhx{#V-iuslXzfKFS
zynd{a>2#T)(VP_8trF>iG8RQy>^Z9<Vcn5s3*m#rka{px3#NGjZa`!-vvQd&4i8@v
znVX%srhWS*-G(s&h4|D!{Q%L|J!#Z&+N=Ay_RH5}YcsGld4J;QY0}aK=#AH7sQ<&$
zSNKI4ZCxuNN{Y0|fFLc6AT6zcbm!2`&@nWKG>EixBPre8t@IE>4<Ir0(0q^g-uL4l
z7=F(_XYaN5T5BIlh`%Lk##Jtne<^bcr7SRFTSlOH*hL=3C!B@%(plRQvr&!W{o{c=
z3uBzu%0ZMNx^ttoa~zBLzOz|B2l`JvwaTU-3{%(g)nV3gEe4zuTR+=kW9PQ!dCn86
zs_!8_x?-{0J8_4f#X9)5zG_#L4<&$F989OwFzZ5AAjeSAvAk1Lfd>s!*r(Y1G)x!A
zCZCwej6%N%kAPOXedb+@jh~2Cy1%<)e&km+o3-X6XNO(lBpV%bW!gD62Tno<E!&xi
z!H+~c7b-Gxp*~0J!plCe#W=CPQ@=X>ii?UHnQ`^=>IUv)re(r1x7BS6_x8j?!NxLY
zUoIrC1$B{z0PBg-us^u}BBqu9#9Kqmq2Uy(V8ktT5`E5(T2OTLbZZbX4-#wjX@Y~q
zL_{(xKV5xGWObWsyO@L%W<BS=$=rt_&AiKJ;lU&4>;+hw)qB|*R~3`zQ#e@ql<P!^
zPoC83>;~@bpYE#PlP}KRn3Pk!0pDy+D94VTZ4Y>EF3S~JoQH8GzoMjtM7Qei4nf&D
z^-HpAZkH`fPhz3==EAgdTUPGF+wOabK`=%mAGNCr^fb1VJGfWmBw;&jaW)bs{y}A1
zhH-qWdw7NGL>RJ78Fyo~c30@fo-%f$z)p0kV0kck<f@@WcZiymPlRlip0X7%Qopr$
zB$*imTUiz>cAa*;Sh4BL(t~K;qb=p#wZ+a@8EePbscdAC9WVj?7WC%Ko8jgL5h<^@
zVtLp;>JZ1zv|<@zlw1c;qYwaP_X7xjnU|Nhb^{QI`Nu>>y`m6P0yHa-um`{L2O&QY
zp%_8T;xCt)FCxg`g@XTZ0KRn22KC@w$BJlkW|E0%I{j&YU1R)g^^05eig(%lE<aQA
z?XKh}L9|x$JT-4%K)<&MOt0qI1eutBl8>Mcv}a2ZX^0kir`@}es%k^$O-5F_7$~B%
zG4i@D?d;pUA%iK!gAQ=C2=cM85gb3SBac+035xIds@)uwku%)U`@e$m)0rFeFYE5^
zT1-OjO~KjKJ^SWE>_+lL9hNM9E9ldK+cOyc$Wwe>I68;d=^c=~-ak;ypy_iGytVcG
zTW_&}{v%e!1+#|0$5(rq`f9r564#gP`?jpyai)()s~ks+{Y9Xh?2-H4n{Qqk*a-8)
z-N&b|d`2m$iO(K?Z@-LbMz$|-8Ma96TH0PTxe9hD3*zmV;E$r+_4_LsyuE>xRG#jk
zABZv;+15_)qiD!vkZRXD8}Z4%1@2oerUyARZr=S>hZjCkY`3j4WKOxGqMLYXK~@c_
z6F?!WWigbPA3UnFSlOU0#~#BsLihMnJ61EWTdosNHXjLz4xn>^Tw_qhnvApCwt3!N
zp3d>0Mo-EJv$Wa|dgw9?q}VQa1R!1e<}2C#vN$am{<SYxl8>PVL|1Vo)TNrO2I<d_
zpJf;}!2Bx{-HAs<3F#+3y}1`Y{$!Ae?Zb`?^%jEE_Il`V`U^XI)KUKj@CQ0D5o0UG
z*hSp3R@(>O3Y>Js#Svyvgcg{jJ&_p^q+#Cu{7I){gp}1_FtM*1E)4KthV}dF7b(N<
z5`8mPV5$O1cXLCodvaS%yE8#v>_&RfWeX9nviymD=%w0sQ|u%B&4~4ULzn&SS|bjF
zWElPFPHMC4+m-$|8P2V9GJz0aLU#ylow}Uq694?xs3LT~+1A{$I)kz8YBjj%hpnaf
zyEGiSp1&FB{15dLq6^HkO+|l4YIxH4=e(!)hZ7UgkvO^F!|=x2qrd{3^|~AFRwA5I
z&E0^RQdXfegNH@AE`2t`EFpEnQXj0u#l((*>edU=2JN#hoou7an&VVC*{=p=K58x3
zkc>#;2eu1Tmpc`c%)1F6xAd1Bt_y?{?CVI9jN8Qppki1yJ9cH=8Jza5XaLou3A40A
zubI=fHa~NWd06baCQ;Z~1UZz!K5vv8Vif8J%+KjY#BzO*V=(4u5uv~p0DOVlCR6LG
z8_LO$bixv22Xv!}4d9D?U4Y8yw!B!e?b}0W$jNugcxGIGVd#Lv21*huS$#X&WR}@d
zs7qW68)P0TpgP->Za!EA{za#w`fJ_OH^9y!5UIp}AaQm)U;}ZPlgK%$dr9w{cjeh@
zNs=3T)m+;*1~ua@aQW_R4&5j*Dad2q{eEuww|F~Cm6-rK$A^d8W*`)!xNF$=-H>9$
z*KaaVW#vwpb0WgtmQjfyv(buucj8lQLLpFmOnr2zMcT`}&s2x(Ht3g`>x&-P3g-|N
zy6%&0%A6?mdqK`@mnT@~xtGWpn;!h-93tIzyO2z>tmp7&!}s@^146u&${vMlrIuhU
zB~zhno&^6N?ZU@T@sK+lt{=*Rf|?jN(7DK@MXT*nGvB+QeQd$EcpDbfRA?#dSMqoJ
zPxR%74uTU=sm@8n9WFJ8fZ&P9mXK9OG|ubTUY}-CP)^t^Om=sB#>B9tueWcq%Le%=
zQKwCnQ%vW|V)@AY!@U~+!y$rugwf3mqQ@Z`KJLF_u;UupA@BG3D$+@*5_vOQ0}y{M
z&yew0FWE?`{5j6(Q*WF0cdJK+`SlV<+O$fJC+jmUuo38qq}kDi9M?E<IP)Rx@i&ke
zqQt`o-s-U{l(*pSjGTOv1Ig&U-uuGZk7M^0y2tg-6_Z35HdI(=8_^}l11_x{bAvYy
zRY?F;&6yktUaE-}klLlk7e?2*KurL8+iVJ8y@8hxpxx1no_r5sr=p-ZP9yW(@k(P+
zGoc2^lZ|}zEvsb(t>FPkRN{HQVT<CSKFAunlz;IuZC1G||8v!-mZnHDUttz7cVYiA
z=W7@nzICs#$-z_UHsi7<F`W5hL0Z*4wdLpga$ge|JTBpE;N^%yGch(IGfG}bsZKD*
zH9=B-WrQl7VG)asU+S<7&fN%v1++&q@vi*G5XU-7SQ<|u)}DA&gc>rzuIe+{d2i!D
z#g2{XLPB;g`Y6GpdF;acLoz|d@5nkOErM0_gU#T%%*;7DFo(*8$Zvh0)d%5%yc{8&
z_YO#5s!CajE#_)nl84XdNaW_qKUc%uy;t7Ltc%&>PdTW`u@T~AR`%_G_a~VfzHPhX
zZJl;`<u7$5vm@9Z(;{m;TceI`k(J_IePT}+6X~r)e;C)`710a3f&M+y^ttC#<6M#0
z<y+nv^dqy|b3L{0vOQj5QR49p0i?K^`o!LXP!1)291G+F1ZbEfyjxJGXW^1{m%biW
zW~C>-aPPu!*%Ei_Wfu7Y@Wpm`5#+&+awC4DjpK)<QF;)?V5u^baP#kZ*ZF8fWx}Yh
zrMuid>dn?LaleMST<8XKf)dqozIRjS7`q+E(MO1psi#HliH9(DegCPIH_$R?ZV_Pt
z)*_R)4+w2w-G^v{!yCg1Bx%uELU?(iwK}wap7cOObF$Qwf@6%G=byvSTX2p5cgH)J
z^{kr8qfEhauB3zRKHaxcP<gob#NX}8`7T-4r88H<htwl8_{rGj*Di5iZa=l2xe6d>
zrs<O{o_nnOdQB?|Y2a_x{`GC50l3}BiKNgzTc?T>s<*Zt715}BtWBL40R=-hq?y)R
zsv3}o$ncc`(f2owIcoatm)^21O7GJ-h~y&E6?a2|t^x+N5+NB`LM|Omw<3HKOnlps
zx{dh<7+LA-DlphqDH)l8$7#l%HVc5iwM2ES<E96T7h#RrE`$$~TWP7CdtE$=btY%k
z|7e4Ejk22x@9yFAWoKW)p~5E*`tRyNcitk27gUM<MpcQJHK^1(otd44wU|h+$^qz{
zChESUk~PDUx;rZwbk!QUVEL1HwCyFf1x+lVUlt<94q>%T=&fmDcXn7htUEr%JU$(~
zyb_D!kg2G2x`ExYPnL+)TIGe8>$dv=y>NoJshk~IbUXay+Jz-gA37elZQVCt`wx%T
zYr~Bg3_!cf6PPZ2e0XK7*0EKe+VZt02^QSS|GsMCGF-$l$q?P%8A>`N=2(cMtXtg4
z|E}F1Y5SIc*O4jZhL-(|Pb*^KO!rshI#K2mEY>_u|Gd8|Gk<Si`_0MMTIzWyxWJrb
zmqnA9Cv?`vdezFjLR;4+jIa^ykpw1w6&lD98Gvk#A^_f#8x)?3a%Vul?y9Cb>y@$O
zcyt{`;Nz`3DU8*A+UuXpFYUnL8X+6)?P|oYIDPC<6(U1ZuiAiv$VgO_O!uI;_3%IB
zdJ)So>hc)|v-^EZ_ey9;dS1H3upN5!bMj}_o(>SKWw~A3TQUs}k2X&;&pgZnyH<`v
z+WJ)AVBH@8`ZZXPZW!s=Kd|XzWEM6$&B|X#hPP|h_{(0j*Ey?fIu?xl84H3T&plXI
zJbbgXS9S3Jrm|J|1Ik#ynsR!dZn#rL797;n1`Gh5$sN7)^Ilb$0e{@<wt~~HmusOb
z0uKBcZ7b=XENEw(WLBR(aeRgv%kA@aP_yn{))mwnRnj(u<LPRoPoocv2nw9EXhT5e
z83y+;MMxY3g(PL2-QBip!+L_EBGi=l4l+=`lX9L_rXO2m`ReZ2b0X=T?%Um5AoU57
z1K)Qj8Xf&QQI^Lxd00GDTBlu~VD^Lgk20C@9Yci*%2(5AmNH;eP$1C#TzVdY=o1ZQ
zXU#u|>D32krWb*Mdo^+ya9e3YsboUAqIHlV5fkJUqfzSmp|wS{5WWPejIm$$cW{bK
zihhCy4HR2x&2leUxLNMP9C~JRh$KEab!`QyERM$?A|g37){&(4cZ&kDKu*fCx<V_M
zGT7FcW5@9oOP;D}ZNEivD81fjRoQ(9Z8=bp<G@TXXoogQ?Vii->BwQ$hzOog)uA>C
z{Ikx6pyf;KK%fo+ZG3vGARBvp6a0D!@_#<PyYM>|yw7)lmI1ZO4|3!75b@G~S%fat
zs7FS$==Qd?5;l095xr8%4sTOj2>fq|UcM6+zCC+c;7ETAL1w0;Xdg*h0!~hyrTaVB
z%|tjsP##u>@~q!Kxv`VZ0{UDh;6$tZ{yp;mjjg3@nYT)X6%dv3CfQ3?m5qFNy*@12
zLTUn4P}ue?uUWcmkFnoHh_!yCv}U5Ar^&}PQc=I5$`U&hyF<30!Oiu1O!fqj{FH$^
z#O@zu@N=$Gw_rxM_i<Ck#k4)^R3oC}iS8jTZgH(i2q;_nEd=K(;QSwF0IVQa@!{^Y
zG~pMXgTaYMeKY)V*{5WEeO>CD1U&)$MUr*&q%}>=$O=x=YtaMYv5h;7w^a8nYkw*Z
z+FQR8l-vYdh)WbHFd1>pl{D_3CyZrW`^`rO!3v@p%qL!*nQo;u&mY@K-s6j)ywhSK
z%?jss0<guo$SiqA*{F$u3C0N~xq*;LU*B!PB;Bt4_!o+*_dkE1Zt53Q4g?L1x@ZFo
z(x1WhB*(moD_}3Ij>lQX_R-BavMEyCH%s?;I|ye4_UHF9Ko-|Sr_Z-a?x{}A-B&8&
zCR8emT5FgtMb5_u{cH%Z5&vWnR_Xw3P>q&o<aXAH_8ru03}YV^d$BMjksaz8F;Q2v
zC*Jp+kKX)FMDK^|Ykt7s6;bIeq?_sSBPqtEN?%x>Aj<qSY+>bMIBd%f_;2kJ?ay$_
zL9Dx<!<7Pxk6VS3fRSkygjz_}<ILGSxlYd~ddG9(6~)L!WQjQg)iX>NBLpOP#B~zC
zWw@q>_g2KRLXu$SB-Of_L-Ty2K~O9f_a_$q>V9j@Ma1^<VbFN22q8>Q0KkvF!oyZ9
zC!FsZ{K%xb(if)G4hjC2+xY*P@p<BUwTj)QeWQ+_{#ZV61YgO>pslSzlzr_*j@lu0
zlj=;RWXnce2q<x(o~VZV9}V`hxJ6o%HAC@UTCS+jU1e$T<FK75)llXeIG|iXB~-R7
z+p|ZUD%Ny}kdD^m>4@y9@+^)^9DDn<P3|`8Sp#aZtW%;y(QO%-qwAK24zio(Ar~)d
z*uW9xvL!P!+kz#kv<1*IY??h}*q@zWh;Dc%_`!Mq9>+n7udDv{)#&r;nrusVEx`e4
zMl?AE1*}7vX-PZpd3!s&cK(zC@NRo%4zvI(maGnId3SQ%DC>Yxc1{1twT$ZFb)7>7
zWdyyimz{2`0JNIFJ-b;-A7Be~jV4P?V17ww{nKlw7R<mu9s{j(Hxw(_?mZRe*j>L6
zbTK&ch)Q-XvKKvr(dly!++2kW)%s-`SWMmn-`D<LRE>M()91r|HlqsJev!7G`9b_!
zlaq!n@r-%8sXnOGnt;u=3+Q)ca``mRy98taGv?{{F!?wR7LmctU^O=Bu5{fhTKE8Z
zB6r6hbjRf|4gyEHGU-@-^y>_Qb+}pv>+3uI1x(3_^xC%Gt4AHObBI*FIt{PyldAPZ
zKbTWQthO2ltMDv@PnyFlp9#uaS1s+M1j*P95&JitNLeMk^rCN0hIzBe;bTYPo|!Dd
zEk5TbdiMU9HCJXK4a^VjEy}kP-u08rKV1$gdBH%Cy!?;vY;Ip}i0Z5{H}AUiPHZ>X
z_)wk>Mfk-|WH7W+0(vB&iDnme)oXpNpH8C2eS7>9_vw|GheYZ=xjLmPoDKZMqaWx1
zTbhe)Lf5z};Hv?AS67<R11_2AZES)ZfWwbOUN<YjO<XCeKV!BIU{8EqYe24>LsW|b
zC40Tq4pbwEpP?OOO@%h8-kkDH;Ci_=qse8{#XDuY2LEx<2gL$a?qq1C83>3KC}T2`
zDOHFXs5Ij%CJn<_Rn>HqFXIi#u6l;W&}bn(&o?Ch-n%9Tk0kBFj)zcc8XTsj>$_D-
zkq~s0rXumEJOg{`h)vH&pasG9p985ue+jhgw<S^cTE@t(pN*o}vOoE)c)hW3r-WGg
z{O-_OQEK47kGc+9xVbZJZ~bsJ9M@-62)+<JURjdake_JW)Zc%me1Etfv=D4jO&?*z
zK_=zZfpd8ySpjof=q+%7%g=Z&+}>P{yS?oTf`LF+&7>W`dXl2-3k6N@;`5V){5pWg
zLEeM@t~bUJ1+^KSAO_ppYbNj3@oZXw#fiSP4u-WJdEg6Yf8}UMk0F1S*8ND;=)DN!
z?A?z$1Gucgj>tUj|MFHZpt*jSWeV~=*m8p{^09zU>tF+E&mhC>dL7a?v7dMI^UFrO
z*PtWp$&vIK_4lXEE4@;-1hjlTYAyrUUJ_MhJSJBaWx8<CxGG1AW%){($c9%Yr)YWP
zx|fG7WqY-jEO%^jDtpTmFn74Q^>b8~wME^k!I$q2bp;yr2l|TwWVOtw8wx1Me|wE2
z?^W5{>73v#ni3qmY{_Z=s-9=r%OCIGk33RzK1BBfg2BR`>ic`U%f`WAPO9r|xo!da
zx(Fgp@4zRkmgQdfD6Fat<BXpyfKt6Jx`8M2dU}gauqO|)=-X{vo}T@eu=!w8p=+tU
z(bKygVZ;wn7A)cP?tmU!?ce!svkS>si~p<XPR~3&R+?f801}AOGw;M8*(1l3lsxt+
zL{_|EJ_x4C>MKGC@PsiAEa6!cSH{tc(-G7UpG!n_TBa~-VuwWDo!GzAED?f}c^3Q-
z`qWu)M!J<&w=PkTSD+^%NrdU*4Nz6Qxynomb4)=Qp_Y)vVnFL|IqNBGGzIft{f!B_
zYx0vrQD)7`wg_CiIOvDPB%Z<C054>CdAX41d{oYBHa)SQEq&Ks9^B<C{U>hzW**~q
zfLe#DPDLm-`Mh;Lse?%DA<=-0N=_+h)Q~?j2V58clyPgovv?y4;@ZowK~5vsAa`)o
zU3*TyhecX60nMe|?xBH$S?Dm+bFxPEhw72J68%=YRD3O9r8ZMavi`lm`(lC${huvA
zrRlVpQ#K?WQfG^g(L<MG2$nK_-sZ^%7;Xgh_={C9dJ1j)fkFrc`_wcVP9Y;c$AdkJ
zzWjg`8n%Ld*Wf2M_Hfsgr*ujh)1j_AIgU!nI58~XAvQ~{H1sjhH6ks}1ipNllJ;$c
zE>8q3S<v(6<Zi=a4hhhISQ}NiV(;wx+VpL0vL=itojbhq<33iRRZ=_mQvnpBSxkrV
zPb-wqO(y^`hVY0D4}Gb>5j<Y~wrO)u1X($o3wD4or3A_Vt;uPP^e-M^;Z{NKMVaWR
zs3ox*Po86ON7t013{cv%>G$vT3?kgiH6y?OFlV@rZl47>C`J074b<&v>uUP(?8cp|
z(LIFSvO9Nq>WF<TJU_og*3cS-`Mf(hIJWlX{~o1e?Zy%H1kyk^F%@C*DK>1|Fy^+V
zU(nXS@H{ymOWq>3Hx@Ooy~(dJqM!@HM$pIEVqw)8Z^<o73jn8j#%U$?=Utv=Mfl38
z=7F|=_N*6C;%=3%MdQfjesvuS`|gNbrDYN&P%K6@f_Hf@Zi+GMQ9)jnZM8MG0+W}4
zJ%)o@Rd}r*OHZ?0(axTe1$Y(J8rb;+^$&z79#0o}4S&__^Ph6pusoCO+CPBL_4@|=
zHA=hQjgwdReSjVVZNtakV;xBLcOvH=@OY7xqO`&fnG>q)DF)qFH)im7Y@5Ek<Tm6=
z#fDfpQ_6v%TykBUecugud(hh_I_v!b>POL!O1E_hASfLxZ3M8LFq4%-^pdvy=p$QX
z9DGIX+;lDyMVFO5OWGp;6MV6^9{*t>Ri+^hFmR$r15;A68KxkSh0COWS?SO}d4FSU
zvjq?|wawa3$C#Rze;O9#00!vfyE4!MeGnkF16b(1_XTPusrSE$=JFIb=5{Bl6jmzq
zA)ayr{kE6+b(TA#nd?h*2^$?AsJdE4hC16V%TM0KjCBy<P^|&q@hBf|&<54<L)+WK
z)1BiQx6r(edcSel`R|EapZ@wC(PMq3XnhS+gL*L@`cr_cK2vxMGv7}d4kw~-W3LYf
zNWj!u-hbsn3m0FrFnz>>?;4nHA0{(Bh?le;f83iW*UpigZbyH4zE3W|b%LeAp;x+&
z?jd@)LCXhl2=r-2S~7JF+2AEO4&w5*(Z~myknpfNEOFd5qA7;3t3Z9c&`twspr5e<
zi=yu{zeTUagX+S@8%;O4-)WB4a#JRtT5M1MsjOFMV^z<KrIOjt8+~qmp0pXZm&<-~
zKdr_ih`d#p8PVi0bWmDr|IGz2(C18(A1s&Sp#J8|9^H>w7umFwtT~*c0Za?{JLGn^
zRWJPN*7xKdRCk{q?03vWJQRApgd*uI+^WGB$df1xFLs@+xj)I6=iV^?th6B!I?6=k
zP37DHYu%AIS8RbQ=M7|pg3#z?k%3p+q&(H8j@JK5zxlg}ReNBejrNqKcm?o+L-xP;
z$^!BA&Ge7B_{gB4Fu7L+EF+L^jsr0#W9bhw&`0;4_|Wgp?#wl%&ej2w(F*O!!poIB
zU^~|Kzd=oH)rtAf99m;w@zz_#y=bQgR)laxrOIu?V94>jTHIXkhN$&&YI6@EC8>XQ
zy;kFg*twHC!sND@y@usa@`#4m0jNfBr558qdYD;p*M6fN0d-IPG$PT}YPODsD%%=i
z-Q%6OnQ!nd1Ku5Oxx(Ek($y5uYn~PVQomr$c);^so8nnrN-#IhR<1?S03Q%h%8ttV
zGHn$B&Xnyo9dmE?6idaR)2@6O0$oHn0r$1kx(vY^#2CSP&04tb<{p7-9D&S(%*>*@
z&_*!42q&Z3f77JyrUmTA7ZwCN_UNcdRGbr9aZBEA*4V@uerTJW^E|gYcSvtUno6>N
zI=L@DcU{N~vC2Wx%eR5u3IBG_Z0TM!@xJgcY<_gQheWC$OqK+}d?dQb5@VJ|;T~Ma
z&6l;4MDJ{T`S;Aa_DvnL)Z0<jN-6hWCJ(woAb>Xp(1DffJ^0R-%^F^t1G61br~VtI
zgH2OhS>_dmC;tm&$bUjf?dPeMCu;{dX;=3*1pc`mhvB3LT=gp6fP=;DQ<JlZadMPK
z!ACM@->R~)fy|#+9;NyhU3GtW++wwp;%SpFI*YeDWZFe^Cs}=?_s#O$ZQT=`#sCl3
zWHqdbB@-39vCTY0D#8jYTlCmFw<;hS|8wvncK&>CA;ca|f4GycsO_y=@S*sezJ--#
zZ|iNFNRF9<9#FW{h4I`}TDFB)eJy_M$ClhpnHjxn)Mwmpbs3OV#I^D&P<IQ@G$_>E
z>c3>$B4K3#UhF-mkn3vA-`L~o&}=Av^~-AELZ1`aXlibwptPN}{Yak<7&xUVRnCj=
zt>10OnZo=b()Xkt8&YofH3U{9WTV@q&_xGJ{YReHs;>R_z>GpupR@@KO&|p|36Bhj
z`LJ!YCP!ViEO_57Lk~+3GOO3RHSD#dEb&RuE<3m-DjYLfffCS%^CWj1_#No!aC&E5
z*vrW>S|VFg8sCfRo>oRGl8nMF^@*->N~miL^|8J6JeXTjHhnn+lVuEn2i`8X#sAAt
z7yvli11NR>cE=VHBmv8p#JGS;mkLCO4cPmXXHKP@J~{O1lm9sBYlEnfEtdg#XnN7X
zIJ-Ne0$_T~Q-eacbf>49oBO%lx&7=FE2DDZCBBUHd~Vs-7Nl=eO5<Z3GG`kAx9#~g
zkK14zBY&&pY`pMaq1oNibuR<q)7$Eart)K^GhmN{UWbmiMRIwXVY?32xI)^Yt(O)X
zzm7f-Bh4pIc*|X}>-r0(%x)AAGDS4DeC*Dm#9{=hmgQk9d%@Nnn&;egELsCd&#4%p
zzwS0KFkR?H!H75((d;R!3gzb0eWEw5cI@=kv5V)u-cxe|r*0!Z1PLeVw}W(cn@=wm
z|DJ4A9>b{?jro^Vf)m{qV~Fg9d%hIawsZvCZ~B(8mB8y4DX2e)8SW&^$jSa}xaM)s
z0j6{GN~Pz(_&{Q7L{63n+Pu&jGUh_tp>e*!p5QWb1$e{uBWxVb4xSV+et2u<4d`e<
zO3E&Ke{RU(<G|1Jg=&(H8fq$0JW$mg)(pjNz|cUsS2)8!^fJR&spL?;)v>VB;}t}F
z;{BKv=6{*w165#MR<Q~Ogc1b0NFFHcy&I^G<&S?Dfs>Z2QDk`6A0{T8!Txz%<iD{O
zs<OwrRqBI^6-e|aueJHbP$$#c{V~02?`6jw|M+e))>*Pv!WN|fzujElbraZ54nnb5
z_V6C+4Cg2o;YjkW^klD8XjbQz^`ojhJ{7_9c84`H*Wa}aKn2)#Lzvf+wf})XpI^P^
zf7zz9?H74}*`<OiFB16qM>+%A+k~k;FI5HW3L(q%2=M><4vP)bDX~XbZXz>3`zvLG
zD|#)DI%u4cGS&YW<^ihn01cr%!Vyi)!o)N2^0|S%yS2;Rcs3t4KI&gqd+y6%A2v9D
z$&jt<;)O0JvIWvw`Q-e`J3oUq;j2ushp%5*vOaOB+$c(2ncik@?|xgtMEi|%>vben
z0<T!4Ad^;lA=t<Guq9x%TyojlQ^)~HRs*zOAg&!%d)rO6I9XPz0+_<Chh;&ZUM8~x
zRY!yV`z776JGg~s*2L5?f{JT5zTGgtzdr6D2h5lC>YE2~O-GBj2$V0v-PRnqnu=n@
zKA2{0p2QEVwC7JWTywz}-bm#FMxV3Ip(?laiHxG+cQ>d1)p#uw0IMUKWeE3!yd;4h
zHv({?ie~a~bFT;ETcInJwjQ2BJ*)C}%>YwzzIBGt+r3nfc)G!%`ziDjV0U-YrDoyN
zYOQ~B+#+Ify?1;q>~ZG0+??2VQYKA0y6c?8!iaur)mr8@q|@wM13X%pHg`O4HYdaF
zy>N~so|xomt{R&dSX^584E^Y&I(2+~U)t{+&rm}}JPN<Mcq*&3y%|Rl8SP<pTL6!}
zA}bJ*yr*hY25YRVbPMf&vrRY(kVW?0DQdK%^NjUkz3^GhA;NlZ;U(2Dp$?fFZ;@qW
zw}IN_fh#um<Ou3{d-mGDOHRIkcytgXl~1dgE?$TSL0G9?|E@#X*fDOi85uSquCjmp
zY9k#zhBsHPnA+UgZckvub~om!qST-!5~cpJ8v|$I?4`iIC#2dHL^e~xl<W9r>xEYg
z2{24Qbu%Si{uF00O_9N#a>w6)^9&AgymE}0%(ZKqL<)|Pj?~<GTob=TWppyGmaAB{
z$9lU{D|xJpelig~{(Tb}{b~+o2q=Lps5RRq#&60%&z7c96X}3#chM!GJ+vAHVc2FC
zsmg;l???fa(0*X|p$0|o=&ZhVQPg6|z$#DkT#6T6ia3atKt(Ev=91WpHaG8+`qRj`
zz7OA14}WwTQK&fiv(QWwKPP<r?D-(~)N(xd1F>&JL|Lr#3wLpM82aWzM)NYKy3&NY
z+gy3c;eAZUX<Cx2)^fAnviC&8G$I<re^mj#i+VG86M{W@pMzE-VT@B_IUzd8ijjON
zrKP2{+V}m|g7WGhvPVBQ{qU|m*YEdYE%Eggs$1%rY!mI_*!e~y9EWR2&tWmegQ0)(
z<h}Z^QzDU?71--uE5t*2f1eGkOP=$q^fhiEtYB#7)~q7H$TE2Yy-XI__8mXsXVcbv
zl0d3Amg(To=n@>W?0}*Zt0k#9a!)D%E<H(25YKlXd{hwO%xY_b^K_IriLAcCv>}|R
z%0=WAPyadbs%}8ihIf5*UwbP4EB37IqN(?k45Q1EPaQI-`5@zFOODmzd<H~auW~MW
z%sQCd&Z$y-N@lyRc&B3MzIEFdzm?a18#++2m#PkS*NN@a$CGgcg*FU(W+ck3siydW
zrZ*UIxyf8OY;ds_BG7o~-gdsCg&n>1Q%4M&nrV8>3C1&;5~X7Ke*uT;i(jH9m-J(^
zvz2=vU%>32!yi9b<#T#GyA79%aQtwbnCT0z|1~y6Nb^@QqgWc;PSmfL&@Y=;MbnDL
zK+F}*$9(Unw9~odO}(Z^C`^3uF`H}+s-EP{I~iumHu~ciVn;}{)1?+fPPd%huXigd
z4eE$9(v=2Vc&q#(s3tYTH<kJFw3@$+gbxE@Em5(uw;b#k+sAc98WqKaVUfw~SP^!!
zluhbv$9;}7;r!QNs-$zDnuU{<xc>Ld%nTPmL=S`#2Jbp8ZQtEneOt-cZNnFEK`5}l
zC3zjZ?(F{DK=JXMkhI+qHnr1tP!H_!=FaDcYoi~5okzVQUa#U=SD!FuLcQu5!iqML
zsy~D8@}A;>estan@_(c>OlDi>`GkYoCu@3a$LQv)sQ}-jOczc%ujmHx3D4SM-mK@M
zwbN|fbbRLuY*-o&uzMWn)#gY#8YKL-6~U}i$O^WPVC%P!Kov|mjCbyV@!5kwg<rbx
zRKJ4k)>zZiBUMxEpxjHn54MTS`po_wvc&Vlbtex`ZmFM7OCUPZf&~e7fCyVlf<X~y
zh1`l1ib9Fq&g8K%#RE*IDcod`)jD%l?9nv4AU1Q7YK%2+MedUipv?4hZUWF_Z`y?#
zZhn7EABK?J<4%D<<LW%l@wMx4xO>Eo<CCCp7vng1tBUkki8S~={D>w+WW;K-#aC->
zoHLHA(IK~UxQgg914-nQ<!E3azo;^=OE3Ir?J$0LwZ>n?lhwvMlqce`+U4atPp!Zv
zK}rL#1$7EXD^}v%)9yv%1)o|`b3HJ^$B|gUt98zXcrq+Y#c*%oK>`y&qa3J{^DA7Z
z#^SD%^=b0u_oHAqQmbblRwKr~;%}a^_e}?hB00~2Fbk4H1;f{?kC>SkDR66WHv|Yt
zhOd&8Ch$z!h$P$1p=={7i(gI=Mlt--^z6FNQ%2yUD7Q)p)HC_2&%O2(*rXSxMRtT_
zgD%1b$37QVLDOq6oLMjyq-p(24RP8~>#+v&ieyiM4#hV%8qTRbdj^@6#;=&;JZFum
zKNMRi*C>@2bL86e-{SJRQz*VqbQa+#OVT=Y(qq`Lbur@CuRJAA>Vorxya1&s16jg8
zuqkRffqA~U98ciN2Wvk>i8=<Q)|+m4@&hkOlQ;$Q?9#mBJgLTz`$sng5kq6>$I6^M
z$$sNus!&Y?Zqvo11p`8f94lT&rAEqq%3PJs!pGkrbOO@LtIdf|_%#u2Dj)ywm!Lkg
zq47k^#4nLGUEHQSQrYmNn5I<*5K>zw7|rR{V;Baj_$rdZgBGqywjW0npIu(ykffB>
z{K2#c>K{5ERl8ko>h=J;&XcAzU6@fm*{%FUw|6SMwx~?44!$5DNg=^bO9AdrJ&;cQ
z+{sl^d<^fSu}0`5+C9`ieY?2N^l40TT#Sh0dcd~JWL@=7mw4L^`<TDIS4l?tOK(NS
zmZJ>&x3^u_*1Y)PohfrqUQfb^pi8i);^E4QTBo>*CmTTsNnn!{^wCJ&#7;Z5+8;12
z59a-$@P!jOi%90L-?7^#@547{^y(YG>BW4`|E%PuaOnu~N_-4MLp#mrR_a;lf2ZDP
zI0n2gI1%~W*RZ7ne0Hm9=vXdEkdz$?5x?S9NC7Vz`lKTX0*C6h*d(>h1~-tylrFW(
zws{hm7Rf5lM%mxX3`7ld{Y(%dn1O;da40fk&ncdkGjO)h3XAyNQ6FKhEvMp&Von%e
zCwLf3G~@X`b@wFtLNMYR%f9tYi|Hkv@KXM9dbvOt8U-GDlb?zdmW*8sS>3aQ^Y8F3
zm9fw<CQc$^Np`7&*W=6x?y-qvums`U6i8Sy_s>=XIQQes%j1B|3|>y0b)9lF7dO7(
zPKDRsC15?ptufS-bbk1^(h9$rIbD{mM?AT0CRd|Oi!=QS&}#gPJIj>k`+;X1n?nh|
zhB}`Eccw-31~$=N{~maeXTdJ1Yo^EN;oUW1F-cdKPrD-F0M5f2iSW7^gis4r)*ICn
zxu>g*g(fXk6tK)>H}Ix$Y6?|l2eNc004G-z{Ml})X68)3eBU-nM!HA#m{zL9EX*lA
z;lY9tKlTC*G)(Haeq5S6Au0MQMXkk~BqqVh*=1<4J$G~%{4eu~D1_OGQEE|sjHZgD
znzX*f{Sz_zFlu&mWj%!^!oTeY73WOIKtYX9NPfni=9>w1v8PNSM}DPg{aEXYk$!rG
zt^szZ_}Qs2ni5!FMdS!(*+OVXNtq>im&cKU+511pK!6Z=DD+mSYT+^;vYEDRzN)Rw
zLq~RVerq}<;*=4r?@gk{DFW`0u;ciG=B50+mll_O7cB-+BJp|f+Mq27<KF9=-$KSp
zZ;o<xFfC%_BlP(5+{KeyWe49qiLP}2z?qYp%jh!f{8=G(2der_*~$b;Ss@f$7OxJj
z7`J$y5`GubA-m-!Lw7?<r`tw(4^{&gilcUq{14vL2x~ItxflOmZTd4emz?>|c9z7@
zsS!Jito{9{f92|ED$m3*T%HhA9qiel*13@XRJsZ6@L<ezHfS&R6z<lyC48~V25w4N
zqkyE+u9mh1H!f53lC@|x;{?-0UjBX_*trkoMxB|ig^!eZkIlqJf8>(#Vvpr9ixMVR
zYhNR)#!Qc=a&-CFhaW>?-4QFFajWxpGtqdsv@8(FaFvp^ud72PZAQr{WEo<a2zXk}
zYsC>78?WtYHonE+YyH*<zs4_?DOlQAsOQuASX;-{OR5KMaqY)jCNb-I8Fuo@cG2cJ
zc4#p6C_5clN_YjkS%1{P>&eWy*FPL#sBZFh44Y6=qLisr+?eli(i{vBiamOQcCzdU
zE1J`D-qcj~K#^edh&imD_1hw4ZHyZ>f?a0XkgIm+i~dU!R;FBx6-nM8+#0sK@1PLe
zKz-E9Va6XCV1{9M#(9J$u@&UYkjUpM&nF6^yPv@rmKVv~4FN;^V{Y_4##50{k-y#^
zs!!iOMSbXe_aX>qbcvcXce1@3r9(4iY!|#b!SzyC2W%lGOw<)!f8{H)awL>wk3G7-
z$eW2<J%_k@9R0%@!zA9vF!Qx1(kS{>WogLyeu&0@b7BM&fHmGZ01C!C!?;HLOpdvz
zrTCge$8J@HQIaFJq5yv{>Dc!vBO5n3l~Ln8iN;mlk(b#^H!qDh*J7FX<?kK}7-LgU
zpZIXpm(gGc!pt8Nd=kp~mj7+Su<_1>t?-kVKZ{T6Iz=w{F^BwvHY{Txli0n@JnCgx
z{Qk>*R`cuV_}26TyfM<}Da?wlT@U>;q>a1l;yo{==)m?0MOEa)^}&HxbL|9lo$C5L
zQYRnQaGrLZdSL9cwLVEi;YU*D$4r2PNEhiafCI`+8km~GCHeQw#@F!{yu>Z+G~w1O
zhS9D(NbUE|*4@H+2~BFtt{^P~mXMCx=q_1hbKj|%keGVD^oWHX&X|5nv%|q5qW<O!
zO{Tw(CO(osL2*xu$#{Jj>shD9mh{SEL0dcM7JB|?VOp}{4|%2#3vW%Qg5#~S8J=nM
zhqx5NfO(ni?x)1Oqk@Dot2<wH25#jpWyv4cz(sqT0&F><%5&9XPgyumDIT!ttO&Ge
zq8&wJ7BbD>)K+&Ya1M{+Jw?2p@KYe1=CFG<$~(DAZo={u`K}~R;w+I9?dmw`lS(2I
z9YP^PyB!{{>@B7h2oqBWYcl0_M^W%BSd6D%l>^A5O*jg?pUi>nQoM{NyGH%<kmUV5
z^J=BxA3A(Q#O0sxWLcu>m>?Lltz&?slfc)dX8OuwTkQuIvYTN^6Oy=RqhI-fI#mn#
z+2W``Y;l%TUPO)DV_WBDwtTfVIiyvf1plEI+=IZw(A{5rjES0=HyEx{xLo4g(wOZ6
zf;dM9e-5h*H2hwTcTKBcXay0Fv}6f3KOoZQpj+9aNUAb@E1D*^MvCh)_Wa63Fd7Md
zRUeGW*<sDF0dkx#vvqKY4HDS-Ny)<567?C(AeW0E01NN7n&Tu+K5!&y3|2f1AbpV{
zU`d55bhT?m?u_5xye3wfTaHJaq{@kF+J=SbQANy~TMZm4Grc4;SF(Uoxxk;UM{6*w
zFcN$2NDuyAjIa`ye2jHY`D|J)!i?803VV#5^K`kX<A`M*Af{Uj80p)@_l{L7WT3UE
z><h;haZHQFO=xDf-h2$+k_Qm0TZ^?9mt=1QI(#v^qI7^*sfBFQ&0pqkpAZ(?BT0!N
z!QoQRdm~d_x-h#9lqvs*J|c>_yf5}W{ym-<ax`O%v1zkJ)4Iu%bv3F8-(%i49)Dq%
zIkG{f$o(a+V-x#*HYBf(Y{quSAezFtaz*gi)3kjR*MvmUWui?~*@4P7OdISs!@SB5
zd+bB=4Blj}JUVu<5kJXM5zpQJUZIK3xFXEErqt2(P@f+|Wgl$lm`ca)_MJ<Xmr4U1
z^D2WAi$<I;sqXvi^3wj3jBO_9?;jmWJ?k|3_%0ljU=|Fh*b}@QMA}c{`{~)Q=!73S
zsh|A|i3XT`4P63ZT2K2H|8>tBte08kw45I9CU$}c2qk~=#I4d;=3CaO7*j5&9~9)6
zh3DNpA_4#oF<<CO6ctItqt%<(ZcdRb_K;8V>$09ud<I;ODeYJIE}dmzpeW~4#(5*8
z23-$Js7d}d5fB$5;l&&*&N*^%soch^l%qvXv~;9f92N_wmL<=#(WRbOcyv2nmHe-)
zd!_RFXuPTQ(0Eonma@G4jiS#c3mI>{FE^fcx@X5>wS8|hrwDK8&VPfWI$-{P#mp(E
zv~;f=0EG9G0Q6udg41SYvEd9COf)E~Cjs)IbOE(;3)4rK?U}+cP7#e$tUkS++fXbg
z{S{q>vm_pc-;!_jdgq*-zC7Vr?b9c`i)5;(D7StL#w5fUEeq9xMqWy@BfnxsQi(__
ze7ko{s2y7g;m}Ip7ZhbPT^%VZb0X8IGF|KQjI8}LQBa=wvaoFU`8<FI4?fC-?Q?sQ
zkNp14Ct^FIPvA00Y*n#h$D_dF<)n)jpCPwauh5U@V$t(EJV(g>E1f@cM*qX~^PpB1
zps7m52uw}$fKDABb?dcrehNHx5y$A)Bup`O7OapmhyA4|0JcOo5z6*{3nTI)_!Ruz
zJSRYe1Y3F|_>_ErEd<fWDP!Xw{0tqc6t0g_9yKdgDqO#eI%50f@u7^IqRaz&4&%<H
zh$bd#1XWc%Df_DVJmp!1X-ptRFyCB)ah%CBy4Oegg_*=5{|ghw%`kTqyyFTf7Q}Ug
zk4A~cvmJ-*l6rh}-~JNTpI&7Bo{9wj!x6FCE02{xLW^Jk%Rc<;q?DOtBs|0lEpQsS
zW0e0GPNqSP#mc;^Mr!=nwK^ZXkdVY@B+21}n221~vbz@k*!^7dIQDrK$0d#`H-|?>
z@~MX&3Ogqe)iS+g`AV5}*#Xv{j`lN+I@9un#p3-awTh3Dz`zJ&FYYaYg{vz#Ssl9+
zykhz#^qDN}&e}3=t@*;12lHN&RnuC|w+2iJ3^iplZijImD8v&<LE2Qc(a*;l%uAAD
zz#tB0xcF*-j`>{%y9oUK;V7Gpma~XwG$iBXAoazU3Hc0vms`&Uj@XELzI`^AbDI6J
zne;r-h->_5l885fzFyxQmDU)IzdQkb{eJd7ud@nAp`u?o*PqD-ZSTH~Gq+<5we)eb
z%?t5jE3-xe!y?GkF6P@$(DygZ`1E+?<fLg5_%{-5NQr;l5DdctyHh9SexnVc?3|gC
z(*!XE63f%kB|ZHRa8sW38)v4!)Rf}CNEC=UWnLa9S-z=|w)cKdl5!`BX%QlkL;ehR
zYDrIOmU&&Y@{&*hr_sPO%>HpwW{pG2-=~sh@>G?E>YeQ_WLU(Mv>#rf)r^QQF`z_v
zE0ovTyj<l3#)(B~&BN97;on%nUMTGSR17DtE>*kN8%J2(1EI{N+)^c-uW&p$J2@Sf
z_Ps|a8L~bXRe?`7e{?VNROTLV!bU@`r?^a)e3XoXLTy$89wY-h6$*|zjK2;r{5#oC
z1HFcccO^#&9mL2^Q#nCC?Z2_nP8rg`@~LE=zsxr%uIzlMFLh7t+XeWN{f;E(nQ^Wl
zUb9R2k7S-;he!~j!Exg)rw~w6-pP0ob#zH;EBe6HZ1S#;;^>@)@#0_D7B}yMjNO!o
z9AX_YfY|=$g9ceA?GTP80E(QK3WjUhPl4`VJD<i+jKyCsT7mzVXIk?f+9}G7#ekjO
zpg<SrR`=Y38ic{Zc@v5~3j8S7OX!KG7LumoB7&OhNXHn0(&b0}cz1UDf+!trJZ4y^
zrXkFG3pv_ecPP*76ebT&f_n80nMxxAT0Y=?W;#N*zS(WVyj2}O&Tr6H;nC`y85IL1
z{}v^ll;uxuTJ;{M0i7qQQHbh&03%GYjB{;ajSCq3G!31H#svH}0_2d~58#x2S6<`l
zN_)!4yROmV*T$pd`)?M<<iSc$w{qsMx;<#5{=p|gG?!<@v=A9{3dy{)2lJEJ0<KUF
z-j>eM9duJh6MqUUammxGwNmaPHfbz)2soR@nCC~2964m-H{!#;v-3Er)tP1VdK~f6
z_06R14-=e_5T-viXguO;?*8OC5%5etJlVVN#<1O?D@^j&jAf`S?9mxm6dI5Jsb<O<
z?o1<|&r1^M^ZL%wZ}6qGC$NnwhZxzj(>Gmp+^BrwrnapTneja`91m&E|4ZQ5eOB7P
zh&P-uwZKtE>ce@BV0yUiVmwcptQPaO194{Q1ma|nok_}pc^}8=*(klkAl~ZUDF=#-
zyULKWa-}=*viJ<O!fkiOxfT9{A4PifDMlQklP80$OL?<l9G9@0X*~S1w(~#0#tSrR
z1sO2>xW@mB^2ZT9xso-s1Vhr*xSIN_02tfmK=_5|la{qAYNev8=c5`_wExa4d7#&=
z=WS@MtXL3L1%?SrNNGO(MaTsDcMGzVhDF8M*VaB17KTSjrG}ym`ZTzkY)gLvcPm~8
zwE6uKjqf5%A@%iTC5}i!myQ?UwB2azHUAE;y~(bx`zT^14IJD6Av~Gs?RHjnh()_B
zGH7V(UYy)C6kut9VbotcnLrprH*t#fW|y0ra-K%v-!)+hEb_akrmQ<r|5Wq1cVWeq
z|7*R0eW<HvrX6tF`yG-GCaZZ(v@yGSl!gv{YmSq0tz*WAyC%G}rVh`y&9UeRs@8G1
za7H)nClR4zl)qXU5A-5pGW|x!Nsh=EyN`Ben6?_d*H~V$O;FwRHx-3FQ_Z|mt!Sa;
zZdN7*um@pnaXnS$w@}%oSN5R+*^I_Fp8J8s5x-c2I_77;Qb$p4>t@eIk1IxfF7n;l
zED81hDj@ar9IOruIKqJ%hL1r}lA6f}cmCR!_GOgpuvFxS0ho0Ek;{iy%_#t~oUk`F
zTYAC^VhfQk+8LAl9kKWI!+50_sUmPdmFLk3jw#qvjl=CL_HK`qy2}YgbOA^c-2G-Q
zKw+Y5sY2!17_Vm=7Q<UX>6`|dbihDa+^L_z!igoB0#vQ$8p5)+d5`e`Lcxtkcw+d3
z_k>-DrwDrsj9MB?g8zTv*RW0xd&!H!sNWAgrMN9!ve}P%-s2AK(jRO>Yk!OK9`yE?
zZ)MZ#^{0Q0TTS-td5Y1{R#mGv$wl_Bbv7gN?0~<xCOJ!c_`60N1ZrQc1j23#9^B-c
z*5_9{9`hmwNgb;b_7p>nN7~FabWMQ*7LFU&HlvyHG=3wI(hri_vnjyYG=`R!fRork
zQ2iQzRDBqakW!91{k;l;N6iBCLt}{*<3#zw=gX`AjQ7k<Fc1_x`7x6_;=g>X#hX9~
z@Yq~)*$^f|fnh`DbN1yLn_AoE8edQF;g{9bA4i-i#ZSfYVL?aGl;Bp~{sQ2;Mm>S%
z9UGi#y`hsj*?r3I9BowF+^PQrb-zUUC+yIJQ$^D54Cmzz*NYc(^`{}oFUxfRyD{?<
z&4u#|Gbc6Etq6-h&kH26dsocsuSUP0f3e-pzEK@##_IBSMf?Ay=_n-n<9S&pRtDPr
zCW*Mos#r7D(N6*SeZ*S}xL(s7aw|3O`AjB2SNuGE@+-07l@r}#G@h$`o-lpX@X;w`
zJIPefdJj1wa=-hgOQ<1WU<)}%N@~7l^+_3p^E2(U-1-SsOvFfB4JHQS{9_>&&j1yH
z-l4QP@I95lt!U_`3ti;#4g<nhp8PD8lnh>XO0?;pd2fg%BVg~D=qM~uW*E3NdjK?T
zQHbi)?<VBLdh+j(hVoFnid6Ks9kU=!VN~qZ+^zMVr)qcli2aZNwP3o}RSVpI+Cs>f
z`Q38DzTIml^fyp3y21p6!z?{#8%2_b7>%9L=Bn|3lCf}h5EB0o!yv*&sgj!(g@U81
zThCWRdAp%<o>&So=1_YO^AhHtGG<_+qezkx{Q7NRs(s%v$(wVW?cK=d!pTnatFJzD
z#s9%-HGdVpZ;Kyg98dlK^Js|Cs6o_m6RqE7FS75}8S;2#7|%oxLebNk``9j#b=A_j
zqbQU)7|#$v{40%(V_7%ud0=Yy-Wbg9maGjpiDe|X@`t!4;iee-mwj8bnaQFan(p-Q
zSu##_vw5(U0|k=t9Dm(iwr@+j7|o^Z{Y%92emw-|mHhO!{m7EiT<%G;@plk@TQ{3B
zR?3Pf&_Qe-xC^l$39;3f^<!~Gv9mx$;@JS+ka{+1Z~tETn&_sZT=L3t(thyskIe~i
zPB<naVfx+nOGE*|qI}BS2o5U99}>Y2zIJuZpE@+AAgFDfBZy@E;+@;i%>pR@u=I=e
zU*8HwahcA$*p}xL(nurUBbw2{Unph9k4Kbxp3;oric(J%bD3kUxJ8DH7|%Wsk6S&J
zTT5LR#$)@0{c2x}SmXZ9+ir3bUmOen9uM}6m))G8^T=0G4Ydt;8|x(9l7#IIqH~r;
zFW9BGL`siMq#F9Of6Gm1K`6d%U~)cT%t4m^#Xx-`Zif}MnS)|1jW4bA^{~mhzl5Uh
z3tuL8EeSpQ%GQEI;WVb#A2)dZ_NHisDSUq-i^%Znu&u_%Rd=v)rPvhHrgjR~ew)WT
zXMa9)_b#Vym!}<f-}8^3-^2qqvlaw>@?xXUCpdl}^k#O}_4yu-fqdK$iBta4-YnC}
zI09Q6d1iXClsL4&L@xtoH<4tI?ISi)*$y(7p<bv)pEs%ypz(1?@$M6k7}-95JJ51{
zy?<^iwsVt5m`p{IY-;}B1Z2=X0B*&!16|M1b1J0<l}}mk<*tu7$7aP+Y>c;@gXL3N
zozF6l;B>gEX15`ZmeW^$<oN&YN<!z7DCi+hu4z4=E!Gpkl4luQImkT5!U<`h>hCf<
zJhL?S6EMBKyvCJTT*x;nbbPl@L0RS^`Q_%!0@!>X!Ls`JOr3l~F5-)LYqwp^{vRD%
z-f*b6X+=Yi=kzuG%0$&mGg;gUf?cn{Hgrs{ue$|BG-_XO#ZunkG(9b3qX5(mpo@Ux
z>y3riXYZ<&S}6*b;CxY$OB}yF`Rw8{?&54uXeNgJ#Nxza9>sr|Qr7vpHaJcHD%t5g
z)duWkMbmZSH6r}Vqx0x|q{)GWr9<q6m(r~9i@#qsQgf=?%wI*{Mz45f-le)x<%=cn
z4U)bma2DMlzks*Aq&OH%?sB98XRW&293;Qld*?)`K!P1Y`$GBF_2m6j_)6r}$~cW;
zRYqPtf2?BD1if>|b||ytybp|d^_5rQLEBas(X$sR%#F)w2Ln}S73WD)9M`iZb|{wr
zN7PsNMcHj_BhAnfl7n>1&>$t9(n`mWBF!i@Al)6p&@K9iq?FXq0t1LB-7!da*LVAz
zbKdU{@P~8nz4p5H6>Bp9`o+^`wB`c5M=5qAGrtuRJ|mKSfaNGEmR6ga&bxBY0wS%d
z==vAd6oFBzeb9n*i_XvrP3yil;z~69Rr~Vm_1!EuXU?-T)l6GcT0U>Azy0DQHE<^0
z6{lm;EFWu|cJLPa`kw_Pf<5jRn+_hVjg&vT-ADe~{rKu-2yI`}+GO?kc~*bro)7Y3
zz&9=ao~c^}-@TjID=>0(mgFxSXAsdY`lwi<K{xv)Z2r`qzjTbVyS*XR8pw14B?@d`
zt`%gLG|8Jbge?ho?vU+s9q@j6c}-(HwDZ*FNugQ(x$OR}E5xYh`N(Sqq4TS?@M85;
zBwxUbnYL;V4EU)lQTu}rkUYko8m~P1vgcM{^p&RvR)i8~QebI^9we67oh;MoK-c7@
zeYjzWr0iE6q}wM-T<cM()CwJIvCw^aNLRu64D_hL`pFDkH<{3`B1c%((V>d*5q@UF
zRos_@sc?WXPUC=#(?>$5Tx5h3n7OdEB1lC(AyMrYv<OJ<LhUb271bu)mojfRY5+(3
z*C7VsM=I543!IdL498NRh({VH&KN)u#h;!5k&}^Kfg4OM%ZXX&=#NUUm+P6##pn?5
z=HP!n24#K8MkXRT*&L;V#{ChvA|!tDLDcU0aL@b!4kyTc5;K33e`UMjSQAga3)|sH
zeKV+hSm5HAhllyJEUxCuuYIG4?cBBKauxF@HXVLgDU39vahFEcu*lw*o|2M)&>=a%
zTwC6~)~8;JjE|vUB}ltXom*}B_#5sH;&{9pxdX?4KS>gYqHtiB76ZHqL!|_$0Q7PA
zXi04a^pF9{8BnN><4{?(*?i5$Go&wY-}?~@()_Cn1ExW3JzA2d_t4)(SsfteC`FnI
zJ^2)Myb_S2!!6<aPV()K(2VM~;%LL2kb&z9DMW6Q`mr&15Dz+WTVEmfzuym3hlKyj
z<wElZ_w>qcE954~t^D`iZ3uX5W#PN*x$UCZYqP>%+x162ayg@v*aVb_>Al7i%7a5$
zCUD0|{<dX|k6K$kd&g}#g-=0X9(}3hq}t*Y0M6h;jO^53Iy3YOj>{4a&9%vZ%amnO
zxId)Zm){bzCDdjfv#yXePeuD~A!%?ss@KSX+sMU{q!H<HEM7iO*3uA;<SZqr&2ig8
z>(>F0#o`~NG-ktV*$_!~8BeCmYkFfc6+7+A#RjneY-T@fo$T;oyZp<BfB?+yIRWfi
zlFDH6)C!tfE=ppIL?gNgNdHoMmrmDOu>IX66H@7Y6J_7Cs^5r5cb2}N5A<ks?0NOf
z6gW1-{Ea4a-)R74^4|}HY1^qE4+C9<2uE|a7ORz}U!N0MzuV_&0ppCk-eEmA<vpL%
z)r&m1G!#o(@YAgMi;^A}VIJcy>(qt0xgr0-3#8m!zlNVvA39$SwP#a$W(E4Vx<6nt
z@nG!p1HO?quZ=UePSt@n(8!haaL~q`2Q7~}D+_5Nk-4ww<HGd0Z^++IlBPA<3rP?O
zzkVnuhs~;OD*5Vdw=I1VGw*T@!GFI^wyrfeO?#CMVP3T0^=oJ&9F^6|s-R%>NJvOE
zkj}fyFSA*DPf+!)@0*(e7YpvWiOAB7g>O@}Q4_X9!gt95G(*MLQF!=uQA2&y-*e(W
zAK+Kw9LAYfY*Qci5>*xCAv+9*Xzl7SzrB4)((ks|9V6$A+T)w8v^DP_*;7Jev%vxS
zSVvVEwNj)@)0518LDc6Reqs52hfn>A8j%l1Nsq5DB%(Xch*l^9d7X!x=%{ONr#>`B
z3zur41S?m(x&W#Uma1qc9*d0kTs@$^gfZ{L#*N}LA`~4K5rSB*n<CVM{ct_MVaP>n
z>t$wF_*%tl7efKyV)zO382^<*h+p=B&D0+Q?m5=avn6`Amglek{6R|n<S<pZuonx^
zuGA422C6@sY0jwQg>SS+%7F$#dP8i(-2SVbMhFv2t0B&RPRYN&ewnHAz!;eckMsS(
z8?S`wul3_It`?c<^wG5=oD%mEK!;qkAt?AD9Xz)JI*NEzWQ&c|6lHhl?>?2I$|+J(
zG(r0neaxP5LjaNU6z4{F@VGjSZPabu`CkDvb-NSQeEz7K(=fRRA2t4=`Lw^=3jO1F
zft9a4S9^61;B2Z9?Sh`gW=W2O1q$n;-Af&1=50AHE9lsI+`lB*b8Clook-jIOB6ph
zWZYVKJVfVlL`QKg(P=94IiaiTo)m|9N%9v6xs$lzzT(GLcP_iH_}te~<od7NBUnO~
zq+a1cGY?d|%)&=)5Ql{>z6-c%Ql=Zi|F((0AAp?nulOh&{h&PhQQL}|9YfD&RIgUi
z*gubsnQELyd<9k0;<6T&Z>MzfKV@{Ie<WLnOcSR(kzP+<N9glBIp){N_$C$Om!0=2
z{=tps5B?#mKh4tOmY$NL?if-Ux4EqLSD7U%UN*qBrJ}I&VJ~}*jUE@KFbzjesOlgU
zSKB#$9FaRPm^KPZ85pO)PVPk3r`ZP|{+6ZD+)P3m*8BgP#niHp@s=5TLn*R#+QDrm
zYrj)lcC~!mDZmw=2}dJAMl_>K9c!xv*4%6z2ZHDTf!`?#V_Hk#Y;wH|?R9f+CcZT*
zBaTRO0f^A^T)6)KeSxtpk8ZL@f}`Blc}5H|sm=1zDfG|Ii|v6ElBU6Q&*wcwg3(S#
zgjJg3?1Wh&SBghS*2*7ZLZjs$?d&uyOI|z4!>cXLnBed1>Jp+WM@$h+al#uyq*m}F
z-^}z?^W~DVtIvE@LgfGL6t3`vR44&z9WqThYpJpx*tOCP=%XQ{Hzj4cANvRUrF8wi
z=pwXRm;-#2F~(Y`|58kPm@v4McbeR1>#9)$V`%=8_Qf*_0u=YB+4I3@p030uC3m%v
zYsTu}8r%7U-yOkikoFz-j)&S&3S-h!O1{Ad*N}Y|GhDhsyX4&#YL2;%-=}|J?tkk)
zK6dY&)-ct9vm@E4A5Q9bnv_S!p=C6jqgxUjj;{8S3J&h%;{0?ysvDN>lTyxXY^K&v
zzU40n*b_>K=cs1h+!L<d%Pq4#Hy4wvmqXpHep&p8^c-u!g==Izd4DARWyS00uUW&N
zZ8FSHm>K3O8#Cw?)%yb*ecr*e`V-)Cs1>_G;@@T33DXMv4Yl$C39H?`n=myXWYk6K
zKP~g$`^yv)w>hyC3)f}pff?kj6i+r_8v7ndYt-MlW_r5NVcE_G>uNqa->nYc93Nv|
zFA!h?0NJe~l!tvcxpQOf*qDnlfp{-VDuUs-BP{Ej=c=`K)3@Wm!&vA<?|;3ZVyUj+
zcE=L1e4C0B)h+Dv&=H>K^Tfc#@vRPH+4RP!sPQ}p2dbhp8$u(7YT%W@p%e*^jtSe8
zZzQCG83Aw!Qh$Wdw43I-l1t-SenUoIYK4aRG0>DZp%{Ko%kK!>dN-$UZ^n9YM7c;M
ze$I@#Y|JPDp=xXlM3Ry5(Oq`s`o@vot@N@GDCR=mU?Ee30!_KOz~|A&p?4+!xyV7H
zR)fN1bXqpT%LzEU@qn+e5CD5K2;&RM%l({GIxaz*W{w5sEflnvmDpiD?tVRW?B=+5
zK@vbW%SB0mlaRT{DQ$28;kj<El^nQ?>oXuau37+|QvP>C#P)Rpt^`LEptqG|9xZk(
z-&PhMLOO!Q*FhId{k};r@!3;6$g0k$52d*K9|Sr(KP^0s@WY}><6XgCw$O{t@+xSl
z^Fzh*y1OglqdqWJG%7#)wo-dDpH#Hgc1lNuexd?@lGI(s5ah9fdN2EQ`W-3$Xu4-W
z)=gjlV*e{8fFQKJqIXm%5GHXVO2zoERey-ybZFs56%*3N6M;?NVX9!9Sh7-n&~MVe
z^9nGQd;9{B&r`7-8ZOOgI)9suR9|R)BW_lg?fn7xGc+aRvw}|j4HsJ61`%zQU%sGa
z_vk4&tU*63UpesihDZ;&ubar?uRi!kSA>oOlvNdgaaR0{D^0aQbT)TeIj`lv2!S#I
zhYFFyK%X*rx~eG2YSX(B_^Rtokg$00ZmTj81#tGCk*s8r$T+mw;}@V7;aLVqw+csh
z6PFTZZ1mcp@@CxU+&kaibw8{rk<9*-pFVe6h<9uJj{0hzm)f7|(kyJLXgj7cwNtzn
zmwO}&dQktnW8}C-s&9(C3&(wkprxaB3V0!VNBI666n7E;UCSC4#B7+SUWJO=oyX}#
z{D<lXsMA48tz@}!LMC~8$?g@_ETZ;c?I37!j=VLs%Y)VFGcKNe3jwQC!Rs*ncHh}0
zny03|QanTC)J$m`1FqjbnK^=#OM<V}+B=dSE?~gPAEytj@poZCTPJ~A)_=E1C<ry2
z)Lu`sO?mKsfdza5VkbXF?>%4;bByUw>@@w+NWKZo-&%(7`C?n5y2`!$_)JBDzqH!d
z5K|$ryvRoF@jUKscn|hy14RjNiV6XQp<zP(l+rDY?<e(K^;OW&mmbHm5iz_3uJCxu
zA9?+<iH8pF2G#SUzP$y65MDYSJQ%?zEnJIFlx|AvZCxw8n<96r3zbsVc5grep)#Fm
zmoHpFuI|CWErWs;0Hap7bl%4i8P|xaunS;U%+4NK`e8kS_Lrnc=9zdqJ0BOlVoH*+
zds14S{{=GLx);&43BYYE%1qj$d*9`!hG$Opuw(gUg03ljG4B6uiMR4N=!s|N7~Y<s
zCLyC9LdzO9h4%E}RZc<>0?luQ_|uM`R)-S++_qAngJe&*ZAe$B9Ysm9;B6h<Bh>&Y
z<F1cUrg}{Zm=ZbBGG`O^N@`<dMF36$l+q3gTgx|00qR#TI}VUIlg)Tw_k$n%v(YET
zqEq_$_LV0={!rDFb7?`Kr<f?(MezdmvW1t0N0!i|r^|OVv`nFw`BeKK4M8|xn*&8s
z$&&uVugy(x-16F21-<vJOI|JxtXC}|M8yXuYp+VsFi*i3{ReJ#d1q#8I9c$WbehJ9
zf4b$b_3N4B>vE0uq(2Zzja3pc0xTZ=jI8hY$QF{IS9P_Y2Hre~*Otc$Q$t(-uc5NM
z`G}mOE9`gh#!x+{_GmHUi$R=A;4q9eP7DLQ5Pizlh^<cZ<ZI6CgKl~+Kl8adPzy`f
za1jV4Yk~}GX3@6ybwmrEOxUs|%lh3!Ovw*=RSN$>E9z8E?my0V3>x{;16L(r&9~8h
zn`Q7};C%EuRrQE^t*UqQ|1j5FGUjnS?W?ru4R|%Lmkgh@g2YbK?v?^aUY|7L?DS5I
z=%@4PY?muJ`G3^0tXs`1Uva)TwojC6!(q2e<IzQcKQgj1!}A(;dBMlCZzf>ENwg@3
zLtd1wb2WZ6=AabiIG*cXLU8>>_ZL>B(}gAadUiJH{qXY-VwVxgo82r@a{ueDTnd1o
z^(D+;!aCNVM^XV~PF1W<iK04uT^SqzXnjrwJ5n$V+gwcTbogDj8Lno)Q!}Iy0Ga#D
zsLcQy>7)IOJF!LAOqwIRmgH-Ps;(eN=recnQ5DK>Z7*e6*QKNyK~lh5RtsFC;*R`%
zPiT;LLby-fm=?+brNTrfhzFwuoCKK~%VLHL0(?RNPh_w@6SSVBni5GVQ27b)#kl<b
zmmtJPK`gH_k#4>&Ae2$@{Qb@BAI!QX-R0Adt&a2Q-&#%3W!=!hVV`$wJAaTPwOCCw
z)cq0Cv!Vb5ZHji>E!T}tJcJzgDbjo(<;}?+C9ERbBN7()%NhQAXe0oxQ!{+}6fcC-
zTQcXoSJLp$DSl>f*A0Hl>WA*$GAe|={s^I|CC2G@0Y4oQH;Z&b{Or)NO`gj<0ekf~
zor1wBSfzY;gjF^quiR-VdgbQ?-Zp%EspAg%d7NSMdGA5M#PV@=_1gP$C(MRmWbPW-
zbOIu*nMkGid$;9<b!1B$!|oJu_o_@+s0S8L5It^|(P?0`?hiend+1}MM+w|R+U_jS
z(_NnqVsIZJXm`s=&syo3$SSEVL1|JsxR{y|ltDuUS#^0qQSZ<lhr&TdyTVnfB)8`6
zXYaeDtS4!fQ{K=Wb3XT4qy;JeM4k_*S}WYauPbYt2Sled5TaSJ|G&ll-4GFf&rhP2
z9&>UZxN_R}SJD#h-XICPn%>sllZy1~9sbcPdQLj|z_2-I>!P`|qOi7g0>Iy6w2q4=
z-Q!;g>s}{K1QB5^ru9E6D_C4%lYa9iv5#DtL8pW?k$%4pJD6T<R&+0Uo50<pm=>N=
z`!7HL5l@&-L`kfsAAHoVx!Q7c=w!Gc%qMjIr~e<C7zQlXE#DvtTy9FiBx})qCthIp
z*+0Z~h`f*nzQyWL0N|GRG-ZCbL6N`*X>7D5^<x$GY2jzi=)(}`#s}-Akw0o(CrAsZ
zB#am@(q$tIkn1pLqCO+`N*`dE#B{cfKYVRs@IRbIIRLl@zeJ^89H;3&nF1ZMv9zyI
zdb|TP2hzhRn+1%%wO9%He9=_hX|JkBz@^Mm7b)Uy(iI6pbsza{8fkLfC8Wwtap-dj
z^rz>uXs4Z-2-y4EBfK~w7@{VBPL;T`_m#+cUcUYQ_FL)Qqc5G>LTByF=O&!zYF;wf
z#Q@+chT@<fCQ>gX4#FdqBQ2y&BpxVM&wJBEeWjP3ec3jjIk0q`mp7(#zy(PBnKdAk
z+unM7&b1V1e_ge-fRVnmKonpr0-rwp>BIm4O@;N_bXkPPviGKddSE8EDTTSCJ8@cN
z*IB?PEo!G=VC)2=)sX-HVQw`*mv@tC%5TH^>oe04jFTk=+)E?GP#1Ed*&IeBDQt|O
z=AQa&q>cg890v_r;Jm%0bQE3K5)bm2Dx6%pc`=pMN72=v#}+rlVRx>-ydctZAgf8U
zWG?K(-siJXQ~nF@xMOjHZ7ye)6u?v;FlHjK!Gpqo*B?)KY$4HyHG+i?%m?I5bWGhi
zi8`lhrsTI0m|uUvulYm?dIh^id*qK#{5=_raU41=Aw`R>e$yoDi{Vj^pBay-P$Bvg
zy3OoFZp~$7!v?RpZx2nfNIQ}|eR)|h#*0yiu$oe5-y&VZ&^|${;Kc2htgMqB#0_|u
ztsVb65Mm>N_WXswpZBRB-;O9ag9=jvI2Gr0rziPL!vZsBD6y}TEvqrdVK{u5*KJGu
zQB))=clqxa-gfDW2jx?JMBdPCMqE$2kgFQ}s(MH_@$^s4tVPc|{d_9@d`FF<r8fKL
zAf}?RUiHt_+jG5VUB5ErI}0M&gcfhg=dNwaJFi{-npnbx$%~C2vd~H53C{%-syuF_
z<+!F>mi4vu4^ASuCz9$C>z;o{PagFzxT4KLN_+Et2*o8Pk755rd34lQT(~PWqXOXf
zP*Q@bh+EyC&!2A!cepyk`lI$`S09}SWeWbt8dx$o3L3>Y)*KJ!ijaS&U4Hodvazxz
z2MX6p;{PX+MRMsb9CP(K&4((UoQx!7X|k^ri&)qHRSHW8@SWmtGHvn7qvHUcVuH6&
z>H9{lU+#1r<}7^Z5eHZC`pZ=hzljb&_WZFKyMZ&eAQfGSqd6~)Ec&~51qh7PWEiFl
zda%Q(RwN(i{+{BSOj<3Szvo!@^8_j;=3K4gjW+)ZzaHFQSnl|5SYnhXKzr*okaE9U
zIAYKLYZZ=){NBI<>5gV-=(f8TPV3C|qJd8S;UHF(m6bsGbX)WNhotIbf#~GNyN<i=
z83$0lx3BHpXDwt_1WG+87@dLy{Op-RH23Vecn0M~*0ZjH?XC>6x<b^&#upMOn(`WA
zm|W(u>2b%Q#ar0Z1mW2}8>`K>)uBnfuM9Lbaj+D+gs-^MBR*X;1+6#<yiZ_N-%l+l
zOPNWC3z`>V?RA(~rWOLgPwBPDy+K(Du8zx<5L`TW3T-N(Ek=XcgKN(-iV4Y~CD%u0
zYuTUX&^wL^Q)oWnYu?f82C{aOrjq28(huEV#pRAG(zcD-@h*1sPCO?*H;&Lp@WJb^
zp!0r!OffymB6ECX{fcQ?zlo9OI@;$6&F1B*f}i%P3DLFWM)iC+yi2K$=UAr8r%Rft
zqhHp_Ev!@qxNPH7@IV@$T#2#F*MacXV=okQi)!I<O-6e`;m_Tzx=v@FEZHqTb*HV+
z-7?JIed_P3Pjr@14hZS-s$N(Z)QbB32zOc;WfuY0TUYK;8SSHPxkawkTZDv6TMapP
zgON9e)EV^wyCYeUcdEc($6tT<BJ+(%7lme*5&-jv(%PhG^k){Bg{!Vb5z<N6-QAY>
zn!qDz5sTj&rniBIF90Y++l<^H&vf<eCG9W&>6D9srXUSgp+rbMJ<z5#8nSM*Y6Nkg
zc-^gryrTh<zT-yr4K2hR8-ae{4M)1eISUp!VCfbw7~eU_NzS)L7XL<u{Z8uIn^DeU
z5L!HX3t9f5DC`zPKq;F@Hd2qWwO%Nh3xaHo5H0UPj@Q}P3_NH-8424IvC)(kB%v|5
zr|by7dza;A$bQL?^%>oXiyTE?;SM&!@&0kVL>+Ow1ogUbUDZ3x_pY{aFzcBU9#c=?
z)o~1UC3(Y}w5}w4IgH~S$}-~&3FXcdWcznn^{|k(2&Smbj=1neswdP>$e&PI8eXB^
za+S!~L-TV2Zvr8=IdC)j(QgF@#)cDc*5nOFO4By$`ML#k;gR_73qOP(pGNvM{=V5B
zt4o}Lb-eww>c}HY%yCr-agQe#u?G^%%aGNO4uR1i&tSOKLV?W@(XwOsG4x7usKeZs
zN~@<#XcE6Fc>4YbE(38z3XU8L=WDY01psCh(;`fm^S?R;|B_O9bdaxd`gda_Z!=4!
z_t8{;gQ~roWWzA=3LhtrUPEvu9B&M0{D8q}lV3;Xm1oH1mH7kJyjli>E3rP-^jevg
zvQdr2+xqm48vXA+?viN=tDEylq|K2Gf`0zMD{aD>jETEo<P&`#tIV@j-pqiIg};KN
zG=q*^^o~9WnyG7#`OAxtW=f8024f=LT#aR~>DZ%SfNq@9xw{vwW_@MQ3gq#U=_LPd
znk?iF`h8hQ_yDoi=#P&*_DB1W%S^1cWBTXT9}bNGhqB?%AN_D9d*bxV{g<D{N3BhF
zce}lvfE(9Or*pF8{PoILe3N%pm+n@s^kMh8O*kKf8N|2?d}xIqFl7Wu5j&9hkvOzU
zKe=~}F?aUqvHw#ZO=k`p=XGL_+$<RUz|wz#)n!FM`YfRM8KMwErd;|^<(>5vO(p*X
z!9CB;Bd<2CkkC-VbITLlN_!axLKoZE!IL>#3(x1Rm{qEc2Q6pl^NStxZ4V1gMZLY=
zKg2Y@g?+k8#}6}xshh<dascnZbj-Gi?6r@zl345s<<y_>hv(Bc`=cVyTk!8Sr})=Q
zFMPV-<Su!mw~J4g-NWS~1)nU{=~sXFNH<Ye8!%Vyx4(#Q7f$1U<u4CSEw)cKG|)15
z#!5SD-7;nz3?5jQRbWl^V&bM}OjOx(x&&VaZk`iDiGFb%9bI;BF|mehuYZDU`g3s^
zXx$N_T7qHqjVRU>82#+_eYhX<sOR7+oKxvs3m8@)1{m8>9hiB|m?N-Q-$C9|pi%fA
zU5wqdwp7<gwkN3hJUS#P59W;+^L;)T=oMxRZumY2#6i28H;$U3`|wZHRJ9a70Z}-0
z$;1IFi~ZS+JnXn3w>+vt*ycGLMgQ^E<2SYMs~jMRMO}%soW>kUsa?o<Q`Gz~xO^S$
z5?e>S*C}lq*@`DrNhM5b&<bP?8tsHGroiwfJS(eUg7H7M+BB_lmH%ONOUMuwA#Ao+
zyKf;2CVH_$^y5zEKgO29GuR3x1So)~$w_?>JABkYSCDph92gI4<G>8tPCr~dWbz{R
z&#tzx)GqUgy1B3VG0TAVh(E@{u}!2(EtvhF)fo;%YlS-qg*T3^_xv`~GQTGE!wXqp
zyupECD+tjNA$u0J!zKUa`>?|wuQO^G-y@|THb)je6MiiH+!vxXbqKuj3N6CoK=DVg
z<>y!u+Xhe?27FSn+>nQo_kBL>hJ@xkFNx4r;)NKoPG;&5xCbBZl#VgBO|Oz=`^iO$
zewf^e>579ctlO6ex=%YpFHM4Yr1dY{!dbt4Lb|*%vv!ag8#-_65&#JVlN)r;@E$&G
z?AqaMCM_8w*f(;&ry<ETFxfw;zIUh5%i3Om<d<lHMx^_p1y+~_jweSSm=j(&0u(-!
z7RbH+5zS>p-T~2|8E%^}BdwU^^HXZU%uAOxdm@T}1)Km&|L3abtpaNx6*~vnSMFJ_
z<q1J3ABc~~KF<!>2VExYlW)V7J+r0!z#qIy`se=F;9EdH_uQ@!$bWiFm^h^DTwRua
zX}xUU#Tn(7uT+hL?GT$n$1)4nU+uRYH5`-?0c}4X9CG}*i;(k%O<rB_Vb4ZiJLLWc
z5X=B#VkJ?%EPWqqR8+Yq!%LYw?UHgk>xcLW9FT6~6If!SsQ6bFtM|=Yjlfk>;*)*l
z449*JouVr<JgiF*dNg=Rp7Im11CITPWiN^fIIXxbiqtB5R`kj;y_X;|<>XA$9+te{
z(*kYH;Kmd)%RgnqM=1_mreQMml17@_zrm0@_Ap*D_2ALs+U$kEIdYtJQ=Px(Koz!+
zWV-6@`JI1`{fMyx{*AiemLNhhT|rnTn|p5-cf+kxGRrCheBw-wVr`@Ln(u!rR`9-f
zwnew#V<l!?5r0V5ZWoLopI{6ay(V?`!Yr_qVZHFNG%dP)r=<QYsfbICFk->I3nkvL
zA(}ec<JtRTCJ+LDJR2zjR4O%4sbny~cAz7d=molvt3o`a(qxXzVm(uAP_%DZaUm~Q
zxousq{tiUA%%=fd3-wKHNJidN2uIta?4PfOwIf<((3VNwP91^v@#ZUtr%C+o%{rC^
z^hie?^GN>Hf9mLxiy!obmifW}8fJ*5O(lOd3!2aW61W^|)r3oWZ{?Ts71Cm(y*|1}
zD%Y|5w`FUumj4Z(o2f+7d}VprY#G8~WXkvV4mgE*i~x%6LxY$&>5>zq`Rg44Yej@p
zCpcXECo7Yvaf3Jli3;nHMPRAy%cP$kG9w`A6}F30-e&Qcer^x6Bs51#7)bH{x*=|S
z(E8>=M4O&(zXfa<u(K9;b=coqH+9xSCjFmP9+vSpcq}@{qOoW5UEN^A#qm*lG?9n@
ztk2crbV>fvp-bg}WDcBu=T}gbU>u816ybS_;lQMj^x8~8U-T$zbHMj0>K%kgV6y0s
z(h)YgsI=*f0x{QhieFU{5*Ty+EVk9CbMLB#&gn;=`Mk)1ol>~z*<&6E{R|KlCMQaa
zB)HZRc(Whqn(7Q{hbKl52wRNag{?*}bk|w4aQu*3gm-X#Z<974gcx0hKyXJw&I18y
zT?GhXa@j6mG!#2JR7OX&g=&NcG&e?DN%GDmgnm&jiEG~$w9G%(^cro6<c^{mtdkKq
zg<Aj|_`G@3+d`n4(8TR8rg9j2X0I~75lVgzET*HTtd2N&znUw5qds_?o3@lZ)s<PA
z^d()^`!TG7^#f$(G&p~SFvG^2JNMSetM7naaAD;yNLN)*v~zE7hBtN3$%v6Ue<4J%
zmypI%pBEk76(uk9I<eXuV~j*XE@)uA+MijgoZ5iZU1yr<TR~RJzQKlW5XDE(>bQR}
zo|r)UTMFJRj#?kRJ3L5dw(yg%FS*BNCLQ=tgewAAMRu#>T5AL%=I?&~V<-0^YPk|0
z7i?!>xbPUZWdQ*jN-xb}^?fdYdO8INrspPIEOHN;bI&okQ)c-+{(cZs)s(_FbMQcE
zJET0CWYrp*9C-3w?1^Ni$8Mh*ZX3i;Fz1p?8qf=~0vhiXnv#CJ{6;-8I!l-oWkS(+
zn~1)SP#{wlrt^#Zeg8DYB_wN2DkBtJ5<LYODOTlb;~EVU)QW@M4#8o81FZ{09jF`B
zUke;`3R8FpS<lQGJ|A{*g})D%L(zqW?yzCY@vfgY8|87fl4lV8#7aG;-!P#g9gC9#
ztuh1ZC@xT_>0`emv_^S$vU!)h`lDY_b!jLCm<}GS8%#817`_w?A`lxTUNS5Xz6REp
zUtg(%+YDI~m(dIz4}F4dF4By=`S^99nP`4NJox$Q5%O*Krz+%$Ir(CW(EKi&+FC_d
zdFjZ==D?~2yL@|5r*{G8Zh=_@mYw!RR(uX#9(MFKu(ZcyGAUrxFbjl&XmYUnOLe1Q
z+2rO^<H2h#`+LTC5B+f|J|aLUJR*0B`@Ko@grO_$AHA(@{mZ-eM?TL?6ED;K3XxcR
z{=-Me$hQMnxMsIPTug&@9&f%x5+$%e#;ZZJ3^=UP9A$esu|N0SGulrGPzT}}<vN*f
z4e82}sIq(D)+&R*@*VM`L>MifcANRrAI|%UYc+M$Li_5DTI%`|dr7!DhwEHPbS}a0
zRPfKV=lHJ5u#j6j9@K+3%D@6MT!K3&9OitwX+D2dpcySCM6qp{H4rMJvlqKxJUjIE
zK_t(iVi@Wty+n_0yUvG=tk3wMC%xKiR@NNTxJKRKrRC&0#!D?(3T)k{`fEoE;!nJ8
zziC}~(g^r>02>*i6_VxHD9u_F67~M}Epci8p(vy;PDFyBR=_bfM4(ei<-k&0+v}Ze
zOQ3}WE#Msz1a^fD7h)DjZP<3nNkv>JU~GVl7sr#PVUNs4ZPT+GDb3c8DuZeAHX45M
z7wFyPuT=DyIg{(=7+Sv@J3X*WDADF#dhWSy%7L!zxo~+<kS@j}!NKI=vf7Eq{cT~|
zEO;D_^VBs8kwmANRWPk&e9<bs;SN-_8BNZQ3P2Bk6-zeV(-X7zA+gBNmC?on2GFOx
zHZOpyFSM(n_pkn68|W9jc3R93b0e$H+&rgRagyfGeY-Qs$XJO3PsU1{{K18J98Q3+
zk#uSQ8QnIwctqxB7*D&3_P?n=C^G~Fg&ZPy==TBLMKbY^-yQSiq=nEff^~7DRS#^|
zLB3~pmFFy-xryvAQ$2osPsKeR0b#bJ8pdfCmJnd`a(j`e(LldAPf2yp){4Qwux0qG
z0sP8PBowLT1#ZXWaC9?PnFuHkdiMP-2hJC^MEkv{kq|%5->taZMHC6lkhFjfp>UD&
z)zd`PN`dBrfliuB_RhF*%}UWb4Ayk7`#+A)D}c(R!4%$xn<2Px%}F`FBzZ{BG2aMj
z^>E7BhJp|4p!u9JFnVI~4qS>C&x&+KFoA;f?ffxZ_G9e+0K`jv^xmAjV|&0h@@<MI
zt3IV)p7h8Y(8VOgzNSlmWV3%gU(<imS_jm(P@E9XL4mN^VeH{=mez({9gg5kLUXr=
zDX@GO0gRKDzUll^#YckZH)9E6GfV(<j{t^!99JRH^)$R9*rgVK-p{u^+_w%7W9Tj?
z{RaKjb_&h=-=SL)gN>F!7t73}BZF3!%NmK)H-tAuw5Qx!bY<``6>-oXHYk3MCOU%6
ze?_a1A)5EBm9S`-g`A>Z4)+*s?k+kg=qG*aOW*gL6E?pYs;ymtLmgZ1T8e<l4q_SU
zcQO`RFzJJGA41Yy-vlg;4w&B2t9E>qR&<fz{%^y+5YYKS24-|46Hl7vNC)MUy$HW^
zD;9HXR-wLc3o+zCfv352uq%vIo$f{XJ?Zwl=9v=4CXNfhI}ALM{nbSBcd=L@X(w~i
z#Igx}{{-)!Xw&EnT<iHIm8Li)A&&E|<HoL<K&Z%55X#;&3m2c#GmWb+W1xC3zTsI(
zZ_3%1fDdiZi5m4epY+KQrjVr5Zwf_f_`pIDg&FFQo3%hg2z`5aE0uc99|#ZzH5UeN
zHYf{QcKA?v|6%L_fIo`z35<i`a=#8sq8QH3(Oq6WLxc%aw+qspx#V*cJtKFUXdH7h
zOA4o$u(26^EX!<9ms~5e%r@(srLfUawl`vC*mKP_Sb{@s&G30d-vv#yWKAnK6z)>4
zMvP$!u%E`MYNTifP|EYK^~;6&A2&<G=Z<?*eHQS@k8?{fVFt&)-_SjJ`=$x1=Dg>a
z@EGEL{w`j#v6&^3-<AU0Wy${e^L`k(bzqHFsEP>b%jY}9#IZkW2YFPoCd5Z8`P0Cj
zF@EFNNNl=?IKjA&9^2v42fVh&rj<a;8U(ng<@#kdykiYdx`K!+J+<i;)01=vn5<>t
zQ$tMuM1$?eLKVsyNqN%b!T*fouUSEgA~$<q1P?e$GI8Lj4S%Z0#!p}aR?r|a;0vdm
zz$85WX(yyf{ZZyn>31!G>+?cy(midF45Zc_RUcVlhUn8nY8nCc*dcXllBiV62}%GK
zNk25S7OPOTM&FEnO$gu=(E!o6kAGZmrTtxlqt(&ahOT@+S64&o!Sn@?J#kp^QM~K>
zDe~MM*XifrSX?EhpUp3(rnzlC9hg)hA2JsVQhDOa#6$hUHeylE)nNw6X|F}q;uF*N
zjsYfjl|xhqw-LNGvvb?3qb1|tfps*#D^Kgkk-=e5faXDyq0I@OL-1mz<y)W6+|jow
z&uy|TI%h=^_&5??42G^F9Oj4Q{M7g}1b><#MH+{3YZhXD&;U3I-4b4)l3)|_TqFlD
zj_skd&Yjb-_)@z7l%n9#3j*}{>~r9%Dg7GNX08JKhS0|D=;W7Io;>UR=BODPLYJrZ
zAP%<oimgqq;HAUrQzOWw!DpL2^RJqIf4(TVlYym~62w7BfHCz~f#nxUx-_DaG@(ag
zEzE>vOiVG$Z3mC=naE@(7827MvlC+$1i0v^1FQmw5`@oT@9eC~e+-IR*QInJhaon<
zn4m|EmEr0JU11li*70a>N-u`KTBhe*bU-+JdIXhYu4#x5%!z|u;U(0yPHGD^XD(gk
z!V#wVZ4{k24(%HfqcxQ`sp~{T)fw9y?VqFQf-ig75Q~*z^KYYw7ceMX<3pWAZy0zW
zX{P{f<0J9Xmx8W5$s?XWB<3Gs28n*gXpfOJ%f9W^0$aff&*3YF`$~H-z$kkbWh2db
z9Y^y6lYo`&!#-|8gCvl&J@xDlzAC(`8If<DG3#lIi<S5U=jVVA(TH&gIpS59Nbe_k
zp#~Vfe7}e_@aVl+99k4u19Bl5{bqeabo$m}er@;Mw_B+Zw-<H|OtU~F!u#T6rXd7y
zJACC#(outN<gIO(a?n1!ru)=lLWx%0VjU)tg`X=Ub^vb#?dPC0f1rB@MvVM*Jt!*L
zsRAVOzJe;ZmY6cGyJV_HWWQzfRjZ-`K!iUZ>W?_tEA@n0yut{{+CR|gW}r5eY_$P9
zUtVjonTupe;FxKej4D)zbuVcxXK3LTJTp2C<Bhb>tj&F}I$r};v7?=72iDj!Gs;C;
zWP_PrX5cT2VEcg)CO%!dbr(^5ng@YxeF8kG{^?xVSUbGs-LvPZif6M?5tD?+5J__#
zs-G<d3n#-Wl}@A-V735(_+%~l*c2ia(JeR6Un~%qWl7BsYYa>Fo4kX4NdEnaQss+^
zB?hhK<P}|AkNFzx<+dne>Pn{x0{3uQ7;5u5bLOZ97XF05WDK<8fw6meXZQ*6)yTe*
zN64k%ve&ZTg9}1L>qI=w0Uq;c(&b&n>80P7*EXi2kR$p@SIXMgyN-$j&eDVwF_F0Z
zB3@jL!g7ls+lsz=u1Ug$MLhQHSlf7*X?^|1eb{t&_2_SnJ)N-FU+10Db?2t?Q~Yyu
z@1ymt9ooPkVS(SX4lY$k<{sjx?E9z7_3S?nVggeF!)==;t`EVj;313M`Mz^YfzZ)y
zwt_H62+{A6yZ#y<SyWT|Gv6|gF9+@rXGfJyf|;PEax2WVt-tLWIC);ihXY1Az-`Y^
z9&DD22p9KnL4z*-x_~w+878nE8|r?|_K|EDLLZ98&T988`!L2{;z+ZN!efMkllZx;
z<Cc|ra48U3wtj8W<2IW*Op^AJADS#@1VKY0g~otHRF>GWiwIbyXA$GV2Xf`!j}DI~
z6>R#AgJME{$-7C4rCdpN3NCC=y?rG1R|+#u{Z2OhN+@Jn88-^afD!JAB*$?dv^&s?
z&%Yje3g~hg?SLgBG?7HS)MGxd;sKz5E=W-T$s71QMD0*WbKjngVf+I$KK*@uI3;K6
zC^-i==6G<(D$MxJ+oF=;aQ<>mq25GRJqv4e2t8@PpK-tUTIb{YS(#0-Trsy?`cRm}
z58lYYjj=O938$l3)e|Ls5o(DAT;1vO6^?5T(tGZmT#h+X0tmGM8(lp<>IAfCskTbj
zQTlkWwTOv2pNQ31_ti?SY21NYXXGSdZ6X4nbx<$)$Wj2mIe$ZT-GN5u&wh8Q`I3Ku
zt(yR%bs=CvQh`p`0+)5LrqY=zbFh?40<W{*n-ds{LOJ+)n&OzqRv6*4soq5eIkg<J
z{Ns(dIw3rZ2@Ib^;(IM@<1jDYnzMSiw_~d`<q%MihrrL;B80;W9-bk6sjP@?J&^Tk
z{DCc68F?#UVtnC`Jt7GI<+_QkKxnZv_B`yKM&;rR_`h*KA!+=OsA27JZ$}yKe~#E0
z&FyxhIO|5?h^(!&dx;~Cap_cB@i?{!BHm6&D%->k%bz9fXi~mQip(1NXL%Qr5)hgw
zTIsX@@XpXzB3>}jsK+7adBkC|1C7ra4jtLyC3odDdCnw3N3b=l5da{6kzzb9x_6jr
zY=iQ!(CAf6GNr}j&ysKMPQ!lo0bwhvbA$&Rk&nG`J(DPEzVF0t5sjPo+%`+UL;I%+
z%N>#xHIrkbEhCKtma2$sa~XT^{HuD|#B*V}T_5MJd+ew5x6bSI+ZQz?3p{Ov^6{T#
z?2|wRU=fYhYCHcd8Sf6P-U?|8q?`r3Mtmdu3l)z&3czo>U3XTL>mt5W$1}$5+B<ZR
zC6IIE<V><da>l0s4PZQFS4|I%@he~PfOv}M5>uG;pi<!$Z?IeFy%*{9bWoI3fEJxT
zz?4C`+E{q<*_FJ&t1`yt-J#pNZ)Rjp(4B|2M}8ryrNBhIG)nSs8f)^B*Yu?Od%$Hc
z{U$IhWCsi{v_J=fl?;&Yj4SVzAp&*9v`Cqe9H<hh`(j+0bn?bk3f>eYcx5(@n~nh3
ze&vswnl^E89=5k|H5>#8{*g+BL6qAbA8&O+FrWW``|V#pLVUz12q%ZCn8|tZxv}2h
z$7A%Ki=9i#;}i;ACaxNnH*Hb{t0-_5Qy`*3M|Et2>!2*WzWai7o06z+$5Q34coZ6~
z`E6{A*XB$=;5mAETx7MQi7#>elk|;qIn8lb*G)#jXhCLHXhKF(GxdRqw&KBIL4=<+
zeKfG00)Og$0NYZ~k$K22GxW)HogIjSr;})$!gdE`$Rix&BFRWYKAEaO^7)n(A(qn%
zi%Ci&+?1InAI2)>C~YI8I%zBpZX%uCM>c+sNJ~_{a#3WpjZheS1<X=d#`=_RKlRk8
zCn=95{~(s1+)L4oEOP_o>&}Kbt5W($W`}4+srfu1kvyQyzE)$>xqc~9sa?(t8F6$k
z$NsjOzd)`NU>&ZXJxS68Op%*URv1qp3Bvtev?o{ZYj^pj7X^#HtrFaVgDE%5=C9X}
z`%CA=nO@ud*s_Le$VnM)p?Uh(Vv~BvO8+I;Uq$Z>i*>WnK6uXl-{~RXKEaCYMm}{`
z(h-FC*M+cd-9iA(rbK~=err@E{Xpll5u$`0)Cx3O@H|h%jx2?VgsiS?xT(53`@Y>B
z#l=v}L#E}TZUv8@ajg2Q#Z%J8xDIy9gTu#$a-+iRiKq~M2j_&#PauK8mQDdW1`Jcl
zz&OdTEs`=;{n$oYOAo{g(#{LXRyLOOQUW~kv>$Y$b1BlLeLtxF$=9uVtPJU4hh%m(
zr%sk29vOPvipBeUWr8pW;K;=)ENove<It$!ErCOAIEihX{r2?{t52gXL${YkSKTF!
zd=}*)f*&ak@lp0a4%R`xCU66yu1w00r2@_k_>L{R1tc2{3QKB8h~1q1e2&fIM~7sn
zCJE)S4FqUobG8#r2L&Gj8D$5iqNpeR3jAR74e`FZZTDy=3R>Ul)0{u|qDk!n9?!5D
zecx9vOf0H`IV6bJc`Y~D83#p2o60ZeS}uz$teL|(dHSDD^6bd5ei;U4edr6q&?xO?
zC#WYd`sujsTvz0Q7)$*}CMiiA^A#HAa)mcB(I;XujgC~_WgEqfh$6-O97+H6d|{Zf
z`3BA5FEfs9z6e!x8<Rujk_$N@m&d^Uz58;>!hSz86Bv6^e$zI1diBJ{UHt30{u?%V
zGJC55k`>pVK^;7&K56OulAJ7pwJ*Q9%5u4yA*Z|MOPRb<OomkIY4ybXCDpgK`cfN%
zlo<i6iw{Wh#@V<JHCZsbKZzPeyI@5QJBLH2#hDSA!7l)No`~J;39FrFmRc<mXeXA~
zLHPvr2V-yOtY%(+CRbc%C*Fm0?K6H1$cHq`Z#k%|_D92HWK#dgD5OX%PN3U^hMY~S
zkgeEd=Y6H>NBJ5pmp()J0;J!-g9esz9`BuZdF!}jCasq1p_O^jelPUIo8;D^LHu~*
z&@}tP^YKim7YQ$=Jb5Eh>wvMZ{+{Av>!cbmrh!uhuYX7I<NQ?I{sTJDSWD6v;g-zx
zQcG~CIF1%t=*wGTb-jFN?+)iQ?H*ppaq~3ET$KO5Fx|Aytu&{yMWwj2iO>6pZmVvk
z83ILY+ppkA3bd#*G5Lt&r8|xLC1mU$Oc-bUgU2$fZIg5fE*)@dG%p=>B=c2WK}}LF
zsj#ZwHGVJf*3fzPpRrg8m5-Cy36#iv0~+w|)jQ<vi6P>ba3(T(o-&(OGIaUtdEo8m
zzwWkI8Yiww-za0fT@#r<N0D2ytWW^sg<2fB8y0Dve_S_Q)QI0Vi3rQZ#AS$`J6zu?
z`=ch9pm6Qlrp&j;*dwsMzsR_10e;ca%ChdOebs3p8S9GmXVWq!uvW8X81Ug|V!PXi
z-ma!&F0k!-ho}1_q+hFF=AL*=t>G@8>Z1jI+R8k%jl~U-BBiSN{A*MvoK$k`^l}o2
z*$9)KrO~pq(83_A^+4Pp`?ZWxY-m;Kb2oEztOhoKbU?D1BTJKIKfV){QS&_HgTNAg
zvEN9!5a@A8izzPu1~S*|`n$VR27{L2U;O>@04YyG%d}{QE-rK~$(lrnsn}SKP0Ra5
zhzU}65%0Tj`r>Z^QXpa&(f>hpP!5WI0?u$VyZTam*okG2Ha<CP{Afa*IXy||L;~2u
z)PeDNjNy~a2vva%WB-!`Zw`5AVwVcPvwB9<z9}7A{{}>1?sN1lV9#_SB^Qh4pPW%~
zQ%179Xd`QmKQ#kaCku{jN0QC?bgp^Yf{l26<8yFgIlCYm5sJOAu6%=O9a@%LaBh7?
zAY%0l5}t1PAsm>wIUSW4{z+#anAjJw_!i<fM;j^Qvcpw+$RZOobo^M^B+<nG;Cebt
zx5D94<tP%IDDtxC&~a*M=KR(2`!4zjs}N6)zrIf8x)NvZr_{jcsAkMNbWdhu#>$>;
zn{sZp#dah^3^tKwo>@ZP`t+z*e50E=3oJ`OBJAR|4*BePWhV(>aAeE$f!SA)E$|1-
z+$NGQZE^D+TOB{Wr(WPb=d&ZcuoO$G#NmbHUB;KW^?fV$5(&NklkAmS4Fr$0@8XKS
zF^*eOy0(pGh_uf*Z(W80voa;2+#D))N@HY*I&9lIWv6kLBYjn!_tMvGZ(sd?l1g0$
z(Fpj($DMo(db<hT{ZbX&(3{Uw5DCCQ5#s0ILYeH5rHVuXU(T+YqpD~6IOu`28iJp0
z7aC@;EP^DN37K?vwo*V`o!EI)J?p^MzfRl5O#?O$8k>s6`y<^`^2g$g4Eho3FK%Ft
zeoK>mu#0xA@)JCsxevYEy#+EZ{OcwQW=(Wx$dLm=I`8FG2Me_aSc3TdE&pp-5`#oY
z>kj3YxKeynu1^POysGW|v&u+KuKQ<Na&k(L^Qk5J)QQcTYbQatonDsphWGXHw>El?
z;!vjL8j3?gl)1PSU_^^o^3Oh^RqdX&ChC4YEnK~*hk=I6x_CAd`5OsD4t(BeyMMJN
z97wwCFW6X&MB!tbkN*Ct`l<;j`I=`ZWHk)#30k7ek^~}G1Oxkc@woCSGxXX1BDJJz
zWYz@~7;{!I^TQn{fzgh?Ht^ys`IK_X>@y}$0%?+nCVBCwO4w2MKX{5oB}ofuL)G<j
zLHW|xizOt(Qt7RY2Ve798XRa|gO*Of4(+R#nXAv6-?c`uR2w|g-r5XW6E_+>ucH1b
zv825mcyt+)dl>odry$EtUbFlhS4Z<p*{bjBhjIB20E*?l9ELTe*)%^_Hs!LX5nlk8
znMotNn~9U%EQmYu?U2SRxXjn5@BOWFo0k7OtVy}4T0Ch)?I>OxB*yag$V+RJ+xp!|
zxI~a(9~b7AzhfW)*+|@$#GxxsQ}}{L3;e~DZ?a@50k93nVh>;QK*afmUIG)9qM)pd
zcM}n%Ak^-zulu0yroZBRgmPrRtmj~IH7X$#2;Dr0Oc%4HTi5`l<vMTHt2`*C^PT#B
z*WHbBEEVA2LquC}Xd%TIS3Rc?1jf&_r!bkeb^^K|SiMerevk-E_$<AFk0LycA>J%!
z1rUP$ROf?()Z0TGr;<8N6^z$fQ_|LKxc=?%YqRvV&SQb4`yVDnA0vSc(*p!vN#rL6
zPPQ1hpMnxNoQK;2NaN2frt*g@b?x%;SUGSkgnuAo`n2EW3Oyz7K%#uyegr;Ybr%GU
zs4Xq5bNy<lw8metB5)#df|K_A;o3>64|i+_bEJD2+q;N{WQ|^XBK%J*f%!+Qh{zNX
zo9edw)oJlwOaCk3(SQy5ySH}L762%fAwQpfg!J8?^_R62uW0|9$BtEzXF7gJY6Bhd
z9qeO2IcPtZ#L1m&qfTqH{~8HQ=CDXQB*d-mn7dyAf)q?2TqXcZ%^9^`vW>C55w>Mq
z%~2i!WxkFyli9mX8yCPBn%FG8uwc<6MNM#-c`w)U&^_mz*QI}7@LVKFb<DrUyq|bH
z*V=uiUH0oCtrt#!#Z?c#zUpY-D-81VQH^GSEt42pmvT1HxhHhb=IeNqJUU0S)IxHC
zg*0aOuLWrCqwp_;JCI}$DZ>*MbRb%IBD`Z?&6_bXm^torBc%SXs863LO^Kee$S;!b
zMOR)xPXjZRLj3xOHZ`5ZGpGFxEqQ@-j`1v@A<lAm&x>Kw*!a71U%Eq3brD*i!~UYW
ze=t%qY8k#?g_G}sVQqVyHknT2b7$y%H4w(aH;2U!lQj2%3^<OL{4#}qexv4wjE-kM
zy1MDHJ#yJU2K=GLEF05i5mNF+ADPr2BFaJcn5^0U`z8r6NbQs|7AqR#^v+ELI3aN3
zr&jZ>U9BI+<GGTN?g=#~uD8Gadis)c<q^UsO0eM+XdqrpkeR#iYt@k<`N>eqlOR&t
zIlw?f-T;v1QzY_NlInWd3l-3WwwFL230=i}L9;rOj9YTelHDA<be9S@pp(IF=A8U;
z66}!YTVN|v8KvIbSv^rQ%>J+K{X!+SxYD2}h>+<N1R+m_esdoNMo82>ovF?{f$Z+}
z|1y72PPiX8Q{qJ#;>T8Mp0vTE6so+o{A?KxmGdqxum9RjOd?ZD&pIlku5)wR{2+Ix
z1tg`bbt-E$q;OUAYk=)j!K$6h?eiItmsRUU|A{})v*hHUKTskxp|v`yt~s)^rKhrA
z$|%-}%P?Wx$)WwQPn4BIc5}MvC%UxS$%KsRln7|oV@vFo+m((nq+g);G`87bz<2M}
zxz)f1$iUo@ra4vtPhm3Ot#?Df4Lu4fEc4=teg68c*7t34`J|oHsUU7tOWbP6Wr1Uc
zf&*_hp^Jfkq2INsVd=qJX%>VP6@LgD0m?kgGQqm2c=OV1xEo<xN?ISm)~?Y+SWIQg
z)ovMQJ*uPcYiIY*SeLI4i)BdG+ks|h8J>NE1ano-pjvhn-0j5Xinb1>1X4m;J@!GE
z3Mg-mbRtzO#(8M0$GhVdP@|U}mwO~UJBwK3sw(tW>KNy>R?n&h#nR(3kPIWa*B7nd
zM*eNx_*2opT8C-?1->_tpvIiJM@b2d-_CA^BtNM*qaNol*UM;rhj&n*A32nZzg<KU
z(#(zgCg(}(o$7tBQ5oEk>Gi08Z+<H$%-8acM8Yq?xGiHbnl|v&HjrxhR6rP>BthV8
zM1`<b9tWWyIU;D27J~Qou{q*Ewi}xuryM|C__DwxLeofRurRJvm+!f<_u~}ZkSxEz
ztVTgK#OULIXMNin@;aNiy~&X2dfyD3^)LEXQ3*mFg6{JSG$~mNJh`yF)OWXGz+Ajl
zv^khZ<U80Yj=$RJT4IO&{=JP{L0AVgM|fXIIp=k8RB=tG;w06_9Y?+7+c*{t%{^kO
zm&t4)SuhS(Gi-9>N5%do5-$ny+apVxlsjYhBe*)>u$(-uQ1&h5>KENq`=b;942LWr
z5@==UBpPaRe2QNJe>56FAh>sXtF{)P(Otb64c;fOl=rBqGiQQU)S0PUqwByVb$!Qo
z@7TO#)btASdlEtwp)B&g1-%0IILAOiL-awjcBQ9v|Jvb8!iuL_d8<ndkj>iRD_f9&
zY1;Vyv@DNfrmCJC7!E44c8+fjqD8A{{}~w13p+6$S@+ET)t#Vi>&Hh=vcjZ)#|Pv8
zJb6T)oZa#;t<0+6@#&ke?fP6KWCP$_78AoGzdMki-^>iVP60F1o<6O1=2-`8!MeBj
z{~vqb{npgdg^PHw5X(_1N<E5=j#8A8U;{-dBE9z-iu4YmMic}TMS7wHLhm7=2T=s1
zgn)z+ihvMWhzLo9kmPRk`_4JvbN_^U_YXWg!)9jB+G|#M*E_S#tn=cg+GT`~0+rTd
zcfv#2hFiUt@`#Xqub9c}I^XvGsdFs-M9~$`X?2C5Z_WDWU#mJ_F%_U{Jwi5%QeV0`
zBz=o_JCM=S+;yEvIHjZuoNT>zFXN|hxZ?U>+gH=CZ%RMzsn8t&JqhU8olpJVDBtB<
z0l2jL$$R}BM7X3=;w}SFKS=Nj^UW!Z(}2`Y;D`2|RS=I`x~9<l-TnKqFCz(U;;_`q
zpH48&MPmQxZP!g+CeT%Kp5C-aO`jw^WU7ZBh@cuKZuGDPbd9rLUU7g=w?8UxtQ*dL
z6sloV4+qZaNhiw!nbr4j<3$E7H}#MCL&pt%#@*dyzU-tTo`!DSDjPX?wluO|)%kpw
zzMuT_{>ak%^3zuKr|Ldl__cQ@_r%xu9y7-@>s@J=`It|RIG;MH_37$(rs&7)%Qxl|
zg<m?h%mDjf8^5$OfNg6rS|_eMm+bF}x<j~HVOm7;)lt8Ms<i&daii*g6)yP<Nz(k#
zBDL@714pfKe!&}8qk?W&{~2adsj{eWF*oVqp`L;H;C{h)b-BWy4PV=SNXb2V@Sz8-
zb3#Pxi1FPcM=k~P969njDu=PjIDJ&#XK?(i_W+&2iew}DPLtu|<Kxb1VXLZSm6%?A
z8hNey!|~efeG}|0dKr56p4*tS`<F9Az7)<a^*rQWS0>g=q|-(rMmH4_*t4ZGMnoFo
ztm4)+6#kQZi>@kfkLZ!8E(J?_lKt+Oa^aE2OOqq?anBOMl->p`A<2r=;<bS0t`W}?
zOa;B}jjS?~-<n}v#?w`LDNQxr)QSl+uOCyIhZmJc-I0VCyN;S?s6l#wjol5;Kc3R{
z+V>}39H<(e;%8Z47a>s))@Q7RPu6obDOdg?i4JeggSy39Alfq}x!qIsr4*JOYeBLk
zt@JHUkNKS=3yx#UWx-kD1h+=H8TVo8$E}<t(3@uS;)EPqN1FV`a`g(2Yhrp73^x1n
z%$BGYk7VL@WP%$E#$hU1t_-Wb&8_FPlKN|S{WxB#8zN-RqIyu)EkPAGksUV(eufT~
z)Ku4A6n9PE5u3*^TsE3=6ZC9d2NU2WeJ@K9Cp`%DMI1ZUKtL{x16l-co(4^g>VD(&
zaXlE-UH&Sdy#mwe>bpH=;Vd`0+j21Yt@BM8$t3oy>Rh1AA<YSl4Q9-CnOSq{M3t2h
zy=JlJ4flHP$q3_j?iDfzNP;Q3EJy|%<}&9PbBNl1D@0pXQ&)@5J{eJs=oY7Kk0u*+
zl-6pPj#=9ih3;(`{3v^ssv59jCY0|oYjBBpXshiur)oPPe<dQ}M;LTU5WR(OLS;8}
zdNNAD`^g_-iQrYi%SQS<^9NPXgR<7sc8%1sXL1Ya?y(kj-xm_D=(!zdkQNDoQDf8+
za18qUnc>bo!XsFf@ezFy**7qG9y`=S(i^L>B!L#){#$%G(xQe}gk=?cmDHrPVQD{A
zF;YUilvlM>C&CYV+u!^tE{`39Sf);GF=?`cVc{!MYRO7v`7NzU2)B}exwBa#Cj<5h
ztN948R6g>~U!P2-kyy&CCon_nA!-0?vzow2z>eO4%~3M6@4s<3i?t9eb8oPFo2pjS
zTFTkYnyj{YOH%_0((+V4Raf95%gFgERa5(|=IlX&n0BVAe8YDgT+Lz#x+>6u&RxSh
zjKJIt#2+o2Eq(|`Y2fdfn;PF}{<3}+>h-O#FAF3?aw<H|x(d~M7n~bRj~uSHyTR0h
z_Nw*N0K=WwANbzC$-8+W;MSeD=yj2dea+>*=y_C^n6y;?GUrBS*G<k8LB>=Kvt`?I
z3}?p86?jVTn9cdIgp^>xj0F1ED_uU{H4lq42XBDz!Rg3TGZB5&9rUuQOIwhfMdskZ
zwRK&21#ug6&UR!b6;BgM)_ydld=S(v_1ZS;0BJv~WQ9j)y8PwpT0l<z`~u7HI0nXU
zD8JLjbh2z}p~wK=mqU8jcb!E-uX^MS<v9m#OwW4lU2Z)>k62ygsP12`YZ<vmZ`LBj
zJQmt)bG+v+bU!AMQ8~u)yc1~$H?p-uA6o<+h<cV=k`JXf^RF>Jtgjd%IT!l2N1K_e
zm_<}p65QYwCg#=4L~NztPq$V@>{L6vVRIf+0+X-bV15Y}i0{nwxhL^(BfqT0dB{y`
zepXQno!m$|-Lfb1bG@~_I8`yAPrz!VZ|iyO>uAd(ITc$6sxq!4v*A!ld~N$E2~V@p
zmgjcQw)M3!uI!ypuIliuP6d-Vu<$B3Y8j^1ZCM3pPyuV{7pIQ5_7DI9D(@+&F|M*N
zl02@gB)@c*tZNopLxPrD<O(14_JFt{A2a!er`M6|V>&pqrE6bgB>aa=VH4pJ6^$|x
zv0=qXZX#Zm@4zxR&2z7L-Cfz$65GJ(s})1?odzl_cJWJ8!&8+>T(}{@7PqbfRpC!p
z=C$aMoy6Fp4^wN_ZuW<q;%~1J>nJ5em<XPa>8SbAHuiPv&`nO<$`_6zH<ubLz9;H~
z3ieM_eY2umkFE*tyGjOweNw%JnoZ#RfnFA`QqA0A6#9-Pbw63rgWYv+$>*0)a&wFw
z#Upx+pVDSG9AbRZPOF)oX~b@a6Ga{!!soCxhyqV2Z|B@bgM~)LG^a+3D+e%g&qqqO
ze@1pE4<@ps>nixE&x&rVgL(uY-GOp?!DF<H(S1>pd{kfQ!Bxjm<K|Bm1AViG$#iHn
zes6vTS(Bf6=>#F#Y!Z~JYi6`2q!b>Bnqf^WwCL_N@@taPk-nxR#g_48;3V7;!GI8o
zskA+4(*nNqAU_gTwzN@LbJD#2b6d(K4@A4vgT<)gQ-Mtr=7fFbvQ9ORYSWS^3>i{j
z%UDR6nYAz4B`wG&)`G{s_du>i(;@U-cdsw%GR&vGiEWYj%8ZDs((Y3n=US$jJg(|)
z5A@CCr4CCvgA*vDz`>FoOC~V3N?8wGkEJJVj4f>wQ@{*MN&qxy!-BRK?*Zo#kP{hd
zCv8QY;Cc}FD2;SB>I?}xp*4S+?+k~a>3mMk)*J5*jrDErB?z@8;L$l-rleghc6JQw
zRCl$nV=SNZiutXjFDKa|aO30EVLF5svwa<N^c&Fc`pDebn4?5)PQBaWj}&m!dr3@&
zE$3$#UOPb_t(TVL6tU^2_I^D=Q#(YjBVjLKocC~XU2*dSEEe<wr2PSL=`nW=%GBf}
z*|J-ucg+*Dyp19-&S|2*w>-lNOH28S`IJTZ9DA2bzCm;#qH8l#U1e;DVLYyS*GmnT
zHP^P2VW2(L`Ud5MSC*ENtBtm|1yDqvOR|JlG-U#eUn*V2N(7<D<60)Vt-d3qK>gK{
z!~)6Q8~ON<GDP-VKxM|lI#UINY^Sw+IdClV+1t?oel7(gS)pn@6KDN+jG6~IaP(bh
zP!<^-n?f0-*ZK~}9qAP!6u)ffsyz5JI;SG4$I?Bkk7xe<RnkJC38qAwK^LSXur;Dn
zpO5sin^tvDT}oY&glJAa4v@5Hb+hvAko#bq%rm*w<L1yter>M|91(ZCpXZN3--dP0
zV{Sz*F0Om%lGf6qXt#llp9=>lathEB+_DKsNBuXap7Ne}B%s}uPdK{Aq9CKZ)Ty*h
zw^0=O?!Exkso5PoHGy6`-XIfkA)p%R6kSgK6T~ccLeSb+PBNq~OP?3v1s=01M!QyT
zzM-`7@()gV_Ev<cXYuw>(JRRDIGIQlJ-4l#=?Q`O9t+n1cP*vj%}5YbdAXU`x45v2
zE2q>czQ=*DRE#qYINsieL85EPX}rC{TWh2Kqe2mmF<p~iI~E7wc4r+nR_ofai>CI(
zjp5xNh(mVhM^QN3SNyaiUQeh!Q+Jr~dG5J@?j3qmjIHm$^1Gs$S+b_EN3%!bnEV0K
zJNA}9x8RX+adF(D0B;W<kXwW#?wa_+m&}*r!>?#H^7b^`vNAG+{z0yE$vR|7Sg_U0
z*B~+=Ht;(#R<Ra?0niw<icg~l*rk~~8&#U7C|eq<_C{{GiMz(Xk!%L%&_tXaG;5mn
zWC~eBr8-!|?%^_aRi+evt+dvQ$CZDO)-u$7qBmWSXI=}Av!q~XxiRFRU9Euyc+bSI
zwr&oAes2QG_zBC%q8dAwQB=!buAZ|}7tfuE&^byJGd)s{GFODv<JW80N31IIF3%5@
z8tH4*!30U49xZ!ixtxgzcj%nFcYe$*<w#?pf+O~<6JM5kb89I%wY2i);RB=VQJPcJ
z%<bCKa#C(0wG**~TjK%DCo490h+{r#Q;Ca};)dpOk3qUVx^02q2U#?+gFLQ)l=5~$
z0VFX~c4=4DGrsgGUx!|2{%18e+U*kyBU0SNK`d+6QEfT1A5I|T%opEGxo%r~qRcL)
z<+{XmtLa<awtr@o^UUAgPz`B@ExspP{%nGNi%2sF^Qj$UFS)1*ru)*HCNjl6Gy#pd
zX+a06pv8Xbo>xfdORX0`5r%KAmI`ht3773=T{pnOBx`nY6$TBoWBjJAN$QfLj1;<H
zhI06it+vX8p7E~Gm9zm##kxtAezn>=rXx;!OvhGXDa{Rc(H~>r)Acfhz^S<+n0gto
z4sdB@)s(0hI9S1PqGd{4M6?;=81tRQ>Np@JCEZ{?og14gYt=%8tJZzS92XH;*Q~7h
z-0Dff#3YKYRB%S4pZk0tw@N=zu^DqM_5>g%$@2<MXEScaTC}T{?P|1p(lj<WYcaox
zA{(fzNm`kd9<SVj#h<f0k8+h%n3_{<%@jW|#w#_$>h)w)*|xaR5>Ph8(H%KuvU*-I
zyxeAE5R4-rt>HRExN5talOB^C-dE$rGcPRr{FXOeBL^@*dV`TULq1mWiVpEBp5e_Q
zKSW%oL()rRd&XYOjYH3-GHUy5qSJP9<qhteXa^!1)(3jx>C$J`M(B*<!jZZ9{JZE^
zCZ|1_4LytVh4w{JR=`xvA<pVDsvYnl+$UQyvw39w41VmJ_442tX5!q)yphS*FSCXF
zqJMglg5SC~%G-EXtjLc}N6T=Alc{0wsWU?P*4669e;6onM+h0j0J*>qY0wAyvleCg
z%=?oOuTg5r$po)Fr>e-0w$zTB)HNuW(SvUDKf{;4i;L^ULvBUa6hOyFId{N##uujM
zRv#Rs!LXvxowQ)kGM-XKsCv<#flV$5y2UatH1MZ+gI73Ty7cBFvZzY*%dmPE@tk79
zB!1wNH~k)n?D6K*RILE&=u~H>BA{GxmX{GT-ph061YDKUJvJIN-(P#TsV?C9vY$t2
zYdazNYRURr*lh9K!OZ3>+<?74cxX&J0blLL_hdy{r_1MAf{Iag9NMRx90x=YvI$Sr
z*qYx0_6)~8Tu<T0Y|a>Y375HL^HI{T5loBCA!?4HA15a+Iq#Jd#x(CCj;`M6tFd{5
za3S%VjuC|%yk-riG~CK@`j*1s-wWvJZE6wQDY=ZA0oxIrYhcboNEqbaag1+R^g_oP
z7LGd&=wsut#6J&9l)70u_@OJa50IY0hUm9N2Y;w_&x+zt7{KP*gd%EjY5JnatDq_O
z#q#u*Ax8h!O&=gjt6Q$wLZ)dM`6ZiUa-YSE^9bXpT_{?dK~<%SUsE#8Ki5rA*eXpA
zIBy)n2)qy*Uqs{0w6rtR8V+TK8yaGdX&1C~wFNw9RM@f?!kU#i;2|$51*qqIX~!l5
z{-}2CK>l#dUi`9#c+K~)BC^J4yQ}Q8w`~|yoG)Euj{^UX&24(Ub-DB%0U7iDRXdbI
zuLqh8EUU7+XoE8nNLW{}V6Ku2ouBnV<t@wdLZUYE%`ldoPDWkpT3thggtHenOE|3J
z(|j@VbWP4j0Q1q)T(y%-j%MGxhsGaxy>cv`!&*1N#!O(<Kg5(v`&JaySLn8>HAtOa
z_S*<Wwtve|yB(w?fh2dNvgX>VQC#ApYmn%7(b|LOgY{-tD>%EJY0C&w@M1ZFlxR%@
zo%0I2xw!t~;c@LbJqRQwgifeNBWVOGZ|2@iR+Q0=5M>z2TOy5-2$IK5141=_*v1LJ
zQrJU+Ip8m2EjkRe3hZ1vD`cL>H?_4XHL2k?tgD|7l-(e?%UUOzi!UovYgdp)71ek1
zyo34fD{e#$GBt+HDeR%{b^+p`HxUpn?$f%_PMG0SqfkpnZ?fz5Hx#;<;e|ECYZbtC
zlOHlqTtVz=_<0rr>Ak@^D`4x(l~azzGBzS1I0!VdQJqDu*e0K2k;VLQt)Y8{NqeZ8
zc;L*4(JS|=HP2AwZ+iqTY4S?YCxB3-gwVJK3CVrm?R{*DoP3Vp{@wpQ<Nc3Yj$kaa
z+sLHkd?_J6b$kxb(9dsz{P@7&r`UR%P1<HWL#A}-n>f__cG7LoZNYCVMR>cT+(w;P
zk>LsA2;Ycu(R3b(H?wzt1!}y!J?T2AJ3w5TWEWxUlBic!%o&WCT$eAQl-Q!R7Zkfm
z*lrLQSLMZHk;ogFHY7jtE1``4DCAN?ix=)y)R3?KZn?N!%vXwDrQaY%n)q2FiQ-O+
zwu|*2J+UQe>N0$XNt+qP)YHmK8q2B;V}tQjNmnD_b=)D_?_`yV<YT6s+{BDqf;L{%
zuxD;XWD*=<Rrlw$iaT$DC>`-hWpV;z-CTh!&jiwqe%K_J=k69lT^stUMuJF==H|Jl
z30^x=gL<I|Gh8#~8L>Oe=beE<tZoo`ocPmqj-9Atg0vcoeLErer*m<M`n@fmgDo(h
z=ApNPHBB#_Rt!y?Tl@FAJOsoBN=or6Zlk<b3E)c>-Z3=)5^|z|_7&`-mRF$_Fh)d^
zFD<t%8PzZ`@`>m&3hI$s!p?9$N*%(r2Vh{5p*2ZQoBC^#^OGH;tH~POlVmfEMhm9K
z#I={Qci!uB1R~!rRn$%tjvCt&zpNElV4TVhSY4*ND%$iT6SUD{Tf<;Mi{r78$wOil
zGWEoOy4nzdU0jLo4zU*XRRsm)`ZXu6pfaqgC9@9je%9y5lDm<v(0Em&Y=z8v_o+qk
z>S1?^Bb3Tt?3CL&-uAPeT(Ri_b6{>r@OF!CjTyXDwZvLM?+X&gj=)k5o^!|yY&LLn
zai~A~I|{oRdqPpCKrmf7+yq)s`~aQmQiFgMy{LJfZbCA!NJlk@Ziv)EinO;r0U$%>
zDa*lI!-FmqU?Z%?(7Fq-n@6T1fj5bY-Z(P0rAxffeKEhzzaAElC;CW#L-dOVNaEux
zt5}eqhSskm9{1f@QOi;JK~HLu1aN(AJzSKAuX%$DLBr%&SH8PzHZWINn?J>|+PRG7
zXDalbKe*Ji5<oqld%<9EgW7>k$G$cbjF!WQt;z8dq9?*4tb07J7vEHj`M%&ly1p#h
zp+4!umu_`b?{KukJ7+4xX|pYM2qP=;h6N5w@vd4CA(g~*CapPbNZ_Zs2TSow|Lgkn
zWfhjQhus|$b-}G?P5Z+i!XA1O4HKlcnn%VIt5^BqQx+6RVw$4YHlfaj$mXtb=k?cH
zkHii^Cd~<(-?c#alf8Ro`GjUEOIolhq^#~<jUnT|AYB2CL#t-Z?l+>bwW#wDz!j?5
zQZe`6>oi3*yDx3N0vptff)&o7@Y;HD6+y3S=fmGH{DWGkVM)6dyn=Xp+`jqO8-3nv
z;Y8V}riHKG^57ss5otX+>R3^nyF=#St*-o%m3kn9OL@H_?sv67kKjH}($GBrd}JG$
zfH69!m!%gQ)<)R+tk$8uH9QiLmhFTexV6Q<tWK`=-m7O-!Od)a+WcuM4k7z`<({TC
zM@n3tx7*}h>jB_&1wOv?3$Fql0dwJy*g3SI!XJEgD<+ixu|9SM8B2>+C!(S$Cj7$y
zwgf~{MYe%WO1bx&49jeYCwN?&<q~9`A4(Lju#I+fh+0BlL!gv7WkWuSwn{X*b=FwI
z{y>QM!e;)^Dn3+o%JP!SI-nih(qPSMF9~w0X1GA83U={&MY8rD17O!C3zAnvrA-=N
zfIxz0=eY4D2d}sZm$Yicu(!{(drJ$*S=ad)7~t8HtAcC6sesHRW+@M}11l;ICPSuz
zZ}-*4<~f^%i#-Rad&g`0*lOOb_64%Kpag>|Z(>O6J~Q5)3?wbJLO(t@o}9R2q%Er3
zREEO@#Yq;<eQ)Jff3|=fEC}<=3zioY9EX468&l{N36oX-E2hmu_=r+vawE!ws8qh@
z^gK-(Tv62}-*3FY<mK0?B?c0?N2X->JeJCEK++4yh_G+%PAPqDNegLU2VDe$dQ<W0
zl|h<k=A=PraP3AL&AGn??mDfJ%qXx$f7&PjCu51}-aM|Nm&#6N2T|-r8H(Tt%Ppa=
zudXH|J?c9m3<?Msc}Ahn`o*lK;o@t|utFHV{O3qwTDOL;GFOiGDd2D+{>)~I#TsUA
z{4I0V`+;`h{Sw31-6ktiWB6Bh$IB$Y>jv>2Hpq%{;k=0Q7`+{_c|=-!cqzTuP6-Me
zN34mgAU&N>xm%6c!q0BkMiddvObv(BzMhIz;T^QEz<`+v4nkglsolg!d?ttYR-HZ^
z_d5Z;YyU}i;297Gb%NioUxu<9kL%4AUvnjTRMX&t4K4WUXUT8-u0fAgY>d0qGVjTc
z;cH|x@M}pXX+iq2o6q3lR+QPH1nppG_yRhYa~7U#PWPxzEs=eGHzyu%7KICa?Sd$_
zAOxs?96)ex^kBGa<aGP1hH?3R;<H-h$(!KDk)PBW*Tr>IcMXml_NKYBDbcMz00|39
znR0eeT6bQ>qR7L>GZykmn>8?o*aLl{Zmh*2^_9x9W|rLyCDmGvF<%FM@jm-~5XRzO
zb=ql(VC{S8m<}GeiZ-&CD)o9Lq{XfSIx+d)1Zj6#cV4rz=)6Kff%RX_Bz%rENQ-Ie
z&<UIr=wu)&QhjqXEZ<I~V7qpSPpE7n<P8`T(`h@~u^0gFK8kwZC-RSR?z+L-bIB@Q
z#Z-bQlt@hKz4vXXaF&BnP%A$n)fc(rxuc%1<vLcx-xk`uoFO4tyQfkc<r*c*r^ktU
zOeUzb$Sr4cufLMjO%~R)ZFWaj^$l0`?N9U}w*O&~VC`8U8C}aVkHS8T|GGC+uG)(V
z`wS_sQi#WxOa=7<GHm)7Q0}66ldspa^5eQWbOQS4AbDt201|@MZ5`IYV?N3I6QWsZ
zZXeqMzJ#HaQ>NS;UbpnU?|RGV-7C{{b9NShru%0^_q?r9wz3!S#mP*zWE;(5u9!;g
z5!(bu+<xaQE}DbEj$AMTO39F)NqNQn<7U;e1^!FG!F0;?Q+5e40`V9O&iT$80DF=Z
zuND=^cFa0hj`+ga2x?GGc;YoU2c;B6dri)spgYmb@HzVj?G4WVZ%SiOt#GC$UO92z
z90YqS0~1Yb8SI>N<sd4G%1yozjj_N|736OausU8p;{LM0aLZO_S>y1?N7R~Wv`?bJ
z1j!}G$bJ&PE?g4TwDu!dI6`)uBBL9doqyTlSzqFi0C921EwZGDtDyGP9hItD&!E_k
z7lmVTC^D{2u@(}Ynm(gj6-n_eQpDoU@y>{H!*JO^-P%t-(<sx!-%LS*h{7j>owJJ}
zkCC$0e;7AUWop~8TE^Sl=S^eY6e*OipIQ!Q#8fLZs{k4q&m9mF!YE&(-<C@oOg(#L
zaL-=vNAGjNsLD^1kh|!FSBEk#GBx*WQPua{4u)|yrK3u`aqVs-l`-nidbAMFJVE2d
z^O3UX?HKdp7$@cvuPH`FwT<zL-4UWGy!v5G!3k?$;U)ma)F<bO7aRkfY5bTx)XOGE
z9MhJ*#Q(oqDP`{r77W2C6w7t*yHr+CBI&I>2URpSRvQIe9z<Kh@Fh7$=DAFdnhnK#
z1N?+IGgQ?yS<OEwT~9Kqyld`#BP~cf*1||b9Qtm7AJEN01uNe_YDByoY=)nS7$nk-
zUs@O)gWd=&X>l8f&=OQ#c=)W;=&f|P98}i2knoxMmm45n0J+)Nz9DznknKKwys&&A
z{>b|+-O;YL`jUVf)a9j)!S-_<mQI5KxX!j)j8FuB6x{PM=N<@|8&ql)1X*m0o0VmD
zMkba+I=KQd0AsKny9phSBcIfJCyM1c>)yAMUH&*3;c>6pH<bM{&@xtrFOw>CeoPsb
z5><1*Tr_aQl4NLfw8H>p;_4A=QPGGBXz2|wbQpmYFT&H_tn7!SVrXrzwgoFCMYJY8
zlpeD461->gp>6Iin%ZO_XHw&Yxt-*jRbP%$)VuyKFr%2ehRrD_6vWhewd1<M@-_3I
zMxl)2ma~?|_x!f*zy+<=VJG;>^zV_x>g1(zAgL*yBai*mDu|bZ#>855XHY1cFJTYO
zwnJ(D2{1?F=3F@M2Ri@@`@FQU-S1~(Kh6EXwND*9++lG(n*Ajpc?e=FLEt$gq&4-K
z(0$Pg)Fbr1M6xgl(jKKOWFxYs1@*a6*>~8x<RwMC=5C$X@_NxlFcvi^o**}VdEgV`
z;8o<b$zw>*x-N*C@oXgqh8aSSYn4QmuKBmP+3oHhwKTV9`;FgJ#mAGld;4FH!cIm+
zzH_Hazg`(Q%_jtn*QE5s5IH7<kdJb@@xkNOLm2Y4vcd70HnNPAg66l5wb<BP8*&9J
z1K!Rmq2RRfJp8Hhn8X5q&ma0q0P1wv5e6=g8EIhs%+^rLwblBWJ7EPmH|umebKh?$
z?QQGx4Ci9rp4^$Iyuuc|-coh$w|&YUjrTBiN%6tIZWaJai!{7y45bw-tSf;ekixP`
z&?(yJ+6{xgHnmENf>&ip@lMoguZrtwwH2+_VeF-4(wnje=3NL`-Cl_81>HekJNHr0
zNUAZ!H)z)b-lv&|EVL-veVVm~(|5@Yyo-n*)=o}>hdL7AA4cJ1tL0uIcMTkCYt~z+
zKnS=&@H!B#=^+r?EMu+vc9BHOcoAH_ppw(57A%xldStO1tXVT=*zFgn&nC9P4sVU?
zAPu{wYCrWU(+WU>#(a>rHm3hgP|fZ<3e$X}rT^ldEiaRpwb|!s0665prDg>J^|*CJ
zz5bAT)?JnVXtKG3m8>u(?aUdg3wNty+S?I_;(5PfBg$p@C|0l((T1O1%R-_qSrqYk
zW5v4G?Ys5o!y_&_;(&KkO}2q4)7y}yV@Z!nyg)Wy_BBm~rHqHXD^Vq0;+hcgwb`JT
zob8BQkcb+z`Ns*tf7uOVf%LgD__7SCaDpVm=aOY3Y^F864nVx>vQh^QhfO*-^nS6C
zeJ*F>r-R&Q5@={#N?5qU{|Y|a6=7<A{*YT3+s}AJZKx*8h1plsT`ia{U;ACeM5+5I
z|G-90dQz;m2c*3X%GsSYqC|+tc<1#OcYJ9eTHtFgXRb1M@#i!uYVWfP_Z6Hf6hr<{
zTUItB7%Y20g=qEg!xrcFxn>-NB++f4aUoVLkN4+1F#_pY9RV-Np3qeh!wsS3S_q*{
zwcMjPs65ajl)5}~e8}GT){y($b#Q^XdCcYR@bNJNJqpXU1k6_3JLNRJ`H_?;7$wI3
z)W*ZV*CO5KDv|-<&~D{@e$8n?(>Lb8Wq9Q%;InZ_6&Cb>!XI@~x+U@eCJ*D8_ic|l
zzdqj^aeZ9CouA($EqV(6me8|XR4ub!N-BL4S^E$i^Bot11nmAGb__nz?6{#RbnGHP
zLSm~;u68V%3B{5wdEH4Xt4fo%aA|bcgX+D6kcN%R7sSwdDFG`MLR}QH5g~bk23J4Y
zyf>XY;`YDO;w7Ns=;t=}Q-VH|?d$M(G8AdHOkf<aBpbKCErr5RT2b5Y0~%n!hYZLx
z0b;1hhIwW+U0Q&*)&db=sid|>R;lv%uv2@q7^D{~u$!I}p6VwaHMyWmPS8&9d^~$2
zAuN1SO1tR$(7syr(YC-=Kf7bkCMBfqLvK`TC*cTU(K(0F3M^)vZ0Wm_hi($cTOHlK
z0!caApY)%MTH6ccNFcEHcikB$-#{KASQ@c9;59OQDF(F2skSf6nU;+i0ET21F7I(V
zhNiVw$~I;&Q}|`s!pz|^2lhb_vV=A{+|C~^zpO}373hoNCmYnv^KO_~hRQ3iXB@JZ
zLRYqK_tqUglh(bM#$!KVtgXn1tIaUDgSz?7MB<JzjP4Cnm|MT^(D_CdDE~}Fyzx$R
z>7DnmwU54n^ah<_!Z_OvoXDFFsX)B-wy6)ZC_VZHiF_@2KKY(u*iKnxN+_aHypcb;
zhn@YHz+{}&jRnvBE0xd<inTC)(fGNV|E%%l%?EDk&{gK{#En5VXQ&z+Enzr)3#wdF
zSfpgN@(jspCSD`Bm3sIuFYZu5_}i70YvX@d<hG%cs(4MyF85usIRDk{MAJLTrzX##
z!5_mdH=3j>YS`oP5B-CbZnKvKh@IZ^kri$j!F1YoP*N%SS#q)OO(jRm_K5<kw;;Vk
zwfC#;jAKR@6m<okEHsy%*k%0JaQ)~?ge4+XZ8bvZLEms2_j-KY;dbWmP*`hT$#d6L
z;p!0n_+Xujj*NDmcZU*rA2H5qF^8{ovF9~oEY6P>v91S)I&{))0lW%;_hqY5nJPT<
z-@~n!>)*zVv`ph=*T~^l9BRR{3yR(aKoU<$L5$7M&1^Zd9%@kBKL-1kQ<lgdt+tQD
zvmXbgau)03f>KGs|Ckvab>1GSB+tj=qNb#fX7`Vz5*gjmu>*QtTW3HLH?m}3>zbeu
zQmch^*k(Q5G#7b=kZT*`%1hv7%d*U3D<igNB1y*R2v%JxxExdYqz3-MIfKIEYUpRW
zn~cVUD-LEDm~7bgKZV*vzC9s&!^lG1cd+DE^dnep$+=|>?*7}dG71P`#=D-ZOVoTn
zEBopLhsO3;W?^Fz!4(RQh@J4^nRXY7FHY_9*i>W6jQf$bCl66<v8o|hZj_~jSz0+J
z?Z5%6G;PL~W@CU_ajP{tB1+R209^?Iu^g7t6z3|=J9<hgt8s-%*G<&7jeg@@U`T!>
z?RBi#b#3`F$>U3K_2*#P`cHe!Bs<11$Dgwq!U;1ws*M{Ki@5l2RYN0NWu*9^A8p>e
zM1+e+c3*pk9;KcL{t-EJ?WJlr1TB#|^TSVo9+4}!5;D3WqX6Bg*7kNg(QN%L;mDzN
zYsW$4gThnqhzz!TH-sk~2%vW7NUUoJTDc)4wnX}&`7_nTQx%bWd}hNcU`1tC7Y=fq
zZz}R;J2*&1aSdJtToNvUdDp+*+H71P-Ajn4(JR#(2h=AJ&qpq4NQD#mo~l6{WNrAf
zm3gX(!>l;6JS@d#e)H-(_bD0<p`hSs2MF_X-et=#L;Hqtd~KHavg7TdsP#h~-#ziv
zZM@*t>|ur=Wuxsy)F<C%qa&k?>7edA^cnV%h<HtQU-$4>@J@gRfQ!0Xg9OJPTl+a~
zZrX$UwhzBv(dnwEn`*+(h06(@A3X?HXDR0wj>rCMneLQybNh4zP_L7i6@vWdj1`Pe
z7F#4UB1f;SVT=-b#^m)3tR&?wA-hp_u@>YGH;u-zj~<pA*JVm$NnJdb^}wad+2pZr
zf@v`B^$&q-ZSMpo#a~^>53eD-9@rB3nGJiny~Zfd<ew>n1Emyi>X(cY8yeAbT^8$t
z`*)>Fm7Q0RH?Oo;oSL7kJB+71dh{srueV?#DO>A0rifTdv2&uz@KN^aoF5?a#iJ~w
zw8%`}GmYmAPu4q&-;Sr3=AVgZYqcbKuX#p%st)j~0QSFmbDjn<qkPv@;sl>iZna}o
z8=89kc1yfD{q9OrUF1|m^VD_b9pY@*BhKa!;Or4V3?SA$JYqD26Dx?x0sD-qk!+2u
z4tpw9$9D?7)IC}6Ak;oV?x^)0IkYxB!>38dt!>NhCoi(*hSj23I&uFgj{m|20G3PB
zNC3=5ezkaCUafkEwWrJ@Q841WwRf4FV*RLl4El{9f?P#xlFPCMV1#)v3CVc{Evj`b
zw2!DHYk;(t^`%-p?0_l-9HtywkXt9imoBcu!9a7U;+E&>9m#{`?#48#Z0S`y>NQZ5
zn5QMJ!opm>eh>vS^Rws{k*QHw5^4=3#B<j$s_dGYXRynkkH0N5URhyklKi`FlbUhv
zR6V7j8q`~T$>Vw*d_V<jq$PzD=h)n9QQ=X;Qibwid0V8alLcbTMg>wus`@Ze+aEE+
zg&D#rw=Rq^d+-w}*@sM$ZyHfCC4{0isb;KYzfyYqWu>p*0V6kXGNJ_+9CuCvbBgfv
zd&<OX_q=L3k$p%a)Yfev?B9x4(^_iSgQ%@Uxo;n*<z8#=kmZZBT5E<8oo$^RoO9wc
zDRPjdHIqMv#@<g5Go55@RKUDFsbt(|ORM`PFD+S79c^fmTJlT>e5r6>q2pU^56$jO
z`8>C)vGs5IYFRR+n`N^pw3#$hZR+ksZT36Zk)MftF0{;5FNe=wWk6CH^eL=7!Nu4*
z8mFvzZ1;mAATh1MYRXR9cFp<UnZaCXEWBKt>p4|P0njpz;@#@*6a`wK-n6^g+z}qv
z!0c5Y()Wd03=(H5FMq%a$I&1KXkEOhHn9nwTda_jGWhVXYQ>m^y+GNS;x-@Rsi&w~
zwlCh5D3qDwXNR|d3ztSAIZBpm;rK2A;(2cqKa+;ibpW8hF|nXJR1;U*^0k<ITn_G}
zfy2-fT}`L&wc`-sldGeBmH|~a1o+Yi7wZ5R`vOQJu3#(o82F8t5znZ+WqR4kW#$!x
z8@^US{X@Qh+)UTj2~zw*y*rR)KpURAS?yL}DOuUmDU1y|qk{yH&Q@0^Px>52XKByK
z*}9Pqy!JA{e|3LXe>n=&3mm80mwiUsYkh~;h?DbO62}+_#0R(FOwGdB85-8c<zcd2
z8@&HyD0ve=ZFza?D#+-Mj${BUZKFvN$7(fb;DSAa+auSeJ&RGjrVbWb>*D62dd5#Z
zg5Fnkt6^J9XVQ4f$8hZe+76hlO*5lufGWb#Fta%SP_NzH1pFkv0JolP|8C#N=Y)OL
zZ(FSq@YyU`>%d&YdCRP@i=f^mE9m3psjXBv);!(j-3iN>NO6#&R$*t!aCosCoZyxz
zM<lGNx3;MbbD@x~yge-x;bw~78YX^<kzWVy*HNO>@b<z?=(&KURgfG#v)dtaCiB|8
zb8*}O%wM(KCtiCslj*DCm^19~?4^ocfQ1pt&Uya4Mf>?ZxWm7dyAkk6N9`O(Z;lk*
z%`LLel5P8F92(KE112b#Oov>IzmxhhUaGCIGV(R-)O{i$K5?nK4>&5H*E6$x%MrgO
zK(sP7^eT4$_T5TWH;Z)+WOURGd|3FIoN>PvAC49|+NRVag~A-;nO`%}Zo&;JIy(gK
zvU%SB8P9DKcs~wyitdST)G(+d+FnLZS<$N%{5f#~Vs|>~hR6*`2@q*z(0jFrpNifW
z<pp*rjDC_*Ap6%tIAQL$1;QYd`csoADQsX!h^Ab$%-ds$0-1P>_twpou-*9t$>;JL
z;}>j*ZTYPlH1r{wgS7YZjscDO5d0K(=v-*uop|~=PFMm$-Ex*09sgNT{T~?XXc;A}
z1{P3Ckiin+2~v>A*0$)vk)r@?Vm%*L1$)_FhJu9D-2G@eO->eww~*p3Q9yPiPnFPe
z>0|osr}<5Wrb1--HpP8knVFX_{Ma7%OrCbWSy7Y=;x^81Y+pA6Awz446s86qT_h`G
zHn>@kz-PO>nGJKw!&~uZ)#5E~s+hrRwC{TwW8%Yvj&M%mC*ftKio8Cl0C{-vr*cUe
zBx@X?egzR<_pQw)@i(V!h-K=A`xf_SGa|bIJf>r4Gn-a(#k)2UV<MhrbzG-{Ml^wX
zxHzby2tTB3#l0N1C<z+FmUW$1m`Kn`Da8A==mISYIuSEO#>x%!_Z*FjfP1}KuRma4
zoM>Y2bL#M%zpCCKm#(z5>BtknS1+^-(O0e!CXLRI-oICk3tLSF-O1tO?RjOu<D%JO
zMbB|E09M~H5DuibJ<VApcjfC4d^U!sgYi*fs1~@^zjV-D)G2Ym<b57N27UK5G~Wh1
zg;i)%jBW}yj7Ur&eL9=W>z6rcvaBWU@vb%R)HiXNCV7*$Mkfs=xNA|qXSN!(%a)=O
zBfo@~OD$xtge8+t2oPVuiye`?&eEmy<NB_~8iPV4A=M~2>{_Pw6Y+0^o=l_7ZQ*y<
zX7mM6+HAC287$#=IYvRssNIhrUSbDjS@0P0FMgSq0xeG-3WGSlyWiZb=QDl(RN#2>
zcWe9F3ae%j*H@P&s(6Q`?3Y>mnycoU&tUkfqTB$wg@CpYV!pn=Ey9YrhQ>c!TE8x6
zy3lqs>d->-cOMX9cSD_H@zh=^HiWo-8-yIdc}$xxDVmTg=_Wk)`IBg>s0GypRZk*M
z^SQun8nt6l01`)IW!Qcwik9O#YXns24u{2TwQ`|k%Lp1fgdKD5Mw%eyYo4>6LEBcn
z)L+1%RAZvRsrHvHy9>*xzE`octUd=A$tmZLZvazRElve(Q-?S4q@!g3fo1PeZAtvt
zzwEz~zmbtK^!9=_(gS8?BV%;+8Zs2&TAO^y7Vd6@ODZFJM!$nyD~j(Er*Bu>?A~ws
zg9n5(Rpz$jsU%xmo-Bg|Ck|X#sveuYltEZ7qP-5f+UA_x4?mC<7R+cGTMq^een8ke
zY`D>&WrkQDkc5Xx<si?oEOWBawE8lWb3rAHT)8zAf-tBZ>kyS5<Cq$}nmQll<Ix;w
z6M2ti@jWc!+vb`iNXt6$0r5E~C;WY^_KRh(`f%HqwTARtJRB$RBls?m;F*nZTbFm2
z%ct&~Kfhbi0*HKv@j!ZT(2vs^n15;(h(Jp#2ZHr))BW*#0qnMg(^U2f1wplrYcvyY
zmR6Q+ABPkHHG+3XR#;lh6wY>)_^k9Xy$Bn}G3bxZzr_V`p}1UI&hEUD=*;f9$r=uu
ziG|7@u=fW_$RGeNQ;wonpNw$jRL(K)(C6};ackp*)rS&1^D9cXn*oBYzUgTBZV7s3
zTP5wBM6mxoG-x}WJ-U9Fvz`+zJZSss!rvGrK+WJv-wcn4IQ)6eze7dH7xxzeFRtc0
z*_0*+M5Pyp6?UohEo_V~d)2s<crjf*V9YK$QuX44!|zo~YlNKkKCgYxUTip9>)aTs
zJI*VPtxRufs}%im$*Y?+5=B~9#M5-re}o4<_8)jM{6ZK1KXc^zYXXp1VTH4m9Dr_|
zE}@)ZX{i_AMN@%1^ZG?tJK&*`72F6fywxhlFgD?Dz_kiq6;jzy>iY1vgy_=SI>&>e
zuOH?89CGg6+i~KN1?R0N|E*F1d{*F^?dKaiH~Pu)12zVL6)Qj4j$HDh%DrAO#=8J?
zBpTxI$Mzyq7XiU^a*rrFQM;f4G8{~ET1&}u_m$?O%(BTf8iLHJ3Wiev^e8WzadtZB
z?OR>Te~RwE264W4QzC0G5G!V;SU^=62S;POUWlf>UJ=2k>JZpDk6+d(^OQVuR^dxm
zR<74hkhgSD7pWb96YbE${6Kh)C#OsUISVZMK>2O$6<1D85*553`Ks&m-xastb2)Zt
zhm`la_tACwdA)paC6T@)ZRvXtoy-@Hk&+x02Xk(>EsX8gD*_ww_AC#_&20bTR0Uyr
zXMC;9fIMAad*wx2r7Tr`Op+aWhix50!Lr&p+xXa&3Ha-R2D!VsXN~?TS^b(C`j*Kq
zWz}IZR*NsG?~d6hKz5|`cD<khumGS`;)1t>ZE}?#-qZ#a%nIZb01Qv$OTp5bUV($7
zNC83b0v9MHgFS9Hs_6ydy-uIrr6+5Pp1uM5mSRKQ>{wVkKboWClDT{&zAKOQx7h|r
zY+PKec^OE@<%NCZAk0v;35GROQwAONB-O->*iK<TP3G`XYIbW`Eo#QVUu)l_c%s*B
z;1PR18(j$jK0u@&g7q$>6&h@i^OTE}^Jz@ekyk*;e0>j4Cb>qKXQX+xGMLg*eP<jz
zm+!nf(x_s1QdL(xI*n6Wnr3bEMI!GX#@Ei`;Cdj5hVbR=mB;FBvGEuYb8nKTEEx$b
zu&581n*fbr2>U6VIXq$rs4isxpf)1sx%s=8)Mopa*rCp^@A)x9JE(`0?M}KNMhWOw
z>C9ZEwM?jyZm|k+grFDA90uH#B&#+kh6_vt5BWy;`In1+WezMegPk2ZA?D}D%uJ5U
zzg~%)!bzvx72#O@=p=Ne2BnzXa=mdeBlv&icID=ASq(&Bm3OHmCfVH0$|GlH4(i=_
zhCC{#Lr6Mt_X5^kQ=DZ{okD(otxKPVu+x7+dEBCmiCk0-{sttEgwRDRmu{xnXh$Ry
zze%_If*zXeI3ZP-%H(8_1*17GbA*mdn@&?Jn{K&=#CHE(VINoT2&K<wy;wZ{H*T{d
zG_F9E#2qtu0Y3$5ClFeN_)8^GwNGkLgDoEdB644Tkm9<O(r-Fw0&P^^)ArPfc1dl?
zJXvweTr1)Q?M0W@!T*NR0Dl3v)BwT-><BaTUbWQ>_wkd?KmJBJf4`zW?bwVL^uN>4
z-wP%ukz4U3W&iJ<M#gIfJQG@9o%r>bTwHJc_|o@#82sj@!FYS7&|{~*J8SdIFKbu{
z{~EJ3^t{5c0r+o&6|V<IRR8A=JInJvzlP=da`RuqdaZ<EhXi8JDIN~*+wgM#)d$z?
zyH?#>5&wBwmG4cR0l+;E&q*^Pj{fe#{SO}3Yh%ZL3-3b5`el%jX$S0U*Jp2d^TA(z
zygl;IliDA!jN6nDAZjNiye~^ab+BK$-TA-!IsORM1tbpIC9?alY9V0)$A9&61^KV}
zhzhlD`{8$+TJiaqD1~pHa@x4~>!wbCKl)#peR)@#j`+Hns@G>cy0m^4AN^abF90Ur
zf1mhx#}oK)INP&K{vxM)eHUK)|Gve&^(_24_>vVgzGrDR7=I3&wL|p<b{haF(|=#U
z+p9t@+3VJndc0@Cr$p^#RnEjUKMN%Cmo4C`M*bti6NP^@Vcka7eL?Lrbq_R5#Wuzl
zH*4MC7i3j__3FOopNH#oPa<D{hlI)f7+<RI4!{lRBg(ipzyd_embUc^1^}yf=Tb`e
zXU4iOcza2qbXsJmqVv0_kwL04^lb8GHrXDZ1V5;a{p0r#0{;y0tAQ&$_#essZfQkd
z|3@=^H=%&o=`s4;uUz1Fivs1Y{n<S5uNC~e<$(ua^ZzUU-%YyeQVQoJ=r6Bz`A9t<
ziF*8^NatQPb%%;R%Q*e}7T{-pRfE~c6Tl!DaGo{;1<mH<KQAg<KpZQ?t=&i+RGq(X
ze2d)KLa}2Sb~v;^Z(Tc1;%hkRuV_u~zwi40zx;m*{68rHd&h>x#!L7)f1A@00M8o2
z4v_151Bb&|tP#=MXJ5}nA4sO;{e(21Vq5>V9$XJ}UV`kDh9eC}b-Kvw9ZbB@E&2wX
z{$~4UnMB4;&GF*9@mULleJ7o72k36Ai~UEey77~flkZ>4OYT4U_|I#d8^68O&L52)
z0Vd#Ao4#C{x(ZwL@PGd0EUe|lb6nnO;BWg*4*p|EbOD$4yX`m7uygyr+AM#Z{#PGb
z5QOGgc1p@lACGkrfOB(2S<8jwkzc#cAdXOzn7y9wAM#|iv*umj_v{B4oA^WoRvaHW
z?sJv^rEpNE*-9~#`igC8PgykS40{{55Cc(|M2StI&dnWfk?~3UL}~_4Vj3GG;_4;>
zzcQOksGJB5yAriS9bt4@Bl#m`M{aLt=Oh)5UAv|3pS&X=>u(k}tMjo~EMJVy9iaqq
z+$@V+B?_-!qlR*!G-G@@l6Fk>8>pXT4#5v+(VKP2ms)3s_wZ}XKv}y-eO#vxf2*vU
z(7gb#2xVESY?<i3p*5#!mC16kDc8%ZA8OQq$4aKb2*KFHhYtDtiCuP{n3zcJgH4@T
zHBdd2YW*HJyCXD|C<osoeXk|Y3mS=d6LILw=-}XB4Nm++{b{T15z4G)<1IP>3Hge}
zl<&+zm90*6+79^=_oKGGP<N3+nUG&qm!`TIj#v03kb0HajjI`J>+nmylL0fqiZD&w
z;ST(zGqBLc@Fte#y{*tzeD}Fe-X*fub@!coLqgCKhi8LxdZs=kuSc<+KCc@j@b>#A
zGIt*A_a!tyA~DBapE4$D)Y-p(=snX?^CMb6fk`Z!2PQv@UvF3ln<7k1G&_v@puUqs
zqPEo{<c4L=v)2!AHF7q?)`FeZFpc>eW?gZ!^r7(ZaN9q9XIW7feTIva2Gqn|C!Qff
zn#aMvsF+dz(QaVmSs*VIza@Fa=FOdq&oQN;OoI-V{$qwwkEVZpymYOpFQ6K#-(Fk~
zzOBk8cTim8f#u?sr!j>v5&Y7POcJ)0AHlzezlKQHJFJe^3%8Sue`2UQqpoAmztdQn
zGA<nI4;fkjGz7;v!zP37jYX=ig8n;YuH$A9gxv{21dyK!exfL-`qz)gbaYIu7XdS#
zCKJ_X$fT96iOG(FSqM@@#Mfw=5e4?4R_K_Gmm6)eZAGX!a{rQwC1TXK@r-AHP0;Vi
zZm0XVCpRP6pX#opx`fX>>(8v3J7xO;X}p{Pye{My#7vYg|I}2TYHCZZt#728L?lP7
zg2Ec{5uZvkut+wS@ShF75R-^0TNySMxS>4L@FSf`#c@z;tl2dd``&gWnE8oP{hfk0
zSgcW#SX1GTp7PDb#3YmQ2muuk_A&6K%xPwWQ%f*w11d8YHOKtPDx7#k(+>U}%k4a$
zGBDL+xw)_6yF*lrLNj-?s0k4?R8)#wiHUwi{!!d9Soj(InIS6VOJ-J2Nkqh?i*+zK
z36rat#cZki5iB^eDc4R;H+lPM#|nUKTQ84o(N#Ijr{u}vXzKS!6NziHEsnqfH|UKc
zRAEiME36kSOh>UloHQIcwW#~!h@b41&s6mZ(zfj6rvU5}=NKpKV<=u7lmnV`s`|7}
zecfRaOGh@i|22teqsD}=r=3KS^GvR?+CH<`<`Zjln!6_@GSp^<RowT89j>7|8O+|^
zwAzX~e8YSABSmVvg%V|#Ohcp<<J}vX8k+0OsVq8sjY;nshED|{mL1|mF|}CXB&<51
zTyPY#HmRD!QiKn|r^p{5Q72b#{-`C7vefXi!8_vmb+xAN;281o`cib(X`eJ0!|%pz
zgp~8}^Di`OLrMRzEX7%-$&DnY*{(VbUKP3TLpNaEo9kpvl~v>^asG)WmdRY(cl=CC
z?soO$SpOuxv8fj<NirdC`|nyq5Ga>ASg8e<%QHSV=|@wTl>y@hrqMF=A1%>o12KAm
z4iCxRe&Q*Iq0nKkS8u`v`ST?0`u}7c6Qrfi&U&+|Y{jrfyHtJ7N$p{>u<ewNP6O8V
z$k{RWGe32kzYHhN&U-_S_|B?I_?W$OY}8)wz(VbQIM~cDb+C=b5B+zBT!qqU-BB9(
z4`>;6HUcfIBEy(VTeC7cVIp%B6OxVKL`WmAD^Sw=%StR?n4#$U1>$|9DC<>)?5d~I
z25ts2ko&UOFAgvyjS4>MrUUQ<Urpl)g!m)9fidM52_2!62iH0x1ERN2g;`FXV9a4z
z68SckV)39FK(94q@)7x)V^N&OBo`Vcnp7QXrOB?S@R4922XF3?bAP&}3SRB-#i@E?
z<$R3Z8GhcpnFKduQ}7OHb(GM^M*JrxS@nX?-oNMaB^6J`Cx8s<t$k9p{Zxb!e<p5~
z!nv&sr_BfVcJP!hOBty`!{=TzV~i&%zIhJ6EZe{}X|x4aVK<G&wY8?|<Z%>9zsBNm
zGApu|^fP(<C7U69Z!G&5V-ZVvSs}|Sl@^w?*0QxtmcO@J(dRc5wFMF3$F4=|58Sd#
zoTMQtXe0pYSI?i=UTP6aJv^w|!DFe_;j*#)T{Gr$jBV<`{N&PNA**)cB<K4^pVZe5
zN>I!5Nh*F$eTYeTI=P{ogDB_rjcl~VyOXFNz0tTK?`_r9?26`vuUDJ*h~@m{yB2AS
z)$$g5%KSW>^sS|6i|r}to!ai#xb{<JFBv-Q(V`hW^-tIx<l(s#IcvmD%D8?=)tPY{
zkqN(KXXRdUgQtHspne<#w%KvWg<fQ=c=`a_D3z(z{{FR!;K${Y&_BG&F`e7pODe!_
z$UBP$^<xZ5{gC&%17+bRdcC8rvO%wYB6%hlUSXX4QBFLhtXBkEvJr9R3t4N3!Hljb
zYr(Z)BTU%S*K)N#FU`bqp8x6lT6=RVIOaK{dp&MxST`{C!4!MqHO8*Y$7~uKqv*4Y
zCu{jN$}}Y>AR)fe;<LbcyQ#_A2go17M}`=7Hc_~512;G26G)rhb~+wdgCJq*vV>n2
zwwdBhGQl3vJnQ?M6c){&ydZ9{nO#`_y>)%GPWErrxSJLalY|wN*5^0ifk4R&g*Ieg
z1TaZAQ*7BhDeC;>*|ct&@v6Mwfr_ND@p3}`5X<;y6g6oKa@?Rf+W?{R6R}Rl$J`5Q
z^e4IHqoY3VbJc`b=y(JejwuaU;zp@V=xru#Et3#Un#xs)xn<>MhIaMW@Kh<WHrG^}
ztgvD!hdYj&M#f-!hV^61Ue*W9V8$4Q6X0KF-mg*n{}A`xZ%uAX+gQ-8ps0Wm5!iN+
zqJV(Xf}()*B2C&xq!W-%=x(J50wTRP=_T|KT2xA;hME8gM1c@mKnM^*NcbN2Is5G6
zdH;ZqUjo+^o@dsqS+nlBXJ)OWxVbjH9FpN06>N2Oy7=(^Xde|~#SGYH5Q3Vh5lik~
zdT$8GI58GAoIMhDX1maHmo^UZH^9hKTPvEiXf?C(-#PZ2+kL%THWmZ7P%V<d@<{TB
zdd5G?V|kfO4l*+jOjimoP)taaPB)^3jgz0#5|={TVAHp#)UB;m0m6Dv_S0!qDYt_^
zo7$+<)@A8noQ}a%E38?o>%vv_IGF1k-O~==?M8<!OONj}Z+DUvupjHAi!T{J^<utT
z$MzWp#=#v`6fcTbE_%n{3@Os~ow!}kkcYs8+28`74sGLtEvIh?b6XhjQ|FTCo=eUl
zaaH1q-S@|-FV|y)<M46l@9Eng4L|BY`+Xlnm^WSLXdd1f8}l9SSfs9|w5>bOVOjq(
zu?}it6}-Hl+Tic-oDY`|cx*-58Pr+DwgGGz?qd%&Hna_~#{^~^-No>arL>cZy4K~c
z6*7cx0j}4lUAXz6YHe+;vP=Me1%$a2{3xW&aD|p!oD|FEpJGHk+gJR~Z@WUxkvg2{
zGfr!Xb+6JEZ_#7Q^C!TvX7^v!J7$&>4T|=S;=xXo<)#=C+c>0llMLz#+FS9^B{DZD
zZ>>_ydavfhy@d}LXI-s1!Xf{X4Y+g)Jh;B?le3wb%;Bu+eCkJ&K%reTq$gsjubfJK
z(;L)|Y}vuY%C<l0qAe>fFP(CZFRp|J2W@-p{Ab)sH)vb&Wq{4w=AHc@6tu<zFUO3I
zQsxsO;bpja0~KC-EwPjU_i-D{yO)Ei1vTZg#`4*n@h;f|y5a_-eL%I09!X;eyn}CS
zp~!!AGcb0nit8)i3cxishZFluw~92~*XXo_CJT`i34dIjDlmWa#YE`{BY00QeIMC`
zHoyB2>qb|>cf$kBdY$)_rdW^7><9%D;FJcSmo+6ln)RBM+z7(NJBOcima!W=2%Bl=
zZ`g+Lt1Zoy&6M%{0|`Sld#}2#KM{IVSmlWSZJpxo9zshL%CTu{YeOWGW6NFVLXZ-L
zT18Jm6Ppdrcd=Q=WUX3GZ-Vpb99m1TmS!!Nce9K5rmWqXpnAeVS|GV(r5?Y2|K3``
zS>j>aeR)L?Jeuf?l{GpPTwF3R>7L=g;vsW|=<cTJNU7a*R{+gBTaEVVHwJ}~b`r^R
z`@+OVTlbKBHjXmfQvSP5^6f!v)rmH-nP8C_^q+z1?vtK~9B|9OLXW>@Bs84pfdCb8
z<ulL0`>qIJf@0|VPIxg44j1sSb-EQwnbDmFmW=b*0wypuyD4wirlQia{9cYkVI3R_
z-L^5>DyqoHfL-*yg-C1&M%_AZ?3b}p0O22VAU0V@xz)7+?T|7XbB_^Pqsk~;9EwU)
z5DF5F!x<WENLZO&V6+gVikmIkcck`>C0A5aqSrROW`yT_X5k0YPASO=<?!LeiD29*
zA+i@P#qb?gZFloG3P*9h2H%r4(&z)dmWTy(4zloIU(bu)Hgh03OCC+U%^n#Swi&%q
zB=e64=^h6L<fksayb6GY8B&D)p8Pde5JeiSJ?V-n8Mu0~3~{UoQgw3;O$m((beG#e
z`D?U>_*acXHicG%0x}q7xTh;iOOwK`>of?yihq4>lBPScuc2<$+k{dy;%DWhQoaKN
zMjkZz@vf8P#v#3r7n=wIPXUvI6J*v#t;#t%$(?Rs30Gd#uI*)DcWe~}+(U)RfiXNG
z5aPZY>;{M8wY!`Cu!aU^sK}ylMLiL0a(A5+)+oh4>t>(g`IU~3COu#-tUsUn+W?h{
zf%25%PS#SuPZZ4gn9e(6lPzEX%<>uk8NF}$_&&l}aKb4#qel=|e&?+fCg8P82}l^V
zO*t?pQ8l{T;Dpshc=-={Y~0kX<3>Q^2>ec=)-qSmEH&u1D-8RwIVL-Y!1O{gkPQ0+
zz#RCkf<hicn<)aF4Cr|j1eKtZqrHc6Z#g;sV4oR+!tHMOLA>RVWG#1I^u|x0f>N7R
zoI_Rqz`+ONB-{Ml>W!KX7$F7Hz9K-hH@7UEU;2sCz&c}vBA3w^GOFbDDrSoSw!Hh6
zt}%s*HJ~hwdLF1;ZJpa;k&R^5Y;vTp1)2z2QW&U%J3?5$K@3=eGcEaV;1FumQ>+Pe
zd~`ytw#J#|E4szvI$H<$H-h~J0arNKlvTtUi+c->(=looRb-fi4VBTYvPZDkvSz<G
zf$Bg^*g|(7hXm3OqHumyTRvuna84wH3;>m@WmA{FBVc>Mg!R#Kdg(&kL01zPNge?3
zOJejmyekMg!vz=BImM`Js$3_b=Ng0Qs&hW=A#QJRwgCPb0Y8N?XQ??C*Od*|Vxv)Q
z4bCqJ<&JgbxWwCsBdN;wE1NAuay-OUy!R(AopIw72JhUSg(y&$+rFpC8387KFwq2N
zG5AU-iI_)a2S2Y^bgGz}<q-bcsopR{+_}J!=<fh97Pw9bulLu>5v%+mpcx!ajtd?Q
z&G7BSK~K83Z2C8aL{Cj;_YwUoTQRVMpnwB9X^y!sjxhotuBe7%9RQCWBO*a@XWGPE
zjc1TeSswBB0&Wtj1qcRkvQaTCseE`Qz;JH`h@+$gVw?;E!C>RIqN-z`h+mp}s49?y
zBqO}*Be&l4iDjgW<}0R1NM#O&w`hdvsov6Z*V`9gObB+x8==L4pk%$HVVps8-!;G&
zr4tT5_z9GG=;3GF=#SJ})}4@2+x0+AUN*&G>fL=q-!Q^KykABc1pN+I+*54sHSDlI
z(I&XY@o6kIK?89`&}Pt{8Srm)e0n<a8*mv}`N5f=F0C_&A8?|y_Dn7}EI=Fd0?szb
zZCjoTj*iglH7>?i;V;~_U4jS6xRU4_`^(bYKar)HW5zR<{)k!k{oR-a{uut)y?_w)
z7WJy02vVR<Z;)*Gdik1g^vs<sFU%|ND9a4xIVS!Rl7T596J}LR9gaeK=R$;CL4DY&
zHjAFO?q(rbR!JjOc_oViRO5lTjRzajM7$#A>Ec<wQ9`i`L2Ye3?1Xy+OOaAL^LRkV
z21opdBv{qoR{cjUkwRncwm6|>fHl6mlk=9|FLh@C2B+KBPrtrs%?B)CMf%u{;0ke%
zn3J7CLFU{IEzW;Y4CIeM^FBm~ak`WIQC^}oKA&KljZ}d%MApzZ?hOr4X$kJCro@gh
zqD*HHwr5(_$Q_)27VRDoVp%@xW9lbxyl4vnI7)->3Nux$kY%rxV1gQ6ItpXSRPQBA
z75pft1<Q-u^4dD$r3V1-V3}SEDWE;Z3xo>Y`N^ITor^O{VyiAaBi(P=m$a;4oZaB4
z>X=3SZMT0&%u{H<fn{UBeyQ3;uKh~%0KcVQa?8R3*k*t-EZdRYH|J9W@F>`M6#v^Y
z2d5>@(W3nw6#lZ6+{wK^n=BrS+4e%v?SKP9x}gGly@rVLxo2_2peFd9BA9u=OEvsX
z^$y7BGuVsvM5uIk<8%@&!2|+~h`MmyBKBkjfr1KBZw-2^C?a*y%~umI#zuf~6m8LJ
zn=Ja{7zIIA>zb20OYUaF4z8!TG#z34-~ivl0*|2w9D%j>l~#fXFMQ|pHk1s$4j%R5
z{yM1nn<37dl*XbDp-ny`)38&@HqPbT6455k8|{^T1P{TJEo@gXxF}VH4HYg9rK$s}
zT;1!wgeN|pXV`;hU|3Q^&zdA!Ub5w~HIgn*vFbBB6`Dgi#a{raN}_*w+t^T0jHBni
zem9jZwq7?jG$9sGKN4!G1P%>J3HhCp_5?X`IteE{qtCkUnC^R|@q4SF+uyndKC8ti
z8ZG+5YQAt;V&TGX&XkRj?jT~`vGs4>OV7yQNX9k1wx<I3I!W(v?d{#_r6qEK&Jw#7
zvu#DzB^{FM;!s!N0^s1l5Jc8H^l_plKWJLJ2xs7jxdp}yx`Qmv-$$BL<4SXoFOfid
zw>G?Z!}At=UFhx%-y696{q;V=I@HT~lw9Qzz#{+;Y%O%bo>c^S<ZPasBj+_n^x$qi
zHcBtFx!1dCX%)Ci(v4kM@ubI%GCDpg9!W93&Go~x363vP@Y<*MpoZQiwhFK;KG;$|
zc;jGpEjXOHaVrIPK3`zBBaG_E4>i;NR&4WMBL{xX<=}**h<>xJD()r!sf)uAA(XQo
zX1{)Caqi6-w_{w9R;hQa9~R<`y0PFhGu~<TX35^gNmkl_N|?L;!^r*huR)n-W`{!$
zOOo=Q-tvBr|C`kQ_hZ`&-<&zPo!exdnhC+#50Ni#Hvam;AEFYj_$I!vGXkt!$*;d)
zowMysF+Gj<&L{)|>2{VUj~+aQaJ;Ag?Fs(%&!Gdr(~{3ThbTQjbPW0E43S|=J^=`w
zUmrc8X`1XEb>?}4`6Ds$9KAu!8wo@Y<geeU<~jF}y-<W*@b{GTG%YPHvtGupuli(n
zD@k4Ix67^{1Nn6yi6!HR%wM1Ihot=l6>wi|INN2WD{kj-vjwNF?&)5R`SxGG_s#5D
znx<=CmrXBsqRx?%eZdR8xc_+-oC}m8+Se?f+thFBIs5jetEk&QB;a393re&#l_tU;
zMzRTFb?pqQ-s8M;-vOs_3EqL#L0XJl|CJ&C>#IXN$+lUd$b)yk|EBM>OE^O#aI-8P
z+spGeWj-hn;r+{sApviUGn47xE{Veb3Cma(*|}2B9$nG~5(10XTd%zn_Fs3`KmuCv
zjS62fX!s(Iy;qe9|1G=Mv6KD3{pocRNT6@s5xdq0cA$x9-$;<_Biq`bVow?1(1#nX
zI<OAOxL=2(BruN0x5k`#ZmRSul_H`LT-_RjZX`b#TPO@A-}9XRCuI89h)^dk51tY{
zLpRk(dvHI(<1nhW0y)*FAbgODpFwlXGk#ULl0b^)4fvUdt!H$vt?9eq0EhAG>sh2Z
z8zHKIz4vy_ip?YRtET@;eLtTSlxk}l@Y`j(V_Y#BCm|tb5^4AcYoK4>B5lwJ$w;=H
zzWT=GGARDY=6#}y6IgTYAK?Ah*W0h}PW30-j-R-!or?qO4=xN*i$@_*9v2#fQsDn1
z)vwS0|MOR<SbB1%QG4#*?DT?B@tkM*O^lX7QBoh|q!z(@*0*+i=>esL@#WCcG*)P(
zcV7BeDKpygVs>`6_fi=V*YqQHyq3M(BQft~OI#D%+!$)kapB=L1ii%M_wwO0pB{g|
zFJ<5}_~_XHVi@Bod6u-}GM1KdO3r|!Duy?9wGuR#WB<65qKkui1>jn%DJhzFAtHFU
z5p54#)gAVN9J{F_^Av2>4}?JGnIkJIQ}phEdDpUZ{GUDzdTeu|qM~`$DjxPw5sMRX
zIhGk`p#r~%T7AN?S;|tXbGpnLy-YKyyQ|!<>%9?LX6D<??rzY}1sV+s(bc+qca;Ar
zc(1dPD87A7OoX)Kxzis$Y9nb)TGqVQc8AwTu~#KiD$zSFvQCg!D=r028Hhz!eI3`-
zwkxQNMkv^Ni)Aj51<)$bV7=jHs24l!4jj;y!4jRwSK`ynWn|ilc7>_9A~ie+tbp_#
zqRI?;{@{>ZN0!S}j14_n9-1mTVb*K26gzdJCZP<cjjQpxjuE-IQh)VgcR2A^tN(N(
zpJ#Tx%(_zdR81AAHn}!)lb?7W{@x)fq|3{B|Bvj-sgb(64142Zcm{9wL}JW6<AwY|
zwn?>>&A|AkJ60nM;}->-zVUE-O4;c6q=SEy)qB@E$OM@hrP<`YwfYL=cHPY?YEQ-4
zq`&?E^s?Pwg68SgrU5k?@6v3}DwCeN$tT{sz=NMopU%|0(YJDu{LEh==tA{+m0g;h
z2{@kmUycTtiVU^>YP4}%KFl1yQW?l2*?A><x*|Q9&+Xi_rBCx?#}l?+5~%1$nROEJ
zn6Sx6%3~$>raS2QFO^Ra0=_#QcZ1j!nEIcjR}!M{<=9_cVjbJj@w{3|?_qFBUb8+`
zK>6YH2{HM##;Byo?`?a#h16<JdBKN(UK?RU#^SKqsHb1qGI#Bfo$<nor&@GvTfQpj
zzY1;sZ6!Zqa(QOW$Is*(=c4-Gmyezb4&S^vp!S&Pp6p~z9_zgi+;1fL9phX(T#u~q
zt_l21hBkUwTbYXtKF;i`)3Z1HB<<lBPLWF#mbA7vu?BaIEj=%3egf3w)kSSyB%bVz
zR|nZiQluJ}?#~mOi0v&24PzwUbSRz$8uCAeS`vOQV$)H{-Ob=drCp?qO)~>DP~Y<O
zX{K~_;VqDlHeiC(BT=9Dh%qi7x8H@um*PCYo=!!pnWyBCJ${dE5D-<U0&kXAnS51j
z=eq^}>@L2SivCjK%-mN({E<@63X-&e+uflqQq~-KipgJtc9fFGS7whwbN;U!8j|F~
zLt$-Npe&fG%Ry_N^{Vgu#`%su8+7zX)fXMwV!69GJtO<JmAml%6FPg=H)b<|`9UAA
zU-^Bhp?-S*zSL>-VKLLT;Y$=dx8s@7GiZ+iE)M7Ld(Zgo1C~{Dnxr7qzR-66E5bJ6
zO@=c!f04lDaJnNyPpT93Q-U<9GHV@DTHB`I>mRif2u7xYWPnxu@eptT@`;>n$Dn}c
zSyDpd<8P<y(i@>NobK7ZDrQh9x#19G>J)_fKzez9drA35uzl9ZxmnX8M2SIN)mN8q
zsgC2r;s-as$-CrFSd}z;o0ZmmT~Tf6v(a(G>BPN(E$stg?9A<7#M+V==dv$r-WANP
zhCkty?2HiaP%QBI@<tunABF*Z<ieY*%ZO@EJ?SZbQ!)Ab_rdZ|>?H!oeJwuv6R)_H
zZNbuUPbq`beJVHC0F+x)t4A7fxyC>vhMzQSHnAc4G*)&vH}6j*4zq7O=&ufjaS6pz
zHYaF@Y2EwpzexCJorDrtSQNh0RzwRLbF2L3p8a2mA{Lh2J>;y{_?h+NT-(y#ehbUv
zs&`ES`fS9u7MYwKlWNUUypF`lUMssRgQIC`&gsubus278ZnJW|4v?>hJx(j3P|QDi
z`$;dGrxv&mF@yy*JoHHUtmB~d8*$UcOQoO7tZ)Bz?|6np`BMXVBbnYAV}7?h*#c-r
zs!%Xrb&!tT^{gD;#4BB{ETLj&9|k@$y-9qG;QKHA2}apP5=z3iU6u4;Cdmetd1L{O
zlL|Z6rrV#Go~?#F$%ELJoJzhCH;K~Fv+t+`_dLoxc*063L_|tT@<J43IDM)HRYnB*
zSikcR9tWJ+c70CW(n^F-j_VJ<F=!J1!O~sZXNW-dimKr%L#_CvIOLm`XPI--q!})q
zvT#Q8vV!WSbXHG5*<@gp{LI7UlKJd4%9sx>n_Yk;?A_5-waGlh()_PN#D9w&I#L?G
zd)?L@kXWrkMlb~P?y0iwa*W;U6e%Nb=lrthgN5ifhZtU5&aa$`RC;W7Y>$5&*PMX<
z&Kp=cZwSaJ>=htDJhA5wRm#Zz3QD{wL?yOPjy$lT-Qsk&YlJ_Q<|A2qwZ=6nF!O2Z
zlAZW3l&WQ)d`q~N?19h#7c!P-2U&W+akx?Rx_NGUR``VvsQ%dvzpx^<q$!b86OC;Z
zQ9QE)q4a2?fo!(GR~r_VMWE{VZ{7?TgnBX0?9=fxZme8eQN;U(@Yp1JLgIK_{Z4Ms
z{sT|85y;-+ophn>d~&d$Z?DIZEWX<BTLQIb?H!dKz<;k4u<saT+uF`ZOo9lk`%BE6
zn{d-RKdh+uK_F%GuXV*t7h=rY3~L7HYJxcWP*x9<Jr9|#p+RwR>sH0l%Fn%+Ehb|`
z4p#ZmnSN_aelw|6!Z8=DUh;^izhc`BqvP@zJ--|K<C{Ri;{Q>6k$05SRF)W1pUElt
zC;@NvKCks?{pGdOiv=dxF0ja~Q#!I&c)e2!lGzZ4o=`;QupVbgA&F?5;!M&<;8yW0
zV@b>J>2wZ<x(ApSSC)VcV*c}H&C9bUQeFL_bGOgY*$m;ZpV~O3@ov&$yo)e42=c}b
z(+&jC`pI&(-YOK!yWysgM#tMlCdy>1ch|cwRb9mNLHO4N*BWOFe{R$Ar&&T>fs@$&
zvGH85nm69HPLbFtK;K<lae7_|<nOMpMyrb&mkJzKHM8QK-qQp8joWu>DH$%qlYOjj
zi@<%&8n$vA!`Nt?yrsv8Gk`ewEH`CU8Q!&jF!&+9M$sd+pFfNOR$E;LoZQSGG90La
z$ghU|-t(dsZ`M1tX0C^g+4dO{@v!8Vv%8CR4xdgV)})jdDQIb3^NmcK?F~gm6Gu~B
zW0FyaS%yUas(}4l<V3OLGhm}r8|q5pu(`!6X7*9>9w85A<wwWs#oR_!;~m1AoE@%s
zm*5G1UbO#1I+N+EbO>%T)1xwCO#D(AQiM~L{I1O%0e!NH_OK?!n_pwVUFx?bn%9F6
zTJaV6csZn9uVom@Am_{x!;W@UMP~kR7toX@CcIj%pv+7D4dNM)p%_J)27HJJ)mZKd
zqSDU9{%HNWpoUHOy`+>i%*C!$Kzv(PGDROM!oo5Ql&k(NJ5W-(bk|^CO{!DlP*>}7
z=(Qp<cR<}^J8G2_>;+36NirL>u^dlmRoiQF>4b{shMI{u*TrL4Jv%DrJmN9m(;_{!
z#a89tZzm%bW?}ej3O86AvW`|6u}5>rotiAM84l7pRxf%TP0U}iIT=?x=%DyMmKq!C
zIpi*@_lJ?{b?Q5w9WZW7Pkwz^G57}6_tEPH-{Kqt3lXDJD|ZSDuiu>zEsMSmnexh%
z8bgH~??SoVj62Sf+xJ(A@!!J`w9o|IbO<-$((PZlSCnd69G&M85_arHf}!%cQe~T>
zCX?|lRbZX|F7B(jb5Bg={7xFKZ}JMSR!(-zJ*#6}ZqYYTb;%u-DRw(dI=?5CM%tM?
z#5oStBfT-d%P2M<LkRMER~7om3{$Z`s56WN)IfQ*If@Lx!}2<7(iayx>(X=D`<)H!
z7aM)v&O=`P@UNuYHzEa;J=mA_sSZ{<*_@Q4TY4hEAepf0+m`$-iU8_DDV}g3SQ;6C
z<Hn_>AU#3sk{{$oS6o$;TArI0-B346=VQse_N%Q8T9i$29h?%J=k8+@SJs@Jto&S~
zQy@($nd!#^G|swUj=Q**TE|4S{0!}TPs_rWLDu_jb~Wa$qM~{vFk%t7izX5)q|O7X
zBC)HjE6R*CTNQQle1-2T5DvUsl67ySOG})!*UM1UK!fTTL^kJQqdf*@#byV~PA429
z<(ViBvx|4F7hk1auWgag@E3o~wId}M-0!oMe!or&7fV5u;SPS#BT)%vdPpRjgYh-Y
zNkce>dW2;N^6N;nM-7slqU>?!H>0|q<?+la@A<pGT%`g9AXsOdE6%$8BVOx%?c?;e
z{spY&<PBp5q0e=Zl4tFMrDsLh1ZRCx%{ystJZ30I%*y7Oa$h7<9dnR<K9HSFiHn|M
zoQ_(gE0qOyD6?Dp!4avYjK%v+;@`}<`cP$#_rq-1KLC(h%A#b9IUcGQMQmv9+)(A-
zk@0%uA<%pfAKFL`w-OfbP^Sg`5LwJ?zMvjgnTR`o;&uYVJZrdB>w7BTYT5rPk^i>=
zhK@LfZ!3?V(K*hw>91gURv-Uyd7$Ie+*t6YNsxSk--GS07Tf%UcP<k&4e8kzLRLq)
zym3$bMW<)d-<mhY;eAyX#x!N`0B45RKtjkjIo%bDaUxdhY28zwB&(s_Ue-vbE2he*
zkeEP4^{?l~t8bp);m)m)7Pq6OTIcAHjadQHLUXuQ@9fBxS7zxxq4GVU-OYsrn3ktw
z4uGIc_g?khFou821f+=nU9k|`I~)d7t9kuh*Z;3k6pm_cyiOI$J}Ef6ZKr5>N>~|g
z-*v9E0}&|{SEo?7vyY2KloZ%sSqibeI>_ts3f}hO%3!AE1kVjBHeR(OAvPBxkU)33
zY2F!};2{o5&Z*FLyGbSS_V*cQV2FoEg(k@*%}q*c=;E>-Q9vjzNpf=P<r(&gb`zr!
zA8Hqyof#^Q2gx^0jR<Yzvn_O*7)^Rn+xjxF;pzw6X6@u5a<pxj)6`>opphUE=m7Y8
z@dNbNNNKZog;V6sBT;^BNks9Ebdcm~&gMHKXkg(to`K19p<$P?tbzW=7KjPn#Nyg`
zjKJ3*?`xcnQjRUyBYP)L#$27=Tnv2+sk$o4JMx|##qRF-39)pqc~?`i(<0r~EKE<U
z55vB4Gu;thGUJ^>aI1rycng*6)wz9182zw_F&Gb+2iixmyjsBSf(p{aX1OVJhcCRl
zJ{-*T8S9=*KgzOr_g7E>0ybo)NEA>)ciZm%Pgnsf#93i7FjS|E^u|P#X_R^A{d*v9
z*VBSK$B4+E?si^L_Cs`0902J!j_iG?3-_=p)p&{WDt{)`g*-f{Ra0T)y>WKBT{kt=
zS~MRAH(bZ};u6!P0BACD(cn03Za$4D9;BJcZ+3pDy`a@NrCO#b*3;577`XZPaHCMN
z&a#*I)3q}!xud_j)-0!!7E+!2p@%Kq>_v%;)XQsSl3j6QdOZrnn)3<e*<MNj+~w8%
zaY`LH>}arNvYMi&WaVLG&>}|~)0{M2yBaAcJW+l7#J!st5*mhev_Jji+k>I2Kbeoa
zgJ9NrvY_*#)J{dZ+M2Fkzhb-9I9cvUqi`}Y(83V`W(n2)SJ&OYCp*{6yBShe<R_9F
z3U_=SCFMnb-d`Pp)==%6gP?!XETr%(!i%ee8i!a3x$+5YI@-MquWP(dWb8Do{-hIK
z?gcqX%esRQ0;V!{cibu}#=Pj(mF}y1lYJ{F$BDp77eo+Uyep-C;8YlcOZFoWu-%=O
ztsL;_s!QJsdZ={+{{H7w0%wE%f)CSnsM8NEaEE7#2AzR$M%noS&ia+lUKJ-YflW@i
z7T{~@^hjYY*V?3W0sC{2PFH-k2D=K7Mcs}qHxKVQC1_5VX1%?4+wN+m=;YUrpX<ua
z_WBsKruW+D&tsvU)k3w;q$)tW&j#<-ON6{@;JoqFDg0hKjDD$F>v)aPP|ZL-B!rWz
zy}Pc=ECoJY^)c|AWM%W81}9jCpuhUus`g<;F}tYZ!%@DEpHsa3tP16E+SvuI`J4KE
zt(qPg<md<#w`9fB?-B#;G%16|v^nl%oj#Rk>pd!&*FECl9gni)>{Zd_@pSTUR&!~M
zZG~D^eD+Un;QfuomiPB-A}MW?M{dukulcHtPE;2~_s!1{j+%C1-C_X<4Yj*-+qQDa
zN8^)p;hQ~fVH}$R6Nvi@Um%=JLc}iJWKk;queJ^r7G2o^F7V>BSElj<*ttST9EW<F
zbz#Z6z=l4y%yqhmXErNTQ8sq%JU@(o%0YF1qggV!0OU03u*WeO>3N&j7MkxG0W21@
z=UEiAa<9zCp8I&30R^4~z?V8loRmin-*o~rsJTs!(<eCRl#b$apH|zTVRK1+J;gz3
z6rsiRvqm9Gb7g*SxBxCD@7j$%q#%F@B}>~Hji&NPqfoxf(4#C9EdTC}`nM0AdtU7{
zfj-K$E*mRkhXF_oe9k+2fpR+v5c+8qGJd!$GzNKdS;(X6QW?zJVGW-&P}Y8mOD9cJ
zY(6<%vlANZ+F8SfLTEdl5XIXpC;wz7<n5LGecOra0ELLT&Do}%W|SSREgQb)r@U6j
zNxR1BE%wYru3=;y0IE}QFa!L;M8M5*p>+UvX1m6nxP{r=oTyO=c+?NDGcL#{1;fs_
zlGXUc;&@ncU;K}GXoGB?D!2X)T2ue((hHzi@>`yPV+}b1Lpl*XASpCYswR;3tMhI5
zD~DS9?ripk&O(NPP2gm6n;0(>6^geUJfHSsp5Ymf(W6`$?Ae+yMZEOCqM&%6(={vg
zZ6ovTn%sS0F?zI2jvH_12q0cs?J=VU2cI)#m*M~xNdG&c6pLDg0TCO~hVb?K<ad}J
zP+CB?>bht^5WQq@83=zM^#_-RP%@YMq^9=@<n35OKm2Mm&8cGAL+mwzxi5{c8L5nC
zMD#HlBG^&^l1~p-!Yk@S48{nln@TU6AR;Nq#x9)xhhS-g1m;F-`qp>E%RR2;m1oTd
zLd)x%%@$@8$<beF^ql^%tn#HD`^ohB^tT@~mR24^=wd>*7p^2c`i{{1iT2*Xk6!ZS
z=zjJXF3`ok(F$x_KOv`N#R>Tq`NfOauuTm=-@T4QZHeP$uC@%0-+@lQi@%!9sAfQ`
zQ)J5FlfHZ^lB>`%*PHXqDbCSKrzxzV2l#qdy=%E-I>#10y`$^9+FmT6_nJU8gcwNc
zKpN!kS^xw8<}hlBv1{Fdk2E3+aKb_wN2a{rZ!Nog^j2wH_|xG#;=7H3_X=Rw7V_oK
z!~G1#yV@YnO{hb2zCZdMoII~*DMzdw@J&xG3kG~0QVF1c5wDM6gL|u#z1pK+`Qw0)
zC}KDO1b-8USk(U~e0Vyo_(ZBx_|W5)D>b0bcZrv&SpMunUdc`^2wjMOYOTFlr3JI@
zY(2jZsT<xB5X?fHrlgo=zrEzX*3(oMH_F>@wD!oz{z@h6MZsX!7<%+4#~LdA`GlN+
z9-UGwG}_CCj=`kLs!1UP@jDaI^BTQ;%I9oOPv0q3o?fvWy^qxl`1Jn!oR5gK2121#
zH0!|g$PPoR*JI#Jai7g`7Qqw0s@lMNBc#k%)Dq!<wSHdYcdN*(fC%VXW8x}6YA4Z!
zu6w*nv+dAU7MtyNGw3{Z8uW^g-<lXxAAISDA82IniS%--k`*#*yK3dqklpKwLdf}f
zr%Y*VrBC$da6etewOMz2ZmX+QY{FP)>P7z;d5jN%P*PDOe=ZL0RT_Dk1xW4wJlp^8
zpG0k`N`t2z!B=w~j7890TVw7XA%}XyrLR-p@?dD`mlfF;ixctsu((NN_2}d{Xf-)T
zHe>*BYe%9GVW!D3AFc9y+=j4LN9AX-0@GsZ+qLKXq*m672j*Cxgk(m4ya^8a+PTif
zf85jl(#lRn=$u$GQC`@54OaYPbE1{K*{NBwkQfv)TKgzChUNd@D#vE;bf-x8;gddm
zs&FPqvO@B%g48`h@zJsZAJsDdTTkGQ4H|tfTk2A<4&mE6Qt|k_m-v-LZ(G0(FS>N5
zjW(vP*g>SHn(uM?*a6TnY%9n<d2~%P+4f2${-9&>rN8^9xy<pj$b`Hac47HrK$_z9
z^JYeM;A&Ss&vRCy5dxX5kGtlhujQWIN;p^7$Qw_;b-i!t5>s9A7)xmW|3Z~WEB*7w
zxmKfylH=9cy58<?a?szA*LIu}lMdlh5>tf~pYoCkjrH|!=4;SZ+HNUt?;!y`*!Lt`
zGeT`I`VuIqV=xS%w!%HOFddrjhVBv-DTehvz7)|Oaz*%d8XwWN-VEJ@v{MJuE?me$
zbv!lbkB@76^OF;oMj-k1eL4jLtS;jI3}u{u%Lwz;a&&egU9BpDSOGcR;ff+GzW;>;
z;`4Upf1w8;-F&<>eA^5l9a*_n+l8R@z#gs64|kgh-SP#}Jew8p{cYn!tRR+n#WDVb
z+|6OEPNUFoRKF4DrJd4C>$fwR)?%~x_uJhyH+UT_lHV4=pe_f|Up6O#wu`nE2C5fW
zF9p=N)E}0-9mglBe~fGc{S+MC>2Cv25)ShGutk?}=_y=!Q7XO>eJ(#b?(S9Yq_|&m
z5B`zeedfHgucf=1-Bgl?CI*T51@f*0Ss*xv-QWYVHEtjA@odF9rY4ftWx?_r?_D|(
zLZtIh;-4?kKrCaV>q`2QYIEd9c-{9W#hcX0mn%qUAVAhS`cuB8UOs>9Qb^5@{vUGl
zO#d%;t|BC3mevo^MUoB6tZmun#y(9>b@_!shCil1*HR*M)tZlXq30EURbWIX`^NF{
zGa84Zyfb9wJ$GXLuW~1#MEb}?7x7}dkyn%*tq|YF5&fDw9?V8yV}ePXB`G7$m|v6U
zq;d8-*ONzn(mQ0$kK6r|<Dv`@M!T{fqxn`JOvotmYIUiT?d8wExWjpfl&dBpwlD)M
z?jya++v0RozZh`Xi(`>PlHBSWiGlj#L+)+7L%uZ?0O!asx$#fq46xY$xT<%Gu2UwI
ze2i-afDk3k0a~**vm41Lq5XWO#e%k!{G~nf&NLofmNh|h^WZFx4gAZ!L?A_xLt7{M
zkh`-)`Mmkl1lp>N_>EPc7a10(&6b8zbVUNEzaO7xHuv3m7yPVXyyPM#Dm3VPUqG2F
zpD=Zj=V$0tR^)B=Q*+7P`>*WyWt7%9|Moq9efo#mJ+m(l-@TS=0x#VxwTt9=_HbXx
zFv0BWoM^cH=@#t6*n0wBS-)L@-Uhi3W=Q_tCP%INeX7}4dUan4q#eRF)L76801_qw
zNSKtwBp0R6hJ||Si8vc}WM8LZEoL?+N}n8}3!lFj=cmV)gg02LlbqeyU^J*~)P*uN
zdN%zH_D%j*CM8YyLH@<ry3mIfZY1G>Gq4dd_jb&VUZAKYw4%^2_*;FM%Y@@smCOJj
zn2S4d^3wy;2M8%31uC-Y9PGDw8A!-IYkaeEk91|JVU77^Fbm-BYpQde12GP8VqH?G
zr@n~!P>|XhECWxhDJ2{nU4@zWbyKGC5qL-YALp>?jvV@@R-!|LxcYAS^Gz3edH$O6
zKJ&Ys2?<b|M6NE@%(Uv_PM=pmL{W6k#<nj}+z0bz>O|5{AP<;&qGqQlkUg~OVYL>D
zUQl_aV_sZ=%N2x%3?v34)Sl@IxE*sbc#lb13;qJQo@&WQYsIOTPdKM8b7P5~26w#%
z`IG4@r+@Ok8LD0EHTpBgCupgSau#cK!r|;-AJy?{evS#%nwP^2#$G&z-{YS&l&)-W
zf~TjpFA(ZV0W%%+p)8uM*{^cC%C5oghg42wilIUOTb^0<f$Cs=nUR?k`>Ni#mDr!j
zisa~zI{l9_Tk6h|M4K(9wcU5aHRrzs&66kbYAeBGYrPe6%BC|IQFCNhU9x1Wh_BbI
z*yc`QiH=Ff^P+8JsHdU2ov^|#JQ8!q2js8aTr0mL>u*VT$AiN51^GOd&&HR{aFKfG
zf})rxS&Mh^Kuq=TpU`uaX2ppO^@4c=;(;!k8$O5Kc$Z&!+5gmj#?yW^Mj~5V`sMSk
z?@Ge`<n2r-i#un>tJjP1O4I|3adN`lHt0)$p^1?%%DyW%qaXh>Iu*-gLI4Mu5dF~#
zaP_xOMs>pUf^WEy=1lv4M%5()5%?Lilep<%J?ZpzVNO>Dx%JS}x{nXWBkyv7eneSO
zP)6WdR9H3~wdfNy@;W4%9U!~2vdeom+oK*!Rj`UHn_GeXw=f<#7J6oKc^|snU-VZ4
zgS&Np%C+6;qpUZd%ip78`HIspY%GyZn$9N}@66ap_E{DrDwNOl`ly?izK?4;DzEG$
z*PET#){>Sb#NYym4MjCRDOa8B2jx7a;W6<hU-&;WIQWue>jxcXG=45oo5fS+4ucr;
zGg-V(a#ByprPy5^1PB{Sp!A5pjkN#w2Oji?h;&Rx2}8@0zNK8x7=sRP?V693E@@j&
zqUyX2J7hH#eY>v48UZ-gBV_vQzq1ZgYvpq#%~Ix&<;j{cplrud(R{|Z=7)3D&E#kQ
zDcRWd*|6J5E+rW|Y&bs@Rh>Swr@Fs4AQ&i*4IivTR^#P&i+gthD>j7|S>V%eJ>+fA
z5wG-un^SudPb5~)4%h+b9)!9zg#~C%e~9h{sA6InAA|Nv)X*d2A=_1wn;cA*j|AM&
zwz^!t*9ZBJ4q5i3#qPib^L9_=Np7X(b;ZQJ!K89TVDvHD+ZO^a)hRp2nC7K~>##8%
z(ks7U<wP2qR&GZJ{YJdlE75x~3~fTHl}1h%M`1ar#>@XY<>5ciZhwFgvJJSNBzke}
zv_)p>**(piv(J=GTP9!FIW#D?t!{iDO&q&yle8{ai2h@TtzT7-|D%X;>qBwSJ2~DN
zS8<}edc!a97<ls5eHyXm>J!_XPKK@Tg}R63l(0$fj3=0jk`IMBIg$q=2^H@meb+Ab
z2fF(4Xj<~qPxLx<37_wiOw>5xMwK9@M8W5!e}S;T(FLR?)`bTlB|}@sdr9ea7_?<F
zTSJhP^Y&7n?bWpt7T{G`;!Deu=8A{-zIk?FU6`yy!@`v=+tcVjEY)fR%>eQ9|8?pF
zAfQQ{y^aeOW`#_TiaJ?YR<`WdtWZ~5yQ#mo_n<i_-Ib7!(-E@L!{}l3jdT@rMe_R#
z@oMD3LY#X&<LuV<p|SN?8j$si`YzMtn;Qa-{dmdZVQhYg{pn=6S6cahGRXp$eq3S=
z&xXG`Y1a>lZOkVb7rgs;_iG;ZYu;E<PU~499r8(jtJWhAe8GP(mFHQ)#JiLI;cz^a
z-r;ttfj>A~KLI?WzuRZSq1$R(Nib}-{9}P^Bse31Q#I&K8bSRB3=ll_RnDPJCW`a(
zXTSFL_Kph57#bS7*ZA4CN8-mT@)ZgB_{WT_VPwd`S;pQrE@h{|I~d8r^5E7cs~YoL
zFX{RVAqKB!9sY#NIov2EzA*6o4|!IiQ<vE?xAfKhR~-JXIfWIaE_C=I(t;=)@4*3;
z3q5Sx(4cPHVUZ^ZXuoSulbYTI+&g1RP5v5evpbMCT5wai<;O<v&0N0VS83pn?%cMe
zBW;Hz-9ln!w@K%>x&Jo4(9=J7)yzCT!&_VTBa)JmQgkizD^B`dZMs?842%cT%#b0f
z(oCIk^T2?0OFq~ujv%?Mj_#YCON1>F5?kiA|48_Dm=bcUZCrGoBqVrXr2EIrYVIfY
z=^C*R263^*jGPbFW!|>2TQm^+dW_4F?g7|fU$$FQ+y0xW8@3vXmNfbL*SHPMxdK5B
z!5Q}XHQ?68yiAy%WIp1h#qvEs(Dt(*dR`!8Zz1f19<o=sZJu-143a4_8{hEr5WarU
zA{yMkOI}u)r0+qR+NOR#eF6Um*#bA#iHx>*J&SUgsQUe9N-*4dxBN%qFcmkp(!&^k
zqwuCZc6@2}Or`aznEVao0005&T5eyok)N5ZT9&z!Rx!K&(@Twp_2RHV9wKhEoq`jR
zzoB@Hzi&t-<95vVzIESz=5h&)DmS6IW>ihs#!=d(M)^O{%?~Cb=NmiTEgYduBqb;N
z7$-P6T%hwEm=9gJ5=QDFWgk59I9@64t(z4e)umY;iv|<m?Ptlak%Vu7o0P|>J0bwV
z{^|%eZa}W_4IKGp?k#GLJMf*C$m@O2ZX3Bb-Mc``76VF9?jH-Lk!HHkJnPXo3iKO$
zq4Zi%J=nE_UMAp3z6>S>kKJ%*0dur+gsg_UFYj@uHR&7#D_RCum98Ht#G+sAXL7|(
zPP=_}hTjPkImwg;!oyIv+%hJE&QVfUrfy_pG@}Fse;Xjgr0F;is3YXJG-4fYKmH^O
z!AbBA)dC18PxcRq2EU@^T0P|(d1*!C>+T;)F@JwbaAarvM@?XoU-jznS@^9J*O$(}
z(;x;bC4R`4ukyp{CbOXm!`b>_y1`Gr%hYciyYbUsnIAQbS+D{MyK<bO0;Vs_o*u8h
z{dQ^gP2SZHbU}3uweiaD>6gVU2!qw@d34u;#)NSdx%(5Bki1k7uWgrMM|$&v5ZX{s
zv1fc5Tcmltp*Q`SAxOqRAkHBBBg%fLG2~*p8sD>@du4Qlc)@+*j%}os$18n+UCpI0
z+o(95o2LHerfOrV1c!HO0(~YS{Hiv~E$P+_L&+{6sa*DIeU2G^_gaV7!O@H8yS;Nj
zNr0py-Lq-2tn7(pp6>VA`!Qs<RyQqBrLt<FU&A?2gTe<78Hn!<kS2pJqlEg}<vdUg
zBPlw#fIK&vs!xCn+uWlF4m09Rlme*P`(}fJ7N0_Q8+)X0zfcF+o&jxGb4T;V=oQSB
z3k1z#0td%bG(2&OjRscWbYt*S;D$n95scN+{$LR#+lOis#k=Gz{$jT#@QETZvZ%&d
z@%CMeFv4#=E`e?7Zu<(&yjeiyf+P6>&MGf`r@_2pLE7A*i1eMrif)iI3TgEUFlf7P
z_aaz;>d%<mKcrw}lyRX>j!fbAAFv<=hyD&7Ib9!zlL_RT^zGi2D#6V`FST9<eVHZ$
z1bv`LLqkIn)1BoLJk!>6Jh6`W+vQz<RTh@ESA=_0jrsY{O>Aw6VTLo?iA0&807-#S
z6W2WVOUiRo78`aifKc=k-WQ<L1n(&OB3SLVznoOqP@-M3v2%FbE+`CgqT3ZkU=}5n
zbK+XdSCg;aHatd(gFnUl-0XEsOi5fDu(W&({qwod46-AO2S5p*WDcXdW0o+6WtTB}
zmYY$nJ0gZ~rYZqhz%%P5w(3-*klY@k2V`cw<gBE+j+lQdL=om2#YbfkjgDWEfwW6K
zgR(V&N;kXLSPY938kEofJe6TT-K|N#J+>e^UTwK>IL~PcK0Igsg86>On;ZRN`(*#l
zdg=5t|9y|_>D9qcQk|7}@OD}r#O*6cfbfmkWYWiKC>H3nRuh>5q`f2Kyd4ZY1e^l+
zr>SDo?VXP|1Fg)zF+*d?uVgLQ3yW;c*?rKjeJy8-*AHgVnTA?@MebF3bT^?)Ewir*
zu%cb51{z+@9@8S;vDdb_O<=Q?56I|-^27uQH3UVm<>PM>P2J>oz`Lo_SDXrhKFFM>
zUtJH3I$lbs2oMSPQ*o}pIQ^yKa%@i=l=<7mBcLp(ofp$Y(HlPO2yUkDw`rp`RoaAt
zSI-kTOO+kjy)3(&y^Ag^39z9Rizr*Crw`77+QuK%hVKQW?V0q`DRC*Wb0Auc`}Z8H
zo8_$}Y_>LO1(OO~uE8sMdZXLEtxxg;!&AIzICh8u@WVe`oP%RZ0@v6#xUZAiXEenV
z_Dmnq<cn}2j*gBe5CXDyI@Nv6EaV$MuV}&79|1KQm^u+*u{S0+0jjVZe;x>F1xYfV
z!otERg9Rhk*1~EzUa^O*&kPN>?N1)KH`bgywXZrU25hv^=QR$PjH_eG;I?HktD`J!
zr&$-;*;wtAUhy>FE=}afF$&h7{X9h3dpC3|NK*I?Z{0(U2*SlZRPI6XYPnZcbswZ$
zD8!(ZSP4DmX&lwxI^fa!nv40V#ca$N*p>D!ST;_IdR%;-Rb&=&DOJ<8;Q**vM6uJ^
zo7W+|(?4ktCrK5lt`LN-DdC=7YOSk<DI{Uy$#^AFFCz}1Y6nGAFAjj8BEFSI-`_5}
z(8H<?Ui&c6im#h=DL1buV&3i8>%rE!k&jLvoB@T<g2nDGplZ^0<P5bk>yLnZm*`kQ
z$_h}?6!$7+7)Zh$EVdULj{5kWuao1QtX?BAerT>vo#4DhdAtc1M31M<s+wO~2=Ze!
zaX-Gj!Gmu1iB2w9y=+-CEovV8crML2r#eo5S2+-X3v3NE9<+pIV_I|>uomRaQe0Dj
z16gQ&-q_LmMdX9HHByjRt_=q9$Fib85hF-8hi!?;zCAoIhMgAF)@}Q;p)A#1%GfpE
zlo;Q~BUp`<&3j1iah`Gm)ugTjFzyx@cTbBE(i673UpuUxncZUYFtZB58njUzaC%_j
zwPTLJkK@M{ncJLucw*<N4cTe9gPMUvf1exW{>Rn<It90p>c_r+`LJx`@uiKztzChA
zQqILh=NKCfecF*D;eHV*_(8X43xf{SsGevVz4`ldV`?VP^Akwsu6KvPAGug%PbU8M
z0`}DHX>Ew&_TcA@?J$^3uq&hH5a5c~Su*sYfZcQe7%n8YmH&szy3Az_A*`NXPu3^!
z%N9r9WjcwQn;Xu}*3gpXs$<;BbA@GyuKUMY3qT8UrRL#$?j%A~@Z7Sv)U*C(gv_YE
zOR!J-gFoaR+OOTjBc<dFCWwv4^{R3M%e*R^RzayLdR2v&@_)XWB`8s5WxTyp)N*;&
zH~8~iE5U`rM1eB5`fzvaxc1;z_N<<l<8VK`J)jE`(+3wp1`+QrdY0Hl3Haem*#vKH
zZpTbJ3(FLp_J9@31NG3QaY%!VajyU&kP@Tj<!585uv}Et_p~8by;S>aE3-h0wv~Bi
zOxmp9GL_ryMmdA--|i}YTe4DyqiPgV2h`#bC+qiv`ptaySf>syz=xckod$^`1p6TS
z3=iu>wncG^z(@3q^0b|^=41Wn%^-qsMf7#~f>c;orof!j@<GuTXGymqt1nLdIVmj;
zfhdGz;wkzGOQ%d=2(bu3ogmRdA4f5<7GBDB1Y%bMH-y-4Xqmr<{<;%2=Zsh+qkH)z
z0`al+3LuQ}4;uC4=0Nx|a6e3}y<&i5I~ml*GqckKQ!N#2-M_z{v6izac4F;(>U3H!
zj9oH)nlu4XwE>Q6$@IMJ2c!k>D6D(QV{qGEgKYE{-UWzzF4I;1brT^5KXlVta`rWC
zEvo_<(g02rce^10O6AT*RBpa=mR+Ax3wV!<931$gogZ%Pd3o>Z42P9(2C|6W{tcUb
z;6G>c-E=7UfQ1DBL!XMibP?4I4aX5lZ6TG!!G-9ksHgy;{o~GY=hNuhEAZRfA%>q6
zU)O#Z#FEWiYPx1Tz$`<;|5%P3{vqkmO)88ZszbZp{ccG0*li&)cbd%Osq>L7@kgom
zCAY#9>-O6V+)5ov#1n&Q&e|z|;x>3|Rdv@20t>xhpIYBY#D4)t|D^hra^?pw4FdO!
z8q}ACdtU%uw+RnwhaasCIs%GlD};4<wz7+ZlAuD4V)-8NNo;}jTII4%_-ym}QT+3=
zmN5w{13ziix#w;LS@W~kf<nC8zxu_>l_P>|X|_eiLrr3D1w8wGW+VnN#9N@?X&%%p
zcXp{JL#Iq8-2jg?zN3Ar2u1TBL2ph>+uT_V8Kr7GZ}x6%Eifric<2oyt@0OKm~y~2
z+emlLdC7HIoSQnM(-P*J{TSjQKGuHOA-Tfr&BAkq^}gyss@?uGgC#mu(0(}QoUQq9
z#V`TGY1^_4o$uuEOLDHN>(8*I$G|m@mS2kPdB&S>+c5`y*}ef>mhqxb%EtxUTC@j@
z?h+MmDJPsja4oSj@nUvT)-=DJ<~pA`9xk390tR&$PAf1$P#+KG{dv#r^KmuR9Nl47
z{-=JdlCM82xYzJ4uXf+XA$ScmO3@_)h`8PdWoq)r!YsYm`~qq2o9A4+*r}Q*OJ7e!
z)4JHeK;K)K)fHSoTWOI(LiT!26#*!G148NQ6N&Ee^+2SX2?U88M`pf})tekXxm|!I
z>pO|(nHUZE6&pW&da2aUEr0trAlJVKB>d@Oz%Q(Ji>Kj3rP`&JJ~S2Y8`-EhlqyPY
zC5bz80t5j!IVTziIoV~wZWTZ?2(qIcxw<X_ZmfDdS;X08+YN#1`yFzB4Aoo$7|~X8
zMxvEo<5OL8Ad8_`R{g1BQ_2VW)~S7=^0KAj^p$*tSo<LiuaUYp?37DLo<CN0;o-ao
z?6*bBzKOOMk2H1oilJR7jzy+wo3pS1J3SmIO*G=Pal)sygg5kxZtY3>GB)mNO^oC$
z^~dRDSh;`KldK5A@C@L70(fV&4T(N`_e1yWe&~;^H6bn_-b8<16EVwv<r3K<gzj`$
zU8^us#y=#3=WR3Cs6BrB>As?NzTI_ZEIu%D|6`4nM}{EKkWJ2f-0SXZip4wF<J_qc
z9%nOmwBnFG*!0-zt7luiWTxTjqhvV*3wiMU!j)>rgSefIkXAm0;E&6ebrWrk8xp=k
z4$C+B1qfj9>&%=2GzE(awjxs2EjZ~XeG!h9!RGMrQK~cahBPSEvBJICjeDAkf8Uv^
z%!cX-^Rs-OZvT)M*xoi-?Zk|bUszp`78!><m91YP?pJ@(M1977u$%><6+|z$uSvw9
zEUsk0T2{d$+CPKvPkjZ*@7Wsb>HUEs_s2D3W_>JQO)hxWsLx#=U|=&Gb)uJc{E#6m
zEWn)qVK7kzupwh*Wo2E5+}4sW27CHlCnf+MrOS&hL3Hdr+$-N3*vpwn5^~d3%ojle
zMNfuKiUs&ba|_p5WOi-Z%7N`@(coX+WsSZ|UBg6CFT=#Rv!sDkBdU{C+;ZuoDpMuY
zEweVy_$fOd)R;y*MNLM#HOXu}>R_~8nzEmLH57e9>}7yt3Gul*NkXzdYO`IuZ9g^~
z*Qa{QMXz!Bg?pNNfTceDZts=C0M>=^#GBkOlL4@579Y@t_(|pqaKtB|?Ui6lSJQFY
zv+mMk?M8RteO59wRx!nqIpgLoW7xO7M?nQrQ&zLImFu-#%JQQ=9c{EQ<&`V2mx4Du
zUyX*mgNarEWo=T^%&TPZ=df{E%PSf7Y#Fmw=BdQPH$hs6uYsxY_(=<=XQb;TmE});
zOJA&*iV0GrQf&Y>fn=jnmX=~OC_qZNTgAo(%L3~CW!><wQrgPp7S=yT7o6{b2m6#f
z52f=5{250OeX-e);O##5=@qY7Jb5r%n!H|a9*Uhlyf0_|d!Zu1psg?~XWkU)t+J^k
z8;g4i*L7i}!Fl^pc6SdRNmhbK+64;fh}4j<mPbBc%&ufmbx=}o_cBVGC0od<P@`(3
zo1$Bpapd5}hdJ6qxsVIr48?wcV#nJC-M`3QFTxcAAeyZ|-5NP}zHJHm14m402+rEP
z0{RnadBv=?Qra(U=ArAWNv}AvjaYXGL#_RjOQh7=ajp`={t??XiZa?$6SRM*{mb=Q
z<e1+M+>uh@Ep{hFeNJI;-Ti&bqsD>9J7~Y;<`6K85-+exDBYertd(C|D*^h^?J8vY
z*z_Uvl|9!e`Eg=Sa!LxXm)x5*x%bhHiHxNcWxsCm>8V6d#E`*KTTOpm5%T|0^__ui
z{@>fJR%=vIyVR^zyNV)cZKag9_Eyv`YQ!wHYS!M=EHzSF2(e<58YO0GB%wwSM9e4O
z@9+12p8s=RB=7DQ_vdrYea>}V=NyWS_O!j`)QzXU>D1N4uh{>(Y`*0f)Xh_tbv<I<
zUo}=8Wh-qP296Dp!Kz(wXh3HP0Zt2pRMt2esO6kc$V*tv$q`Bka1eE{PCJ{<fn;3?
zo=|+PVQHagbFzcV+?-MY{gqU%nx~JK&wTskAGC|O%!cCv>s4|bWWSJ+A<EzbR=9FI
zl^H&ZH4@l$4$ID*EhzFIaSLV59M~O|@GBR2=JEIM);*9DqT0W~4pwHKw(fEtcgGe?
zwdO)!)pjDrWMrrAtq=@4xMQ2{Sb2Y@&?Wcg)l${_sC1}rLwZq2ZtJJ*Xv%_Jslu0A
z^uNtS1DXSu-jx7ZLq>)E#yT|?>b+?o=(uYK=he0~817U;sM{`NgQ4-Kg>PrybHLW{
z^Wa(baTu}5u<dElx32TzoWjVRomq7Dc+#GZ)v_3e82s`rL9WmImlP~PB{$`4?>NUs
zFDcu0<*Rzt#8Qjs_+e~;G*hC{Bwhx0NeQQeca{%~7Y9{(NzBpDdTzKBOq|_8UC62A
ztuempuy0ga^B$gMtaWpqPZ{Wq1S)oB$++O(bRre_M8$J$%g;LGg>oF-7d-k#(W!*b
zB)ie1qtgPvJ8m^7h!vdA8SWk|2I+zkF0pZhwmx%njIDwd+!W_PuB5dl8$<b<fEq*k
z;E9TF5hV{(9xJ_h%YI1+;l}Od6eJ+m0@6DYc?K72!DRpXOzidh7=rzX*MeKE5LV_;
z1eO79|4pxg%H8penVq#amZzpnkl}cvbKgS8x;_iOyvzr9hf>g=o=%zEd`oDf)$faM
zH{jdu?3O11BUX;ASC%(E-6l#fH(p+x;X$>Bu{}E-;04RppQErPATeKOdh_N@{I^|S
z{TEjyQ44N*3lE;Jlz}nmvo-rhR4~Ixs35<9!mA)+0r*XixOoSw90vBN`c_yb@pgHb
zKW+oF=l3zNb(O(OyFdg&OEDVc)}DXT>)N>U=I_;mw&1+S#+%C5*}hq5l|g({>Zf8B
zMxY?L0Mw+gyCJb%>T`D7ln39gAoOvFH!S89DrAOn6wN&K^WnN=0eto1MQNA0!0e%2
zkZ`XD_DZM)eb<8|xRRk_W=L|O2LVZI+0;#amAlq*v_KyM1s95)pap}LhCtd^ikAge
z?U5k+t0VK1`=UV?Or06~ZBdF}D1CP0-}%i{UGRtY*Xu}%6g)q(B{p+-U!TsebV1M^
zj7TYs!Cr_S9vIBuw;ITD9KAoIw0WEvVw*7YzF;ZGs`k{`86W0gJOzF>tY+=8>1tU#
z*NH?%JGZGVH4&BmaWY@>{SBMi8ynkeS_?p5MkS-9@Nz?|BBGDH_jBiA8)vsSFV%7c
zL)IUBR){(6eD32&wa{Q2b@3WI-+#<&K3djH3I00nW>@}p0A>#D8pT%<3(g=nEA}Km
zVvR#ZWe0ujU-0+-$^7ul$KMuz=jA*vE<P4?tUeXH;kMLE>f({@8py~Y33ou-|7=p1
zh)U8%6<0bfnShA;-c@;gIufI@+Z0VO&ockkoH+q3kjY9M6Io_mWp*DRb!hP)iS$|X
z@5KH--+mWe$Rnp{84z^6P(!ZYupn$4{xTj^wll_tMoon3?u0&1QmPJ!+!zWH&Pk4)
z436`UC(I_|&vRymj?f6KCoQqMcs@|~CN+bH<MBD-f!(rHD^U6%$dcWBs%GlI&ZM|)
zw!)CUMRTm7Z&OiG@7%QFJ0>kLJ#+6eo^lu9&~j~8<ag&}ezxUaKndo+F=&?pPkjw(
z`)>&T!7WH3<|qQKT+sJ-+QPhV$88>qml2ou)fR``ExR1ABKD&QksQzIxBd#`kIc;9
zoC)>U|6kE|f%`BIV7_T_zxQIUc(f=_6RJ@X+al~UfgDgECaE%AEwzW28BePw&&<M=
z$OeL|SD<3_Sd|agmiilFsb$UPP!+i&NqRbB(B3ST)1M<S)aLGvvz_`zLe!b^)qPKG
zHORNtMFz-&D~oX<%0-a>h}}r!+W5NUu<G#L7vxG)!D&_iA>kiKCEJ?hukV&m>#wHW
zj6?=u>>m>{Km&uA)4;T_wCia*3|M^nqK5>;DrDDxp|<?zhD8h%v)a1YIel_*EX^*1
zHDcGR6eQfxQ>bXi<V?+J>z%k=5E-IpXqP&y`U-LclJ^LzD>XbZ9QBVg01WmmF^?jZ
zp7et@$?P3r?dvx=GNDQ1ot@XEWeZT1#70|l8k9-ZzJE%n`ur=OE#m?w?H%_&t3t>D
z$zZA1AD$nm1#Fx9yzznuEZrYdTMG*F`TQR6dfpX{<RAjRl1!gKe@rGmo@M;C@H=_?
zo1S{_@xnhUSYnwC*4%LSh{c_|NMhoak}As07SX6eAfHIE+t?~eoqUO`1o}hU)qPPi
zZ`z!J5mGHbhkAFbvVf5==ni&PUOF%j`cayxL~5xmgT4K8<~sW9Wbs3tsZS<_U(zOb
z!OQJJ9>v<zh+ujqKcKH9(<CMP>%XiuHXq&5o`rzK`@I|A+h#4wv0$a82b36Ixq%kg
z{Ui*z5CxMKuo>jXzU@LdIl1h9Y0C+xNB2C1D`MuRA?LoAU`Uhn=J=l@&})G1QEknI
z>1AGr8s-zIGMHR}DbQ0M^bKnnP4FESBGd_^O;!L6xtTl|UjU`7gK@)+0%Rm)y;<fA
zA+mlIorye7D%|&$L+IQs9$<gdu@!_f_E$Q;-&PPewW|nWS%!my13)a1a47}6$g~Xn
z1$00>r)8x_A%9Bkz!(zacupMCKuj4vSvJ5J&9+~gm;h~Vdu-oAX%CkW<9NpE#rGzD
zw-Hj0zgNlUA?v{{xx~b`MbPmT2dnVHlnK#q4h8>1LEtSe4kR-&Zvpg1HXOJ4PA%l*
zteAKAZoF4`MZEh>XH%EX6+2pKI%RmSr{F@F_oG3<Qjn-`fkejjc3$x|Gqe^0UI{zc
z?63YcU?D2%?GQC%z~RBWOKtIFRhoC^$=kVlR$>_y$q{nn<aQeA3w*`^8ltk|Lg9PT
zuoz8%Nho@ptIIRRYzYp$4k_`69aRbX8L*eH(_<lC{!SzDt0G(>ti(=YLag7;mK{Cl
z*+8T$RA5%0D{*1(ia$r7(met{;&H%qX*O`SKHFCm>nGv{%(Y$d_N-n#TA1r`^!pH&
z2IpW>MoFaK>vZQ0J!zF8Ce7Zx96En+yh-1u_Xudrk2G(KlT(!0B@`&dTkfy6MxLf*
zFd5rY++CA)TOjEXYiGvAthXW2U+@O*<>tChWqHq6=DsXCJ}8UonN2y0o4b4(m%wD5
zx*A?+_C|*+AJjX79C6DE_~->cjgX3`EP<HUg+c>&%$M%hN}J})eApH>|Am|P3}uaq
z>m=9{iMB*p2l<wo-7#B~i=g+uf#3<jGt&;e;}j1$e~jE5H~(GZ>?i9!bx9fxWuHvD
zbG9()yWkSw{By>dV}<EeFyNNx@xk^S^Nx{rX{QpgBc`xNdjCXX0lcuKT$2zu655W(
zjjo;V?^oN14xc808$Knh9j|V2;&dEDQ_hkk{GjTO%CiSJx?v~hOZL6dNedGS>VEwn
zaDq=n^W!!zeIeAH$4^=&4$<A0N44N*3h(@#m9Qa2UY?5`mlDBn((|4RZ;qyyL&1Mv
zdO`xnPx7X~$(bj7_Pbuw4$g0?_49@~<o=)r2;c+OYO8*;ki<=&F3|1I{V+^o=6)@!
z6V?m?w49?+bXbzhu+)QoNXi{ISvlAaa(yzBd})Bb^!>cL*0L7-TP7xXzHVT5Rg|)p
zqwu{Xqr1w&)Ru{&;<c0Je^yTfXqiO%?bwHWwhG~C?%2~Q(|P-r;VL@Vu0v~~u)l07
zg#E{zkkr<W>T=Je!a~@h{r`4_m-dI|Y`Vbm`p?>?d(oX6zK@r@ilUHNlzD`iF%n5=
za$_J62;@2P7HYaEtzVm+IwGb~7@31>ZRKj@zncZ*soKkfbb2pPNz9VUR<GJj%`II<
z@6^nEk6w0e<^Q-cab<W{rd;5n&BWXRq5zczQlJ=Mx!sGuiM;Yz{3-+FTzLlFBR0Ix
zdID+drWy8!3H5ni-vz>iYQ>6#Uqj0J2?5hp3|Pp?3f2hCf$`<gUoHk{fg^iH?7j38
zDZS1eyaF5I2(oJ?j^-zUb#VkS`@5(s0f>%+%|sIAEv1rkeq<t#2M0ZLXDzKk&|G*k
z!K54DAe!M-lo3~u&(ff@%TEtNAS<<HKHf;m4i%e=#e4H4RT`h$tz*6_arS`5j{A46
z0A8N)nJ1{LD=)4~Od6#iUU1oEj|$d)_iP{*e|oOGpV47OoJ1;e267C^_hxlJa|&6&
z3xI@{Wjmq|^eC)c(h4~W7{-)T&SgwjQuhC$(tis6gWq<Ph7yS`!SpgFhm$cRlYo*T
z<Z91d=VD}JL}RLxaUCy|RMsI}9B(v#1^3tg|BeoF=SWGQkQAWdbpj5K3Q^blUMO#w
zlNv`F2`wDi+{fw{fLt+t3T}acodV`34pPvtjt2_m2DyT7mwmq7XvsW~Z9*xtJj1}!
zV^8ddEqOeu&OLi?pm0R33WNE>y8>*PDNT(RMyH>PjEEu?i*J^t#39`Jj{=CpY+9*B
zkM{y^Yp3syS);?JbcSEq_nO28vv7Qde*6$&Yj9cWuhuY|4x$_ivFZGIf-W36Hb5kL
zfcHjWvZ8-2b!dq(%SH5(@zQM23R~_k`BLfCJmbX+s-!bgSgz9}`0$#2JGg69%^TQm
zZ~$H%n_PaonLm3ysSy7A2}~$zcukNOfYuf`=PK)>mObLU@_~UWHPZg9x(S--f-UxI
zi-<Eu*^qD!!#``Qy!N45;Hdgn`-(JN&?xycx!w1ympNx>Pp|GA%C&R9BQWs<(YBIM
zb$_W(qF148{~S&m5kb4Q!3zN#r+t#KUZ8|#1AInATDEJp0=A!>ospt0KGij5>It4#
zy5UV9TZEBFJ}V3-@kU0Hcyp-2dpU=v(ygqJt!aDC4}&_;p8wPyh7fHNS1qMDWLY5+
z`AY=u>yGu=;|Kt#K^=i$rOOWshKCnj=l7Q2Zv$pSGFqI(0O(xlr<|Wwhn@Ka(p9c1
zRn-@1<$mYA?fjL;U^<t-S}!5l$St98*fSv$eQ6-0Cj0dab1^8tOS1bpPR=~5-E^Pg
zxjB7G9FaJWdp9d+IZ+LQzc_?bU2$+#`)h0A4rX!mAQ~~I<#q3gxj~qm=&6Esw$22D
zhk9M8u53`kqBa9IXZFDO1r^q=c6IEXNKoyGO95<T#16;YHmL;7@Yk|0_|j3g?+-K-
z6~nR}E~edEj)D&?#o}>4>*Dekl|tW#Wu{^-4&aG^a@!R*%d)(0{!naq$npu^v1wJL
zY+P+gmWVLjf>MX<3}LiG+<c`%LcJ!kdnF633?Zv0*|GTh>&OMb^0&2r4&_q<aXHPk
z4JxAM1X)#4(aQga3UPd(OoY#w!G#H5j{V5&P3>=we0X7^y@N%$XpaP^6PI>=QY8JC
z#6v5_glbH}fAziaP)mi&@1O6xt|x6A-}*}&#wC0j;2;?M%a`NV4nyOTX4?fVigq~D
ztNm_ILoSfuYRM3pNlhv+CRgTa9cLhQXkZUbZix<Y8x^qn1}7828hQc^H?)cQ@6bw1
z^!Sgf=*K_%-?x07W-wo0HA#a!x%fzgy7-n8<MNpVmbYg+<KbdV`#=YLGKHA$u7y1K
zr}%8X_gmq}NZA6L;Sr{!*N`nr9Kyd@)C!ah3S9V2O}ZxyC^@`4%xT{{2MQXIv`i`=
z9dFs$R%}&(8=~g6L-d2}_6n;$3sMVOuvQU8aG8)sznc1e%F%PSgU4IvVWiMj;iSrv
zJ(tg!rJaWR6oUub_LpEZHSeDHAeQprHn~5+8T@Nu?!uS<AVe0xZkcjs0~30JKbHFW
zuW0Gt_fDj+cn&<8hdugk*-J5%RQ>pIqIA~bBs7n|NdW~DpWgu(C*=p17!aBHU*G>j
z(J%TPl@!Y77o@TZdKegfTm_n&tymNi)`llDnvQ$jkQKs3oE9|h16R_*#f5>QDG!SM
zBPl<n?A8DN`v&!imiU%4{lyO>BqR^*{bEN>M}K+QsS|pV{UUXfUPBek%^gDOQb5Sz
zS*soA?jd9%i1(ZlmV302y0-li;9j5xRzmTonK58nrR|?5QhqV#1nBQ)^rKhB;Leuc
z{7L1A@ula&t2@u}Ic{?300+A|_FTzMLW;c@4*p2*e#HkffON_jc4wR|g&kbcV{hEG
z2u?h=#ki#1vR@NT9<w7p32KF53P$bIQ!<q?=_kI16t48(Mv%BI{P)-vHYuhLD3(dD
z&|L1wwUxcey<U2(hYaC}?4Rq*N=Zcce^Cw-@!G`7>gpibKU8zy_6{;K-5SQE&p9qF
z)y`^40QBPJ#|PZ@)r)$S(hVe%SB?e9S1U4nCkebt=L(MAXK+O*%WMDYb19Ci{s20i
znPq|~o3@oA=5kbka1wxHBle%~W?8v@EtFI`xRbp+%}VM(aI6=*VjCW)7rabJbu+TG
z3?aF=qx&6$=^Y*y$?d4?g%*Op2J<9f1z_!H!DmCJrfILvfBd)+g}g5(wEt|wN%XE?
zRV#p}Q91Ny(DUB>A}g4fk@GD+nHLk0XKNwA@=IHyhL1l~Ueo~RGXjRcJi_Yu?PUQg
zQCX`Z{O1t|NB5==B=8O=iw8$x7u?=dC=H@2qc$m_Wo=8SMW8#AV%3}05aRM2cjPU9
z*0$$ZUQp`_A(6z7aJ2m?ey6@^$^{XJh`^aK-V0M4=skjxw|O6WZZfE2V>~f?0e5fe
z>Xo#)T>icYyvzM9WXlD2un?(c4hDHBp8(hPqFH{Vcb@tEGxqB|^mzP+PD*jm8d6tp
zY?s%IM*rzoff!EN#U;x{3;HYOySsOXC^Tt!Vq@4S$OOJ=zDoX}InFi^X;Ht<$#Q+@
z`4G#^b;K2ZAqS0i4s>V>3~=cEeFnfb__bTSwTi6Wt!?Q90dDa@y0%wVCa!UcdsrQE
zdokIUoFrPF$ydT{mDOl=O-Gc*-&nP(mvW{Z+x16v#n(%j(+G~B(P$~D3l>^}MxBPW
zyOWd2+}L-LEz-YHf4T@(+CoOJ+6~{H&AgVE`F*+YV^AbLcMB^$(H2tcUw(YgmZ{_~
zxE)IW_N*u(o$+B$WCbi;=zZ|auK+jW>a~nhY(kpVyQxo?!N8&Bi(XRR`P>Ai!Wl0^
z$>X5O$>Y0PxgOtw>thjLV@gY)r$RmZ5du@Ml^$rjI6df8wTN}o>1h7rciPev$0|am
zq!lV&Km+|tB_=NRY%|MT{mc%w8pX2v`6YM%y5XT9E&_4(Iix@C2kphJzg*(cZlAvW
zbloTHtDzjq@HU>h*1IL+@%xwlVzzV_&)?6ZfaC8&Yh1SX8CTd^B)kFMh2tpyYKfM#
z<<N9Ay!w}_e3gQdOtTX5$q*n_gtDC21g4DNqa9o7O%oA`T%PUY!BT)8fhfYQa}XFC
zp80OOy>H~6z%A{Z+4s5WO4V-eyDP`zi???yHesm5+Qr*$-r)<T@Ub1KQ)Yr|l%r@n
zrylwfPalPT?Otj`jFH9g#AGYsq4wVRz?q0+cR_8?3qsf-hyLB`cRh^e{l!oq7)77c
zO^3g#Pu~Nqp&}Dl_<5n@MfY$fFHkmZKgu{P2eNqXLQ2?w(9^K3S?4G=J8x9V?|Q;z
zi%h6Lzf1PKKqywhOg;hsSULsmiDInZ!&5&DKp@k^7(iR_9<RX<t_`xV!fxUP`qw7j
z`-jc-_Lf|KX1&Z+2!}G)J!OT-9L;y;d}f?Fz{8@7MT)+jht(C|gNB;y1R91ee^(N_
z6aMMfC;d1wGV&@X;(#}em+ehxPZL#E)qNGz+klIkGKB)pb7ZnX?;Rsw-83f-B$8F%
znKJ{{*EyOkJ630&PC2hZ<AM}DWEg3j)1ZEpZqb4*LI|1-GI|8_?+}w~xf-17?K^7*
zATyP1<x0u}<`?;KjzIPwK5YiX7P$khzZwchNIjPJl|Ro>CbmqO&d;3!linoG2I2J!
zN>LVXUG;_o%S$^{Cei0R4yelE;3L00yJ*gZLPo0%{7TGQppbR&_tFrXGet8t4V#XF
zSC_AWxtCYAC{UAL`R3JrtF=VxS*>L*6Qs*^VEvvevtWV#nSx!3_y=8);QruOa<_w&
z?cfs~W>*T@p#|R-wZ433!d+W+=s;ipTGq8F=^m$yQAJ%3xF8I-r(HctH2FK~z3Pd&
z0hBM-d$XI}g6Fn*!BTZp?Jf18&|1a+YNr;Ud!?{|XbzYb1`J3fNe)O3Tr3%TEUEa!
zW4~RYg*6%f(k`%aeZw@3Ql*DJ@zv!#(5miKGrpX5jPmuEZ2O3xS*ZE7bUCKy18Nyc
zSsD>mDs7kT{1spE@?<;oJu8i=PwBMB@;)<UL)II3ZXXG6@j)kTZwxCe&!nIIyyXe(
zJjibbcfVYNQ~4tO+k%`A*4u!%$Z&dWj-#d<H@y?B9c?q&7%Q*wv7@yZYfLBKvAJE}
z@Dt4HOfH$HKf+S%c+|A-*AwZKThs_M7{%WrO%saKB<Z==gosi)<^3S^gGYzA#=UgD
zGQpv4eE~U+6IRp7AJ)HrscB!}YbwSBs=8{P-hP-rh8XOKs?cqZ!rzDQh&^%~Bmte3
z0`a7E$-aL&Rs7*?V+w%(+K+C^QFeBAs%s=TUl`2SFIhTPc%EG-!KBoeR-n^ioBMRc
zSq7u9T%tri1k9OPvA#H?zrdHP=>Z_=+-hXCf}+zrhv__&#~#_sZZv9)aHrf?gVp_%
zG=>)K2At;p#)exBk<Bo%Lrl{!Q#=FGLcU4@@cmx$>YFi-`gdYc(SNpe$FDkJhwdDi
zG8H0uYKF0Z)$f;=W`|`5>BSfB`R2<_5j{gfqVKUiKW%#VxY^3=If@BOrS{&^{yE1{
zu3w*R$h{sA^D);tHGJ>T9Vk(=fcag1(d2G~uDLeEM=^WsT&nam^@x0I&Gur*7L9>X
zSeS@C5g8_tq)Z;!%wnr@`%pTw<8G%Qv$jZ$Itf{h&CL7@^|J?P2A@*BBSuHcN&nfa
zXTX}ryLmJRJpFB6iI;&^H8@k-t;NAW3CLB<)9$Drx~JWx?N^*QN>+<DB3PsGWogtP
z=_^DuieARqqT*byNsd$00}!yei_GtE#~;Dn=Yo-;X6;yDOVinpftC%$KO4W>m0FHt
zb~Cu%s-dr(7bFVnwZbLUmpoTjXm5j$LWFwz^WCJgpYUHrH`gvsx#feeR>Ii0*alWK
zhG*w7q&Plv8XcE>(UCtaB1i4FRl2Fg27v~--+4AI^)J=8)$0Q0%&El-?>U_04p;G7
z46dp_re~)0NR90*>$3}P_Hz5OOw)A*Y`!Q&{K7a5b5q%T`*Q7GVoUX>OaQC4tcn~U
za85)?u(w$aLhUwAu^u_5s+!HU30VWPvu;wKCIB|&59~{JHf>cZzi(D`a{Kfw<dAWz
zzOEcND~bNEY4H9xP-+I;l8pQ9$f)u`nhhon3cUGT+$7XJ9AAO>S@h*5Hb@cqV-uTH
zVO8Q!M-uL0q`X(uSvq6ii1vdVXr)@N$8!U@*5L;C=Hnk+nFNG2z3;p-qGw;Mx9BXL
zeZWmQ>>(|EQ;K~aq-JjQ#fcPMa6BU+ql-In(LbzYtW_YV?XlE26mFK`epRAOe}l0h
ziu;G){%Id&dogEPc+8mp(523{G8?6?iO7LSEHTIFos<&`A*I#ZQhgZvFx=VvF%}83
zKR4el_LyPSb!N!QzA~CdhRzy$D#Uq?bOeKD2+=us$2j|Gy2aZnA9X^WkDQm)v13f8
zOVV(0xSdToho?OER?2q0M`LJtGA17Ty<M&ddLYm3x@GCH#@i0>@GRNAt;n`;p^0ye
zJn6b^*G{bftl5PpvMpN(9O}BbhVDbSy?RvZ^dHsdm;o3?)}@#97+HgTLk>$#x(gTD
zz2E{a_cG<%yu@1kq%D@EB~R2Btn4f0;zl^>zwebpG@O~TuF4jzw$FSOcE;~bE751t
z&P~OH+(@3L#bPA;I$Jc3n4orc^JrVr*(b%ks1Le_>M1o{I*O1IY-x~jJ#L1V8sbF$
zU_46pYx-Ec)Xg)CbUkgkT&>Ty&ywDS;UBLD=8EH84>YIrg4O=QqY|}Y1wx86DqDg6
z|FJtk|G5dY0<n+%RD}`hwAcS&a~!Jg{Go<D=M-mpieV}mM~q!Jp%@XxRMo(@;B>Ll
zJQNOL4@qzf!;*cmw?uLrK6VGxey8=HA{%YQ|25_7fv56y(qu%fCty(7!!LOQCB!Jm
z(nnl?@<JX`hOa*E45vhlcBn1j%xrn6-mU!pSwTKJ7L#n6q?Em$D?FKUw!`7NH*vs}
zx?OO+iEOkt3yza+lvx|@Xf_O~0K$vzR2IIU<B4Dv>Y2Q2LOIe=aRf98y@VlTZ^5xd
zM2udRJMKL=+9_KVVO#-<zU#4ji6VFLxmv4{=;%Xz_1i*{<thZ#p)HT}mqBYOICb*P
z0$9*$9T~|=i%*;IgdC5O%_IZ(^@6-NoQp;yrhgxiepXQ<iB~?$E2rci%WxZje=TF>
z6#e|_*`g5v@{p<BylBI>&apz;wsw#NF<AG=ijlQnR!9sH_Dpq<DIJ<J%o^j5<#{nc
zNYM@Yp6A%-Zu_mHA<J(l+EISx8=Q7)D1W;4MV|w}dCoe$Y^+hzazGLmi_nU+{yPYs
z^X@~dAgx)EW)KgBhnvdonLLDtMY-c+VzK4}i@o02ff8}e$9w-ZO$sq_;@LhS>CZ}_
zi0omYrEEr>q*z|LR9>(xDE3}s%rY5d{&Ac%ap*66FJob>-LmiOaHd!wOAX0Sn?-Bh
z_S8kz1J2XYpSB+!*Zq*h8?hYG+qpPx<uAf(&(-5DH239He^5q724u4htn^graqNDj
zd3D3_miEr_eq2fy;0akc$oavp#3uH$=}AuWgSc4~q$Gtqp)20MKB5kz<87|I*yyCi
zf-KV+Dj(-!##h~Eoy<-kIjW5w;ZKv-@(nhjg@JF0;UcBgYU<H3yK(Nw*0Z9kqhm`^
zlvtuwLm-$Ts?Fz9=;{VZ3xGPQ_1VeNo~B9AWaRUK(z$Mv9h}!yt(JM`kAXD;OiR++
zQ4b=32w2dTER-J(d(64q?yuO%wXaqr!0pNvl)%V3R7C4jL5diVr(5aPquU~wXkV0U
zP<+rA?ihb*C1${Qan$O?WFpR3+=V3qrk{<IFb|6bP!Y<~`ihUzQ*Xw6b(LGxCZv_(
zZq}+Etc4&{symuG{lZ_7T^1ZjY0Y8~N7csdh(6i?G0>yqZJen}dGsAOJC*B>$ya8C
zQUn>Pzpu*W7`A{e5c^Y>=Z;7cl}+KBAN)`aam<6<F_Oy)3hMt@QfD|*7BOGA9|32!
z3-MSLbLDxz$Z)g!*p`WKo9$Lb*Qo8_2RFRilZ&zPPRkKgj0~=%pF%`9d^-FgiDb|-
zP~i^%hdlQsd#2|<k+@&ou2*fKL#h6qUZ4>9zA<&OEC>jy3KpL##odC3=ViYz{-De_
zQ;%q=ARATreVmUw&hu%}O?Y_WoA!?Gu~b6WxuD1u*#$nAHWb8UHS5bC#Jl{oX@=#d
zX>?P%zg0unfCxxG==Ii-cAX8Op~A@I3Gvc6xt?Gn(R)5J=JkQ&N;23G{}2y-aN0nA
zyF327(S7thwH~XqE8_d&0RUiIWR^2CXBNeCv&HWgQ;Q$Uax*J47pW+w{4{YONoq%%
z&JjJ%7HAl9o}n=mv%7n`Y{p9lQ-r+MqYGsBkmahluQdP$ImRg5_T}{FT7B78ejr}Q
z%^O~21~O%)a4ABlUy^-lq*E3NHy0qwpzw4$<0Kcn5l9xNX!?i}dR`Dit;8Ti&(9z(
zNLj}i)|TPdLWB5BqcylA1fxhZX<_jxElkWH<#rb#Q*-WL5J{;mtoyR>Rg2)!RL0tO
zf}o<CD(=LyE>@te^LTAZMCONbxHV?ZpSd=EzDPqd$@ATP%g%qijS)(d5#*o|dLj9}
z)waQmZ<edA0a!ER`~O^;@;@$3Mrt=|$(Jo7%w9H2bU+WD8~pE<`I*0w;C^zF(a5Kg
zNgQ$ZboCA|l{W8<X_2C3(ps|Td;j#)W~q1*Tmmk5Xn{{sI4U1d3z4T9X$Q8G(F8t0
zj_4p0`A<6MC5%i)NREp+#l4SD#)mVx|4uQLB5AT^1Aj6;yqDEf_=Rszo(>9Xco;72
zm^@>ODGyh`1~ogmzKie*GIAqC8e{7M6yTM(`Tl@qvBu}lznd82P~AZrLoR+|P}|bm
zI6+(ZkLt`F_RkDUlZ}7b>;1Rl{|N2mccUH{p_Vr`M#qw964nmZh-GK{nwVRT>|ZfL
zvk3K9T3(wvRtd#kFLgmjA!Oz<$t&}eU8P~3bRliKZ4harWbx+iaHwW8x4l3q*B@@~
zwM~K_%HEdrgbo&4!%u$&KO-$(w1aK+(hQi)y-xJcwG^W~tuim%4@_)B=vNOo-w$Ic
z+PAGsR&XW_j%EgaGS>TJs`}gOq26wETptA(HP0vd6IgNZtd79vrZU)SExf~d{Gbp}
zmq2zCnn}9c>)zJd$JZ0ibB~8FRE<Ddw`NCLho^)GOH(SJeyX4Q*;H&S_LnNnZ8$$2
z!k<?TF|7W6-d_K0xy<%h7!_qJLp41=PE7p&V@~?I)`p!$L61oZgZspY{$U~wju~LF
zbN^!8Sfk1qV<Nzp#==p21I^<_l)$Qlb0SQqmfUS)#eE#~kUU+nRM}a#v`8JY)RxwW
zPMe#X3oV{qt=TEZQcf;YZ!lZi2)G4i7CQ}o<tqFQfJ-+4KRasm4a-LSO8>}>vu4U_
z3LuaVhiXihi4ojIGuK#NjkM}K6lY~MRd83D{TxB&DAAfxK{DdPo|)BoWbQk}*N|U&
zyg{#T9aQ}dk5@Fseov6Z#v4PxXBCn&o1JGn2ae*%GfNkIvwTP!W}a-p!+xBx{b@m@
zO#VP|7~kBh(GAIi)OpOU2D#edj^As+i)K%Bl_#N+o5SC{d(^;%@!7N=U46}McE*y@
z%j6O>9ZWc-XDQdm6rQcqsm_?*d^AZbo%vGg;v&fVZ^APf^=*^ZCbBJ26|vXjX&uk2
zV@Bud?O*}14d6&{^akU(Y8KC<5r0$xY)dQpyZCBuZm8iMH7ke>t)dW@tC%Z9FQtWH
zmd=?)B2cPQ*um(==1J6|4lr|C!#rwz=;A}Pu>bgPTS<7+%XIvUwy_)LaP()WfRfPh
zB?Z>1Uk}0P6yd@grG3?8oSc>5>8~D*+H1JitB67#Qey$@-6zWit^znEArD3&TW`2N
zAD-MhN-2;CSo~`xApPgox-h4ya)q6Yf8Y%LzIx)O*4h35zIMu%^cc>)obryFSknXA
zcIycclmGAK31_@Te?V^r59O6s&#MOOME?7mKlbO9i+<L+7_rdDhM!RJbzWOb`8XXZ
zK6ogxj%<pFD}|CSJX|7Cz8@w_X-Jnv`$%SXYd1!dHM3F1E!QWoq5Bi8x{=cMQ{%`A
z&~$W4gr9s#?s5NQqP`}=9%A#9N!;nd?S?KLv!HPVm4drM%Gb-{q!$D4YJ#IQ_PtM2
ze`aW%39A}}$&&Yi#j3p-*XR|Cj@Wsm*<1`ZehIO;I?Ll0YmE4UD}!Euzm!%P(fczo
z7rFg5{8`jh>0O$@>JEN`=k;@4yy&`JLm4;1k+elv;t)Mi#t;71_{c=jF{LbW1AdvB
zo=154hr7!E37f*|(EWvYV+UfwX65(sI1g-I)p|}YU%N{npi8sC?R@6<@}yU|8?ddC
z2PGODJcUai+V0ep{L(4tg++Z2evuMqS*D6;vjFEGUIkJt7#d!xGv-BZ&F~AV&>oSZ
zq=u~o`D2du?!a0qqy;a`HWK9Yf$_!?_dsyrZEvm64q@p-&%FIex1cz4UP?gq5@1Ba
zl~wiEgh8js>NWWcrUgGlvf-<m{#Q5o-W>ig$Q^4GlU{3LkBavlsZy6_6=#=wXY)Ms
z_@v%=UpC<bmDcB9akGyL{@Pr6_Nxgh=e|(G?BUM~OQiPP%dxd|W_+xGy%e+px0cEO
zgba3*-v1y6H!Vl$UItLlu@{*D6VN!j3oT<|-~EQq1r~6V2=0+PV`F1gmHD?T@(14j
z*&M!|ACSRg6JI!8h!MClNH9Iti@w7W9%D!qW(f^L2h-X=wKwDm4&V6nr@u#@eE>)E
zJC^cdmUXL08|wLT@_Z$<NmSCz)YQ<IDk)5u)ZuUJpt1s)%~@OBQS7RZwkY^v``?nT
z><X(_miok7^!>h5uim`bTp4iejb-K4W&7oMW?gfK5oG42RRY#2&_cks?y(5tY*44;
zWF(4;xH^;0V!-SOgX@pgbS>4Kit^XLq?Nbmt+E!U92IfKVqdmVQN&C3FR+pZhMXqM
z>l#fuo?=B&JEZb{p2%OmPSOv8s)6^s9=nm@t4ST^zr0lTa9(1cDwOfod+pei3dY{O
zkip$u%i7;APf~GJ`=AM7QJ!*p(<tN=YT#fD;y=Q;E&9REJPIzabT7U<8}%e{!Y~g%
zeZ5`mDe~=Q)O$--fvf7Ei8qxV-rhy+M<XLXg}|}QHjhxZkN6I3vo8|2o3(3Z#r`{Z
zI^L&k*4S@<HzPwC^zp9Xnx|WI;ik7!>78$bkJza+=!&YKC%tH?eyjWE;0{Uri@+!n
zZ|Zh04nOn!GyknBZ;p?nBMJ&m4j*8xIIr6Yaw`2vrw!FV$Ja*(90hJkh;g0#d7Gu4
zM?w1NW}ob^;U?0S)_4D$@WZ%UvPJ+i!?Cg#QpY~1s`>2I9jK#nn8>+b%-)r#6Eq!G
z;1igSz~#6!P4IGKQim<<!-CVhq<GRt30f)uJ9!d8_TAUK;14X}PyGA2>lGg!;VkcJ
zErT8nXhEc7Nj7`-GiDqnA(iL(2%5E(r`_!6NRAh$o;vp<P8<5hNlijOx7)Gaa|m>)
zJDRiD=F1cwIo#U-S?AGNh6MADGrI0_J9iBY<A`opSe3_r3><F3%-PC(XA_nlhCQsC
z5O@9(zy2?*OH0Jh|EX!1wb3|2A#?lF9npC{F3RH=JsB3E*lpYDSzfMnvYJSyn_|Br
zHn}0jGaQ}S-@ss?kDZ!BONjpYw^st@rsluO6@N23a+Q;FEh1oqQd`crZIAb%mfw}2
z9@QF5l+0w8!m+<DgU-dOLeC89kRFc>t$zrRw5nE29Nzv?V|0&*LGeV<m=a`fieBKC
zPrZe9qTlw3-c+w!WGOj!FRcqMsGy3S<@vQ#u?V&9Eu318KbkzY)V=By+_87n47gca
zYR@$tWHbq6mZnoT9O0*yK{~H@hR}wYh3=LTf^@6N*p}h(H88S{2RFN=1I7f^>V!5C
zk*+sSIv-Z8Su<pLn}S5*sVH}5itd4B-p8>@UBKg)4to)fcgvy7nF_(>=6oI<$HXqX
zeuQWCo4lacIA*a4yrI-PUh!y>?HkS`9$A!q6lG;TXw0gdM)i-`9pj&lUvZ<_rdB6P
zBA?a%Jfu_xzQk*!o(lufVE!hEU7B#bw=ik221kh3m0;!+G-=Z1i)PO)n$yQl?VVM4
zzjlP$?i0?X7tX!UNrg%W+|3}VnHC}M@eyvB*vsQ24PR_4_Qeuwg6WUI{W8<x0MSe?
z90MzPGZU)`?<Q`@Zm&eEt;{KI?I2eV0%$Gm5K7%cP&NqRJ<Yz`-?sHO+U;$Y1v~Ov
zu#B>W>hA`c<v+5@J2YdNS;}`_j#m~q{-fZZb3moV|0<H3VH5u9K$i~F(FFR1yDSIj
z*6-itQy?rBpC=<ezyL};$1iV8N>F?5?&#Vo6gT{AN{h*eXlLhXhB&VZhGI;-A`TvC
z@rkSSyi^iuPMD0RuEi*Lysfhx4dn$>X|YhWBa{8z=t)nL{j7KEp6oDEbos?nQCiB?
z4(2+Z?!&fH&=1ghX7~E@A(PV*qRI=s`w9lzSqE_Qq;86SIbHgabS1{Xhu1aq>*v-5
z>uAMdin|xP_-e!-sc52KN+13l+F(FIVNvytl5}v=o^dSKv24NmS<k<z0ndH;?Vr64
zuLBE9%qf9;Kq7RLrl$V}HX@xMKo8Rcu+wrMVJt`kch~oc8N;;;{F4jWwts4xZ)EVJ
zbI#gWdJ-IOjElRX)U^qFB|8PnrGrPb)O{LF7NL)PZ4}1Csogrl%NMi>U$r>N=tp>h
zWVn6rO+ONsY(@<38KE)<^;Um3v8lgTL`u)#;38G`Sdi30c-k!!Hpj*Ky#uG04WI+h
zR7i)c$44KTsYma7rpy;u^)!e=#nOD_<VPd4o2IpNRNwQ4ax*02QtOe|1iK%T+IA~*
zHHmjOLUyhRdJn_2E!!gejk}f)EEeS?-|RHMpyTC%e$Bg7Y|BU@>EqBa0*ARj{Nl|K
z&iKGFHGauD_03`A)g)jhBMaq8eP8IjzXRix+VYvRtBM1At02MilY+KO;CaPU#?=g_
zjG-Rjq$c^{#l0L~4OC5eXr9>1>FALI--heCLAFP!1KihCmxk`e%PS$u-F)}(L>BCf
ztfxbDwfZRt^PM3FG~6+(D1-k~Y64#4x6I*y?v&NvT%NA|B1l8w0H^}dZ-wO=cD>`u
z`_Q8C2&kdQ!zoRI=sQD7+rnqcI-R!eXlb{5)(RKcyC#nP*j*3%Hq6o-_(g$-RqT)H
zwg_(ztjQJzf7->~+3^$Qqb2I#r9eV%S>?3c4KDCYdyq!0eIUN%6f;J47Vk2w=PYR#
z<~Wo%tijuXe{8q?td7K>k_w<P*9r`q<`bv0%=Jb99W_k{o{%j(t_p<pmMC%MRUtG-
z7CmbavRg{F7&^2lUQtH{{Xx3xr)Tt-azw7uv!VOii}=w0LJic^Ns70Zy>gjc$xx-M
z$_7!ZlHto+rf6tdL%N3GcY#^jtz=lSTCWQ3sIF_V6zE<7kuI)&u}#ZC8}l$5??^Tt
z$ga)Km+iPyWqO5t@z}GHF)aHl1pbJyVO#XwaD6&M?0u(9s-G$lHK-n+JDsS%zT$BI
z##l^qe=JaI{*2*4;dYMh<T9~K$?s+T*RDpGkTXwQ^kSUTrw`ERx`J7)`QryWbVSb$
zc}1-rmi_X&|2#KM+i~XF04Xbl{9&wCoSqYVK3{k1dcpKZo~3x(Z-!wktXQGtXeYG7
z&-l7rF0zSTwAB|5Coq(zUG~SL!_JEzT8z9AF65Ot$Cs4To#Z(KJbcfs9$8I#HrDp#
z&bB(lWJtDIEA*tg3slO~MJAXIIGuq6eN%Nr@KFQS&sv@Y2e7ip?XUh>i0OEc4<>Y|
zt^|nF=g50=Y{ElZ#wf>gKRW%~IiY*Ec}LZuUuuoj=1W7Wi^-vSsqKw1>UZ7Qy%)3y
zSUcXvR)vF~;cQWE&C|C9@oL83ww#trC=B8cGEo7hO*8f{Ha*ba_pt5QT1AXXk>$nU
z$auAO$VML0OR~17G`>aL1vrv1<u=Lq$r{{R{*2K+$B!87C5CC70c|@XQs+mH7BeHf
ztlkE+m<U+Yy=WnxjORIBU9mBb8d5!dZDoS&1NJA^dyhI>vEs+M&NSgmXSM9_YHi&+
zHC&%0gT5?%TK)H<{Ljw>wmbaLFs7sYlG~f!<W9Lc;|$i2o#W<pvFSV>SPx6F>5`K5
znX$F{2d+k+dv!PV|I`+@nvZbvDla`9+ggr!Je+*lrn_)Wud`KCMBF8ZDQ$|kyn^mV
z9E(tRKLbA(d)U=0`R26wex72*oiW0wI}1wly=8F<jkAQP;q()-D0fTLVDL#!xy^T9
za}dglxg5CjBS^M5&#RB;NIxP}rr|MjJc*=ZxJObA?QtfL8?iWGBT1H>0DL>vO<lL2
z<rYsx7uqsax@6<8S>mCRLnX`r(HXmH=Z>b}yAt~Y0<2C?OQ`C;F+OnuFEF^$C%3wd
z=5<!RDYT=4zd(`qtEa`}Dy?Tn&OR*|*Xr{?TNWf}!GT!WLKDL0B9-XJe&tK?e<b!M
zye|&|0*Acp#6}-1PGkqR)h>^q2+0{2rVERIV#zaaS|R^FG^y|ZFjtBa*6h=V2&Kka
zacQfGkI%kb%bzEU;I^--f9;IFrT)ZPv9Q)$E5&V9n`B!Zfm$46g=Lt<Eqy%{i&Z$I
zyUsn1qmi#z>$mM~h$6HCjktTnmk(Sid|{I>x)c+4p0xs%CxIm2x$J)}H_B_ge|X(z
z?L9MKp)mEMbm}dS{foZljs?AQ4*ioyl`pONmZ*lwZ3j$GcJfM-0073fA2<@<T<QQj
z4p{1JL4hEt6j$^Lm^g<*L<_n<{ogW{NWa^3m3R{&&W3tK!8=+Rmt%i9Cf6)DOCI)-
zuYR#5rINyib{LC(jdjQ1lGkvu^lJ6U5^t_=_d<F@!@jEbNbI}1P~(qZ;)v5OdwOJT
z2c9whsos*BPRypYPv_aWXr0$Hh6*z}?;R{nJ$&zhq8xs(6hHi}EBi;lg&8d&jb|t5
z$2-4EI&R!0U$#F!@pz<YGVr=6#b@HC3Q4M-e3wAkkWwF8Pg&hSSa@PCDC^xS(&{0Q
zKx7)kP&;#QlhHtHf6v{Q+<(1yT6p`_?{w_mp_BaBIJ1R3_2wBWcb6S=iPG}Dn#fG=
z+o5${OKR$%aZ7boFDZ2T%z6au%)eY5K$NijAn!j<sGv1+X%lk+D;d>GL?hq6z}ySV
zvAgA;xo7-jf=ArLE23KBa&_^`j<>g1lVk9uSJ1u3XEj9X+MZ5yn7I1pb(EG|a`uza
z4xu=O*O_}WS}@Sp(q4J*TCY4r&=lg0hcz8GoJ~JmGrPHy=J~EYYIB&X^fEd1{cF|j
z5?BGuByQ*HVa!^4OfVxX`0ncg;w%~a=kl$<OuMJLG%hUso-plNo2eeOEo`%q^~Pjr
z7}=<+&YFMzL+OU>u)6NxWOZ|Nz~6vKL}yYP$bXqfe2Rm};!W27^a-rf0RALIytG?E
zB%1kAC#jJxQo90Hvb|~$xMY_vHw~!o<$nTSa-)o3<7NN1RAs-oA3<fmpShQ#HU6QR
z$wXECSsND_2>^9soY)?dLKZ9_jxhniF*vw-8WF1tEEM+D=ceRrtF4qaXVy2A<u=a~
z+E<Y&qs`j9I>XJlJQd>X)SU#7uWV-+B{t%*T3Zeoj4)hss_Ua?*9x8+$biU%L9+!e
zt8wBA%X4xC<dhi<0gCfd$cB6KVYeA5J%5WclcE{zdYjvuGkI(lRvX{myCWt2uG7p=
zc9oRDQ741DrYk;L%u&>c!8i327}RRR*R0@tdqsHP^~X)Xq}XFanw@lwn_mpN6tad$
zJ}gK53{fOTA4u88CM&rczv}Ws!t;oYf$0p6FBW}UCV$X#mB5SsH^<>2ZweF02sS6P
zdGkMW>T<=ipsAfDG`D8$^i^9JvIfS)b9M4q%omrl16hk?l+SCF$g`Ky&svEjl=?#T
zpTj!GEGo0_-x+1J8VU}c)LEuzk^cNz8mQu`sXHFB4N|7ZUgf<EO(~UEX-zA7C1<i7
z=-RQReVL{L)YCrgr1(PClNa?X_u(X++n{%p^uxkZw;06!r^i}{_qNwuw?4mn9<x_^
zqyeyx=WAl(7FXhxUUY>wsQ&oua)LguL`WJ;A>wNek9-Y6=&rf-wm#K%rpvN<&taM5
z%B=Rpg=x!8w-6^>CXCK~wVk*S&CdRr%C+LM{@`96-YJwW!{T5xKoeMu5YRPoRX_S4
z;%cvgmBeoJbog~Th=Iv-MC6)G3RzA*A*dh;H41;TEffYs{In^Ygxr-Gnm_U7<=U<9
z`x;IKXO+>BSzG1fjZ60Ys%{-JWejR_wlPb!_S|vlNp2?wj3bDOo1?y@_MesZ)Gt_@
z7$d>BfQUp;M8OzIe?V(HFpb9%5Q-cVxWP%n<Hft_J&xy7b(Fdz9hOaSRclJcrEP9G
z<F0eT_G5?MxRTD-DkbQLP#?`>5W*{HeNrb<hw<C?<6|Fpjh?Ussy<(=Q|*xDNdSvE
zcRy9NJlim!v$DQyM~I)8{1yWNmu-q8jr*CQ8rS+2iv`V%w5$#9NF1}Ue9s@eO5h~B
z6p+sI2Stjz<%(@c;h8`_Q@yy~?5gBUvTj#3X>41x_;bhIOhI>b_PASQDjh5Aw*3~&
z9~<FR?(eaq!3gaWH({mSjo~qm!py*sa~QbL<22Aj(Z0fUw`yY8#I_g6$IzvmvP;UV
z@lJd|9cW4vutvWOv4ceosBh?)S*Zd5zY|ll1|Gb-dFt{SHI}0L^NyBWUlh<5*82j^
z#bh4$YY^Ds<oYb)dHhQC?o-G6Q_YZGo(L<kO+XlJCh&6YA`iXoaMPjZi=DT*etMhd
z`QU?;+Wb~_Gri1%<&jSk-FzKL%^ebNW6{#F0)0z<Lsvy(aIW!lH@Z5<aGXN7F`3Hh
zfEz1yQrMkcwSL89CB(xK^S{d)7ul|jD}iBDSthkf|K+WTf77I6grE>_yHTf89MF!b
zcujbHCf)y)AT{gbcVry5vxCX->0Q-?>Y6_9r|t`@F9XBhF%3Vx$xX>h=^i7{z{Vkj
zkThQ7BobL^<tvLtqrWC6>tp9X098#;%mvzcQe@g!Ks!>#mQhVF4n~!JBa$7!Tpu-o
zHGs;IA$$>bFXyU|XIMnzrk}4P{$45D9rZAvV9gYKdIp!kkmE#onyl!h=GbGW=O;Nt
z;;fSU+K{)iXx4*k2)+7-q#*-m_od}q{;j+bORepXgwEwSvJ8fj`|mkcx;c!jLv$V=
zzYr%57oDQ9i>Zqc@u#vXIq&e2vFr7sqbm8KSv2eJx()6}ohBloa5f=BO~C~#DY8+n
zCvgQY_3_z{Y#M$S#2o?0xS<2wWPJ-#eyOxIm|N|QV`EFmi8-EKQ;$`z?d|=?yIfa^
zG+$hJi|R2gaPnH#P~FZBJ7^y(zskyP?yy;zv<=1czvQS(TR2e1R->fryfMOy9~GR^
z**VSNc`EI+4V+4Q{(#s9POLomQW&Y-BYHomJTs42ixMApQVnqBYkZ1@sqR<I_yXX2
z1*lW;C;lmq1+j1I*k{Q~$zWQODyp8U*>*6+)E{$fiA^Pa?u(i{v9I}ZmG^UMKYF}o
zl-?2EJmrbCVU2!hHBS}jPQ&V)Ic5cYwJU1)kuT_J(UcOeZ%~BgzU9YbW}QEltYwfl
z+X^eo@;4$!wUUHPs7*~$6mI;F#tHy9y$NZob&e;59R%1ZM4g(thJaoPe25Zl|0xNq
z+~o<A9mL?)o<MK&3=0fsgmcEMtz<LD9QViGb<akf#5Sv#?-O~?prrB@wz}$PfgVMK
z&ind?^ek&vu1-!YI}T~2dzLc$18hMS3u{IOz`EPX2Q6v~ONaWZoP7;Nme0rvKJC!j
zYb1k7u?6-^@odB>BTEtI@<5|<2Pt_hY!&gOPr2{OIB4nj%de!3`7=`c1Mp}$j$pM%
zNm@)qAtm}gI{YE(;dNHUyT83IS@nG%HhJ7leYAWdV5yGpd0M!+XiLkRAxZKl@CWIR
zE=zYY=XQ0JgAoHdlszFF?yeunXG2uC<>OFqYE*8Kc*pQIirt+&f9k+JY<9K;QD|we
zM(juoN?3SpU7BZb1lsI&9ankj`I;~B?D}SwT?8gwTU>KmJo+-2*Yrz=MT`j9`1M6;
z9Q(`iWh4FZ7V2OS0%t!(@w&9P7sp|%Hqf}zww3T}D_(pA{=#il*me!s=f2^T_KA^V
zlm`~|6qacE_tWj;k9BsOLw+>AzXl+)t4-{&Ux`^hb2G=Pv|UF1ND~LtQBpYhJB`d#
zTIADMCW%}j+UNW4bW=34;DNT0!9A9l4NQA71q^m+uBR`YB}=WytAZiMo*f2PuPis_
zE=wPZRozor;F6A1mc`2l4+bB8OIec@ed9&{>$)xGy7b*z?qd_n$|MEAEebMfXvqO%
z9jnITnLTGpf_mncgyyev=r{kbC=TeQ@5S!s!!cq*3*Mh;<$J{EwQhbk9V$Lda8{{$
zbLpQHsdz6d+QqW{{R7J{0wru7Z_LQu45{OPo#SJP5r<ZHJk=i~&d-pAoN^IkY-dhm
z==nLV&#g*ND-X&%1>!~BxBLGOQEwR*WgD%3KO!h8UDAy-qI5_%2rAu3H%JUAAzjir
zNQ30iF+&a=Lk?Zi-6i$j?)TXHKOZ?}zHqN=T`PX)dE)?<q%sDgs%&AhMv@DMk>Pb3
z8~%QSvoWtPQKB?M{9}C@PWpt^z#0Ts5CRf3AcDHj`D@^#=$%J2wI5Ei7Mk3I9%PdO
z8KdGN&@3zX=uB#b-NXr^h0GabF4)?!TjAb!$v+^ujqB7R{~2Qjtx_cTg+vZ_xFI+w
zy?xrQ5N=ANzp)Xzm66zJThw^jQj=LW2jkQ^(VU#yjhcSlk$tOjlO*lQo4wgN3uO+D
z&KiBt)xE2?Q|-o+{31>M6U3dIiI$s3>F-3Yy%rW1?Whc!aT|Kq$slvyf>x5KTe$OE
zXoiHN2=#cR%pa~9x|0XJTWA4kkpgL`J~jJ;wn|s!`yOw5y4b=BZhDHY*A)A&cPmtd
zrk-`4S}U>`gfdbs+j-@jJ!W4}gzpQrX70SVIq=dyXfE8)uz{WkY&plaD6VHK8@pKY
zxe4B$jP!^I1t@2bZW`ws7?#5{_|HXTUW3g82q-LtZG`ZiU8`*}Ml_l-T%3u^mv6ge
zye;EB*wzdU;*&^!enPapY{J)JzsWmjm?R^#reCgU?BjKz&0~pyh7e)1A}d~+oivWn
z@ee+GDp2~&^#4NT5tD?!jSTe}{VrsX%N&gznstiGz*tN%|6V!adRob&ekP<1FD(q0
zrmcL-?N`L{TCcm6;zJ0DG`c0>N>tn<>~~?I#TS8mbVg#p_w3}1*4I=^N6kS{IIrBb
z0EPhW6g=r^@Ehhph2MvLfAoaL#Y6*c(}7qyI2BC#8xSA+u{AD|wVm>G>ais=B3Sl|
zNu>w@=CHGXVnau^oC&k22Pe4ErGvb?G5v<(BbZ=cL))YC%i*$8m2yoWdRyUrA_P%C
z=2IdOxK7k&XJ2wCaGBmIC2DF+2vYyb$rCoVtx*O!MjOUaXBCKloC&4I>9xI6JpVgs
zu@M@U$?fnhZLWY)K(x!gWzF=qqRmUee}cIWHL-8F|4{64Vi7;F5_{Axhj4KjC~-?+
zW!u=>D$sPFE_dT(AG1G&%?~rTfgDVy2s2BSTKxSgGZ@okq-KGSJgA665Ft>B6N7Ta
z^W?;V#Cjj2LyjaK=ea;Sel?U1Oc&x5brNu(l6ppnSQSLEd?ST_>88Z*5{i<qug3HT
zXSmh6EXhgQzwg{mAF_<J*`r9|Gb-2SU7q(+D~6==2LA%!kA>kaE`E?0-N4ZI0?CrX
zIE-4u%5qI-oXr|t+`vX7`r4uD=xV2GF~&UN^TA1_<?K^?$1jz%SDCqb*}xt1oUGWM
zOvvCMeIur+>Tl36xo1?iG@Ao};{Wew=X#Y%+yGTCMRti6y#blix^)hc`HX5k`nqi)
zGFdy}zyBQS6lEko&!TxsK~wZ~)TFvBa)j(>t2e292y(emEk{G9ps@m{3(pYc>x}y2
z^+X}@)H#jdUB@{M_|Za)Sa<E~1k7RX-TE+v&?vGVyj4E>K{okOW6Pf<Mq{Qk!=Og=
z_rP}#Fy<;QJ0|HcK@WG9y^bf&lK}p=g)NXM{7ZKmWmGAKeGyh+{(~nic@=F-nC?dh
zG>nn(WCDFSpHY|GCVsL;?d>T-#b5St-hd6gY_ZIHDVzCmmYa#f>qz5Kqq)pg8iI9T
ze_$5axO}LA5mNZ53wbOhsoi7ZA=MbWXeEYqvU{idaA=~hd}4QNS=JvMNM!n+lg=TH
zTrRfQ<W$3K6J0x79H%-D(g$F?sy7m@XzK+sCP0k~6JDEGRLrDeafy4jnsLH005CbR
zM?r1|HLH&qvwW`I=%_CC){NC**UJ$AWt;skNY?BtXXD{9<?Ne0R_99{x*hdXfjG~!
zS?5F(ryj>%g-D48;}a&%2Py=#>(KG<3CWQAV!Z`pz_R3cm~hPY5k26`VYif#dV7AX
zo{s{mOlEv*^~H*y@zl2`&b^ksikg|t)O2?*AZyT}WJ0t`LtXouZ>Y9=$6G6`DVU>c
z>b;b?b+{UiO#C&U<~(U|<NW_S5*sD50^h}5P;UpvES7m$AJbpzvz+~Jn4DM(l5ik7
zb;iDPL~`n19Q+M@gsR*+nRny2Vhy|8MsJ-mly-Jk0P!$lF185`zaPS{T6V)jCRMo8
zZc83lN#sPawo*Qo&aBI7;6$KW!j3=>%{iHhoUGp0%X=wb0<-wM<zz%GWJj5ojc;9L
z<~DCoFP-<b;CK$}rM;M+99vl1DnSqMk%CuazqcAcx8a#e6P^CbkrWolC-4dcJxlw9
zPnBw-Ce<|AFvn&F?a3?3u_Op@FM34bTYrabr>kwPdzW^D&-6%v+Bu`Jwe<;Q)T7r&
z!V%16!w`;+C0(^h5uKgsUwP}Ko)cvVXok028k473N2hd6Uc8^%=4Bn9TB3@ZELMIM
zdCB@1lC>%N_{Ze#N=TKo0WtJYobU1T_8&0hLumDW<h5I6bix2chQfZ$=&9&TOOY!i
znM*-B1bQ4{m~<3(jm=NTMGw5*MpfTR>X}>Jkb=ztrKFH3i#9RETj3Z5OvSMO6sf)F
zSU^{y9VAdYp>~m&_{b%|kA~eN_~%0Ctc=1utOrv>QP#dvcI@l93~OE@iv|7*{tA;A
zPG5G{IN7=Acv$LeOW`iXpqH#YIh3@?;xo$3*=XyJ*^~9Gij2CYy-v+s@)~lBB3CPj
zDQtaMo0>Z2#WkUPezoGg?5<elOFsN`&-kQFiT_gth`xB;h4x&$Oa3*5>-ll7H-9VE
z$aKHZuX8qZr!V8AD79O)xo5rM_w4wPXHHGtkVUfqA@Mwi>o1Lt_zH|X>^W?+wT67s
zonXjcM0gDw#JIk(VWoD^#c?TeRpr<(>h?;BqLZ{vRp(cwKoepHc}5*kO&7UnoT|84
z3nR4%6jJx{>kUNietZD^L>?QN3;e3q=%X^X*=aClLh75eX_q&CN$8pXJl4RN_hAwJ
zzghJqcCJ4=@AvH_#2g8h_rK_{iH7;fg+#rqf5lf1?GC1$O<^UUP?ybrfey$W(w~j%
zMh0yNPg>kGVVx&3k2tneKUcRLwK(FW?6Xu)I{>LR#<VLm7*;T7R=ahs;Gf{3)BL=k
z|IA8_ug7;GSE1!${9mTv>#tT!h+w@Cfl*xG4R`I;>J)(|nJgE36|yGG8yDXoC`t)d
zPU!Y@jC}GxoqJtasat@#z)Twl@<ng1-E(s<zD-I+>d`DaeA^tYBL@B|X9If6N^UI^
z&&e+gw*+V8PIAkf%6HiT=bVu<ynk$7k7w^kFpe>K5K3l1dM-t)gj&gert_=u$?bJI
zM~7T~9X0ucd2DI)^=ik-<T=hbE(sdKB6TtU`i*X8Y27pA{|!pb#{%PQJshi|<w@xE
zTuv7aow<gja!(A#Dfp(t9BD9yGhb7PPmcu9p7J_G9WJ)W{*pi+-Y4!$(MNvIMXt@I
zq@=i_oSW&j=cJtlslJimik!}0QrY|O=vdt@-BV^3UskkQq{llksXLFA2X|69Zl)z`
z&{x}@B}DeG$hdDh?>;AwF}nw(<Y>&}0SftvWk5k+`XI^bNwUXIH|ATMdDG?2$w8bw
zXMfT%F9>X@OA&(5r9Ho8Z+ie5H~%l+=3)J71V$=atH-w&p5MN0ikB#UMb=SetK!Cr
zsCyVXgn{FarbFI5s_h_*qb^64*g})JWEf0FJ~_VGm4V0skB-_zeX~2k$b=b(I<1{h
z#7Ur~Qj|q#Z}o-l*i~iBbokBRl7wwXPWKN@4#X6D!bGo@Y6gGWomPt}HYaf0-)~c3
zkMk0FCTvh_Jyhxhx)ba2Fw<>*q|U@nn8y~Hyj6j0o1{7xt**%YcbltoDz4fd|KfPl
z5WbbVkzGPG3!j@dPY%!$XWA8Q*zc)cS_?MoqjN}YXrrdq4aQ+?4TpIyp0<|6-<^|e
z_9nO*4*$PLM;na0k1vU*!r#02#el<;G<)bY;D^*&UY!;1^^cA$#CGGm0WsS<ikHzH
z<sg4vr*25A1rC{Qrf8M9yIrHBEJy6B#UK`c;l{Ez!T8chftrroP%Cp+EGvDqAvcn*
zh?0aU5EMQdv0zQr5{*+I?UnY`v0PxEjmJ&`G4ZY&c+AiI{Cx*?)K82&WHcPs7d1(O
z%S?5t0#ehz;S1X`oj=RON7s02go+_vXHH@grgs38&~q}?tPyCB@$g?lE41RqnLj<E
z2+U!JbJw_7A+g4l*9#ZurO8{9AFbDCzklip^;KgR|3SaK0uR+cLZI+%ORw|ZHUTTf
zb%;;NXYrtXqmImS)$$Jq^CYQWGt10K5l*%#H_G@If#i_P^OD5KdV?sI8`(j`P|&GT
zTlg|z(oCe-wV+K~c&YlEM$!fO!pnTh-p;}rlY<t}XZq_w`^}b`4<@e`g2tw_fpAeZ
z<J<6_zz2mnjp-fb^KfQ%A~heSK>0eS!bBO$yYmyQq3&&l_OoYt{32dnU(t>%wsL$5
zuG0*s`tSSYvVUscX8-@|^M6mxd<;c$v#i(qx|e`15~@Gx*E7QL>aT*yveLP?ApKLI
z3^$KJ{44^thO7uLKsjIUsdb<TESzp&r#}eKBTeIUotG#pJ+va*e|TMNd>7<SoR&-)
zjzckr&S6@PM=``t72KyF8VGEHzI?+rvi=h{SFV^nn{|?MMpXZ!XJ7o>{sg7A??3s;
zmX$5ht;J4YLs+~ZGKn<|G)K3ImfO@Tdr6CNa*j*u=>|697F?2E3kC<|^@_6mso{?D
z?9zzcM^*w;!Y(T6B6URv6h7*e_M~2f-&A1rV%Dkh&U)Py!$Hk%B;Hh^QIq0_*YKnF
za7RrAHZ509scW2?YO1<jr9lRGC`2v6DvjHUyLOFk(ON@K;!*uwVm@Q@2wtX8w@*2T
zJzw4>-Jk&D(yY{tpO$?ZNSjDK>cr@)xHT!oJjJv7HxSdlH+*&PRrSA04q_wTv0V5V
z+s~hzpm)NCeH=&-J)Kf{{5;0|jF?IPt27Fe4}hba4^sn!+#Y|=HK7FHkgO<m)i4<e
z?lA;{u*^wkm+oH@z&aK`{C+^Z5z67j1kmMxU9!jMOvUW)C0>#tR2mm3wQ9Itzsr`c
zi5I#n)C0_5XqF`06vXo&7b+G6yk50$)rCOa<OK%_ci}C~XAXu@KRxcZMbzj)zodU>
z(HV08OIP3XZEh9c+E0`(dIEx+H~?>G92pRUY`$sEDZjr2pL8W}eR}K-&8&*Jq*h@F
z@BElV&v@dWuuRc_kB+uQ!=CeVelNO1)bKTT=(v+tL=G!I>bKX=Q138KAG9H$W)ypZ
zv6bPfFr~k42{f8DrKt_@6#PMlWKX3t@jrT^V2rsDw>P$;X93{Gm6VO;alANvXdg?|
z2v#QOkxsB!Y$0<Wzvqd5E=QhnzTfgcR8P3?(KNXkEF*`AJ(3TMhv?>=R_IKB+stxJ
zvhd`X1M+%@8DhJA^IYbOXBOAs26=n%#dvGm|Nh_m^035T=?*(3_2~$`BR&A7S$)y@
z?g0$GuoX>xjM0ZP5SD;A!13{Ugpr9O?fKQ>h8B>OfM%11(5KgsAzsFWIqUenCaCYy
zgd{CyUiix`G-4*zZ$wttq?eDjvIE<z%~rFv6oP0-Rb9C}^{v=<lf<J%XY{v3{!^e}
zfBojEJlU^hlm3o<76z;>KFV7;`g5NOzUs!Cb90T05$3!OM)+0qW!6mi4pMfJC*DO8
zr%gmE7D!UI=-;~QuJX>&0G@#o3G-rUr{z87tv}tUuWFkl)XI6n$9CsQ!e$Z8y<ri4
z62A-E6_lw(+cDa+>cpBKg*H9m;?GN0G6p3STdEH)-hh|9_EJP91@6VExUc5NUI|pW
z7GO&iFig<T)4$-WREtNW?sUwsE#PhrLmyHG*Iy_Kg-E%1#l4judvO=FRQ;jfc~t))
z^$ZPSPB~7FFAmk7Smv8n04KS{&Z^?-VjfUayGVv#IYknGTH@WaVGt;1cznG0-x9ur
zaMoQ-2bvtsoB2ZWq{3G`py~0>ep7tnnmf;n_t=3TD?*o*s8WUD-aAm}J<WInJ84Zb
z(SxjsKUZFPAiOv^v&Eg=jJIM1p#thGN-_<%CqbnMltHPz65HP36-e#|u&{I6Z2{FF
zo{@*|Kvt^H;ZEv}cm|vy=^ie7CNu=nG{YZj6Ti2i&yIN+mdWI^^{hV7xs@YU0Ncpq
zimd;GiIGzu0q)l8QIjLu!^I-`UZ6#XA!oed^-A=_0SqmDYi2Og$w;}}9XL5OOF}<N
zz#fJlX{(#nsS>{X2_BgiLojWlFGaN;7{i*`>p|wd>aSgc&leVjvwt7X==7;lw)~-R
zc5EelS|sw2<}u|Rx6JN-vU!2;(hb#0#eS5S`FmBNi>IGvhqC3%dbhr(PKswib>wD8
zPViU+<FTY7e(bbnD2kAkjoYiYM(|nJ-?OzvLS>n!U1`$sxMJIrQ}qM`iPo6*zfO!2
z{uR_zxsa+9V01p2VATiFyltKTzgwGuUyGFpb01amWYbst>61CybXjuTYxY0%NT|~R
zDCc0OS2>S+d}?uV(J>+WlP~N{*->tKOdjQ2ZZMPKj7Gt?q_FTu_QmwrHc)vtRzl2|
zIPGXp6+_5@qk}!uer$ChROTq0imt4lPIRrL#8K!M00Pq2#$y*leuCAG2@!zs74zD%
zWTKm4WIcpjlB5YaCHy#%GkL>#&NLJ1`g!41!Z#E*bdqp<MCv3|&JKLX_EZj)yRD8z
z+ZyuDX%{md0>9xmu${{DH(W)tjC&Scv^&Yv*y-P$N)=#p)B!X@pGwgzrR?nHV@Heb
z`oN?Ibz9o(q3Jk33cn;<DR`^vgm;8&uaTRH;ZiU)6g~66r}t9dlwfKm)6=sGQCa2|
z=Z-1myJcvh`(w<=?`x;GfDs%WFFzHb=fJ{wtP>s-kh<Y8SYO4iu2so;g>A$i%*&j(
z@%)rp4pmIYUPAjfDa!et((-l!W}$)d9pYo`XIGiwmB@DW1obr1+#Z`&-h`#OAs4(W
zm@=wch~r<gy4jKEGQu6}*?7xP<p*hdqVJ9H2A_<U*29ijke-n0!QIFF`Ofa=9F?LU
z5AJtj)e&g$R-e3gUVNqhQl0#B9*e;wjZejDF#aGY4~su<rYPvlS=Gl~#$Wqf;MyBN
znPhQ$(|nXj%2)XK3*K4rwqQX+N*?qBbQa$uhpzSd+LWNgKYK@tU;mXdL5d<jjYjpi
zQ__sLJbVl|nl&ytY>1gfi?Q9oMLC~mZtJ`8HSe)T%`v(>Vz;Fd^n@((`B|T~=vZCZ
z8X@ZI>Z0BGN(obD-a37AVC}x1Zm2DE)!FD|<~d)Mr!cfK!>oEGF}iSa-1t)RBo%u3
z^(i)jOnSP+Ccr*n?xS{~uA+hlbY$;UHUi0mL2t==$~e?z9`-12&&*Bo(A;<G#jq!p
znNl0|c%S?z`UzBa#%un=8NBYY03leJbg6ZN9LSksfN7Y`u%%EvOI4xQWCrN6Nsr&D
z13~8!B*=_+e1Z@FG1gQ?akDzg2RMU~L@vw*T`Sj~ZKK&2Sv`^4E}Vbxc2B;GOTB*A
zj{I*gy6??#2>Mr>eX7VHfrv{3hmaZmb72&YHJJ3V-Att*++*}_Heix%+n8M^>co%7
z?putXb{Y-?q<!+s&rcFm=LJk*CrwNVa_@6f+9$yQ@!u@yq+E{eIj#&eWl{^=!wTpq
zFS+5~&Mcspg9K|?9|xW7d9G%w2^{}3N$e(KqF5i*T&8P4OxR<xGTE#UQFv#0rYb#b
zul;07!%f4bNm^ci`-;3(hOVhC&MSCURT?)67B(SH^XCi&V#lwlNfYAsDldTYt#S20
zLrQqWu;*~+unUyJfyqOqoEe#7>&AC%;A__uZAWMxFPv1wSwZM9(wMA&T~NSPcpPR=
z^y^`9A80s0TYVL&VPSnAq%`pS<>wAv!>BxFCtz)_i&QiDdq!UF9ap(l0jq4Ccc#&x
zA-I0@mVIw?k;`(7MJ~UuhC}nau3p!Bc|24ON+v1b@(p1@LPsNjFvBNlW4%JFZLc<9
z6(RNG;%Tg_EC0R8SGiP^7f#AU<L9N+<bI^AWqZ7pXCh66(kQOs`(~e?>$&)m*321o
zIMAzyTlTf43zFHR)M^#8{6@|6H&|f*IfCj*Bx|~5Csj-U3X0PgXIwh{l-v_T<^m?9
zyN=(ZIVn_N<B?#*IjeksW&ahNSc#)?|1sx12Xeyk^1n+9y6-($gC^raXec2mU+V5h
z!sYTLt~J5s(0UVr_?`%pu_)2=FdT}|k|oxU5?i%K=GFbSQo(?rrzj#7_u<?A?vHw;
zgj&jn*-g(Q*%;I{h{#pq)@ka-!on7+;US=xN7D0ne%oBO%Oo}sKU(x9``toWBJeez
zT*~gXDRyYrV&YpK89MuJxma`4ZjB7#Fu2`0#-=))YZh6%oV(;;CaVe`scQ(Mr|w!+
zsxjf1LIHDoRpl@a$jSki&9*(FbWlxEfcj19<4x|Y?1`;AW)CW=kLbzLJsbXlf?Fyu
zelZ%mmna=EP&Xz>NIjYI6K6@Lr6kQ2yN9=p$t^Q@jn=*~sW}tee6H#94t4%H_k^I|
z&e~7nn=LvH(eoA&uJw2q1SNlEoS@Mh-IWpJd<X9!Xi&SLu|hXq)dDIpwclz{aHegR
z^Ty>yeskO`r|*fygHOfaS<>-KBs|={E8`hOp|waIF27lJ@qNW?j7=F)Ra{qUk7J$r
zq^81_Y{;JNA>5!ZBR><ojwwe{MaOaLI7nFN`H&c&Pvs?#y{+ALJGZ)F1W+&dp!*L8
zX?Fdd_5ZRVulwGVn}K9gzb#f7?3WE!WdF;fJ)`}#HHbBR4npFXi%om=`46c;*<kS>
z^CxKacU|argQs9x^gme`P&^<5GQ+mqvm&_b7AqXa_qO;x{JZKJuB}w=#lclj#6_?w
z2?kyXER&3VtmQJ|55;`1Z`Kq=c(Fd2Pi3{8G|`Hjy*r-^Pa3MF_=(m>isOjtR)Q6z
z@F}F<%q?LFNu2ApX3PATg$hg_+GDLC?^dOChUQ1$G@qRI$iBLu)76dt;@oi}QJGcw
z9u6s=uX9=Uu3E84aMJQJoQ!4L+FcvJ-bpE{@Mlm~gK{)(7Yqfr<4Jj+Cx(^Ju5T`c
zNu6&D5Y&M=7+6M!AAUO+9qq=BF5aWB336@!(~nXGC?17y#wENjbj|L%1MJ$izXpCh
z;G)+yPkV*i>lEqd&zRm#fwMOvb!@$voM5Daz90M1r%lHt{;GUx%SIAxe`fY9>xTt&
zW8&hA0(^nAhpV?Xf&(jaAehe=Lsd1Cu|Ce<gz`^M7PHt4;qO~BNRU;QPe%Kv0Deyx
z<G|6g%6zIEQjyq`53!jINmH41cTs+iTD8ruZB>nNHjI_#$z4|%b`C0i<tBX8CqnJ{
zZwfCe1RDGD(3W8lGAG=-V$ACaILyOn56>0_hTgcmTaYhr<dt&E5c*BPo%|QX>%`{K
z9j|Z6H1SeKbJY)(j%W?0_l^my-x?{FvZe?*kIUrVP>2poDvntC)%(Jbv^89;tnD3@
z+nR>icAt_-n~3beIF_m2-zWx*St#1<m|JW?j<NKk{DA@0)q=<19YZC1pEG!C&OJ&8
z-!FRBDh<eqCLKv{KDANOp?3$UgGGg|q}F0Zjs)%J>uiqYHqMt{i5zrPk^_seI56HE
ze$?P*zZsv%Sq=JA`fJ(1+q{3_rv{^T3Y6Kw3e<(v&3pfME1QHb+m&~8VEOic!q3bG
zbvhhKh&Mf;zJm)Y9#(G~2?~uIR(hH9@lQ@ffCv%^r`fJ@g8_T4PDY-WwhIDNy-#wp
zhOso(4*{GWR^N)EP;H0wnA>i!fVd-k%oGw+B)92!5tFDg?DCr*Qr(s^wNlBrx)3P4
zK0Vzl!MgJ8u(BhrPR;8_GFIpD+PK2IdbQLPjpEG(=8;#G2Tj5Ng_Ydw(`TF|;dNkR
zIjI^OUnsDcZzdDh@*nPAL;Ij6As3d%-VV0o;m)AncT(37XUbIcVfE-%p6ds9kK@x`
zHtn2`lxCybOIauIdJfUrSt0!cGmq{CUbHl;nJ-{A{sLD{_ddIr_yR9g$VV!l5hiA9
zWmFe1u4r*iGB9r`W#^aR((>y6UDY3kc6}T!mMT#C($$(Y>P9hSIUXG=E}R%MAzG3I
zj!$=#xWO-NZ<#BmI`90`zsS`>*V!0}(pI}3zONg6#tK+nrZC$RHjYk{HDFL^k*mj4
zhV|uP^v+8rI1Yy%sH3>O@nWthzK5LgmpXh(9-2u`ED+CB1iT&!83|Cvd3qIYFXd&7
z|7KfxEwYjrgq5^px{~XX$^2e7?2q|#;;B1pE{F-0Sfa&}kT)XS|1#1@w!@85tIY2S
znfvwS?~G?Tkhk>e8XC5W)w2I;hVZCZ??a9*H-F7IZ5MGTEH(e52gLv`gO7nEdFV7>
zDwk(yHh$Reg*n1x%RqCc?H_ITBN^R~kq1{+=8KPUEGt7K_x@!_&~K6&7OWGLp`+Du
zBjLN?j%b`jhC#MI?WtwyMU}k}XUQ7m)nnbf?-v=vyzta3F*E7;X+nmxt9zZsdj(sX
z2l)sAWhQLqK;02O7P(o2*z@`$j__(8uuz(Ws_!W&F#nbt-V`tIGgEjIYONa}a4M}*
z+q7hi;3|}`-fmC3xki_P?pC><>4c048=mQdr=VK}y0`1|fGbF&=a%>$Kx%2X@toGI
zc0m9(CniHf$LZYMqLd)Fc|yvh!a4!aBjnI2$F<Vl%onf+&)H5c9EJ~_dq?!T+`8O-
zX~tdHdbh-LB)S`qj5J;A{#yY|ItHX)rzU*!TFfef1KcURqM)NKIVmhsOh5o|fd9*&
zNvK4K>j;gnAxMWL{&1PUG=VICv4LyP7@>+vi4czRl|j^~LolouHkT6AoK=+vJNIwD
z>Eiw7bIi70!GVWTITeh=pPhVXE^r{o916Vlm%(gfk(SK+i7s{%!0eI#;WZsQGzZ;n
zd8PZ%2m2LIZ@B%p;tT3HMIAMhSI38P)dXpTZFx|cL7&~CwE@i>rzbr$%YI6D>%iay
z#jHw5Z84)>b;U*b=C$Z{_^8g_cL1eHn++!`oM9k3X4{07ri)+*JVbM3y)yN#<Jehn
z1w8+@#kjcUN<zg<+v+ig@qVWBY(n(`feiKwl}!G_5OmWfpTVRz0UlMHFJX@;0)D)4
za8om2zw&}Rnzv7$gEiG6_oJ6m009a|*^kr$>%r~wL)&hKN(W-E`gvGvrpEh=pB-6~
zf9q}iapjWmd#Hcf-<+%swUArjbSc{DN_@GC9nUfD<4ZAaoniVAfS2ah3+!5KWk4Ff
zIjZDl-KWzOLw0^~V{ZYcT6a-@wTT~`dh<Gn9>G6U;9LY7OH=w+bny^$)mZ246dGB$
zn1`J<q58|j4$;qttwLQ}u3rt*lJ>9&OMl%S+x>~jiFXGy+Q&Kyv6v1i<aN;qJ6&$3
zYOJ^%J*c04<oUt887WWA5rP@~V~~YlYIpucPWQnY{trs(K|(H!wn=^gPwhMC;>#Ys
zSn0<`KaZof2)<y67zv5l=1Y~@@uK$BO?}T@kY%V@=}?_@mmMf`_wBc+^-VVizLD5O
zOEm4@s!rdgn7KR~T^`TAGz%~rf$!G0`UY*2Hc5<vs;M)h#Ej!*XI-&Mrxk>|IBB?^
zZ&Qlf%b;5ZCi9su^~@I(;GpZPl4;85^CPF|>k?y~n82SXBE-VZj1$`84D`GC0#0qq
z+*QJ8ahLb!l9ZQifZy0Ofzi$bB8k-#kYPAXzKOI_1@%HW-Ts2w_7XF~Mhb{)HoC6j
zsLR1Ef0+;J^UOA#X+5vHIyD>Lq44>An4MmN#}>=Y9zv{xUF6byd&@<r6M2+OEdAGn
zTD6~t7w@Z?A>bCDG^c)wEUHs(JA>iZvlYE8h2r>Wd8n5Bz@T(kU{aT9<Ea)-UAsCe
z0?px}Noja_c;XJp`+avGpbtts!-kkP4AAg4zx($7>nkgB@(2Bvh5>>16ithELd#~!
z3HwT&!aeCVCuDH0Q9mQdMhk0T`ZkTnz1qPdMVL%DEu*hsyBx+aH*4$HHIL9F3H5S?
zLvil8g0=SyxN)69rJsr&k1)Gs+oHp2ePB@z4Re+yMt5p5f3=y_!HekJ0>It+3}9!)
znRUsuWYXDHR<fmAKa1x)K~kQyLL4+?c-Ez}d4eN=K?Yx5@BAPmT?Y+ED4=h93FSBU
zheEo<QK<8VV1wsLE831)M9qo5X%{a8g$}kEsoS1DXWsUgvP_k4c~o%cR@gX+&f6TH
zC$D^}(HZEn%>sy-Mcv!>@V_#roE0=Q(w8VwJejOU(X1ko!|GNF?%WTZ^gNTrId4w|
z#%YPfSz*$?7HEmi;z2+hCs(oSnkZ_j5zzKNF2W_+vzkpMA{sG5#Hl-r@TqF`ZCjR-
z`*zy7XL1@T7D(1~f9VFF?8|UP5MSwo;>2f^NaX-8l$fH}hl1uyr(0x$P^ZPAVY5I~
zUJbEj#jm?lq@h@D;fi%;Vyovw`D(e#_Ga4(VD0m)Y8&TOl7&1yzxvEAiY<6)X&P|;
zwj*(_L8UmNi)sYVLg#PX&JGE=5DzT?-J~yXb(BpF2w_rvCKxgh17d<liza>Q$M|-M
z819`5Nk7K)Lf%i{XQT;Xo7U?!KcGQL>mn}=EHnfh9;7Eiy*SsX_*KjMh%7?PY2jV#
zdDknuq^NOUdb}GRzZ{^nwW1qgVMN4|jkY%G`*WI9?4FzTV3EM=93}+|u%W5=C?^Wa
zdNlKy9yj$<Ts%y4^yrx{q`1%YmgN<qu5Y+|B5Bdt!g(aLW3^+G&?pcFmOrXriJs4y
ze0XA^s=Z*cndLYMNEk_<qNbkyZXMRN9ottBO^^oYpYbUYnwA+~cdZWs4tIee@MV=e
z2L1|eq?UHHs`Sy^QXzj|Agc_<@UTO<e#IMpN_Pd(F#!!~*72@wa7}eFuiIm@S<a8b
z85e&(>bdX>(O`0ljVGMVTQjkr%2ifvvgL+qB;T)TQJwMxlZ=L1kvb9J{q*A9#)<ui
z%9>z~#Q+TyzRi1?gGNzApx{?&H=8!#4_6=kZA7H!{gpSJlGH_Pc4+d{Wv0i0?G3Hj
z-c)IvEIPb;35M+XS8$U5jKy0}sry$Q<k03I8i`43mt;4Ud?6oMDH}Etl#vWcYE18J
z<|ypu+@cMKJr5)4r7li24YT%sv=O8c4_UWUTK~a%$|}7jEv*t*!PIMCN&6w)bLM0(
zvjdWHXj1~!88td6D-R74!1f!69;8gPIP|N*U5=p5n^1^3+nzZ$J1x)SK$6ojOZ}4Y
z^bv!5mcQ<TPeIZXUO(|c^QJwf^FD#>U^8Dngc1!7PgK44NjrsQem4H~S_S2t&2}Xb
zpI_dk4H+!kFW6;iX1#1@M_whHq#qH3mnNaL@GAMt%5^$qtvign_;$toPu!%o`@IXl
zwvdJw227h}*ZuCz6a1k};&}Qkn6JTaMWfB>KrS8^*svSzeHKXMP`P>l=DRz}Dmh%u
zoNDpOECxSCTF74e_=gDIMT{*nOn7WA+vH^Wl>4cTy<eMHg(GX-?&4ZmXDx?yE!S1!
zQl_<U8#vmxQr?<n59EZ0(UJW1v@6?4bx2;zS<pyXxWKLX8{&4RUk^6-pbk)cBpbP7
z<2=4qA2p~{H8_I!0m}7b)g}(ldCR_m`CL3BGj*ag2EpJ|G$~JBehkGZ+Hdi*A7j0G
zE?8<xV`Vi2-Z44UWt{13vL_f|N6|Y-ruAp`gsxbQds#rDnoV;~07k%vq(hfg{+p<Z
zy_X6}T`k@-DYNx5v;D)^H9biq=+h}!e}4|?qVO0l%(2+0<UO8*<|+E$9PLM^OhE4>
z&Z5A-d{nTabc?Ht7xNvj37s=b(z`(C*|RwJ5WK&zZwG<ZhkqJ!eZ-Ery;PnQNb1Zd
z8UO;)t|wAiH%=WFmvr$t<;qpkzjB70iHEQr8=?6)S`;PU<JkZE<y>{pjLnYCP%XMo
z8-tebx8p*M1XWaAFUZ#bMzy4l=p8+qU>=kMoQb=1A;a<#N|a}}lDb~8c>%Ly#G5gy
z3#VL?a8hl-Hly+;L*djTc32+~cpU0>3VnlSXJr4vEb+tcrPtR_mT!Pd>F=G7=7Ohj
zQim@fwkaD5d^Gh<E&^%W!;|&w0n?Zr^)zAkz1+$V*u^U8t}Q+lSg<2s0mGlrI&T1)
z_YF!(Aql~TQ+sMO6X-g(8#hNyX@7M)Ed^E+;SiJ@yH7&m;Gs<&XJ_NLWqG)0mV~IC
z$P5-!l_l4_aMtthDNm}38VEA$@rK!=NU_<I+%tzwzQIqSI23Xvzm$N=-YvIjoW4?9
z6cC(e6zzppC6VP_-g|3!xemQj^jQzQX_ya8<)gr6`*FCn+{{Ry<0``;GB&~p$S>l?
z?=k9+Q^;lElYL2Elb^dy2-39QW6Uck-jtPlW(PyJ{2~>Z@4MvChfg%JGj;ksNeyge
z)y^MsA$e8$mO{&{SlqrGsOHkA61E2~@OxRWx!n|-g>&fL=ie28axDHhnRx%(g8QL~
zhT{kJWm6nXh;w^#ZxfPAo45Wskdop&sFu1212(cV$?Bc}(V&*`Pj~Kr1RM?5m-XPp
z1-GukPLFjiY~kLlY#Ncr#rqN@Z~|g(_5G)2-OkK=jpK6!fqLHVA8Q{c#_qWq66MOG
znn5)@Df(2(56uI$t$dPm_xUsCZGsqB5;(dyv5jkV-vn_Ih2UkpB4y`iCJr*74_c=4
zI88?DdxR_Zc)zdUz>2cEml<2qIG}HBrOi&sZMuf)sE_6+w}#`459qdwdTMFJ67AFV
znDJw6CBnURvr9J8JIwfQr{)C;vC*JbaIWt&^!9Rr&=lf@WppZCbkbynNBdVDcY@e_
z*+jF$ThB$Os8i;lx9jmxa=Q=%3zZX217(_zR(om}w5Ww2XK@C|y4a5;A=Ew+gcQx+
zdHXzBj-S>9SFLW21vm6UECyL5L~m#9gFdb!MP5Njxm}k{#ThmQmO%fO@_xvd6R~re
zfY%I2&0pW1Y~S0Nlu3SkO&5@r38lHtS-Y0Z?w}O9;andtn{DGXaw!g+V<M36+?<69
z@Y6t#1}w0<#J{QD<h6lOAjrjQ_)~<YzhA;zkX$SOcolk`eK<yE(GR<VYd+`DSr0a}
z2Ujo$Wla|7#AKJsltalx?quuUG6W!4;jV`%awc;3TRV*t?~Wt=sC@vhVDoB|yTe9f
z1}WOmDu7@~g3Yl2LkyWJ(1NOf#UK&$xSo;kzixpAvp3D~{ML8Txt>m!vi*gm{9yae
z*NC2as1rMlz$WQNz84IVD@T|QoEYefnF&z@@9F+uV^ueD{#MX*_}Ja$+uCXY2PRiA
zFOAuz(UJ58X<)PBg2Vd}9}gKoTR`p51uAv4d>?dDZ#;Hi#d|8oVm$fIx8t)OoV;k}
z(B{uI-3)$$$k4|dXmEK6$SA(!l?;zmNVAc$1{rXhDD{rISB_|WjrCHn@4w)X4Br`G
ze(a6X<OeEUN-BnA0d2<6ty`uF5i{pSALq7MF%pZR$on3ltu<7Trg#)Z^3nb~Ykq!g
z&qbe;6hOFIxoY`mO~Z+nOC8M7Nz#{oxv-7K2W-uc>ELqVwVUML?}3nKz%<VG@d5ID
zm8U_X4`luK;wQqYVX=QFU{o;`P2iuUgT|9z;#ot4_QuiVMVy15Dq!6;GymQYsHuWu
zL1JC*X;#N?Ctcd+uew5_+~gh4^mPjv-+2m!bdAlMA+meQw#;?Ce)7CgL<Dh}X$8<n
z)p683X_Tk!H((up*WOM~fc0@=b#Rci(;=0!oD;*Bih<IrXNZ*d78_#r-*R#I#Kp_q
znmQ&%kQu2N0|ixCOmYD&H~r_sJYN-W?n*1gy{<0ds2i`iHMEo>t^(6fy{r)Gbzm<i
zt>N3Eb4ojK7mH=T?GrJAfsWM3WGqphJ@#sW<f7t}nSS^D^^mj>ULkRelS9bvWkTT>
zVcqFNV02^OzdFk*^Qv;40%QlF(;#06m#C~x44J*jI%_o>Qkl%5rdIq;3re`4j<V5F
zG})s5YTtLuTtV!seuDEzxXEdD%pQyX;j_Cy&e?-}QgBH#14b#$D5gbyC{Z{;8VJ1Z
zH=IF!{#gO}8A-fPcVBP;?!NUS6T_$APFR+w&*Hig%OybKu|reD7jCZAE!G$=cY;@d
zIb&){Co}T_clLEVJq%$lcd(WA-?a!A5Fm>XW|v=Q-!Z=$8OrD3Gx4l2HmEBvkXJJD
z!-ZEWpIj&KV1MK%794>#BQP4Ur&>C~?2(1slpWR6y&IWX+K<{W{pZs%jwLG@Rr_t}
z$EK~X{N$*OqvIA6`%tO|X*h0{M#RjwMcGs8yQF}GWm9{z+?^%WG&6w}R(c*|wbueU
zNjfU$iwrpk24stcEWT-|n|B-quUSazr?;FL=&*7SwbLJ;Gdl1hDe5!xyw`uDI<Q0S
z9$V9I)}yZc6%RWXXs2%?|D$1Jr=V!#A84)$*P7<nsMau{f*s|Ciw@%BXU8>8EOphJ
zVAPwoC^GA`<(Ws|<_ltd4puZV>Sa7G<8_p)|8Txo!dlZ}(PNsAYI##LFC_ch$vOfJ
z>R!(|w<;;~qOn|oWShq$Q2Q?#O|HkNYCQLP;_Z)~SiyA(l6MG)<XMWwif%q}G11qN
z|Fe^&JKH@a-HNr%Nxf4a0e2b7R#~F{0cFfA8pQp#<dXdOXd8Ba#75!RTJdS%5-VhL
z88q|3exk-GhlkJeYG?8*;24myckRo{!*`4zVV`J@g4mEn>O4enWJqv%tR?pg9s!2V
zt76gD(lU)4^SMd6P1<Z{$>zK7br<jxd~Fhe@(HIZtRwn+kXOvAZk6|k5Eb}KRLziJ
zy}dxe7#5-9in?}vEv*7Ey!S0|?|{?AsvgH)KGRbC{#ja$fvX9~7h#g-ryK%y8T_r3
z5;?CW8V)VN-pOohBQIfp<MNjm_=6HrOJtpXfTB}5Adw^&6@sPyq$k!YCi>@led<Et
zwU<iwBRx;pDz(}3{%NyqKT!wIpUZvm4%~BIyfiFWGK(B|OgwxucoBiB=Jpeq15dXP
z>owIZJTezqv{-GII{PhZBw+4c8q{0OpMse7a#E}AJ-ZU&#74@eIJ=zlHK#GIT`@}+
zJyivb0+(}x2-2!IvPm7->-H!4oSyO4K=b0&cCNzeu<dM4iBOuKuMduK&H8z`w#J*r
z<1k<Mzz|~AA3}uju+WS@<LT7?aCPAP&vY?dDvH-qp73cH5zq0FA!Mm&QAAE*qoAJ@
z^GE&p?fuVQ?{gRaLOW@uWg67Zue&fZVl1y>Tk7_xet}4IzZtgMU!y31M%=IBi5RfK
z{UEruofKw8`*zYYXHqoo{lKWPY<TZ_-TaW)4lrD0?N}z#a1@%@R^N7DwSl78KEwK4
zcOAj3i|z&26AvHsR!cEatXQv7xj%)wUx?B89y#t7eCSyDH1y;dbcX*&ry!Eu8J8rG
z9Ge#zwN&3s*?i57zn`wjo>52DI3{|`xDsCTTXT|zabRmn4-gKR`8NT=66j|VuQC*M
z!;xcq!hm3T^;;zpn=;P+vC5YjH<<uDUUbYz&S|+OYe<BM`0yu%Sav=I=lAs9*4ocE
zC#y?mtmNa~S|=4(pypO!nn}|zBe}>KiJYDlG4!}^%4fe;>&sf*iy(cGC~#l9Y=`s_
zdZjb<^*NHtjFj09KlCr3?mbk!xlD&n=(pbum6;vu#B>>_k}T}whFidP(LhF0XP019
zudajhx6auvA1<Qzdm7wlGI*whlcexAKAB4ggLv={AJHM39Pe3d8(b1Vg%8?RI;&g{
zstc#`Y8Im%==~^o)U~hNXMQ7qiXjTN*`Dd~{y^|ZBq@BLo$4s0sF3c(^2bJdKlB-3
zAUi8_aO2Tu{`yKFP)Mfi%m~7NBFgyp!|p>*3l>F?0fgsG6=~~Fc6K116bJ4`(g%Om
z{Kp=(^zoChmB#fFqtRz}R4wgz3Dsu3U0E~OZxHfrYKqa&ad;Mr92<Ay53zm_F5QOv
zlqX0iQ0Nut>VS0|$m59#{J9Qtk-+|BwQH*MQ>ykOf;cMKi4b!E=AUZlVKd$i+cc7J
zn19#x{-5GC={fi}JH<cSS0C-<gm+o3BF_V@N()mWz0;LY#G5b*l@$)S%%!<lh=uW%
zbuOxU&TYvn5ZnxBTzz4fKTt#zYEeu(+w=KX(+^U<#D>^zsw|sFE>GErABEXc6DE;T
z_!wCmj2HL$q@QOnEe9zF5TtdY1EPYJI6y&g>gBI@+e#-LTE1aPMkw9n6q$*Emzr27
zusr0e)3>|*p=zlEjtiqq>AVqPvbb?S&{NyL!eUx~JK%S3i-+nf(?hOo!h<m^=O7~7
zwWZ{niGylJ%07u=>SJXND^7IWdBY?!mKY8L92ia*D(?Ky_6M@qGUPg`(VUwh+x>kF
z(v#4VMNMxX!iyk4x7PghXZfYrpXP+6z4s1g<52$3B39{NQLgXwu4W1wcsJL65{Yed
z#R<J{2Q6f+27&pK4nc#Jf+AxPUJw5AR?dC!A=kTTk!BWq)5UEngce*@l#nLz8&>l3
zKn`HW7Z9w&UFk~w^F9`|H)JL)RYPV_1~`h5V}@0+#U)?R6_y3M^b4R;$l){W^S15G
zgeA2``L#{!;Z88v{>fIpo9eballir|gzu88rIB@Aklf=<z<@gQ*0KzvN29&!o1bRF
z%WEpr1`Id-D#-5IT9D^c`O$R&3ZHovI%>J!S4ps&AbZpCQj+wyk`qB}DYDj|ndsXG
zn7N9I;rBdm<w#1tm{_*Kl*&7d{`v`M9{eOObR#u4A&p?+sRe;?1PU#(=^Bm-O=7pw
zrCO7W?&=bb8UB8c@ehA23}~01V;(>L_VC4onMm+lfTAJ)0LXY`JO4UH18p9z$X>6_
z<{8I*qnAMM*md+jYdfWWc0JX%Rn3|;CZk>V487jp4*^1I_-HUl|0DScO4u(EB~W}(
zd&@^wQLFM<QXfdWLS=g?kYviIu7ciuKA&Tkyq$WYthml_lzS7X&1uD(<CqX3cG1J0
z;lUrIJf0~u8isPYq%wIg7a(q(06Oi|ju!I~I(iZO$jq~ir%@k$&M;UBr^Y8am78>I
zy!jIqMwiGI_((SDHB;C3<_QoXcRMa8-*|U=lQPdK!kSV*-y<Z7=w{k%%EKPHy2Ehe
zT5X=M`DV>~RYJgNii5rqPTg)Sr*L&P9yj%+-+qtD{vX7plwmRe!RX5*X-IoGWcl4Z
zGL!1h;yGRDuCI{VB)IJ15*oq%J!O4ek7cwe(oVVpg-_<IWo;kL<u)l}QHSaVj%sb(
z8CRn3sCh6(ZtoxOo40+VJQxBHYd>mjwQUeBj5LWY@C+{n_dV8LYC#MQ(*LVKz@q=f
zNeOvYD|F83>jqret=$%w$m9WtPJ2eT`LzT=<z4WVQ1czLLDJpajzs?+#gT@VsC(yo
zJVZu!50|ec9p0t9CO63xh{lA^u+Y<c{wo>;>p1I;4mV<<jq%b{<1abHv=B7<#Wt}H
zeIbEoh@-1m7rjd6jA3dYn`uR5Ev*u*Kwf87@!&SQaQUlld;_ku_wk#nrB;$m#i;NQ
zK}m3(BCMDFHuT$53R<_z?XTJR8o{oq34Wl6b*{dkPP9q?Y+}iP)`x*-DUS80&=+zh
zGT|fk!`e(B5;o(QxE#Pz&P(xSQJ9JDl7^F)f20>)M8L_DI;yz2#SEQ5e2z_Cwxbfw
zdE~+>USAO<E%Qs%Xq+tc)Xy%bY+kKV?_f6LuS`zqIk#w;jh`1~0fCd}JB#R$lKDXC
zaPGu!F!E`%w4*(O_<R0GH4~2oq2CqcGsWRz9!%464*%xj-`{*HI@rPNl|9R?#{*~`
z4-dFIjo-fhiW$nkyT0T_*X&Wbiq8?VOqy3CtL9=lAnxVDOj$wZEbt2msN34ur2mp&
zp)cP#_C&5c83cfTLq~N2U&mQUc>#Ac_a>o|5zkWUctJlvbm53j(GP~xU$qrorWf1z
zm)kTVbM)!xN%o;Jj}AUE)l8`dHS<GG;g+zrC7shA&ME@PW;;69AUh<?H3|idE)4%j
zi**)nWBqcT1{i$5MY`d0&o^1&huwcT-<=Xg{=A98d{8xcQFo2Xxo?cWscok2LqYM@
zhvYEEWX4{ABIFQe27$4V16}@{7cIjwGx5a<$A)xpTx!!Hj<?J=)c|)H@U#MrL={x&
z7VF4jkAXjl<We6Dy>!aIocFujX6hYf3NKJRvgi_%=xu^8{}`zPV0ZwA6<wKm$N%>k
zH4WX)r278xaTI`cloDPaKQ(&Nn7A5VF5Sd=dLn-iD0O_x+=?GdNc&!zLXqGVc#pBe
zeK`2yd4x1dAoRm38cO&px+GLDn`bJ@|CzVApi$B?UcA<eeDh4|`-hU;EivP4a=UMy
zS{d2d$%TOXr)axXLH5FNwnzJQ)*V<F)>pW&<FEdr+mM(U-eeX#B5EAw<F(|tJ=~a5
z&yGWzT2}W8@viZ5W&il|lZUNtTkE+E>Tqdv&N92W(SFt5y?SL_;~;E9d4J^3U?J?4
zlR@fkr8_J~D<{CI0@_!#K@XPnD{5cq6;qp3y@EVAz!GMBLr1pFS~vq7sh*A=ZnxmQ
zCa@ZZxPy8hE2pw^klAvS_o0}W;ZoJNhZ@eDFCI(g*hqtx-Zo^4>&|Ia25e!=&FabH
z{FYBbg-nG0mstEhsi$zjR$nF}`_`*#b3we~UP;eB^;BZ$rK!R4^T*$HgqitjWR+oq
ziZhEoyi3&Ed4(|8%+@(4DOn-Y%*^25MXq4l<IzHgIG97C;@6B_*UP~AHlrd69;=*!
zWW%7O9}{YpaXtsfibX$dvUT+KAvRoYY?I1%Ei*~T7N6G@sct_N^de~b4HvDNYaQA&
z_j3!W1ngW?x?I(`mE(pK?Fyf{iMtUTxEI&X4?g&+b1HMEsM^g#jW=K=K6Fl_7~k|a
z)iiDB791qSN;RIjrHC0SwSO&HHcH|0;Q}jHG3KKn4uTt#3$JZQk5evQ)Uw#PfZbTT
zN$D(Bj#%>pikbTno)zDG#6+t?I)%8CEodI0iacN})RYUs1pzjbQ3oop)_$Yyk}vM1
zoyF~nMLx=weK#edd*QF4#cso+f(>U4%(L6O!D1+7h>L^6H$cYuzWZ)*TRfElyAgE0
z6}-RlXa{$04xsB0JY%{%X>M*teYGCyKlDXSjio?kC&e+PU3}#TH!J=m@6o(v`tjOG
z#0BgqGC6&vq}CGmdzeL=U~6c>c4WAoq^P;?o2U5D7o<qV<V3;=7L26&;Zs0%dzQn;
z;@iKdE=%c$oymLej<oyk2E%U;D?(FQI`8^yVO29n@9%LRo>&h0&oNS&=N{(}IceG1
zFn!%Q;;K9I4oZyH$C8<#-$!o`Dnc8tC1(CwJMCVL`YS{&D>i!e-l|A^HypU4RtRr%
zL0j-^s_}2WVW^rrI!)>e2@%>NoZYOK82Ibg%C(S_lx$2}Gw~f1@Ei5}E`h`By+Nzh
zu@d){!Y1q;Y3o;-O{pBu8R079<`1i4m&Y{w5;@?PT5ggE*W@T#f?wM(?&Hf{@qlK>
znId8yT($)c!_aN;$}ZJvFw7&m=uBAUzibO7GKm2PI?Y=Hk^1s`#^GG|X7HlEuJ&}W
zlEVGc<JGXR@2^HXn5Hg5{aS*(#uvn$vGG1_@NS0JqUM?`ZAvmsqRwC|WM%|FvOzCl
zcIRk(D+@_SkQsTbt#av>zmJ{STxot!R4an#@;0Tgpp>EK<oqnpoQ(G?3bV__K6aPF
zz99Fp>Z!lX_t^W0+@?Du9>1F7x4FHjs0nb$6sxCwUHsOl>qp(966r{A6iyN6q)JHw
zvc#BU6333O;vkhY$s;d7Z$I8<<D8Au$Uvv4bvXFYU&CgB@qBIp(!`>RgdR1fcs%Q|
zjg3&QsTszwA!re+N7fHbe85KPje2^<jVRg$V5EI&j%Ovc#~!j;_(gFrVsSmA&&3Sx
z{qZG^s|<MIUOaFXq!9OQ{l~=y2C0MFy5cjT&)1=068Uu<UQ<>w-tkI}qurBgjdMPG
zhm4bo%E$H#Yo}*odqx)DShHM?^z{&|-Rm3v=T79O*X8n8ldf`T!#1kfSS@ltcGq7M
z^BC*Q%W4N7W(k+p#Wzz#vaC`QS3ln$c*~hMsp@ew&@kEHuy8c**JcdLM<tsX&JCmX
z{{Pzh(zqnE_irt$aY~)3nVd><nik8wu~J;iW?Iy+#awO$wL%BU6%iG;Da&RoZE?-5
zMWj;H3`9X(D^pYo5>iAmQ$qw)QUpYvd!~N>|L^7V@_F&R;`2G@zR!JK=UmtKTF!M2
zVueD{wE%p>aTYcYxu@GalAx=I8T*8{CszCCNd7+Y3%{9zEOR*rAA37P)#uomsss=L
zkeY3-`sjcuG5rjnxRT_TM2o|h3Nn7U+@tY*X8miV+mqxyaAD3Z_5R@gr|42@pqKc>
zRu??y9FBmI{ysmO`>XQkYx}vwd*pH`rOjNtKB19wPF?Q=5f*OfHYbm)n3yIsYpO{<
z%A)n84>MjpHe%nIt~!3Xr&M<A6GV`7@+Zvg;2~{UG@|LKF@Znob9(S9M7UQXUPG<%
zrM%#&*g`0-Wb^#cmTGQ=3fagCIAk^V6NR8xSZ0iv@bNOsG{L@{cD5_CRdG&CGLtc5
zAo>c|kod3THMJYF390pprJ~vh2l+jUk!EP{iXb7j$(_2=+C0smdj_IgbkCfB0ZKD}
zug0$)Jqs-yp<Ne<Bd>P<z6kkCX@@84@Jy(T{_bJYX=SAU&{X(vk3+?eXE~_v`fIKn
z(n>03Nh~2mt$plc_H*}H3(Or|8t@ud)%%Yr4YMS-2~Sh)gTPaK@z6j6ynj)#d^6aB
zUB-!(Fg6=8%d6&}16*^fV`I19&^6-l@U(9WWt$nRY&)g<o33rIr8%XA532VUj4u_`
zE9Fy@dBPM;kX5h?bvm`iO4Bd4oEx603#y*yAwENLN;dQ;^$p79!lU^Mkk=_W{hHb{
zK$bjm?TFFgu7%7^J6nT}l9T!;6~Sp0vD;W|Mp?iqDI;A0!xOo6(>zj!mvl#G=L8x;
zETrdvXxrGBK={5rMnWv9%Evy@q#@)AhY^FT0g#o~kqb8u>qE|^rY`R#ztq!M3Ei{a
zByWQjJZCp2DP!sJCKJO!O^(dt;kVNvp{fvhxqKJOpSE#6OTk$s?{pVEz~C3S>5z%_
zR|gVX8o8H>CG~;p^TcX0l|Hw!8YjT#^%y*=;v(@Xr5d4TeVFe<APa)VfGBPB+Y`UG
z(kaxn9+jtDs~NM4aMX{yl*sD4p>2VLJl>v)%xh1tmdB1eFGVR~IbKQI*cV}$yw5`p
zwFjol;t|DV@PgTEzbcgN;l^<{Xt|x)Y2sny17-W=(>EsjlVjP5wi>dcxu{&cE`n+N
z{YOdpd=@BCOL3q*7t}D8-`oi6bHa6+_lUDcPU(AK-}^Hqf9Gr)t2oPE>An{0gZnix
zjabdi38z2fysWQ3Y27*gpli&7#{E#>zE<DruZaEM^swD_n??&x0aaDU^LLI_yg&h2
zmBqc}<edHkz4E7M_kd+&#=WAKb|~kjX=-rraEe2=r@1M!{PfUY8f0Mdt%$PM@1NmU
zNvdEI*NfHpTVXC|hhjr%p0Q5g9FF(SRsN_{g{xzbAwB!jaGmZI1Cet3Qqlq;9U^tS
zTxPa<4hXOc?vGWY{nfbR9n5iR2>fx(%86qMWs$YACReAWDi4$sD@D*Jt&2P3{Kp&T
zN6FUie#Ok%i+7ssc5diy=zU=mBw1H3UNXe1CH~L_v<p_&V&@b|sgG=$KYTT)|4}+b
zwDg|UFRehx?elDs7E`dp=@8bBSn3`o)xf$lE8MsVWfROF1piiUxTB*ZEpFvUz!e@C
z*nF-P^kc3kxcE|5c>WUPSCeOh0)?#nNJ+*>keO+x^%I7okk#@BRJBIpS-QNJ-YxNW
ze6mZg@Vu4$gjj5{4c*bN(_&yP-g=L3tsZ+<zH;KzfPzorx=kDp*bJ>=#VS11K34`u
z*f+U77|}ko6O6S@mLpP*r#@{R3930>-IWfhIY~JIunDv=NrLJ@OArMGpX5HunY@vZ
zT75URZRwLTp%zYi(cw4d`8ahimueW)7vo^#>gb4@go`?GX}U$SmeiNYwAN?l@0u!%
zN#~CiHT9L}iOOe&id0cfs(ZF}<&HANiOIg?sIb?~t?|XBGmZ>SzEoAre9Tx{DB_kb
zUL6$B_-MwdCPA!gVA=DZOB^1MHsB$H&NtpirB#>g@G^hM<xqe3Ppm5lLB~Q&8#n0h
z==Cta-%g^*0<wCNW9J-ANrICi1C;Y)j$FyaWC{WhK|$Uk1NXJgRMSs{Jv=Euh6h%%
zH`|MfgFn*AbDO9vH6;6DPN3gS@tvPjH*2Fgj$I*1mdAIu3`My3NdtGxsqn^z!gKp&
z?@WLpYhV8wZ}?%|r*MDKRy_GRTrA(_cSAyt@jatD)3zk6|GZ*3pkSfsHlkboS7s2d
zsd1O~WVqlk>l)EfaGba!IQvWw%jcagM(UrxkXtZo;tiNjN*pGm6w^Ink(G*8ROu-o
zXBR(2#(xjRV@&9#gCizZ9u#i$_`7bZxEmC)E<VaVV1)g$y4rU$XI=G<VCcb8CU_Q(
zpUJ%9E#FgxI<7Y@1uf)4$@ki^z+OR^YC(*ZEIa45VhJnjRwzEi=YhsDf)2N~e7R6?
z=lx}OY;9Bd1f7u}4=wiY$XV0h*LPw#De95W2AiX96I(cl#Ex$-4I+Ix+vJ0N1NZO5
zf4@bA10B1fsC`^OIgv`_BZds;pHbHhjzIclm!nu>XZIv?`+BG*hJKa=g&|U+;^P+(
zm`$;W*>E@F{AQc5w4jHP<plXc%BzxbY8tYO@5%R)PoMbd6Vx`ihVc}ijlQCk;+Pbb
zMb7>aGtnf*c;uA1DRv=#{kg!*0b#^6S!Ei;GEBU%Y%R=W;HYz37fN-Wl54Pl0zU?|
zrZ@|4*c#Br{MXQt>?YfC;sCHuI(12Tdu_=EGx_Cls;8?UVWd}npv)2d=6F<b3U4;7
z*J(-L+LAI@cis#p3LW;`UdxW-7W+Kq-@%LU^>AHXbomQWe)o#*b>(u=Lw6Lih|{To
z$CutS=_NQyk=`90B=6;8#ION}`<xRVqSQFn#;T%F>Rd0RteOj`C4YH33T8DVQg8xU
z8~sw_j)b8@2Ysp5UAK@E&l5W?S?3N|qZdv=mozEJL*3y)tuTL1GT|8Wx2BuFs19f!
z1M>7%@Y`cOW1FihsWNEY5}pODtDJn29M(7Ig)44z*Q0a<Mu-ix(Gz#FZqKQzR9nlL
zPphLOn@XZ%7Rd$7Zi(|>rjL`M6ZEvrKAUX_<DbplY2Tm{N}8n~oep73NS~lLm&y^s
zr0Zw=^P9_WpiwpJWL$*Y>S?BV8gp`dq=Ek^J@K|XL#@h(b+x&#n0PfSW7VsaA01zc
z+*R4S)xJDRlxyXdp9vcc4VlCoYw0qbV+VB<?=!uYKG0K;&sa#J2lO+U8h>J($B^@0
zdaXI}Rm*9Ua$gfR%%6J4pJ76=)jcw`1b-Wl;=vhM-Mbh2>3$}>3OY#wOhn@1Xv9ff
zX@7ZXX`ebec3vIcp!)MWo1+bKGLBgF*(=+xhFl$YBDuePjY)}Qn=j9DAvV+iyidN0
zo5&|{fiv+1Aw)A`y(Xe&-X)rDaTMF`^9Ox(?=9}&eo@TKYlmUVk?XGF9$e2+(NZjV
zVJ4-q2GA@^zVDQhD}S<?bdHODAfNMpsnp~wa28fP%BZdV6r{747@he3rh_<uqbweo
zlKkZogAlw;O5o>nt*dCSR&sg{Ar2SO2G29j=A?`di@g%~y_ca$j9zoQGH2Q7iOJsN
z6j`FJ>qv5YFLO<Z>?K*h1ZzM}dkSfv1HHK_#;mvxw9rXo{?Ed;Vqf-d&razcj<HL;
z0BJU&%XWNzByY*<**E8(NCTSizft!YW#oy7u@7Wfc)r)((WMHW_1L)V7t>5BAR5H;
zX%4Xc25S;8PnA9A2VlCG%dfgMBpvst*wz4ZfntOE$BHB%6T$bP3Vuyqkgvj%KS|2|
zC~xp|wS38PH?!37r8e^s%mdxj$>;OHmD@$3E}e7!>ZjmdA{5~|{!3=Y2Zg?iIKm~t
zc~b7ZAbd<NI|J;#>C>T`wYsZk=6Bsr#yjV&rjI3`=L!{wk)~@qyJAP8w!#34O!wq!
zRkY>8_Uh*(_?pK9I=zW5hSzp{EGD@CL%3u&rX(d(wl#NhZphPPNRs&HQYb@p5nJj5
zNUhi+%Lp)%|5$R=s-auO3@*vICv<a==D`%|lC3}1i@Wq9Yy(?@oX8~fcVo>th(evE
zATtj_fxFiaitMKp*(Q|-c#B2Psj-hgrTu~A|GiQ)0HWtI^0%8yY(O)hR(<&J0ZkiQ
zS?$(xaI%bujF=w=@zXlq>$LQ;d)?B!a<OM{a8OfAs^KjRT-hhXv``*ll8DkAzlInT
zXiq+RD1Mw;uy24$SIzI0-;?yC?&bxnZ80u6HH_4;`p-w1JM?qYA**?+M!4f@4s}0z
z1@WRZlZ#m<vjE*{{E~fBa;ZQpN)BQLsBp{6b^W4<c}$5(r{a6sa87TYA5$BB!35T{
z6+93aA98%egUT*CGX8Fv0T->N;)eT-Vqb0R+IiDx@5olv-(ckSC+Wx*&^Z@Jagva=
zmMVL;-|>|DR;t5ONs~IyN1&TM!>#0THL|2&qSt560Sl9w^0z5-%H*1YRP%YsrS{Z{
zE1QlMj98Wj)+CDz^NU9$_-AWPTT&;4TbOs9abpnM@{u@6ubo}FpkqQ=sQg`%+UIz9
z^8Wk88M*Voy<!n;Y@R>{Pm4~(wD-4af@Kifpm)g&(>UoOr^v8$@$Miy8;6woP?AnV
zocJL@&BN?l!jB&p_~B|8QX2{F(9b+6x?k@Cc~icjT)sz4TMnm$j0oz*k$xx?(()l2
zh6V=ofqh{kogqB;@b6yYX?>(4>W3@|x-i<wG!|`9@go{qVz3Z^rOUl93nzb5>h(-f
zx<)Q22PL{cY^c}IzfTJs^IjNl&E@P2>$pn}T<}zF#*-5m$)=#|S#WZ!gtg-ObSK$Q
z(ylt3%`AD{8cEc{W7Kz5AuZ21uKAof7ly4p^bG%e^v=AWLl!lcg$VkFi>4M$bsUXl
zL@U3Zl(la)FT}E8mFF*Ak#@<&B+%4`L0IQsNC&8@jJy%UxlIFVmog1OUfJ%<L%;Em
z8@*%quvFiBoi@z(I1t<)#SF{G6K`*LBB-%|NnbTRzAl?(SzX)Q+;!uLr`*JS?QAW~
zX91qS_RkUjBPQtDnmqIY+_P4l-doHbBVH%JH;h*rBw9cqc&qJiT3A>()oj@zz0cz7
zx9uDEI3nqe=>u~~dgQHa(fo1?M}DNFu`CoH@Q!4Z!kM1FShLQlo|KLbN)tltwU5K3
zne)30wExMf^b#-;zz<wuOTEcHFWX?nDPqoBVddKKrDHz@l3n!HoNjZ9j=pp+?oB|!
zxE@`&rVn&(p71_?Kfl)$T@zSIp?BRfiQqmKVGA5IzVns5i1=*AFjIa>xwvMHNpiGX
zM`U`_W|4DncJm}`S0@LPlAKZ!nzP<jn7Ldma;kynyGrI<66A2vF7Qq(#6QxW>J+B9
zO0X@Kw>zjUF)mS3GS4)wE8x_;z$2)TNv&6oY<_u5NZ?7Qc^Wp#@cZf{=lsXgw-K07
zVuad<AtAr`bK+k8!(O8l05a{vw8|9B710BRB`==IorJwb1pNHW%V%K%%3isPOW4Gg
zVrIFbePV6svSYifa&2g8y>O|hyb|>fgfA_9s7w3}UnNR49CqzXaJ(#%VWE?>;{H-a
zc<BP+K&dk>q?G9aO&0fRTGVSNppAWK#`lyIplSH3F~W*LK|=`UY>(RY@<*6ZB(ao(
zN~z;8P{zKMhJPS7H=W$AeF$n%?hO>=*IGxU8s^2%LHOFHy^L<9w_9d=>zPC=;$LXN
zu#P_Dap^&swL5CI_)aepcEIT6&S00P96>%_<PbT2AqLn8BU@-TIa^*H@1I-<V6YJW
z$Hy-$quoWfZFy;%Rk4uE+*cdic?8{LD#I3vXqM;3*D^>S;S5A}`vP>FmcZF&czTlJ
zo2r`LEuN1)gcQv;u~uA-Du8u~X7)PTN3PkQY-Dhik+8|t&_9{VQMscYFIBTRj`WOi
zmCq$9`P+9P<T?1To?sUdv;&WI+6>G*0t{=9z_CrA>}pjOhm(GI^?Pk+ZC7(p^1W&o
zl4mJwVWdyiIOkuuwr0fHJIca?CYSU?eeliWcj0`If|R*bCwHp}_haQ&&t#BdL6#;E
zXj3a#N#tYpOV?R$+-YuJPmAExP#EB{eRW<6TKX%zN>IWZJT<f`vgu?rt7kGcvS9DK
z?5@0pO?#XB=1O;T^_Lk1<wuH=^(}s&1MNyOM#On|M5p_qSyY7<3NbS#FPNO1j4H7)
z3puKm50YDk1Jv{#Z19c-vWuTa-3$=k_T*(NZWSL4_X;}31)$dho4C|{DF<N!+o`cI
z98RCS{_c+5$FAib{4}oSM6+*;WB!of11N*Ws*o96(jAK12|J=mt!n<t{488X40spV
z?I7mhWV%t|r2^C_YipHNmp`T`79rhf$y3$o*kT6lILc>mn&ILOOCI{-^zaB7ev%u5
zBMv{bc6F(w)4THK9V^nDN^f%GYcW|)!r9!{POzUezr!fB0dc-bo?@Gmm^&L-+C>Dj
zzIGoL2bm(idIl2+d77N(FtQh;ajNF{)4Q!dA3wRY6j*Uu-lx}-06RE2(vB3D`E-tb
zF1yLhSi0%*;QENPKt#5B;5r;YjiAo^A`~0!5@91Y?MPUQt*A~geZvqiFzZ}C4?5Gc
zUS^i*ufq5(xhXh&v%%Z(#xP>RfHE#BFGLo&w%7VmP7V<hsIlE`>V8CQV1M%CB7f6?
zsCTC(A{SoA7H#3z8Yx6+4wO-S0&_(E+>YrSG6s2OO`NZEY)M%#1}#>KrV<_I@^rSj
zh#BRAoybz(TND-dmnn{Pd;&DLG*M323G?FqVuhRWglr45Nb%8Flz<tQ*@5c%@+F1o
zq^Yp$bDHBYi(!j5j?PEMFplP$3lff|2$Eq%{8I&EX|aM&WE>1ZZTfkKbU@b}{boFg
zSYl7dAWf+Ry0b04nXBJ1vE4(IFViNM!2$EplHe2OWv!N$e|U}y6amob%2lU<Lxdl_
zq2ZIX0BH22?RK_g>zX28moqH12Nu>vbj;^m?z)Fq0vJxQzq(o$Q5p5Od#<3G>f(be
zu$AR0Hx+!m1uQ?$8z>==2LWp|_x+-gQ)o)PxW*6gpr62F2<mIBOSJ|0@q%8XXUG_J
zcD*X5c(cyFrtdyj;$hw~Nysi-_NW*yo=o;)pnj1V1+KI4+xX4|ANDZvm;;gsS|l6c
z>}VOnolei=Y&ZgcrkkUA#xcpow+D@g)<<HwVYhp5-H&P>ncf<2jYO6QT&AXKq`2Gi
z;$*mfPJw$CkFK_2U@LTZi3Y7+MAZP*7bz`1noG}Vwev4;_MMQ0ob1X6?~}8a-WkMl
z_lSM;s~%U6&7`_Frd$|I!6CNjo76tMfJ4yYDHs<OwQEydnT3^AYVdFOngb$u4dPk;
ztQO~3%dzpS?z6GyTDc;%PwV{HN>E43;U+=*?d-6Twd6Rc`A6nFaBuG;uK^{^p|@Bx
zmNfxXeZmcNo!<aAN&0l3Yaz}`NgbAW%{^2VDEW6snysV}8&Ww4_#=($e$mY|d8d2~
z#iw%Jgo41%jpqjqA=)u{4$>f^Z206P7D!o=0by^Gd}~+j(|!I-EGy)6D^#edF<a4?
zS~PZCebd&rzjpkrsz@@vp&F5jt4g&gx`S0^wJ)>)*#j8|=<($fVHa@Ru8Ulqs>|nH
z<aSWeN9<>yaAA_X99>682Z`$!U~6iA%ir$WvAXNbiWe$Qnro9wOT7>9%j7RNGlDLU
z2|F>3YgHo6G+^*Z<MK2ceTS9WTs^+(mm&$`nKiYmKNW6QRNlPhbF;yC*SMZaH93L-
zyofoqRT}2+yPBdyC~{sC=hD*n{Ol>=_Yb(#j(*j&yWmce8h6L!jEJ<H95jrARZq?q
z%}3=_=MXrFr8WX8hbcnj4mMjaP=3d%rhS@bwsm4nU&r{Zc<UhGaTJ<JVi-5AmCRA$
zXxZHMy{b1#IfaXKO>Cj3X+n{Pwg`Xjl%H`||5#JxnCSCRSnu)q*=71d%j{57d<Ybt
zc3CbPO-h3mkUsXOrh2O7&Qq;9!q&`*bK6YU`9;koZK~OL%?Os{%S#@tGMZ0UY~e4B
zqA89O!e>~>DEXObc^s!#4NYeE9`>?FPrNZL+bhf1&|jE1H(Oht=)M)@y!_m**J-N4
zj_H)Z_P1?q9UXYai&9)4bQDaVqMy$OoIWP`zB}2PdGQ?)F?DF?{kfsl4A^B%pp?wv
zq2I>bIr($Bw(f3rQrzJ|%z8y5%T1b0=*Bjbyf)5==5I!9Rz)-+Yw;^%ng~yOAE(fV
zVGpw(x5wh=_?CttJL+ETEnZt66)^PF!6?0A{ogyPd*n~Id#GG&7U2h+az+#3ERg~3
zj%1j&O;|x5N{8f)TA5jX=AS7XOexhfE8kOKP0Wj;g7=akM`M$=B*6WLF4=WXe7HTF
zbf4yO$J^>7zr3HKRksn0+)du<<<c;7-KLYB-)L!RSy}44dwg)xUv_;Edf?)2v|ZyN
z$l_-1?o2%{x#p~XpFoy;b<18!<UrKZ=2~%JrpF<1RfJ=+7flm%TzulHux891Rig0Z
z{n<Fo_M3Ry0+%>%G7965Pi@UKfBfgEw96~oX$;oQIM&@GZ6)qMm*5DK53-x#wAhfN
zRtvA3uVk8c61p-;r=cDnGI}m6TqqXeH{03p>Uny7+@@Vb(}G!Q((3mI8|vz!6Nu&U
zhsO`$ub{C;Amxp@gPZQI!@%e)E&P^O)p5!CPJA^zuzmR+F0iKJ{lS8t8@Akr0_<-K
z&*&CuT1b-Xv|948+0+&CGM;0YImJNu`f@jEw&3{zigpB&G45&W+@JEmb8>`n*}aiY
ziOkdXGuXIsW4~lI<FK~uGPQFh_*JbohnX-05`@JQw!zSyizxxbE$fba@~}^S2xv~m
z8)RypktAeSn5csVY#?~3A`g~of@(QMh$Pr~LcK+k?`$k(AMp6I&K=Hc-M6&7*4mHp
zzcqp24W63~@U@I^y&i7tV!<wdxn%1(QNa85!m@w1Gk*bPlZ-8UXlAG%7X{0m-+bM5
zN>O8!Ddad&<ge&=*b&N+mw#QyzF<WvVtzx8g{9(XKmJ$;2VMUq4cTgn^H0P}cA1!%
z`-cOqx{;+w<Y<(gMQKH)u?=CpRykaBHzGG{XGX7@<?0`AnMPH$1QEwju*a^Rz8zgU
zoF?Dce2Qc1z+0?4DuzyZA0Mb|CG{5>9DSSOA<|z`BA9>1Ut(nlvBddn0JEC>kmE)y
z&UL0VK(D36`a4M5m!#f9B($|^e8;i=xcYLFGeb)zXGPd>ew`-Smt!iYd!c3|$uu&N
z6pi(B8@c0-bCHj>^h>;gEV_<%NaBd88kSW<LO4-SixIIp#!hveT+SaeA9Jqb1aXpL
zkYi<lh|6GYl>N(~R-M(3>*^5y&H2#NsPAnIDpj9ZD_TQzm6Fpy)%ZGj9_SQ$VskT!
zbT#lZl_pjM7dWt%k39ZLd8Izi<?%(P=E?jM7FD@*;KPAKaxjukBu3@&t#>S*BBI@S
z@WG=GhiRn9SGLJOn5H^NfvwCtsNRY1OY^Sc>o6fLBYs2m`qpX78ZQ0{iGLGfdCkju
zjBMp)IH^HR`q6e2GD)(fHQ^D%YGD?hJZHjHdLu>q3qvPVqb#@2I^8hjrqap!R@;jW
z2+5dwj0N{F%-#6O#Nj9HFWhmm9)_#W_-`IQaT@ib4Plu^vax~L8hBj0>+>$iI--#|
z;81y8--#WokoLq$o16k`cY7S38Td)R5bY||xo=`_`EKbPlIJLzYh(c*;$a{!ZD}Wm
z7Ciix-ms|Od9@X%+>U(7UZ3oJ(Amgl@~1>EBrK<uLK=6!cef|?xFH*M6x@j?jeBaT
z6s(EzE@?r-w110WH;tl%iUg({<uRo8oGV8ek7V4^k_V~Mybw!{q&HRvplu~Hc0|6W
ziF9+Z3U3+EAZ;z!XdkwWMDm^U*Z*z$TmNUYKx$X8kyIWq6t4&+`sQ<-9ltPLZQc|$
zjbgCs;%);B5CKnqhJ#A`I^#|n5P#*S?vH6ijXXqmE7KaqPJ-lFm1c6I$X8cR#0gVm
zEb6yj1x($v4UrQ~q~D;vgD~Ax_7g|ma+C*3T&l7*eSLy4dFWLM?kr({6gRqU=^o%~
zMN~~))Ob2>!<ONWAbIhAS3Rm^!-kGL$3xM8ZP;?xKoSbsjs-NK0Fvx<Q=PBcZ{jba
z;w__qL#8*O1)m6?HDx^upCV;3VA%)5F0Wx(x7q}V=X*f9PIrj1!kz1<TQ0`LBHQCC
z%Yz}R?On0?I~n;k4Vntl`09UIPRHyV^<U94D!F4yB<mNRLBf(Imm53R|1kk?MD8gb
z9q|FQiXuswI}@Ov?4fdo`Qr?{Fq_I7!t4hb2SY^WN1gVj-NA<k2j-TH@#Oxk`Qlqz
z`z;HeT}1xnIQ%0ffBrCDT+nJw6wTC(rYm&u<YUk*`S2M?J8H#*l#_G}IV!8$?y<JT
zR&6m^S&w_4CTF<~d#VP&x5h6FA-^w4hcHT1=F)C^ZP@})z9&KfmK6PzWW}>*;KGSK
zuyp23e}j<)t^p<yl%!y?SQ7Kn=w@8<ihucQqu#8!6?G<_ybL=b&Ps={n;wzg*YeyT
z{4fF{5_l2XSdjO|63yNjNByfT)OS#rq?-8^)~SsK4IPp=Igpe}lovFV>GqVL0X{id
zSY8qM4ms>vcOr`)g1k~vQ_6mUp=P!m_NO{$uACrcKz#>_d_YQCOVp*`LE6Ky!xzUg
zYB$N}@mIz=*Ix@8b}Hu#Kvow#+o+8OEC}YJYE5U)C##MESrwkVoB3iLq@Pt9@3s`j
zE>q7{02B)ksioc**%Wn<88x_`n#zUW+e&&!>*IEP5;dHJirx&bwPW0VZ_Rhjp<2fR
z_bXRP_5qP$n{)Pf1W;=@(QPG|<EW2sfBwm<u|%a~Y6Q(14-wWtLMHEBiIu0e>3XyG
zC%H{U4LCfth)4ldk${g~@s{+nCWj@tU!vO9TFHsZ!&}C)_2N7D*NbsyG(8e8X<!Q_
zw;_3x<Q>0!qz};P&Dm|a!^@^*;}U=D53lu=lP&_?rGIeX^WfFNU4veaDe}xOxRQ<u
zdwpUdJm9mW3e+zl%n$JG<wwt4157Mv9WBGI3Eqo10C`^+et06I7WtHV+ox^}#FP!6
zD_=xWmzM#g$}B`$8g4vnlIrI+Pg>WnG}Y;w)1&;_FAFPZU@ntM8=v#V_8=QuM6PCF
z!Al(}7-E_JOw4+sd<FhhGV6(gj=xb{w;vX3K=UVJ^B0y9@s@&E&M(3J4i_AERip(D
zsuy3yBwk@z`5dcSjGQUcn7(F;c3F0^`3n!-*RouFhhis|Ihrt=FA}CzZK}H!Z7@Q5
zK7Ey7{y1fm4dK^5ZXU-t&R1gS^l7!DrH7%1Xk(J!r4EwQGP9m~6QVtp*YjC#O>f#M
z!0z2(sFx-2#YK)K-!bPdw&MevQR72S^8<qKe&@s+P0MRYsUPuQCasok2B*PFa~jD$
z!6%U-+hu{&50cf-g;q<wtn9yZn&cLiH~W@0Ot{=5A;XN9qlhB%n<GG0x`G0V&B;ml
z05YQFo(mr^HS!8xx&)fH(Ty{S#K2b4Mgi7>h)8G%YdHQ=eTQXb|0=GWWwPdr)Nx8P
za;(j&)&g=2=vpNx3-DJ8V7gkZ-b2l;r%-7Bp}e?d%`i(s;FT`e=V71;)QxhUv3_AW
zEPj=+DJ{m0Pii?K`Zz32@D>SiZ<QC3?H7SPu8kZbD#9{iX*kUu_M7Q6s80WY!z`Zc
z))GS8GC&PN5;3x2M<r9w<NaMl%G2qo)sABo__(dEKJnxZ=rtbSx)WS7mwJweyC~L4
z<wDGn1_CoBSsUwNtZyNQ(AlGZ2IkJJ!NacK8gHs2owtXjhi&=j*RyZKudekszOJ0e
zYekC=R`(@!+f*awIG88O=rypWdEwrFtgkYk%nIi}u@KMM^M9U9eeP=EL7-EV2}pUJ
zB;MLtkFlz^$d|g$>s8Tw51RYQ)y{%9R)p6ph{z4uU}msf%IOnMDIc|eW8A4Nn{h6D
z=4@Mf$oDmvXdo}!cgB{Fx0=y_07C5bb<1io;AIV#IY_{@g79W)@R+gEgZ8}huklIC
zrRTR<7H@ck-RYScf)tgF+D-@4E~x7x)>8?2wqF*gIzU&Kx%&>~9JDcCC9L!qJ+$+J
zXH~za%pAY!{^dzlxWl$ktiv5|H@Yg9v)&E{F)-7lxoH&Z<bScKmnHfIKJB{OH~bV&
zpa<et#Ym)1X79?b^0>-lM9m|x!lu>fb07$XDCFpDnEP6{qvUh_RQ%2eu*9#CZJe+|
zV4cgEbL2Z42#OQmxq(Gu?Xi^gk3w6#o?!U($!)2nEb3>d$COT8QQB5p$wQB|lk#0%
z?h;u^O#`zQGcED5zIEP$9}os%5;0&%9Av>mogq?k<1dCbokq@=9dzWyHAURX9~Q1~
z<X;b?g%(%VyT(R|Xu0v)tNz1()0znCt!1eH=F03d$dJ1}Jp_Y+s_>L2nq;bRzh(^+
zAMjzVih1H8>%nIQo?mNEOj<Kv!s=*kkCsX`5ebEFi#^ufps`^!tRzq~V@IdLcl!0w
z|Fp~9n2x30g4Pvp92CYo+@AvX&FhnOW2N4rw=8Nuv}$}>>U=y)ILvThOF4ZWYny4r
z_M_dgJ8zzlv*l*Q@tIrd)|~S;%fT&8_Q78DrS++smO~NJP6BRz7^}d#2fQrKoweHh
ziW;UN{Eq2Zw8ZnX9PYb(VJpyVhj+3+G9<?C<XM`T<pZ`d$x=dJ_oXxX;4#!2QirF<
z$H+%d06a|^H&B9G`ETT)r4{%D&v>7in};z>K|UEiYA-*!m68?q<;8T27$EnX6UmWw
z!SsE;ZgOL?)tN6{TJQD{(K(Oj$Lsf9pp!WFP}7|#WACqhjbJ(zN}PXKzwaV_pHIg^
z@xWcnuU!@~R{*ca^Uvz{Mbq~U7=-hid}_6#g-^kZi6FqwFM+$S3M!lBEB<}F&EZzf
zy;>g;d7OoFohW~+90M|c(6WMo5m4D_QCekS)TjP0z{9zf(aW_GFL&~1Fmx^x<$Rwc
z>CkQ@`G05gB|_Q}3i|b&D+uH_v@(&9Y~`!f7zg~Pvts?PU_1mtAitxHOcBUs-Wd$7
zg2=TiLjR@0??y)N(YHzwi<%=@{Y-Q})lYPvY!v_w5a72fcm#}NIKWZpX+lYC&sm!6
zU%Qo#vDhXXC71b$jv|l;aX)igxkIN#uUBY+I6%%8>#zNHU<=Vbpr97xj=X=-?ve9%
zqQm6WDo(O?`hUoJyW@gO1?LT(U!X8VApgMq{5Fl76i-CG1!vRd=iq-mvh>FuyK(bC
zTA)bK!_va05p_P8RAUG%;Jm5Lnsk^>@4sKF+qJ$QUdibTcW&j#dsqjtX-}-VNq-Pg
z8)4^G)){Gk0f*|xq|lagL{OWH0T94C`5yHxa8Wlo6;C=CScTBar(~uprook>U&NEa
znzeB3)_+GoC>&6Z1C%|)-(|j_GmH%yV4_m$IonajX94Ux+f`Z%p$6Kriunf;r+=SM
z&*~eXo+fVLQqzd&@DzfD-k$_mQ){ebjrs|It;LiA1?HC!T0Z9vwSAG`f(#v60TD9~
zfIdDE+<{1G;DF^d8MJM)?q<Z?4S{L*I6?Uv%{1Y}s<#dmpT6Cae%#`l!hcud{q20!
zKGTaW;Y1O~p_0>0-WnjvBllO$&$7Vk<Vd2)fKtl3^eKv~JV!L(Bn1!+xyn00>lslf
zxdtZc!C&bC%vaVq{6`kFv|c`~^K5HwY+SqtPDF$sGJ;Q?fAp0xEiJz_`D+VN#ysPV
z|8lC8?y>jl<Z5Z1|Ne_CYH8VQ{J(C@ch>2xdLrgg1PzYXU&hqZ5B_g*z6yetR<O~3
zH`mq8)B%PkXkdCJN7DaA;J&{9|IW_>`O1|mnQxQ08S@kW$W@(-O9EIsMBzU@-{px_
zekHVegF1aNt<u|Y0Ql7Z6}Hkb!9Citdu5zbgRf7mY1|K{RZOvyUy;5|`=Z{;iQ0(4
z`tjG2rLPa!AG9>Ii4wG^EBPU78ZHjbd0?8i8yXs>(eza<lE%a1sb3UEOH1@(hqkN^
zj0bzA+b*P@!X|tjLH}fxUg1ONSR{W|g-ONw{ZBLdn?gz?KS?H8R8#95E&fB*Bk+_&
zR#uj2e3z6fqIG_CC8E>so^D&30-sI#?Z3vT(k&F(6x4RQR5?brrndaE!$TeWI#P5x
zU=<8j?kycmN#@3$?NV`-i_atphsRnU{Fh{a08>@%Re?cc;a}e~Y6BB0Tw!VA)MEnt
zzN5*12#!AUZDFgw=<^rZO8MH%`3n>{jgRO75VU!AIGTPwCT8|;zVIvAb^R7!r|)ve
zfvAz>Xf=;-2)u8;@D2zfvYQ5G;=ZN>{K8)97Ot5R7}U-WO6t_gRN@%zKaA8t!3wf$
zVgO}1+|?8QE0ib&^X4OUj{09o5Vg6loOm?01jjDkSH4CPN;#tJudCjs9SPRCPT53>
zvTYx|c=5uRKl=59_H8FN+mt8Ib^F$Krc^n?TT`3C*(;9g`r7|q>j1LN2wVsrZA@=K
z6+iuMFWdO5)0)2WZTO0bN1ZKC9VPdsNK&rx7hKZ<JAPO((TPfV=$P8tLI)2;RAK&i
zKoXer1htET=`!(7k?j9pQnXHN56*Vz7tz>1Jvs5e&29hedRK60R7}qvLLkL)y6hq-
zn`W418I>*MdWe2r^FM=sor(Yd^JNC!v&J>re<#rSfZlJkf0~Z}4*jjhE%?&^0k-oh
Ab^rhX

literal 0
HcmV?d00001


From 15604f23c306c38bfb09dbb46fc8a2624ee31758 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Tue, 2 Sep 2025 15:17:17 -0700
Subject: [PATCH 40/51] Include 3Laws Robotics in README partners section

Added 3Laws Robotics to the list of partners.

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
---
 README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/README.md b/README.md
index 7e85a8225c8..d58ef3a8b6b 100644
--- a/README.md
+++ b/README.md
@@ -40,6 +40,8 @@ Please thank our amazing sponsors for their generous support of Nav2 on behalf o
 
 ### [Stereolabs](https://www.stereolabs.com/) produces the high-quality ZED stereo cameras with a complete vision pipeline from neural depth to SLAM, 3D object tracking, AI and more.
 
+### [3Laws Robotics](https://3laws.io/) provide Supervisor ROS and Pro, easy-to-use dynamic collision avoidance solutions to improve safety and application throughput.
+
 ## Citation
 
 If you use the navigation framework, an algorithm from this repository, or ideas from it

From fbdf75328b36f3808f5a20d79288cd8f7a4a2d09 Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Wed, 3 Sep 2025 11:28:00 +0200
Subject: [PATCH 41/51] Fix seg fault (#5501)

* Fix segmentation fault

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
---
 nav2_smac_planner/src/node_lattice.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp
index 7922d353d19..56dcbfdc271 100644
--- a/nav2_smac_planner/src/node_lattice.cpp
+++ b/nav2_smac_planner/src/node_lattice.cpp
@@ -230,7 +230,8 @@ bool NodeLattice::isNodeValid(
   // Check primitive end pose
   // Convert grid quantization of primitives to radians, then collision checker quantization
   static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size();
-  const double & angle = motion_table.getAngleFromBin(this->pose.theta) / bin_size;
+  const double angle = std::fmod(motion_table.getAngleFromBin(this->pose.theta),
+      2.0 * M_PI) / bin_size;
   if (collision_checker->inCollision(
       this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown))
   {
@@ -269,7 +270,7 @@ bool NodeLattice::isNodeValid(
         if (is_backwards) {
           prim_pose._theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI);
         } else {
-          prim_pose._theta = it->_theta;
+          prim_pose._theta = std::fmod(it->_theta, 2.0 * M_PI);
         }
         if (collision_checker->inCollision(
             prim_pose._x,

From 1d399b8a355b3b8e86d70d51c6e1b79d91a5c405 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Wed, 3 Sep 2025 11:29:27 -0700
Subject: [PATCH 42/51] Route graph vis fixes: Fixes regressions from #5452
 (#5507)

* Fixes for route graph

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* Fix route graph vis

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

---------

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>
---
 nav2_route/include/nav2_route/utils.hpp | 26 +++++++++++++++++++------
 1 file changed, 20 insertions(+), 6 deletions(-)

diff --git a/nav2_route/include/nav2_route/utils.hpp b/nav2_route/include/nav2_route/utils.hpp
index 04deda14bec..9b9e9078e7e 100644
--- a/nav2_route/include/nav2_route/utils.hpp
+++ b/nav2_route/include/nav2_route/utils.hpp
@@ -65,10 +65,10 @@ inline visualization_msgs::msg::MarkerArray toMsg(
   nodes_marker.header.stamp = now;
   nodes_marker.action = 0;
   nodes_marker.ns = "route_graph_nodes";
-  nodes_marker.type = visualization_msgs::msg::Marker::POINTS;
-  nodes_marker.scale.x = 0.5;
-  nodes_marker.scale.y = 0.5;
-  nodes_marker.scale.z = 0.5;
+  nodes_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
+  nodes_marker.scale.x = 0.1;
+  nodes_marker.scale.y = 0.1;
+  nodes_marker.scale.z = 0.1;
   nodes_marker.color.r = 1.0;
   nodes_marker.color.a = 1.0;
   nodes_marker.points.reserve(graph.size());
@@ -79,7 +79,7 @@ inline visualization_msgs::msg::MarkerArray toMsg(
   edges_marker.action = 0;
   edges_marker.ns = "route_graph_edges";
   edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
-  edges_marker.scale.x = 0.1;  // Line width
+  edges_marker.scale.x = 0.05;  // Line width
   edges_marker.color.g = 1.0;
   edges_marker.color.a = 0.5;  // Semi-transparent green so bidirectional connections stand out
   constexpr size_t points_per_edge = 2;
@@ -92,14 +92,28 @@ inline visualization_msgs::msg::MarkerArray toMsg(
   geometry_msgs::msg::Point edge_end;
 
   visualization_msgs::msg::Marker node_id_marker;
+  node_id_marker.header.frame_id = frame;
+  node_id_marker.header.stamp = now;
+  node_id_marker.action = 0;
   node_id_marker.ns = "route_graph_node_ids";
   node_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  node_id_marker.scale.x = 0.1;
+  node_id_marker.scale.y = 0.1;
   node_id_marker.scale.z = 0.1;
+  node_id_marker.color.a = 1.0;
+  node_id_marker.color.r = 1.0;
 
   visualization_msgs::msg::Marker edge_id_marker;
+  edge_id_marker.header.frame_id = frame;
+  edge_id_marker.header.stamp = now;
+  edge_id_marker.action = 0;
   edge_id_marker.ns = "route_graph_edge_ids";
   edge_id_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  edge_id_marker.scale.x = 0.1;
+  edge_id_marker.scale.y = 0.1;
   edge_id_marker.scale.z = 0.1;
+  edge_id_marker.color.a = 1.0;
+  edge_id_marker.color.g = 1.0;
 
   for (const auto & node : graph) {
     node_pos.x = node.coords.x;
@@ -141,8 +155,8 @@ inline visualization_msgs::msg::MarkerArray toMsg(
     }
   }
 
-  msg.markers.push_back(nodes_marker);
   msg.markers.push_back(edges_marker);
+  msg.markers.push_back(nodes_marker);
   return msg;
 }
 

From 41bd110f9b681f68ea6cd843aca9139a603ba9d3 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Wed, 3 Sep 2025 11:32:51 -0700
Subject: [PATCH 43/51] Removing openMP dep on MPPI (#5506)

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>
---
 nav2_mppi_controller/package.xml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml
index a27716ab997..450dd5254cc 100644
--- a/nav2_mppi_controller/package.xml
+++ b/nav2_mppi_controller/package.xml
@@ -14,7 +14,6 @@
   <depend>angles</depend>
   <depend>backward_ros</depend>
   <depend>geometry_msgs</depend>
-  <depend>libomp-dev</depend>
   <depend>nav2_common</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>

From 491433a5bf0ea2d4f22af6cd6d6d5aff6a204743 Mon Sep 17 00:00:00 2001
From: mini-1235 <mauricepurnawan@gmail.com>
Date: Fri, 5 Sep 2025 02:27:58 +0800
Subject: [PATCH 44/51] Fix pose timestamp when using
 transformPoseInTargetFrame (#5499)

* Fix pose timestamp when using transformPoseInTargetFrame

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Fix frame

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Update nav2_route/src/goal_intent_extractor.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
---
 nav2_controller/src/controller_server.cpp        |  1 +
 .../dwb_core/src/dwb_local_planner.cpp           | 16 ++++++++++++----
 nav2_route/src/goal_intent_extractor.cpp         |  2 ++
 3 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index 66b62e667e2..98a6eda8629 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -799,6 +799,7 @@ bool ControllerServer::isGoalReached()
   geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist());
 
   geometry_msgs::msg::PoseStamped transformed_end_pose;
+  end_pose_.header.stamp = pose.header.stamp;
   nav2_util::transformPoseInTargetFrame(
     end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(),
     costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance());
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 5c1b47322a6..b85727b95a0 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -290,6 +290,7 @@ DWBLocalPlanner::prepareGlobalPlan(
   }
 
   goal_pose.header.frame_id = global_plan_.header.frame_id;
+  goal_pose.header.stamp = pose.header.stamp;
   goal_pose.pose = global_plan_.poses.back().pose;
   nav2_util::transformPoseInTargetFrame(
     goal_pose, goal_pose, *tf_,
@@ -535,10 +536,17 @@ DWBLocalPlanner::transformGlobalPlan(
   // Helper function for the transform below. Converts a pose2D from global
   // frame to local
   auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {
-      geometry_msgs::msg::PoseStamped transformed_pose;
-      nav2_util::transformPoseInTargetFrame(
-        global_plan_pose, transformed_pose, *tf_,
-        transformed_plan.header.frame_id, transform_tolerance_);
+      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;
+      if (!nav2_util::transformPoseInTargetFrame(
+          stamped_pose, transformed_pose, *tf_,
+          transformed_plan.header.frame_id, transform_tolerance_))
+      {
+        throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame");
+      }
+      transformed_pose.pose.position.z = 0.0;
       return transformed_pose;
     };
 
diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp
index 14443b77641..483739aca05 100644
--- a/nav2_route/src/goal_intent_extractor.cpp
+++ b/nav2_route/src/goal_intent_extractor.cpp
@@ -175,6 +175,7 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr<const GoalT> goal)
       node_pose.pose.position.x = node_data.coords.x;
       node_pose.pose.position.y = node_data.coords.y;
       node_pose.header.frame_id = node_data.coords.frame_id;
+      node_pose.header.stamp = start_pose.header.stamp;
       candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id));
     }
 
@@ -202,6 +203,7 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr<const GoalT> goal)
       node_pose.pose.position.x = node_data.coords.x;
       node_pose.pose.position.y = node_data.coords.y;
       node_pose.header.frame_id = node_data.coords.frame_id;
+      node_pose.header.stamp = goal_pose.header.stamp;
       candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id));
     }
 

From c0da22a1c8bcb43a7762e7c32569059f3257682f Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Thu, 4 Sep 2025 21:11:58 +0200
Subject: [PATCH 45/51] Use integral path distance for MPPI Critics  (#5495)

* Implement integral distance for critics

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* .

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* more linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix tests

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* reset path length

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix sign

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix return value

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Revert "fix return value"

This reverts commit 8b21e890de598ab6b35c4a0200c82ebfc4b0c9c8.

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove extra variable

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add doxygen

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* rename getCriticGoalPathDistance

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* delete withinPositionGoalTolerance

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* use only one path distance

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* cleanup

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* reorder

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* format

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix tests

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
---
 .../critics/cost_critic.hpp                   |   1 -
 .../critics/goal_angle_critic.hpp             |   1 -
 .../critics/goal_critic.hpp                   |   1 -
 .../critics/obstacles_critic.hpp              |   1 -
 .../critics/path_align_critic.hpp             |   1 -
 .../critics/path_angle_critic.hpp             |   1 -
 .../critics/path_follow_critic.hpp            |   1 -
 .../critics/prefer_forward_critic.hpp         |   1 -
 .../critics/twirling_critic.hpp               |   1 -
 .../nav2_mppi_controller/models/state.hpp     |   1 +
 .../nav2_mppi_controller/optimizer.hpp        |   1 +
 .../nav2_mppi_controller/tools/utils.hpp      |  75 -------------
 .../src/critics/cost_critic.cpp               |   6 +-
 .../src/critics/goal_angle_critic.cpp         |  12 +-
 .../src/critics/goal_critic.cpp               |  13 +--
 .../src/critics/obstacles_critic.cpp          |   6 +-
 .../src/critics/path_align_critic.cpp         |  14 +--
 .../src/critics/path_angle_critic.cpp         |  11 +-
 .../src/critics/path_follow_critic.cpp        |  10 +-
 .../src/critics/prefer_forward_critic.cpp     |   8 +-
 .../src/critics/twirling_critic.cpp           |  15 ++-
 nav2_mppi_controller/src/optimizer.cpp        |   1 +
 nav2_mppi_controller/test/critics_tests.cpp   |  18 +++
 .../test/optimizer_smoke_test.cpp             |   4 +-
 nav2_mppi_controller/test/utils_test.cpp      | 105 ------------------
 25 files changed, 41 insertions(+), 268 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index 2646c98fd9e..c285a14b664 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -144,7 +144,6 @@ class CostCritic : public CriticFunction
   std::string inflation_layer_name_;
 
   unsigned int power_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp
index 58c108b21cd..08ec354cf3b 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp
@@ -46,7 +46,6 @@ class GoalAngleCritic : public CriticFunction
   float threshold_to_consider_{0};
   unsigned int power_{0};
   float weight_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp
index 6b248f29945..8fb8fb688ae 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp
@@ -46,7 +46,6 @@ class GoalCritic : public CriticFunction
   unsigned int power_{0};
   float weight_{0};
   float threshold_to_consider_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
index 23fb3db2ca0..31b6a3d0df8 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
@@ -97,7 +97,6 @@ class ObstaclesCritic : public CriticFunction
 
   unsigned int power_{0};
   float repulsion_weight_, critical_weight_{0};
-  bool enforce_path_inversion_{false};
   std::string inflation_layer_name_;
 };
 
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
index 133447a8774..f7ad2fda6a9 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
@@ -52,7 +52,6 @@ class PathAlignCritic : public CriticFunction
   bool use_path_orientations_{false};
   unsigned int power_{0};
   float weight_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp
index 85b9d48eb83..b137270aba9 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp
@@ -81,7 +81,6 @@ class PathAngleCritic : public CriticFunction
 
   unsigned int power_{0};
   float weight_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp
index aacec055ccf..1ccd08c32fe 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp
@@ -53,7 +53,6 @@ class PathFollowCritic : public CriticFunction
 
   unsigned int power_{0};
   float weight_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp
index 811ba9ae9a6..e17ad235e5c 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp
@@ -45,7 +45,6 @@ class PreferForwardCritic : public CriticFunction
   unsigned int power_{0};
   float weight_{0};
   float threshold_to_consider_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp
index bc40b194404..3e316a567e0 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp
@@ -44,7 +44,6 @@ class TwirlingCritic : public CriticFunction
 protected:
   unsigned int power_{0};
   float weight_{0};
-  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
index a8c81c3b07d..50f9d400a91 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
@@ -40,6 +40,7 @@ struct State
 
   geometry_msgs::msg::PoseStamped pose;
   geometry_msgs::msg::Twist speed;
+  float local_path_length;
 
   /**
     * @brief Reset state data
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
index e70cfcab786..1914abde647 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
@@ -33,6 +33,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_mppi_controller/models/optimizer_settings.hpp"
 #include "nav2_mppi_controller/motion_models.hpp"
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
index f518f16e169..4a3cc6b7c06 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -243,81 +243,6 @@ inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path)
   return pathPose;
 }
 
-/**
- * @brief Get the target pose to be evaluated by the critic
- * @param data Data to use
- * @param enforce_path_inversion True to return the cusp point (last pose of the path)
- * instead of the original goal
- * @return geometry_msgs::msg::Pose Target pose for the critic
- */
-inline geometry_msgs::msg::Pose getCriticGoal(
-  const CriticData & data,
-  bool enforce_path_inversion)
-{
-  if (enforce_path_inversion) {
-    return getLastPathPose(data.path);
-  } else {
-    return data.goal;
-  }
-}
-
-/**
- * @brief Check if the robot pose is within the Goal Checker's tolerances to goal
- * @param global_checker Pointer to the goal checker
- * @param robot Pose of robot
- * @param goal Goal pose
- * @return bool If robot is within goal checker tolerances to the goal
- */
-inline bool withinPositionGoalTolerance(
-  nav2_core::GoalChecker * goal_checker,
-  const geometry_msgs::msg::Pose & robot,
-  const geometry_msgs::msg::Pose & goal)
-{
-  if (goal_checker) {
-    geometry_msgs::msg::Pose pose_tolerance;
-    geometry_msgs::msg::Twist velocity_tolerance;
-    goal_checker->getTolerances(pose_tolerance, velocity_tolerance);
-
-    const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x;
-
-    auto dx = robot.position.x - goal.position.x;
-    auto dy = robot.position.y - goal.position.y;
-
-    auto dist_sq = dx * dx + dy * dy;
-
-    if (dist_sq < pose_tolerance_sq) {
-      return true;
-    }
-  }
-
-  return false;
-}
-
-/**
- * @brief Check if the robot pose is within tolerance to the goal
- * @param pose_tolerance Pose tolerance to use
- * @param robot Pose of robot
- * @param goal Goal pose
- * @return bool If robot is within tolerance to the goal
- */
-inline bool withinPositionGoalTolerance(
-  float pose_tolerance,
-  const geometry_msgs::msg::Pose & robot,
-  const geometry_msgs::msg::Pose & goal)
-{
-  const double & dist_sq =
-    std::pow(goal.position.x - robot.position.x, 2) +
-    std::pow(goal.position.y - robot.position.y, 2);
-
-  const float pose_tolerance_sq = pose_tolerance * pose_tolerance;
-
-  if (dist_sq < pose_tolerance_sq) {
-    return true;
-  }
-
-  return false;
-}
-
 /**
   * @brief normalize
   * Normalizes the angle to be -M_PIF circle to +M_PIF circle
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index dedf910187e..6b15d992ca8 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -23,8 +23,6 @@ namespace mppi::critics
 void CostCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(consider_footprint_, "consider_footprint", false);
   getParam(power_, "cost_power", 1);
@@ -135,8 +133,6 @@ void CostCritic::score(CriticData & data)
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
   // Setup cost information for various parts of the critic
   is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
   auto * costmap = collision_checker_.getCostmap();
@@ -153,7 +149,7 @@ void CostCritic::score(CriticData & data)
 
   // If near the goal, don't apply the preferential term since the goal is near obstacles
   bool near_goal = false;
-  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) {
+  if (data.state.local_path_length < near_goal_distance_) {
     near_goal = true;
   }
 
diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp
index 8ac899140f8..8792df2aaa2 100644
--- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp
+++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp
@@ -20,8 +20,6 @@ namespace mppi::critics
 void GoalAngleCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 3.0f);
@@ -36,17 +34,11 @@ void GoalAngleCritic::initialize()
 
 void GoalAngleCritic::score(CriticData & data)
 {
-  if (!enabled_) {
+  if (!enabled_ || data.state.local_path_length > threshold_to_consider_) {
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (!utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
-    return;
-  }
+  geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path);
 
   double goal_yaw = tf2::getYaw(goal.orientation);
 
diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp
index 927ef950de8..9889ef3f071 100644
--- a/nav2_mppi_controller/src/critics/goal_critic.cpp
+++ b/nav2_mppi_controller/src/critics/goal_critic.cpp
@@ -22,10 +22,7 @@ namespace mppi::critics
 void GoalCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
-
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 5.0f);
   getParam(threshold_to_consider_, "threshold_to_consider", 1.4f);
@@ -37,17 +34,11 @@ void GoalCritic::initialize()
 
 void GoalCritic::score(CriticData & data)
 {
-  if (!enabled_) {
+  if (!enabled_ || data.state.local_path_length > threshold_to_consider_) {
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (!utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
-    return;
-  }
+  geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path);
 
   auto goal_x = goal.position.x;
   auto goal_y = goal.position.y;
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
index db2972dfc9e..0577db0a76f 100644
--- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp
+++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -24,8 +24,6 @@ namespace mppi::critics
 void ObstaclesCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(consider_footprint_, "consider_footprint", false);
   getParam(power_, "cost_power", 1);
@@ -130,11 +128,9 @@ void ObstaclesCritic::score(CriticData & data)
     possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
   // If near the goal, don't apply the preferential term since the goal is near obstacles
   bool near_goal = false;
-  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) {
+  if (data.state.local_path_length < near_goal_distance_) {
     near_goal = true;
   }
 
diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index 3ea13c641ab..befa9e42eb9 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -20,12 +20,9 @@ namespace mppi::critics
 void PathAlignCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 10.0f);
-
   getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f);
   getParam(offset_from_furthest_, "offset_from_furthest", 20);
   getParam(trajectory_point_step_, "trajectory_point_step", 4);
@@ -42,16 +39,7 @@ void PathAlignCritic::initialize()
 
 void PathAlignCritic::score(CriticData & data)
 {
-  if (!enabled_) {
-    return;
-  }
-
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  // Don't apply close to goal, let the goal critics take over
-  if (utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
+  if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 
diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
index 96ceae1b93b..6f42d33b042 100644
--- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -23,7 +23,6 @@ namespace mppi::critics
 void PathAngleCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
   float vx_min;
   getParentParam(vx_min, "vx_min", -0.35);
   if (fabs(vx_min) < 1e-6f) {  // zero
@@ -62,15 +61,7 @@ void PathAngleCritic::initialize()
 
 void PathAngleCritic::score(CriticData & data)
 {
-  if (!enabled_) {
-    return;
-  }
-
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
+  if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 
diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp
index 4f8c1c70eec..240c49725d6 100644
--- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp
@@ -22,10 +22,7 @@ namespace mppi::critics
 void PathFollowCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
-
   getParam(
     threshold_to_consider_,
     "threshold_to_consider", 1.4f);
@@ -40,12 +37,7 @@ void PathFollowCritic::score(CriticData & data)
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (data.path.x.size() < 2 ||
-    utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
+  if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 
diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
index 47c84321559..20c58324365 100644
--- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
+++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
@@ -22,8 +22,6 @@ namespace mppi::critics
 void PreferForwardCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 5.0f);
@@ -41,11 +39,7 @@ void PreferForwardCritic::score(CriticData & data)
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
+  if (data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 
diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp
index c9cfc79233c..2c6842c3a26 100644
--- a/nav2_mppi_controller/src/critics/twirling_critic.cpp
+++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp
@@ -22,10 +22,7 @@ namespace mppi::critics
 void TwirlingCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
-  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
-
   auto getParam = parameters_handler_->getParamGetter(name_);
-
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 10.0f);
 
@@ -39,12 +36,14 @@ void TwirlingCritic::score(CriticData & data)
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+  if (data.goal_checker != nullptr) {
+    geometry_msgs::msg::Pose pose_tolerance;
+    geometry_msgs::msg::Twist velocity_tolerance;
+    data.goal_checker->getTolerances(pose_tolerance, velocity_tolerance);
 
-  if (utils::withinPositionGoalTolerance(
-      data.goal_checker, data.state.pose.pose, goal))
-  {
-    return;
+    if (data.state.local_path_length < pose_tolerance.position.x) {
+      return;
+    }
   }
 
   if (power_ > 1u) {
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index 97a16249814..013b29de502 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -257,6 +257,7 @@ void Optimizer::prepare(
 {
   state_.pose = robot_pose;
   state_.speed = robot_speed;
+  state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan);
   path_ = utils::toTensor(plan);
   costs_.setZero();
   goal_ = goal;
diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp
index 813cba14318..88e823019a7 100644
--- a/nav2_mppi_controller/test/critics_tests.cpp
+++ b/nav2_mppi_controller/test/critics_tests.cpp
@@ -251,16 +251,19 @@ TEST(CriticTests, GoalAngleCritic)
   goal.orientation.y = 0.0;
   goal.orientation.z = 1.0;
   goal.orientation.w = 0.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0, 1e-6);
 
   // Let's move it even closer, just to be sure it still doesn't trigger
   state.pose.pose.position.x = 9.2;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0, 1e-6);
 
   // provide state pose and path below `threshold_to_consider` to consider
   state.pose.pose.position.x = 9.7;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_GT(costs.sum(), 0);
   EXPECT_NEAR(costs(0), 9.42, 0.02);  // (3.14 - 0.0) * 3.0 weight
@@ -305,6 +308,7 @@ TEST(CriticTests, GoalCritic)
   path.x(9) = 10.0;
   path.y(9) = 0.0;
   goal.position.x = 10.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs(2), 0.0, 1e-6);  // (0 * 5.0 weight
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);  // Should all be 0 * 1000
@@ -314,6 +318,7 @@ TEST(CriticTests, GoalCritic)
   path.x(9) = 0.5;
   path.y(9) = 0.0;
   goal.position.x = 0.5;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs(2), 2.5, 1e-6);  // (sqrt(10.0 * 10.0) * 5.0 weight
   EXPECT_NEAR(costs.sum(), 2500.0, 1e-3);  // should be 2.5 * 1000
@@ -360,12 +365,14 @@ TEST(CriticTests, PathAngleCritic)
   state.pose.pose.position.y = 0.0;
   path.x(9) = 0.15;
   goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
 
   // provide state pose and path close but outside of tol. with less than PI/2 angular diff.
   path.x(9) = 0.95;
   goal.position.x = 0.95;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   data.furthest_reached_path_point = 2;  // So it grabs the 2 + offset_from_furthest_ = 6th point
   path.x(6) = 1.0;  // angle between path point and pose = 0 < max_angle_to_furthest_
   path.y(6) = 0.0;
@@ -478,12 +485,14 @@ TEST(CriticTests, PreferForwardCritic)
   state.pose.pose.position.x = 1.0;
   path.x(9) = 10.0;
   goal.position.x = 10.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f);
 
   // provide state pose and path close to trigger behavior but with all forward motion
   path.x(9) = 0.15;
   goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   state.vx.setOnes();
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f);
@@ -536,12 +545,14 @@ TEST(CriticTests, TwirlingCritic)
   state.pose.pose.position.x = 1.0;
   path.x(9) = 10.0;
   goal.position.x = 10.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
 
   // provide state pose and path close to trigger behavior but with no angular variation
   path.x(9) = 0.15;
   goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   state.wz.setZero();
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
@@ -601,6 +612,7 @@ TEST(CriticTests, PathFollowCritic)
   state.pose.pose.position.x = 2.0;
   path.x(5) = 1.8;
   goal.position.x = 1.8;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
 
@@ -608,6 +620,7 @@ TEST(CriticTests, PathFollowCritic)
   // pose differential is (0, 0) and (0.15, 0)
   path.x(5) = 0.15;
   goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 750.0, 1e-2);  // 0.15 * 5 weight * 1000
 }
@@ -653,6 +666,7 @@ TEST(CriticTests, PathAlignCritic)
   state.pose.pose.position.x = 1.0;
   path.x(9) = 0.85;
   goal.position.x = 0.85;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
 
@@ -660,6 +674,7 @@ TEST(CriticTests, PathAlignCritic)
   // but data furthest point reached is 0 and offset default is 20, so returns
   path.x(9) = 0.15;
   goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
 
@@ -668,6 +683,7 @@ TEST(CriticTests, PathAlignCritic)
   *data.furthest_reached_path_point = 21;
   path.x(9) = 0.15;
   goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
 
@@ -699,6 +715,7 @@ TEST(CriticTests, PathAlignCritic)
   path.x(20) = 0.9;
   path.x(21) = 0.9;
   goal.position.x = 0.9;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   generated_trajectories.x.setConstant(0.66f);
   critic.score(data);
   // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term
@@ -719,6 +736,7 @@ TEST(CriticTests, PathAlignCritic)
   path.x.setConstant(1.5f);
   path.y.setConstant(1.5f);
   goal.position.x = 1.5;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
   critic.score(data);
   EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
 }
diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp
index 733229b77df..c88422f3a84 100644
--- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp
+++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp
@@ -22,6 +22,7 @@
 #include <nav2_costmap_2d/costmap_2d_ros.hpp>
 #include <nav2_core/goal_checker.hpp>
 #include "tf2_ros/buffer.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_mppi_controller/optimizer.hpp"
 #include "nav2_mppi_controller/tools/parameters_handler.hpp"
@@ -79,7 +80,8 @@ TEST_P(OptimizerSuite, OptimizerTest) {
   auto goal = path.poses.back().pose;
   nav2_core::GoalChecker * dummy_goal_checker{nullptr};
 
-  auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker);
+  auto [cmd, trajectory] = optimizer->evalControl(pose, velocity, path, goal,
+    dummy_goal_checker);
   EXPECT_GT(trajectory.rows(), 0);
   EXPECT_GT(trajectory.cols(), 0);
 }
diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp
index 6f70e19cfa5..70733e37db4 100644
--- a/nav2_mppi_controller/test/utils_test.cpp
+++ b/nav2_mppi_controller/test/utils_test.cpp
@@ -116,56 +116,6 @@ TEST(UtilsTests, ConversionTests)
   EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6);
 }
 
-TEST(UtilsTests, WithTolTests)
-{
-  geometry_msgs::msg::Pose pose;
-  pose.position.x = 10.0;
-  pose.position.y = 1.0;
-
-  nav2_core::GoalChecker * goal_checker = new TestGoalChecker;
-
-  nav_msgs::msg::Path path;
-  path.poses.resize(2);
-  geometry_msgs::msg::Pose & goal = path.poses.back().pose;
-
-  // Create CriticData with state and goal initialized
-  models::State state;
-  state.pose.pose = pose;
-  models::Trajectories generated_trajectories;
-  models::Path path_critic;
-  Eigen::ArrayXf costs;
-  float model_dt;
-  CriticData data = {
-    state, generated_trajectories, path_critic, goal,
-    costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt};
-
-  // Test not in tolerance
-  goal.position.x = 0.0;
-  goal.position.y = 0.0;
-  EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal));
-  EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, goal));
-
-  // Test in tolerance
-  goal.position.x = 9.8;
-  goal.position.y = 0.95;
-  EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
-  EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
-
-  goal.position.x = 10.0;
-  goal.position.y = 0.76;
-  EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
-  EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
-
-  goal.position.x = 9.76;
-  goal.position.y = 1.0;
-  EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
-  EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
-
-  delete goal_checker;
-  goal_checker = nullptr;
-  EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal));
-}
-
 TEST(UtilsTests, AnglesTests)
 {
   // Test angle normalization by creating insane angles
@@ -650,61 +600,6 @@ TEST(UtilsTests, getLastPathPoseTest)
   EXPECT_NEAR(last_path_pose.orientation.w, 0.0, 1e-3);
 }
 
-TEST(UtilsTests, getCriticGoalTest)
-{
-  geometry_msgs::msg::Pose pose;
-  pose.position.x = 10.0;
-  pose.position.y = 1.0;
-
-  nav_msgs::msg::Path path;
-  path.poses.resize(10);
-  path.poses[9].pose.position.x = 5.0;
-  path.poses[9].pose.position.y = 50.0;
-  path.poses[9].pose.orientation.x = 0.0;
-  path.poses[9].pose.orientation.y = 0.0;
-  path.poses[9].pose.orientation.z = 1.0;
-  path.poses[9].pose.orientation.w = 0.0;
-
-  geometry_msgs::msg::Pose goal;
-  goal.position.x = 6.0;
-  goal.position.y = 60.0;
-  goal.orientation.x = 0.0;
-  goal.orientation.y = 0.0;
-  goal.orientation.z = 0.0;
-  goal.orientation.w = 1.0;
-
-  // Create CriticData with state and goal initialized
-  models::State state;
-  state.pose.pose = pose;
-  models::Trajectories generated_trajectories;
-  models::Path path_t = toTensor(path);
-  Eigen::ArrayXf costs;
-  float model_dt;
-  CriticData data = {
-    state, generated_trajectories, path_t, goal,
-    costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt};
-
-  bool enforce_path_inversion = true;
-  geometry_msgs::msg::Pose target_goal = utils::getCriticGoal(data, enforce_path_inversion);
-
-  EXPECT_EQ(target_goal.position.x, 5);
-  EXPECT_EQ(target_goal.position.y, 50);
-  EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3);
-  EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3);
-  EXPECT_NEAR(target_goal.orientation.z, 1.0, 1e-3);
-  EXPECT_NEAR(target_goal.orientation.w, 0.0, 1e-3);
-
-  enforce_path_inversion = false;
-  target_goal = utils::getCriticGoal(data, enforce_path_inversion);
-
-  EXPECT_EQ(target_goal.position.x, 6);
-  EXPECT_EQ(target_goal.position.y, 60);
-  EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3);
-  EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3);
-  EXPECT_NEAR(target_goal.orientation.z, 0.0, 1e-3);
-  EXPECT_NEAR(target_goal.orientation.w, 1.0, 1e-3);
-}
-
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

From f0403189ae3a7fec7382fe1bf1ae7bb081d11d1b Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Thu, 4 Sep 2025 18:45:35 -0700
Subject: [PATCH 46/51] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20actions/se?=
 =?UTF-8?q?tup-python=20from=205=20to=206=20(#5510)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Bumps [actions/setup-python](https://github.com/actions/setup-python) from 5 to 6.
- [Release notes](https://github.com/actions/setup-python/releases)
- [Commits](https://github.com/actions/setup-python/compare/v5...v6)

---
updated-dependencies:
- dependency-name: actions/setup-python
  dependency-version: '6'
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 .github/workflows/lint.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
index 337b0966f0d..895bc2152c3 100644
--- a/.github/workflows/lint.yml
+++ b/.github/workflows/lint.yml
@@ -31,7 +31,7 @@ jobs:
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v5
-      - uses: actions/setup-python@v5
+      - uses: actions/setup-python@v6
       - uses: pre-commit/action@v3.0.1
         env:
           SKIP: >-

From 627a8b6cb2f9140f5412272c19f562351319bb26 Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Fri, 5 Sep 2025 21:13:48 +0200
Subject: [PATCH 47/51] Add debug topic to visualize whether MPPI critic has an
 effect on costs (#5485)

* Publish criticsStats

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* change header to stamp

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* make unique_pointer

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* typo

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add readme

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add to readme

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fixes

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
---
 nav2_mppi_controller/CMakeLists.txt           |   2 +
 nav2_mppi_controller/README.md                |  22 +++++++++
 .../nav2_mppi_controller/critic_manager.hpp   |   4 ++
 nav2_mppi_controller/media/critics_stats.png  | Bin 0 -> 119317 bytes
 nav2_mppi_controller/package.xml              |   1 +
 nav2_mppi_controller/src/critic_manager.cpp   |  45 +++++++++++++++++-
 nav2_msgs/CMakeLists.txt                      |   1 +
 nav2_msgs/msg/CriticsStats.msg                |   5 ++
 .../gps_navigation/nav2_no_map_params.yaml    |   1 +
 9 files changed, 79 insertions(+), 2 deletions(-)
 create mode 100644 nav2_mppi_controller/media/critics_stats.png
 create mode 100644 nav2_msgs/msg/CriticsStats.msg

diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index 931af234887..c28c711dc50 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
@@ -59,6 +60,7 @@ target_link_libraries(mppi_controller PUBLIC
   nav2_ros_common::nav2_ros_common
   nav2_costmap_2d::layers
   nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
   ${nav_msgs_TARGETS}
   pluginlib::pluginlib
   rclcpp::rclcpp
diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
index 68db5b73719..630aa19f214 100644
--- a/nav2_mppi_controller/README.md
+++ b/nav2_mppi_controller/README.md
@@ -59,6 +59,7 @@ This process is then repeated a number of times and returns a converged solution
  | retry_attempt_limit        | int    | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure.                                                                                                                                                                                                       |
  | regenerate_noises          | bool   | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. |
  | publish_optimal_trajectory | bool   | Publishes the full optimal trajectory sequence each control iteration for downstream  control systems, collision checkers, etc to have context beyond the next timestep. |
+ | publish_critics_stats      | bool   | Default false. Whether to publish statistics about each critic's performance. When enabled, publishes a `nav2_msgs::msg::CriticsStats` message containing critic names, whether they changed costs, and the sum of costs added by each critic. Useful for debugging and tuning critic behavior. |
 
 
 #### Trajectory Visualizer
@@ -293,6 +294,7 @@ controller_server:
 |---------------------------|----------------------------------|-----------------------------------------------------------------------|
 | `trajectories`            | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence |
 | `transformed_global_plan` | `nav_msgs/Path`                  | Part of global plan considered by local planner                       |
+| `critics_stats`           | `nav2_msgs/CriticsStats`         | Statistics about each critic's performance (published when `publish_critics_stats` is enabled) |
 
 ## Notes to Users
 
@@ -328,6 +330,26 @@ As you increase or decrease your weights on the Obstacle, you may notice the afo
 
 Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to  deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off.
 
+### Critic costs debugging
+
+The `publish_critics_stats` parameter enables publishing of statistics about each critic's performance, which can be visualized using tools like PlotJuggler or Foxglove to analyze and debug critic behavior.
+
+The published `nav2_msgs::msg::CriticsStats` message contains the following fields:
+
+- **stamp**: Timestamp of when the statistics were computed
+- **critics**: Array of critic names that were evaluated (e.g., "ConstraintCritic", "GoalCritic", "ObstaclesCritic")
+- **changed**: Boolean array indicating whether each critic modified the trajectory costs. `true` means the critic added non-zero costs, `false` means it had no effect
+- **costs_sum**: Array of the total cost contribution from each critic. This represents the sum of all costs added by that specific critic across all trajectory candidates
+
+This data is invaluable for understanding:
+- Which critics are actively influencing trajectory selection
+- The relative impact of each critic on the final trajectory choice
+- Whether critics are working as expected or if parameter tuning is needed (e.g. `threshold_to_consider`)
+
+More detailed statistics could be added in the future.
+
+![critics_stats](media/critics_stats.png)
+
 ### MFMA and AVX2 Optimizations
 
 This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway.
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
index fed85867905..237649bedde 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
@@ -22,6 +22,7 @@
 
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav2_msgs/msg/critics_stats.hpp"
 
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
@@ -96,6 +97,9 @@ class CriticManager
   std::unique_ptr<pluginlib::ClassLoader<critics::CriticFunction>> loader_;
   Critics critics_;
 
+  nav2::Publisher<nav2_msgs::msg::CriticsStats>::SharedPtr critics_effect_pub_;
+  bool publish_critics_stats_;
+
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };
 
diff --git a/nav2_mppi_controller/media/critics_stats.png b/nav2_mppi_controller/media/critics_stats.png
new file mode 100644
index 0000000000000000000000000000000000000000..c373f8e0dcc5edfb00b450e2f6c7d24ea562eec5
GIT binary patch
literal 119317
zcmdSBbyU<{+xJaLD5WS0Qi>7+A`C;9A|Q=~bi*JF2!cqLDBV3nh(R|)NQtD>fD%KO
z0!ky@JbN(C^NRa<uIpX*Kkr(srH=EP*>UV6zQ<=1q^c}ScAEAy4h{~P+`YSMI5>os
zI5>EG#CYHpODgLW@CUA=n(Q5%f-br_92_Pbxx2SDTyPg_ceTzByx;ad_F?7d;Tf2o
z4e}D{;X~)$F{kXKqr<s>TbhFQED_!77MkWOf@zOW6W{sr{L+Oll;7sqTAHZBvhLFH
zkSBJ{9FTNV=qwZ@t@v_T+&3`L*k<?+tJA5CHgL~#>wmgGHB&Hu%%-<px8s9D{O9M%
zX<rg52Uf*aC&7kQTwF?=-#@GDcvLLYM*%7+l*)tzk~qJ9rmv`$FMFGyrz^+asr`BP
z@l_nSzxTkY-w)&XDl_B2XN_J#x~P9YD|rL22$n*9T>;u{U(AGq_w%QUz!?3cn2GB5
zyKo3Rd~xz<@@W1xL|R;Jh!Jt%WAZ;wF{LTsVDfrjn~&}LD*hV9C(1-VD+ebhv=xPS
zPA;5937I!{Izap$4yKdH?zA$=U*A9sK2IC+>#<cUJVQ2!=^%K7a(PD_{MSUCtf17N
zsacVHK<;kX$@g?A=nI|<%ID69MkUx%4o3>p6bcOAV+%9c^k;!2aWJG9(qCWZfs_;>
zo1MbNqn}^}>#@I0Y)SlU;!I3j;JaQPDaj4H2Av@~SvghWENLAJi<u1!qf^Fh-MJ=c
zRI+FvwAVw~VeM0Lvi?BCF2g<ayLY&imAwv*na#~)w;qX@+$nf`YOg0bd6bG(IYsdC
z4c35t?e?JtBs*K=pWQURgY%e{D)cQCJ)AnOyyo?uxza*r&XfI;(I}o*CgGn3I}g^B
zApZU$8&GwZ(bl#Y-mi_D3N)!aRd~PlG@CdpU0X&*#>>k~My4g6OHcWwcy04cW=V;f
z!32HQvuANmP8H6}oX%SHf^`MUPaW6m0*%#<2SUVyxEFd^g_+Irexwm%IE7r<b|V@1
z0$Rp$6u{e$b2mT2dC6t9m6adeu9tVJXt6SbQ!^w8S&l7>X|lp|B9_^2BBQ8Bdi2aq
zXQz{6Bi@W|(}}r^mM%^|+`cf3R~~^n{j@H8@7|)8x@U>C(o==SZ!FX6Y!cSrpXFbd
z9tyfo9xxwk%9~)zVH7{)#>FY~<9n<s&94or;>T^WBHSM66+}-F_g27~n>*C^UI^WJ
zL)B|Z9jj6BQdF>p8IC!3!B#JIJVGhd!aj=aZ!5xe={`(OPA)EA!h8a;SJ#xmt~Ba9
zT#2elGoI;6nL(qE*GzkL490AG9B<dRgEg9Gn4f>-IGi=pg}ly6aXlxua@S}&51Di<
z?a;?MOMdEye$#f;1MkC4EwFx1mpLUKe~Hc9Qxu}{o8ps}!Q@@v8?Ln+$!W9lGZSY>
zPOYdk9f^)7=3;Twc|4bFzK!AJ<8j|y-mLT+J2cCjrK1Zk%8I?-+?+c|<iNu|6jO4J
zxsh&O{j`cAA~ELNx1)nsJ%0?h=DLp{8rpS5$gs(ZUI!JQbROz5zf}Si`Vo~=3*%vr
zNw6s#^ezQP^IDGm{wtHIwlnSIQQld3>>ujr+(atJ#&oCsPCQGiqU3$!Yp;fsGT8g+
zs=mtPAsWxd14Hg5Go9v<31AN8>&p^|V6a;RW^w&G7qgj-Ov_+wY^k_s=#~tP9;7UX
z*&Thfx4<2PQH-)EAmPq?-GHI@P<kH)4WSdZ>{zy-v2`zLFr$hIX!!`cnd?$3l4m;A
z$|kGrVv0Fv7}M-$g>S`ob>AbU2t(_tue*I6Ebk_i^%pW#%b2aW`-q4`KA}wJ5z_IK
zF6x1vR=J7ekAgRvmou6JWScX+%QJS2l{VY_nsZBt$_EQ}f}}0<f-yEiE~}qDizOvk
zwp83GYONpIO?S^xW*F7fz4d0s<i+Z3h`@zHbf~SjJY#0(gqpZS`sUN>yK&)JvAcRt
z3yo@PZC2Wlht&g$wj@yc@S?<xQws`ka)umsy$aFXO*;KTJfUCSjY$ZYWDC^?V3Hhg
z>eNKdqG}u0uE=-acczn{MUS2OdQR^d*CN!96aUxABq!jz$|)gXgnFf@e%tf=%}1)T
zq-||d;BX1lWe49iJ9UafC0S5<k97odZ#Wbh5^^~*MXYU#nE5^D%}0{@qds8iV`KRO
zm^nq<qnAaxNb?Z6Bl5}&^T=!kW>!|MD2wR{SWHYns^DY!HjRD3F$R5z(y(yPr&grU
z>wsz7n<MW$99mlMZmuWJ*TSZP5Fr8=714^JK}vKLHSb(BXFm>y84R7O4oR3M4GK}}
z)Gm6jpFXA0TQ+8h0*+^fh+=tI)U0ok+U!D%P+ieO<(5y0iA@_q>0HN&v<%b|j~z8N
zE$XMEelSvWB^&9M3$u#zaeCdkKNLTMz&A5Him%PGUz)yszc3#$8>?HRoeS*^yYWVC
zHi033_}w)}Rnvn{R&aUwZ=_*Q@1}PSmAHE@x9%i1#nQkBx;xA_NwXK0q{x4Tn;mYj
z`1Io<5nuSIFpFP1rJI*hKP|=x+IqIlwsEawJ{$^)7qo(qq#C1zYXq`>M=?W6RlQqi
zcXXe0m6da2c0Nt^n<3uWf-A16KbnHnY2g`{_#hZX*KKc^7H#QB;J29y`LRw>z!|wf
z@(u+nfQq5-moISKkB$ENf|t2F;D&*d3$H&o@0j3jm)h=J>yDhZX-x}jct&DjR@0TQ
z@izHvDLicQhl1uVUrINdrH6h@)6;&InvJ{2-4&{)TXww^!|A<^8Dd8gi8e|wAjQob
zrGG@nW>Lc*l$*dz7tYw~4L`bqkiL)(M-kNyK_1$mb-U2eoKR&%o$YqX{rr^Ytjae>
zYrRZOI|xf{<Mh6nw@hBBvaQH#%zknDr>UMh_o)mP*c*$Ov`U`EAlWAfKb6GP9ai9O
z$Qc$njpS@34y<s8(3a)6ICji>YeQ!EnQ(taKT?t`r@@W6-u3nMHg|Nn%~168+M6N&
z*=0UHOsHs2VoXeK4RND^R5LOPtuNvnA7d#Rw>{zf5FL1)nZm3J1#_J3bP~h$!i3rg
zz#knY3`a%P!sh7))O*lY;Gb*$(6Q6V&hWgUg+LBb*u&XH^o1EX>dSE504JZ@>dvWC
zmpeM-+bbS%<_~HLf3;i6*I9e^WuWl$!~KHHeqru?=VDmSicc_IGNQt|w|vZ^c}7oQ
zWn8^yZEGZ+mG4YuRH3i6Rdra+IRXCb&ixtD?ZMGY9S-W_qOay)g*xvZX=HnOL4Yqt
zFUMb8EEf$#ig_!!N=sY(Kwd`_9`9~J>B!XRp8d?jd`bxdp;F1SkMLY*Gb_trg>__-
zhju!)6NG7~fxIG%H{yAS>gs16zfzL4D{1AhxVX68-Q8Iu+u29@&BzPq^K2d+X@NMi
zcK?XI=VaNOUxQpAr(l_XS4O#{I!6jVYyX_Rx~)|mF-@YhR+`axHe>N_Z?c~+wAb^J
zx`28wYbD)`Nhio5&iZO=IVYK0Yaro{dvwfjYA7FvfaSdr2~FYGGF=mD@7cI?4{dR#
z@GT$R7!%*zWGKyhW_l5hsT#;@F3bKZoKWfug?lbL$jL8Qm7Xq%x4ZVK0%kkr&kExj
zE~#dMV7}JEUI_j8Y};)otjh?sMZpTYCHUN0`_LAb^(F=|{K-GkU!DIIpr@}$=FxoN
z^LHLy72)eIzLx|&W9NwvOnGr<wP<x`(W1F<0#1EY(On*Iml~RY%6WB#?3a!85%gge
zfsM_^4bAmtj!Ba?A!nwi-AXxvH!|*;&3CnhL)K(&t?#4wz)CNFf-ZF-qcYhY9US5U
zt^#)#y<{dI4!4sJU%Qp@y6t?B%sF|b7!sAb&!eqKY4Vh+R}Kpe$+5o0Q}_KK-)tw;
z4P)sUXTA$KM4j^?OD=z3`b1g2l(}Xm-Ar@8LvI_|47)jDP;(14lW)!yrMHWDp#%QV
zpFIehIrUX<{9f+vB^}gbct<&x%CX=LQ5Fm9sd~Tk<t?N%S!%KC!dh}uR?+IaJn50A
zg)}d-N?<wa*2jhrTaE14ocy7WIvwRUFJl-kGti0&Iy*MUpOSiAnvhm`^^b`?C!oxv
znRue|mCtSqSynT3XSe*#+LGP-pzIloQs9C6^FE(x4(-02{wKv=CeF&&@$xzX3m6j}
z-G?Bwa`rq2pB<CAQSxn~%+ewuo<?hCxTcZL48ApB<jqbPSyS9_;QTO}%y}C9C|G0!
zWI+YX<*xYmx{My%(;J4X)q{A^(C}48*}zcue*c>NOnopKqSm@?C39U_UvF!vE%-X(
z{;SVYFq?vhu7s!z8-vf79!xC^<S^5S1rB8fPQzPoH%0hZxrYqPx4+kq&A?|ZSY_sS
zSn%Pqfpnsk#@qGtF;LC8B#aIB$gEePPN+jUw0T!3D9U!?)$ZetKs#l*L(&YmMm8Kx
zcR%QBd>*&e;iFGr6g|W5wfdxw*^Q%qXUrrdOv;dVb08Pm!*m*xp7;G-GZC|-#hXso
z!f0q{K-x!hQv#gB`MqW3GLZ5ac^}SXpHp*Vel`c(K9a(9P(2&Q5|t#9uk!NY3_{6|
zg%A@;ERxP8v~g#AX;eo!53y@^UXxJsTzhY!eE8#rK_pFuH2Ytr&qHD+<zAfojBd1<
z^~kvcy<A$ho?kokX^x<eI<KUJmzmjYZ)0v=(T}e#hE?VE?DTYwgkj)~FCC+`9g3bu
z%Xx?@atoxWiS=y1O)P$PQX?|kis|<mVUkn<g@Kq<I#J3tlHKCoUv;4{E<QWP<bLMM
z8Uz*7M5a{+EFljcj=65far5-=<e3n+?JM6Rw9)WSG3A6upTpyK+JISqVOR?njwxH=
zE+GH)cF7NgB*uln{gq4)^qyg*I60=0Drr?!EgwQ?&TDE7e2NHXaG~;nepR}3_%oHn
zj>l&J6j{dTUy166OcF&T-_V)I4-5<-Rz2p0U-9|Q%iv%XFIkfk{I0EV@KVqDK-cFc
zVnu6HvVMLOh}Dxu;FAs(yryh!|E_1>g36T;PWab@DcH}-X}x^$%<tDuo)YGg-zp4x
zhWb<DPTt1Bgc50Pko<ld4xS?M2V_0+kEQrXW}fwdnw|Unn(9wANr`<7Tevb*P<j6H
z|2V`u7eOg_TxBGv_vek-#HHGhGhTnPbMO}pf^Y`09N{mC8hJDyd7eKJ1C?Lo<0pi3
zO|P7~G88Ov%MwS<beRh3aZ;yI`_U#h+s9kWGm5#zPSphS9r!}2_<eaOf7_PN1SMFY
zjR!oWhKCOys;Kl&EI5BL*m8ctkRl=I{yZduSGD&OWP&;wje4EOPGPMLY0rK(`^P&l
zNlJsdTHPb%QnvG#PpN8a+k28+v*gcEm%7w!JJ0%SI5R0)8E}g7&fF%0x0oHJsr;E4
z<t<RVcXDQzyZwCFR*Spi*jKkRh0u~G*#!MT=Mt_7ejCc|1Ck_3;}Y?2<G~?#0+x49
zlBaXN>0T{W0RP>dQ)j?qN~iwA$7m(If=z@zHYn2}ziRH953C)CV=8y*buM}TUM+b7
z=Mw0`5bE2~$mYz9QH-9Rp8L|luEKQh8(E66{aTx$H6)T53YCq&X&`gs`>ckRx0b<S
zirUAr+;oL?xs>Y)@m29xiT)aK6LyVb9UnXh&s5UxUuk-!WALnQ!_UaGd6eCrx>4sw
zGZSg{vSeaUm9~p{yI)mIzRvy68A?g~E2BEMm%q&QT87zKK6#?(wCVh|D~kE!-quoP
zlH-m&S@!yvv+VxX(zjvDI<L<G<eD)DU}T<q8&3ItbbqZhrR0aqJe}?36<yAFoQWeE
zfkzIODQTHfz(SsBe7HMrD%HBaul;RY(4M-5F{?QZy94Rx>Qk)HSDBgXj*rZBhl!h!
zDZ)-II}F{aheJr{i?_-qQ&l=BSgzyy9aFA0o!Y%KYL;kZ0|H`Mxh3Q{yQrCqVRKqr
zgH>T}|4ypGAtq*;6v5=lMr^xnmgwgDiXR1qDV_87Cznm^r~Tgisn$)$eZxiNWJr@*
zMmz%NvDPckLoAgeLYYx|lbQjsmbR=2f1tT(tFM0$c$T@93OQEoT<7wGal!+&Fj%~j
zz|}EOTi6lL#m%N&`S@#sv~<$x(+_SMR4KL%6`3%%=~VG~HwQl>yUbr;WK`<08{8a{
z@a}1u%a5LegPDbai}`vLS0uc5UrUD=EHM3T4+$t&&%!aa`_*2rI&0LmHsaB6IIN6t
zYu<v4n3(cbLRAmti6uFcCBOJVWX+RRuS05?eb+uy2US{ncBI_XqFDPpd~3P-7=aL)
z9dkZje-j~|lV8hk+C)6Cuk<wj6yY!)zHPocdNvEYrc0d&6j)G{+a_|n7T<lFT)%cN
zga%@`!mPeB-QAI-sah1=r&DH)4yA$gr+;lx!W_QMjOa(p!V<1U#z8BWVq)UrAYL9_
zJ3rK0EgPQed;IbtJ{NGyKPSR>-cHBn?;AOE$%hM{rKFTBjBnbQZ1(5gqk2;edjMz3
zL%5Y7<i2VdL1Moz_&~&@V#ILD<Wyt(KOoMf?ar>H!7gV?(;g`%M(#7Vi?MO)S9rSr
zXbCe~9zqqCZw<$?S2B96)(;kcz4zklM-?`VO=lv{`Yf7tx5|!3ef-H5)^IoGMhcBK
z>iq)RIE2U<MCmT{2V5$C{v4Ntgyh;-g<V~H`;BOE`^hx5fa!PpcDlotoTU%eK8B|>
z6*_L@QGSm^@j6AVB7H>9<PHRWzl_H#CIu1mLD9&^I=y|?$xjPmvNJ}v8U=1x%PCTL
zS)-Y{NLe!BQ5qpW3o~ara4plQWh6VnsWTNcD?|(=Gn07_>p(0>EeeNZ89g-&Jl}$O
zfWaxq%g3E2qF{L7jh-p7=;(e~_hflgdS;&|flJS8d9)d2$<5YJ&21=b*3wy2bT5Fp
zZ;F_VoSf%qa49^ENmf<02n};SBsg`d+DkvT7;SYqDp|XuvQk8>{A)WVNv^BbePrDG
zxGq*G=(#h&s&k_AsHpX+R)(pyDqcbNur3d&Y^MD{=mI7`>C!i8iC_s`)T6PgdL+UK
z_wAl%Co^X!heFvPZ|1lA0g_8+Zx(X+p$5K-+Okntmp?|n;tT1kfYFa9Iexpju{eZk
zw4OhWi;G)de}&cpRw2?VYxe10hHtzz701#jja_vgKU*Fm{G&PXgy^^NT3h0${od_0
z_c~BTZny!ORFW|1o_CD){IiX3HEy)pyy3w=YH<e<In;?^h6-5(BZR0|+{=-cOz=rm
z1a6Jy>ZRltXDRt3MRSQp1j9l=m?<DCJt82g8#2w<;dLg#w}7FBMw(9N(HQwQe|mvf
z$yu63mc*D_y9!^qZ*+yJyCFxvWQTw$#Du&HPf+0$!j=za@BBSq$KL*XJlDwaZg)~x
zShsSD@JO9^O|g}OmavQc+Sbx_6O*Xr8<7Xm(LGWD*7^%gEi7DyH8y)2h_`H4u2@aH
z&++#UAS0uwJ94W)Q|CS6HEYSMs;YhfpL{DP#GzGCtc(Bk3*l(>Hj|iVX_an(l{UmL
zyZH^6nGca<jOR0yoa#ou4>I)?g5kpu>2(&T>Dy?2<pmG8OpT1Bq>pZ786xWGutIG!
zDW<aqHg5_Yne`{akcxEG7@E)MXxGZzouG`Iqe0H$CZknf=P_R}2j>yj)eT;5XVky`
zO({vhPOoe}1Pt;bx8brX|J;L}{Rvi~6!hVqXP#b;c{t-sF<aeKhpcO+W-(na>oztA
zs`D|D)ne1L=&HRv|LY2C3i9oS?+-jj%1zTV`e_A?KYV!htREceAD$23#>U30{$xp4
zukK~#IwfMtV3{f}FN?W_RxN%1Y!>)d-4giaK{)Id#3|&hF2NI$z%~L94vGgz|F)TE
ztvsEIM`Ob__e9iZZ}tun%$4&#x??oVpT(XeC6Qbz=F)^{sxE>VayPAU<{2`-wj*xc
zAVqKBbivr}W1>#;jy&x25lx4N1b*-B+YsSV1&jZjgo@jss;9zE@yU~8d?Io&uY--%
zsg*BZsq!%&^eUF=gf$!&FexJy-YF2v@w$}za&lHujg4UE;F!EJUHp_T*cy#6-wY;S
z1h@#7)yW^R?7g#{i0wuS39xwX+shS0&p0@8fxRR}MovHBUl}gjZi|_?Y4je6L>_Gp
zxZhiE45;g9L(D(n&&<f6QYeF=hy93h<zld-@g6S)F~YLjP&rlf1)tSJmwWObL)+%Z
zL>?|+3TbH>#%pv0ypQgLee0o{*F^zs-pR@N!ef57jZ2v*9U+ZvoPSx;zvVbZVZ9!o
zk)QX|_=|g#KYVtEk4lZC6~R0h&uj)l@@QtxadtPzlLTi7G5t=))~YfzB8Ozm2s_#H
zb=>wIW}<|+Y~Sh<>g4-oDyQUwxZ>e5T<Lg`+vvS~-jKwxtK-gUDa5jC;@xBT>ZExl
z%5`h85`p*%0^>RQF{9dnMX+uHk4BMV?vG5rOagb&mB<7C{wX$r+xw-klm9T1URWb0
zo_}q;mZ;i!MSTUThk`9N2B`35-mMwjo_N`gPI+%!{{<{i4yYh7Hivs*DygCzY}#ec
zD@Uv<sWcKiIdF9Adg|R|8aBux6p|Rf91IL>^fqNAMHe1Z%oQJ9QtOj!=xisy^7<7x
zZ1Jt=4O=P>unwo<Wd&Yn&Xgy?+m#!T_ixtkh=iU?L(KZv_@DEnx6iN~G@($t4Z9ph
zQ#50OzH_I+sCLh<#?)r?9f`7~<MGiCV5~!>Rw4bg?;d{c0R|uNLVEoDA>3&*%625U
zO`J0fy1&+uf7cy|@p=^n->2Ikg@%X44?nAwK|9*_s{3iRHJw($t1S6LJ3odVz|(x8
z)gG1QH3yf&9y>aJ??SSF=^VQW>AA9nr0G-~yP8dnYk+6Udy)g3_q+AB3g1M2!E`DL
zWYFc0Y4d0xh0{@Jlk~`IiyxW0ieRz1`J!hyuCpO$eH5j-J@oAPyW1C#KWB6+vlM2l
z^0444Et|-GuR=n>H5PIQ;iP^HP;Ect>pAn=48G-m0DPXbv=M{&K}zpXa-!fpGxWWg
zsm5L;(yOpQJURxl2`rrt8-zJ?p^+|pWl(07hvf9gJ6-c3+!%WBSV@wiCwaR&I<}`H
zp)fu^zPi^O>46WId5Xwp?C&dWHiY<D%@qJA_<^1PcXyH#hnEKLuT;_ID<8I(1|w)<
z6V-;8<mPCc$-S3@ET&|8=wmNTV5w}}KPHcL;2AAha_YJeFbI#!A2O8mKyD_$CtBmS
zDZBQ3_LUU0)}3f+xJ=9Sj=C+{pF*Yj$y|FPk4|GiCOE?CnO#gstL4#MU*Gy*(<E`v
zS>P=X_dmv{nZciaX_^E7&%#Qf{+`VE#}|w1*DaKk>JnmO*$owK(U^j<PoGwAf##E1
zl}Td;a3W-9LJkhrrRxs28md|O;)o#76~W#>V3@s~E#wsGx&*>+>kIJ&*UfV0XtJP^
zg7nWItVIQ5?S4qxWI}jLCylGQV-s=4O`%{};2b-h@;BIIQ(yEmbJ}p=^XjE8yWv|p
zn_rs!6}2w@j9Qp{qr-!BZe`2$nU1ocAiq4!hv(1V8hO_quY9ArOv?a<&%6Rv*F%lh
z`tjS1KhSdCp6QqX=ZNauIfhG@YCwPpxmXw-6Tl8SJbYh1y+VQzp{d2VyE(Amg0X3M
z9wc#m&@t6G5k)mvcRUh6@sbZJl$%37ymTe_BKTPFWxbi*hkFa2>3p|t?QG0Bf}w!)
z>>{Uj-Vc4G)L1Pnn3p+@tcfNMQQz6e6>WX?({&~mDv*V@)G_*@Zs9c$Mq6$gU(S@X
z#U8Wwd)6N}7X%DzT%5ppuXL2i{Hz#^MhMt_28my*NdP^AsCKRE`UB_wFJFMH%B?1k
zPF`(|db6;wA}hOHF|>G@-)3<_BAl-<w>k!By-#N9NM1OqQo5Gubc`&F8j$}#eBWF?
zLgtzi#>B=Zdgql>{*b`L;%5&t80ckyX;!8({L3u-q4<_21kcH?b<fuWC&LN{>31tw
z!=v=Q-Sut>1%(d%Fu5e{xycb6YwK{sIxl8!yQelbqu*X~du%OQ7>?Tvj4RHTlFesj
z3sspm%g11BT0#*Z`?eRjD=)9>dFYwm*GaNH*Q;{Y7Ni*dq%<<fLPLW~j1rP_?cP+U
zEwv%I9l0Q3#YtTD$1!&{ADs1r=%rTG%9V(i4gY2VP7X>(jCljHvYm5Xs0!2R;Ruot
zM7G|w{;O#Ul8P*XAycGq!mb8l;>-6^`~7T_XH_tYZ#o0T_Q5O+PN^no^OV4bhN!?o
zZ!c^Y_ta!P?9EoUY@Modk`{P$pl0M<_wF6vwQHR)6v$^MH|9{^x{bgLXAqE!(b0s%
zHczX^ih;EO2mfe1kAqWxdqqhlfMP%}1q~dk?GnZ9+w}r=7;sd#ry5zC%(;vV?Z38g
zWukUJ_)1Mpo#jvR>`rZOXxO}l{S;s#sp6i+@BzaB<|nO@CU0{w+d+x~f>UDdHz??H
z-n*ifr`vje{0NF*lqefo+}z|+F$CWv^0YLt`Le*HFxogP)R4M*Zv>qXW^iXQ^QBiR
zzpcN+?osr^>dGB*rL(!%B_&2aYQZrVFur8z2Qf=?8<RU(#(;GvU<R&jGrCkjz_b3r
z25fo~_mla{Nt<)$J)vnCmJc(C?D?gF<mg>hQDMCYZOCkXM43X>d@6sjsW}?B*LPsx
z1La^!ZP5qA!;Jo93iP*(CqW$^9HI~eb^r0D|FyJ&soB?ZJ8mJ59g!roI2@CltL3ez
z%n*+h{?hP50VEKoPg_?EEP@OT1YwHH@z$uJ05SzJ_pLABIGCAz4QG_7blbd03kPT3
zvgd6wE<PbxvZu|#ar{XJwF8}#&$kz|YdrUFy5gu8!%kic)kXoICgSXMIG;-o+#otd
z<oNR&nJ)xRLOU?n2>J!oti<8zry$WYa3UrbYJ6UmD!V<xZvvd}(tnmm?^ugq1vfiA
z$iXq``*I)H)(8BwHu3tQYixVgl1km56e%V_fQ=D8DfZgnXzYB+9i61K3iMNWEoj>q
z`M$>xQKVABpU!rkiWaignB}e;EgxD0*~2@hg`2LY)ry7YS^4b7^Bg^KAYz^x78d=f
zVsW(aa;skI9&eMWJ<t6dKFrU_ao!ldJ4~yG0?nWTqq^JH!!}z%R1#r<-`~*fN#B_Y
zW#vO~rnIEAqKar8Cv{J^>i@Wr;)i<n%;?TZE#jAi2ZxPERbrll)IwJjn_9Tw@C~sF
zCJ{&*k!AhPw@uY#8Tvr9a?RHP0><G!#do*YAfF*)94`?G^#I|Lf`lYV!n?lMj8#tA
zZ7*x2VnNPrrX4DAu&NHs9s=vMxgb1T3!ARImxN|ue5rlEc%eNmH{qs1rTtXT(IJ<i
z;^A`^SB+j63Y>eF$q!e;pb$ZmbSmD{Af`DN4uJ|1m~n<aQi8=8otT%%A`)e30wsAa
z3zeL`-Cw3+|J0&ERQBhT+)ZrIoYWz?H`dT}6TAF4>mI8h4F97W2;D~d{>?9gU6P4l
zP&e|H;N#Qhk#%)NDJVFD<BT>$$wtt&8A1%;GDzTikLNO9ehj<3viCg06#aB*xE4OZ
zpjKC8Uz>^D4zwNwZ3vkeK4im%v)E`#=P?nMiF&&SP0c+zZ~s)*-{k$z<kPu+5P6D#
zVB`Cb@c_mxUb|<)xf$L%C<=+i{x=`paq}ezor$Dr@zl8RknoGH#9a*!@yjmtO>+99
zH0nl+!C@Omx`GxGf6`%3I)`Of@=I~C?kVEKMU?CIvRG?JlJq$I=}ddS*Wv5NMrPoU
z<6}!}l#IYC=4isfi9tk0Ub=YE8-yMN<Qu^$p`0A@#libB(_QE!r(h@t2ggL+vCkL-
zuZPQg*yV~&ZPeShw^d$>M}&m1KIj-DsYby-Uf70CQ6A2jfR}r^%T2uhOvUQ8H(CF+
z^`7V}b{oW6oiC`!ecTCK@t71YTo0Ve775mf!R9f*J=1DKLYmJ}QhoxOQN@JUetyOH
zp2YjZn8;i2P|BZrUro3z<b&5QUq+K=q*r5eLT5^XX+IQBt2yy+t?>o}&{1|{pd4IW
z@NF&}Kzw|A=^Y<m&c=n#v=xc|T!A}ivvqX%^#(^S5}6F5b*($aY}cXwoaN!(Yv3FE
z2PP(p-*iq9J6WRlHbaIg3g_$nK7!0=WxSSKQ1BXCE02SD%hHIm_s$gS8a*-?s&MaK
zmBUO<@6bU9cLm4|kXU!(`7yj?!u#0n_4;T^kqDgMdHEYCt=rq%`zKx@QpM-@S5!a+
zw6RdQ4K%4Y`XzPOh9BL0PbS$kA6(Z+ph{?t8<`$_u4%Q8UOJ}@NH4uJa=ap?uwX%V
zkZP%^$v9(3+<5rz!+`ccSV1UVKmd-XKDUuJxgdVoEj&}ghFBnj=C~xD1hUQ=j1?6J
zy*6Z>9+^Fattrhir20NzpFC@Mw>DPc))Dy|f`_0MN_utIh$wlE=INl{Pjj>h%cmK7
zgxRBeRd~$G%v_C9{=D#tZ23&^6Owyn#NW?R#*hGATp|6^^DF)I#Ok^i&qOGmIDKPu
z>3-i6r7(nIgiSbt{Jw00k1Wuj*|hXrsUOIz@}Y4N{;9j=lQyYw1JLvnZOM526%_Jt
zL0wu$Cq`t!L-_IcuBz!STCE)EG!&E6OyIQ$b}oN@5T`l&v8je1)DLPVBdO)U)BPMu
zRh}bXRuud;hvtm$GeY7_Df?N12V?mslduj&K1nEsfu!uSKC-A$nND{Qy4NZRj!;S~
zUGj_iKhmOCrP*cB4dQW>VHl#Dy(yPUF7_p~7WRK#pXyNPLet#mAgRlvLpOB@`b-~j
zU`H5dO8IxC9;g|=h8tSx2mM;?Q?;NDQd@dl++81#_v0B!!jyfmT8taeeC!peO%PC$
z;P<quy%4@$e8HqwO>fCnA@zeMtdpZxu0FVMB9DOC&xVSY2$>S7C(t!BcMZa2DjV8S
zMQYDpTamZ&ZFE+gvRu!rDPaGphJbVjUyvo_EfT?ku+JgvOa5S8b;^>A(knm`9g$yU
zY5$;i5*bef9T6l0m|Z9hE>AQ$CUikQXHGx$g{I@wRSH}kQd@iK4>~_RX*E6^hV#)p
zN(0h+jY=7)H&oh66_a_m4J(vcyTzK@q+Qmu`n@M{?)Q@NJj+%H|8mxH5&A~|DbQ`$
zZ-3?^GN*N$9eIXwePB2c`%}db%0uAN#Yn566=M_yHSw*LjiF=DP=v&FQfadvAB;f+
zbFJ)^I(}9Spl%5Dxg<vfZ*jWb|Kujnw<_(U0(>}uot&BEJehg@m@gFKV^(r62VoUW
zl4&i8Cdtdtv&Y~}ITQCC4Cc5KT0+SRtbSQ-q7XAM*h}?&*JXbMLbgC<Tn+S=*tCa8
zd|ooKb=&$OQI10RN#O<iK!@qX32(ca4a<;YNzTEAvKAp9)$8CtpSYq5s+?t-<bJ+)
z88cWd>-X-z1-oArZ7rN&Y0|I%u2o8sTIzE0Df{@7nNKc%K+=5R(Ye;&$=CK=9hwGg
zxR*;1?z1XFVC`koFXo3r5)PD#W(~Di6~Q`_qh$GILyhO7-?%i<xTDz;4F2b%MgCuG
z7T7LkCYhdk<YNOlXbO-jt#9g2UttP)rPL_Id3|oBUoRt>`*^1}%CrlkBKyjB(K!Jt
z4ZrjFFDxt+0a<IE4Lqx8;BY<yh{ZHBB^-YjWa9lDCmuHr9Cp__9Xr}hK5LtmjV)p!
zGp}oG{%^nPe>P^?u9eNbTTUNKk{9A?&8{{j=f4cUgFm!#^F$Wi{dpASpb#bADGjnL
zA>aNdXS!&a4#<6xjCxMplX)5SP_EzVgC$TO&N3%fO|vmPZZU+kTWF=eh&y*)$x$-n
zmF0V`>)%#`vi-`r`6QB)q3l=oFY*C}vc9bGzu0fE<HUhcPl{mrT53w)fKejPfCQk`
z^r5=La@}-qU*qMb<ecO1J0wGOH<PixWa)o(0-(-<WU*gKqG@oRFA8fREjaVPVf;za
zkURup<SxljC*^zf0PL_0{(m~`VCcU0?%Y2h;FqYf+P`=6E5{5YUp#;Ax!E7SFkn~X
z@?(B)^RnYNujTA*gotx}B9EC|Zd%&p;-Va@a`GdagQcUxfC*1$PJaIJvPK}63fPT<
z;s)fF63)vWYM__a3}i@s4(5+>IH1np*6Iv-yGNfWBo;%DSAy*)u?cM(ty#_XpKf>j
zzJ|tH8{=<llWk%9(`U!Qh2m!C>r6-bn%zuLadWu4t5{<s=7Q$jHog81T~vBaKexY+
zuGZ^N#&CRbaPVEXgwf98P)AM8P(eSbLr!i4r;(xL4RiBQ&>h{?GFrOCYd)BDpXOu4
z%bKT4RqzoS2rT@vKs^j~@qC{6lPsjhJq3lA0^Meo--atmg>1L`&Yr(3z9*27_JImX
zSQms~kDhlnhDf^f;hHmO80^2DCWX%i_dx^BmB0huC*#92$W@{U|8VfQj8~Y-puKF6
zFI_C--dw4D4OvQgfb>~h4BFh>-rwBUw$j!po$JenOxurDfyB-$yRgnkU!V5eInX0n
zdiwFbrzQoXIA~~ME*j;58X5F@MSj=fAblRKJhOngP?k+6Tj#|C-UwQ}9(V4jNj3UO
z1?0RGa~Fbj4uWF)%$Wghunfb(Dd;O#OxC|~3qD=EM&ow47ypV;%v#i?NElQNLSom5
zean;1z*)Ww$YpyGQR8aZkxF9TG>At;ucs}YDIUmctiD51*?8ocgO`VhxZ5H~_E)c2
zl`3)=$*$MQN1~O|$MPUFZ^)nQvh&jL!Y~Gg;?{{a(%Ww=GRej#Bo%d-)_(jr8x4C2
zg?IJmN&xXoNlvcXeo9TPM=}XrFb?0^m<!q2Apr27W6{te35guYf4fqQwpSP}QA0t{
z-bJW--q702DIm4CgeBbqZH_Y+Z{`~ry><8JwJaag*M^*}laSvODI!jDGs7uixUl6K
z3(~sI(R1>xF#pvQ;Ud`l>Upcb2ETe1&tM2yHWc|ZM<FFU%A);M?j5xXwH~B(b94IL
z0IRj*rOE>RN@0t3zG#VupI9jvM2k<#-JO{P5h;IC^#QFs#2CYuC;UW20pE2|Kn=f<
zADTY|X4jQ0BugVK<{8^VNpdG`$-e;^v@>%*XTnz+r~fj3<Yrb;_ZwhCu-SOkJN%I6
zD}BDu^{CcMnZHR%fLvIEixYN+)-qP;(`N>jCcGuJq{nLA?RHi*(zCMXoYx{}+vDI~
zJ5!lJq{vX|W=;==qJ|bD7@*{|8$kt7_k*46o}^-EwfnZAu5OhY4T%=`z^mR?vA~j}
zjQh=SOMqqbUH^7_<f5{h!}Ezbr?6D)c_h$=IJpG#{YFLrr$X#oV1s_<Clx9Y%+h;J
zzbUvxPYFezJ$4Z<ltC=x{gIcp_VXT5Ae~vwDVGdz3OEa&Xa%4GjT1ih^40+zo|j&G
zu8V`5n^Z_3C4p0@U+EAww#@VrxE8aI?^P5ORPNp5wL=#e9zNb$?Cxv4lF#xfqw`sR
zvRp<LGP<P#SM`YcM*qr(*u0uY<gJEp*0Zi$$9nzJUtS;F*0^UL+WVX10(%U_4w^1p
z^=pJoe3VqJ9sVr~v<?4$x`Gzi8ws-(#Q^3FRQK!{X#8;(&RiBs;B2dNn6U=j#ofE+
zO6bqg(Y#6M_tyPLkej!BqzDWPW1gr1!qoOk;c)SrbUrq=onDnpKK`R}i;nW?X%jy}
zGSIyKL?Ll-{`~pHE+m(3>2r@TA2iJF*br5%qvIVBJnz0Z7z8wv9Ac(x#Fk%LjnPd9
z^K4*Nrjb?Qc&!DBF4L+G{!oacI@`}!_5qpQsQ&g3wIumLmiUPUoSK&Q(Se-dK0+p0
zsmAEA?IOzW*|6<Z$Y^+90nEn6!@@#SJ*Icg=DqVueU0mx^N{{M>G+EIg_V`T`v3<8
zN{iQ_>DE%N+v?<F_1=~0r%R|2dj2Fapx79U8JK>2_Usub&K>u+3^Gxb*==CowjVzx
zal_bPB^-<xoCnys$L4$l_>|dhqb!erSpWinhS>IeKcj2}LquSpnlg}8mcl4Bbs(}M
zhN!0k36l*V@0hri4q`!c+yEMP2>yftiuIx}fBx&{5UCwyPyjLpg+8YJ3pPq)nPkM|
zS$3yZ*bO8kcq_dgvFL~&=%dTd7Ako%$2;I6khW>LwJ0<-71i>xsHnB5C=e)Qk&*TR
z%zT1^O^;Z+jmull$ElI9a9@OTKQOGmpcWJ7t43-7|FVffJaz8~%I?lrS{T#`<Is~Z
zbo7Th9#Va~AFfe{?|#?c#6l8A+?&QB-<2I8Fj)&DbdfuIeVTlKJNxvPe1wX@`-`l-
zLA`IN0=DpCjRbJ5c`7J?Y+kO-Dal0$J*fj@|AtH$jyAl04G16G@k&&KY4KgzkANbQ
z6&3<MapL_UA3Y%f!CmQ1b`B2g;(6}fy}J<<>Twtp_+2Wvq47;aqu!{_y?crB$?STO
z_wE#HEuG81p`;!&^@{hwJ;1$Rv|L)EX#6}g*A4;QC8WfoRE94rw^V{$?=wc$`pimG
z$#*p&Jt?i%O&_`<Rqt$ZtAcL6ADkQ93ImtDp5m6LxR9g>me!{%MN0#otNeuLVkb1Y
z=S0*Hi)n6PRzDHZq^zjt!k{6cp$A9%F;-U0fzTweeH0Xax{ZsQQ(pj%u|Y3U5EHke
zQ_79&?=(kA%WQ=O_jp}p-j|Un&ngWJ%mSN)p|><KuLc*^Zc1Nff6)9HRv6e~-S&;8
zEB5w;$1C4zdP_>n^8})^r0|U0ud{QnfYO_3ehEWl?thD{>NVD2K0IK5{XB2u>qQc5
z;&A;xPg583bu{}c9zXuUEECG4^6;i!IrhR1uwCj4wl~<c=k_9Dl9C^4Twivlh^T1A
zXK(5x3BrNE07KsiO17rrvC9h{D>$oNDBxXI37J`dW}j=9O3Np)Xo}C2)e9R>x;+%G
zz;HY6TOHPx{<(@&!yaru<hc^W6^~4E&=8-jGDee5CUQnw!;t0;*|e&Tc)4@b$xz81
zdOWB^`Fv`<j*=f-_hjW;Fq~YrP35*mI-JxXJl4W|eak;c>Ma<egpjV2B?EbdBAzn*
zZ^{4@J$yuF3P`R%cKG269G+qGLOtbL+}jx07^?yeTeRpGQ3vw}GBR6(#W5u%ED?+^
zAzlZuKfY#0${MU1VI$)$72_{23R%U3cug>L>q@F9IF<3`FsnPvn^*D>NPlN5S1-BM
zaEitUl4#0|+gg)0I03e2RZ%l@1p-*9(DnUE1sHmww=Ru@fbN4scdBPu*+7P3o71)-
zs<X4RTRaI~tgSx|$1I1z+Rc)3w34E>k+r*baQdl`VC)*zBSizD#dI2ake}t3+CP?G
z(U2iH^^TnPI<vfX%()Wpvu2<ZtS|B(SLQ8)NcL^*Avmm98(e7fIyy)P3U&@-x2XN(
zBmlvJpl67QZvioC+8yVPRNKHq|5HS8hVE;H{OV0K<M51{h5U|du}Qh2q$BkT3&!YJ
ziRkg`(}48&+W7VY(}@y-O*@3IagON4V0-p1CMO&H`rxab!+DJu+q2)BM8es*NX1n{
zZj)gZsxo=diEz<Dk=q>rWz%hIF<To8{k-t^cisVTl-7H(7}4XwEg4AJCACrc>bnb4
z-t*Qrz3tkd8B%Txt#xgo!Q?ec1hrfM>Lm{&OS(@dfmuL`szX<eZj@^oo;x~JNc|4~
z^5}p(c05`{|8c?XgoLWYq$C$qfN~0ADdgqkWc)YAk@54G*BUnE#!2Y$z5ApJ9aPWS
z%{*<n+uRIta#VC(k0-Xelxgk@i3L>M8Ig0t5+YeVIm<R5!Lr0kJp3P4t6#!cmy;R`
z$Bf@JL$^e{G_^<Qhp-**IEq!=#0aT|RKCJW$u{1FByZ-00UyXgr7U!!;p|32)oW)C
z(d*7g#W-U1^NA1cu=`6FmV6l~&Hw(q@3LMd?M4}|8Qa`<<>;mB&hmB)CkNF2A7q|?
zb3m&?0S?g4^G>k<EBF*#6IrCo61m&-7EcgT|1g6p4YZNolM4bJY{>7#>c!2`fp*p-
z7c;z4YQD%1UmxYTmX7R^Qy|xJx5%0omcHeLzAZKbT$lr{la|uoG`2&Di;YM`rIMNw
z3_E{A2qw>c+c{3lFp}#)d}reER~AQMj*#ZHE|HZ>w_>efwDy#$1bui+y~klJv)8Iy
z*Z|eG%=|}v1T;kk@!7A>-E}ftu4+S0Noy&vIZBmfu52!0QZ51h;2%ZRfN00r{$Nm}
zH?U^(yp+4R><bQ6lnM4wxd;3+zB#{23NEsdFTBfL9d}=;Ze;84d_r_vQC!szbqTLQ
zraty3gX|AoN1`ZnAO;Il1{q%-^pUJo@lNE}a-s<<3TE>=@xetfED2%AP4@Q6-v!=)
zlbTyk@@LKcQTBvm-Fv}{(#3bx!*&`jP*SFeNCBm=ir~2g-tYE8@a;=#W4^c2or+gm
zV}I&|p@@E<;@I=b^FCQP3sS}Q{s;f4di;xM(BV?31BkBbTCMl--6KfA)_`iCN=^}r
z5MFv)^~MK<B~m5PemU@$lZ)-o)x&?OT_@_t<EKTiWQ^+ABVbid+(wElO#7$4$*F{s
z4!@^Vc}P3Knau(LZXYMl7kcL5;SZ?D?6OCbHpJjD3sBS&74U+6baBjWe9qw`LZAJU
zmi(_;D#6#4Tk<|IBz_5v{Du>o5U7yf9!IO}bMM9;DOTB2#6}*5kYa<(a9BPT-5S-o
zVbpOEi%0Gi+)76x3KF<^iL#nMouI#Jqz#et<j>jjLte;c{C!GTK}VEY)!7p$w^htc
zOpKAIBr_;Huq74Cui0>eX6qsFC%2-S&dZe%2j0#RIW34qUr72yv#Pd8x(XBpXIlR~
zMG@*yA7^QjP9WV&7isk!VW-eWgXtu-{#C4DQ<R0*6|nmHS9;|($KNFE`1!>}oaSu+
z-vblvo2+jesYrPKLOQxVgqo|?<;NPJLk0>A7$GbQy^Vmk=!nlR(#5ZMba(>_9kV9m
zWM>z9k|kq6=8h&^c1U0cec|s-)u5+7`Kw0-Ijp;SMnq-LcMvOJ{t>h`LIQUgfLq0C
zzy#)npI!SRu_j7092<!yof2^x1P*mT+76A)r;lo^)@N;VA9#NLw7UAGxx*SUbMtV!
zP6hBgpynU6Cc`2qpr}1MoVcm~ZhE+E0k9s-yW0)<VZT`b`H@p+s3{mz0c*?;75dhR
zuq-3|MF6V#&WiAXewF}$%$&5|$VO!6R_AtIvBJVt_e`-J&?csS`cui+GeBUCRVfH8
zwOEoJ{zpN6@*f2GbZl-#2>q3eV^b@q`?{#WKylDr0wM{Ep%vgwK;v_6M1V!O>ev@Q
zfI-5`ZD-XPH1Yw@XZaF+y!*|FLCkuz*4^mg!{@z2isDiM8T#+__s5(+-lG6ntx=7O
z>)JGXT%0EOoL%s4d3kxeu__cmbO7iS$pTQMyu3VNr};cUF}iIeg0PI;bH~{`=H^CP
z{pnqg1snPTuF+o<NGn(_xY`9;pI@Kwa|F!q{Wk{{Yt{empgvboq%ZY+m5AExz5f>3
ziM2lwWh-<~uPl&WK)T!iw9T4K6d0iGQW;>ax>BlLcGom>p{vVd?iLmo0N(?8ltQi!
z1#X2ypk~x6yxF&*sxwsb!WMu8pg?RwW^W_U!l`M7wm-jViDB*Td|6lYYHA8#QG6*6
z$(`~M{j^zu5PXZRm5+Bo%D@txh;1VB+;ft}wZvYk@I?}74uF1r97!hKNk`yqISlAJ
zB2QxhWrY6Lodg=`#r9LCDz)9qWBWifh<N}8_5N^cHASRO+;i_sPx|oZHy;6-3>WU~
zHhR1~dI~hPFJ7$k@;u9~$qS8poCWS^g!a<ri~v3@cX=kHjlg{3+rjNDu665Ec{%~k
z@+aLsh;c_FRx_+~+x)mO2LZe$2v7jv08j#>_`z5lhlI_9A+Kdu4ZxZ}7Zw21luO{|
z`^>bWlW@|T*L$BuU&;A1oe*?tM1az`8iYNKlztk{=k2l8233};s?!oxvH`-rJqqj0
z(J%9K`X*$;B@TJFyNuc`+qu1uY+Vt#dZ2GuKmQzF1|)>zA0NGyU#2F+^4T{BYpSFk
zfTpI$fHk2|Ff=4QywYg_dz0d0L^lI~7ytzLQrwfre$rTJq_lLybzK~=N$l)aK%0}x
zj?T^wl`_ElB>{3d>r`Z*VIQfGv1bbY0otPl;fqD6SCU`o`HIF#26{0a3U|Nbc;Scq
z(9lc6swdZZ9!K^L<(0!8KgI{>SGqz1sAAXGW>$b~bzcwFkpPEVhetE)ZG~BR-hpVj
z682h?J+VY*C?aN`@XOxacvS&2PeAP5S8@=aKW;$MC|=`{28}_}$oX)*F6`YAJSas@
z0U(tBN`zv5ijElvYniz`V0C0LHh?bK+uQftm*GGIccOQ{ID}3)iT^8bPe3YS5VU{z
z!N>QS<L=rs&|U`A9T7R*X^IG-zXd}_YFrnBs4{gwdfGaIU;9vP(kPRJ_F9LH6&ScP
zh<lWHd44|FJrhnZB4plrYsTq{dZscf!aT61Mh75PUV9tOFU}+YBq}5M^10I|3S)~E
zm+b&BEqHYYwL4C$sY|atD2iMo9x^4-H8|%KW0m`m9&F&99>L+a9ad8<ssaZD$Z656
ze1II91NiMgL2UxJ5g93|xZ|vcm)C6#jSV2-$ji3}rXv9yV{CkDm){3)Ce)nT9!tZU
z6JGA00WJ>Yu(fF&fDkJxa<Yd7&u@Rf|8;GKqp{(t-RQeA*LD7~X$Pfh_dDvYx6}d6
zQO1?}i7!#Q0qazs-*=)XKCwmIzjgb5!$T+1XLBZ7&`){@R6AgR@>EWygGI665`jwM
zEgs-3fhJn0U->^kK-Tc5^L+@{b?U5q8X+Q6G*+In(au4`v<MJxg@|YbQuuZ8kBZ&(
zi%m^U0M&d6F64VYxJQL&D{v7LI^nRFVd+YZ5g6bUJ=JO&*=x^&;arV50;)xzke&qn
z3&N5K11x|&xi)u#(*IweL?VV?P~rrk{pP$1xC%6Zji|zW*Kgf&0U$lt)~Fuoka$Dj
z(}C6lppC>^Ca;0lf};W0IaYVfCQ&EJj=eZ!S66E<oWyOA6BZndDKn7ID1fTR(qzM!
zx{3z=g9EQ*2!Eia_CQ}>{%+@d{|oc=fx0@y`H6_lxY*dZ*c#W>^{u7qxw$vzt>RL}
zb4p6AyHn-M0HV8V{b-rb8G{9Z-&S62eUh(ZHP^MaW_J$_+@FP|iUy(dohvx-7Avy;
zC6EnjZm%_)<CUy{Y5-W_PoIz9uz^v0td^1#7^XD8O7P^LDZ?$o|0LOADTD&B_~G#p
zesBnoj6txjbo{23gg#oT2++#YPCuU_;%qWpN&{l&g$va{V|JVCi34t7X~HPI90q`P
z?HU&Z05hyLSmw}!NYDKxlad!oG_)rg?mq=DagfNyY~TConwdaf!usRr3~1K=zug6(
zhg!nXIfZqMw6p~-i0J4Nz|?+UhF%tO*eEcl1~R_Xjf>O;1KOxExIkYYou(cNom9c4
zT@)z#%Blw$7r%V4w=mYe%k=8s#SM^7tTD(z5!jORxe0nyh<5;VVf})xWngcOkgAQ{
z|Dc0KGSdElQ0zZpt-1=Bq4#B=oYr~l&Vz#3vNI8&Kc%kgm<*P0vFz%4MkbgwLlpSs
z6-j7A?RVid10ZYg0Bil!y>)de8({4{+Ni<79iTruuZ$<eL9g7%nE*!+^gQSU?IT6t
z@v(g0=X$>ZC1^e&B|RfU!gJ4byrx^~P6Pj=QGi8C8`xfRWyzz-glbe^k&v;Di!mgv
z@4FE-pGjAs3#-b%W@Tf7)?Z4jru&a(<y`{v)-%l5W$<CIJJ_w5u``s+`2H5SjSVaj
zCC1PH2+Tl9sSAj!00n%sw;`Gy0N}Wz9}&YXEfeJQIpGnE#;75xbJ4(Dz@3HTwT`8n
z96wq(=ora{UUHpkxW499%IJOY4P5L167HA|1nAdhvO_C%$Igc1oHNBrN$AuBfqlTQ
z0owgBaE^U_XR(_{iNz}Uag^a!)!5=?#Grf-Uy`~p$sgpoZSHsRho(XMGwfIUbD0hS
z2fvx1{_Z(-KoBw!W-qf%=7GY2MRh%GewbCTns+Le`Q$Q^gbt)R=}F>4EZc)3-~W+y
zXCy7dCR|qRO$ie4HgY)hl$0N=#^%<>Kpy(^OFn@)&#ILm6<j8@_q=1dm+)!OQLn~c
zmZ_h)tOwAmFE=7QARiZiknbu4ZVvuO8Im!2(VAH*W~+7f6X`Wqy$HcZ08PC62WY|y
zyoQ)o{QE3{+{VO|2CglN9~33XmGuSnztcq-*5<9$ZhJ{CL~tkqsA6Fzq!XyVrC8=a
zq#3Wz6k7mysVP~V0RI5_$1{BbNYo-t&72pS*EG@E#OL_{R^D?G@znnw@j5;rbvWaF
ze0+f9Q+v2IVpqF~IsUQq^7-@nWaU*rnYryRRgBs4HBML%qC#9jHHzctHY-8^02($p
z9`w3o4EiU-`9Jb3djBAFoD882L0HlTlt|hWB~tir?AE%-?XPvqphhHgl#@KGdd7$J
zEGHo^%_((VEMWt%Ei8Ww?oR)PTL3T^XKivpP7^|ncZ!TsIe;&@ku`%HRK3`vg83x5
zT;yn!`vFTv2%EuW%9F~(6TdGUx3#v;dIm4wl7~XZZd1lkU@<|TqWibff89sjn+g9{
z@;>bNhtA;IbRGWG$}QY&a%?#odM*%((&mgwr8|o?nxD8H%>E<5(FkolbJ#fG;PPvP
z1_Xs-ih1m-OX>~gfZ%nZd>U<F3@$612;4tWSbtq-xF?Lg&Tzx@6i?R7?uY`Ziu0RK
zV8<L~HDgrpb$a7g7)!88N=1h@3W%nc^jQV~V8fynP5+d<kN&Eu8g4Sen6H;a^*5`A
zT)h43<6&K`%t_2Lx18GUi(<homW!&+{XjJwI}0wv0NhahhVkFs_^3sm4y$s6GTrcG
z*VDGEY1EE-SW9c4ygexQ)E6inFUIcvqJjWddX|!T&KUhXKPT`)Q(F8tJ8mA)v9LRh
zpq!@HR1zmMSJHa#-HP1-!~azb24#xoEqvfXX$_IcvVmGL?bCCd9M}I`lK)$w{m<mc
zzsU{%8>-`9HIv1EEO!A>4~vG7na6<M6ZE9qt^eNuUV81Zz{`<>c$qe*%bngG*k?)p
z<sl*kY!tjm{GSM~sjrtmLss8JsK<2^mERlab)T4Lx^QG?UkbOfoB>zIz<&QT2Ueh-
zdNVJ!v2BPhvU$|`q%v7K`(IBjGs_mgCH!EPVy1U{<wZf}#n@Qr!{(vbC9-1F`=?w>
zd31mIwrlixh}QIvy)QJUE1eL<I69sAZ>RFKQ9wn+lbGY4eSY}(JZ-C&b1k-v?g;o#
zAUBo!Bj-M*FFkxBmQJzgIpKLM<>a;4c{&Wb@VR%(<l(5u$`h>|Pvek-y6{Ugk??2#
z>K=**LLGS4o|vT5DR<5KaL&H}|7t^G(o5Fp@e2R9)DpD1Pnd0G%_bI|+J|!Tb+#Ff
zq4q;RVwbkn6;clXB7WR2vY4{G(t;j}DSyLLw)1!~uW(}SsGuiO{bYoH{A7!j#pfjq
zSg;Bzb6P4Kb2ITg`IFdFbA~TciF`_=|3peU^`DstM)cJXhl6Sag?m86!Ki65K}&=@
zm+R}~{9&Zdn*gwh5yz)Pnfk|FGu>BNW7Nu_H3J>vUmM38EBc7Md)D3?7f763;`(Q|
z6dMwid4^-g-hrRUoM$`b-=+Xq2=xC>o-MIk57ffM+z4B`_E;?R8e8p-?iISEV@Kz7
z+`TQ4`rytyskY0GXYQMoiT}geTR>Hnwr}H<(xM_Fp@=952q<s}NkzaQr9m1F-2&1e
zp@Niv%Auq?B$ZSU$wPCHE=frNY54BL%)IZsG2d^#-~YeXtXVU&oU`|S_OtJ}?(4dq
zJ~l3Cx@|OrM^PUex!{$5h2LUT&kkFQXsbI4-AcQGD-L4htjMFAsu2Hk#{N6BEVwB(
z3k-{e%Fl9!(I3WTzvw2~P#a`H6b}+i-m8zy9V``~oo_3d96Ma>W~4i~$C<nA$0bTM
zU$?={ah4QIJ^fE&pzHU!+WUgBv1nY4R*&>?Vbk4O;L@E9lDzj^(4>a$sQJ2f)O`Im
z1Ny%Y`v#PHN^uPlV5r4zKURP)Bg_>Ldfc6RYD#vWnTi#t*@ad59f3u+?rBu*hTa|Q
z6nHYVT|=cKtFt+|tJ)wmJbJ!hW#xI-Xh2Sl=t4{R1rhv@I!Lurq!r)gZx{6+;?Mok
zE6nZd9XReta4cND#sA4L9c?)i@T>tH-)QXikFpmKjKME_yDL|x5pI0{cB|m-@KxTh
z{Url^v~ZPioPfwpJ5zbqZ8s0dM}+@c`RK^Z!S!-G>v+>Q<u;vjJ0;xQzg3v1ySEo?
zZRUoAipr^oE^D3VsAxS{6R~Jrp?JEJHTa9{!Pe|#&(gBTySDGxdHHsSP7xm+=x}-*
zhcu{w6_l5jqW$BUpWos)#b0af{0U+<<(~UrYyH8l;oQu^NL7A;tw<*2nKQ!fPf*?c
z!Vc3Wxf88C7H1g}wq_NRKidL6rt48M37j@Aol5NgI`lE2LiK{6{bP^){n^<A;KJ7h
z1OsvveiLX#_V)JLKXwh}2FVe|EI<f<!?<@|inv3@LHuacLvAAZnw5BGZSIvwhxjj{
zi$}4oLFeysm(kC1K+FDom4%ft_sM>+@08cogsiSN-C`W}ieiDz-Os5O+wy;hvOtc@
z!)i7bNR=Hgyln*{NX{x6k=jgko+Nvxg{xTBl;g&!is4RX=VIUTA2kvds!9&)@RXnc
z0eO+=q5C+%Zfif07Op=A`GN7(Ck~=zG8*oe%Ia0+*F`<O(+b5duqiJ(5pG`<EPn&a
z&yW9}A7zX*TEDFO%g&f!&WD`aez9$rnspBtsNz_hhLhoMGb~{m1*{WVFW%;tABy04
zIm^F9B4T)1(}6ey^tJnI;2)x@ym0ARclwNW<&$jyM?pYEe(rjh5hv)(){vlabszhb
z`20C&aPr<GH#W!t*1RD61PmlXzr|L$Z7vTwkHNFcCscncvB<AGb-#R|*sKLW&yjLR
zlkwVmkmgzPRSck}rqJb2R_zTZi{|H<?%m@^HUUV0FFgE0U?3kBwLlgv_ba)~Dr^8p
zN7Uu#V|Vun0CKJP+(77;UerHlIRe@XWYH*Szs;2a6_l5i6$1tb{2rFx1RYyf6vMun
zoe|Jr`;<~0pmUshRnLN>LFPaU1dJ=Kv^7g!OS!uVZXOLMs2$oo`Hxy0_5a7338sv<
zCF+93n$Pmc-d@|eF7f^+a;6Vx3`+wg@@nVE>0bW?MGG+{^Vct5tiKhkLca&Q{98c}
z?1&J0!JC|%2V1KVAXvNgQsoCFiX+~-E2U}F%WJ^e89peRrdRGjEgPxYwHd7zGJ7az
zF!2u5m3iv#yFlZl{=N`gG79urGc)H(O2kfDybcSy6W<{td){uQEguw!5wDJWtfAw1
zOgO_DIELdE<3%_mWTLrykwq+*FQ3Bps=aRL=Lk~Lpbri+efiY`r|9U!Y({1Piwp<|
z_;Grrx0PR1*%+;3^P{MLWa!b<9yY1>|LdHnE?T|lkgF@)(Nb$?glnYi7q3{J@=^VH
z^=K{GF$;%N>67^X^k}Li{!T&06n@XOK0ADLZ`?WITV)*Nzh4-z(wd(}Yk@`_HUO=@
z_MfbOWKGX??C`k#id|W`izSh<KD!gqZy5-5qUA{&F4dk;nbO%Pz)U749ww=&=5t;a
zDtd0u#>&dhYSy15DeAn`lb#;>@I-gFG?4f7mPhsj5lk!vppOKG$>3s0PR;_nkYdr4
zxo;KDhcE=V>kk6Ts3<~9J~x;!3=Pet)WP3CBB-ib?fAp#A??|-t3Ywh$fyQ(Vsk&6
z$?enNa=-;Tx-MD^zQ?n+ujF(3!5$`Gw<eJ{L14AJv9Y>2k&ERF<pAo*zGd3*g|1&=
z(=jpGiW$F?gs@!6bX)aaLL=47!Mlm$IhAcWzfBQ)o?MK6fGz0xf8U&|2g5l3H0R17
zwxp+(yhN$aQ|t2{@ncxXw&_*s*`TSa%@GUJCtENWhK?3?4XvLqNPrX-JwRH-bSX(W
zLG;uqk4sFtUSLOY=gu8gluU$azOrUy^h8S!@f7)f$^i&gRa8c^29ZWD;ltKZP+0lw
z+k5O-c2p7!gJb~T4-3N@U;vu>J7T7ya-N8UWTYtFesO>txP=h#gM({MZ2*MMj}V-k
zB>c9>fqAJ6e?|u!49Z3?fHX5N-v;QuEc+DaZ&oIb-%?Px-ftl#rMAQP;`{eRfBzr6
zCfTtg@C5*WHbTMxeHTFJ&mD<%_jW+f2?LbxGx-Ac-`{q1h*379$<0cTI}<V=K@jgN
z=86miw(~c#lNTIuqOrpoGlBFABPW5Y0uX`4G>K}Q1ir>_sBVfrx@R2mho>73o^}9`
zrJ2tfTcRUJ3=IrC54Nf8pUn5rTp2}xSx;Dukk#j>{(($EFBj;jQupsj{pxMxo>JJK
z-~ZJtbBg_@weir0)r}2`+Bznxx`z)RV)5j6@AL}$WSVLm(mi`JUIIi8-}W0LK=l%<
z5Du-;Je!T-lTN!oxOl&PEHZ$lTgvG#vFL-05uOJF2u#OHK`)BjeLdsG)vF%>U7P%h
zZl1jy9)3gBp%f&otQ^BMXU^oDbs#3jXAtwekK*MO?osD~D|Q#@p08F!Hd9M~7_^hZ
z67e_WW2FTj?w?pVMH3K(8_g)=n`a7`3yrr5b*H%M<|MV5dJ*SZ1uiGS9~N5TvS7wW
z8B|z?Wbpjv0E1kJ4j~uf^xvzOJ1={#PKZ@+Bm3SPj&f<e&ZilH;+KG+HD+^r=;PXB
z<7mwU*j8~FY#y0~NMLMTDtPKk<@~EWQn-fZ%=Zt!BZ_UYac>1oTV7iM;FhVBPIycZ
z+y~r%4{2`hst{%f*cgaj$w}-NhBOqxe9G3=6b1}1ASu!0Aq3)3nGkrgRX?8l$rnX=
z)5U%XNS{V!&rYUFmR6Q5A{=1+Wk30q3s7I;6DM->4wis3f9qBVF8*_bLrQ8e=z6W;
z=iXgxn5chXVUZtB2|v{gKqss|jMdO_yDs2#0{>2MjCHVIv-<kuPdEl>%`PZV(q&ti
zteSL+R#qB8#%eWM&2)2T@?He9*prnH@(G?K<hpVhn`A(>xr&?joadpe2P0^Hy(Wa4
z!-4c0+IT&OtdUHg_OT=u8o<dZe}25MoT26}cmvj1{F7!{P6obove%kNP~o`X2nG{c
ztFpmSk4-Tio`%ZZB`?qQPH7-?NxeLmD2V^23h6OC2YT(Y43GZLKgF`{>$bhE46nSE
z<7g9Zg1IItnwT{|2*51_I~ZQ~l^<r;^B+4V2wq|PlME&Bt)3Ts#G@zvP$7)zhFwP#
zyINe^G}iw`!1k^cIw>g$O1{8gS%n;pANVotFiy&|I8r{oH#C6?4j^9O?E*jz&PzY|
zpBzjAtP6%HG(t}Ff~g-q_<`aVED_)r!Cq%=n&a}el^ePkV>wdk4|+$Zr5qv>`;!)D
z&wM{8$UO<7M=`IF$IBzW;02d;EP^Y&7n4J7f7f=!!UDr46{U5!bK+GCjUYEW6B7`5
z?|~8!=7}_7p3UI7#VpfUe`FulO8NH5_>Y<yuK4SW{JTGr7V?8y`R|}PWxxQiWzX81
z^dD~f3SfEIEx-<7BO9Cf*r!`YLH+2M-4%`}e{ccHz(ql9Ys~k(VMDkgXyp^te`n7<
z05eaoe2ofz6l<u}E43KOJ9qq_NeB2#`uX_@(Q|aASh=0rX2X$;D_-DIS`XYgrfwCO
z8>h9Zc`nl`#QaOhrZ_7TR08GI2i5k^{Wx*+1;G((nxZoO*~3Np)iWoucf4BzQ|!ce
zW23;?nNo?+O4n34tTh!{fe^Z!=$n;Nw10f6?^?j@uKP<W5dLIjT0!~G`Dm*FT(oMo
z407uYwr7(=L5tCY(GhyGVtemiCik8XJ|Q)Src4f&X%oHNak(0tEiOUyxP1AVfIy@$
zs_y{(C{VzmxQY9xAM$}`cDSNfFLQP=cZO8&9K}tWky=O#P)ZyleC7#thGBzTX$^s@
zN$<wa>dUnnT}48Rw7nN=oQy!K9$`$fId<ejB#zzgWcoq%AfUox+2sHARS?9!%#BWS
z_N{~Xwz1F;y8GdBM^bX~<rT`)T@x*hoIHh{GVT1V)~VW0W4M{@@6%)tTl738B&P_y
z#-9DbV>b`#)bZL>m`hY;&$_k+HBq^If(wsPyaO4YCm~;xp9?}98KPV!LE9Fs#x6=!
z0(4GrATiE1>JnGIx%!6SzpbrbavTyJxkhBVKL?&*b*IdyB}R~`p4YpPq@caeWvFi{
zwq#ej@WaAwcG)pAOVn&?-)nG?)A-KRN5%i(-@WgX+4h~*z70ZzDv3LzBuB<nKG=qu
zrWRbFUcE~1ZN}_#xrjV^^P=94@gH35YMF~(dgqAMy4ZDs0U)Z`{W861w3{CTwP<8t
z7|mtnOEQXjn#8H+&Q>)C>UNhVQ9iV>j5F`(YkrxSk}Pzu^vw`!l}jtEMz?M|?}phO
z-!yjmaweR{rN#nuIu?risnhYNQTiJLG}-)?q~dQ7D3>*PT?BzSmH~`sf-hhh<c;Ex
zcdqg&65?)MJCZO$k0Ir*G+2FoV{nH`t&4M9hmo~6QclzU!vMR4N+0XfOp6oqtk^y`
z^<P<F<k;a^tl6Ii(0?Cf{2ys{E&t9K2t<(XrFhMclTQ;y<zf|shX0R>L1-bP^EOsE
zmF(E({hYO<ZF>1$BYm~h)>Nl`Y_sN$I{AN7_x*o)LXhzyOF+gO`|=gpu<I8Z%zu{R
z@JcV6&^9tZTZuD2qHI0=J6PYJq3g*7J@kjX6jU2bDG=10KTn3$ccdL{i3@<8{*ztt
zxB0#Q>DTk0x9!pu!DMu?vaj`=0J+g*yhlMKcD{ur{QZyM)*rIUqZtq2pfr6^%H1D<
zFf&~%c>%BR+yF1oTxCX>{<n+rAL#DCseH`@puh1|&;kwJns2je1Pu7!8(_(rp1%pn
z|2|*!Pv|>y-yLS`I3jxNXC9^auaKDZTx^jRog({Zm`(dD3eG9{|8wjCo}~W;u8xM-
zTTuffaXj~3lc5hy!g3u3@55U7^?+^4J9hCMQ3gzY_UL3=X`G_5Bcq}Fe{(X0eKCMb
z|8uv_cc3<AS9S7rRI?Es4Px)#-}`DZYwJg&W~(-k{iXzFfMn-jL`5z@bQRAWH|$jW
z3m+LQ$rY5*SfgF6H`0GokUw2@K6gc&2XroES9We5ZHS-gK9=@GqnTf^a~Mv;Kg>mQ
zFyfw=R&(-&-WJ-EC&x}CgJ%6a!HsQkV1|R;4|eQ^wMm1|%mN5U|DYx69GYz=0dsZ@
z50x&9)30PO(z}T&+M&bgF<X{PwkZ2aTPS9*3T!j@O>4r}`BM%hzuA0p?FnM3k9Wq#
zuKk06UWal)!?*o!vghc&2l!^sYwE7zqCm5cyxQ5fiYgk2%<NuI@7Zu&feR$99J4sv
zu`_Am)H=LPRDfLi5)FekheKZ6l5+=GuD0@j;pP5Q^4x$opC=C>i8BGgcl<(}MqZ!%
zuaT_s7J-6RhqS*LOYnT?JT7l~oM7*8p~^*>O1dV)qs?oSf!`)2x${d#f4Q5@G)?rG
z7e!Z}dK=scUJJ{38KP)-hKJ^K<QJDuM6?nkYM*Y9&<1BbZ+?fDlqAHp8N2McnPU^@
z*6z`+{X<K`*~7>Fjg|9q(zAJ_&3wZ^i?W*9e%9_>QpSEt`Cv8_ta;a>DyHbH&>e>?
z5m%a&+as^&X)?d#BShHpJu8ZD@->tGCPe#`(`vxch!8RC0wy8mZFi&4H>9s`G^;Ax
z1(YCfKGSnLt{V4lU74`ab(S?hI~uIhC@-?x)Oy_SiVHr~Mw%0Sd!G>VvLi@lFx4dm
zu}I`5E%B+p_y<u2`WAUCdb~A~C{0M-unz@X1ws1rC!`zJX1uWEh@A}vO8V~gxjA;#
zoZH7#bDP=Ng|2W|7P+{&?eZZIUYo-gV$g(5ajmTjg@rY%l^)R8fV3+DGtbAuWwYo{
zsuIdjHd^hmIcVeDzbKl-rj*V+S~Ym?!iA=_UsUXc50$zVd<F)}DMa6Lu?>;(fek5N
z7B`{*bx)<|tjw7^;$#CotzaJZlRb&1@F(+7`sLyNbK{lkT0Ay3HTl(STZ?6M92{0t
zQ|Vw=9{D8W?c4AZ($e>v9|zT=B8qnQwu9jMx*zR15RL&EuKMG7uU=7}4MOlcqrqZ{
z<LnPCht^{GdEjSFo?4Y9M8#WL0@sx+T$AoGd~*i{d=m!&o{g!HC1G?#PXAjw#wlu(
z5$pJ8V*IpX>SZoDJp_XFfo0bj*<PbiPM~IsFB@E}rlE1LFt>&%_Pluv4Yt5hk@V6r
z1Qw-WW~l3#S?-+QG&m@4gl3kPd-Rc;l2G}rdd<P)GYYM~+`*IQ&jTDp$Zy?qmTK{A
zFF7&sV5yZ*v#{sh_i{%%1=sF0jvF_a?)bZXPFq9mJkD&QcIxVNfqRdWxV}dYQ{j7#
z-Z_4+4!BW*z6hlgx;#cxvLEE{l^5Npb(t$eK086h&o72WE~n86BQ>M$2YZs??XQNv
zV6H*qH9iZN!?T5jO0X<N@R#}flT)kSlTRqI9X}mZ50VjoFqhK0hB?I~W%aG#p1RbU
z5HCAh+hvw?5{WA;>Gz{z>ka%%kbXKw1Dt8cal13_)8Vx}WIjHHWA6JYuolnc)X7P{
z{#&xn@zeJ(GAXTLLKU4`K0R**>H{xX1Y`kAZu@%^cYGNk0fEQnaQxS=gg{*$EHtKT
zAB2_hR!xO{SzUdQt!f<gMom>U-onbmerd@6hT+#lBEFbSXkmc0r>LVN2&S3g2+H!(
zcCkY^reXy6L`6jobhM}^Bh}_As*?3{prS1+5y{OFrl#f(oLThoVdUTleHEd!N}QI8
zHnsAwEZYUv;)j&)TfuZC>2fMxzPxnbDu|j{ymF<X(@s@Id)mcK7M+`tawfj)nZG|^
z!|&d&dTI|>Ee?t`bPDs1SAGBwTRJ&iDWr*>hf7;pI!>WrBf_CHBt+&ax`B|m9Z`2R
zg%}-{nm8o(Y{~MaZS~Du4Fv^Lx81c+v5}Wwx<OOP_2|PH@aVg6!DZdFDPiqdR~Lh!
zft2Po%$G0U0|R*$7egMr;gpv8s%PP}YS0@h_BwH%k#QVM*J6d7byQWY4JUmQyhJxz
zxKy4|P``PTz<KrRoro~_m7+rX!GU=AA?WZ88$w`1WM!3eJ1<>I+<TSg;l7s|-Py?m
z--Mps>)6<N+s9+F`Yr{3^R<$3-JraZ@~eSWwOG9<Cnpw~<NEq~c@*z<Tii}l1f|Oe
zr=|s_>*rN28!Pv{x5?5Enp@aAaR~^Y4Ob10{rFL1Z!BlGdP5EC_8FR;l_&G|Ix8dN
z`>c8HQY7q#mZqklz(9DCYaILGGmG8n>3uyzk@4r-2Px<U+;@MSe~iyuiWIh?&O_8;
za#FrOad9z*%cP`4rlmb&W$iXLbiJRpHUagcwSWgrbezw|lweFeUgPw|zE%w*^usJ?
ziuFmfATrcKlE)}iNGW7;w(Tns!DG#$FJFwFB^yqBX}bN{=)HPNi@chOcC>)qPk>Ok
zbaG17K}>VuTj^p^M#eXbK64V%paO*3R)HgYuSxS78XqAfLFoDZUD*ijXMrw2uyctG
zm%J9+gz!Yosr=&Ub3WtT=z4?tiI?wM^>S$vNuxgoW#0NEQ;<~ZT?7Zz#dr@0l-4$W
z(zsjK!7G$8*Vg?B<ycLA@!GXLm`e2bfBZ(jy|smYyLvFyqNbMH{7SQ^xtUu2DxHv%
zyWu3ha`uC=^9B9xcz93gXqVsv>$e=)`iOjjRVzjTEi{P@kc%%EvId#^_dfzvwS8~~
zt!2AvfDpZ`Kk><@?n&>teiUfM#5{*CoI2&7B_D^Pr>5p2o3aBPLqJGM(%1r7P7)=*
z1LegS2`wIz0%S8+7_zZB#x~*AV-rKawJtOCt5*-{LqeJ&(l03my!Hj@^7sCt(N3Pm
zH2H)%+sAjLzWVu->sB~P+-rI<JuO{d-%wtHgybka&=Q4M+aQ1PYhe>?@RJQ(<1GTd
zL}#lYuZrd%nAR_KJKaDKy_HfKIBENu`^Hs51v&!NGiL=rPgB-(!n}NqH!YV2C3KVi
zyjs4FF4!;rlr}Ttxqm-Iw^ChO(Jn1*T2)Riz6Uj4hD#LOo4}8p@P_2q9Hmi+S%5w3
z=f|3n3*2)(ivUArX68h~yen5^vvYGJN;HASEae>fvr_vBSyu@Xz9buS*Q<12BzS=2
z!T<6;QxQ+W{r2HvdT+}U=IGeZDApQr6yrZr@`H2)>4_6w@K}%{;A;eoFjxJ3^Z4vt
z`GhIk$3(gPjp3_~j+<H8`AFonvT8h)W>FAzwp{EMadc)!$MZZP-kHf>qq9N1us&;~
zj|FamV|dyqDp3VltA$XM8L!bp6IC@9Ys--1r71@75yYm-kTv7>9QUE}Kh}2X;#aFe
zU(}Ltu$e``_sF{!fE7l*SKeA!s6>(rYI+$6ADoHZE;D1Vu7E&Iu1T{9vI4kXpF!+=
zCR`)Nv|4X^zy^o8&KA++!~{E=+~A^UbFPM|=@W=z85ySZU%Dx=QBW#1yq`urb;err
zgg75z39=33jJg-mVNv|!qF3hN^O{NR26h@|V!{wIwb%Ak-^Y&@z&}*g-e+%j!v-!^
z!5fxu0f_Xa=29g*=6|V<>hx(H-O<a%$XP2KNNFa<Q#v2*7dw+#06d1*X5*E8l}k*-
zX@Q`!ta9(M*<5I2PU5b(fBb_;C&y>Z)i)?dvskm9L#c7fHQWrF@nw?PtcV02M4yYq
z&mhNRCn1fM6n{B*RBfRew0Jaa@IP_R{hZ2%J9{!B>UEHW!LT%9&-_W0UPxxyoOING
z64#d|n?Nt3`4k(-Z67?)dh*0?{RH~yQyZuSn6IRMTdYn^Wg)`6oLPj!T+on|U_a8G
zJsbau8?5T)laiJ@9neO7sPlpkD;-Qu-K5@oRMgc)iOoI@p<rXFq@~60fIKd$Aur<%
z;p<!!Hn&8@SC|Rn^wakorF%~wUJ2C^zum|bAv&hmH=Y*LGt}w|3QzWTWQ#wDa&vz*
zc>ne5*Wrr3tr?L}a`H{!0PDS12kF+xx2-WuZaJijE5%4Cj#x}K>Vs6^Tn1QWUI%0V
z1STDs4;aho)|X)5LNa>%y1kLGIrw6iL+of7;EZL3n8qzpPc%W6Lz74yU5breyjfiV
z-bIlwZV7r1Tr5Z`_@qJb<%6t~LU&xc%PXKRyX4G@;QU@jDeprO5Lkvgc9DjrXnABY
zRytnDdAv2+p%LZp?+-$X1Bew85_PMT&3?|(I_1j2)=kCzI>M>dm6i4|Tz_bB_Vnpe
z2b0b?A##cf#oyt8G%cQrVdufpMab+BcHn*a`%5}iP<%#RVP@{dV6>~<Rp9mb5{B|3
z5Mr+DMhzi&?r9fLwmOfV(0SX{&9yW_NaJenUq1u8_TzuswWBC~j@}|-t9Ha-#isXc
zFg9(nv9;Z8KlXPQLCM-B*1#X|Eomk7uKtWmF}?;6V}?I=zq>AaMuI@uPRhZQi;HVA
zAb#XLB&!m_d?vV=YGfNO4e{!+{;i9Y_t$chnO2!E`w{R;oISo8$%`$Hb7;zt4~3@L
z2>;f7_#l3`KVyyxy&Y3zAc+CZ6;v6pS>1juaIx>wMPd4hlfroL@GvqnZ(G_vWl=`P
z&*NxNh)g*m9Dt{e_oo8W2jP&~bZ7hbu!EA;><%e#<|T-gtqAcRpzrzAPv1u0@E#j<
z$a84U8-HDM^5<)4=hExT0~z^`4%v5<EX)N=l^L;_w;@PmkX6GQP_{KA#S1I3Q_^~H
zlvWfi0&*&eH(BpF^GOMCf!T=Av^nM9d`_N0l;REPYlOHK$f4_O!L4nd*hHMW?V&cy
zz&=ab9-i61{&*g5^i_tx`Qt!u2=*4_q{xZ3tIOc!7Q)Y7AChgsZJd6GIci@>Q~un(
zG-6&(3WN*W56W8E;466E{9B<KRt;g2(mef^>8-0VdzL`5Aa5nP_*x_Owa|ng1mb9j
zNnAO_i+vOVF1F-<y<TfeOSab`E?9^tTEB74O0mOOcD!SR`fO!>C;Z>LP97x+A8ztY
zONowD<M-#nf6Jo<NO&oyu1j};pPT`XBUMD4PcIcN<xku~;;;=F_oD<S<A{V_1JCFw
zVp5<Dt_TS{zvf`aBERaF?$9m^-!BoS6DuWsI&S+LeZ-{b^}QX+Jko_%S3dPzy!hZ#
ztjw6@36pbh;)WT4jK%Y~PT!-g@-Givchu{^^2#Z=X5kx&tVj0sVr#bv(n5Uh(~)H@
z%J<M=BH^duL+}wV!;l3P^$qWW|E-bliGsnJ4Vev@+q&HP)bIFkxijb;E+(tKdk`Z*
zClFH?o%#FapuHw&4S%eMmZ4LLrd3C$u$hxGlK>Ch%6ytWnxBQ+=7cC)0rH$bZz4@0
zZ{+Sw2e!k0V~OstFUoVH9Nteb>qgSJJ|_r(i{TMcI^mjAUaVVhrEkojn(dnIINmmG
zi!@(<CnOJh22GP$C^$`&rqdtD);4@_XwofpmBNbXARyXy90$9wt*1X>D~I0~_PaF{
zhk06ys2hgX^5-d(M2gZcCOhGl2=4h53C3>7T#yl}9qByz*sME`>Gv|x2azoGiT)m_
z;EENgrb(vXA!qX62nuVLBw;}UwY#a?D8>g8AH7asLx;OtFyeuHg0z6Fg!7GF9`Y;w
zR<`299GBRRdGk=cy#Ex({7U5d8{D=X8n&08j;gByWKV23Ta00d-h5PU#srxK`pk)U
zd4cbF3d%3(-4+$+!!5jUj0_{Qeas+>ryEx4;wG&H#&<ZGFoyY2zs^Teg%}5o|HaJ*
zuU*vjw7m6emWyY!kW4CklC;o2RM4T9K4yF1qT5w=>^3AQKoSU$U>}5v`$opOcYH)S
zSFhQE-qD&;fv!RI9E|{}**O&QFF_UUe+5;79rPw<bG>?O_LE00@Y9!E(R5AQ!%@GQ
zNnZHuRU+hEaU2UN?_^tQB@rb?O5Z@-KlQ!1C3cqn;XXxS0f}ZuInW33Tf}$;Udf3}
znO6m1OV55m4=Iy=9Q8i1sS<)h${WrmWW*iWA6YP&?sT=*G8atdIWOwQR*3X@v7AT`
zO_}IBa&E?VLd!|mjL)jhgGcXQ@pe9%B>J2(*`Eqw;*4NJ_Lp$p1}~Xx5jFs$gL9(0
z1D;TxCaHR9N#O5@pmyM!H!Mn&pLt;#a-2&0ltMiX1~o_T1f`qf(=H`#4x>@K_gV+e
z=N}4c9cD0mH|*`cDdgB|FJ)xORBIADA~+XQnW&F@6`OO~O?_pEX4@*Ty^{$E<|U5w
zIA@w`+HL0JRQP`?Kk>{bzmYKEn=q0~bT7OE^l9z7#WXMTFcGW%jaVmRebXr+Iy^YD
zVFIB7)$^b4<@>kPIrV6({bWz|JMZE#{h8UXd6**<P&Vh!zu^Cj(m&JXhivRpSW1w<
zW(nQD*aeCu{ayu*vnvF9G>wG!vTKZ2ZyoER&I@1ir=KTKpY2lG<G6<_v1$@n<9}1?
z3a*BDus}y#`p0m@+OJ<%ycV1o_Uy7K`|BS)DM8)UYdV{j{A6YIs?hD5!CgN+E}W6(
zfG+61l>BWP*hF{)$<CSn^kLV{M@pC+xE6GtOwRh{I}q;nzZ9(<Ii<^C;*MeMtsvZ4
zeFDi$ic!`y;2QbVf#6Z^bnGuSl}gOs^Wsc5uZDz<&9b}MgA>Rv`C=lO@AL=K(^K6T
zpWVbgTfSa1C3ny;a0k>tLrpz5*oYhP%)j{j_Wnad6JAkpTilgne0C{ZR1fW%8wDnF
zM!YHN0dYQ2mv~DOdBsDx8c#Id6+g_f3hDnzE5M`*#rr1BpNscnfAM~^__o8lS(Yus
zRcTBq7B?{$^*wvKhA&xQysnEia_t23?@SorZ@+VPzqG=9x%iY``U3{}AU4_H+mU#L
zHU6*THAa^h1h#T-j7a3zY|5{=Q7^{P>d|nI)z<|{jGjM-SU7KGDOvLE2u_m1w6=4-
z8w}0QuYtedxys7&+3lpxupj5(cbl6dut2-VK5Ty{<(5kuVXEMsmL&*Tq*FZ82~wB}
z2TkA<LL73YPtEbRjHrGzi$9uCm$lW0m-!tHcgWZ+Vk~<0!WY5xN%2EaW_)JYi(B<L
zCAah>^?jf<&yvkVxV22gtStJj4q7lTTN1~WP)Sp27FF+$UMQKKxpyp>*dJ{uTDAA<
z`XT?$ub_jKPv7G>h(|?VYL?CfSFboR%g8xCjpO7fMv?{$=C{#ph^fkdEU{hRH#Q7P
zmrQ%6et5;=bnT{CpG@@Zu5}Inh(P$+{ZaeV2MD*W_i*0zZgnE<Qa07mQb>)Wfa?HM
zn%uy^Zql>Y?(~_WQI24%C~P<JeZH65Il#?v|C;WBzPR|w?a5^Zb1$pTKezzjml<Yf
zsK<WLdDm<7C2tBDle8PAP6Wp{9A<rT9WPM0*Qaxwlva}1a<=^R*71jy>pFZSHpe^a
z9$^c!{E&#OvTro%D#?jzxA2YDQ#d}TDD+Bpe4?p`d3kKnpTPjUzueGCLcGri>Ngp9
zuj!dHH`rm`rufw&>19pP*TyH)m-hFqEN1GAa&O$TsqE}?TVJ@R%TIRBU|G!c+n4+$
z*U)%h_S?5PaGlDFJ5Y@G4__fNP45f5(x3axmD}(2Sfl7z-C|8XYIdy+=f~jwpqiYz
zz);@n+zjEPM0r&UJOXlDT(Sj2=&tLDOVsUyVZPrpX?n2(rgBXd`PTq{yvU1v^G40q
z_O4>*CY%`ZRQ|4>-d%-`AbCAKq5*IXp+DI%*XGCGrlZ0Z*G#s{t&g{hP!{E|DzEDt
zw|y+^b+8Hc1HhM5JUx>d5Oc#2-1VX}#F0oRjeb@|eR}eGXyOAXLW>7=t7RzPh}n*<
zz@b!@e9I`5R#Q`^fUZjIsMt!3jgwA=>e~0FA^@6H^IBi2%HKyhZCV@Nxf6+=uCh0F
z(j0iKJpF4?2r!+n*PCJV?sguFPU~~6r?elrbLye*)L5JILtO8Dm8h<R4@^?pEsL>M
zyCHEg(Nx#60zq{P<`l;J7QWX=CRk&z5vb_+U%oH^KK2Q<l3y>ztvlBBRMoI7c?KjC
z=W2}dkJ45boQ$Fg*WP(u&$7NRzgpLswzMzu7?b|t(&e7h@V&Qa7Om^CUt%sn3NibQ
zNhj_bT21s(O7s*`MVTC>WMhkf{QGzAd;!XbV2D=rUNSmHSJ=o7$Xak1UxTqiZA**A
z!JfZ{M*p|)W5@iD69$Rhx|Mj!@FBB@r-Xp;70*Lnrx6qFE&@7EOBMH6RmqXulvG(b
z7{uggdV!ALzXd4n!onj<tvfFOcYUPkif7!c!IMy2Oas8n!ml@6ytC%oQnNr50wcaP
zN6jJzuWJ=fxyZRL3gFNIkUB6B&#AqWIQtf4-lbN%WXAD8iiD9#@;U(Q&}_dTacg9*
z{xIwyWkP_FS^tstpCKpiP|W4A(QUKp0>J|0ZQ7(_zQ&}B@r6eXj>TW5F4htxBM)E5
z@Xil!MyChg1OPED@OtSraX(~oWOmS|_Kl+_aZ6=GE6tljI`f;4r&;i=Tpa_lie`=d
z`{%kITKvlIkLZ%nQ7<B>aX$3eFdDsuKQh1h^-wcHpmTFrGJIt7O|ayf-4<d-{nurV
zI<7}GOJ^Ap`iBEJ90Ho`%PjJf0@e^+==j|C`b~5#M-R3G$Qd5kbglpVsrixH0Zy7I
zER@M;ztc=MSM+5$fh4E@LH4Cn>Rd~x$9WHa+G!S1FZT}>&qV;UxgWVkOhU4Kuv--z
zOc5{af=MlN$K)TL%fGm`NdbOdoLperA>22x^BsIi;c%3KL60=~<zi%;v$vC|6MArv
zU)X)C+JlgmZW%l4;j-Z<x9Ks1un(cXw)WDUdePm5AV8Qh_M+JPel3Z3!>M4S(Kjad
zU%u?$TtYZ<TynEpN$~h3>Ee=#o_1In3kU24d}l=+v+Tb-SEOJl9k3Jy{H9|SaNp8?
zck7{yMa{`+9vAyAd9nSWt}dsIdB9BGBry-I>5ijreU9?@OhbBKS>_1|oG`1`d+(Fq
zJneyN;!Jnlc~n+j$Z=?Jl}}9z$k7><<bJs4f=w~^eEu@Uc-}z))Fts$N_Oj~)80<^
zq*x7NYWG(`|KO?+Z$uz!vM-^u9)0etpz+VP*PEiELdXXj-*RiJjaOZgZE8;Srz$y#
z(3UL@_;H!<Qg||oDxW7iBSr9St}8W@YxKBxf(7Qo<Aq+A<gdGAG#&ZNka-T@^R14m
zIak|Kiu5h4?=l^@EqzZ*H5?SBwW0f_Ym%UU^o4p@1aKNDC}58(SacH7gySoJs<Mxk
zU=}1gqh9AANF7t#{jyvA%+iS{M`HRvR=3U-=zNT7*Z7u-XlZK`vK_N^-<$RG`%>Xd
zmnL(supj%rZgQv~k#Y}Ol6$lJf-i_HJ!81v(!#E-QN-YR74-iKmJ5vll>%?d1PH5q
zF`<Bc1qFYqPm^1${MdzDd-43axs}J>pbftzUjhKPya<uve)m$FLr`x^+~)V|2FnC+
zwGs02G?Dm*+IMNma>&(~S%t&MPo8w2k=us@e*!~7v_EqH`0RVu>+m9d!|>_b76Dw@
zWt*fH0lMUjB}h=PzvZ)1)f`x$;h9cL_X>*_<r482X^mbR$0ra*iGK~Ah;9*eKU@ng
zl5?NzWSKZ1<nrG;7#Mo#;5KyFI8GGIEbh$5jG`rmVwKp;^g+?um0NTSfh9Ny$+vzA
zpQ3Tn2el!ra*R{nsfVw*{j_NH;X8fbeZ%9s4VA{LG&hCyXIK2@TGpPOkblxL(fzS`
z0ge0m`3roj#M*NBWALA6h4kKCr|dE(F6d584yf`S4R|q{_+m7r$!@5iY?tL2+4&}l
z)726VkyZ*1KXo{dxNrYdE%CE+Ir`2OOTiN9bFx&u1AQNSCqPP-ly3Oonx_FC7}5;6
zV*+wN9~G&^n+Dhnl97>}LUHSB9%6lb#{2R|8=IQyN*nw7#6&zWFpJ>fDY}BWR#Etq
zo=wnm?+Xm|8d2jLrU0?SQ7ry`cZy3zL!QsNmq!mTMZ76Ho@-30Q8YGIfWb73kJl-R
zKgvlQKS;T!1bObJ)4~%E4<#9y1vqOkgyFCgxwidlP<zy~f|Mcax*dG7?V`JQQ(?LN
zfLP~gXMc7CTTeQA`T+i64#r|69s`L5dIiT+`6<8_SD%thn0vp(pg!k5C7G?hmgK|J
zVE3@G%Z*RB^t1bYIwOjDDQF$M!K&u(HZRGR=u4L_!F2_lm);mQ+!x#aUe9x?FGs@}
z2B$#xBeiK4c3oGOljAHz>ged)A4z28HLhLHZvS4(Z{~FPh-cwro=(+4dc<wJG<kV0
z4iURm14qgFXgYp#2M4u5%aOJcWSqP_rDz{n4C|Z&*1WE#!*=vzF<OYp&`G5iluUzW
zXq`%OJ#eM~mzh=8F$3uk`eBH#0CEB%%E>8;ii&dh`MrHsPjvsWmfb4Fhd}JQom%ez
zmjfZbv%`t~F0I(hm;rwk#`rAq2iJ0b5SzK%Ns<x^tyRb{cvcPv5ey|}I(#!W(d#9?
zFYibm_8Db{lZRcuu0cIO)W<CTcg-KHU!?A2b2E=GA;nir4gg1<p19vbJ0gj@PY?SJ
zH?tFmtJsP5$t-Ai?k*np)J8(hREP))4*ucl^p+nSKrYYMlPAIiG&eUCl5xlQ)Ko4j
z`!0JSvewx&sRY?&z~K2IDQCe3HuQ_YK#h;w6(6=tA<T@|C0p?=n{{_BjaZQp5!JE)
zQ~FtjlLr(D48l(cjx84=xk6+yD42Z|TjWM>t=q{s%Z^#>GzXU@1qD%n0RN}%z5Z8#
zRTie($fX@`Y()7~D?0k{+D;2_R$mml$aL%5Nj>VWlc^=Q&K_Hhl*Pt+jlXIq{>!A#
zwG}l!GS<7%oi5M7z##Th+O*mB=IU;`@G^SsS006vcpQ7^ij_y(F1hZ4<P;ogH9x;3
z$euJGcc@eCj(}9yJ_x6=4GrvE=66Q455jJO946)8smT-hx(I0+tYz{;Y)_%wW)v3~
z72tCY%xJKU3Y%Gk0oTaU_#Gw~SFWV4RPN7qzVg^T0qAh8fxiB!++2CsvTbX>z;?B?
zG<^%_$%rSAQ=6M%PQ|scx~lcwK*htOyDPQDWh5jgdgYr!`G>99wm8YF9+Og54<82B
zc#XvgZ9wS;KL(@;6zalp!e>c%)>~YqdfLS9#=4c`w^CaD8YnPihe>7+3U~_4CnZu}
zA8#y*k#82wh;Lq9$6TZp0H(trP4O+&!N8cvbSw~ZrW9U!y96x}4(>%b-TajLh2CvT
z@Dkdcun7;t!DBuNL342XOXz2t%=tV8brzoqv+&0=X4Xb%0(loNm&3U0JWx0$#m8;!
zcfX}5CFLM)AAGNWO1((UorW#pH>QUoa6L?nTC3g4I)OLgyaeY|&)D1ld@=$Pdpj_M
zH);2zAPrJ4VXMelIFt1}ho-#-3hRfC@Tj|+4CdzMa6ag$y@QY}ef!|%GPM@=<Sma~
z+xLjAu(pK5)C^Ppfl8p?^yzx}I!j8{(~CYD8d<UxxH<nL27=-3iWBs+k1PVZ+6;eK
z^4_?SSz5XT^C@8)X$+b1!AAZRmUL6m2U-Mafe&;nI+}`~>o;zxJ-R5bds6{$BO8PJ
zPB%8rs}rAOYBu!-c&^UB<5TqH2fr@&AG3G$9SSfqcO@b*$2HD;QR2f6=}gFB0MJ8|
zg^yj)gUbas+Y%-};O_6dsFtf`K6yRz)5RLokjbY38e@GOGI&+}C`O8jN7Mlql&%~W
zu%%ydVU^aQ65$#S<k2(<$x(4BNy_#fo}pCH-t09yFO?sf{2H*xTK*eXD&Fp#$RT)s
z%9t11l8@guh0cV&oJRE<IG`|+D{9giDV~(?CjeLTb?N~;JaHVN*~@U~y8=JKXYErs
z7HMnzYFZm3mgs_$j4WrKgEXNE(>-3zsBZCV!oJ0ctN?kl#)}&soP{0Tix3Czs&@<t
z5395sDsohMK51Z3xK6j*JC8@7?1*#rL3Hzl`)k4_aC|N>!bPGmKJF64-iROJqnmIE
zqughuG7GAH#8!!sefFjPYE6m9tgBS@SXm^>QrMutd+a%5G%r*jr;5tX_J5g6E@x_u
z$_z9YWD>ZmKynM$>CexDVOrBOJ%in_?}BmEc_fBU5;#kOVy9GBJ+1^0;8zNA@1Ih~
zMKcz}20uk8Jr3s0<~@SN_70kQob}>zc-7QMX|(8KJBE0br~sF<1!M2|%L5m)&1*eK
zd=cG($^P@$N5SQRCvZr0yf~_Wr-yFcLo`_VAe4-ZVhxVvL|=M@I;I+`{8;Caq|@n~
z=r)_TG9HAOruz<#C1LNc)e)m*c~lfROhP7)@2o-r>v)<xOr_pAyH*dqGIZwj(a&$P
z{ONUIn0)coc_e8YSykZ~YI~V}K5$5G3j+T0%jHn`X+HdnM|9?llj)tlh={s5Zg~z9
zBXo*<2>t@+DTmnOjee%q)ucHZp{>4&im>mzz4_ptADNwR<d1|!vZ^V#X#qE%;`47e
zUw}lqCu5hMMto{O0hWa)WHN<4@w=vXr5v8QpaVV2onXeNM;sjTu+&?^R|w%*X*iS#
zvHR)mJy`ZcYoezWm*7cSbsdJz{>3BYzy9YVkgA+e=MhZ$>4N=v>ag&Wx?vn7gcuou
zJ0CJ}3k%9kQa_`{KeAr-#yN?Dw<NFU5=qfux4-kt(U|`t1JSu#V^*Rc9PSHlacmx*
zpOcj)!pMXlw-2N$KnB{HomCfk*y&;i%(&HK5t+-MkT_8m5**OD+n=duS8C~xLik?s
zB*4&Kq)hs_&AFBv9Ro7#71si{b-nIkgE`}4I3<GAXJEvkF?idtJ0zDML7+WQoP9La
zDZV(-4u$Js)9i!U-%?B<$ye;#g0Z0Q&9|MyAIiR(nCLBjX3scVXp5_Se`CqV)1Bf%
zB_SgBH6;CT7l%u0(|6lXz23SP{f?eIOxxf7&>frNw$EDai$USDe9Y2q<&0~AS#8u}
zR0Z718!EUJnj9KizF==N^7lNhfU2HK)i|lnLxpL|oIjK6laxWEL7E!KQz@Y|a}vgM
zM?*(*#)?AZ$g<>4NKmiC<*JH2P58uk*9pnP5XBSBDp;_Y<*&fVuU7v7HUrdI8x+1D
z$rdAV;+PvT3|k@I{Ijkp2{S%IF<LzzyDDzn5|Qqz_>nc$YWK+fmXo;D(=((whzRam
zg0vSaNTv?9|F1ahB;Q|v|2VfC6^D*tT06p(&%^=}`FH|po#lV67)hMEjp+=hZ`clI
zc)t<41AYei__=3!C`Nn-9<~4%G9V}$w=h6;D1`(&vq|{`RoB&_dgXTI#xL&-{yc3L
zx1!h5HyXQ^qHl^i-vcQQCU}BODtyn9Bn>zs+1en?&lLPI)iq}%WhnK}?T5Ka+thw{
zh^2?an3n5_H0nN;mgq@=muh~;F0ioRJ>^G;e{~e$yoJTqJ^liq`0o=Im=zJdom>Xv
zu$!*}p8`K7P;RJI7Q56mn&+<U&bson*T)S$#9mF!sa2&~D4|-gAq}@skPvx?9lOR%
z1b=}<{ns^4o_?v^PwzmWly50XWupGCuNTVJWZ1##Jmlbqj_C-DQN~ViV$b~j848SJ
z|1l~gJAY5|Ghk|xEY{O$nvJ(DnB5<F+&059lpYC`#Ez1$#~*G*T8cvn5ad58P!gtH
zzxw>>Ghu~s*z0)FP+L^M(){N%auF_mvbLU!9lkKpGh2blUIunJ7LCg4j7x`6-hVt2
zQ9zCUO<cCjyt9SaS{eLzi+k<mwys%pdtr87z6cLEX2ERy)V}Ao6IBD#Pi!hrAF{xT
zco|O0b$4isd@D|znF{zE4Yb!VF<U-z(cg*3urgTE(m$q$GHW&!0f8(0GWfhDDm}N)
zLemGQPDfX2wvlq3|F$E&UpyR>qW>9%jRL<vURfd1WA_(!5M1R2EU!T&!i-Y0kjaw|
zLuUi69m9Z^By#tHK+JIlg&1GjqnHql<yjo<&<6U?J?vVF*eR6R1zH8kEjlp6el?$W
z(XB!HYc7hXV<+$G^Nz@RYNN>t?$4+f;xG8L?aorw#TbfhuO$ECfdSb#eXQ+<wd^DB
zem~8|3*^lnIT~Pp7C`o1{hBKO;T`C$y!=E(56%XuNOVgCsPAktjCE=kOP`3(y!B3+
zC|T;y>kqqSnhXQ%76jh(T{z?g<u!;<_N&pSeV3U#L1I6_Z&7-nShK!Ev3z`o$dDvA
zdWFUW+>#<w%+z92uPBB%9wB@SIt9o=^|z=%JNg-BOpeW{=Y_=9;!O7a*Rn<6FqH#$
zQCV5}Xv3-T&6RgfDN*Ej(Zi>4+J-MBso*GdU%D59AKT=9b;<Y$d#S)&Lm<%OvrrSp
ziyiJluick_`1T^;V)DB04XD@(fa?l9l9@g<hJ%A;nVH)a%fG%^ir!ti8+6Lv{HUZq
zN%DOvx5_qges%KKVA(QsR-6^X(wZNjn)IcZUaRz|56tQ5;Z+`k!<ThEKT2qRqjdO$
zQl#gvAg^!xG{8YJN%G;Tu=QkD$>Fk1cc^t4!7hmwH5*+b;z8y}a9<p8iv;Z{+xmVg
z2GEE$p8VN+o7vC)Q2QucUJ>g9PQrs4O>UN_0mpQAC}cvhYXD3L5hW*TC13ArxDMRd
zy|1xbbL)t}rmflh<sJPILIDh-n3GMqqXOSI^G81?r0Z5k93CiS?$`xV+34R^hLu?a
z`$l1#vOy0X?rBWV5U9gXMSYHe)1lc|1w;6VF_xd7*4OMkbX?YnC@W)dv?%r5ZvdDt
zPpgda=1t_U1qy)pIM@b)Lr=1RP3UP#o~JX5*h6K?tnr({WOxo-Y3*o2<plCG<=>zd
zzRJ~}N8KcR^!vcr`51PH+wC<h)DcPz*8a?RNw(coCThK(Q6Dc!G^&wCpKRWD?mXfR
z=%K+hWIg+e!=)z~2z)!K+1M^DWv{E7pMy{U-@KdXZ8yyGfrftNDbPNJn@}$zY3xnp
zYuu&hy<-g|!x0j_U`Ocw(}<>h5FhVp^{%x%aQ9f`<E{D&gJ%|hV64QmyXZqs=;|J-
zWETS6mYu!5q*N4OirTj~CKU)`(7W(<VK8x(D?e7ui}7XvQ2Akc1n7y4g`1{^3n!=b
z88%@@b9*4u0<X<%?gf){2uv|v%q#+d^mc7z1WSvi=7l&huNxvBXW=IBCE#Cy5k3(i
z`yi%sVCS3lrgjP24?u3;aH@#8e+%@D1V((rBNfN!qb5^o=JeUK)i$F(U%&1G?haTT
zz4Y2UzayTm;bZ?AeP#t_@z);nm5nI)-jjX(N|4`!B*(64-MGkvTSP^5QU|Y-^DBSv
z?O&#kU~3VaF|xowuj~-}=0H`r6P0+lDfQuI!Gk;AnYY9rFCne*Ou9|~AooLM+}a<J
zL!)EBeE)86s8WvUd!&YjAhc+KK|u+;W<Q}pjbLE`mS-_0Cv{3uXJlY&>xOjb>&Q21
zN%%x3PE@#+eeUb?nre;(c=;Pj#O3ET78Wixw(@uH3X0dI<I8rpkM;BpYXPEg=S~kM
zcM!PO5FZkBKfw{Iz)Md~HdM{m8SGiVe(jq3&iXnE71PBt<+7Wlq5&9)Zk20pf-X)e
z5)h1UC=e2Gapk`ZWr&E3?1KN}igCH6jB^{5>)43&Lj@a=*!0Xe?t8~RtQx#|^Whow
z3`;*GRJTNtrV*bsluuic^aqj<aru~1PsW%ty76)rAk9TUP!hVdLP7z7Pn?mGDG=9$
zDWmx-K~JH2h6$d@{9Wy1zw1tl+DyiQ?~Y~B3JFmUUSeRVq2U?9szl&!A6lqtxSWWh
z|BZ1RxzuF^HWLZn-eUm)ySTWw_+p%qNIuTk$VhuIFpn<-diz#as`(Hm^+E4#i&E3h
zSKz)dRyEV64Zfsvz<=k|ao0GuEQ!fUn&55wOp5VyHob&Bm*`;2&h5^f-*)7qN5Xlt
zti%vpj^ASX3HdkpUHWxq+`=3LC8szX^bt8foQIKO6@E3M{qmE>uP^?VlE*%wUU-i|
z(P1YkFN0<75Y6gZOfN2O_NpDL*>_4#P0a<)D4gncdYp=qrwiz~_o72c`Y<Cd2Ybu!
z-?NSQf_sad%eL9j>kd@?*RSVqDi3wo)&NWyFS6@0LPrfT42Xcxrk|#a7<IF;gppK@
zS6ID4)sy$;Xw*in!TqkA3E*w`A8{^P^Q|H6!~TGG4_XXvT#7iV*JQMK+()MX2?u2L
zd9kAAz{^*!CV>X4rW~7@`7tUYf~5qxWrYKpkWP<qVVlFXuQK!<<Fo1?xk)v69tqsI
zK`e65>SzMI=f|OPugNYEp;d&8yB<>4`QCM7ASK1Dq{I<mKt|U#+Rv!q^Fq~`bBP6c
zENQ@DzTsUS*wprEKG2(Zo1if(r+;BO0-|$Y&?k-3vjAM{8nS7<>6a-96QMl=bttaC
zr-c(Njl){9Y)q-;$ll!kE;xqkDo@1tVJv@s&jidD9LXV=CZ%w)O*W(Y7pp<%GEy}d
z_NJ_~pa5%^3e4Et>p8viaS4YmdU}n!%k54AH*0GHKy{LnGxUOzg*1ru9?T6v{{REg
z+Ijd}Nr|(c;8ifr>fkYU1}1pp>FX?%|Jh*NutJB`osLchxY$ScjfSez{Y0#3x0$7T
z$Cbx7m+2oB8o?Zq6~<^yuckmpQopG4K>^sXVEmt%A>g~2<Tt#uyQ2Ri<z9rH=c7mN
zlJ#bgDL*1WylrFSb%Ba1*!PJ6oLy&@%+^ZPdP{ceM{h2IQuO_W9GY#{QvxljXQ~sF
zm7%jFA{tR9rBPe|LV#tcdCP${z?p%OgT2X6ekO7_*t`}jdj0+Uo;{PkAfowm=5U*x
zWB&#-GNs-?Rz@Z-L!f(P>BHkWd)S~ID31-5&k&y)Le7i1{lW`$*s@&(6e>I0%gWl*
zFIMfzlkZ$L$_Rcjwrkg1w?K5j4lvXA?>*3z0pqn8wmJL>LWXNlL7YB)T9E)>#`+H~
zKn6MnOQ8M$+tx8V4^UHCS&E{fHDJ>A^w4a1T>=|=kcyY@?X`o1Dv&A~xXd+PH6IHK
zh$JuWv0I>%lBIhvBcCU(2@3X(&wh%jHT}=LuFp&sE6b#(DY5YK0<<O<Y|y4v<T$&@
zkcy_K^Tu6vK^&o}m2lDGbwtFo3)!Jd&%d-ZtY{X=#)t~K787U{!NG2Dt~J<^wYFM5
zI+qWQKcrskvrVN|gJ81@x-v~I22yOK6^AKr>I1RwE^ZGW-nX*SmTA2kUk2x5^TU8o
zr^?kzUH!@Z=Jf|}vVsDusn3V5d_+ozoLL^y@M|L#>&q!#|B|UhwYq8uMom2!3_3R2
zM}A1@^btK!KTq$3d95z+P|tAT`V?8R>!FR_hw(s}BkizOg)MU;dd<)f2+oI`m}ATR
z`LkO^MU7c5)_uAagdf?kb|Y1UFgFH!hF=3ED40|xA}U-rb|93f<uqE_ID>-?P~+>K
z1u=-YavOf#_4luUcK{+2_)^WRt+i@A4evD(KTF2Pu0{tTg{J5}uh@+nQ$T^=ng}{?
z+7zQ8m$;s*<%mF(z2?#lfBV)iC)zfbrVwdnHrJkTSXEG93K|s<6G0$<^YzqpqbAQ|
z%B!bM2#n$AA}m2ZZ2>Fh(!iTG!UHW_oy<9b7^zRi0pDmuW{;|osTwim^$XsARA5U`
zfl&cn{uVbk5@dd}wvR!M^k10V{uUJlh>FLVw8O8=imIO6_i%BU0{>*_1VMY!7N@3Y
zS6(%|IvHvVl#oMST^RY!x;{f{sq`AJ>miO*b|8$iF?0ED`xr(@tk<p;4tfw_t{owv
zCpgUOFxNmDGxnKDRu&z4L8SkEGEPDX;gwiw>XGAmUuT=!KA9hvrM5bG5rPRYe(Wx*
zTLdURj=5<2I+qk9bCy(4iNnaAmw)sDw#n6q!@$uV_2aW|g<Z1h4LG>Of>mvP7qEcQ
zg!y}Q@OJv|8WvCiEmysV$OpzTP!|mho}V2z06mtPYJ7=$PO?-m#J4or2NjSTfNl?3
z4p3F>tP7SRO^s=E5Ki!qPZ7KDB7v08$JZ8xV81rB@gu*6xayqXVlztLZrVJGhJ^Hg
z3Vu*6D_J}JE7hFo<#xBcSiW|J&RcvlJY1F7PWQf+mX_l1j5mlcP}}=bhu{E6Q~%@k
z*tJ2DaXy(^&VGdjqb&c0P9rSVauq~TV`D1-bB`A2C)XQ*x77ex$-(YiU)MMx{j38O
zOi!b$=A2)?V-E9N<0*|q1kmw$7Am0@UH3k$d!{Qq$3T}iKd%c8+Tf1pGEz}B?33FM
zsXswvcetJFWDPlvOY!O?3nR})6N}b^2VQqzlH+#WPG(NCsJ2!cqV$y*?s1)`2nm=m
zmzk5WCxJuK?e@sw`aiLW3y`1sdGXTVnPzV}#~;V!z^PlxqRmzz!%Geadt44PZIdq8
z0$59PAWcp-&7SNUfnpofr2yZa_zXjxo44jN<-CV-5p=T+>B`H~0@m*sDB2NBy2tQL
z`U43NQtAQBbb)c$G>9TQN`rFPQ@?_OLPb1>4yNAd&P?Zhw4c&++{hPR*4FVXPx=0(
zK))2mC_0tu=g*u0f!#Ho%4?S|S5{ZwGevXi<YeUt3pp=Uf}1<c;5<M~Gd6ZT6ERqG
z*s(aciH9!%pSZ5;vuXHRCMO%>1RX@h4juORyoZJmAaPh+Z11FOUWsTaKu8!^z%do7
z+57`S9y_hSoEpBoh=8*bH-R}6&BK$D_24Ze;c+@1$#wxsjK2^s@o-l!_FGpwDbq;U
z#w;!+i}Qy9_Nevvgz8iTZJnC212^|{tccqIG>ssif*hb;pk3UiNCh-75F-EHsS`8;
zp%EC_cWy!9_7p++nbGpJKnn&B3xLFCp5+!q#Vn^{5tp1l>r|d4PtN!L02$9LXQSKZ
z%>?Pr4qLB<Z^UPV9DiUIfg55yoVB|>qx22Mp;5TEy;cKuaU^pov$Llln2GT4NSmWC
zF&RQ1E2TBqou}qhYK)v7E5mJ#DO&+~5DcvD-<NyWkc6$<1b@~+fdY{a>UO7!`~^*(
z?B3mu#0lt}pbo+nD&eZx+X30h;NW>vvzSJQ7a-B2QAKX0JjxaD1(|C+YX!($#9w<0
zMq`x?GOWkD!<zXBv^CELor*r<v*KG|crY(t-!`}o*f7&^1+^Pcp=zJH&X|CsVU8|f
zmLLq^*N3x{)B>TRvIYi_<KvVC&LfDZ#D*HOM_!mceflWs#hq8TVqUyxvcTkQu93zH
zyUc?&TV6Jf(VS>Bk+qrsu@VNgIP50A{<Rerf{Mx{MH=t7knn$4`_8B+vu#_lfQU#^
zl3)OpEJYN_f=bRg2tpA>ke~zs0Z{}LBo{eDktvWQS%N^3K|xT0L=lxF65lH9KDW<3
z=e*Z<+&2b4T5YYb_P5vCYtJ?3TzM?ERsqt;N+Lg(%BMDTcthnkUe4fI3$Q-(IuzoP
z`~hiPyb32+JJwMSXxr`~%~b?MFgWz0#RaFBJJ1#2roRW6)@Q)tV{uy5vSbVJ^iTTU
zay$!*ORk=)P&#W8&Pt9bdpZ=|12po#9U7QT-+)7fRZTfg4)n*`{4@34=}C4Eek_2X
z7cNQ+lZDq-Aer0Kn1n&yGF6TIoG%Txy%YcTNsNOpBpx3Lb&B@F!sdN+`n4Uz`_`X&
z6XoDrpaA@<a^a4)mEA3&NOUfb9?jzjR;=O1u5ojb+ukP+COrlSag))VxyDMT=Y1&l
zsJR*M8&65ArnxzTM6?dIp+B<u@$cJ^+ma3fTKdJ)7=>Gq?OWKA`-9=(b#huB%=-2U
z3-M(*QJE!KTqGGt>nRZClMamdgNzo||Bm#J!(jgbau>1nUi?@On6C~thvc<5X{)cx
z<cCEmpNbz+jzOv^PmPG+oC>EY60Jo8a7@P_TW{DAsJ_w=awsAOa;_4u?M>+K6<NK(
zh4MK(&QP*MI{yJP3iU76x#tO-bY+7T84e+ozYx$FqEBd<Qvr7ISUU9L9QnQR&QaEb
znUSFuBc}{+<%OJsPjx_hS9`yDqUYMZBNJ(4_E>&;+h#gz+nZ5xCCJ}uq$19;#A5og
z>lhptJ(caS=YsB#aG9i3`A@?73h$TyN#OB^m|ZU%$o>1NU*|hsi-|sE@0KdH*;}1!
zkheTPt$hAIfdDl~b_0Z3^fSp0aW;rEJxkJGc4ug0s}Fb&yAUr6<AJjYz#qvm#_8m2
z<p<iyR#^D&frbOT;v&}O;YTKC2>@?NcLDV()HY223`PRlb?(;lOz0N;sx*<q!I1c<
zF-{^}2b#Q)0Vt~d5od)+MwHJu!Er#kFrqhV0xy*QsFv2{=QHC?C_wsVXNRFafJW{f
z2w!&iR`zI^c1+b+Qpd8*yuMbz@3Wk?n_Ch|3NLogWDoQ=AtC?<(32HjX_eTVe4EBm
zI)<fRhcV-F&VH`^%*Vh$S-x{jGUg`O*2J*w_YMn#9%oS*GV*%R!wAB8K^Xo6h4M&c
z<*Ptyngy6{klJHw>WFX~hxA`J4j?KWP)%&scpWICqQ(KJO|y-YqS`)h4nQ2<qk#Te
zKTLn1v$u4Z#wiqDPtY_S$st7(^~3;?bNS5COUo122)d-7JU$MxP#M<5na+LbdJ=*-
zv}_Om7rN&!83*7bWt{9oPm>GiBMbvInDr7(DhU4dK`iZqfP7pS=(yKgZj13;+7c8|
zsp2hBioyFUAZR3ahf&8C#7Q_s>md^j#bf|}5N4}KLa1vej(ngITZ4bR#jz%;_UUSl
zU$OWKh3Iu*Qhh`cA@3j3p;~|FdLP%ZHTk`90J#1Z=|&e=;f*`UkK+z<`O1+{^>>Bt
z*tpA;InQ&BB&_6Vt>n4mkaBX+4k6yk#R@+W29FMeR>qMD58NXFJ%gRqawPCe^|cxz
zds?wl3(y3`Cad->9Ia?yBN5{K7JW_7^If(bt~*TG1ZesNY4%|g8HM6jT8Vev!z?K1
zr%@}e$-lU&HE>1T%Gf!EoxkseTHZ&VTBXg!dqs+6F~#I%IW4$eMc;zZ2)D$;(^G=S
zCWZr4m^@7#PbC6&edHJ6U)(DW0WMH`z%{6n)y&Kne&8u#t%60@=6=QMwj#Zpb1dIA
zm&YDw{Ng!sN+|5{F%vz4%BL^B&Z0ygw8fwH&A|zUuyTZGQr{L=0&UF+I?F@7)nl1P
zps(dB)=qOXfv*^Mj3YuvW<?m4a1t8vn*<xTel9d+4{NTjulkK8a*-aoj9BphcUjr?
z$nv0CCHOhy6<l5Gzs}VcU4*W=x4BHbzWAx3VPW;33G4qLsk6~p6O{UY9p(xyda{1I
zP(k9+ht-M?{W*yCNrk+W$2&)~+o4mo@J{6`2fT4Z0otxw{{ld4PFfzZJl|-DaV&ny
z)l){-gnDbSD6B>I*)gNq10R<$-h%TTQsTYD|D$6G9J$MvOU-<K(X~6n==mxz!EE%y
zXKvTgPUKi81ek7eG8{y`goFXaDdCNc;i;)7QyLqo%DLFsMA=dYlOSQ$*=3jL{BR!Q
zqG6_YT%XMttuZ{}tf1GD2d#$C?=X}2Bs@7aBT2L~u^8JqbN98kj_*+B_Vi}qDph%X
zsR2JzONvChk_mpjwfbCFhR^U;n|O3E9*1e!`27be>A#^DT{J>W2^Hm4Z>-dQyT`0&
zYI>>rWLDkl*D!ZGE;>4l1?NlQRI&vI*o?G8c>@H50egD^HM)tvLRJQ8$D>DWJHOWw
zCvGo4c3CWQ*xEpWIN%b}glybm{#d3-`{uN+$~-o+W-R@|`_m^CaZ-VFGm(k}$Om?B
z)w#cs16CK(op8=`0Ger!-@%zOx@5_x@R5rtJVIF~Tq21K2a}Ne{jMDLqWvrHUr*%a
zKm8^zLa8)bd*94p)C{<ITwjTRd&i}R1g~LD-59csc&$V?UJ(~Vgj9KESM~Vvtl{~!
zLWUUEk01F87-BpjN<FWn32C)@fK%#VHdaw7LE?7X`1<<sfx-E#oZbYezP<hYc4!{p
z$esymJy(8&eOI^sIx1=3>u%dqK=Y}43?nh4Q0jCtQ;6mP;4*F15=H39pEYRWUv;5)
z6Qgmi?t#!!F{ee*EPLPV3w^39FRc=CUX0R-V=b-fq`Fq%O;X}SGN*+V3tZO+b!KEa
zQcY6AqpZNJ0a)>0-hIPHMGg7vePq7kY6E5tm$b5<^1ik#G|1(87(}0ebA}=5q{#;y
zr16P+L-z3zRIty)kYgF51kEm2`kJeQ{2eN7*c#Ntka=)NwVQLR!K-KMg-4q$KIbHp
zDV4?O!mgV7AM7f(q5o1&wf4>EolXRofqJ2x)vjW4^7>Y_JyubtTcA;J^^`i;6>-3V
zn{3|~g7dvXUbDf-tyd~d8Q4A-tZ6+}V|5>egzQm1kuP)AfL9CxhywQmDEiInz1>=Z
z!u)@dxvmKosYy+^&#TS=j@-jprQsbp*YzYX>55J;qwK*7)b9rQ3A#3PMs=POz&K5_
z+24Qk$s4l<XbTkjCbhbldsZ>!(QaRa6UAI8XXodAPqVPtV>~Xf!>qPS+pX_5wjMMt
zHxkXhlJLC;;*DfkM@*(<N$0EP2iaUuBrmvA=p|Yi;ls5i>jP}q+OWAf;zKS1+{*t3
zcXUYOz-Z;OZ-L626yL5ShlL+p$5rnqD~zI4QY15~tLf@^*Oo_*Vvtr8ELYaSQycK3
z=PX=2+~bdX*I)>fm{>J)yNBD}MUN*)bjn`o#=-XO1*xfCU&_6C6K|>DiYCX|=)ey2
zUv&>wmfeMb9VaDmk+Ji=0-#!Q+h#WZv9nP}Y~6$Oc%Z*8X}7{H%#??lbtF|l)_xOU
zn0H0P+$P)RU<{$Fs|t53z|&|e#H~7qbn;E#mj?x9!##b>;Uloy0GBx#y&S9(n`m{M
zq<~>RY%nHOvcyh2^rJ}^o#p5|3&lh9o7Vj}EL=YDep(0aWOH^*D7dT<93p1_N$>X0
zJ^ldoKlEbPPm6Bxl&^PhyB{<*-3|M3+TvlYufN&++KSlo1e|0nl~wUcy*R(!d-01Z
zduDJQ=ma(DC>%HpS0C`dLIU3MW9=xLFUS<CF$c`sy}}71y&pV2Cd`+HEqSj<)))4A
zs7;5|nXrG&%d>_4R1N2t$H#i-nK|ft0pk&f$AHQI)(Sw6P$`C6+;`tXN$CTu$n~&i
z^B!JGN=$}A?m)Cnz%rc_b)L@I+WNWdo&xqbF7rOsu6f&2ExPbWMu>6fo&lhAZ-2KH
zpuFd&c^JgX5DrM>X0^*nbsp%p#KZ_kH2^&ociH9UhEMbGu;=7dU3<M(_p>ees=?+$
zf8_RdHn=i;e+}cliqg{Zzyt?g*U(TokTj>KGeEdu{91pn^`tmJxZuELfDyp)UXQ<c
zbG_{04%DjA>6jkSTX2cDu5AJ>zdwFu+<=ye>F0NIP@H?L4jnf8#(?)(Z_`iBjIDZ;
zuRvio8f&MiFa6*YTtE&KJ7WZGZbjb-8CBK`r!z!+zCTHcb=-M@J8iSvHT5Z0`e+BJ
zvUOzWb=TkE{0bfcmFsWlR+$0~ZLstdm1apIOTGok_w1(@cDmZ=)k2hym+mW#oOd-n
zaKT0e*MHmenhB$^?LXkTFlC0hz@il9iTTDf)C;QD!uA4&IsXyV#^PW*4C>r%QtG}&
zmc~BiAx?jhz<%=?pB#=p>-ecY<co#9`($GT^u*cdqQm*Kz(%UTfB~ktfz3~D!PO_7
zB7%ZpOd^lwzmky!ySZ&ZfQB+|q~n$0ENjd#NE6W4l_5PmzBtKUy_>>s4rnyEEhEmJ
zO<!G;0@xEZ|21FN3WmaYZ!A?nN`ltSVou7tg2jRg>)w4J(}LUDw8zeIsVGM<5}yAG
z)<%Y45(Zpl=%~DZ|KOA3&)~2y7tt%YcCWT}cgmgUnEfi>gQ>W<_b@$CP*CuUeN4*1
zX#K+{f!F;9n=z&pOOJ(SJj5ln9I(54gkP3cz(c<$H#aVt3&DET1|p!*nL_n-dAfhd
zmw+PCmucyQc=kcQ{pIWwjQSNU%6HKlj^Vw5g<NxqV`9@821S6JBG%iK&yZVUp<o4|
z=er9wsQ0EK-!JchO=U?AW&OC>a(L&;jsIF<`Tw2jq+BNV@dyRWL4&0VCyM&HrM~BP
zCHqZQ6X@JK?R?AU*nW*@gqYuqrlF^-)RMA?z!oV(`rkiS?|mhXU+a9=_v{-{PjwS<
z2$!zB4HlfsVR&ssQc~C2CJcYL`)Ob7nSu8D-g6e>i(kL7z8~2X6cyE8c<F=>WOz2V
z+`_N6H*V+ybP25<?r*9|Xe!BfFzNIyfNb!yRRXgMBIDx9?R4l3uLrGyLz%ka_PO2<
zz?FtUBJig&$(15PLV%A@>QNjGi9=X_oN50GjXJQ`cvT@GUkd{kjEquJXKk<E<>Tg-
z9a#p0JQ%{rYitrnmAnBBRU2TDD8v5^tzY0%*SQyxyu)_H(a;XPeUo@R8ypDqgFBgl
zUYSG~E<>2#ld(iR{6wCpXs4mr0hH05&#&r*gD4(;z$N7Id_DlOAF5oxux@~U`)2~x
zg@T@YV|3OA9JrQ+YeF9Ry~_Ua*HDq1nzF>v`2U}TvN5)yGPr-df7T;!hjmvbZF(WQ
z(?0g5_(G8SpD&{$K=fPo^6S=tUmj`lB^)&D=j-QYpPe@+$`fi4_yR`^xXD~|reqd<
zZPg_lwLkSl)0l+4Ep#zT_W^NCd1I9`jR3;MXTO?xCrK-Zb9Hru{P-%!ft+2g-j#Bh
z8wUfwz#{Sq=TVp&$-rO&200dQa*ZmlU=8nlWudiheb&9Y+IjRO8ZzA|yj@=2(?&eW
zl`o)mQeRJ2S>-rEVu^j_FzWfaLnDz-nY7pN!TBv^=UpNDku)QN$HOX0x6!~xdcj?W
ze~eMz>VpLj!e=ji*|(ZD@#+Yj<>Qg=D^~>41kK7}l%Kiwif>)i>$@%oGK2;YUEU_n
zF+6+~k?9st6+XLnD{@8@SInOJ`^7AzJ3A~$d^J5gj@rje{#<D&PF@zbX*AmVxMgNf
zePO%4_Ts?Kep%^)azi2&HPRzt+W@R}K%U+r`$k6=xBaz8I~1tsXo`erR;af#Ols?Q
zmN9&RUK)(GnQ$|4#<$LpjE@(yvVu~#_e0i*a?Oat(vW3ZzHFf})J0%IWM~B=U-F?v
z1igu?cV9re^v*;7U2t1tm2{vpz=Vf`PUiwlL%2w3Y7bs<o<AKkz6W)JOV_ut4HiT5
zCnjm%Z+wrpz;*YjrwkF7m@{1X=k=m<yJay_??!R8>7QGdWf+h?ExvC)GhrgeG9$OK
z9~a(<Og)&FNT>^-i^e#$8VHN(BMi7FkZAw$JAxv7AfSTA%->=k3X{6oE_H#q&^$_r
zm&MENW9|=0Ig<4+UcRI6q43)MIh@2deyvwXd~g5hL9%$Y?c)1aFWNqkZ07ik{8k0j
zRPg9WlF=N9t*2zsC0P*jUHPK?^x36SBpI2Oo&6UMRpm(!FI81jP~p%L)t)zhG(J8I
zg_^^wl|ly3qEYA>L&0Fxr8^I0?Z=bBhNIP(1pv<vdy+dpKYe?+TlKKXUmlhi#A~)5
zQQ`cpjz1^r_gtQ2$XR0d3rr2SXF%NO?Y&FSYwZ2a^qir6+u9~Higk38P0DS8U%dDP
zvaEvQyM|?!3{9ws8XXakw`IIX{W|2Ha9zAeaqL)cN8BKAM!*H|%}AfheCtE2OX>O0
z$6sXjgB0&}TIXA?BlstrmYX9l<Ft>_tYbQI&AV?2^Rek!rvYG~Ow9ju#ZSdy_uPN;
z<h}yM*Vdi5Ef&<43W~Y?m{+YYsLmpG*`~w=uaA<j6H`2#$JQ$(3cwBLf&nIJq+Zr+
zv%SxEyEpCN_28JEoiZ?;isAg)@h1+!$s;N@oPmvGXUAxF)Ch4%COLKThEhV&8&ukT
zoBdTK9T4_FRR-y{<|7H)D<sC+x9{T$>9J$;9y>4*0UV`HQ&9y{-kNE{A=n<fReOz(
zTA;_BHNt)M>S{*>EA);JHa~=cu<<sGQ)hU%<>c&D+cLF~3yWNOmWG(Ns00GbsPrrb
z2qU=*UJlrO?@FYF#ems?Auv>S>b%0bV%!uZ5K@P9_oq|x%!8?qbjB$nKWt6y6|sdl
z)?5qE6b3LgUbD^%V7k|Ls0!k9jj91`o_mJLjiaitIoXwfX7jpo5l{(&V}EO_A6l8f
z3L<9hfW&=cMA<{|F?&)NXPFrJZQ0A0pvz}o7~f<58)rW@45xOuhchvB=6*M~Vhl0B
zm;osMdp;Yj-=AE78hH8K0fWCu$q%$pct9v6=oN~zN|&N3i1!IX`#>wb{T`9@ou1(%
zIB%P;-%z&NS;u5zaejPHScYz4Ys^MJ(?WB+#+rRt(=mDGb=9-w^Z$j$F{14;NX-O7
zfKk15k_<WxhRF31C#cHv$jcmvj&~l~nL+sfjUw^T+~n8@N@GnC?eIJy;nID{tN-L4
zq7Fj^No+jPxl7#>hKTYiwca1=oi(^+#(_Ts#~iSrQfHAolrrEH2Y6Hq&IBKC&pR`}
zE56$R>B6-|?ML~I|7t3<SAcBSyE?r-1ep0lixGqhbJ#P`mU-AH1(L#qfDu%hdNZ*)
zEb7w+bkBGp?Uz@6xo!%=Ix?HC95}WbBRFsD*5WhcgaN-lmhJqc$gd1l`^`B#5pCpw
zegaMnFb$3MDmkqaye<wW3w5E9z0!By`)xW{t-;6UUOF0BFe)we5qB(w&Qw85y9wo{
zGy!PJvkApg$D<uVUpL}$-xBsw^C%+o(LX-Tozzc=rbrTyjD0BviQ<i=?Zv^I<2?Dz
zYwkJv`1a5+J-&2H;r`4=#uWG@4zp`8lAcg+IKDR8VuGIk*YbF#m`?b_=Qtr&0BPLo
znHu8>@P98H9)T>VH5yU2f-0&yIX1^=<>fmJFX|*hLkHDBmHzCvwB*jlmj5z5FARzX
zQAu+Guh{d*XrVHDEK(#Tv^ybVGuPAE12#nrhDx=PIPmf4&3ymmMSVaXt$V{s{#Y?Q
zqW2)s0eY@JB8;f}$9<fz1&!0iAe!zmzbh1q{0P%eP+8jFV(3ZiIcieTS1>nppUGC&
zPz_iV%;v&B2y&UywF$hQLkSKZ5xjRQYCQP%${7evE0^vQ%5`OQzoX|{)*lb<W{EAx
zIA<ag^Ne4wj3dkQzT$@Cx*x(DZbAIyQE+5wQ`~X4$7U8=g@?5m!WB1}5}0^Sn-sQx
zu8*`I3#<_6`;M{LWm$n29C5OJc0b-g4VA(hXVdYOZ-FdVhRC5m^iwmtp74uh)+?AD
zTk*y<2G;+OzfNBE3LKF^pYm@O9{$;lZ&H3SqAoZY$+96PO|B5^zHk~TCJwDnkKD>M
zjU>_pecjh?a@yl+cn}0mwY<7j7{Blgq62&))@dEl{ssKmALt3~IT9uQ)YHM*+<F3c
zHVm%+VQK=Gm~{`Z^Ez0KZFsHl?E)v~b5s7T2!$}lobroNgED@NzxaHAbpdOEvQzn>
zQU&)-+Os$bMp4JsXR<_0YHH*ljZm+4iX(#5NRQsP>1@`ZG<a2GZRIa?d%hMw@zP%d
zL&E*^p>76B&1)jNv}z%l;%YQk1Z+KlQFCC~%uR2E;O^O%UA(~S$eneAvpJMWL|)$Z
zB03uH5keCT%+?Q9aq=%IUY!#Qh8U2$rD@6WeS#J>dGXWL9%u=4sOT7qS?ZWiVEA#h
zFmuSO{S+Wo{UmrL@qk?7z2H>mW~T|@*<Iu~JlmQ1PrAI830yE$>^cL*vJDXc|4R0x
zyt}j)$`C1i>E$#2$O}2S7mvwTPy-oyG<chh>C5b_)foTK4x@jAd7H@~q+Q6b&`2u_
zFReQ9(e;w&M=8ly@OUN2xS;$%ZtKxz=rldeSvHM==9}fz#V6cG6BvO*`%KEe1vDLY
zkZ>zCcHTbd?!Yw^q0#ZZ;9#1_^&=WgXT&_R$2iqZ3GoT6m%F6n|6#VIo&-|5Dts`<
zDRiKlO=4(?4GRFe{o6lZ(V@9G%qPHq-fRMHXOERBI7$BEph+~;R!XNp(j&_llzrH6
z<`A-KW?m~`SYnnPm0_3gxJMo>iuTXU?$OmbO>Td#kH=c>I<o>!8iMkbxnzjh@^=`m
zKms~BmlFHoRqks9S1ImSuuzU(WfE?;I?wzZI-%)s*q6Qq6f-B<6$`XAr=^AZW$8Oq
z`WpI^+yoZmV5a`OkYB)wet=&Pi*qdl)zgd75#{&t<?*x{g>T*nYCI=PfMH@nX>>NS
zF_@C0GKNz4&_&xT7*a^J$A+`D932Eq5|R$zv(+Bv@Q6Mh5CO=(5-nsFD4MRuIRp_L
z`THldA=QtZ3xy2PD*|py6r9f5#5dIrq;)(l7KGtBjkKxg@W4mw=;zWVPk0Lm+*P&w
z$ZXK@pqhr>JhvY*WmaMPNz0IecsJM2z{s3Hv)bmGCZ6ju*!0FVGc;c2F_*oxaWoA&
z?A8DMH4#h~JW61noi&)9bHDxxolhtSUSb$W_mI7=vfm{;f80LMu)u=Y3(kwFXh+Tz
z_FG#6Y@$a-0K9Gvyw}lecp8-rMdAO%1a0G<eN0r8qfKSxu^v^kD2a!I!1yQ2#V>Ql
zAxHgM^H%RbfueOUKI%TqB~X0O8S_VK_@C}Ol95oKO3NI+0qY!&vrf8lO1BOC3Qi>Y
z+cDyfO`Pw|#Z5L=pPt7ONAK?QbDt&UQw|Upl#kNpNxXKyNAVn~OH|Z}80+)cp{F?A
z)54j$Hq^!>`H#~MC?_kg+270q|GzR1&~q)>EH7n_{g&<c#!tRWmei_#+Gy~ceD<oJ
zGD9wcdDSd%9yK+q+FXGw8|h}GE88^V^IdKD;{UR3viTM^G1F{c&%;{R;!RYP$EYyk
z(rmaFJ~eKug`3{SBD)t#bLuy54F+<3;KC7K*C*+Sda|3249%tD90ozncG$LCFxCw;
z^NO{}UBT<ocJ4ev*s*HtI)C=y@lYJMCAU=x>5QoA2(+n1_P)Ow_1%k^5j7SCT0K<F
zHHu}4@>YEuL437(9t^S@A8VAvKATE678V$Ms^1fRzIN{$>!^rXf=h=YYEG2We3>{2
zb42Y?o(!JoPd5BPR1ohSN;{mPBaqX<jean+$4Y*y;(l<@66K^aU5+=uA9t_(eO?oL
z;g0qz;gwGd3_x56DR1KXm>0ptZ{3Q?>&0&hHcq=g>%Ass*?MN8GId8Ev%hzQg2AFx
zzrb-HWc-MEQgAGnKX%E%vx7<ry8Ppny&laGhs!~X|F@UJ@cMO*sJTHy`=I03t8jHh
zn?E(2M^J=bnN8?C!}VhE75l^+)W+8jz2~C9r&}kt=vuQ<OEwZm^?E{43O?i2cZyvO
z2yfd_Ei`rEC#nP-|6{NM94WY_7plLyfV5$gw_kk%KQV7Q>NrXGZ1qCVSBM_x0Qpnx
z&bjny&W9_HBkQmBgieK=B?Wd+d^m|J*OSov2ae4u1;Av8B`F>{r*pL#9FuiRke)tV
z(2idB8!$L2`h_U>^_5iJ9!0gAlLb$b(KUbYY_ib_mpHps&9nnsN@N>H`OEnT2|vej
z=Vpf=$R2dcwoQ|S@B?wH*bfewxKs7G69xzj&+m1V4NGuE;*!WpU?u(ox4reJ1cQCK
zM+^{D)`65aKLW{4TETq5bkgCXskxIhPo}vsqbRs4(#HRxG`z+V+{_{VFoJ-a<%}PO
z@_}MLfn`>vc8uIlBxfm7Hh|C`t8L+vRkhl~Ef`%KNaZjYn@GOO_V&_~@uRnJ+8Dp^
zLIb#|36d6^&YQk;=m`PojOFEP;9dEJHuu#vj;g<Ip7b-k(NZPo1S$Taz#pEM!B?ms
ztGI8UCDy+pi)YR9JxZ6jL-A%Fw{<zL*$7U7$L|tSnbO07RuCVafG)q+fo`s`Hiy`1
z2EwJ*m#y`y@478Y0jFGVbHoLgt5Qo9LF1_CtMm>(L!vjSDw7UW`nLLi5NRSe+3UoX
z*X8Rn*Wm`Vy>$tFES%0<(~Ptr-V^C*4|^Gusu*2L0~eYVtq9zbnqDc42UWm7e8B){
zhItX4in#zz-Mn$5T)zNk+8@2AVmQ~AW&!+OJt^1RDqw88u0I8UAKd6|J#1CAwT&Gd
z#?$0r`s`2eb^kVsNtv&E+zLM6x<+2@7NtGOdrj@Ecfp+=Jw2liYd7(B0jDieXgUAp
zNDq;y>wSE?nyV)Aggr$a>b<{BxAYN9CJ}{3+6yyU%n?bSJX&uooFwX4k9U_}#_iRN
zuaqjlP^)w56n8n)0>2jY=Rbmj^c!tw!=34RRdAg`QT0;fhf3d4r>U1<@t+L0Z92LT
zVj9)*N^!|urz|deVTZWF(jQHr2k_ZmEY2k_L+a+=_t~F^Q4FG#XD-9&OYlAU_VG?f
zmywaa{`&gj8raC{>id8f6G*tq^b5=$>U;-}A8z`5^p<E;flz*~Xk=pIvo3w^5pdZ*
zzZe)_0u(s9=w%B#Ds_T4sRE6oY$XFFejp)x$~@^nq%#`#M|^O6NOH9*8$9V|K48Ki
z`bFXWLp;B6iE}FK;Vw8n7~B~ia|3rqNT>?x>_w}HS#F*-XK*7h;8qlfmeZxiV!Lo(
zSvBUFkWsl!UaJL*glA$)|BwA0jMT2vEa>uBo<9c*mhZ2BK0X@T=UMLdMN-ncY75xy
zvAiaZjy~G@`hlSXzqJnaTNdpA6W7<{(}w`U_Wr%-lZ<mhZVJDt{Arw^P(xbH!jG`v
zI3LemX~o75@ShrfC&Gq6^iC~EbB%G+e*x>U(=eS-UevSvGr&v$M%}ri2BhjIl-X%h
zlFy~UOdZVX85vhTRcX}P>Ad>QZ8U(bH8v)$p9sn=aLa$7bqXJ@nE$)|U-u}YMhI#r
zs2bj!w1l<P^|h&c6|~w@@CN0!gA6k5ig;krd4{ItL7VDf@M)DJf#rB#^h(<cHBpzj
zU6{@SMmM<kPkfZ#h+B8c5ChE-F|quH1E1oS{>yelHX*GwUO(%>3-9cK@8-uh#prX`
zgM%X+#esyR{selw&&r;r`=|R8sTcc`6U3@CJ+9(7ORBhhjf^Cg>Sp>4`Y?rvzMIN|
zj+bTKnN+5!2jAtMsB4Q)-d8=?em98tg}skVoO_oo5ZyVtA~vs^R68^iqL-~Ytfi(?
z_eD&|P0D0GG3w_1mE98WUz~Y%uWq-mQMQMHqyOnELyu2?HpXO+EWZAk@W}y<Hk@|-
zXjHKj{PjahgXgf!k`}z;MT>8**8n$sDP(U415o6Xg<8;~h~BGToW^Y&#<#}oi^Z8m
zrf05)?1smdp&2|s(vVqmzGO%g(jt@RkvHZcF!bXu!|w@gRGc8NyF^1T))-H$W010&
znq8|#IIJw3INtgB0{ukuXLqZ~WntNOlz3(K3PIe)Xrz9XOib>HSZl1bkXHP$uT7y-
z`48Z4&_8#F#QzWyzoW9%vbHwD)$_|(1rvC}l;{^27z`$$Ucr3MQpi^@=T$Wm#Uk$B
zY*ET_LCxvPH@cItFvlR|WbyL&uk=<6m==>E_oOBhJsOX&{<-YYYEk<9)a9j-Lh*%y
z(L$s9;c;#-I6FI=hZD_UCoeA~?lQN$GQpB5nEg^$6I<ex?b=E4D-eU^>|ZE*nlInT
ze$1VuF`mUJ!djY#pm~8>6hXk{)^*&jW=BvRn_2qVky5I3f%nqSB|__;Tegsbh0F);
zzA+T)vMIcom+yx`e(tt8K6mfBh=GRiF*LARzlK}o@91(&OvH7*&&x#`y>j5;DSR5D
z1lbpX>|XlT0D_vIFbAIb`4`RMWf(kMsb6sO=Gz75p50y9^EGJ)K0hb0adG<nG$;uP
z*2>BThf;m81>D(5x*;TYq<#ORh1sWAES6`144ar3e<RUzdojDGN92dCKu9YZjoNs<
zHSI{370Rutf$h75Sk+J|JF6IAmrWhU8&YLRWIS-o8ZU9Ql+J@>=qy_(?<2&y63xm#
z-z=lj%G`IFkd>GBiY1-gU!-J@;Th0y>?tg$$92-6wyGjaG00TZ&G~Cy`~h|xSvi-l
zi(7_K){TbZ-fJej*L71wCOMD6>U(U$-la>C;emnLh$k~Zn+}<2&sq8Kb8;pw({5yB
zb2^&q6R*B{^(`58&y{;q&GKi5m!tJIO{<(PUv}=E!(z9{R3hwx8c5WQj6RKQYWf}|
zy?PaEse{2LB=DKl!#GWE<FWB^_?xX;Tm5QROE<SSzJ*Do1+#Y3*3BrNPki*h{Ki`v
zo%gO+a$Bln*X7(a<YF)mEb7qGvUxj1X%Hg9SD9AJq{YDPHq6b-`=c+d=vzYrfVn?g
zL-fM!M&4J!=<>TvjEq@m19(@!_-2fW;We(Rf*HUOb}=sn7BRg&6IIUkFj^bJ`a-gP
z$nob>7-U|a*5E*P{r)zSUh$Rfog|QpGGDf!9ck^r08vE?AY~XKOlNFY(FXMB@x|9q
zB!&FM&n1LQ*axvAwB_YRA=20xP3WB~NUJiNdhkRsx4Bj+a^<B@nGjej+{$wN1-`Z^
zB<Og-m~reB1Tg!o>BGt8(^4uG_+=c*+hK<R{0M{GUhI6=cdB~<FZK1&vmy^$UUt@t
zJf*lH$(DMe%(pQezPkBzwjD=4mqu?lHp$4W;S4XY^GLyK@%ug(FJ62p9D%x`gL-@R
zx~|Zdg$Fw8t=*up1F4bm7`{4IJH6lPgkFfB65}$3A42@q*<L7p0{hvWkVLvmF|0$b
zKW$-$-F`d4>|@byqPYU4&&rQMZ6-GR=FCqWadrxvHf47#D!&J2N4;pfHIj}ZK2B+{
zJ^Co;V5W-!`{D=*iQ<Q8;p^ut4<_aAi)1&yxuA_HC|Cq<*F`7e2A{p1uOE!EG^1KA
zHVW)GKCq7}|8E~@WYhtxe1b@3kxXS27afZi_LPo=H5I4`9p$-Nk~P(vqH175f*$32
z3L_NW`kRhj&_)>ix*0`=W~fX3ZmPzpXZVf9@5MTWXo9fDpA2(j33@zq{PR>4B1iv;
z5W7~@*GrFFr$|I=@vdc@VjE1oI766^{3Y>q(~2vTUSCM*BF3tGgpKekI=&Dimex)3
zsqcK#%cev@5w6Q3k=(7eVXW=tsyBJ}cZH0PQa{Bb8~qmFHxZUMNieLOSzrA+>Y?nQ
z6^{d+_1}3t>%FKuAIDvn-~WK=G%(Mc9IN$kZ@Z{y1%BMVEM*~bEYB6PV>MQT)?~m9
zXP2+3SK?b!HDO2SWPU#Kv-5=(bi5FPSm|!4!jzHY+gm=n3HOEfXG13Ys1~UN(a6ut
zH<O$a<YYI!NL}yW=B$zyYVnL|Bg#c`PQ9stl<g;^On1;|KukGt7Dr}&DKc4bEyz<+
zDtB>Z@M1nPk|XQRhgT+4fqi{_#q~9`jEq+t$G2LZe7d$aF|jxDzVz)tb?lhU+lcLm
zIX^oNt7#Fj-SM^4i2{Dzq>}sBvX>1Ini0-gLPqa@=)CRuxE1QHK=|WKrD$A$oE+o9
zy6xbeQa_D?(#~;od{pozGlhW{-KUC7h>5AxA{3c_s)o{P6~ooEJb^ABtvADGCz}ii
z|8iptx*&MWCA$C>u?I@J`rO=HsN^I!hB%@cY_$f<th#P8MaBH{!<D>7@kisupRiOw
zGx!A3Bw2CYcYU`(IkZ7}UWLzEkc#zQNPrl>Xl|;#{cJU10MChm{YZhVa@zUuPL7^H
zqM&X<ay|_#0Re%mtn3rYu=T2ob7h1divb;GWT2*I{5~jk+9Iy?RJ9?IKHks*ji^1*
zY&}D&drBJp@kWEA?3}`fhr{hG6pvZ`bXKhUQGa!_hWgSUZ>>`%YCyVwdnU5%xUJ%y
z^3OKGOzPNj^;HcfpB0bes#MZQBG!E5tDd=+L53P!@ZvS!G^KRVj?O6M*~NVD=DfYp
zb!JLnm8z)qra#48R1LY2HD<Z-=42VkMH+|^(esuDgub1)r9G2zZ$z0Y(9h5B`e~2P
zQ%_iZ7Sf*VH`N|wWvQ%bAE^-B>ytY!vHQK{yCx!9eMl45J(ZaB9MfbsX~7UrYK(w+
z1Yx3qdi2KI!-JgzJvyj%cV)$gOY}iF8|lJO+%V(qind+X5#_nMcbZH*VwTMBzv`6v
zy3Emb&B<}#pyI{=q8Jl#&5dO(cQ2jMw-W5fD$<jq1czdh#sdVD425IdC0-0<ne@h%
zJJ2#j#%6iFidq?6ZTwoj2Qk{oGT~C`gKAhjWlLyVTZeY-jXCo4^FHelGUJiS`EDz&
zsH?fdFYQk(Ka9egm^f9Pbt20IiYZaA_q7MuM6q{TEJN=FUv|SwyrzX0J4W_>%}ZIF
zhCb$ZUFK&<e{@56Qkf<1rJRrY;6zTp9v0-PAIEAC=ReO^aw~mO56Uh{O}(|T)UIJ@
zm%)Dh5^aaKsJ+6#cZ{Ia&(}vTP_k(~VPzC5ce*>6eXxC{Vdjzy)u|*QIqOk)axtEV
zi^s0#zGyzl0x3mPFKzviMwm60NXG94&y5zpKyIE540BEGWjfI%G{Y6g%|XQDkNYVe
zYl;%X@+K<z-Ndxj=NlmqUXq9tPIf;p>_DW_<D0vZB{DpUu9*e?=|e`V;ewKqj;XDc
zH^GAo#pUG`5}6*;Zx(0T)goS4pT7FuK(CTg&GO-%XGB;Gv$4q~d~`fDgH*@-eDcpp
zOMJ_v?{ep!%t=<Ryk*_FXT<8Q^*+l^L9#G!k4yz>IGXDuE(-QxFXtYo+>7?4n!V^j
z74PhJQ+9Lb(I74d-pfPf4Q-%dVmop2(u*Srn?vCe!@O`r<*>PINZvgztDquxBLP8-
z(_<MIo`wS|N(b?E?cwg$+oz;+#bfxx$~&Q3iR|3P9Lfx^=c||IBGO=Lq&8x;Dl!3W
zVA?*B7$d2W@tvpAlK<Q=_tljq+Nb-E?7+U`%gexm{B%A>+;dQUXBsZ3s?xNIQx3WM
zUU1OaBh96V9{>32k-wx5lHqQC>lb;*Ag_Y_v$@C`r(ZFb;d8rB!sjxuYIWKaM)(}_
zi?NIOG$?uh0KR?S`Ik);XoieNjxz|mX0LI7yX^bCS7A=3gCQTgSta0l-E26qfgW{$
z?F}m?|L~aB=ChH19K-!D6nrcqB*i68Kf;_gB)NlMCgX%2?&#%{RL2?$KkL4PMk3sO
zUcPhI`<&9NBoG&2hr}w&Tnq{eW3^79le+zkjg*%)pmn`uJ=ld!1`;~sVM5=XD~LbR
z(h>lBqri+F-_9lb(7Q_;@mtU|ZM~6-yJv*@JYA&;Ssz{eL7KSHCt!0l{FuL1AyQum
z<0gDN?fNU?UDq?zuB%>SIT91ZXa);;63h)DBli_`$k5fjQV#~1bCG3fS(hUN3re4T
zL-oTNJK+s06F+Y|TXa_I9E}3f4|PI6!U(|p<`rsn%xxu4lAiO&vTYAz=#ia*6!))S
z`R(QP2Sh!dHVoy&9VAV8x-NMr^B+o7M5}d3Lv76UN})E@#78=q;1`v1<?D~-+w(V{
zhR6!U-ATg2@l0kVEVvqNc3)(oA*x|sBr=A+sLsXnM^173tg<^9aS!U|iHPv5ET0%$
zsX}l>8$l<^7*h32IZXLfJieX6EqO;620&V?B#nK}v&bMCDHxaiM-9iYR{~eCMa<yL
z3DGZUR(<qrOhh?`VYJPbDSd(kD3$RN%O5%OO75F4(FUF^5xJiohEF?d+#taeVn#<s
zA&MU<B!|aV?>?aku8tpELZep9>}q^3`OIm@s`ounG(kCAN&h24pi1~(uG%>rOjOOY
z0STjPbD4Gur<Gkio^}qs`I8F(4596@EVC4|Un?mkcEYdH>9rAou@uM%JNnZnwN@4=
zUxWlF^LzHkKH(_jJ;o?_L-5jo5=6H0jgA3LsV-bC{)iAAPtF`BZ!Dj!z!BPwCv>jl
zY$Xf_yY_B&`9W7=p!wEa_t)}S0A)UsD<u`x3d~zkBy+nF==Pq7Y5(X82A&PXdjV<&
zieQ!tJR7f=4{v`9y~Uc`19yY(zGxf`{B@**n+>CdZIjS7{LyN?W>)i52cv;e(#*f#
ziqt7P_4Mwq^8vV}?Kh0<Hw*{@CBF<h9s(fi=2bg&=-no=otaVgRl${A>i=Ou)4!~$
zRbYG^I$C{w(MegFEb94<Y5GKomzuTjX9ZqCuI|2mUb}VI;VnmA%9K`!fbeT6%*MT?
zGKm4ryRZtlnAzXuY(2Xdv$aW@s1oPv*G(YsD_9xo($7a1-(rxwR;WM$qaWAnAg5>A
z#c02e(|+G!!B#&TNr7C~L>O5k7-ePSk7*+~F*jwir~49;aE}{G`|mgA(??w`Yg*z*
zH8on_JZ)N7h!^5Mf7#-DMrp)I36+sC?RF5-)Sq2I@RY&xFmko9(-x^>x#_BSvW7{=
zTQ%8-E^Con*Ey+-sc$_kZ3=ku@>kxAiV$r~_|v#FXBtqrlo&kdUO02EeMafU`Gu81
zX`u|MV8zlK4ea$21lBZjtZW#+ZbPAj+5@DInz*DeqyW@4fjBE2#QH<c*n#tuWM5;d
zo_)G-qr*o^A9p+%r=s#2`}N=GSlU@OcTlLqMgJadBp-$GOY~?u**VEP<%4%_m>qg+
zQ<InqUsJ^&r@lxP#s_O-8b*4PFLQE=@|W1}zmY1c*h@psHTe(8$UGupe9Wmx#FiNi
zNT;4Gj}Df^NMY37Cjm1XV!&t2WHc7&C5uMNoH1YAxZCScXJPMhwWeBx`-&w0#Rt>R
z^x*qrU#<Pj>)IqD*((s8S;}Vr<C{AFw{NP9Mv_^*8Yr5VFRXGIap;MV8%$o2MUTFw
z@hXhJRB8~NH(Yy7#p%IK5*Ht}S}3nIsEP2Cgb0Vb_j1|J)PBQuN#I^31w_`r%P~(M
zDQSY`(}69vI~Msu&O+aLfDb*|u$PJALGe$o%pepf$tr>#uNNoRKWHW;43ni}C!iWo
zw9K97N~4!ZV-UM?&eFzJ_9KlyoBjoCX@!`R+Ku6E0p1J6GmddWC*p}oNLrCGz1}PL
zY(75?PwPj#@?}Z*wU$%q<Tglsc;)L)5zW%i{6zf;&Kk0{<KYI<NHhvAHBYrMxu2KU
zgU)f2OdCnQy+xmgyyQkV``ob(9lt4(`iuVJeNQD>_x2ZRLs^gq7zE&+;3P=zOc&1>
z&o4kY6x#S~95*DG5Rfhd=a+QVFVRTx>=#YNvOA?`epWNwrVu38i6WK0!4;8~TpVcV
zIl_Iu8C6qod(I_x?aIpBt50nbXLK;BG65I39p!{0?XMg$Qh3fY8fx}D=AH9>hef1u
z|7RkoVRDh{Ick^a7c1XXUY~Sm-X*l84}Ys>*_kYKZuV5&HymKZV5AYEsxjJ|REEs<
zvkT&gfT(j~_uNTfBTYs1KXaiL;7Kw4@&pH_#Qn$3s;GmJ8tUdgy@s(vhJ<W))fi+1
z1?fg<?|VI53DwY0(zGb%?UFrqCqa^9lp2|;k(+)ELN1tM!<&6bbJ#(-zpWtqh1|L8
z`Y$oj+L)(XH-A*;6U&fNkSEuF?GpV^_Wn9oS%Q?@#A$oqE8bP@>sE)kF!xD%Ds9NR
z(e#INPQvE<_yR{eYP1nDR3%k2tMZ0gK5MQDFs-pXFVl{mo+V{);5y~`AN}c7mpm*f
zkJ}2w{dh;+JYzv5(_vG1>AvfQsL>l^yz$}F*-E!nDR2R5`F{^kclzB`A-r8v4A%>*
zhnn%p2mt{>(S@b50#>Gb2it@1g!{d)ANk^U`6@8=9^7BrIuoa+&ir$1vLHO_7wqvO
zG6r+*)`pi(?Ip(4kJj!)c2!Q^wdrxFON8HooSLqM?Dd=)E5D$2wI=UE{^CqiOvEFZ
zCV1`E4G$!C;?;8QO0ZJ@z)-6^WaDKSn8<l#H%DFNNSLKR^p%t|7ku^54;E+%EUz&8
zetoQsEr`Elb)l8@F8$lfo;nD7WGBRHOZrSd+%bhh7^4$)M@rVee|=p15P?)^)&J03
zmFLpT-QqhTbMX3X0K=KqL>ZwBq1p46hLUw7$8nG%zl@O9qm0t0`5i+Yfe{R;fSP<h
z97|iJ9Cq5jAt<;ZEw~}`5~-}Keg~X-UpP5C?%HG@XT5U4{C&7ggEv>7^q{)DBJ7CH
zK#k|rOW~yXR^zT38SouroW;(_Wt^M7Apb^<(SbqC!T;F7u~(tqPMr?wxmLTO-=}WB
ztvT_-wuR%pzii7}<;cN50$1|C1ulo#CSN{^XL}C&d0@24wg5Fn#7oVN{3;&PZ+IK4
zT$&;tt^NL@4i-7IZ0ZK{AU_@aM2k2^vsM|s6IXvH&d=?f-tofB^koyb4i?Hgt)<hi
zi-*`PDUO6ViXNdpZ`qI|1Lczc(SM)H{s-lfiaZR<Jr+N#xi;i$e9_C=x-T_FYB<!5
zIZ^$d8X5^HHQY|v+Z(x<<{Nxk!WKej_WFS#JYl~1-R`Q;p>)Pflzf<??!5)IUzYqK
z+HinEeZ`=S_?eRsxLhy7_fr}kR6rjfrb!#_X-)wl0dq@|T!u$<)wo%?%e?qP;;%BT
zCzS_oq2>-RLXbpSUZ&`0_Nu<P4*k{`86!Ujszf^>02=d4Yo-qN3Kqhw1N`0ck!v;e
z@<&pa=T&FR*Ut=d*Dcv*K3DnVIn_VRXT|eL(xN%w-KauqSl5H;;qrD<QnDA?UyD;{
zUp9qmBd(r+(v+c42=eIX%9F;FO#k2O&|@J&{%jqRX$E7JQo>(ib2xsSKqQ8{vC^}^
zvU;%G50P3@0mzEm5l*nwk~8VgZaxl!pLAv4x;IUsS(kVH#nvAn73`1k4iMmLBQCL1
z7#M!`6~o;gyi4p9d^%YMev=wgLmE4lIc2E)1N?n<7l{wPn*j?B3aE#*Hq>K}LV4~C
zxXF1`JKJsW9%eCZcQi>z)%NJE@DpKnjqxP%CnfR9j!FkTyIH}0l{b5N+$Vx5;z|&o
zv#jL{-rE??J6~DfUrC_pbU<u=ZA2S5*r~2A%U_-m@Fxjw_(_|f6K`0jG?H*&HGQjX
zAT9yTkms|_%6t1<b9k7}&q71o<K8|p+}%|<*Czqb<?S!MyR36u8zXZjH5w0}&5z96
zF#eG{8>IT>H$*aUH%WRCp;_c3d<y?s9;H(@ktX(-v~I@ja^+&&h3@p}lNtNz+h_VS
zkdC6p1A9W6SY%!O!+s;+2De(z#75`4A~?mw>!zMQdn83Sb*l|M8W8ZHP>7UU(FmdC
zkc4Ht4h;%g+8}P8)dpsDtoQVN0at3AIk+DwoJNnSZ{Xo8<7p$pn6%Zgg8l>MZNt4J
zEXFno1lD8MtakA&87?|nGiYH;M$UckaadZf`S@s-^<(nJ+(@9nhu165;nz^Zs~OeG
zj-0l-SA;STD;Vz&8xXxt%*abHOp(z@R-{Xzj-AXAJ;YO<{L_cN4%Sigv&-&jzi~-V
ziYXK?UHgcr7D3(|30r>uWy7G}=YXuulBrl07oi@vhx+WU`$pgQ3GGD{O01b#vDS+q
zozoOWAhWs{ZfjHQ=8+yj#I!jPXd?`1!p;odTfq8A3$3M@b@k~SvLthn`WGJ&Q8f}t
z3k_Bc6Ade$yU2nx-gD?{di?5=Q?`nB8>@88qiOe;8aoc1c7Vm57hPN`eT8RBPqyjt
zUPF(TC%KtxTpmm|OHfB@E%MEy8DzuuLudteyR*p!jft#r_5P$?E8Z}LPvD`sz4ueR
zzkFy*g8l{onoHiHQTH}5O{8p!Zhi>%a63o5GDemRt4uqNheq+H!`vJmW6gSeg_0X)
z!Gm)lnXEizs;2EG88hEYJw@XnrZoBFY}K*jiP{vSDpHafEZPXx?FtGLVgiVce|n`}
zs{i?w_UmE==vkJ{$`1^^+pcCFnNpLp)4<*{+-^>vnK&JJT(q$%R0nb1ARVq;xyomP
zlv@*G#qs~cHyf<_@Sm4cw>l$X{Y*_%vXGqF@y1^<fl+S?(lxa1wPajBD<!{k;IXxJ
zomhrH6_q8z9IV*Gu`^R_%iIkYl~04ontBm%dH@!@l-WRy;OC_nl{QJqCqTy^-!lv<
zI%EgKAIg%Q_rv1==)7(3LN1a~W|4np$vS|Nw_N2B;!e82K3*9GZ6)TKK>WhXAy;*+
z8$vC=Zi;2O)e*u^OS?6>Kh<GSA2oQUjo>Mxe1c~9bt)Sm^^6LVe5B#isbBA`LhXOu
z_s^#z)JANbpd<p?>i07Q=P>z1GNhE`&7S!1`BV_eGd;`@NvW=J&Knq+BPYkQ8{>sF
z7<};zNS=K#I2&KEJih<E>eMn_{%z6gnJ9M4p6h4><$E=3qyV*9U^pW#U*Uf|%$(R*
zlo>p<WKvaAK9q9LiN6btT(DK)dBaa*(pDAYdx78b;4}wg(l2y;vDU_|2M<bY%~FKw
z_KpmB1~Xue^t4qm<Pzr|lSZf$i!%8k3&;;Ka%mw1{aw3?tzdZ}^5rf+gjXAZG!lPQ
zWtiJe``(X7Qav7vADp_JWT-mc{Q^MuB{@?hIg<g!;0N#NTO2x=&=JP<aE@H0_S{jq
z>}hP*;T4f$Gb_7c^sA0f<4Azq_gjOUh%d47z-P532GSTk(R;FlYmBmE=liZc7W`c*
z%h5V|Z7a`SC9oFbj9SAJuuAp!GgAwa2<bsZpeV~u0QmUAGW`PA+jhj`1GneB(o;l^
zN~lR=umH}#c;7W=NQcxv{L^63><(F68vX0{agjuNTqIB-DF#9sA-I0~gkD5m1lc)+
zG?uiWg9pa5n%^hO9W7;-Ahdo-GAQ`RN@TFc?7lhs(A^UrM53HxjKDHrGAh5i7-c1V
znnma#nP%;^yX<m3rRy2;BPSLeJ6V4IqJ|yG3zxz`0VYi>WYvp>R^^7@?S9Ot37;yH
zI;LpL4S8R&kklV`;lD#90w>s0k?k?-6p1$(l?VUmyoUPd|0~v2lx-r!l%#XjZK^G>
z`&aeduMb@1w2*s)E*~E@eH>eRvv{D&Wg;SgIX}M`xn^2@frw;!gd!KI@pcq8QOOZ0
z)yZ2N!$ol6ug+kCOt|*^cP3m|Y1399|6e~G$HP%yz`;klX^ja}0y{l+AHxAKhVkVR
zKWLgsIdC4e7EzSiu^j%v`o;4g#`KrA;uPb$iXVR$sqsl|M5rvPu?a4<@Yf4aqgN8v
zyv8b0gjx{ncS1feQ^X%3z#X1%fJN}{irtc0qM_yYFOwf3ZU)$*{sb$JEBy;t**T1B
zX%61xDlGoOa`DsJ@cKjX81DzpQ>DEEhW4_}K^&I!*V;H#c+FmDBfh*pVm5H?09Mev
z{|5jh2VWXf4)`dM%KB79nW!_v)VR8iRnGlW^iQu;+*OnBKU_84WBpJpMgs^Z4qJYe
zH5iO535d$J`CBz^Ad0r1a~hHBO68FjCRMFQxes%5I_{2}d{PKgg$rW5HpZc|rX68u
zUor!EF-KdFqt}%7^4SyCj2c)A1+`C|#s39~ogiu&88u#6;ON~mFDWAvwdZJU9)1R;
zF~{Rz7s!PdDJ}q%*b<eiO*Ev<U*k%UXmU2)3NP0-Li;CS;_xqZ?%&{hE|NCjyS3iG
ziG!P2^uR*+d@~pBN+k;q*4=$=<!#TM9adI~4wX;PP1@}(nElX_9;C#wtwl+R+^ryz
zq2ivQiTx-U6PLnHDhmZ%F4D0_G-x@SH6>RSR|{J<Xked(aOniSE$;|WL9oQ0yfSZh
z;yar!V87nde^5JRsJ4E0su}jEX8#4{G*&@dYUtrYZ=o@&oX55SIkH>?ulL~(%y;v1
zSy}s00Ri#a*s^5rg2nx;YA>Gv9fa$Oa#v%;<nVNRiUYr;FJshzK7>J(;g6iVHuQ8n
z+6)|nXO9bGU5!l_5`h%*&o=ooLWpiC>RT3Qxti}3KMk_OcglXI^^rk3W`0_sNZanx
zSI=Z`LWvPq1#7z4<*#ZTobe+y!fD+cg%`gPU$M_t5@L!L&d=RTc)u>2CnD9=)D%hv
zT{I7i`+!tunS6jdp1{N@0lK@UD{u0(!E*DK3>Cra9W(PO0yVcjr$Jvm9>U;nv|b*{
zo^n5d!*P|A`LF)=@MLgJM1-oAo*mzOW|K>gXN?w}O~SYI;C1}0*viK~?4a9X4+osi
zPLZlta+)0nsjS^bQD(D(NZV&79cTVXw1IKY=j$TeN$QucQw2gJZudEYNA6{Px~dY-
z!K<e5J+C+$*N+(}aFA6{Fa&m7>_Jwx>W21lEv=mA%nWy{Hv0OSt_BK5U%ppCs(uF9
zP39!rhLz@#zJrH)#MwBCCwN=uk2e}$`amncRgD|1am$P(aTGDP(0`xiRys$=yO=a)
z_TlCJ*$+#eN036x@;aGxRy(l0N8E6&KX_V49mLBsxybX!Zrc{cSm6MJ$)#^XN6E=I
z$=(93a=XY**ww~TTH3FZS>^->be#OI-|-<3O=x5O(|oH3#V|*KjyHFGMmP0J@0wf%
zo~TyqrEe5$T`tb&^f-Mk<sqF1$&b359SvdZF%$|>>`}cdI^7oMoxw>t2j|}CaVyyT
zJGUka;-lz(m(jmls}!th0@dl?&`2O(!LeV-58WdL4(@J0#8GsaBz^@q{Zf2m{1r|1
zCx0?+E~SUguhE3GG4$t>CEBXHP7$)z;$=4ID+qnSuxAw!cr{;WEs4w06YBbAd%d_k
zR`S?0qcb$W6O-k0vutPn+s*0b98tIiUAl5K4h?tWx3rBon8m?--!;VS>7lb5*~-^~
z{Z(T}(j2cCHGLM^NmuRGXn;^Y_b1W7X#^Td>R5^ltpd`@y0eUE;>LJRRS#=CgX9pV
z>M<S6`+`m=Ko%Yv>5ICI-mA_qD#q;}s6u0tA5P#MLF6Krv#I5f?*e~5*rXm_j<|mM
zGWYFx<Nh2S4D@P=pp3fD3adfS7V6-4$Tb48WS~5y6H-QJ<ia1PaOANIm4H*rSMpZ_
zS+X{)uY%BT85N;=bX8|P*DkNw0I^$hnL9q>`al$l9eQt1(gv8GUt_oC`nCSTo4H%@
zkWbKqsup{9sw~0BJN*g{DG49$MF9&)e#d3a9`EqIL{qWjC{P`b5)w&zvhn}qv<G33
zL1F=R$Wuuc@>O5y`qg<3Iql*&PWyO=TOMjE(gL6WsLCe9>`BlJVj7}xI@*}sWcRXX
zo`&M4Zc_W&&(7=&rbe|*bD<5qcWrXkk_SD$^(3+4NIyuG!h-H{VDJeW6}4JSRNg%>
zy32Crj_CvQldFFF3GwkNF~DyFre3kx!AFbI&5ehn<Kt&JTVu<tu>%9yD-*Q`FoO$h
zNhdJ#(ZCCHxLc|ZqnEuG2Slo?U2;^q?C#tNdBGayasv?ymw~9}I8RBv5Ii@}F%|;r
zuLa}fL_>}QL;5Vg8~cB(xQo>0o6}GM{^-Egk@RqUC+QeE!=#JFg&cCZfI;#d2WfL(
z+}4>9QG<EOq~HLCFE7)l&PQVnw>Hw^ZoW<T&cf|D%fIY6XVBkh;)L%t51cn15k7I8
zsEI_OIHKc$$ON_%z`gVNS$p&-y>@y7Ft*Z{f^;w^&M72`-{1dI%=d0ssJx=i^~*@o
zeDa5zS1Ju}CnOkA)3irLMTsZ<d>MIRl6j!p%J8O;xaa!Tcr~%*MzzaPQ0zYIma!GJ
z&!$ed1WFN}m{a5+?Cr_Fx3@Ka<Y!vlR<KtY%C%IIg;u5h;9J|pqan}Rt#Rw%Wv(pL
zVs@H}D9z@>+wnFL%cLDw4k!lVm{pzhZ6I;0{Nz)~&p-QrSbGboD%)-ETWOI-kdPJ-
z0g+gAw*t~#N-VlTq!FY;Iu@yPOG}GLNw+iz(hbt_UJG@fz2E&j`#tA7-x!Q%JYzVN
z`@Zfg<~8T<|DXG+gY$N*wY}MSANP`|F~)5qGud!vm3rcsT)?ITg+jp#fCiW9g&Nb7
zXF*_E0K2}oK|ul@r(%xbmKUe=N1KygYr~1yLK{xp#nqJ!2S<HUjXR%<Q&Z)osmw<5
z=>;ziLiU%N@{5Y%1uW>;j!)OS<OM1LCAzkpcD1!KumEgKkG8@=|4=5Kj~xID>-`Aa
zopVwr+;`^YD=PAyEjIvXis}0Cw8-7uA(xW$bTo>WjEvge_U|(Sw!iJ30|ccnh41Qk
z0b+ENX$Fpq?_WBX`@H=8%E2fn_qrWN9f0cE*9KuYSr&oF#e<nFhluq@i2(M~-UC(k
zB&$(%ecjVdo7pxGUchML(?82-iip5TmZw`=A9Hqc0zW0%>TmZd7bD1xhOWs9_MSJB
zsSJ7>_qYx3Y~bBU<kenXcumVpTJCI<T%A{EIaZUItW>!Ad<4S-UAfNOU%xl&;_)4|
zp6@w7N-lOTzaCxQBj8Fs{U)%EgUU`Pdq~gU=A%+hELYI!9?OvnRpKTptLV70cUOcI
zl+4aK&UAH_k_ma5uMQgX@UYuV*Or?OD222>O@-9iEsRc7uD{&g0>Eijj!BffspsK%
z&B=ZUz%JLvgn-rV!o`I%AyY113M85YbZ*WqSg+>cDF=2PXVcDHz;Zj0U4ZW;|JAT^
zQi+~Z#CHIgN#u|7ZP%--RvLYc!N`mRfM0G!diqMDNvadKH3z4)JFsaa3nv}Kx6Rb1
z9l;Pl3Vy>*(1|f)k$rLb!yLCi+^?3_HalFUlPX<}T8xW=d+0~r_ps||H%7I5V`C$l
zUI^fJl&a+!QnTubJR&%4VWeKiJ4qecb!SDThS8Oaxo6^8=`gc*iy55Pj1#4|ELmfr
zdEC(D@aM5cO^@^0wmugO3%%pIIB=u;b-MUainqSif3-uwh;@zwB^L46NOcPI)l`F!
z-Vx-sQ)^o!pbaX|G1~z5YbAwW0}Q`W94okp;-`$%zT9jBEG`qFh1%!=pm4Vv-8Mka
z6pOK9{5)uTcnaWsJ3H~gq-t%=VyXXdTj(8df*T$^q!x62d9bppt~bu8)RTz<fjH@V
zIGJru_?Mdq)!P4<1TzurD2CPX0I3CkRYL^W4}eOJNpJFWL!EC-o5~>IeF=MGo+VOM
z+m6c8ig&_KJNr4h9MEv*)1QV%^Mzotg2CI_u?=YjGC36`xRbWw+J$zb`wwM#D^boQ
zP6)Ghm5xk!t-K1jBN09oG)L?zxP^}zktY_X+5>J4Y;n~D4fe`7NN;nYK9qAARv@3*
zDR1uoMyl<;iFl}VX832L0B(345-|m!A?TQ5afB!O;Wl~yg#w?S@tv7;CydN`E$_^4
z`Q5(Db8jpwe;hmt*xRF{q6QX{-_vtQy#HoV(okFsYdD-(kEWBe<J|^sT~NT%rli<x
zO*+qkai)<B6_qz#TI?48Iy@xL<@B;KNItqosmE2%-L5)VNs=(H0J;)Ge3DD2c{fW0
zGW6=ULnWH2(dW+?+~w&H*YOp)4Bn;QO3tHEwePq_qrQSFD!CFos^WE8^9>J=pLJbN
zIXy)gKPf2DpZ#{cSl(wnU6z#v_%FcvRW5ea;u-q@l;yj8{Phfjz-O3B8G~7Fr4uJT
z0q2s_6lO@c6hPOElyH!AP2dUr(yeSj=7``#C=p&~TP&&{h%x_R0d&UM_nlKil6a-i
zFqlkTnu?vDM>WLpqDPZ~B&++>21uhRdH?gY<FW9cvG7Mqr<EbEb14_1HO5m4i7;OR
zsJn%m-TCn_Xch&qLqEU6%S%2^?U@QQ%~=m;&}6+-8m#2>K8u;g&BEN=!jjqkSua#F
zlNNo%+@d1Tqb9HE3RsfK06u;VG$MH`pTdhNP!5jfe`Ss_h5nT}{tqY7Qog3`PWt(|
z&)l3yi$5kG*d)ixO=Ql3pT%hwv8q{g%bWD5nRZfL{NL!AH{^9$Yh_5<jC$pZyW<Mu
z*GO4Ma+Lo>S+pDaC`HHmN9~W64mBB*)+Ml}CBJjI_6W@<U2ULR-1Te=d<t5|ui0I_
zn^NQ(hYuGuM`dA&O{*Hsr-;bw*xg=-zOg0xdFx~v_KUMi4To<luajXi`j>;&`YK9g
z(pm${_Kzu?-5M=heaBUfuLL8ukC5EeAt50${EoTnXWKRuf>Ry<OwNQsIIZi!TJ`sr
zjL!#pw&|+P8i4fA0F!#&O<MS}UKvWhxaj@(@e3ew!Lbw<w<#S5+z(Gr*KKqf)>gKi
zw`RR;ff}bQKXHVl&UyP1aMfYtBTNq-lxZ0P{&~xB=Imm-NxM8-?Y@1`qu5^9i%yL3
z0yL9!SY(}bEXVnQ>Ob%fbcKTbcdF>gpb6`0!1b=#4PW>>C;PVt&KoWcaSLGtckUGG
zU2uOL+aqMp3+qb2LAu%dFT3Ri*7>ddI6zx6eRD{4eD65QROzbzOlI~=yY_$y@jxPr
z3P@B%3r;+#Z8h5~Oe}6u_*KXN;UeBLKdV(IJHh5MJ0X|f8>!IVDxuo<m9K2B>X!_k
z40YLw3C4eF#xUvm)<fpsU3_bMR~CLhe#F%W3p${|*2{)pu-2dEYf7~PLJayWnVZ<-
zhw`v0s3nQUFnu@2r9`#12mQ@Z{^fQfs|f*pwa<g_bAY+d%A`$63b&o>=ETH{7JoAE
zl@2$gP_bWDRW=0jx$ZnS(VgaX*VJ#i1k@d9LPg#5Zj}vHUHvIYpozC(@4aw=yuiCz
z#up(am#PaYbB$-s+K0PBc}Bje{jx<fv04olZN2gl=KdT^Dz^np^K>C%<VYg@S+I8|
zqPvqlK4PN2sDAIUFv5w{W_F!)w?A!QmMpwJ)<HlmcSOf{>-V0q#bYNwn5azsXRxA)
zFa&?~=_8f~1&G7Ai6uOPdj?ovVH$iw=-*I|ifTPE*-|hsIENRAW+pfdY8oqh2M=xF
z8_^f6i0Rj9&$U5qPM?ltMv^yjjR|nl4=t1HuIL8qe^EQ{gO_rlE9|i2mK38ju>kR`
z*ijbV^S3+@GNVSDEW5`tIiGffAJno<p_b$aQn{&?5IOmS;(S)L?I?zfx<CF?lb#Ab
zkna9ZrwzGs55|#(x`aZH<NO}8p@yDZ0*m(smeE1KvC<2gRq_1tZ}CMP;F8BSUSDhj
zu(9Lm^W675`H470rb0VjWu&uIZZg@O3pPfLC7Ftk*Ll5`?M(;A9W`5<Fh6Rsf&#`P
z9x54bi>29WArdlh(?X>OO*#Lo``v%Y4oujCY6(d14v~gOEGeZR=RsHS<K7C^QRG37
zpF9!4=~nFVr_C08m0Gc2QC4jPJ1?Y9+-5Yp)13}uO<}n%<_JR!UAazjuR+CL+<$KL
zm;3jZDvrfAs4Af4f@0`2JN;F9qT&mc!0wQS8?h+?;FXmI=xdSJubV~6+KiSy*2A;&
zA&0uO?42C0Z=$<*i#X7s;|!1aaq>{izr}i~ESk6O;El-QAT_MT=4^Nz6>1t~BB%Bb
zf0P8#>VG=wz+H*@>nlNE{+)~)2K%pS;Mdlb%}eU6Cu(Jy6%A&c+q0qpUHzqOARm4d
z@=kU$pLlS3XY5AXoyC}ji}P=viNB_JDBuw%#i-SSOrG&W=!$T1@N2HS@1Dn{Ahb-Q
zjL5zTFCg#*^F}<XUd5|KQ&?D7T6(4bU1aLu=Zfm}jC}CGv>cAMpX9GPTwYf&-2{P)
z=I_oZ4o;8TIWo&IMWnk7UPS()m%%KTq7f>UPD>$w`$-d<*|+M(I3>4;@WTYc5z9>z
z8|%yM5XPoU^OamyfDG|SVFj7j<iUOj>d!h}4W4>kJ}^I!ZfOxAxBxlA+RUT}3Qh_=
z;(-5V1!0`{{Z1JIFqm)t4z<SQ`h!~A_J?9f|KtEOju$;U>r*0w-XKppo!JK&M3a}6
z5~+W*CJ!PW=z!Gro#!?42;6ly<t<db3u9xxU{Mf8H@=fKsyj_lPdzWS*_G0l{6Y`F
zkYJ&;SdZeN9nEqEMwzk8`HiXgv|!ml0!YyjTiyq|ulOYaSd}Y9E0~8566ggK^+wOo
zi!M!*?`ZY7G*N&#3Gza*o#4{`Vt(Vr_9d6Yk`BnBrDR7M5#r#avq>quxn*04Hm8`B
zcB40&^-l_5Y3=y_U)Ckc^wL38Z1y8;L{~^<tBIIzw^=N5@q}sI_nEOwochcw(mYEb
z%aV8go;@JuM}kZO5IRp$5F9!ZJ#5Mm0ErwvjySZC;L?$Xvc2P`KDX1J;gbTX?D5c(
zT@~iKhKKIQAKFi`1Ww{v1}zAcR{h(ayc>*@=bkrjg{PPk^_@mA$hgi{cSUTj`s!V>
zQrF&}$rZ@r+=)c`i%iBf9sF0xoWw;uLNZW&r-)4^10k2nx4=uV1A;0Wxbb$ae@q&)
z<I2e@F`7+*r|?s+f<W{YVh*iVkL9a<GQtEHF0fTuPk4-gg09?Cvv1|aU*-Z=d`xEY
zHczy5w}rvpKI}*8q<ZwRbXpH`f3)^E+QaFnS5{9?G)pKQ7WCM@(%{uwb2=DuNdwbE
zt+~8Lmk(z0nB&TuAYb?jolU*TO#qD;f=Ol7d*=q|2jo!c+7BmdB!$H1t8)5OdTDIq
zGDLaaQgkRuV06BqmSD=sVriI{24($17@#jDX-xto+6@mBGot7hO9G!g731l^%QEhR
zMdoWh)s>jfW7*@~0(*`@fa`VrO6P701gIz$ur0n*BL;o?l>X#UD-$NCH|GW?Hq9_q
zT~L!SLv^drbD`8s=Y>Q*lpv`VBpnt12>g?6Gs%x0T^eh$*QAkKPAo>c*7LwI?Lm-I
z`Y)RVIHvs<AHwqnao3AQVpis0qNo<}-~@iU5gjGCdM7f9FyEwF{l`&7-?x(>N;H6G
z@NBa5di)4@HOVqtCJv6nyM2?qNu6{c-v{yyD}_KpBT<<HLr3)W0gV$o&D>42baS%N
z^$g$mXud?pIKou{HUmN$tl~6EHQ<Os4!4ZFX0E&5?<~jyQ>{gK<0S`L)&zf*Dk?NH
zO(rI`f1t&t|AH2ec2OBhFmV>(2|*Z?zJR)L|4r7tP$Fgt+1qO8=2x9R)^8s<FOyr|
z#v+zgJC8N5vTjbkC@Fe%B?KL9z#}4h(><jQ5BtH{1{8wBv;*pbo%64(NRS<#>iV6P
zylOE-0(5}zEf#%N@dl^;DEJA>?&&bv?K?l;oUIBSWeEZOormDgIDlmT)!%uy{2w#q
zUl{G5ltxOpEpNr1>QgUi!XK~@*5i?H@nhRMKG~Izn<KOk@Puaa&bNFbfHBc_SVN`2
zRdmydm8F;jB%^@v-BD%a+sN-Tu2u-h?7CQB>Sj;+GWzGz)9DO?w5lV~{{lIR!!^pI
zrsJjb*DEMFz$*nrw(r8OIK-Wa@uGAyM5s2^?W6ASZJ-ZSfXbA(@-pPaaDX{V>M4rf
zAS-tmYi@+^Is{64pfKfX;nGiz3^WaLqQsRYUV{S7%OczrFldEA{~Ecj5WI5c0i=Nc
zpONj6+hCTax&|dQ+p!h<H@+q2Dz9BRsF`Z92~rWNYxoKv=c+ADAeK=ZveO5e)X^0z
z4nUolGC5uh{q*+%rIK0|iJ609qW5c!PEX)b?aTiS0fkpaD1p%+F6$`S-5noDe2;#?
z91H^3mO-rTm($oGaIOY6f-(`HJ~^2wK2JRa1&jla8*8W;gYlac&hC7pYQ*FOV&NTp
zLk|*GMbp=N_-a<y2x<&g@>fFI;G+dFkiV=muGA)z*(63k#<=PUug+CL9e!y0fbgbC
z@Z%BPRS7b@7i65x{#Tz3C1ozKt3t(5#0=V>$Oy!W!8Mk|7cRr3AqQ@s0;J|-?NUpr
z{b%#i0B;R~=B-hA5OFO!P0>;$9t>OjWC$NKQeTk;FHsDJILk&8;FEXbRmq{m;kEA+
zK>+|)Xo7(cE{-zGA3uO>f_Mo0;L_k3^DU?*nzp7FI1Kz;QvDd}<X>Q%2v!d=_umvI
zb>jh6{?kzG(dC))CR6O1w-z#1EA^Z#ZgwqWL#`oRpHvglfLD8*Wz;E!VC4hDmNRy3
zmYheEG|WF$uzsD7%ZfHkgWESL0HxdJ688|E7?05_NYiS#-Q+67Sx{y>4yABKlBdDh
z?}-3;ra>;l+3>}z!x}lM_@<-fjo7|-sgxs3E}ekcicv`b65F$U;_18dw|yRaZk0`x
zUzM*Nzl|;NidWMTGTp)1A2+wbEh2K9oh{+epK$wO5~su51^;4^e^Xrl^6AI?NgfDg
za}5l>Ja;_kCLxR>Vm%)CCT*b+$ovT1LyVoJ8IPDGl&M|PFGtetr*-=cbemiRDys>m
z6$vu-EZfoBW0}*pDOfpQ?I^Gb{VaV6L<o4e>>Snx&!6LP0;Pa}(c&xjP7aY@U%7Me
z&sSzb^C_f`o3XtQ4yI=^X)B=#(zu|4gcTC5j!o!Y*ehkofJa?w&S|sHS1I6?q<Zx*
z{~0Lnm==L6WDC!*5E>4-02?R<pT{kL&wJ8WFJNl>QIe9JRSYiRQ9jlI;{e`#A0RJ(
zg%y1HM4NXFYe#=Q@Y3a~v$0zmG1fd=&bsrq^l}K6C=EHYxY!`hc<+OVShpm9zMwTp
zVGbo{k_O$vO#hh@Zpa0xsOMb|CVOohs@z%E{VWV+l5DX-9Pr>iQ^VzEVv)L~Rk#$_
zk-tPcDhjf^&hA>_-Qw|=mx4~?sh`r*Voh~YTz#%<-fQ^xSHdJt1?E~Zyd2u>6HN3A
zwr3;c>J-7$7_+<E6x_HHn{-Q)HW%@w8$qu+Z(Zg^A@OKO#6GC4B)5F`z2QT7|3^&R
zW&^7|Y?>prjYPD$a925#BuN3r5;k(t7y2u%rX~8D^-J{ezAC%QMcQb$yV)W4eSZS_
zi}$dLN%?Ese@jr%a4|x`LPPp$Z8d~G^l|?K6-DYBR7F5c+Nd`S5^yw9CT}ppo^`m1
z;vlVO@>Fr6Zc*jIG~0fFwToFU_{uNDX)qZ?58t}?68m(q!tUX+ak#b}mKd@#+Db^?
z`lTU}kDdWoJE9=}=_8ZEz#u(p|K7kLtU#ATT7$;h;|10}fNlq>4mh#FE9vPGrP34g
zOccZc7Ve_>7Dj=>;6enDQm^>v$glWGstl)JUaRj)rHC}kszX}v*ZdZKO2>n1AUp%R
z^fy2(VEZ*k3b{Ljb)Kk}lMS`cslU7U7I8Ke%KFcQ^jOGWqVM<r0)0O(ILe5%ZX+Q6
zBUX@eHH`GcKHgbLNmjkXIxa=Gs~kOZ^6ts@n+=DX;VR{zfT&SnkOPx*YoseR;FP=%
z$VKPsP+flP_`#=dB-)oN4hHtxI^Is}NnBKr<wD<ge!q@?@gwiEh(Kt}h-4fH65DVF
z@{#`mFPJjK=0m-56E*plOh&p1@BWN;Dwo8>EP9Z4esnqorg>cE7a-&C<F~oy;Ip#`
zJ`6|$>Y3)jQP08qxO}K#Feen^agT)Ob5wE>z<9sN2lweRqlrFO*g3G$ZyS$m71WBH
zJ||wg(l{HMZ%JJ5y{4UODBKLNT@4j=D2gj_AE%Y8;EHVz=lsNDwOZgD`Rd-0jn!A!
z2EaKW9iSuNtj&P4EtLn=dqW=khZlQIGa&r<?(F>NV!F8>?UbdA)=8N8f@2m%g&jY^
zq0#_eyY0sMo822RJ_WhS9rH%h*v3~w%`}zl<<L+JOsv{kruBCJsiw<=O1T5Q@*^3#
zE1Atg8}hE&QZ|!UuL567z6-{?iDpL2HoX7yeRv|j{sF|hV_<ctTDRldjwi@wZA@Qu
z{?Ojx;xMVw>;1NKxsxX`z)%jfDOMBF^yJgS(IW@D&$~K4rO$OjQp)w~Q!l!6hN@Gs
zgN`-dWeW;#L|iX+j5sY(8zv=-1a_%AJcc~o<t(`SpKoQNjo+3s(TbNeiC4KiWFcnM
zOZW*50kJF28!#(?ZUw&fRiB-OtPos2ls5(7A>E^|v&UnzBQt^o0+-EhNc~bgU1Za_
z19K4~T`c#kYrV|c*s(9$DT1$j#9H>MJxskyZH8m`gRNI<%O(qQy|6(JFK8+7A6IUq
zk<iQ+Og*YbBx2$Pgz4E5O4Hc3YRef}91vHuOA@+Ef4a-4Y)Z6&7gP>-{zV13luhpv
z1>IKwJuilGu_q_YZ1p-!2B=*g;SR9%hw$G2Y);IjJM8*g;e)ZxvyT21AfdqBjRwLW
ztfc2HUcSsU6v=F)cg%@}XxxuA%B;oRCqCS&N>^YPq?EiqGkD%Q4uS>qtl->bpo5sv
z01U#hfWdoq0}$1KpOs1jy~~cw&iz8%vd)IRnx!)J%F%1RclXtC?9RsoRGWSlOWB8(
z;)qnL>{->;Kx_I4HI>_K!pD%M$ID51ctx?}+vqretB}uNYSa92ItO#*3U{tBN%$S-
z3)BH#k*#vh{~);6tOj8ul<*0G(C?!~<!`hF{}0@1^YEb(t{9s*xFafkao?piaFw))
zVn$*o=+E<$2Qu|;3)i~zYF#x*m%uq8uf-B@jzAN&{WdrEL1D@&6ZTFLStR|^fMrTy
zG{34n{E~=$pr8-5$r}f?7TdIG`A?ecxMII9_P9^>0E89gU&^5#X@-GYq~#4GeBvbZ
zNfRz86PClx4rZJyL<q3P{X_SW24L6k!ZoD0<XOQ9E8ZG_TX2`epuho%K=G0h?2vH!
zWRXG2n`ltqIZs_?HqK7om*&#1kUVchpRZ(s?MM-F=G5h5mal30<wHem$#+D@_tvjk
zvZnQb+H5@t5Wd&9bdvvZc+1g-)%C)<Q5=JtbfKhxP<Yg@D~naF=Y3Q;2n{LSnBhJS
znq6)^nPiXUAoxgB4|yh&nljC*)w70G)7d?g6x!@_`8Y-~xAR)Bk*{pFAPoqO&^Xp_
z7ubG@=SD9_d}&TZM1(F6e7ufX0CW8iiDb}A&^v0<Q+q6IiM9>IZr5rv<?%ccrtg50
z4a8OZBKs<cs4rRK+qf=*QLW`JBJgWbtFd1G%-+;zshzWToCyS^-!l9FIh&zihA{at
zdS{8n%*TAGx7)n1>({f0f@>1XxQ`eY9F8?Z*ezEVQ0PJN%YA^XfCPY9>K)5xcoT_$
zPv!=h<}zi7#$t&OskB5lsUG!GNiwECS(5pUMC!38GPl!?m$9XmjZsjLfwFeNP_v6q
zQ2(TE@(|xfGW<#RwpqN@fI2XpF%TK2dc{9vUGn~tgbi?TFtzLCr2FX+yCXRO+-Cp`
zjpB-f)Bg<M?E{1WKg7jBk8eCK7s(HFxsv(f9?9;v!#0L2oZV4Z;U)!&`?qIjLBS%f
z;>voJo@=SGhTeX|RA0SE8WKUx`>+^D5q%V=Zvo&m7g~R@=)Vi_<GCR?OkSMeO$*0=
zD;AJPWRc!{&Qiay{A8by#Dbgg$*<~m$#+>d+nMj!l*-9LpiIqmb(B@HM!m>hvhe6>
zcZo}9)p95pURdJ$gFQB9u#LrVpT|6CeHU3=e@F$y+#O~HF-vf&ygSEBNNgF{EzgC4
zdRRKr&6}Ixx8qyIia_vhzOyx(=tC(COx(d^Mb|N{dmP5T8GaTe<SlDgCApLtd$@^>
zfi5aoY90rwKohkl7m^T}Ze7j2uayJiFN0TdKZ0!($LQfq1pX^gWu)k9)Op6!zU3A8
zec*F~da}CgCp<8ubQ9m<r9aX6sIPq}O1KUuc%?w62WC}-rlp#(iv3IQqjAuu-<baR
zeu<-qU%7gCcL*<X1j94`6}fZxR!5gIbSUTE%rBX*2^9J(=OjbrP+ydQJ{umrF}{e|
zb8-ns6&-*Tg9eEz_t4=)ZQ%XuQ5U>8Bj&{hlgS3WHzF+>nkhZ_L(`Dk`JffLpG>?E
zX=ZRjaeSZzt%>qV)^?q3=lD^;$_efbqhJS0rJkMWP*AlUC{F}O4zyyF=nNRi=4*b7
z==?#Nf2gm&)7Eet>H_c-5Z@xBAS-_{{YG$mgy~&3)KU-Q?dL8=kk9fqZ=002CW5ak
zK{+t3HiK_zAOL?GacyhxAK|H5aeECr<FME9f$!K2JiUawnH!smyGQ!U^|7bx8NFpA
zD+|rx<xk)VG1#SYKOuA!2JWqhDUn~TWcy(6$itJlJzDFo<Ux8(Z@-iHr;Um;&$5*#
zqpKtA^`&y5LAAOfMcvmJ#f=*58W1i0{hVlm&k4o)FQ{L19vV1IG|ulwK9P87iT?4_
zr48ksMahkE2u%+m5J!Uh=ZS@$u*5?=42hA@Q{`Z#8!hcZ=CdaQd(M~KZbJhne0?LJ
z6y|k?-GZl_6$b_ODNqEf>!tSUtP^tq$PPZ`+zRb3#zUgwejr_Edm~QRTN?p$Rj6s5
zq57hVW#;(LmgBa=hj4xJy^jwBX-JXRQA30A;Ah76OBOX;cY^Qf&j7iZ40N^Ks2z7+
z1YZY{;G_hzrZpDgnGK^drJ9cvq4Lf6-Y!481O57$F!+8LIMBL@xO6LjN5tGs!v|a-
zVS)Y4)fNpn9vDL}%Fq;&*o)v+4nCBB8WB4X41(D1vILg*0DcC8^kSuJ{6XcZt1pqY
zTf>*oaYPftDaR3nj#}G1(hxmuf230ov4bm%@`&t#Y0fQ^9`Sp>cJ4s7o~8;G?a0}o
zXVto~3$GnX_Xuo$pRUS)Jp8MR+h6eD`wsA=>mxxL{~sR!_?XRtyFgEvI5YSt1<@ov
zaEj|j#}@k+^2~hAXEOTNIZy_V2s;h?&)*$Tzshfsvr&_{4Mh<}3cgz$E_Ok!)@Zxj
zlhEB)E&s4`44f#K$)M)Sg9?nN$+<~Am?U_dS39GvO>Tzbr$}77Cvr}|ljubMeXoR*
z8govQXRJdXqMdr>nZ9DpR>;m-BA9#2;+9E6LVEJML5f-B+lmr|RA6}^$CH0{Ctzmf
zR)*70lg~aifTMx{djxJbl48?FbGn<1%P4<XfNRch>ZUwxZUGb++gLR6jXGW_P1x7v
zp!~1`x#R12S(f&1;^qG~hFuH;Px-Vr^=uE@jy;aL1Bi>W$^qM<lCt$cF!yZaIpAHW
zMl6pP?LJCX^#XTR^hOT)NO02!r*d*tj2460c*1~IhKe~;(_+zR<x%e&3`};l=__-)
zUr&gH`2WWfCRG(W{ph^n^PG8zR5LY@0(=G(pwR^nF@nOv11TP9XGRfQS7MNoQb1gd
zI{Ixbmv8}7MEUk?E)ogsT`xR)>obbTiNl>azy6LG-u;cl_?s?*`Af?ueV-CDn>l)Z
zJK?Pa(EeM=sm6RUX@N#6tl(ts3Ph+zC6*EqBdY8?UMTzg?_uxcDTp>$v&@;mk?X9;
z@N+m|+V=MihxdOy9wd1%%%Gq&okqd!POi4TJjsX)Z9rM8a+cRfg$KL10Q;RlLHae%
zNsz)N`ZZDD`Fnr~aD-;n1j`x*nvNiVzSa3>H+$FIAH*S;2R~NTxoJYc?HKZhJ)h=w
zp9}jpEv61c2}09_y9iPgAR{-356f&wGJBg0Nzy#q!_T>NI6U=u(PA*Y(KNpX+<=tT
zflm(5tB`I*U8#afB|Kcbh4SBF;EQ9>e<ctC9!wn`Bz|>4>a|ySuzb@fSlC6a2U_T2
z*0XxWWWIUpDA&AAH0Gd#>j{HDt*-j--3EW2D_>J_Ks5FR8}r`tA=&mHe!alriNAy5
zzgZmiw+9YHKqpG)KCQHxg!uaP>x^Y+YhSXtsqLeO60Kor4L^szF_p{1z+`XN7k#kl
zO0L3wcYpCEt2(2<dLC3nq}hiO>D5uw=g+`aCw(RxSwvWRx{>~)1KbO!c^vuwE|;9<
zC-!iz7nD4@IIOl=5F8mR*<M@QT3gerE+#h##)G>uQPU50@i+u$B^El&Rhh^Uoq&e&
zwkREUsN8DIp>lE)=lc5U<g;>i9hn6HTL!U+5!U)aAj1Nq{6=b7J_bbPr9Jj-#X%bR
z`Z7tAZ$jWfk8+_@RPQ1q2}4@fes}=`zMk#*j>uiYkP+db$jG>e2!)m-glYo#$Z>c5
zvk`YXK*<4Y(bd~4=(6YQ<AYEL1D)G0cjmbiuWIXNN%%N5C)D$RTTvc`8Mx3Z(RoJ0
zIWpy25e%Yg*Wv}R%Mg}@o1y`TfbJ24Wvy4<(vn@IKvCDPU0Q2K=kylVZDubM6A+Km
z>P|-50q}=u$H!c<7a2Txd@g*51*sB=ql>51$1w^3dk$e6#KXZ6mgLUV#hWw7wSD57
za?=*tuC_?55aRu~7U|m2a}O>^2`d9S5_0QOjqGPB!K&rIO~^Q2{KnsSUEss5Bs2(+
zicHyax_~sd8DC|T2!_w{c?A(3qyj#nD&;`G7R>y$7L@Lpo@C^gkR?vfJkSn#3I%tp
z$h2=kLxyc~_{K^#_c*v62M0gb3VE@4bS-qe_{Uq^1(6&uy9!c_QM;~+d!G$P@sMNO
zJGO5!+{*oh(kpV*)-V3Q-o&2{Km-6X<>C}P*(lepwlp)dbXNiqGkpBbaHN44CYo(G
z>E5*~8L@CS+H999ur2^4Db4zm+w8J7+<39cVJy+v@ht8?4D8m&Uj*fGM;lP{Kl0~q
zY?p>8D^W3EeF@O{y`6WL1)Ia+`lBpb2ma&SYq;L_DV!|NY~f-?<wi6gLQ5FJc~ilY
zJbz2BB{>lwc>m=vX6aW^0w3H=in@lUhc>;*2IKA>NC-f%nIM}Ee8GT6P7Db%Uro-`
zfR%ogXs5SY<5MnB^aEI|8RY&?<qwy_fZXD4GH)qvm%sF4oIP`B-NOWr`#$sCVRImf
zd1N2zD}qZQKfaTP{L^-~of^`-!CZOEzZb{@i$5zmes6x*LG#wzTym>gH+w#(L`GZ%
z7%>urY9-$=XO5YwzS+CQKD_e!H!;P&O8T`wl<*pq+V^FLKe-k);=fW-vbc@$z&m!$
zpArpB>THu9dvjhwvP-J0=f<|u36y9cY;&0u>YLq6M@w{!Re;j;!9ZcTQ%TE=AhiOv
zL-i;BP+sUcilM^p#M9S9&jE4~z_<nuasDS3;%ol+N4o`i5Gw<;Y1Y$i6QE2MW25MB
zkGelt6Gb2Ys}JSH)?`@7@Cotn_F=(0v5N#qK;!8jSx7_Tqr}y}A6jOkhwt*(ou}!?
zD-eO7bjACMq1^U~n6X9j&|v2*82lg~8w?8nyw6|h$buRDu+2XtMsV->w|DaDzqk2B
zXD-!pfVw|h7RcG3N#PUH`Dkm`1Jlw>G;0rEwyM$r7Q>)jVD$_ybtUK}+%)7OpI18!
z6adAV%5E2@M;`<EEeJ_Tt3BM-P0~{809On6(6lJFTS@P}0PEZJbuJAiwJFwQnlhCp
zID=H7aR&e|A8zCh(A$opef3}$-V_vLCigBUe5zu3eWpojLWTt60qGSQ7}#%Z(+zSd
zhEwkE3E>uSIf#_XZ%Xc~Q+1H5!(A)jA2NUZm}dl8JA7%Yp1*h_@d<(a`0Q~8C0bq<
zDPhA&A8|+$g0Bz5AR*FCDhV2p-qpNTnLD~|)y@14!0A$g&8v*NMq5J)u<voh`TpTy
zj=aI)*=?OtgUTp#LNbAKV3pc*b$MvxbrxPxtP~FZ0^t6VxX!Ar+^iSca)8^bD`=A(
z84VN?fSENe)v#-~4ewC8P*VGc8ld%G)c}cQa1Y}$r0z~6pgsli4zO_}WJA96t%0J=
z66;8JatF9B-RmVc^8@=D%`3$WkZy3#8f&BJ;Ln9N`-*~Z4S0e+b6!<|y^UUGQKAjS
zal$FUmE(tx9|ux{tQE-6(~NY(5|zM_qI=ar3Uo=TzXE5xj0eD1Lc45{HKY}mn7HXU
z!^ps(UuxiFvrP|}$1y!O0Xn+FgH`MSmx`b3X;VNDlBaAUc>ZB*{^QK(As*gaAQwpK
zbpz(eA6?45hI5hb7XxZNKWEu6VByrEZxa{^FSKR3?=)+wtRFR9S%WzRKYZftR}{u7
zEoP4nJ(Uy`ViLHs0l;dwU=!g`*RTP8U3)m$?AXx{AU-)+N}5$yd$hHFR#sEvw?Xt3
z^gSq06WHyfPt$K$`!pS$ugeX*Ta&o$_D=SJ3e9qB3L$5oyzz?@Ga)0$&YL>AbpB^Q
zzg*~`hCNq6nucS1jN8xG<!<L3BPboP$7bw%_8=<l>o@={Q)5aG#sO~<RrMBVr$d4@
zch;&Ms**ovRp5aj8*W9VD0}!Nn4JX`8!PL_y)&RV&zRfviq5GcrC|*oyrWuvdWs1K
zH2EIriJKGmNH;E;eIFpvA%x7dZyh%$tFPL^v;lWM*-+QAS|6X9YF4T-IjMhnFvOyy
zL^oZt-vVSc5#}$!Qj?INwX$O2O*v9h9(uX)YG(dp>5G;&;4<A`o~!{U#E<y+W|O%}
zOed?BXS}YAthaQ)k}lMA$rJ?h@$9tm9C&$xGZcIT9|)WKD8`TOG=2BwUTpBMUMz7X
z0SOHF71Z?aK_Lc+LJa0*CD=fL{!XygqZgCueo<$fpT<4>Nd#_~how((Aoc>ij$X=d
zS*zJw)*_%$ey>Jtx#7)DTHLgU9^vKJ(r_DCf@Bp_H|0z>5OR#Dxnkc{g45|7E_Qtn
z*f`Aqi3T9kh$LKPbg1A_tlJ}F^9)}oF#S->kSe}fXa~2hR=G54!(~0SS303Q_{(}+
za``>KZw6w3({PO0_}@8n12D9EaEKku<o%@{&{%ULRiFg;=!2}vbgdI=8iW+`a2_ZF
zauLBd1ZXzE_t(gk`e8}h9pO|Xg+-gKZ-p*p?}z2-Jj?@5n997y{_a2s7TXcS4B*!Y
z+UL*c;J$@6R~8Z1ZQ-P45+8q^|6`(p`0pkvIuI81o(M~maC5rP_`stGFi#J8v}3lH
zXix9w0*Sww-pk*r9^Iei&z7@I%D^?{KIT6!>d7)=3o|nb9i8v9$a%!<(4R-MJnkRe
zfPFPqyHlR|;f&k1Hq&TkrqKbniKi#kXg$z50N9+Jj_xJsZPLNSY?Xhlzsb#6UN}6A
zXC}0em9_M0!Y_c@*#&uB5sDc9FgS;JUwMc+&?sd;m0^6Ac4>Qltm57%s|#7*ybzb5
z!zTa&h-;!hJNHpC1mEv{oD4W4nT3hQZ(Msk->i(e6>@K`+b6$J{<o1K9qBV`W5D29
zzp$@kvwcbW&I0vU(nqft?yv<MP6Xb==C{qvp?RGjfMo}H+C5e$_s)U4sULUL46L)W
zVZ348C&zic&s^G)f7MltKj(tSXn;x+Xq7FNg56qKiH>R<Ee{I(tgEW-0ynN!dm^=-
zi3vS85RQEN;a#TOU#|7lw)51MK>P)i9$?^FzkFd$;^LA?>)%*AwK88D1}@(-o@crt
zt-*~b{exu<rMp}}`nI`xlA!XngJX+ma2WmeCSb_H=o>?!URM`Zbq%r)og#Uj0*S$6
ze4+d&qo1!OVgK<Efb$vut}Lck@5wTU_T~`9ruKK67KwaL)R|3O1}VfGu6yskVelgO
zWBY*dim9;7TuE~T6@cvK>jmRa{1ks!wH#pWV9RI(XW4wq^j#J`k$g?Vg*Ha@Mk`a(
zFYwVgJ#BP9`7!n(!V$c9gWHiIdm$_<Q}%SD+=0t_Y68RnEp`h*^rPR8U~;L+mp{ki
zf%<5>$*U|qy~Ciit*vbnh_!&`t9P~Dd8vDxL77cDD^zwNd_Ed%Oajhlhtrpy^t==P
zd~`_>sy!c!{PT0+Wc(9EC4m5dQj;MMDb01?VOhPW5Y6!|>-Dwy_8&d!9qBud07-@Z
zVaoPq^0aqO<wN1or%!!TuG7)f5b+4*wTG|NdO&3pFvFR)7|ZF(znnPZIfh9Nv-Ma3
z?a2Q~a@s#y>39AJ4R|O=k3?x0EiLsHmCWMYjpw#dgU3OXPN9})LhqhH%bo#03o^2j
z;Kl?VyYGt`Qg3DWfY9uz(O0<8tOM^cJwA6*3vn;(rU{6sM{$G-P=7<3F@JF_2m2vN
z(1x35AM{8a)I4!Gr3{dgw&51t9f7V$lc~W8w9PKHj;eLf?7{BZZOxE?PoF-q);R3L
z(vq|qltKiQNq;}N%PzqP6Qji`sf{BqXwayC<#~3fmWS{|AV0Cf<|z+In}}yy!adBH
zZaRR@QanI?jo6!w`?!e8Qw{sirTs6mB~K^NefTTw%mz^F7S|Mem6hPnB&WP(6cBUm
z#P~qlh!S5YFqk}z$=R{|HZ&^{(r<;RihzjNb>v)qzAkJ$rieJQs`dgk(OM|)RR+i<
ztVWgOLZ5!d??oMvHIBQ;hIcgZEl2sMPC?zVII_P`6&$}w&@74vFvGu^Md9W~Re?0J
zX4FD~mvipn7AnK-WhG0+(b!<PjcX^Ut`r%ILLGyx+D7*)NS@{uNaqy*g@l0MYC9a0
zTtE*_y5fKAbtIy7fN}O4Sbc$&Pdqw}aUZyl4sW6M{@X~^L=nk~J)8uWBPK$^#NU%I
z)RP13PXB838eIUkov<+N!8&KFtIlBwWP4S!C7{m+ap^ffP0u068A&2k+wIWn;gKGh
zLD-PalK~trx0@;N+<-HD!_$F@285|NH|wc<T@RP%gZ64^eEl=*FC!|CnmJ_Ph_E$i
ziJ-d}r3L(O@wAwN!R75gxnBcg&8No~UvCMO9v8ew@&uijgt6ckc}0-jkxqzZ&87@6
zaCll=4}vBYrC%;)I9yKQO3RhYh7%x~IdD;U*&NzeX|_%7)RoVIN+;MDTdUoGH7+tb
ziI2ar3g`{qLb!W4Wo&RgJ8EjbN}_QS132e)<_f{&pF=cvP(Xz9nojOfzuY9K(5nSJ
zvh4web&uey3y5D8vRIg57(k{cmebAeHp=9w<+){38+_z$NXCx*0vjC*3muD-6FGrX
zC|`4CrZ24Yy++sK^XzX9h{bStgcq2XBO@WbdGiK~jAS+CDe*WX5V87Wg<;&fg?YT1
z_H{!zGVlhN^QIe6Jp_{>(?5>{gwqM-X$8>zq%~kf3D8vko5Z<5N`BK6CI5yAV*i2K
z%?{U~yl;kNQbvYsMw(_AGG;5&C-2E&FNv?60s~;>07`JXST7%IvL@q$Ja`o`Blvs*
zgeoBW1UT3Y?gbX?mCgGSXJFuh-|{i=ax4Xt85v%@(CPxVCcr8I0RfnTM*>su<97k%
zr>NVP@o}6zz%qOC0GNUw?6hFF$n;=LiGVu+%5i@_!nGm@RiRE?ycOvGn{!)lU6+f`
zbs&s_AEyD_I6I`bwbHw7)*gU`_tkP^n8VXRu`8vs7X>s_2oXRu3jKV6G-@YaEbGry
z0DCdrUJhRl0N?>N0A!KCq^P>n<0c``Jt6e!6xoy}Eu@|wwB*u&)~w0kd)M6-&Yrzo
zrXGsnA1VvV@3`6o)o`o}GcOq;23IhWA8vWEoAmxvLB-?<yI=0@eIqvSH8sP{myAUf
z%&89E<!0soXaZ)^gW+)KFk%Uk2%&WRkJ?~~Q2-vzD*!Hwii$(v7WXmkHPX}}N;+{a
zLNccCkTx>qI&HQAaIQzcr}KNyK@(Mak#X^3R-Npl=GcC9+4QonSj&f=l^V^VG`+|}
z4Mp*DwB#pr(#DUoir<5rM33x?yWPi`<m0I9Lt>J21QgDM6wX-0iAK=6!K!Z}aaBKd
z?*UVU54MLx8E^D1e))h4waB^gce@uOf^R^cO!m)SAApi}SOPf~gEu1_j(VR!H-&Xe
zNs}S4GY(i}0_U}rSFk92HVSLED3m%5kjx@<L+K#$JG+$z<Qu%Jmpc&Gz^A>E?Qp*;
zA2C$=<wyCiekPrhQ6T$aw2_yQ6eRsJHs<s|=)I4h8R~bPw|dzu%PIA;rGf)&!H1sY
z(UBHf8D*HIzD$$JChQGTkbNf9&7!*m%>CA~V}rhOLu7$RZVT|$=y>CwRwI1sv*$vC
zSAex8_=(W-@69$-8Jeq7H+uR{))K5S1^1;BF%tO${MuhPA><7|vFzk!74-V)BX(6W
z*CW&}pyfY7t^bmfi4|h~bf!=)379c!UA4>mhu>qEq}Yq>1Xe??TG3=Cnwe!}hB3s^
zk=ln_u@dcP<YznUr4Hysm3vp^M>8)Jb*#>hw$qMEjy#H&J3q+S+?LUG)#m-^{dq9t
zAl%Wc3_<CRT5!<&ZwiJ%m;|>KKQL<G5y%K<H0wR4qsvBH4O$#OswPgeOV;%tpH3Wa
z70_{`HF;Lk=OO5T7*LXf!zx2&MBOl?g9?1L5#*&iVk&QG{*3(w(G*K~^m2GDz#C5=
zeNL9t-5W+e?9DC!da%%Of%u~{6e}aU*@vWzGi-Jwx!#Qj{LfMdnh|-oqxB8X`oW#U
z8%B6kH1t+4EC(?(^S><2>b*$%96%|(nxE(uuJD)=4<oQ!_Z(ND<Aq7F;)&1Af%E+m
z`>lIPZa+#JSBLY7liGXgorNbmym@<-BB^_aA|CH!%M0yEOn%I@(pgMWmlb;(?Ic3B
z*y*Zb0G!K>{7Sy;C>V^DDZ#dW-ZXcep`4S&HmQsvG8KDRNrN+}RH_h}Cz#url5=w;
zw89k3OU}1R!)n;u>u=sZ3t8P3e?&cpKKP&^(7U2p7FBF*U!dem^-aoXG<^vbb~K;+
z0wOBxp6ca3sC*Bly9FPVo_}L`0iiHkC85l|_kF#7UtQ}|yse1b2T~cD6cEB90J~_m
zJNl?(B|wu&mn<CDtNE(#j?TpR&bqN)&%LL<#9_l7y@Rw2!;dUc+8Ec!d(K-HUg~$Q
z3F;gjU>|iWt?|MJ*|H*XHd2<*7kk@mliQmxZ{)noCtzUZkNH-a-+-KdaWglM4r3BE
zd37G-*MJilLWNVNppZbv^(&s^w0@y}pFOW5eq)$}M-S1z{2rljG{dbM9m>?ydN-L1
z)3pb%H3#v<nZ)o+u~%g+)<{-}V9#9_=~U2B#K+ItJ<!Lv_t^Ot81Wui%k#OYJ*Iqm
zWvfvpNR3cdgGD=b1i5r!&TaHo>NU+|UB8P??{8wG8A^=o^cH$KkDDaqN7<3#_~iNe
zqknr7)O=#Bdp;881c^wA3M!cC*=S5{Omm?(MW<<m<;b)vj6V^}`3_c`rqem60)%d_
zCF(g7p5oA4)5SObVtaM3h`vCK5G0=Hf!NcD8xcWGZj9lA=Vs=&YdqyYRC6&t_(nK=
z54_&~Y5>#ftuj{SRUm%GhzhsfAjAm7Q%@GLSCXprRj<slB0{$k2r;6*)%}2*y#bSF
zDaKy%hA2+VhXz${*5vvUvfY5SHy)y_OmWR)2oHAY<epL~hbgL^!9R!V|M(@8b~R$^
zN7=KR@CA9niV4+O&s#EweNXQ6I@(T_fV>Vx(^I?WA(Wa((Z@v9N`4uG&OawF<0dTQ
z_9?sA_IPNF0^6%N)aWzC{pF33snVcPic8B|=v5+8Z!*=8;|H%l<w5g`oat3;Le<{)
zz7E`4$t5bm%y37yVIxuv-ZRxB4HSd7L{k2~!#S?e$grfKa;w80YilfzoIRR+VkD8*
zMRhSBr$R5EOEww6hOQ^w|M)GVYBge1hS2oZpIlO^exi0tR()=p|NV1?cc`h}hdDn}
zy}&uuhdUAZE>F<)JT7)WJU|(<F~b%P{X|VNh&-6LSYU6<!GOQ>O#f-iohRn1w#cVc
zGOj+0=_3@OR}X(C@D6Uy>fIE<IrW$OBK9L6dZ3^0t=2R51B!+Y!#_Y}??DuImfEt!
zup*8ImqC=kdvC!kv)`dAy(KJ1EqhPdn;k2eA>^~AMr0LT2Yt7{-;lV6f(3Dz+|+T0
zRA09qUI<nfdU3kVDz#?5boi;Q+8aCus}WR4%{~VxGU9pK5Wf>5%&8y6pIJQ#*A*8Q
zTCUbBcjfQDK^&R3bJyib@qW~BeT3ih4@-V%dRYkAd9b@#+qTN;A1V4Urjg&vzGKhR
zL-bp{?HO|?7H2jBzd4grC8c{f(%Ms<edDd$49QVF#B?@dKAoKb6B;`4;0e*@tyQxM
za}<&O4S_MAHT8&@+qk|lqIBPHq&Rh?gw4OgQenTxjTAEcknI-RyIc~0%V~saej-K`
z&u-sl6!R*=k`kO%_y5kJPB9FfvBO<Yqo&BolzhUp)VO<j`&4wIr0#BEG?VImDGzQ&
z(V&>BHDB123n1U5Z#7ki=00O;oHU26*?O-3Y|_R)j=*gWi3tmfHq+~XKI!IVA)w!E
zXP+D<2<OggXNB*f<O8?dJ62#uS*$RuiE6WVuvNS6{(R!D89tYVly6lnH<7o`jA`>W
z2mY`C%#MM<A!HG3i5W5M{-pkoHCe3L_=rZBtVfpOD6V=Zh6-$~SvB#4_Za+SBm3-=
zXq`Fz7f=*>Z0B&8lnOfTcf5ITegh)$oE3W01OtbM+_|;JxwW<ha}&d9F3ZGHmzhs2
z<%SGRRWV_jA;#4Z^bK>DbkeOcN48N1u95f*z7ZRPC3Zf?A68F<eljH|@Y)nZ0w_BK
zn(#AVX2dXJTYe4>B54N!b~g`3^+mg{NNB4JxRI(N9tzQX@`(Awnz+Yp;cM;Gr_>0?
zF&v)i4zgbK=da&BQ@*eeP)*xUN>J#$^WKHZ{La$-5~bJeQg!S>OncPEh8a&qqh=Wx
z!#(xA_{*j+iez7e4pMXy$p!Q*P_I`#Uc+uvC2;wY3w34cI^SYpJiF2!J}Q4M9OJfR
z+gx-JRjl+ikx_a9eP*Mx6(`Qof9l@bL0a-G8JwgSax$>S6Poo;3}TE}QBUN3lsXPD
zUcX|2ujW&b+xW|km%5z>biR98&K<wXB`9S1IKa7_($<25t2y$*$px(tcA-K*Xld0w
z-2aVid3Tflaz8kNtaJ;8fyu+G`hzYiKM|`7*7SX}JMQ*7MjJlb+qlaE`NU!}V<qNZ
z(r1{S?b5<S-o<%gD88$T6|7*aFN0a8ZjYKh*1H`p?T~lt|2mt`O_Li-1J#B;`fyww
zqHD-SyW+kkUoM(9S#(JL`u*Ss|40W~tRV-BkqBWKGcHf!ZOs<Q`07t-<j%0LkjXMU
zr43w7%&nLRuXH@5Ames{(t6=idsbawPI#IDD~aq`_>3p?eI}GAB!lE=*P=>I_q=>*
zvg@{2-FMZ5z?jdso_ZNP|Hz;YVXr7l9jTRIa7Iu-C*Bv0y_5zUiwQT7*S;3}AYDWq
zO^F|ze{=ZG>(&J1WLDwDSK<Xw+x?O*F|~l2PX-!3Eu<qG%U;0xk1Hf6t6A-I!cw<L
ziniQ-YDKSyxt++^+-LlqnbCHM@pE%hMHCU7N%p8&Rv!(q0w_~_B3P(vWt(&6BqSIz
zdgdKv>l2cMS}57g6z<SYw|$%7K;%v&G_xmkcYfqzo7*p&2?I=&a<)?gL@^0Ua)$dK
z7ujQ>cBl4|NGy0<oYW-5?_pHDFlyM?fgcmI(MoSfxP$vi{*R35yobfG@$8-X=$}+*
zpL2yAX=p~%GR?EIEYh<~fCI^TT!hf}?sBYtYBl!qi%><9Zd3n*Srl(g3lx!vvMr?a
zJ8VmS4=*C8#)*1u0sDy<@H|pWf=-0)jw)g<140PvZEi#Fj@<{U5E<L&E!jYKkX3#!
zGNa$K2uq)E<AGF8{*<_VQDPj`WW#e0@S5Pg{hb9Rlcfrv7;<FQ$ygu5oh9P2y1p{x
z-Qu~&Y;d8Rvq+ao^$g(+2i86Nkl{aDGiiW<#TtId2f|NFc2>&AI}~B(#n8Ml*D@EK
z!4VE)J4p@}@vx|=Z)g@89VEq_gf4lPZIZ?!bXSu~#DrZ)_24|6cZ<!>hP?<cmSPkf
z5`~n@&9`(tr=87z)J0u><=z~tP37c9g~nF)lC8qgyP%V#8+;qgwj_Ms`)8ud>BNI2
zFyyMOGWiFRXNV#SUzX&1&S#Sg0~oSa6xBNJ)QG&*v^O#27DODuK*egNt_+s-k$sSl
zqM(dKXh>+=b|Z2@Q3>LvhZ{9_tOZ$c2yI^q&>}U>p*6l#A|SW4;t#NJDW}BQTDi~+
z?`6{cv+xYvJ<@jyt0IxRcud(wy8GLP=<1PtC^1nSM$goF&Jnu2E7F(b=OTG&9}Tk8
z-)HfO$Y$pJ$DmQX2Sdia5m9M&Y);}{L?2F;Ont)rdufT^)~tYJE%~WLfKf}5FsfXw
zD^(Qi;I@$~3f7tW7G`zcRlV0*Fsf)0@Pe`*n>YjZE|$%aWFcpb|Fe*4rd~{*)u-U`
zCWfsHYtOF4W9;o%iZd&<@rOs_S=ooFW%e(_x|39Em6VT)@NMaRy<M6(w)<m>VNM(2
zvx8iTEq8u&p(j#vt+2{~Ro4#g8}py2G&0p9pG4e}7`b5{{K=pjp_ho#p8sWQV-4mF
zp0N!{&$$}erbIQ#3E8$DpHKP)EncU~RdUBV<&F2Zw;zfL^<mem^AG9B%Y;8utqDqW
zzPH4AgT}2%b6bB`zn`nz>apb}&GIneB34ht;W{`RyFC`<YF>aXtuL1kum2mx1O9@R
zvccz%8yIUpt4(8SxqFUAb!qvR_(`9a`bM-Ziy7qoiCOV{cPbfv21)RIW3Vrxr+(E;
zN{T%4ou>NndVS`jy|wkXO>9xOyylGcl^^XP1h_P_+(jzCClYQ}mdD5sg1?OX^z~mu
zYIEaLVWQ|+9<SNG5ey&7SB@kYl54dV;I>ph<aHZGeBMcy_=gqB(su_gG|yHqTb;+0
z=#TWp7Adv6&s1y8+lB`chumK7I(zv&vZVShBtoaGmLZgo;}}$XeXBS}`r9KRqb^U-
zo|Mi$fX+Ue4oM`tRtv;T`$L2M&Zeh{q~a5>6$?Dw;70+5U(MeecR*{pKOYn~KQGgP
zniD6GOuPa=Yaa?_;c-=fF|D3)c3zJ?<|w<fXf>o_m(6a0z2awoBDWdNAh&G?vl7Md
zV($=u7p(6E7v%xg@C(*JeFTAqX>&oXKw+(brLI?VRb*{k<PT<=t^%o1f-uXl)omt{
zaV>$eVq>F|wa6ta8Ycmo{bz6QSg)ZA*{ZlV&_Gk-hb8pXlAZU1O%sp1)QOSG-KYAC
z&R;=6;7O#4toz!tV@Ma`d-ux|T?_w>@Zsl=EzdJr^0`bbKdOD$u6Ztep9p6h^+gGq
za1)xW>)}^M<c6k`A0EQuPM@a^)7?j8wdu`<w*2}o17D@B{8UYt?>dt9dfc^4&rk#p
z%{~H`#J>#9-qcEY!+5Jqfk`tACClumh};%Ga8z9k_`ydULeyhXCZWf?d9$OjwG(S!
z^x<<P5IF>7F8MhWnS5<60^1rm6^kh`XIAqLECf2S>5R4A>3tJDb9Dt+D=CckoSEY$
zAQus{m?M^K#!r>x*c}c7RLgS@Mc#fnNezl658A&cHkc?|zexc0S}kPgsp^rmhed*{
zs&da4lUh5=`rx~R?$LA?%>b2l6pZ2{ZP^7Kx43Q52=i2p@Qg$iA>J_R4@BkO>?mkX
zZ*M@HEC=Flns8t~9r_bGc3{h>hDeC-C|!_}1~p*_gligz;JV|!Uej5BGIh18q&(u|
zQfu3CTyyMkIg+N<-B2Ib`G$qVt0394gJ=ZfBCwopi#aFu^hODUrWi(?FgbB7W%6k1
zd+e;@s}+-G@7#^!ZJ(Qw;gPFG3$}gQ3#rwTW5ZDBWO}JXztKqliD{)Z+id^OC$jP{
zcbXbw#SCU21d+*^1z$c-9i&}*7y*AJ{QvPQ*&E^x)Mr{-zUliA@0t+5_(V^ce96!8
zwc}GkvOT6}ZJK-3J_(n-!;xDFm(PF>-aQzfV9Yg&i(9pngV;!Q*9LYxP|LKpCeH{X
zx2aqrdRi+hWON_`CFdz^eT(H5pJ}=pV&e-V%qQLu(%KNqH5hfLI_a1>$s{mALVQ0+
zBXkQzJhhc#4aSpDv!hX^gQ6VWJS@H@Rp^y(VU7qKdef**xNi}t^dx9$)m(7Nu*@7v
z_k7+>{z84~n<&0)43PjEX^yivBt+xtj{A+9!obiRX_4kR!Tn3s4x`7O7t7vWJNgOU
zgktB|wQb@2C%Haq-rnYTV1&i4=O;J2Do;qi7boQ&K9lr7RjDG`vprD<y1o4SBOvl+
zFU)Q|a6YY8|Kt_S2=+W&u55KOO)B}4fq|vXjr(J?Ir+-9cM}t&c?e=XJ6=q_nHkZf
z>*W-r&Er8#dV}9WaCBOM+1e=QJc6;ziI`Ac>72mK(#yax1gRQkZKSdr5^MGj6!KCY
zem@;fwQ4KP-CgX|r;V5sw%PEWZ<|PyhYeD>i-^+Dh>_A}Bk^?A$O1kPE&+X2VIiOW
zwxb6JVkB3vml$;47bDb^v)&W>Kdik4P}SYnHjGM0gGh^%fOL0>q@>af(kb1gAT8a8
zl7>TfgMf5*r*wD6yYc?t&-2bZ?>FD~%zVz7GsuYN$KGr2wbpfAYc0@36vAq|giQkT
zT$=GI$7j2CXhJs5Q11}z>3!`qsJ|GcjsI4y@B#*kKk!>#@{@sqceJLSTELR;^we|4
zgI2XwhwGJ{9fIN?2HwEVTViu?)Vxx_b)UoMDVNV-Ae5r)ll^;|L@3G2%~x@hP1x}R
zN};oD!R~NG1=bGujavhELcBEBls0}fHJ(86udz*-;y(}>!#zt6Qo!<l@@cj8i+QSb
z++osp%SF6dmsWNy@{=Xj;oYY>mzLhzH`!#5IlDlus9mx9md3>IGN*T8$?0tUEz-kp
zpF`lF)6sUyAD@6dhIukh3Mv^YRQ)o+@61Dr#77zd^RfT=fRT=h)S$G8BT?&4L-tt1
z%f+d9m3O1sc+;v4c9nVeCn&k5UD;Jm435!EB(pC)^;D9_Li0imq;tM(MXXh$$INp_
z+dDgK;Uchx+1F)>zJK7gLN)d$?+ATC{MiGkaqy)LyudMviwM;lZgVEQ34!HrqEh7B
z_41P*%GTBeE32s`Vn%&7zOy{OMr(VX@^d%kTE8qR*t;uUD3yP2%eYn^5oJcD9Bt*y
zz3&y&o1K)rmUaz&V=z+xk;n(cyEcx<(odd#g&}#z{^TcwR1%*D%oKq>L!VMX0)Ox3
z_$=T+Dx6J7_+fe~SJ{iB>s&yhu#j$G)0}_VBl)&y9URlklTfXlJi4w-u^>r25b(vF
zb4oe*dUi2%e-344&EjF5S7ObRC*YHJ9}rUxeS|2V44aSl&0L`pqoU^g_H8mGo`&n-
z^)*s+^W9ERZKA%@kz`i_XyMbIjp~r#^+osW7Tx<nHRyd9dID5C#sC-4dI<<VvQHk5
zUw9bUh9N!tNHDwEeHm6gkL&Mq^C}zoXIt>6ndOhCqUPF+CM&WJn6O*+nDd`;8QGsi
z<_jwM&`pvkrbk#JH5(e#63g|tsZe*(<U?xT_^J<B`!7;7Jbc<NW2seQ)r!s1u>UMF
zfBq{yYyHbN9(c;l8s>{75IDp<jE?nbJsZlhCYWdM=Vh~*fCyn2>=9#Bn<@09jDEAM
zr<kYY6wmTbm`nSh4*1)k!T4`(V6|bM|3m}>Fh2DRA^wD<`M_o_;y(pS%%qx?HSq1Q
z0XD`&F|QO-TukY_>{1ua7+3ny=Vg*MQTFi4{UA4^2v$TYLxLJf6Yb@WSDyXB#j2`q
zJ+%g|vQ`pv0B4_dncd{j%L7Sg5&NrWq6s#7C#pSG*~ldne=fKw#^=FX2~MRc%tU(|
zCskT`5|20NT}Qn9o%l~kBdeV`69!n;j|kv-go6zw!0fJVj9W7<u}5o&0E=wVkgc@m
zv^2;nBICn&|Hp}4s&OK!{|}9nj2i+;zQjj6IMaM_h!ouG$opJ^Re;KTpo)pl$SNir
zkyn10UHjeGtIJ8L&>vqnV`DriR~4>+InXh|z<_CIv|8y(*<Xz;iKca);599_F(kcF
zVlntza*CD$YNVzjDxw{p^-vH?Of}OX`Z>f>WV`p1IA)ivea;*uC~|L8<HBQkyoYoi
zpFEny{wcV9I5WR(aKVCdtl2mC0GU66C|52n3l2VqnpRP}L&qze@L2j=2Cy^PQ_KnK
z6k;J;X@z$aNAgg3wQ(@RLS~KccB+vNPPXlJt2*m7zaM>J!Rv=HFKxf<P!1kq*{H7R
zUn7PCO$WVU51-sk3QoHig<=v(pL;8@3fpP$$8z_Z_df@_WSJaIU$5RNauwGcwJI!$
z78w5F$X}iuB*6)V-eiA$h+@cpn=urQ=FtD;qdi!n0Ze+@StftR>}&)7)@ZYNLIIUj
z;Z)`qb|!u82nWY--G**<C>j9n+xpgR6vXRlz1eD}4WMf4fvBx2!o$gDiuvI48w332
zuiV7!s^)taT>TgA4UA@WH)9v3J?d#?Z<lejav_h`ptOvPu#|Z-Boy2mWsT4DD#y2}
z07@j0F!_}Hugwk#|Dt&-b?X-1@(z|f{IZfqGqL+CN!L___@D+%Bh1rN3~<kvCfq!j
zJsIEidb7~-08XFr<pH_}=9Ah<*QZ^VtATO0aHia|8;cKh*;SZs;SErsY_%iO)*%jG
z;?c$K16L13>H-Kb`8Nn0Eaqy4>sR#B^fGxI-D?&GYh7gyldPp<hHCFHGR@6Eu9wWu
zN5XZo7){SlAr}hE@lwLU2c(4N{X<$*7Q7RO<HYz{{9u&sraRo|>U-rOd}!imG2|0J
z8FY{KkpN&HS5)d3cqcPR_~a4(7s7ZagAPU?y&f>b9s!~Bu1)g`tnzzw%i{2$P!Lzc
zDPfl)dN#;jk-bzIg;}L0#VP>%a5ZLl)W)4y*UBuTh+1X_HTvgP1S2?ZkU)!tQ~FrE
zF1EfG1#LchXvJZsyqYc#cFVu5T~nh8pS@`$qYy_Mr&Fvcu+)5Wdya1IRq!J}KnjY}
z7xcrhr|J=Ks?(sqwbIj@;Qgf>C=93>F01oox~I8YE`^Ugl4(5O5C;bK!SnBImW&dA
z5w$k#{#`&X$-(4<96;E=f#YlW9Nn-P!F^@Qhw&Ii;$+H;x7jMfmqWC)q4xk47fivQ
zaZw63*j8lq-y{i3;=)qlJv|J}yql#3=+P82$eA8QIH25Zp{9f-$6*W5hdtY2s*OMt
znIoQSWi(rhYBn2N#N2|x`qTLzF2Q~Cjqx+`vffc`oKVu<<rfYzk8%7Z;{PCy4?!CT
zs!Ue#&{zI~>gww9e||!tMi={_-FN-QV6itXMt6i6QZnKMB+axz-}LM%CFi95GXt3f
zcO64^Gpj=;;Q}#&)x&oyQW!Ic^Z}e2JV47$Ouft<!l0ro4p6m~0gBeZ<;;00n<2H!
zx&cj96Ye%5Ed&7en)w|>`EnYO9ph^%9NWTFz!BWn;+>0k{(4h=VAL^x2j}6$g!b|D
zZK@eOFIJ_6@Lw7&Gt25-kQ>;uOcXN~a<FHVJ8<4$a@GGm*&gG6IyP1+ssW!g%=T6$
z|1a=lpKIigcDCH?hKoC?Re%1=p|1Xdo0}V}yQLa24lv=*vrN^+J(@ORF1<0j@Yn(K
z(FY!NyaIS%5t)o-bvcR=k|_u8=A#+1L_Y&5ISs8$#B_+7&FRhcp9pt?u~+swA|oYS
zqM6wR4l5iSh*W+G0{g#Ax$HhgSnJhz-lk^}<L1HF!xm2pa~SC<vO4n<jEJy&gDp3}
zq~;C~6i_nEBcC58Zn)3N&dcZ=bHzf;JoV*B4=}@N(ES9~V67989}BBOrgl_OIChqq
zhTw0;gYiE}4~U@CEk2Ex2Tdg<&daT0HM05{U@UjI1X`0!&C6UPk+(9VJb&p#Ts8`W
zzRHxptALU(3N6T<E@2x=B|(5c?mpFTO`VaTKQ)%A<2c@;9vAh3-*F%=UJr0}N!Ql?
zS}t(Bj{y*XpE6p@3AGHauEpqusV`XczNf%CKoeA4%<eZMme6;s;a6BWYqxL8;e_XL
zJikb=J6{nN#F0W1gy#x&2kVC&4kraie&H{WZL9DKI%M)TxFi<kq4vfS&fYR}R;k7r
zel}S9;Vh-s+9$}Tcan3Azk29>8w_VB;IW((Pn!xSO-n$c@2a>$4npXpZ@}OGU$LyT
zeaxTPq4YB>OFy!OOx6D$?F>zJeI`86Dq6MKv#rSk0Sbj?!3!xV$yi!yneRl###YVD
z)LotA2dw^-X+c<%yys+L5n{nwg#i)X8Z1)K3*?~4ne8a>a$=NQXyjYltOKc5bg)Ky
zYQ|2a6QjRW6qFVVIVkg5AwDJGer#QgG}W-ZJbZ8+42YDI3n4t_F7Lv*nloV*7LbeE
zA}4->B};NPNLMDhdpcH=R{yR}TbU2p22u89HdPVE@B6CMi(T@?h9C9@nupF<Smi9`
zwgLh?2S-k*ymhn@@-dQ`ue7K<sUMOAWcR(mV=06dN!8Y1FVelD+T;Nyx6<jI4xVl~
zKvKGoM)3C2XntDHAiAOFkm5+S>B5C^IROqJU{i6>3xB>f|H%pV;^FWlH-_^B?Vbz|
zW9_1$Z`!r`bP3(=f{$9=i!|RAJFtmjP$zQOP6N{io64V{`8q@IV0dg!UUWBCLjz!o
zo7AY3U?R;U3o^bD2Ftw^J`4MkGH%0Q0%oJTD?A1LV^pXjROpu)4Gs;<Kb%&N5-X(w
z(6PpLkFhZ`b9-D%tE$GY?)a<3mQEX4R80$-_Xh>6O3Xy`Ju&ZplhDA*{nw6F{GQ=A
z3iSGAeJdD5vejLp8=jH3EYF%TnnYUkaZ8U&eMu)sZE~4exypL9Wy*Y)%<Z2DzUglN
z_GT$%vs~l$OF<=J!KA3bCWTg)d@=jh%%+lVWrVuKL4v1rg^GzkwJ0>B-I~J~ZKAo7
z8ePrY$RM+PLw9{jWGA-BU<&nBSM)V^g3a~>q(wYT#uOMbp#p+-TUIBKGHi(rFd}QQ
zSS@5^*ugvfeyyf<a@=q@9`0l_V&Ic*Ohwl~$zY&hzAaI$B1@ql)yYl+CjVL)wO_|q
z{lTVspy58w*M0YLk#u8(DKA68pb+1J>GNG^Y@8Qv>cjiRK20;+->gdX@qum(;xdnb
zF53U_5R8tF1_vjiHs9RcxM7Ubad1>+X1*MtvJFZwAR;>gkP%sQwkZ*C3~^_0X=`cS
zrv=i2lwua|?cY{#IT%IIlc|#imoLFaqEihOVkpEjSfA37O}c`}<WN?Qe?Y_OV^5EN
z$~Avhoe&yICZh<u4~c||+{xT{L!_imjp8^N{vOSWAT{C?3z^%8O2kw&BcRC$yY(k|
zJ{^fDgwe1u+1&Wln}nY=HX@nA;(K!@ndt=r0Re%$WorVPwO_ly<x^DYXbATWH*Ahe
zmLe4$FZP(T`(Nw@LqTfRo6uk&*|ayf{2Zy>=5>yQ&3Iprlb@HV1Hi*QHj0<}o>!cn
zw{4*{2f%k-q7A7@K7zX^PM}qRMAvV8$}CaIUC4+{-cOs#Yto-0_g`5p4}p~9<FkQN
z>+B7k2vb62PjL|5yjAI!mX>zh+ZOP+?h7T-A|lEUFWs9OroiYEf1M9C3w_1yd3nfu
zN(50@C+Vk>f&o|<%fY*Qw!5RC*oq<Aw4tCEU`<7Gf$5ppAQft)QPA7p>)vVPd`Xk}
z>?-u?otsi3p)XfKLA=mkS;*}y?WI=#=!&`=9^+F~a?v!a5yeW0(CKQnpIIUbI&pa!
zA(wKJPyN0oK^Vs=3*yptekyT1oCg6f2cWu9<;gMCu>Mv9z1OBoprj9658wr$E=Mtq
zMb*xa?)y!Ky6b&aEkhkXAn{*c*8uN&oh~(Kv%{Ug`r!ZY4P<z{w*gv;BqZ42)52HB
z1A=CRxT00`R8t~l1gPN@yh_Swk5}@i239bt#2tQsiJD$sZ!$Vm3N9pn9Q_H!F+!55
z5BkSZ9|G%af7XxvtLxIBx;w$xADx(;jV-Aly1*R}+@kVud*Szgup|7JVAc8FzohV%
zs&JKD+@%%SQ3ID|1_gZuteD7Px>qmhR`>^$HzG2^=XW3eCQ&QIFXYmtOcaFBex!_W
zCbBVWpbc}HLSGteOt&cMTuk0yez;0r(EW_)DiV~ljQZm+vb0+pDLvw-9s>9p3|LZb
z3|JdDQ?M?PP<+fqlO>d1@5A{rc_r40)a&(7Uj*&v@FHuS_b*dDsvhcx&eMMEg-N;U
z3?$XJ&QQ&bc;M9C8<>_oDs^np&mMDcPnPr+T@$8-g6O0I^KN$n5-CS$!K)bgemoQa
zXCLI9Pwq}{_1K1|6oITq=X;r+JodFxKJimYSTwFtc;cVg2<(pnB9J@wtC>O*=3e)X
zfvA9GS$?g@<OA|VxtU$Ykhsb6rrIgvFt`Ls(DGTvsXY=yo7+H&rl_-H+BWiz*wXJ)
z|9!Eoz2TeDAo3;yg)9=@jy}4kx*QO=SnF|JeZ4gg&Gf@bf2834gcUPi*iIPYZ{rb~
zY@0AUR4h?y3K7r+krJ{-ymGFlbgxKIytxF&a2;u6$=o0!Vx{Ky*a#Y(I;7m;&Arc`
zln*fs%XZSQ?jn`73b?LRwn_y7a#&L(khCs=83@Jq$Ng>PE_ku)^JCtBK58_AA%M!{
ztMnW`)Eo-*)md4V2EX-6r)chNoGz4N{mUFJ4Gqat^TovI=;<R>w&aPrS7%hf0<ES;
zbrUSnVKIo(JR1udB45C<rW}xLf&2^qzC_`p?jNVZS#BXe#yFx--*iygJX2<&AAX@@
zqY4e?V4sc^zK%2bfEc`$a8|a5!}PBffB}xx-5z3zM66v8YjyrT4h{f{tq4XfSr)II
zdVJrDEKpsWuznb~P*8FHMTaaS;|CS0f-HaOypY>j{B=-3!6Loluq1rPj`t(H>UicT
z*jLzXJ!xLv-qL9~W*(D>9pcIZ>`X(A{wZHi6MwR6F8RjDc>pT%vUWJ~lK`invCMC*
zD&aVzi-=a_F}&ZCF7Z!f8-Yp<>70M#Qe<i%K**1TojJj*L)VNY9%79pqvat(R4T%|
z7FYG{^FSxnY=@}mGtTS+I<;@`U6IK&wEmB*=-Wdl6BfKaQ__S|RSUH&+ADk7yh4LL
z^)KT;+Vu$WOF?xBe-iHX7u>wZbJ`>cbX=D(?EJY;j9t0%XF%4>^B;cq!7y*2y785(
zvSlWYy8nAA_02-xO)dXTckdjNG)}&B&i6ePB2o@qQN+SD`k#nMG8oAYt%}2;+1*Z<
zLfOPv)0xzH5RV%E^A4Y$_~`*CVnY9)j#H;dyuhwchJaT3=poe4X}litI{#)%?@lfb
zqK%5JJ)M<z6KiJFev(VvT(cQw4w8>_b2nW%u5x$SMYP`*<`XPKQRAEhT;zljfd%Y|
zeO_9^j_>XxqbuWB>YE1UuYRsv9;^#6LwK&g`4V%%Hwz5O#2F3@z4+rz^2k`y*D=+&
zqbg{GrR8B)>E>IB@oVa6EXL=Z867Ys1>UMi>7=UAbX{GJ&FwJW;S`h|C%5PZRo`RY
zraxg3k~7H_dvqY5-N$zuFBYPpSb4hH1tZvj!r_(XK(&Vx4C0|AXL!*}jF0)mRhbx3
zv(MkrbMhn*JRJZBIkW6vx=u7*)>Xs1R6=B=l`bV+Xb>7Q29X|l&r9dAdb<8%Rk~Q=
zK;3E^s2CxS*5S2OX(~U?R^^`|Y#+Xca`CdMJucbNo6D{{sv${Jx|zWIRicTd8bMlF
zI7N(r=nA0+UxMmRAWF&U20UHYSwdn`jg{Ipm4@yglSb;hL&#h?@%Y`P=^Q1gCV^@k
zV-g;YEXwTnF84ul)TBo)8(#kO?W*7U3k8mYQ?GVhI1A6{$_>JnuW##jdYAl!?c6$P
zLLZis)ibh;yT(S2-fFFUs)HR3hWANm*!Bj&Os&ZQSQPJX4>)J{FKcvluEm{@(cO2F
z>)!da=rXa%5H%Edw1yNUv)^-LBU_3r_K@0a1XkI6ns3}l`buJtDGPz`;sOFuk;k=m
zc?H(mt4TsJ?kzh-zP)HVYnfwVjahb8Yl8Y6#;>cYth(pc*$^f@;-6w4KSE%VsM~`%
zivFX!L?IF_9yM|40+FLOim!4sWmLsdvQHNcy#Y0js|U`y;$?TN$>+Lv7oq1!dnM6F
z4H;_xBu>vsq*}%EP34QvejI2Z((<AjHl=GX?8!Pk_hw>2>YT`hepwGez}9eV&!;nR
zwvU8e?QO|5G9kZy#cH5MNDv)Sm<3*0sfoVDgqn-={46dPBSYvk^zNF&I02caOaCBH
z%Z9GD2SXwqtw(`x=N@F1?>7c+r56i2GDFMLkR6>j80hz1n2R3{dhVM6O22f>-ZXBG
zF)l4E?Eby^TWw+O&W}@~L6+);!3SrHg}lHdejPZKf<M{p>jw{y>dE=@vF6jZAL#yA
zaj8Ix2>XmoMvMA})A*$do$I;XUAbWE;sIOPIupCj!G#E4wDb^+TA703+9O|u57%UN
zp89a>FX9j=uTUnb@`i`pd=d$2^l&xk1!NakBR?f<o|>JtDcfiUslw!9r-?VvFG(lG
zc(ebF^edb+*)?=lga%`UPhZeqM!SlPzkfpyC;W=76XRp*flJ@7EqB;(!*o;~5sU>j
zgnXmvXFD8%Gj&$E^adYX&d@hj1l0R%SAU|ma+<DV8>F!!X0zJzU7Q&*Y7qU|GyYH#
zSTG*WA5+E=txoKb+iU>0+97?p%i4_;v@wLyw+nMoaDy-<FkrGLhdG%1o;CS0p#%;U
z9*aAtk4isHb#EINS=&zbfuQ6t<JXFzi(AUeGxtu~tshrEn|G%Uwlu!%lyg((GXh;I
z>#_Q0a!4IKTDg25|78TdI41Im?89E_yh#hH9`Ciae(>HitO``s6<-S;9@p&MuRYhm
zl4onW=t&KGN{3)vB#BM(-#r?#<xdDTLn<ggrTPU3=>Ipr22sy7Vq+YZPgPYsOjK@J
z|61-16Uc@T9{90!tn|>v<^z(0ETYWJB10117d&2#lX1-D--mN)jg&=>@jY2j!@?Y!
zV@1QsyizL5^Qn*DvVYPNG`C210IzAt&~0r~R?HLmeCqfQE0i*;@m_fhEu2P?`J<nC
z(n;gVl_#F-pt|5vMr1)g`+en8uYtSHo&=FMzj0^dslK4W>+9FrER=W{q7^Ck;_Nr{
zb*8CLW;^(cehZ45tg}2bMWzC-ECN*v(}DMP<M}A({0kic>m|{SiJO%e6GFeJSafUs
zjrZYx@IK2Cv08Um8NNDqTM0_vnaQpso*8t@#>5DN1v7pL%#DNNJR@UJOgxp}<#;WI
zR(NeK=8`Ph*Y|87xt>_S9aJd)P_P?Fa-X8RUA#Imx29<m1;%RPbFt6=oj;YI*XH3q
z1<a1oYH}}xnmwsQ3vM6ir>}9pA_#7mmFdsT*9chcnqgNp_rnUm6g4{&h9QQ_%KFNh
z<viT+#EcDfd*mytg9UT!f7x{K59wd9NT^{z$HMmO_tsPEj&H)sm|qK?e~|H*|Grnq
z+}fZ(=p0CY)p4#IIZ?oMts)+j82i1(EhtQIbi#;16_3x|=^heNo&Z^ycZ~A5Iw?qX
z*Ws^j^*SlqWYn1uyv=6dXfZ77vE69X!--grz1JL|BYy5Jt!eRW=h(;!v%}KDRw}IG
z(7{1=Y1a2z{GzJdWv*Mg#T8#AyyL(ur6AP%N)e$vrS`+##fC3d8zQgz#*f_-I!VQ$
zW$Hw8rg#E^AHOEFYzP?uz02JC8u?3jXj)!GOjL|`S9E^_(i1o-5W{_6+en9?1J))>
z+!GTez_w0Dgkg(u+lAS<^RADc(=KeG(BqoMVdwQtI8$btfwx&#BtH%F-|*v|#HW6B
z_1M+bRm<6`ljSz5{e5k`Qp7tv;#4-C>w*&NA|D@Q9^xoaySckNcY4})dMeGxs3Q&6
zEe?W}3OH4EWRxU_hx-iuPT7HEjVj~8rr}}Zt*wMjvxNoE%1Tr^oEsnX8NsVnvfy?<
zI}q4F%^Wv}mUDAe?VUb-DlL)lA22AL()Fj5<?|tvf1W#zt|1@LZ3+7`mM~cw^@(wU
zHMP9jfAo|IWS^)S3$K5tpB|0IAajkrBY3BbQ5O3xCAMjQK>qw<mcJ*xKU2N_)}<d{
z0ilg6JDBX1c)iNvUJw3<GkYxV<3Kvyl%>_s)HgrsKH0g2-kitq_AJs&CqkMmxLNas
za9b?W2zqhh-7*%Vkl20Y{2{y&K(t#LeY&Z5!1e@F6-_>{Y;qD?F+#oEM5VoQ0C`J>
z_NO9?C;m)2WWHR#Ev|o%r%D?L#9iF^Ls{S80MXaVfT=Z#?8c>7kMTrbd+HzDtlqLA
z*3t@eNtCzFf>4`goq?4l2oXr?+PhW2`~1d=48-ye+2o}`l6J1&k5TqJ(MP=emP(X8
zk(Da7UGKzM2%NLXNO=T=;{DmlldBCK1Ecl6WDUptxg1Wb&;p(2;g%MBH6hUM1sfaN
z?eegc*J+=aZ)#(um7cM<0SvyozaviNvLR6Jx3Jj7Y<WvVQ&oM@6+&3e!=tRBs_N!)
z6&flqTV=U%cN1o2X4ZVU5?T+sOWSO1Zi13mSaI>kjuP2fhJOCyL_GGF!x_U0Iq)@_
za`w2IMH^rck+|5tqkRFen$S=sVPVdKLd#{@>M!%dV`D$Q$ZKo694xc}OLBfWS-kk&
zZf@Q~OS6X8ahCwU*6sQ<;qdV7Z_7NyJZOTy|HWGGB*row4UMDyDgJRl6_~*O|2bp>
z3ahO@IkdRe+27c{N+YexWO0D7pxEusVp=^CUQj@u9neg*J7*)AvFBft5mwwnDZb3%
zPu>cN82pI(_N~ewU;Z0H!6Wf$RMed7vhHIAjV%l<EjRw49Lp=-IYqMnt1Pogi6PMa
z8EuuZw)<Rm_B1j3iAHZS)_ZS)U?hh9q=#`+aMQOg7Coo(i;}Zn*Ipn^!eeMWv)Sr@
zy?B){tO3EmEL^=7Ac`F}7?^4w>~V2sauz015J$@DR>vI7p^txNwhn%F^jaC~LM8$N
zMEJW{LD;2CY8ua22{p8nYBiSnBG;yQ#R>KU%)~@}a|E4|lYftmJFFOkYa24<VnSE~
z<RDcU&RL|E;Z!FH1MZKwR7zAXL`jJrkH};F&mTc9=d)Jdme$tV>1h|RODU{Wf=t#E
zLqj3ozZ<C9+FDyzRXf!=>^LeYl-Jj*$PK1ZNX5p+sQ?q%*;EAv7CfD{bwF%=vfdR-
zuaS|i=kNE$-oC(FsT_=;G&1ZzIzJ~Q@`z1JDs|ku2G@g(On*WGH#)IEX-SE8v*(|c
z_Cj#&Yz*pbZmNdt>$UmGkdt44v5Skfb{D|jj*ojhZd}jz`M*x&sM2xNAr?fWO+rW?
zVRa8&1{ClhuX!EIsxO{*JYW9=HS5qT#au=aXL)AcZ|#l$GD7rSq32X2oQwc3Or`sr
zfh8PQucY+;^}qS(Dq8IaLZ}>_Y|z)(r(hmX9}Ek1_{n=2Vn?3BzVZ@-54FA}RcVKZ
z@5~gF_~#~LBBW_Uo3O>bPH*7!u?iV=Dodp8DdtT~(Iq5Y=3%{mUcRZuZALus(n3Bs
zjV@r+2~DXD1OOvQ=QpAXeSNgX7g0!FsBUrQ1id~FME`PgB|mIDPkl4vlPER4A4(^V
z#!248(Wj%J;-6P&*lxFBB;s48^!tnIY6?+?;ZM#4s(qKE5*sYz+3xMPk#ZVLJBbP3
zt<+zZRf%C*1y*OtvJ2lVy@=4Lxln7ndjRT|ZQ_vG&X?F19$I#4(Ni`I&{AI8;f0kH
z+zyVS3L&wZ>;ksG&`@|fq{Y1W%VJZtLAN|kFh7?wwk+7h_c(DOp#mCZ2Gr>gLj!}1
z;OlDucd7`uU&7+VGx4(7*&YLb-y$^#yohEE)AM{IM8t0EJ6aV1b2GE81HpPLoxvs#
zeyKPHLEBZX;C4qx$NZwAd9brxUGs$+PCx56!9V&k?@xy_+uC{!JP;QYe`VYNn$YYu
z^rutq<QR#0d&(dzD$gjunq!)Vb&BNC{}28Qc=;I6#=g|EC47>*CPt+o{7a2eGe}YS
zKVrdbiLnn>`2wYHJ#OvSo(`~u!-sc!YZB%u5mI{~D(UGdzYn?t9N}|s^_gek*ajO#
zF`+UQU|Q|Rk2{6M#aYG0Yx|}PReZK8a$=4toODHL^BT<*%N=d<_Mt1LsHh>U`-ULf
zg;)&+sD#gRE1vWxltuWo27azSmB@z)QwwRga6)M3^(N`6XTntOr!%gJ(!<dIKA=nK
z`TAAI2kzJ89A0MYjA*Z+&1`smw?4v_`sqD`UV}t0#zXI`w`y~@d#r#j=#E$fxx^}c
z*a$>0MQvs|hK{Mie#lVm<6flgUesggk4z0!lj@CJU1dY-M2mHRM=0jttiD@R`UFr<
z*_2)JVDSL6*o1_WOVC3fu>rcX(CBY%*oP*gswb>iE)`ew3#w*e!Kdlf8*Axyan9#_
z$VEqY9L%`?if`)C5?EP#^Huz>Zw6yx`givDpXM7$;NIOC^YU_*s9H!#4P=OCfG97X
z2r^r>_Yz41BMW7Z)V2s;lSjX!4;=N{TDZ-Kl}sDJyv5+V=4WRT0rv1W>svBknB?YR
z2^;5Yf+v9Sb$Voc$IUcg{;8xPPG6c$anBy1dpa<Z8=izf5(UTB^=(*z#;_ajcD6St
zV-b`)31O%sYm~fo-r0}tfqV^f6o19bo8#edia;x@-SlEH(Ph(svY;$&esp~N)<iCo
zSnhhubQ8joRT+X}@!s8M8_CE@ZC8-74b<69b&PZk$PfPLEBuK8tA^lm6=!VCnxQ7k
z!QvuO!jjw~y~@Ys{Y6eKgyT_VySNMK>L?ueJgwYN64ZO)%ScI6#6LJSB3N!=_;E|J
zdev`^UkAi%qWp{CT_BHLyZfYH%S8e0GensL6sSD=_HrCWU=n562_XCo49mdg-95jD
zg*_OjYRSpU3WK%sH?p*BtgPHyZtH|3&JJC_6l1ruwr&Dfu#8M{NXXr<U)%=%l<ANd
za1uTFTvvXZ;skr#UH6NJrTp%snx5rt(TmN<=si5#z#HFfL+1?pG1uSStx;)4-yn?D
zgu1jjj734Qc4!GRJzZ;39B!|maB{Z8nUV2l>6Meyq7TANUEO(!>Mj_H`dYvpMTqnf
z*YdOI6R9$jv;-wZ|9Ie^C!7S-XmUE_0FOFA`xiI{x{lgmnHdcCS-Bv;F>s?QCXIwV
z=^Cl+N-=L6!7E5r1f+vW=l!Okxe+OH{}8{yhD+0>%;lL}u==d=+TdBOd%63rQg3xL
zTRTNvdOjE#60X;rNiLzH(j)LuT87;{lRIAjn)i(B4Q;&qB0(|~f9A=}Qc1#iD_odY
zzT2foU)lKUyo5Tss2^qPxBEFD0i@?5rxCo*0bqw_?UOHtTbR2qQuNn%2gjnDHeXn}
zt7if^EP&e>)yhsmSE0kt1{PHm>5x*@-M+r0dMDz!CJ(EPf%&4M{Q$~#y;5c<K1fUk
z20$~97T}&h-~+-NX7heNeh+XJ((5!uw(A|t)!yvSBjDgn(BWvjwWtLV*V5AX-Hl*J
zt`Ufka#FPkh&-xHN5ysAZE_kKz)uXOBu|anZQkFBI~@q9)|4PVY8B6qkNNF44~K?O
zMc7Y{j))2Bn0c-(tPjl$41Oq9B6mkaA*7M`nyBJj&!E#4mpCaCvLm;Img?p5LoQ#{
zg}%ZjJ{y1)qSluAH-cU%^{@OXP#QqQ-8lG_6}90BD8jvn-iGI9dV|^;&giLqdD>}~
zN89dxTfku<quE3`(J~C2+AiP0h9D>ZWgSR&;Pm^0GQ3V~)U@;iyuLbY#C|`ukRu}c
z`Loea#E$8t9x~0xunza>D3|W>u*4<{8(l0zco!L5?~OsCokE-vA8)wcdLjuS=JC>(
z?@ZvXPkCeIvZ7Hj!CUF4a}x1aSdFj2tI+bzGx2ZSt72V<pAJjSzr4SnPe=SWhp6(r
z6YC2}c>Q|8AB`yX)k{$FwE}$o`>*SZCYhIfG9fTyrwv?>iWl9&gt@;($#owHy{&FO
z42P@3a76_v)G@=T@!;0lPG*`*@z2mPGY9MIpHA=ffV#<nfs2%ps%*-(E>&@}%2SIY
zXhm@c!50Ar;zW#twIuxa-QCynZYoF85TCzwRwE|5k=D+Ni$O^lYpaK`dp+z<7j)ci
zg_fGpPFwW_6R1tK?L{`ljNiv~nNIt5^#nF?Q385o1L9J^{a{!3+AT+swKB7_<dwkN
ze8i9aZ^p@PUxDn(-JB(+XQond{D*TGkbzA&WnG_2omjD*J)z~6|EeB|h|LXJ_$g-1
zDE`OgQL-n!>`??5#Pg`ukSE5zn6j2I?@}O`%LJ&yGsJO{pOAp+M|#?|pZI(63{xU*
z7paAuWcx2Qj@fzzUe3WHtbuL*7`F@Bmi8SG;EB6BCrITL8mX}%uv+3ieXGLm<I}}1
zFwM)G%A2EZy)g-plP?D_JT(pSP*#CgLE7%W$W676d*o<jW%_6|`s43!*t(NL(gHt^
z=oaGJ)S8t+fG!Lc+aZkYAv5a>3C$OL>+C(aCu65S;U7gx6M#wqQa0O+#0t{Os??XL
zd2nA@^SH7IpM_`HMbJ8F)SfxViEBmrHVtUpu;uw^YPVw*bGeOQpZfbII#Kf025>q<
z_i!DOvdFUb-ot>IsTfZwW?g}#4!QZ7HD}|y*C+hQus1-&KkE1pz$d!{`>~%gDwnc1
zPN@#k7y9a|%n=nl!(HQt6FDG}PTB|A=gb1hzse~fT1$Ln#>6;Hg%2$SsR`tfVT|Q%
zBst+pq&CP@_V3}&L2^+<gCML0Q7qq_9&kf5@^q}XwMakfcB%}q{04zY%k)>f#+knz
zEyKKPkscg2qRE0}t;Zfo(%=dZ;TCi8+E|L&_{M|@QfeAip>9LuZDnY($)UtR^OWxj
z&{T?ikECnxl;DPK_Sk&*KwF(ZT1UDhJ-hQPcS{JBYqafg{p^60ac+~+@_Ex9m8h`U
zlO<^y*{u#3gDuU~p5#4ack^1AW$9f?87OIeP3fN~t-V($VM#K04N7x%Y*Z9&MEKw0
z!bCro6eiR*MFj``<apaWg-h+)`9jey_O~c_XrNb&)S{(D@Us|19~7{5UoKw+YaB>e
zk6KtDlRVGnaHd;)3w8UE>=V648h8f{Fr#dW{nnm>Lkw*C1p<ZnBv{HDUf=uF3fp>~
zLc*s56^#P_XZL`RFC%_Sv!3#7nK&>5nJdv>P(Y2m=)xQcf|y($LpHl*IMJ(+BF|4p
z$4XL-Ot433P1FWprK1Les&|8k6HSO3K7D%`5qWrAT2O#!f1!BaVpd@zRx@l@kpOTi
z4A=xNE(*&zCGOj|54RcK`HO%OWT|tN-X$V^Zab)<j~^fS5z>T+{i*+vf1#(heD;j|
z1v_PsbT^Gw#HL??R&OB$9}mye)U^3xaPl<7Qj{;}fvS*fL3XhUu+j%;|2seU|7_Hp
zlYUvus>N)N%AL9QsZyLl@~p{cXO&Ar7K@229K$@<9>OOhKdn4X>mC{;H9$ttAMFre
zw0<3{3{OBX)Q2%!mmR)+oxps=kg|Yt6P6h!Q_*x;xOH0@nLY6loe_DetSBWrr789@
zBPT2CE?b;PEBo8B`QlS=u3M)1lDGchOl}o|b#)Liv6;<RwC2LdQXW+aDZW`xeA2+5
zZ>Q-QC$4m$aGGB{F)!N;{>0eKl$niv)RD-X5V!K~jXD=kZ0X6ncW18t)0JI2tPVEp
zHrC$6YK~|U1bIKp5WyKFdF~22>D`|IboyrT;eN9D{_G*|JT7h$#gJP<Zt~$8eCO`K
zEB^!zVs>^BZ|lW2{6HvE`$tLd0E88Xe`&69In)2tFp9KP^Iu74slg^?eIn&Xe@BV`
z7WOQ9+vvJ6>%%Z&Ld($76yBVxd}&DnI{Mz>yP28(QlXYA2;|4o$NCGEW5z|AB6Ih0
zybzLav00d%gE%50v)WBc=f_IHmpI;m6ZJYV+C<b1p2f%aDwQC+lWVqeK5Ss%T^#Mx
z^x8=oJHC3dm#>e@al$*%jvY6+G(|v>S%V#WJ@2ksY3ostQ9iu1vBvqj4;bJccWhMB
zKr&_*v(Xk8ccMrKm&etgbYUu6jaaexgdJgw)8+Hu3b;YBbYWahl@$(ZB5is{n>DYE
zv=CU<UtX%J6A}=Zj&|dJ1C&xXXb~(<^8#7`Y5C(2EVmA3_7HOoA0HodSW(gVi#^+M
z0uaUe`j@rN-Wj{M4_#`sI$k$wX;~ZoFDAaEdB4Es0Qc~+bAX*cWdVe=b-G#UhW5=H
zNj0>Q33j>!K#jR^F`mvd$oI(2I1%QJ4L=!p?1q8(Q9(#)gmQ_l`+NGU|EfYl$h`hd
zcC~9z`kbrxz0^P*Oj*(rr4?oEhbW`2yoA?`o>-pj6V|5|=XV>G<Z>H>E3>5c1JtM2
z>l+<6I*e}_E-sGB-ycW7`lNj}9)#fQZ7g+-3=fYDe;!zw%`$Z1^ZdlgG3>f;rak#B
z^T_K>IjFp35rUbsc8uJq+8?8>?&+XZCw5o3+w|21=Y3WO*W==51ti>^;M>i)+;$YW
zO-ESDa}1iPC-VovUYF%A;GZSx5a`@eHZW;6*m;dqHzPIQn+{Kj(9ZR?S(z6t4&I&P
z+6=@o?sRHbHeG66-Vp~EhrZ-<)&}s<_4rgeqnD=r!_Lf&4m$@uqS{p7Vhz7bn>b?d
zBD#}3M{;|0zSc|mS-eP<-NfV_qPWc$*gF<RMS}*BcE(S~ai4B#aOXCe!FPVmiwd?=
z)ZEIdxBQdd!z(Cm*~oP<;^Q}b6dRWw8~0Gybd!GHjpzPgv^la@ZiXvP@1C0a>BS~-
zTdA#Bt6bKL$56=6ulR8T1rwhtT?-BmclR8JtryuWtn=!KoLE&jrEC~HgH56$7j^~O
zUmp29_$S7Bs<~bN#s^}aYyjwp9F)Zg{#s5~v08vk(u?@z*yxb5pTwUt9i>0q<y#Vc
zedOeqAG38-M-NCEj1Oi)`9_|6J9}=UDIuIcWGay2zUz0S6}A;<Vm!}RmyvpJi_{>S
z#K`55wz(#<lYv{=tK10-H@m%MDkj2hwBed%P;)cjTv*SHM}XodcCWdJ&Xd0KLUTny
zlk-4Yt)BYxPD->em0j5BM&afUOonAI;4(Kubq8DedLSeZFar(SzuFJ{RTxngMy#XH
z{?!6Z#yRZRoHaH5NWO{EzOA~vKbhIP89}?)@Ukt;tmL1Y=`d+&U80~Ro1Gz_w36W)
zNnLcK;iOtDPv=?MOlas_oM?8Z(7psRbJumr{gc#o;hTM@<uEUHFP>=DV?`#8;p2Wr
znLjIi?Tl3F%RR3&$or<&+%m7<!+6m80}=W0RkkiIIWKK&c?fCeT)6%htlga?679vf
z`1myp*3pr$9X8&n*2#Wy-if(hwZ+!4zP90!)E%9Oh!Puz`|hTKKZ2?VgA82UHJiOh
za|;U88W)IuugCbEBbrX$IorAY?%`h{O8%3+Ksvl+`pH?s&$~!|ED7iZLZcL*MqHLk
z(6<5tT59)iOU(^Y)zt1l&@ZFbrs&!XoKG>#zs{#bSmDb6aJGc=nW!z*cbO&<^e{;u
z+NtO9zS1QJ>(v4on3Mvlg|trULKa<Dm174r{=O_jcKk3?0s*d{6gX1iDaz_Lzi)__
z8LJW&ic<PYAE-ZW6s>vOOK76TCODpbq_wQ6DYdi+`4Q62%l6r8Xd~`!Q2gk@;p{i<
z(hzs~rDFq$tgwD*Lak?;-hF7J>CH`9XewjL%4TyjnL9X{=O*lB0%#MHx6cZOaaN^3
zAyCsvc@XiL3oeBkf-5)Gp^VmEC5pBV1$C;7G_9HvYj#m+L~>SBR$ftH={M}z)-ZFz
zm5s<3`$@kz%SxFp9FORw)~aLt>9L)$-*EM#g{pocsxM_O%pt!S^S3MBqpodgcFyHB
z_~5JHDzcrXWa#83_2n#jj_>2qp#;!3yl{<;)1KD2_><!x=st2f`EjpT4isal;Z_-?
zENXH(e!ic)YYj~EOJy8?V2i)$R+1sIL}TpYrhNT6Lf<)BL6I)WsJ^B$Bv`%kUi&tA
z^Nv_V$=J_Wd0fN!dNWajK;4E3Hv00pBCFam)tk3_IL|2@>1ngJv~|>)hqHM()$S0I
z3@Ip`Jie-bv1eh2O)_lFk}&-D@(mJ6KCJ>=)<~zE3d550GgFuZq^|zFUBUC435rGm
zT%B<ea}$%pr`!%Q<r5Y^c6Y)Uv7P%+N-Ldoxw#X8qjNvVhxBW8`)1vPol&b@Aar_D
zJpWR#m~tqlH`^OKkmK_J9hR0tRf-Q*??4NffJ{VW^bUiHt(BCaB`Y)X!c%-Ko9F|T
zK(}Tiv-D9BkkXtsu>%yWOkCWS!4g*%C2ny`YPxxDX#Ff7SLba4kv<`Z@P3nl{9{oJ
zzW*f0{nd2e`iqB|xemdialQla6UYy*ft~?ZYf*)3j6Mmkh_LxBc)#2>xKDm<llMDk
z`^L^joTn%CRsO@)i1)$3#Djg_ZA*lUTxk1&hPth(Tz@d@N%MZOr@%+o>gwa+(wgML
zJn;#(TUVERm7Ly#vwk=GOvl^z3+?p~9;^9{uav)Z&CCc1u8dz+G+bh7+fQ&XKpGnb
z9rud-s06Rq;(pT7GRr&?nj{B(>Z}y^1q%OXWx}}GXEaNREJxV)>i?~k1@buJ5U@9^
z<xya`DjhOC_s-KUTkph%gsSWgDk)9T#s>cJP-}W6zg=S8v%^`)fv1l9n(|dLW4-_p
zgF(V30bYYP1?SaUrPA+Q#((P<4*lBsj5&AT^Qj@MN8;5j)!QA_FAj1w$#OBzfj&`y
znCgzoD7{NwcAM=XCbqmiDZi4F`|UBsm2b+RAfCwD+gVIEU(VFR>Qp6Vo0}E_r>+;Z
zJO7V#JMv224#koVq8+GIGPle>t>P54R^s*A7fTpGm@T7NTU3+CRDLIn`zCnJc*pmA
z)w@%^&DsOJ%?<-wA*Ki_V#RsE>KeEYpI8%jV1&X)cR|b#3!u-%NJUmTs(cu^%VOl6
zcXA4xZo-7fM6qdTy&o0iI=d!Dc)EgH3|QC_I?yJleKdSjl2{$lBDdf0lxLdU`TY^B
z2nmLM33YurhVA8AY^hnXxg~d$&*?fy{qeev^-;zAO@9Prk)R(!#AS8!s}3P7wkKZ+
zWPIw_mZXU^ei(iwc=kPA{8s{5Nu<%gjd?zFM5c3ttqF>hFW_G)z!Q95_{89Syg^=O
zR$c^G+vR{{eU_S$H80qjrQb7?aqQQ3tq6ndak@qGm#%`zbmYo0bu2bjE1NoXpDtJH
zuYc<DK5P4+B8rJm*xNEQwJ<`;(^bz!T~|)(VpZ?-A*d;Yn~LY>?w4X!$s5CuT+DRZ
zzZ@Jr<}cxn%!=by9@cecwAm`0_J_1<;_zb<(1wM8fR;`?RDV~RoL4G4G*-zPr2xWQ
zSs6cLm6za}-EnP1SE}6p@<3($PrT@^R^)y6`qH}m25gglUtf4^+2NWJD<Kl#n~fEX
z)GuqdG~#E(;cSgu6KkeG1-#kz)+Qd|I|3`QytHqw&QW)iqOdH@`P{A-^zWh5N$kk^
zg4&9c`7Ic!&o+~j1>N>0dX|SlW<TFXY<o^L%x)BTu(IlkH``D<yb{9sGUu`6g$v9a
zeJouM5TO)O_(>oDb|4j94G{5aZ*nT0eUu9fh(`L#N{ttY_vzot{Wo<6`56F=C&Vo?
ze>Px%6{Cm#F7~NtES;SUYSnt8&qG8uP<U5HNCI5c20n^uM4w=*$$=rhKBqRM(8fVV
z*CWtPAF6aMU8=~u*JOaYu5XO|7zuh*FV!#N3cA&rw4{fx9OGZLdy;-WkBx;lKoxX%
z8AlOPHj@)Ksec!y&#^Iwv;6xkTV<tP2USq(1(h}`F8?O&t1Z*po$^S3e=ND+$pTP6
zf_v{;ySyQV+;mfMIZ?Zbj;l1cLkkGI^SqE0(ZF)`o@2({csG5o={?~q+-h9(1Nv9H
zJ<X+zx@+!bb18jjm|dDeGrRh@bkA}GR?LQPQ%pREXD*7jckr3<iuGB3Hb9M5d+Wp*
zfgCW4v*TEHvB2xJ?RGE~$?x@>g83J!+J?k0_9J5XGiv5$1mDG-UB7JTex>{-_0joL
z{nK40K*Hb!8ys{Q3nusIDqIPPD@xQ>k{9MIIzS>1U|Zf10hs@~43TuHDjs0_Wj3Vs
z981z1pQu6KJ?f9ojT20nL+i@bEi#ktU$6e!q#pOB&m&lsF?6IowqVLwttUidu)9g?
z)fnrYl~4Qbz>nv(>Kq+oXOrO7I}(7MTq<}wefuZ1|B(L4q1Q<eAC|)W`s9GU4XcBl
zSEJEKw776eSBu!9z{?3)52-4JzP>IcWENKyDhCgyi<y+;2w_$Y$u^Zq9Q3`A#oFI)
z+ShY~7b;$E@Z4W4TI5uEUlWPFwrieJIbMC+KxjWqtYZG4Rz;bFm*RE;GY2<9jg3ea
z4zzEG#_+F<CS|vN^{lsa4JGyFh{VYS7SQV)m`oTamhQfKzwO(d=;&sIZS3W41iD<%
zdmiE1K2<*-PP?j331+X9`|`$W?#V^^(cok>e~;ofu*R|gop|iv^Z&;hv)i1CwyZOH
zpb}1@e2dY}ERB8g=M0{cFt$Utkp^lM$ok`fZ|TA|Zt$2-b_GSBUIsNKN?Y{Dbb0?i
zfRf7TUxW9vwezF<4uNy|v3DOhK+gXFPp>9n9_rpwP}pyfxK-;0$xtc}3!@Ue%-&wh
zk3|^7)x6Www$u+N9{gdM9T#=bXm4n*oKjt#N-^1{d9+M=L))%F*Dr%H9h{e8X<=;G
zdl9@)W2AiZT1G3m#L#;Rov>{-9|&0X$e)qLs$8iU^u?#{#Sjx$*?aa~i#9-CwcjO-
zJjLv?c+GjV!VDUcDX42q^KnsmkQPAbi?}~>S_NCTgu1lTu8^H5V9{dfuR}_?m1EJi
z9C}%C3MF+av2g}oOzeY<q0mx#?D95jo%mk%;nH@Ydk|;NL!0%OE@vat!{ocw{aiTD
z=b;9I>Pv&?-T+maV)!C9!Lm5;0XS#>|Md7GdyxPu^UdrM7RJn7z#!)7Ku|bJ`Df7?
z2uUgvVFUIlOeI-vjs*ycHqQrDK7X!FOiWBji1rFJC{?v4l9KABM(t5-86U^4LV1`R
zA6Lp=7ojmXH?Oa)O@KfkiHR9EESyxIV*_kp_FHFMgEW=`zgOhv=QlM8bO_vb=`zjD
z&9SkwXT_2wpuo4auAHr@h%Eff%gX~FQ&sieKb@qp4e>8>(Y8df93ep$ITSLJ2$N*`
z{0WR2W*4n=TJc<kVAZ7ORx1WvVz77rUghHM#4hzXsx5{f6m1pie3H1A<ZqtrIrE2h
zdu@K#S~IoqWKoz^q_H7xcJ7?9L?>LA**LZ|H;!t1P3d<7dwIg1u_gv(lUS5x?v-bi
z!wSj~Y77RfuN%QDQ+oy!Rzn&MK85e))EZV|KhKh$9spb)|Jy6`(BYZ>az;(Y`YV%r
z75!jYov!IIYyo?#feWwaAPW%m`gnD$)Z4N5AqW<|rRp599mo)@X@cd*Mu&#R#*Jh>
zjVWa9)Pe)vP5g<cKqorQ-%l9146T&&;1{-IC0Ng7ln&7;@tEHLr*nY#{_dE1czQvH
zA@v2j^1(vIcF&qZx?Je3%~tegRC)S6ayqwN+?zN?hMg&es-52rO)cTE10Xgdr5!$I
zy!*&|Ee`7P&epteJ+7jJSLO<yJu0hNgJ`l(5fmOza0k~lCJ=WL9A^cJWV^=-(V_tV
z^fxL{!z=;K?(WrzLfxB+QQAx4KmPgksiCX0)A-poE%k?<x$eoy>Z&TotK&5|IJlsT
zfT|Lsegee}!ij=}>&26y{v2sQYIAUK;NakxnVTCM8KvJy0z$5=RvP=guy7oMmb!)p
zA+caP{%?`@uKf`9FJHdM6AevHE|z@u$=iB)VJIptUaVC=F{OID0TaQQn~|ZQso7vM
zoKAhlm8boo|IOcymU-dWJ#StFBRRkjJP5;BXOm*U7>JUja^89&RX*rE2R;7n!L@=V
zl${|UhRp4H7{m&y$c@Ax7HyLcVu}QYRa&}>;TDb+3)jWZniegV)e}uTRLy{e5EWE*
zr=%g88_1X{Ga<2NOmfRjSaaZ%NM=m6WA6S)C<rF5xgFWNxnfDoA+8HRJ9=%g#2{VT
zhia6pX+plecIRR3mslA}#)NGp^@WL$a0jp4o?JgEa<n$UO2#rx&P(<5c7yWho5|vY
zFvjYZ<)@D;oV?xRZM}Lpivum+42>A>7WXm8Stzm<`whynoejOiL18zD0TavWiOf^e
zcYcelTB_N(^ltY)TFU9Y`&D#qnaQ1aezDe-F)~%dn>}DU5%c<;=l9NAtBV`go`hVd
zC+jhjVZ;xWZXy*Yp}AHP6`r#hF~a&%p0`%B@wsRI&B$nT-{3$?RO4X}%ny&8`4+H`
z$Bs3aKcu}U-m$-W3d4@}>P0Y&ar@`}AiIMg2mie({Ow;-gHv_tj($8(V#PqGKmqYv
z1TP>Pcx(}Pb2bf;?Qq@CHn@MS;!ZL;;~f?@2vy2|^ME+r+WKC6qtHdaMNnt61w?kX
zT(3^~MU9M%l$DD&@;4!uTj^d+OFV}`RGgfg&z?O~P*8Ala|2%(8ynY*_4muDT%5^b
zl6i=Vh>T4J8~bb)=jP_d(5is{sx@K#!99)krybX;(5x&9|280}TD8t(3^FdC2BtO+
zE;4Lv@I{w8YX#A14GSys?%v)oKba}K&u3%foQw<~<^CaZ2W#uEz5}4eH28cDjoX7Y
z4A`{AxY*d(#6%5UU0ru~zUw{dD|k@%13vP-@R6d$ZzZySt}*gf%_^QHPS7b~wGfr7
zS4!5;5vB~KWTA+l<970U_rtdnzSH=$4t4RlA0jR5=yPW%2`e5D^Rkcux+>IVf+57|
zY01uBS*Hzm|4{b<6!FG8<X6OAUD#*Gp3{G7b#!gqLT0mVdF}LN?8y5i7=^vWj~a&B
zC8;7hA<L((g4s<R`mu>ZvH_;)%`Js%dZN-}wAw$;M;XHW32XMIjdqX<1$5}EsJGYn
z)%44ATHWn^PO5CpjeDz&1g%EX{{+4sDAGI+K~Q=W?s!_xjA48J`P>eqf=m68du~x6
zo*dLkbgL0uoIdLp9IE{bY_#8~uBiv-h>T0i$tmbOoQ2>3_#NkAF8u4e^TV=|3XXsf
zCn0nkmwwQB$#Vm7^^=*mrCE8I3x-x^enxS=ImQw$5w2P|o<b|S<it4pk}7wnI5xHs
zGP3P=IJMU3={3H^T+#7aN2`WPw^7_#UVo%=4Gn^GqN#J;6KF81H$7Z}rH0cyzs>%5
zOFk9?!$YOzsL%e1ppzFVRNDb#9yj8zeCPGIzs>b9fj`{wF|!4c;eSbD<Y_^K0J;p?
zC3yZhX@j?O=0tv4EoO)HYFa1G|CiZ-oYLo(i4JMhka?TeivxcIi=R*bug1PIDz0v6
z6AKdD-GT;(h7jBd8ay}z2#tG!OK=DhBsjsHpp82rxLa@%Jh;2f&U^1S_pX^AGpw_^
zfz^FZpWU^qo~nAP3U{~s`HX{IaYm6#LEc|Nm7t&?P;J^}dI@oH0UZ)H9H4kzOG|4I
zOzCKJ&R1myR?wV_q`SMj^iA2$pFhmZ%x~{p&R<LriTS@7b{96Xv)c#!F(M9w*@=m`
zq@=Bljp^}mczvCE`>*lw_?>OMyu4tR&(6VvUP>^#j;?MQP?DpgD&FEj1<(&4&L0Ue
z!L^^<vZZL~>CG=Mx0!Z7%~ox&ovS`Na_p}{dh(>x=6HT#VWk!t=Q_5gSCvD@$*D3C
z9UH4=W3wBAN&coxuhxD^5J-G~EEsDBE@wMiX|nP?xc1S{YYTo_`sd}N$mpC;#;Mq7
z^I@|PeGQJ>A5?``pAeqWl$yRuWe#{E{UkpuEI67b>lp*;1g<1LfkgK!8EFFai)cFu
zTm$h=$+SntBn#IU{v?v9G;Sm5=|XP4^*#r~>m&U2SJx>iDOEGX2@2<@r|3_gesA!v
zc{xK<IX*LkjfDjrC3g_L+b@)uj;z=FP4M&~&MBv>-75z2sDnj{A0$kl>af%occq<t
z_1LA)UJxeD67fp$u377TCdnmV2nNl`XG|&Aa06tGNR}M>lbLm8@1od-a3or_75{`O
z9HiK1xPRJ8*i(@28DLGPn1br0hy!El@hHchKYJp@G&i1qcUJ$UQwWzyJ!*cEZ0%~E
zIr4r3-cU(4ZZ>&jmcOGvpem5ow@cOA5x)7!&twY{7Ne$ZDi6o(*l+_jJ1Z{-%j;mi
zB>Se?%aA&6Q>B%bM`uOnI`enmuFr3&hN%&EslABP9iM(SqGy-VtI_Oo7<G{FA`X(~
zm~EI|PYZil1SKd*uECI(P0SN9v_IDi+^Cy3HT@33AS|=3!Hi0n;>(YMK4-6=uBw}<
za+|6($m5_J<a)4cE18@pEl-O|Vw<_KYukD8C~%9Pxa_@d_&B?JH?3?@!cUM;p}PgO
z6xQeFp1x5!4^g@q_`#d@J^uA$@lvJGk7tQ>#SM$a4^<W#y%`n`_w!9s2;VnhN#9>u
z4^FJ^?mf6uQ)0rFmVae*hhqmcGW2W8SJwnAY6_%Cyt(A!Gcs+RwJX&hD)_L18ld|8
z<YG{yhA*n>w}If*0{L1Dv*z&2j+K+dj<yGb%`Kr9fkjU=8e>lT1lF`5IQsFu4ppbM
z;`RM{`<&BzbQ*jv6Kd|YbCU9snr{s9OeQHTzWVc}$!5g&Qy>mvDD(00@ual8?L_FY
z^zpd|w92=RN2w8si9}Jdh=^a$&d-B`f<Vn{PEHOp!P69_=*Y-)5w90SM`>6s*2s~U
zhrimbr&+jOKdne*U}Utmv~)}%N>Iojx4gOzK|-KqX8xn{0Xxss-X73}WuR2!<Kr-k
zQRwAdiwCT>_8e^8^sis-?Ch#4_k!nZtm0v(9Z&1_j(-oD+wA_JqL9D=yR5R3i<FcU
z7Z*1x6;n_BX>dShwt%M3N~8b!Ur7i5&mOmY$KzL5vgII#Spp4wk%sH>-MAWeWlfN4
zeTa``8I$%M)vP>jFz=7B)(Np9TwfDx4hiws+j2RB31cGj#<2I~bu3FSw58bBFICn?
zKl-6SN)pC9QcYn~^3>Zs4iKm{3-{T#?xtskbMw@v7`vtghx)lKPX4z3nVzc!#iwV;
zvf;3;MA+wyPgfL_Xb!B`9SI7FH*rYW?F52WJZgR=4WBT~*e2d-41-M*pA?g55;+o|
z5T9HxZdaFb;;e6{Vhls%EuGEV7}>>)R9dH)vHImRLXz<FhM}^IKd;K{RkuuY{a3Gf
zFR2AjN$|+|%k{HzwSoc+zBa2|LM#2do`z>y*ZB)b9`!}n!l>|LgNDvoPO7KdrnQaH
z@<Yg56-2!zt#{uTeEkA1md93|doh|QUF^4`!5^tNQCsETy61vS4(h4h-u!&oVtH<J
zPce0p!cBR8S@(6=5MFEMr-=QD)gh+U!Tf0*<acMOorCD!#e!XDM*X%C)|-ta=2Ylj
zJy&LI5mbksg5ueC)0`ZGeM@dl`n&xo%!H=_G^%`SaG`_xNA;14s&xE;MXyF;(nx45
z?3tbFNHzIjZ^)CKddgE}M$~!}C3okcjiaV-Ei6WgJDStcDnIkZfBEwM!-uF(pK``6
z(a_Lz99uS@v9YxP5d9YRVfPJ7g0+Rkt1e1c*T^N`24Q5aVGIlmamSV$K<eh0ym6UW
z&0MQH@~Et@{~Q_lW7A#m<;$y^8-zemnlLvvS9NQ<+~NVS`N&XLPY)E#7rR0|wr{cS
zp~9|QZ1-#T_wQ6y`OIhY^X+jUX=-5N7a2bClD4Bo@tt%VoLN*fv~kNC3I5;?;zUJ$
zzgsJCr+BwqLm}1?U112R&Ju9Wn+Owf$~`#MWGlqWBiBDNJzg5hJc=wrl<poeJrhfD
zJcl;sp=xZQB=vkUK{yo(vh3?Z+KOT)L=LG8p?ZDB$s;UbZxfhU8TJ}Am(8lui@3~$
zQ}f&2t&?rEj%a*~%<nTr@_4?Il>j<f4GcmRu60>khPm7+Dqud1-?r4RbM_H)`*(8Q
zE7Y?tp~ju)(QmvA<$h3%2~8o(wmj|N!3j`GVkqOb9byyc<VYw~_$s`GM<Oc5=9#_A
zNXK9v9#~`U_dfJ)d{}juOvttMk?iil<|rkV&m*7JUi3m?v{t`aLX2Z>=S*gX)vZo*
z+73%lV=p=8TlsUCY3;J!ObR1(KtnSxagK|isTp$_Lq0m&n%lJ0Z?(~G>!Y>n>fX)-
zq!dWCkmuvX_YB_aW%obh%j7o=PF`LP%&qV9XI$}P;%mvF^MXoZ@7+D-)j}ty$=Qe@
zMuh1zqqDeN{PfNeZ#qi|qni$lA|F32y(k;x(5ZDAWW(1mPq6y(?K{IMv2yo7gh0oO
z)O0OK#9VaR&m#{*BQ`{Dr=xoQ9$LkJOXu3r?x^_)Q#z?MdooL4`m2!?Lq0Fq2EWZY
z{IqR3PyUkEGC_4wV3}Fr=lpJhfz<6!ZU#ycO<r3?HtM}?W)NpoGb+SpYrwIjBp{H^
zRt4xKRDD=eQ?tWB2EiW(7nj@ZU~vv+5gHWqadmcn-c(3)9x&s*yduId*qFI3Jp@Xj
zAU<$>d`$3EMn{LF2r8A|jn`OIWNB*pEJ3hRs<foUT2e9qsAnh?=u}Edc3N5q#&|PR
z(+(X75%sbr<RV!jCtV8`j82S<jQr`-CsVUwWcK1OUkH4^&8ePu>++dZNW@6#>dplP
zA>F}=U=U+tJzPv?S@%d0defoaG29~4h%RkrKpa*hfVfiqNCNU2%Gw5ZD{)S2FASr9
z;9G<#oG@{W3R&m44<ZkUg?L>kJa#MyECz9Td51K*VJFche-yBMl2D4%G;DrIs(_uJ
zN{jmJ89lFt<gBvRSr5kqZsFXx!n0u(G4lAw7eaAtvN2}ma}MUr{)wsYH^>`x!`M@v
zf8aJq<K{eRkT>rCb~y*ZpHXl66C%>x6IF~{M%qN}d`8u4<(gOGKbOShTKr;+q%_ip
zotLaMp&Q)FN($361p-2br|9@$(joK;&%*zZ_BovJh6^{)KVQ1&EZyhSb7eHw`Py9F
z*2sFaY4&*nauL~S*_!<ua`{0E+SNFq=!f}KNE{|?y!D;pJ5hSNdFB^;Ma6o|Xr0P;
zjfu(0M@nWgHbRti@y`ofG=Dad+Mn|o9zxeQY&JK5_s%`-m>8GV%Vv?35>dvhtdy`p
z*l)d~X+`Q}i$g0fDIIWEuiR;FE?@2J7d2a5rOPfsL>r6TRG8L!_QGqsikR9qx7c#0
z&lmmFzl1adp#)0uG=p-{jl}&m0x}X3jdf=YQlH0qIxOj-mK?6Ge6H-gfS4j`C?QWe
zJV=P6z;J<0jLU5SX*ZR!{_FUvz~P1GWUu|^$>V)KxyNfp7mU4<6d*{_(m|P-#_sO!
z-rg;Nfrxd8X7AtEWM*b&XRl;N1XBvTkNx^3Mnglx%p9hWkB1smre6=H#v!Ai94<8z
z>)VL&^1dJAYdIP^E+MUZ&{kEA0q<>UYO1Vsl#}brbYWjxTeIRN)0wdyt2uH)s832F
zuFK6YEM&rTySlnUMMuBAxuJSXwY<EHgy6q~GBk!{HvFxutg7nmRSi(zN4u${WMtL7
zft=dXKSV#AOUNoH$jN>DFl=gOhL4Zm*#zBVV8B5&F)^71zn%w|*?{@lzqht5%*?u!
zF$3`CW@o{z{Z{11<JTMxk#Ja%+%?g0vA9u2gRYsfx~3g!`Sr(L5np~sTE?4v8M1L@
zbWV(g_jC5L`G2?oLs?B2%ke0&(YHTIxGwoyxV^d_3b5UNl^hNzd}7Yycu_es!H>RK
zubS7V{YoSK>UHLsbI6y#$fA&!<H|-uZ}e&`LwiDnyXuPt>!r-+ouQ(++Zn%}M<sG4
zF27Q=I43|1mH!t1&b-o7D$eLvm9CutB#Bi0O!J3G5|}8f6wYERmgdaod4oLI{wC`Y
zT7G4dB}rIvPJP?{j%ih>hi&qC!6u?Uqd++y*P|WiG4Y(n_Mu9{h7OyfAb)|%-F=eZ
z=J8m;!uIB!<j!iIQYo<YU-sI6j%_#uJBLL*sRk(9`GSUZ<{4`Bs&7VW0(EH7UgFIh
z`^LKt+HY^|kN;}M(SK`3Hum^I)v-tv1ROr>F?Uo1fm66BhOqKTq&jlOx=_Ztc*X!4
zp4DgQg$iCmFMnT-@y0Q6rn9hjQrwQ>(YCDnf6U9HC-Dt-m_S01P*jXiVl3stW=SXs
z6zcfH4!%MF0d18?<-lm(V&D<x<RJPT$}1=^($j<bG3SwFyi>RrvqteW5|tf^V5V4f
zbo8fBPkphK6Kjx_0~GS9UOK`J@gsFsIfIDCM;hxPL~4q|+;895SXoOugx{H)qb$kB
zOJ=_v2Oi|x+=U~6FE@5}xTn6G%u=P!&(GuGAu9G(CJ}V>UW~mf=Bx~5Ok^-OH|MZX
zm5^BJ@JCcvS9d3)b2_chy!Z-mfco8~Y7U0Ojbg<awv<fRb-UUP^-z?+Q2_3xJylOT
zs*ax%;XS{R*2?m|a;>>4;vGNAqkH$Q6hAeZc%OHWfTbpjX0|@9bL#337DVd>HtWot
zQ9nyGZbsW3H6zCD?0m*d^hgXXc_P7gia!0MC3!XM8iR;6i^TmLU!N2H$@;@%ENdvC
zq@iY;QT{G0cY?!9#)?O(QX`)DE9k2Kj8&2zH9P_}w)?7mx+ANbbP6AUVpGz_!gOJ)
zl}-dL8-7Yd0cyX~rB0W!{o2Caek+xvuU2`?>>|ht41@Sm^17>6a~pw)@9kYH{oVd#
z)qmV7;0tAgx{yWo{C~+sJq-?@!(s@zHSf!a@Ng?jOALOdyV`o77hGI!3!Hm@NelG8
zAS)6m>>D;@V8{o9?o(5o^o|qL+>c%WZ`Eh3WN!Y~CN@hOMw0@6#);hG8jL+UPkIf)
zw$Ap^(?8}3-1Uy={baLdPU3*q4$p&Of8`e$#F~YGFJe@0_BnYUYMk_Pgjezken@DY
zIH*6fKQZMl!Q(htosbTg{V{ibH!b?G_kk;J=Zb?IW@Q;&a^3+|qVw;nsN9RGSa!Q}
zX?<~VwFN~P#ZorA?DfvF4$1xq)Np_Wir3(}01HVB1U@VkLhZX0oPI5Hl6EXU^-KB(
zF=Af|CrM-YyOXSt5>ozslMd+hh&xzpa0r+wCTd8^M{Q8ZwooWDspzkVnM^rIj^sv@
zNYK247cY<c8j+aVGMeEepl)%ASH-$gVp%M27WX<n7{faM%&AUkiB~eSk<D7t?#%;f
zZOkk7_1lz8eZp?@6c?l+`ru&(0rvMSAJrXS=UBoTQs#A{-K5$kdxRgfVC%Muboc__
zJci<Mj5vmq1-X|7J9(I>Pc7W`E;uFttI#-lPd;)iw7r!ZD+L%El$Qp}0qy1n0c4G&
z(jv|wi$CB5T%^J|U*PZ=bNNE>`mWQ$uFSbSRR6shV2Yv+WT|FksW6)S)QuX^<G2k2
z!_ew<Hss%N3{*H`4=(0<wac51cW7UP5><!@RYqpxWV+*3)z@Zb)|w+NPQ15zD{sIe
zbQFfN9k8aHbGtF>S;sYNJ^Z%rPeb_-_=d%kHI@u2U-8964x^tej=SU?VyrRej@m~@
zWKpq{fvFNy5xy8(o`S?ba@|UFUaYZixC2pXwicPVeKjGpo3tDp9E^;}Co0*hmFJW}
zDJVz8`1o|Oa$qQ)0jNm)S*FkLu)GIM)%to~Zmv|es<`kXJ_*C1Q&B1!EouVuJ2qNd
z7WtaIFyyP9-u-kZ)r9n{*U@SWGbE}D(80Yl#%$Y?<_5N4#6-l-+G-xeQeQ_d@-BVg
z_Bb)47NoMvc|TE+gTwoXL<7H!%A8UrixVVA^PZddDm2qf?rl^w!bNGJEG~+G+LyO?
z@N68Ag!&r>REXdZR6*TLr~L5www)}Z57*$;R?BqXY{-P6TUh&j&7lDw`HDT<9#3LW
z99hJCcTRAzUh^CZ!x{smb$G>H8Pf$;<bbYTGAAQQrP=c2`Lqt6JBniBENobv!?9ly
z&Z{E|1%==}k}5h*H5`rCHz%$)_}Dtd1hsEGdU$(Wg9dn>oe-FingrzZbzHHbAv}{I
zki?OWq<P0lhnJ@$SWY*`qqteu_AZ2{$|OYdmW@U5Re7%_S#6A;#k0WSC4!zhF#>V<
zkk_kY3Tm(D?A_!#a^TxlBqb%KpQ)*<<4x9m^e-x6F7Q@1p*lG^nJiF-@bK{PQO=St
zg8;$Q)O+BL+5G$_|N8aJ*ceb6=$M$8XlMeCzu^LC00IHA=!b`g+l&2(u&|=?@;2Xl
zPe(^C5CS_o>KhqZhFM?_xU%Wj+1}rrtEs82uCAunLwW~z6r#aGIQIl{){7}Tua;(|
zQ;K24!;q8K*u<(VEz_n_O=#eY|D@N~p2;4l*Ov)XYf*22<2-jA&B;?H)Oayn+w7#|
zZE1)B(3o{mYLwk5c>xxhIKnR!_=Ib6(vp<QzDs&LrLk(;l+M|0xv;rc(_iA@^T%SL
zXa44dS{^nI(%d|n!&kFZlEWw=hvL&1XT3nRoGV^$l89vc%XA?m?O78yKP#}_?*a3Z
zpYZ*LY9G%A3lkAKe4rdCKhw>0@24X4a%Q_8U!WMtu)D}vL7Oc<4e2_*`e_{+W_d<-
zii7rA)?t|jKL{uGk;OETAodACZ2V+?v{kGc`n3w<l-uOPVHxg5Is67J#UQQNHnAYD
ztfq)JmWD}56^AI5miC(pSzQPN1A`1>d_FMP^z<EatlZq(PEJn194ad-o0*wa1{DH~
z2MW4A5wb=`MQLej*3{Inv9U$VmJdeF<KW}RXJj1g?d|>fBUd;HQV^dh1mXZLOikU{
z+NuZsHh6RIXKFEzlRRLbSy)P%nr<HMZ^2G%^1eRl3dMGDammfe;V^FVKHt%L3kKUy
ze*5+fn1~F)4<+T`%Wiv^F4e9uZnp=ug}!%J=esi?$sy+Vu(+^*9#jz$5>i)J2c*Mj
z)4$lHBdfig`m~e6(9kfM+@;0q@?f%1h4~Z6v%PuqhC{b{ee}t8UVc7^02AWkuI%hI
zfl!uh#j%>*B!cR25RNpe#8h_}QQUX0FAS!d{ez@-^30kts}0fX9)`t&J_AyS#f)?V
z@`RGRL+w$)((LP_scO5NY=^u_LWP43A0$I}<O!{_?gI1JEZo*eWL)|Ll2e>2WC@r0
z=(Z01k||CQwIIvOS#O#D(GN#Rz;k&eZyuy3F>M<J_XLNc1j<GUDN3z^Tj^qOIPCD2
zb7#xx7}QBlGu$`T{-L7@an;>VJGG`9t<wRr5f#%L^~r)PwJVO{Lf8c+zO1A9ow-1u
znCz6<d7Xr>on|lsryv|xyFYBcKkPt*$b!x9MQ9Z&nKdnn?2T>wm4I2QPHp)Vom2|m
zSAU<c2?+`3=jRg0_xJZ;X7S<n_V(HuJ_$*K<@hrs3E<ZT1_TI;h_JG<=7NR+nPQos
zXMmobHK<##vC-Gp_X0o;B!2Yk?OXjGMQLb$pKJ^!H@#o)i;axL*sCrqL~(%$Ew}r9
z_P97d7xB9I27{e~m_PHUfRDq&!(h87V>;H<)h#!DC@m{1D=gevYIYM55uq0IwY9Nf
z<>ag=F9+!;dpo=PyK5IIsV)#y1U~SmhzO8G=QL<gRafV+7)b}h0TKdVL5G6P;WUs{
z0C`j(WzL$KU#hB{Kt{^m{s5GfLx+-0Y;A2#O{-n^=YfQl=~PL+dc`Cl&<NxW++9Tl
z_Ih4b6>h`A{JbgwsLkVev;X^-=3d-qthQ1p5)$oG+EVjwrM~a;tJ2s{oS1a0LS=bI
zBS{c@JFdHRc7pd*?nCg3g1zg}@@JF^y2B={pZR!_9nU%-*Ltd7>&0~r<cSl;6+W}#
zmZ2Bvbv~weLZ}u{{xbFl1+p%~b$scc_N4Tvq!(NipO7_&xp-9hy4o)#`RU<+?rFjO
zSds#lN5l~i07_C;&lpfnI{X+fdm^ks^}W}b<??v0$MUi$o9+*e#;VRpf3qM6udeCZ
zNu2RvhXEtM)o{paWVl;$AUWaZv=#SX1Sbue$Fd(@Qr#kW&{@$+Nl?kBbFrbr+tUn!
zpMLH{O(wi>(6s@?C;)7}xw(1z^eGa8$;I9r*m#VLj4oRvwe|I2?_NKJtPv20#$eji
zH<vfZC`j3i)Ifh28bc@IGb4=zqcVh+U!mKa=-ubjpH8-ti)4z5v>N)AK0w{v+(5b#
z`&3-5rnq?f=6v_-*RO1n(^*O4Tuq+mJEvQt+=1jnh|S(W{WpRd_R$P>c6K1F8yg!k
zx#K|B+b_3(XfdElY_F`qLsbKwCvps#JwDtY92@|(0QNojq#$NYMMc%@x(|%c$>n9C
zdNFFiH@(_{rKP2b3Foz*C?LAvm2lPIxw^U<sH~bA-rpEKy;Yj<*h$ixD;pa?VZaBJ
z78f%xG37GPFD+q;iU<okySO+kHsF$zH-Fgud3JU-H8qu$MFW&3PLO2wOCg~OAXy-K
zBW<drL~>x_v$~-wWe-wY^7MEq^c>G6CcY$2WbwS8u*yNwsIDw{IUdGl%fwzSBe7^G
zA^avmX(sNe7G8uJQ8QHBiYL#TFW#$&{X$b2Rbznb)h~)`5<(}w@0|WDc-MOBR8Loh
z3SxZY@v2P9kFjYWn|N!{F-3DAzTij|Mz)d!;x?O56q|CBp(GQrcjlT{NHf*9pIND2
z{*0_A)+TxS7q9qp)52-NxWByBi35lhJ;uo$O$(AJy49dM3{<|0v$MnqSU!-=)pZW1
zLFIq3mkkuy$a?efimVL@c5wF(p_V{$+Rx^D3t8yvRoJ;^nUKL@M`2f5?8pcGl#6=v
z6P@ZG3yv+>Mkl>e=Eow~K(I6llvXJARoOjm&bEP?rm+Q8{UAC4w0?g)lN!MjJi=tP
z2Iq|da(;V|YHRViz1W^CWd2lFQv(N4DwkBNsjt5rPUBz76oUxg-i~M3pGn<*lu&W|
z1w$!x=AoW3Y}D63ZF!L0jj-4srTRKJ{NCAF@J-C)&BIrX;4_Wu#@^FY5HuSY96UWa
z`N<8U#gTV+cc6()US3{Dhq#ZA4-hv8hr`OsN}ws&bt>Nir~+gOEmo+ULYLoVQ|W0i
zf`at3*dN{9{X-zkrmLpr#H=8mNw&jr`5@)ADT?f>nAn1bjy{?$a5|I%0nLltfuS=p
zq9UOPeg63^%b)w(OR%=zySjiLgOkk%tr2X0wNXmM6l)X$(FKv~^^Fa?`5M3|aqEqy
zq%j0$8CdK@VT_X0;?hz;5DK+D74+q8wsiQNYu3olgkB;2gaiLx?0qP{C(q^Cz!PG&
z^Ras&Rk2r7qiBXgDCae|wsHqNIImjw^eC0|v{Syq#v;`SJWS<Ut;N}=Vg+*~0ExfK
z=(TOemr7KZ7Lo*J`mV+AgqisgwE7FBj21ACwnT>G3WP`V$ySP^PVV~XkU9IqXM4`x
zf?3ut7!O;y)c!GW?sPz$j_HQQr)}@1x>W^$x1qS@Vo(GHx*Y69c!32N(^z+R0z(pN
zfTLL#m3414oT5LqzyAt?N_-O1G~-p3Reb$##g643#vE*;_O3-XNh;MJdx3Gs6C@GV
zJzXt&G&@do>s9ChfsDOz4BqzsY54}f@mTpBkLvMSFM^0O+T5AhIoB*Z2kY5q4n#$r
zE=;=)heuQO>_t7P$UZn)^tGY$Qya9+s5)(X%Go;iH#VWq92)r?tzLES!nW=<>!vS<
z5`>Cn2(gV?_g{^38aMxHwShl*!)lL3R}d!Reb&%iNX%?$+)S!LBP8Zi-sVXO5m!6w
z9OPe+GL!C>fAvX4ds@u%xS;mnAil2;Q8~(iETh%){efxJ>8B)UiMBe7gNbpPsnCd}
ztR*%5P5Z)Q$@fRm_r`uvi3XIJ1~Q851?E)IqkVGha-YIZU6(8EzIsW{cGl1Z7ax-r
zKYvnn8_!<zfr5xF`tzuQ-1DdC0u8>?H?^e(0a926TZC^iOi*p*`$K1DISg&>6%=2;
zR#K9aS5zDwR8e>x9U=0&%2jKz^M1SW4Ag{t%{U;_NWdzj=x1Y7P<TC(Ma;;+)GHly
z-OxxO*dWHoD#Rc{2W#Ms%4j!9pbVfvLvyO>k`G5idrrDd`lK8DUZO#eUY5jIXz`9x
zC^ptOIng*KKDN#Cdg4l|3cP5$(Sw&PN|Ldx3Z?))gph&?=*7j%HYDi8(tM}%+m$+Z
zdmbhCnZ^X<yW8!n;86M=>Q50O$-1iE^sG$pXRrq6dO|N=O~%Dfo}O5R#=oD|A|>Yw
zL-lGZmlt(j|I;|1PVBe-%g3Ll4_cVOzq_j#f&Q9;)pKES@#=#8Q;vDJ41f?TD=Wsy
z&lq)H1C&u%RaMn|%c?Q`?px*0$;pL<nO_SteoZ+!bvLV^VQmIKFGaM_)lK*5=dWMO
zK<3ly@nI!&IUpzJ$8W=7?1K+=b$7=%8*>3I1)-O>o=vx2q*2+A?f7!HUQM_9%RERL
z1p9|Odp7p(YH%FqmX>Cgmag{t<54cX+I{b*#>eNzk1HQ!&9D1^Zf<V&{7OqBOEYTq
zKE3&%s4+Rnq_Ubr2dtb5A)!ID&WAPi^Wc;Cq>=P;Q7wCiS~k(mjt!WF188uzvL3Ej
z$TC7ee1MIWXR+^h-?TYWTas35k-6CFy1Vm))6D{yiNXxyV(-iKgN3b`jODiDljWrz
z3pXRNrjtKC2XL9EANcJ>9ZLLsPt|sFd)cu^HK|>eDs%QE527q;FP9g~{d6z0HXfpg
zwMB%}P4NQ`AB-Na?sJ<f97HVJyzZ@(UDfV_!6<s8DG}>NzkREy)>f^R@G}cPVV~uP
zyTt?^hs<-WdX|H>a=*o!%7ZzE$vN0lh0lw=9%XqvHVvcjMXARZt52mQ5MtoyxflU5
ze(^>0nY$nG$uq7UHG+WVs8>x`GckkRpd`ZOIst6DyH!ue%PUMzKQ!vln-$O7SY<N0
zu<)zB!-W}vTI{UDr2*+k0qMPq{O^tW=tRyx?n3Pe=C3b>#2|^Ql4@rz)bkNJhCY_$
zE&`mk#n&04L*c$R1Ifd^F`UVncKei>?GJXx6{Gje_cy0a6_&$=zN1ss@tI>nOMmw5
z&vvwHE?D*M(3Tvpu2M!GB8<mMI6<NILwx4=>P6dF$tw>-OJCyD$>0Ll92C(k6Uw64
z%ZDT4Oggcwn$MP|Hkk`myIw+Lm99Nym78rMDTQ5vV|_jLJq(kOyG{LH`>~OOSnnNN
zIM-nLejROyBJKwdoh3}k$ICw-DzV-hkND+K)dbR*P+I;;@iIxcB*lW#ulRA<=1|oK
z8#fGs8CwkB5HV5Ujs36$To7RxyW;zFlcZL)+!_wsW=P%>R6Q7B-J?TC+8}s<>)=I#
z+jbWu({SwQG`04Et)50s)x+@Jq;?1<ged~iN=-$GB<YZpv@4SX?TH#DHEFo$4dNm8
zqLJ=~8Vx(NXK_;Hj~y2OMI>3}yG?<+&%WTKYS?<<j{Y#&RblJrie(j3-lu%EoSL=}
z-xrx)zRg4NIjMd0<@ek-t6(j=+9`baX;cHl-rs&D<T+k(AIW&OgMYKh3JTo<A4VwH
zQ0Fl&=6W?1JI!reE^jw8mW|F_i02=u>}($9k26**J%7qXJomo+w9@9*xUw?bGivxJ
zY1GmI`{o>dej&ow_R!%mH_2n}*RN1l*R%Wallp=%hEO-S{))`GJh2b1zR(5X?9#Qa
z!t&2oG|ZoTucx#O4euK>i+8#6o0t6^;O3Wp&73??Mu>dMp%xNqO1bb+QaqY_IQCI_
z?0AE%6I0P@?5Wmc>T*AH+7*hmM<k_ja5L7`GDrdozW^lAj}^;Hbtrp=s3W4PeB|+$
zM#;?o8_Ul*euy83g_g|ysoKyUQspz-Msg+?)_jte>hJWfy4o2WLsd|69p~^UKJo#O
zt3Lm|W%_GhKYj%bcmrl~H12NmmrggK(5iGJwO6_$QB~I-ZmBn_XNJ^d>ZeV6_zHeA
z@!pR<=fr1s7H3BMsk37r?)H=FM9)01Z>p$89`-(07mP-={)w=!DJI5bZu#O`f33x-
zH#cE;(spUY?|DVHldLlmW2SfeHIDQ<(crfCx|p~-!i&h!P0?ScnC?%WA~}VzWBPRB
zl#mFjxn!zr7^iAqH9ri?%#c1rVO*nyDZ|%4R*I=&>~*D*HWAX%W>tU4k|53uHSmoh
z4HUK&a&<Y(#YU769E(B6G|}?gKnl|sSDC+@7KUmMKjU2N+L{xo1!K!2eCOd2OphVY
ztnRGw6R3|9X?J8=Z-is|v8Ff=z0cNM|H|!;Mxj_`3|-JZyS!#?e3YcoTzlC2^8?e3
z2b0v1=VFKoR>LJlGr{?%?6H0G5~xrF$Gr4L#iB9q+iT<LAx0Llo0Z#2EZ$8kFANP)
zavI0|U}eoVWz7dsDD;Adui2;FuPS^11vjKNw~BgvmVb#!EVocJm&&Tp?4X7HK4_EA
zb)Tap`DgeWU)kOEoXh6%nN~{^rkmfZYvtH~ntzQGV~>BDf+eBv9{$YHetl^y5NO0<
z)9Q)b>Wti~IZv{e<Pp5hA>dq0vwfEU`E&l5#m|v-PCVYMJ(jxWFuEny;6~W0b$+ZS
z;<+&uA1{!Uxg<kv@XA<3!dOH$%_wWQV!V#%%wGBSqO8Ay%C)9G>+qNN{*Ji5@8el9
zKKo#+<Kxa+o9G$3nV(vSZD`J)(`HU%v0F-+@d~BZ&yAbpH>=}=_qWwX-X=v>`}f8|
zH_gA!d~U{m-Q({2j^9?@Ei<%?JXs92LAMdiyvV+9zl2D(?jPQKbj|d3eo&g}s<1p<
zcu?`N^ive0i-H{5FCKWD;_iFQJ+6H4v*>f0uYGuvRh`Y8>U1jY<EDW!zcl;nI<I|+
zm3`CM*oA*yds4mLG5fsowcyu>LC*F{H|?tfL^s6W`<NgY58|nEa4K+McF=@|EETrq
zu<VFqjTvx*72WIsrj#nvg<4v^vwT_E`BU`;+lEOa#rr5iBJwulX!#s(&hD$vIL<`k
zloEZy;u$v0(Eaj1PhL|sK_k+oEL}xcT(s?-7k$R)I2;TuSKn(W8p33Zssu!9zJKtq
zQv7L$nL}ugn)?Kru^1b}%21kBF6K4(FvRz8TSrfToY7={*sPsSUpwLVIADmvkiW4t
z+t|=cY%BQXu$t4(P`af16XwLHfl^mW14@#uhOHPE)L{YUPLo%W{XDPqGb^>4v0mDK
zUUX-6n9LTAH;;anV_LVovP(DlqI*-|s+WYlvVNJYWm`2vQF>?S`i1kr&IoNdt6R^v
zvtNg=SzVth=&V;O>qx)<xtdUlX!;dlH9Y|D=b`d(^y>wpq<Z@qU&ZDRgKrue4<eU6
zxkg1!`pO%G+r#L)tx{o|Y)@Y%<Hl+JCU@*G>Asm}HFSGInPpDIm|sx1vBAVh?OLtd
z)VIjXEs(;U&BMpe&Yzi>?j0r|b&BH>0-gV5VL6cu;k9Ce@Iod8M#rX73-;bYxTGK=
zpRu5p_U{dFsV`?Rl&zlgypTFQiftbr<DHuRK02oK@;w(fttc$(WuOsGlTGi52hkpc
zSLQJ_+e2W(%I0U81@O=7^rRI<DO&mIpINFn>siZ6s3~}c411%Gl}lb;nHx98YkUw9
z5TSb*gGZ8BocX4ZHy)i)JW$@4@Zs*=#kA0?iY4v1B$%;nN_s36KB>K+l$c|vgQu^A
zwY~3aU70ui;mW&j^oP@Zg`eFGbt3w|cCCX!8}vERoLmIf+aVoxT}BmUYQSd@a=R&W
zNu&>77=|4gxA&NII{YMpwvCitdU%C4IV`s?xm9ct4eCOMEdCrgxr84rlC8E`#kZbU
z3&4ERDG>P^Z<@bhF0A5iu4E7pytmvMdBO<6MM9ge-u~E5%})Okx)m+u?-lo5a;kv&
z#`Zm^dFzW3dhhO_qj%QUg}bt_^z#t=R3IbCy^c3G=a1a}TpTQ4_h*FCgF*S3v|V@q
z?$(HCtC8!z?e%_1wa|2Ld8UZ(dE`RDG$})IxAx$t3S#PJV}p}QX2EX1wvEWHv({If
z1q4e4#KvNJ&h1yT=Zn{;n2U96qs?(ndnTRZshT&#>FFhTVeXl^7FK79R=o#=6lDtI
zW69Mv75Xdp0jsZMxVIllDYhufl=5csh?o0W8{%jg<mU+w;<V%O(zq*?kS>iqUjD>*
z6nwe_0394!qzi%qw3M`&1s64!oxkI%UH+d;h)pTTgj{GabmJi5CZ!DMOX1-W$>97@
zlHW-ymO`{gwS}m|e|Sgwm|jmtx}e&$i*;O|`GP|ef+XSX?L9ca@;aTlS7lV#`z6Ek
z=cKplnAU+ylvCV{hj(Uif3mtC+eBzB;HNl)Flf;~etaAEF-jLMor&<>!$r@_DXbSi
zBLjzn8u&tn%`oCKaJ0I5m9(DNSi`~LGRaFzXi83b2ldmk^rawDJS+zkT@Shx&C+aT
z_Z=<yon&n~h}pFiLA@PtXufk>5rL~Q!&8ioOO7BEQKFJc62ZZD(!-O(;9jb*^!-f?
z6;T$YI(_WgD3hP&X?(|{0L3|H_BRhFa)!e5cqZYT|EcQk#SmDtiY)Q^=j4FZc;}E9
zU&wxvx#VaXlpZP-@k$>x<&|R>8Zs?*DA8eE4CHXH0ZX8m<`g9D{ki`|5dE2FonpX4
z$^Z%T{L)b^jV=I#BW+a-^E{hfH48~U=9Qfm`6rKutOa@CAMBqbx*oNE$t^k3mEE1o
z9yCPPu!0mA1Fq$!-<RU%Y7m;P8FLNzryV3EP>SD<9yaQ=N*pPXaUf5q@K!e`I9>N+
z<T8V_h^Or_x73I`S?*4TaMAUGN?-};W7@{LRm7bIe>w^6*2>uGIFaNg?xJIWL?5_T
zGyD3GkWjhQ>whiITMR7-mSaN;U$Guzl9FscMh+E2B^y{wHgVZQyc{Z#x~pVb3*jiW
z4=Pks{~|X(4m+RJ$UMNvUq{BAI$9oEqSKzD+Q00iSqIlzB68mRN;yOO?{At5#IpV-
zIa4*+;}scm-yy4g#?wi(q_rTT$#p|h`%I<pJro1U=qYMq!nOv?s5X~rTg?TENQ!?_
zc>e@47@6P&zj7o8NKM<Wr!$Qze(%$b;zADd41#OK000}!dL4YBeWcF#i0ea`3~jSY
zR8t5V!v)G|fU<RsXk3CKivZBtKN|r&b+}0ZE7nyuz`a-e*<_+CpLcwp*1B_wW}S^>
z1T{c>DnJ5L06pY1&w3(2ZJ2<7-~qH=e>m#GR^`LjArC?O$4B}b436peK?5QzD?cYm
z+6bcl&ojj<nBtI3NZ<!3d#bYKt_XOjqKosTy|5#Tl_MY~uBfO0sMC^Hn}q2fF#6Aj
zQ+kU45UTw~l!=NDMm!Zqez-lh8hQaTtwpCJSjBZVE(8Qc%M)P{q}<f|o81Z!0A~Iq
zIj!{8j)W2EKX-^92nPr_`YL9#8q{5Rp^8>H3sO@7#S(qGT<O`?$O7n*A3N|;g;lVN
zQfWZ|8H9ouzZ!ky<D{nkmHX;6nris;;z9x^NSp=<C6tIbiGVl>(DKcoY|(VDCP2tg
zQ6XALKmfKFkhB*1vATL-4z}g+t1Z+#e2MABi-WUmn7KJ2I9xV$-X@N9GQdAmd&$ne
z23jM6${CulloVZA*`NLKh0zr9AY1hcM_NmZ4+0SqYv+PMyZ|NHpa*042x&Ic5FXw?
zKc7iKQKg~L4k<AqVM|<*gS8ZJ`Z`fU?d?%3#i(0aS{#;J4D~#pF*7I25y;}a8_($m
zX@vKl5|Ri};xvx<>@}VEZ~aU^))}ff33A82Fdra5l6X_$T3%fIBx-KS2VL~)r5nso
zMb{HO>$61R<dezXkInFrEuBgkNkOqNG7}7(b5wX|QVQoyys5rnnw?n4IwNvl3b}%J
zRf=#C6QCYKp{^(E#rcX!rX?wfiLS3-b6CoHcwCc#3_Uajgbooi@9re&=*%u;8R_VD
zH#VYZ!T`O{=bjRzN?pnSKsJ$zG<UWq6F}&jkFS;nvL}ifke6>>R8kV{Vb5fbtgbOB
zBGSqOfpPzBF=;YDVE$M8);&Q+zQ0zMS5o5PGq<z`i%ris!Dw&N%d)jM%HiV^*y=^s
zq48(FHtqLwO?7AQZ_Zbgc|w0qO?t8XI$m&c`fX3-qXyhPM^RKzc!s)q*zR)gnRJ--
z8-WAi7Qd_8JTNttEP!UA{x&!ygbsD};D8%<uG&v!&I_y)d`~JGc+`T$@*nn}#&J2n
zp^Jpip`sP%2$UN6zljX-DCh6Ahvn$;5)?q}`iCY&Te}_%5c~+Ou~5kZ38a7kgo4!Z
z?I*z3q998FMSCC4&9RV=e$L{ls;Y|PApPZVn_Tvfa?;0>9(El4j)D{aq&-~eZeYj{
zXO424ih~+dQlgj)|K3sl<NaJPw)?g^d$6Pg&iZVXdW+b5>?F9`Q}`d^`RXcP1#>GL
zva+*vb=)!BLEQ`3KbZVzmSR)*f#NhjdU_h0y=YKZx9Yn_Mtr|ltsaSV{}l@`Y#9EG
zyN!I>_B$VPvdO=>1>h+;3+bvkE@bCzZ;jPt&utUbeR`<3cV^V#m8GS-oLq9|FDx#3
z2#>R|8m?IgR9bpnl~0*bQC3=dx%KufkW{HT@kb{~xQbMNwiO?7nZkVYAKeTwa(Pa1
zTWCp`uG31gvKK$X<3TnAHDK!J&*#j{Q`1W^2?@>K*8@SUOE3Gm9_E70`=>uh+1gSh
z8yXq;vT(;HW*YxlgJD8GMz7#?h*9Gp9iN<(mzABKE`YGyH(kc}o}DKfKf%Tt8XBsG
zea7N<w!}uQ6y^pQWU#R(x1YQvvvqW|G%%3#_BJ&#5>4I^@o%W|{`<|x#@=>dD1)^C
z(IJ05tRa^FXfD80yVC-^*YS$>hZ~?RT?`J9KOT+;R6+maL{m__fxdee+aLF?`W+Nn
z*Lwd+Vh~LjWC?9-DgyjQ1d==T%@jXwP$rw7r8Hhs$jHb5%>nS=XhuiX@XinAuN#y)
zQc|#{n`%%o$Bav2Q&3i>OoPK=^ev^Kn_?k-Ez^*Oso4$X@1D!{mm@F&osMIf!Av7V
zT@U+*yKA$wh43`NTYM>X_3DO(b(JcNNIraA-0`t7kX=zyP{{b_EHGj2k9(kF7nW?{
z&d$#E&JHbVwQuWS5V=JEgUAybp@3zCKW1aw?>qllhPr)<e>e&Mwk`P4K=YDlg&dAo
zSJ-R<7L{;%l^#w2jn}2@$MeDD>_`kuO#gPUwR8?6#c4pcYG->J(eFw?6s@uyam~lp
zZoi{RM#%L_YC3X@>_<D|?#@nPf>v_!P>*TgZmJ2h>}rQta|a>|3k$OTd8YI;kV}2C
z%E`<eP(nIdRJDZ|F0I$#h#SVl&5g#Uh&{u%M#?5i^AK)gW`=Ix*w6sFywjkbEI7Dy
z&WCMox-iyQ_bltf`CyEZ<BL-xmO!uGp!^Fi{@Yl3$HV>7f*GR-^&ILz*xC21t3M44
zxPYdn%&eueP1Es>ZyOzSb-TmyH{N;{L`K>IRrva~Jvd}#6C)$pIy|o&pT77euCL!*
zvNHixK2TzAhgesPZkAkckepq%ft66n;Qf-I1v=e6L;ZdL^KughQzGp-?*EJn19peb
zhHF3pIR%B@n>S9k7p)-YoRpviHpt0|#@48||NS92Vdu+;@88Y9nSz769kG?ESW@rj
z%*|`>9_~YDqf=a6FD51?1Oz;Q`2a7zJQVAbV&Os^JeAPX6J%xG5_@bs+JamE@<sE=
zTc^hI7~Bfe>n}_U3|%6;9+r?jmB^)L#Bzg1*+Sy=&52GN2y5xaO!*SvQDje|AKeIH
z_oMNk-d=ubt%tiIc`s<l<%0o2rq%ErHGjS3e<7IPLNp!5(|a?<e)oTS=HV|KfA+gG
zGZ-Da!(zcBg8wg=!wmly8jF(vKHg&EX}Mu@xZN1iL!YoG?;%)_qgRan;;2|y>>N-w
zK5_yAVNLnZ6!IV?%Klv7V0~Tq3nu?VMurhKl`wJy34#|9HG+RkjCk%Oy0f!0F`G`G
za{5@v*Z(ruOQx6MQBguZH*8lHmLnACLwJHAG9UBdS(%yX>Bj~JSpJ+LjD0kTry?57
zj+`T}%w5~b$uVP6$(XO_BpX8esO-t?xp<2o{@u!o6m)>+hCmRrJ3h<FbZ`HVp8xV(
zqvP=`C1vjVIv6}2`FS49RqIqg3c?;o-n_}^dT8loY^(eM!QyP!<sCbQ+Z{bAT|3?d
z>f`Y7;qbHA!&MiySaM1V`wvQZczEVd4_sLCooxSsULZ;x0;1F*e*wiD5G_!1_^BAW
z7XckgH4m_}$;dQDCF*@!{F0PZw=y~8Uk0~w9U%gfKZ%M;Uqf9TP-CLO8$>xc%7K-m
zMU~7}9T-$`0*Jwc_OGqTEC*WQ$L4p~?VUWpFH%}s2@Gs46h!bB$a_0Yo#Re?f2mP0
z1%+rjRD`s%$`~x`q2-U^(knwlww0S~1(jWlBFwEO9t8yjUY?#S+U|}{b6EuiAvAp#
z`@VqijT+FwXA%<pUe)ZvcuT_1q7(9NjHIWGiC+eh5_?Q#O}iIyXyopCE7R}pxR(?H
z1=Tp&equoL+Am-Xk^GnD#RRc?@bRC;7HP8hHp*5?;1}U-A}f~zra-baklVh20jDA^
z!1t{@#Xup)swW^M?96CPw(dD_xpEu;awbmS8=WAj8;YocRo}s=mkbsawOPg2mo|m1
zpBB%3Hw|f)oQp_WSsAmhty11hNp|(ELx+wn@4N}v4B$2xqqtY*;9vf&g(LJA4~vR&
zak~U&`<9Rf2Nh62++W-9puBth&|~@A&w^>O8W!j(JR5;5NR^e@QxT1#I_!JBk@R?-
z`FOoum<ed;IzfPfV?qc|`9FC5um8%03g}xlDE|)wOHcHvTjIYhEL(ku*EWD$#ZVfy
zWhN%3(V?Net79JUxq%tXGW!*hwwhPlTMC6kSVoxwh(OlB03lpDwdRYgk)t;vausg)
zcQmw`=~B|k6tE8jOxTdYe=|T<n2@lr6R!xYMwaZKw`#-xF`kabc6MI_0wfKWKr^?-
z!K8{vuCjg#ipSkZ8q~o<Hh*zuPO*e!xxrD+v*WoJRpL$@0A~Hmu!_10AAO$#qF+=|
z@e5Q*^$P#S7!IHIeQQtqzjX;%S0MCiJ#I#wIuKo50dQ>Uz%a3}SRWT70+kZ!;~5|j
z*#5ruTlhIjFnPDOR=e{p(nYhuO{F4+J2e;AD=8@rfJ6W^;^X_kksA!%Lz-nkZu$IQ
z1{e3hVdV8^FKTZ@SsZC4B{s2kocIuv_|1%Ue)eNb5{Vdqb+xn%zh}r*%KPEdBq6hv
zkOt6z+CyGm@55zO+iw8Fz3>-raIXp(e3?xu1b<fnaV5U}%77P~#HOo2|5Q;?QTt6s
znwXy`6!Jis{#O>>3w}MM#BOoCm{bwl-rapX?xhBa`agg|Li?hUI^(}Z0fgNpJ9?nC
zQJ7{7xyK&Ag(~)TQ?IXV(XlOBP=T>_b3oE*2XoS;r0Ariq<&Wv6d?C_<Dv$ors|1^
z`1~GSV2b$B!DE5XfzVApEg@AUhk+XK`ybPb_u(JYYpeRvz;iXNCNJ-uuWvT&l#Gqd
zc7o(ntk4nW?6R+ni3xWyJ-TU#vgdk`1O|}Eo*p9irhklU$A{soCn?uLHeqFg0}n!q
z&tenVVF>+%1O&<k(~+%~S!?hQOY2^}fif?_bXdTSGt|()PtWe}EkMuxSA6wf#^9|x
zQV$r~+45c7=6{UHUt7XR15cAug=zr+A>eF1QcsSz%3K^Tdcc7nd70PJ<&tj${ufu-
BN0R^m

literal 0
HcmV?d00001

diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml
index 450dd5254cc..67067ca4ced 100644
--- a/nav2_mppi_controller/package.xml
+++ b/nav2_mppi_controller/package.xml
@@ -17,6 +17,7 @@
   <depend>nav2_common</depend>
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
+  <depend>nav2_msgs</depend>
   <depend>nav2_util</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp
index d6acd320b05..305140e2e13 100644
--- a/nav2_mppi_controller/src/critic_manager.cpp
+++ b/nav2_mppi_controller/src/critic_manager.cpp
@@ -37,6 +37,7 @@ void CriticManager::getParams()
   auto node = parent_.lock();
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(critic_names_, "critics", std::vector<std::string>{}, ParameterType::Static);
+  getParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static);
 }
 
 void CriticManager::loadCritics()
@@ -46,6 +47,13 @@ void CriticManager::loadCritics()
       "nav2_mppi_controller", "mppi::critics::CriticFunction");
   }
 
+  auto node = parent_.lock();
+  if (publish_critics_stats_) {
+    critics_effect_pub_ = node->create_publisher<nav2_msgs::msg::CriticsStats>(
+    "~/critics_stats");
+    critics_effect_pub_->on_activate();
+  }
+
   critics_.clear();
   for (auto name : critic_names_) {
     std::string fullname = getFullName(name);
@@ -67,11 +75,44 @@ std::string CriticManager::getFullName(const std::string & name)
 void CriticManager::evalTrajectoriesScores(
   CriticData & data) const
 {
-  for (const auto & critic : critics_) {
+  std::unique_ptr<nav2_msgs::msg::CriticsStats> stats_msg;
+  if (publish_critics_stats_) {
+    stats_msg = std::make_unique<nav2_msgs::msg::CriticsStats>();
+    stats_msg->critics.reserve(critics_.size());
+    stats_msg->changed.reserve(critics_.size());
+    stats_msg->costs_sum.reserve(critics_.size());
+  }
+
+  for (size_t i = 0; i < critics_.size(); ++i) {
     if (data.fail_flag) {
       break;
     }
-    critic->score(data);
+
+    // Store costs before critic evaluation
+    Eigen::ArrayXf costs_before;
+    if (publish_critics_stats_) {
+      costs_before = data.costs;
+    }
+
+    critics_[i]->score(data);
+
+    // Calculate statistics if publishing is enabled
+    if (publish_critics_stats_) {
+      stats_msg->critics.push_back(critic_names_[i]);
+
+      // Calculate sum of costs added by this individual critic
+      Eigen::ArrayXf cost_diff = data.costs - costs_before;
+      float costs_sum = cost_diff.sum();
+      stats_msg->costs_sum.push_back(costs_sum);
+      stats_msg->changed.push_back(costs_sum != 0.0f);
+    }
+  }
+
+  // Publish statistics if enabled
+  if (critics_effect_pub_) {
+    auto node = parent_.lock();
+    stats_msg->stamp = node->get_clock()->now();
+    critics_effect_pub_->publish(std::move(stats_msg));
   }
 }
 
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index 93f7998c56a..14d1327508b 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/VoxelGrid.msg"
   "msg/BehaviorTreeStatusChange.msg"
   "msg/BehaviorTreeLog.msg"
+  "msg/CriticsStats.msg"
   "msg/Particle.msg"
   "msg/ParticleCloud.msg"
   "msg/WaypointStatus.msg"
diff --git a/nav2_msgs/msg/CriticsStats.msg b/nav2_msgs/msg/CriticsStats.msg
new file mode 100644
index 00000000000..f02406228df
--- /dev/null
+++ b/nav2_msgs/msg/CriticsStats.msg
@@ -0,0 +1,5 @@
+# Critics statistics message
+builtin_interfaces/Time stamp
+string[] critics
+bool[] changed
+float32[] costs_sum
diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml
index b81cef7ce56..01fe9656d5b 100644
--- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml
+++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml
@@ -76,6 +76,7 @@ controller_server:
       temperature: 0.3
       gamma: 0.015
       motion_model: "DiffDrive"
+      publish_critics_stats: true
       visualize: true
       regenerate_noises: true
       TrajectoryVisualizer:

From ab4aaf863aea6a7d1e67e0c2ad536ec1f0c8ccf7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jo=C3=A3o=20Penha=20Lopes?=
 <123633705+JPLDevMaster@users.noreply.github.com>
Date: Sun, 7 Sep 2025 21:57:05 +0100
Subject: [PATCH 48/51] fix(nav2_theta_star_planner): Correct typo in
 CMakeLists ament_export_dependencies (#5514)

Signed-off-by: JPLDevMaster <joao.penha.lopes@tecnico.ulisboa.pt>
---
 nav2_theta_star_planner/CMakeLists.txt | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt
index 0a478f2fb9b..09813efc4d7 100644
--- a/nav2_theta_star_planner/CMakeLists.txt
+++ b/nav2_theta_star_planner/CMakeLists.txt
@@ -87,7 +87,8 @@ ament_export_libraries(${library_name})
 ament_export_dependencies(
   geometry_msgs
   nav2_core
-  nav2_costmap_2dnav2_util
+  nav2_costmap_2d
+  nav2_util
   nav_msgs
   rclcpp
   rclcpp_lifecycle

From 8aea1a895f23ca505da3629819b6d16f4d126a0f Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <doisyg@users.noreply.github.com>
Date: Wed, 10 Sep 2025 03:21:22 +0100
Subject: [PATCH 49/51] default empty constructor (#5518)

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
---
 .../include/nav2_behavior_tree/bt_action_server.hpp           | 4 ++--
 .../include/nav2_behavior_tree/bt_action_server_impl.hpp      | 4 ++--
 nav2_core/include/nav2_core/behavior_tree_navigator.hpp       | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
index 8e345abe0d7..924d1ebc28a 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -53,11 +53,11 @@ class BtActionServer
     const std::string & action_name,
     const std::vector<std::string> & plugin_lib_names,
     const std::string & default_bt_xml_filename,
-    const std::vector<std::string> & search_directories,
     OnGoalReceivedCallback on_goal_received_callback,
     OnLoopCallback on_loop_callback,
     OnPreemptCallback on_preempt_callback,
-    OnCompletionCallback on_completion_callback);
+    OnCompletionCallback on_completion_callback,
+    const std::vector<std::string> & search_directories = std::vector<std::string>{});
 
   /**
    * @brief A destructor for nav2_behavior_tree::BtActionServer class
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index b8fb983f91c..10a8289d5f4 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -39,11 +39,11 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
   const std::string & action_name,
   const std::vector<std::string> & plugin_lib_names,
   const std::string & default_bt_xml_filename,
-  const std::vector<std::string> & search_directories,
   OnGoalReceivedCallback on_goal_received_callback,
   OnLoopCallback on_loop_callback,
   OnPreemptCallback on_preempt_callback,
-  OnCompletionCallback on_completion_callback)
+  OnCompletionCallback on_completion_callback,
+  const std::vector<std::string> & search_directories)
 : action_name_(action_name),
   default_bt_xml_filename_(default_bt_xml_filename),
   search_directories_(search_directories),
diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
index 9e984bc6550..fdabbf28133 100644
--- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
+++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp
@@ -214,13 +214,13 @@ class BehaviorTreeNavigator : public NavigatorBase
       getName(),
       plugin_lib_names,
       default_bt_xml_filename,
-      search_directories,
       std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1),
       std::bind(&BehaviorTreeNavigator::onLoop, this),
       std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1),
       std::bind(
         &BehaviorTreeNavigator::onCompletion, this,
-        std::placeholders::_1, std::placeholders::_2));
+        std::placeholders::_1, std::placeholders::_2),
+      search_directories);
 
     bool ok = true;
     if (!bt_action_server_->on_configure()) {

From 97de6f21c067f6d514c3940b6f5d44afb2ceaf34 Mon Sep 17 00:00:00 2001
From: Sushant Chavan <sushant.chavan@idealworks.com>
Date: Thu, 11 Sep 2025 18:20:02 +0200
Subject: [PATCH 50/51] Feature/vector object server (#5479)

* Add Vector Object server

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Meet review comments

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

* Simplify shapes param configuring

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Rename getROSParameter() to getParameter()

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Return back getMaskData() to nav2_costmap_2d

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Add composition node support

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

* Remove redundant methods

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update nav2_map_server/src/vo_server/vector_object_server.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Avoid shapes clearing

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Optimize switchMapUpdate() method

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Switch to vector of shapes

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Minor fixes

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Meet review comments

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Move isPointInside algorithm to nav2_util

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Testcases covering new functionality

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Fix linting issues

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Adjust for Vector Objects demonstration

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Code clean-up
* Corrected headers
* Functions ordering
* Comment fixes

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Additional code facelift
* Correct licensing years
* Fix Vector Object server dependencies
* Funcion rename for better readability
* Improve/fix comments

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>

* Minor fixing after rebase

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Rename vector object server

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Minor changes

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update tests

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Merge branch 'main' into feature/vector_object_server

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix merge issues and pre-commit checks

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Change tf2_ros headers from `.h` to `.hpp`

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix race condition in pub-sub of VO map

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Cleanup

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Remove use of ament_target_dependencies

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix review comments

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix linter errors

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix exception handling

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Update nav2_map_server/include/nav2_map_server/vector_object_utils.hpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_util/include/nav2_util/raytrace_line_2d.hpp

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Add error logs

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

* Fix cpplint

Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>

---------

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Sushant Chavan <sushant.chavan@idealworks.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Alberto Tudela <ajtudela@gmail.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 .../nav2_collision_monitor/polygon.hpp        |    7 -
 nav2_collision_monitor/src/polygon.cpp        |   35 +-
 .../include/nav2_costmap_2d/costmap_2d.hpp    |   98 --
 .../costmap_filters/costmap_filter.hpp        |   14 -
 .../plugins/costmap_filters/binary_filter.cpp |    5 +-
 .../costmap_filters/costmap_filter.cpp        |   23 -
 .../costmap_filters/keepout_filter.cpp        |   10 +-
 .../plugins/costmap_filters/speed_filter.cpp  |    5 +-
 nav2_costmap_2d/plugins/obstacle_layer.cpp    |    5 +-
 nav2_costmap_2d/src/costmap_2d.cpp            |    8 +-
 .../test/regression/CMakeLists.txt            |    6 -
 .../test/regression/costmap_bresenham_2d.cpp  |  159 --
 .../test/unit/costmap_filter_test.cpp         |   20 +-
 nav2_map_server/CMakeLists.txt                |   56 +-
 .../nav2_map_server/vector_object_server.hpp  |  228 +++
 .../nav2_map_server/vector_object_shapes.hpp  |  413 +++++
 .../nav2_map_server/vector_object_utils.hpp   |  142 ++
 .../launch/vector_object_server.launch.py     |  169 +++
 nav2_map_server/package.xml                   |    3 +
 .../params/vector_object_server_params.yaml   |   30 +
 nav2_map_server/src/vo_server/main.cpp        |   28 +
 .../src/vo_server/vector_object_server.cpp    |  558 +++++++
 .../src/vo_server/vector_object_shapes.cpp    |  559 +++++++
 nav2_map_server/test/unit/CMakeLists.txt      |   22 +
 .../test/unit/test_vector_object_server.cpp   | 1349 +++++++++++++++++
 .../test/unit/test_vector_object_shapes.cpp   |  805 ++++++++++
 nav2_msgs/CMakeLists.txt                      |    8 +-
 nav2_msgs/msg/CircleObject.msg                |    6 +
 nav2_msgs/msg/PolygonObject.msg               |    5 +
 nav2_msgs/package.xml                         |    1 +
 nav2_msgs/srv/AddShapes.srv                   |    6 +
 nav2_msgs/srv/GetShapes.srv                   |    5 +
 nav2_msgs/srv/RemoveShapes.srv                |    6 +
 .../include/nav2_util/geometry_utils.hpp      |   43 +
 .../include/nav2_util/occ_grid_utils.hpp      |  100 ++
 .../include/nav2_util/raytrace_line_2d.hpp    |  146 ++
 nav2_util/test/CMakeLists.txt                 |    2 +
 nav2_util/test/regression/CMakeLists.txt      |    3 +
 .../test/regression/map_bresenham_2d.cpp      |  170 +++
 39 files changed, 4890 insertions(+), 368 deletions(-)
 delete mode 100644 nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp
 create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_server.hpp
 create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp
 create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_utils.hpp
 create mode 100644 nav2_map_server/launch/vector_object_server.launch.py
 create mode 100644 nav2_map_server/params/vector_object_server_params.yaml
 create mode 100644 nav2_map_server/src/vo_server/main.cpp
 create mode 100644 nav2_map_server/src/vo_server/vector_object_server.cpp
 create mode 100644 nav2_map_server/src/vo_server/vector_object_shapes.cpp
 create mode 100644 nav2_map_server/test/unit/test_vector_object_server.cpp
 create mode 100644 nav2_map_server/test/unit/test_vector_object_shapes.cpp
 create mode 100644 nav2_msgs/msg/CircleObject.msg
 create mode 100644 nav2_msgs/msg/PolygonObject.msg
 create mode 100644 nav2_msgs/srv/AddShapes.srv
 create mode 100644 nav2_msgs/srv/GetShapes.srv
 create mode 100644 nav2_msgs/srv/RemoveShapes.srv
 create mode 100644 nav2_util/include/nav2_util/occ_grid_utils.hpp
 create mode 100644 nav2_util/include/nav2_util/raytrace_line_2d.hpp
 create mode 100644 nav2_util/test/regression/CMakeLists.txt
 create mode 100644 nav2_util/test/regression/map_bresenham_2d.cpp

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
index 3191de1727e..02fc680afe5 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -239,13 +239,6 @@ class Polygon
   rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
     std::vector<rclcpp::Parameter> parameters);
 
-  /**
-   * @brief Checks if point is inside polygon
-   * @param point Given point to check
-   * @return True if given point is inside polygon, otherwise false
-   */
-  bool isPointInside(const Point & point) const;
-
   /**
    * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...]
    * @param poly_string Input String containing the verteceis of the polygon
diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
index 5e13f5d5ef8..b37f677fb57 100644
--- a/nav2_collision_monitor/src/polygon.cpp
+++ b/nav2_collision_monitor/src/polygon.cpp
@@ -22,6 +22,7 @@
 #include "tf2/transform_datatypes.hpp"
 
 #include "nav2_ros_common/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/array_parser.hpp"
 
@@ -229,7 +230,7 @@ int Polygon::getPointsInside(const std::vector<Point> & points) const
 {
   int num = 0;
   for (const Point & point : points) {
-    if (isPointInside(point)) {
+    if (nav2_util::geometry_utils::isPointInsidePolygon(point.x, point.y, poly_)) {
       num++;
     }
   }
@@ -602,38 +603,6 @@ void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr
   updatePolygon(msg);
 }
 
-inline bool Polygon::isPointInside(const Point & point) const
-{
-  // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."
-  // Communications of the ACM 5.8 (1962): 434.
-  // Implementation of ray crossings algorithm for point in polygon task solving.
-  // Y coordinate is fixed. Moving the ray on X+ axis starting from given point.
-  // Odd number of intersections with polygon boundaries means the point is inside polygon.
-  const int poly_size = poly_.size();
-  int i, j;  // Polygon vertex iterators
-  bool res = false;  // Final result, initialized with already inverted value
-
-  // Starting from the edge where the last point of polygon is connected to the first
-  i = poly_size - 1;
-  for (j = 0; j < poly_size; j++) {
-    // Checking the edge only if given point is between edge boundaries by Y coordinates.
-    // One of the condition should contain equality in order to exclude the edges
-    // parallel to X+ ray.
-    if ((point.y <= poly_[i].y) == (point.y > poly_[j].y)) {
-      // Calculating the intersection coordinate of X+ ray
-      const double x_inter = poly_[i].x +
-        (point.y - poly_[i].y) * (poly_[j].x - poly_[i].x) /
-        (poly_[j].y - poly_[i].y);
-      // If intersection with checked edge is greater than point.x coordinate, inverting the result
-      if (x_inter > point.x) {
-        res = !res;
-      }
-    }
-    i = j;
-  }
-  return res;
-}
-
 bool Polygon::getPolygonFromString(
   std::string & poly_string,
   std::vector<Point> & polygon)
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 fbacf15aa24..fe3764c443a 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
@@ -461,105 +461,7 @@ class Costmap2D
    */
   virtual void initMaps(unsigned int size_x, unsigned int size_y);
 
-  /**
-   * @brief  Raytrace a line and apply some action at each step
-   * @param  at The action to take... a functor
-   * @param  x0 The starting x coordinate
-   * @param  y0 The starting y coordinate
-   * @param  x1 The ending x coordinate
-   * @param  y1 The ending y coordinate
-   * @param  max_length The maximum desired length of the segment...
-   * allows you to not go all the way to the endpoint
-   * @param  min_length The minimum desired length of the segment
-   */
-  template<class ActionType>
-  inline void raytraceLine(
-    ActionType at, unsigned int x0, unsigned int y0, unsigned int x1,
-    unsigned int y1,
-    unsigned int max_length = UINT_MAX, unsigned int min_length = 0)
-  {
-    int dx_full = x1 - x0;
-    int dy_full = y1 - y0;
-
-    // we need to chose how much to scale our dominant dimension,
-    // based on the maximum length of the line
-    double dist = std::hypot(dx_full, dy_full);
-    if (dist < min_length) {
-      return;
-    }
-
-    unsigned int min_x0, min_y0;
-    if (dist > 0.0) {
-      // Adjust starting point and offset to start from min_length distance
-      min_x0 = (unsigned int)(x0 + dx_full / dist * min_length);
-      min_y0 = (unsigned int)(y0 + dy_full / dist * min_length);
-    } else {
-      // dist can be 0 if [x0, y0]==[x1, y1].
-      // In this case only this cell should be processed.
-      min_x0 = x0;
-      min_y0 = y0;
-    }
-    unsigned int offset = min_y0 * size_x_ + min_x0;
-
-    int dx = x1 - min_x0;
-    int dy = y1 - min_y0;
-
-    unsigned int abs_dx = abs(dx);
-    unsigned int abs_dy = abs(dy);
-
-    int offset_dx = sign(dx);
-    int offset_dy = sign(dy) * size_x_;
-
-    double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
-    // if x is dominant
-    if (abs_dx >= abs_dy) {
-      int error_y = abs_dx / 2;
-
-      bresenham2D(
-        at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
-      return;
-    }
-
-    // otherwise y is dominant
-    int error_x = abs_dy / 2;
-
-    bresenham2D(
-      at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
-  }
-
 private:
-  /**
-   * @brief  A 2D implementation of Bresenham's raytracing algorithm...
-   * applies an action at each step
-   */
-  template<class ActionType>
-  inline void bresenham2D(
-    ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b,
-    int offset_a,
-    int offset_b, unsigned int offset,
-    unsigned int max_length)
-  {
-    unsigned int end = std::min(max_length, abs_da);
-    for (unsigned int i = 0; i < end; ++i) {
-      at(offset);
-      offset += offset_a;
-      error_b += abs_db;
-      if ((unsigned int)error_b >= abs_da) {
-        offset += offset_b;
-        error_b -= abs_da;
-      }
-    }
-    at(offset);
-  }
-
-  /**
-   * @brief get the sign of an int
-   */
-  inline int sign(int x)
-  {
-    return x > 0 ? 1.0 : -1.0;
-  }
-
   mutex_t * access_;
 
 protected:
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
index 25238cf8a35..da4720ff798 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
@@ -185,20 +185,6 @@ class CostmapFilter : public Layer
     const std::string mask_frame,
     geometry_msgs::msg::Pose & mask_pose) const;
 
-  /**
-   * @brief: Convert from world coordinates to mask coordinates.
-     Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s.
-   * @param  filter_mask Filter mask on which to convert
-   * @param  wx The x world coordinate
-   * @param  wy The y world coordinate
-   * @param  mx Will be set to the associated mask x coordinate
-   * @param  my Will be set to the associated mask y coordinate
-   * @return True if the conversion was successful (legal bounds) false otherwise
-   */
-  bool worldToMask(
-    nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
-    double wx, double wy, unsigned int & mx, unsigned int & my) const;
-
   /**
    * @brief  Get the data of a cell in the filter mask
    * @param  filter_mask Filter mask to get the data from
diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp
index 097537d40c6..c805a537396 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp
@@ -44,6 +44,7 @@
 
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 #include "nav2_util/occ_grid_values.hpp"
+#include "nav2_util/occ_grid_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -190,8 +191,8 @@ void BinaryFilter::process(
 
   // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
   unsigned int mask_robot_i, mask_robot_j;
-  if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
-    mask_robot_j))
+  if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y,
+    mask_robot_i, mask_robot_j))
   {
     // Robot went out of mask range. Set "false" state by-default
     RCLCPP_WARN(
diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
index 06d7ed2d025..58b89740b8b 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
@@ -193,29 +193,6 @@ bool CostmapFilter::transformPose(
   return true;
 }
 
-bool CostmapFilter::worldToMask(
-  nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
-  double wx, double wy, unsigned int & mx, unsigned int & my) const
-{
-  const double origin_x = filter_mask->info.origin.position.x;
-  const double origin_y = filter_mask->info.origin.position.y;
-  const double resolution = filter_mask->info.resolution;
-  const unsigned int size_x = filter_mask->info.width;
-  const unsigned int size_y = filter_mask->info.height;
-
-  if (wx < origin_x || wy < origin_y) {
-    return false;
-  }
-
-  mx = static_cast<unsigned int>((wx - origin_x) / resolution);
-  my = static_cast<unsigned int>((wy - origin_y) / resolution);
-  if (mx >= size_x || my >= size_y) {
-    return false;
-  }
-
-  return true;
-}
-
 unsigned char CostmapFilter::getMaskCost(
   nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
   const unsigned int mx, const unsigned int & my) const
diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
index 48cd6c74e42..c432a917e59 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
@@ -35,15 +35,17 @@
  * Author: Alexey Merzlyakov
  *********************************************************************/
 
+#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
+
 #include <string>
 #include <memory>
 #include <algorithm>
 #include "tf2/convert.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
-#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/occ_grid_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -200,8 +202,8 @@ void KeepoutFilter::updateBounds(
     geometry_msgs::msg::Pose mask_pose;
     if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
       unsigned int mask_robot_i, mask_robot_j;
-      if (worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
-        mask_robot_j))
+      if (nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y,
+        mask_robot_i, mask_robot_j))
       {
         auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
         is_pose_lethal_ = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
@@ -366,7 +368,7 @@ void KeepoutFilter::process(
         msk_wy = gl_wy;
       }
       // Get mask coordinates corresponding to (i, j) point at filter_mask_
-      if (worldToMask(filter_mask_, msk_wx, msk_wy, mx, my)) {
+      if (nav2_util::worldToMap(filter_mask_, msk_wx, msk_wy, mx, my)) {
         data = getMaskCost(filter_mask_, mx, my);
         // Update if mask_ data is valid and greater than existing master_grid's one
         if (data == NO_INFORMATION) {
diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp
index a66886bc6d0..c1e822cb03a 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp
@@ -43,6 +43,7 @@
 #include <string>
 
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_util/occ_grid_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -198,8 +199,8 @@ void SpeedFilter::process(
 
   // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
   unsigned int mask_robot_i, mask_robot_j;
-  if (!worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
-    mask_robot_j))
+  if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y,
+    mask_robot_i, mask_robot_j))
   {
     return;
   }
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index b897ac515be..c09d60da61b 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -45,7 +45,9 @@
 
 #include "pluginlib/class_list_macros.hpp"
 #include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "nav2_util/raytrace_line_2d.hpp"
 #include "nav2_costmap_2d/costmap_math.hpp"
+#include "nav2_ros_common/node_utils.hpp"
 #include "rclcpp/version.h"
 
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer)
@@ -768,7 +770,8 @@ ObstacleLayer::raytraceFreespace(
     unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_);
     MarkCell marker(costmap_, FREE_SPACE);
     // and finally... we can execute our trace to clear obstacles along that line
-    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_max_range, cell_raytrace_min_range);
+    nav2_util::raytraceLine(
+      marker, x0, y0, x1, y1, size_x_, cell_raytrace_max_range, cell_raytrace_min_range);
 
     updateRaytraceBounds(
       ox, oy, wx, wy, clearing_observation.raytrace_max_range_,
diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp
index f3903308cff..28798b0f8a3 100644
--- a/nav2_costmap_2d/src/costmap_2d.cpp
+++ b/nav2_costmap_2d/src/costmap_2d.cpp
@@ -43,6 +43,7 @@
 #include <vector>
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_util/occ_grid_values.hpp"
+#include "nav2_util/raytrace_line_2d.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -462,14 +463,15 @@ void Costmap2D::polygonOutlineCells(
 {
   PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
   for (unsigned int i = 0; i < polygon.size() - 1; ++i) {
-    raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
+    nav2_util::raytraceLine(
+      cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y, size_x_);
   }
   if (!polygon.empty()) {
     unsigned int last_index = polygon.size() - 1;
     // we also need to close the polygon by going from the last point to the first
-    raytraceLine(
+    nav2_util::raytraceLine(
       cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x,
-      polygon[0].y);
+      polygon[0].y, size_x_);
   }
 }
 
diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt
index d468dd37987..435d9072d03 100644
--- a/nav2_costmap_2d/test/regression/CMakeLists.txt
+++ b/nav2_costmap_2d/test/regression/CMakeLists.txt
@@ -1,9 +1,3 @@
-# Bresenham2D corner cases test
-ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp)
-target_link_libraries(costmap_bresenham_2d
-  nav2_costmap_2d_core
-)
-
 # OrderLayer for checking Costmap2D plugins API calling order
 add_library(order_layer SHARED order_layer.cpp)
 target_link_libraries(order_layer PUBLIC
diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp
deleted file mode 100644
index c4afee34e00..00000000000
--- a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp
+++ /dev/null
@@ -1,159 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2022 Samsung Research Russia
-*  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 Willow Garage, Inc. 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 OWNER 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.
-*
-* Author: Alexey Merzlyakov
-*********************************************************************/
-#include <nav2_costmap_2d/costmap_2d.hpp>
-#include <gtest/gtest.h>
-
-class CostmapAction
-{
-public:
-  explicit CostmapAction(
-    unsigned char * costmap, unsigned int size, unsigned char mark_val = 128)
-  : costmap_(costmap), size_(size), mark_val_(mark_val)
-  {
-  }
-
-  inline void operator()(unsigned int off)
-  {
-    ASSERT_TRUE(off < size_);
-    costmap_[off] = mark_val_;
-  }
-
-  inline unsigned int get(unsigned int off)
-  {
-    return costmap_[off];
-  }
-
-private:
-  unsigned char * costmap_;
-  unsigned int size_;
-  unsigned char mark_val_;
-};
-
-class CostmapTest : public nav2_costmap_2d::Costmap2D
-{
-public:
-  CostmapTest(
-    unsigned int size_x, unsigned int size_y, double resolution,
-    double origin_x, double origin_y, unsigned char default_val = 0)
-  : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val)
-  {
-  }
-
-  unsigned char * getCostmap()
-  {
-    return costmap_;
-  }
-
-  unsigned int getSize()
-  {
-    return size_x_ * size_y_;
-  }
-
-  void raytraceLine(
-    CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1,
-    unsigned int y1,
-    unsigned int max_length = UINT_MAX, unsigned int min_length = 0)
-  {
-    nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
-  }
-};
-
-TEST(costmap_2d, bresenham2DBoundariesCheck)
-{
-  const unsigned int sz_x = 60;
-  const unsigned int sz_y = 60;
-  const unsigned int max_length = 60;
-  const unsigned int min_length = 6;
-  CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0);
-  CostmapAction ca(ct.getCostmap(), ct.getSize());
-
-  // Initial point - some asymmetrically standing point in order to cover most corner cases
-  const unsigned int x0 = 2;
-  const unsigned int y0 = 4;
-  // (x1, y1) point will move
-  unsigned int x1, y1;
-
-  // Running on (x, 0) edge
-  y1 = 0;
-  for (x1 = 0; x1 < sz_x; x1++) {
-    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
-  }
-
-  // Running on (x, sz_y) edge
-  y1 = sz_y - 1;
-  for (x1 = 0; x1 < sz_x; x1++) {
-    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
-  }
-
-  // Running on (0, y) edge
-  x1 = 0;
-  for (y1 = 0; y1 < sz_y; y1++) {
-    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
-  }
-
-  // Running on (sz_x, y) edge
-  x1 = sz_x - 1;
-  for (y1 = 0; y1 < sz_y; y1++) {
-    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
-  }
-}
-
-TEST(costmap_2d, bresenham2DSamePoint)
-{
-  const unsigned int sz_x = 60;
-  const unsigned int sz_y = 60;
-  const unsigned int max_length = 60;
-  const unsigned int min_length = 0;
-  CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0);
-  CostmapAction ca(ct.getCostmap(), ct.getSize());
-
-  // Initial point
-  const double x0 = 2;
-  const double y0 = 4;
-
-  unsigned int offset = y0 * sz_x + x0;
-  unsigned char val_before = ca.get(offset);
-  // Same point to check
-  ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length);
-  unsigned char val_after = ca.get(offset);
-  ASSERT_FALSE(val_before == val_after);
-}
-
-int main(int argc, char ** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
diff --git a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp
index 320eeaa1653..8cf4583d15d 100644
--- a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp
@@ -19,6 +19,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/occ_grid_values.hpp"
+#include "nav2_util/occ_grid_utils.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
@@ -30,13 +31,6 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter
 public:
   CostmapFilterWrapper() {}
 
-  bool worldToMask(
-    nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
-    double wx, double wy, unsigned int & mx, unsigned int & my) const
-  {
-    return nav2_costmap_2d::CostmapFilter::worldToMask(filter_mask, wx, wy, mx, my);
-  }
-
   unsigned char getMaskCost(
     nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
     const unsigned int mx, const unsigned int & my) const
@@ -52,7 +46,7 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter
   void resetFilter() {}
 };
 
-TEST(CostmapFilter, testWorldToMask)
+TEST(CostmapFilter, testWorldToMap)
 {
   // Create occupancy grid for test as follows:
   //
@@ -82,19 +76,19 @@ TEST(CostmapFilter, testWorldToMask)
   CostmapFilterWrapper cf;
   unsigned int mx, my;
   // Point inside mask
-  ASSERT_TRUE(cf.worldToMask(mask, 4.0, 5.0, mx, my));
+  ASSERT_TRUE(nav2_util::worldToMap(mask, 4.0, 5.0, mx, my));
   ASSERT_EQ(mx, 1u);
   ASSERT_EQ(my, 2u);
   // Corner cases
-  ASSERT_TRUE(cf.worldToMask(mask, 3.0, 3.0, mx, my));
+  ASSERT_TRUE(nav2_util::worldToMap(mask, 3.0, 3.0, mx, my));
   ASSERT_EQ(mx, 0u);
   ASSERT_EQ(my, 0u);
-  ASSERT_TRUE(cf.worldToMask(mask, 5.9, 5.9, mx, my));
+  ASSERT_TRUE(nav2_util::worldToMap(mask, 5.9, 5.9, mx, my));
   ASSERT_EQ(mx, 2u);
   ASSERT_EQ(my, 2u);
   // Point outside mask
-  ASSERT_FALSE(cf.worldToMask(mask, 2.9, 2.9, mx, my));
-  ASSERT_FALSE(cf.worldToMask(mask, 6.0, 6.0, mx, my));
+  ASSERT_FALSE(nav2_util::worldToMap(mask, 2.9, 2.9, mx, my));
+  ASSERT_FALSE(nav2_util::worldToMap(mask, 6.0, 6.0, mx, my));
 }
 
 TEST(CostmapFilter, testGetMaskCost)
diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
index a003a8fb087..6459b42ed2c 100644
--- a/nav2_map_server/CMakeLists.txt
+++ b/nav2_map_server/CMakeLists.txt
@@ -5,21 +5,26 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules)
 
 find_package(ament_cmake REQUIRED)
 find_package(backward_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(GRAPHICSMAGICKCPP REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(PkgConfig REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
+pkg_search_module(UUID REQUIRED uuid)
+
 nav2_package()
 
 set(map_server_executable map_server)
@@ -28,12 +33,16 @@ set(library_name ${map_server_executable}_core)
 
 set(map_io_library_name map_io)
 
+set(vo_library_name vector_object_core)
+
 set(map_saver_cli_executable map_saver_cli)
 
 set(map_saver_server_executable map_saver_server)
 
 set(costmap_filter_info_server_executable costmap_filter_info_server)
 
+set(vo_server_executable vector_object_server)
+
 add_library(${map_io_library_name} SHARED
   src/map_mode.cpp
   src/map_io.cpp)
@@ -78,6 +87,31 @@ target_link_libraries(${library_name} PRIVATE
   yaml-cpp::yaml-cpp
 )
 
+add_library(${vo_library_name} SHARED
+  src/vo_server/vector_object_shapes.cpp
+  src/vo_server/vector_object_server.cpp)
+target_include_directories(${vo_library_name}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+target_link_libraries(${vo_library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  ${nav_msgs_TARGETS}
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  rclcpp_components::component
+  tf2_ros::tf2_ros
+  ${UUID_LIBRARIES}
+)
+target_link_libraries(${library_name} PRIVATE
+  ${lifecycle_msgs_TARGETS}
+  rclcpp_components::component
+  yaml-cpp::yaml-cpp
+)
+
 add_executable(${map_server_executable}
   src/map_server/main.cpp)
 target_include_directories(${map_server_executable}
@@ -142,12 +176,28 @@ target_link_libraries(${costmap_filter_info_server_executable} PRIVATE
   rclcpp_lifecycle::rclcpp_lifecycle
 )
 
+add_executable(${vo_server_executable}
+  src/vo_server/main.cpp)
+target_include_directories(${vo_server_executable}
+  PRIVATE
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${vo_server_executable} PRIVATE
+  ${vo_library_name}
+  ${nav_msgs_TARGETS}
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+)
+
 rclcpp_components_register_nodes(${library_name} "nav2_map_server::CostmapFilterInfoServer")
 rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapSaver")
 rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapServer")
+rclcpp_components_register_nodes(${vo_library_name} "nav2_map_server::VectorObjectServer")
 
 install(TARGETS
-    ${library_name} ${map_io_library_name}
+    ${library_name} ${map_io_library_name} ${vo_library_name}
   EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -155,13 +205,14 @@ install(TARGETS
 
 install(TARGETS
     ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable}
-    ${costmap_filter_info_server_executable}
+    ${costmap_filter_info_server_executable} ${vo_server_executable}
   RUNTIME DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY include/
   DESTINATION include/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -178,6 +229,7 @@ ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_libraries(
   ${library_name}
   ${map_io_library_name}
+  ${vo_library_name}
 )
 ament_export_dependencies(nav_msgs nav2_msgs nav2_util rclcpp rclcpp_lifecycle nav2_ros_common)
 ament_export_targets(${library_name})
diff --git a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp
new file mode 100644
index 00000000000..ed4186722bc
--- /dev/null
+++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp
@@ -0,0 +1,228 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// 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_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_
+#define NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+
+#include "nav2_msgs/srv/add_shapes.hpp"
+#include "nav2_msgs/srv/remove_shapes.hpp"
+#include "nav2_msgs/srv/get_shapes.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+
+#include "nav2_map_server/vector_object_utils.hpp"
+#include "nav2_map_server/vector_object_shapes.hpp"
+
+namespace nav2_map_server
+{
+
+/// @brief Vector Object server node
+class VectorObjectServer : public nav2::LifecycleNode
+{
+public:
+  /**
+   * @brief Constructor for the VectorObjectServer
+   * @param options Additional options to control creation of the node.
+   */
+  explicit VectorObjectServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+protected:
+  /**
+   * @brief: Initializes TF buffer/listener, obtains ROS-parameters, creates incoming services,
+   * and output map publisher
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Activates output map publisher and creates bond connection
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Deactivates map publisher and timer (if any), destroys bond connection
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Resets all services, publishers, map and TF-s
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called in shutdown state
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Supporting routine obtaining all ROS-parameters
+   * @return True if all parameters were obtained or false in the failure case
+   */
+  bool obtainParams();
+
+  /**
+   * @brief Finds the shape with given UUID
+   * @param uuid Given UUID to search with
+   * @return Iterator to the shape, if found. Otherwise past-the-end iterator.
+   */
+  std::vector<std::shared_ptr<Shape>>::iterator findShape(const unsigned char * uuid);
+
+  /**
+   * @brief Transform all vector shapes from their local frame to output map frame
+   * @return Whether all vector objects were transformed successfully
+   */
+  bool transformVectorObjects();
+
+  /**
+   * @brief Obtains map boundaries to place all vector objects inside
+   * @param min_x output min X-boundary of required map
+   * @param min_y output min Y-boundary of required map
+   * @param max_x output max X-boundary of required map
+   * @param max_y output max Y-boundary of required map
+   * @throw std::exception if can not obtain one of the map boundaries
+   */
+  void getMapBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) const;
+
+  /**
+   * @brief Creates or updates existing map with required sizes and fills it with default value
+   * @param min_x min X-boundary of new map
+   * @param min_y min Y-boundary of new map
+   * @param max_x max X-boundary of new map
+   * @param max_y max Y-boundary of new map
+   * @throw std::exception if map has negative X or Y size
+   */
+  void updateMap(
+    const double & min_x, const double & min_y, const double & max_x, const double & max_y);
+
+  /**
+   * @brief Processes all vector objects on raster output map
+   */
+  void putVectorObjectsOnMap();
+
+  /**
+   * @brief Publishes output map
+   */
+  void publishMap();
+
+  /**
+   * @brief Calculates new map sizes, updates map, processes all vector objects on it
+   * and publishes output map one time
+   */
+  void processMap();
+
+  /**
+   * @brief If map to be update dynamically, creates map processing timer,
+   * otherwise process map once
+   */
+  void switchMapUpdate();
+
+  /**
+   * @brief Callback for AddShapes service call.
+   * Reads all input vector objects from service request,
+   * creates them or updates their shape in case of existing objects
+   * and switches map processing/publishing routine
+   * @param request_header Service request header
+   * @param request Service request
+   * @param response Service response
+   */
+  void addShapesCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::AddShapes::Request> request,
+    std::shared_ptr<nav2_msgs::srv::AddShapes::Response> response);
+
+  /**
+   * @brief Callback for GetShapes service call.
+   * Gets all shapes and returns them to the service response
+   * @param request_header Service request header
+   * @param request Service request
+   * @param response Service response
+   */
+  void getShapesCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::GetShapes::Request> request,
+    std::shared_ptr<nav2_msgs::srv::GetShapes::Response> response);
+
+  /**
+   * @brief Callback for RemoveShapes service call.
+   * Try to remove requested vector objects and switches map processing/publishing routine
+   * @param request_header Service request header
+   * @param request Service request
+   * @param response Service response
+   */
+  void removeShapesCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::RemoveShapes::Request> request,
+    std::shared_ptr<nav2_msgs::srv::RemoveShapes::Response> response);
+
+protected:
+  /// @brief TF buffer
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  /// @brief TF listener
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  /// @brief All shapes vector
+  std::vector<std::shared_ptr<Shape>> shapes_;
+
+  /// @brief Output map resolution
+  double resolution_;
+  /// @brief Default value the output map to be filled with
+  int8_t default_value_;
+  /// @brief @Overlay Type of overlay of vector objects on the map
+  OverlayType overlay_type_;
+
+  /// @brief Output map with vector objects on it
+  nav_msgs::msg::OccupancyGrid::SharedPtr map_;
+  /// @brief Whether to process and publish map
+  double process_map_;
+
+  /// @brief Frame of output map
+  std::string global_frame_id_;
+  /// @brief Transform tolerance
+  double transform_tolerance_;
+
+  /// @brief Frequency to dynamically update/publish the map (if necessary)
+  double update_frequency_;
+  /// @brief Map update timer
+  rclcpp::TimerBase::SharedPtr map_timer_;
+
+  /// @brief AddShapes service
+  nav2::ServiceServer<nav2_msgs::srv::AddShapes>::SharedPtr add_shapes_service_;
+  /// @brief GetShapes service
+  nav2::ServiceServer<nav2_msgs::srv::GetShapes>::SharedPtr get_shapes_service_;
+  /// @brief RemoveShapes service
+  nav2::ServiceServer<nav2_msgs::srv::RemoveShapes>::SharedPtr remove_shapes_service_;
+
+  /// @beirf Topic name where the output map to be published to
+  std::string map_topic_;
+  /// @brief Output map publisher
+  nav2::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
+};
+
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_
diff --git a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp
new file mode 100644
index 00000000000..dd5cae95b62
--- /dev/null
+++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp
@@ -0,0 +1,413 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// 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_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_
+#define NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/polygon.hpp"
+#include "geometry_msgs/msg/point32.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+#include "tf2_ros/buffer.hpp"
+
+#include "nav2_msgs/msg/polygon_object.hpp"
+#include "nav2_msgs/msg/circle_object.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+
+#include "nav2_map_server/vector_object_utils.hpp"
+
+namespace nav2_map_server
+{
+
+/// @brief Possible vector object shape types
+enum ShapeType
+{
+  UNKNOWN = 0,
+  POLYGON = 1,
+  CIRCLE = 2
+};
+
+/// @brief Basic class, other vector objects to be inherited from
+class Shape
+{
+public:
+  /**
+   * @brief Shape basic class constructor
+   * @param node Vector Object server node pointer
+   */
+  explicit Shape(const nav2::LifecycleNode::WeakPtr & node);
+
+  /**
+   * @brief Shape destructor
+   */
+  virtual ~Shape();
+
+  /**
+   * @brief Returns type of the shape
+   * @return Type of the shape
+   */
+  ShapeType getType();
+
+  /**
+   * @brief Supporting routine obtaining shape UUID from ROS-parameters
+   * for the given shape object
+   * @return True if UUID was obtained or false in failure case
+   */
+  bool obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid);
+
+  /**
+   * @brief Gets the value of the shape.
+   * Empty virtual method intended to be used in child implementations
+   * @return OccupancyGrid value of the shape
+   */
+  virtual int8_t getValue() const = 0;
+
+  /**
+   * @brief Gets frame ID of the shape.
+   * Empty virtual method intended to be used in child implementations
+   * @return Frame ID where the shape is
+   */
+  virtual std::string getFrameID() const = 0;
+
+  /**
+   * @brief Gets UUID of the shape.
+   * Empty virtual method intended to be used in child implementations
+   * @return Shape UUID string
+   */
+  virtual std::string getUUID() const = 0;
+
+  /**
+   * @brief Checks whether the shape is equal to a given UUID.
+   * Empty virtual method intended to be used in child implementations
+   * @param uuid Given UUID to check with
+   * @return True if the shape has the same as given UUID, otherwise false
+   */
+  virtual bool isUUID(const unsigned char * uuid) const = 0;
+
+  /**
+   * @brief Whether the shape to be filled or only its borders to be put on map.
+   * Empty virtual method intended to be used in child implementations
+   * @return True if shape to be filled
+   */
+  virtual bool isFill() const = 0;
+
+  /**
+   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
+   * Empty virtual method intended to be used in child implementations
+   * @param shape_name Name of the shape
+   * @return True if all parameters were obtained or false in failure case
+   */
+  virtual bool obtainParams(const std::string & shape_name) = 0;
+
+  /**
+   * @brief Transforms shape coordinates to a new frame.
+   * Empty virtual method intended to be used in child implementations
+   * @param to_frame Frame ID to transform to
+   * @param tf_buffer TF buffer to use for the transformation
+   * @param transform_tolerance Transform tolerance
+   * @return Whether it was transformed successfully
+   */
+  virtual bool toFrame(
+    const std::string & to_frame,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    const double transform_tolerance) = 0;
+
+  /**
+   * @brief Gets shape box-boundaries.
+   * Empty virtual method intended to be used in child implementations
+   * @param min_x output min X-boundary of shape
+   * @param min_y output min Y-boundary of shape
+   * @param max_x output max X-boundary of shape
+   * @param max_y output max Y-boundary of shape
+   */
+  virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0;
+
+  /**
+   * @brief Is the point inside the shape.
+   * Empty virtual method intended to be used in child implementations
+   * @param px X-coordinate of the given point to check
+   * @param py Y-coordinate of the given point to check
+   * @return True if given point inside the shape
+   */
+  virtual bool isPointInside(const double px, const double py) const = 0;
+
+  /**
+   * @brief Puts shape borders on map.
+   * Empty virtual method intended to be used in child implementations
+   * @param map Output map pointer
+   * @param overlay_type Overlay type
+   */
+  virtual void putBorders(
+    nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0;
+
+protected:
+  /// @brief Type of shape
+  ShapeType type_;
+
+  /// @brief VectorObjectServer node
+  nav2::LifecycleNode::WeakPtr node_;
+};
+
+/// @brief Polygon shape class
+class Polygon : public Shape
+{
+public:
+  /*
+   * @brief Polygon class constructor
+   * @param node Vector Object server node pointer
+   * @note setParams()/obtainParams() should be called after to configure the shape
+   */
+  explicit Polygon(const nav2::LifecycleNode::WeakPtr & node);
+
+  /**
+   * @brief Gets the value of the shape.
+   * @return OccupancyGrid value of the shape
+   */
+  int8_t getValue() const;
+
+  /**
+   * @brief Gets frame ID of the shape.
+   * @return Frame ID where the shape is
+   */
+  std::string getFrameID() const;
+
+  /**
+   * @brief Gets UUID of the shape.
+   * @return Shape UUID string
+   */
+  std::string getUUID() const;
+
+  /**
+   * @brief Checks whether the shape is equal to a given UUID.
+   * @param uuid Given UUID to check with
+   * @return True if the shape has the same as given UUID, otherwise false
+   */
+  bool isUUID(const unsigned char * uuid) const;
+
+  /**
+   * @brief Whether the shape to be filled or only its borders to be put on map.
+   * @return True if shape to be filled
+   */
+  bool isFill() const;
+
+  /**
+   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
+   * @param shape_name Name of the shape
+   * @return True if all parameters were obtained or false in failure case
+   */
+  bool obtainParams(const std::string & shape_name);
+
+  /**
+   * @brief Gets Polygon parameters
+   * @return Polygon parameters
+   */
+  nav2_msgs::msg::PolygonObject::SharedPtr getParams() const;
+
+  /**
+   * @brief Tries to update Polygon parameters
+   * @return False in case of inconsistent shape
+   */
+  bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params);
+
+  /**
+   * @brief Transforms shape coordinates to a new frame.
+   * @param to_frame Frame ID to transform to
+   * @param tf_buffer TF buffer to use for the transformation
+   * @param transform_tolerance Transform tolerance
+   * @return Whether it was transformed successfully
+   */
+  bool toFrame(
+    const std::string & to_frame,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    const double transform_tolerance);
+
+  /**
+   * @brief Gets shape box-boundaries
+   * @param min_x output min X-boundary of shape
+   * @param min_y output min Y-boundary of shape
+   * @param max_x output max X-boundary of shape
+   * @param max_y output max Y-boundary of shape
+   */
+  void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y);
+
+  /**
+   * @brief Is the point inside the shape.
+   * @param px X-coordinate of the given point to check
+   * @param py Y-coordinate of the given point to check
+   * @return True if given point inside the shape
+   */
+  bool isPointInside(const double px, const double py) const;
+
+  /**
+   * @brief Puts shape borders on map.
+   * @param map Output map pointer
+   * @param overlay_type Overlay type
+   */
+  void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type);
+
+protected:
+  /**
+   * @brief Checks that shape is consistent for further operation
+   * @return False in case of inconsistent shape
+   */
+  bool checkConsistency();
+
+  /// @brief Input polygon parameters (could be in any frame)
+  nav2_msgs::msg::PolygonObject::SharedPtr params_;
+  /// @brief Polygon in the map's frame
+  geometry_msgs::msg::Polygon::SharedPtr polygon_;
+};
+
+/// @brief Circle shape class
+class Circle : public Shape
+{
+public:
+  /*
+   * @brief Circle class constructor
+   * @param node Vector Object server node pointer
+   * @note setParams()/obtainParams() should be called after to configure the shape
+   */
+  explicit Circle(const nav2::LifecycleNode::WeakPtr & node);
+
+  /**
+   * @brief Gets the value of the shape.
+   * @return OccupancyGrid value of the shape
+   */
+  int8_t getValue() const;
+
+  /**
+   * @brief Gets frame ID of the shape.
+   * @return Frame ID where the shape is
+   */
+  std::string getFrameID() const;
+
+  /**
+   * @brief Gets UUID of the shape.
+   * @return Shape UUID string
+   */
+  std::string getUUID() const;
+
+  /**
+   * @brief Checks whether the shape is equal to a given UUID.
+   * @param uuid Given UUID to check with
+   * @return True if the shape has the same as given UUID, otherwise false
+   */
+  bool isUUID(const unsigned char * uuid) const;
+
+  /**
+   * @brief Whether the shape to be filled or only its borders to be put on map.
+   * @return True if shape to be filled
+   */
+  bool isFill() const;
+
+  /**
+   * @brief Supporting routine obtaining ROS-parameters for the given vector object.
+   * @param shape_name Name of the shape
+   * @return True if all parameters were obtained or false in failure case
+   */
+  bool obtainParams(const std::string & shape_name);
+
+  /**
+   * @brief Gets Circle parameters
+   * @return Circle parameters
+   */
+  nav2_msgs::msg::CircleObject::SharedPtr getParams() const;
+
+  /**
+   * @brief Tries to update Circle parameters
+   * @return False in case of inconsistent shape
+   */
+  bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params);
+
+  /**
+   * @brief Transforms shape coordinates to a new frame.
+   * @param to_frame Frame ID to transform to
+   * @param tf_buffer TF buffer to use for the transformation
+   * @param transform_tolerance Transform tolerance
+   * @return Whether it was transformed successfully
+   */
+  bool toFrame(
+    const std::string & to_frame,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    const double transform_tolerance);
+
+  /**
+   * @brief Gets shape box-boundaries
+   * @param min_x output min X-boundary of shape
+   * @param min_y output min Y-boundary of shape
+   * @param max_x output max X-boundary of shape
+   * @param max_y output max Y-boundary of shape
+   */
+  void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y);
+
+  /**
+   * @brief Is the point inside the shape.
+   * @param px X-coordinate of the given point to check
+   * @param py Y-coordinate of the given point to check
+   * @return True if given point inside the shape
+   */
+  bool isPointInside(const double px, const double py) const;
+
+  /**
+   * @brief Puts shape borders on map.
+   * @param map Output map pointer
+   * @param overlay_type Overlay type
+   */
+  void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type);
+
+protected:
+  /**
+   * @brief Checks that shape is consistent for further operation
+   * @return False in case of inconsistent shape
+   */
+  bool checkConsistency();
+
+  /**
+   * @brief Converts circle center to map coordinates
+   * considering FP-accuracy losing on small values when using conventional
+   * nav2_util::worldToMap() approach
+   * @param map Map pointer
+   * @param mcx Output X-coordinate of associated circle center on map
+   * @param mcy Output Y-coordinate of associated circle center on map
+   * @return True if the conversion was successful
+   */
+  bool centerToMap(
+    nav_msgs::msg::OccupancyGrid::ConstSharedPtr map,
+    unsigned int & mcx, unsigned int & mcy);
+
+  /**
+   * @brief Put Circle's point on map
+   * @param mx X-coordinate of given point
+   * @param my Y-coordinate of given point
+   * @param map Output map pointer
+   * @param overlay_type Overlay type
+   */
+  void putPoint(
+    unsigned int mx, unsigned int my,
+    nav_msgs::msg::OccupancyGrid::SharedPtr map,
+    const OverlayType overlay_type);
+
+  /// @brief Input circle parameters (could be in any frame)
+  nav2_msgs::msg::CircleObject::SharedPtr params_;
+  /// @brief Circle center in the map's frame
+  geometry_msgs::msg::Point32::SharedPtr center_;
+};
+
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_
diff --git a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp
new file mode 100644
index 00000000000..f478f71b3ea
--- /dev/null
+++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp
@@ -0,0 +1,142 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// 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_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_
+#define NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_
+
+#include <uuid/uuid.h>
+#include <stdexcept>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_ros_common/node_utils.hpp"
+#include "nav2_util/occ_grid_values.hpp"
+
+namespace nav2_map_server
+{
+
+// ---------- Working with UUID-s ----------
+
+/**
+ * @brief Converts UUID from input array to unparsed string
+ * @param uuid Input UUID in array format
+ * @return Unparsed UUID string
+ */
+inline std::string unparseUUID(const unsigned char * uuid)
+{
+  char uuid_str[37];
+  uuid_unparse(uuid, uuid_str);
+  return std::string(uuid_str);
+}
+
+// ---------- Working with shapes' overlays ----------
+
+/// @brief Type of overlay between different vector objects and map
+enum class OverlayType : uint8_t
+{
+  OVERLAY_SEQ = 0,  // Vector objects are superimposed in the order in which they have arrived
+  OVERLAY_MAX = 1,  // Maximum value from vector objects and map is being chosen
+  OVERLAY_MIN = 2   // Minimum value from vector objects and map is being chosen
+};
+
+/**
+ * @brief Updates map value with shape's one according to the given overlay type
+ * @param map_val Map value. To be updated with new value if overlay is involved
+ * @param shape_val Vector object value to be overlaid on map
+ * @param overlay_type Type of overlay
+ * @throw std::exception in case of unknown overlay type
+ */
+inline void processVal(
+  int8_t & map_val, const int8_t shape_val,
+  const OverlayType overlay_type)
+{
+  switch (overlay_type) {
+    case OverlayType::OVERLAY_SEQ:
+      map_val = shape_val;
+      return;
+    case OverlayType::OVERLAY_MAX:
+      if (shape_val > map_val) {
+        map_val = shape_val;
+      }
+      return;
+    case OverlayType::OVERLAY_MIN:
+      if ((map_val == nav2_util::OCC_GRID_UNKNOWN || shape_val < map_val) &&
+        shape_val != nav2_util::OCC_GRID_UNKNOWN)
+      {
+        map_val = shape_val;
+      }
+      return;
+    default:
+      throw std::runtime_error{"Unknown overlay type"};
+  }
+}
+
+/**
+ * @brief Updates the cell on the map with given shape value according to the given overlay type
+ * @param map Output map to be updated with
+ * @param offset Offset to the cell to be updated
+ * @param shape_val Vector object value to be updated map with
+ * @param overlay_type Type of overlay
+ */
+inline void processCell(
+  nav_msgs::msg::OccupancyGrid::SharedPtr map,
+  const unsigned int offset,
+  const int8_t shape_val,
+  const OverlayType overlay_type)
+{
+  int8_t map_val = map->data[offset];
+  processVal(map_val, shape_val, overlay_type);
+  map->data[offset] = map_val;
+}
+
+/// @brief Functor class used in raytraceLine algorithm
+class MapAction
+{
+public:
+  /**
+   * @brief MapAction constructor
+   * @param map Pointer to output map
+   * @param value Value to put on map
+   * @param overlay_type Overlay type
+   */
+  MapAction(
+    nav_msgs::msg::OccupancyGrid::SharedPtr map,
+    int8_t value, OverlayType overlay_type)
+  : map_(map), value_(value), overlay_type_(overlay_type)
+  {}
+
+  /**
+   * @brief Map' cell updating operator
+   * @param offset Offset on the map where the cell to be changed
+   */
+  inline void operator()(unsigned int offset)
+  {
+    processCell(map_, offset, value_, overlay_type_);
+  }
+
+protected:
+  /// @brief Output map pointer
+  nav_msgs::msg::OccupancyGrid::SharedPtr map_;
+  /// @brief Value to put on map
+  int8_t value_;
+  /// @brief Overlay type
+  OverlayType overlay_type_;
+};
+
+}  // namespace nav2_map_server
+
+#endif  // NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_
diff --git a/nav2_map_server/launch/vector_object_server.launch.py b/nav2_map_server/launch/vector_object_server.launch.py
new file mode 100644
index 00000000000..f3c2a20ff1b
--- /dev/null
+++ b/nav2_map_server/launch/vector_object_server.launch.py
@@ -0,0 +1,169 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2023 Samsung R&D Institute Russia
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PythonExpression
+from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter
+from launch_ros.descriptions import ComposableNode, ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description() -> LaunchDescription:
+    # Environment
+    package_dir = get_package_share_directory('nav2_map_server')
+
+    # Constant parameters
+    lifecycle_nodes = ['vector_object_server', 'costmap_filter_info_server']
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    # Launch arguments
+    # 1. Create the launch configuration variables
+    namespace = LaunchConfiguration('namespace')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    params_file = LaunchConfiguration('params_file')
+    autostart = LaunchConfiguration('autostart')
+    use_composition = LaunchConfiguration('use_composition')
+    container_name = LaunchConfiguration('container_name')
+
+    # 2. Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='True',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(package_dir, 'params', 'vector_object_server_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='True',
+        description='Automatically startup Vector Object server')
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='True',
+        description='Use composed bringup if True')
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name', default_value='nav2_container',
+        description='the name of container that nodes will load in if use composition')
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites={},
+            convert_types=True,
+        ),
+        allow_substs=True,
+    )
+
+    # Declare node launching commands
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            SetParameter('use_sim_time', use_sim_time),
+            PushROSNamespace(
+                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
+                namespace=namespace),
+            Node(
+                package='nav2_map_server',
+                executable='vector_object_server',
+                name='vector_object_server',
+                output='screen',
+                emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+                parameters=[configured_params],
+                remappings=remappings),
+            Node(
+                package='nav2_map_server',
+                executable='costmap_filter_info_server',
+                name='costmap_filter_info_server',
+                output='screen',
+                emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+                parameters=[configured_params],
+                remappings=remappings),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_vo_server',
+                output='screen',
+                emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+                parameters=[{'autostart': autostart},
+                            {'node_names': lifecycle_nodes}],
+                remappings=remappings)
+        ]
+    )
+
+    load_composable_nodes = GroupAction(
+        condition=IfCondition(use_composition),
+        actions=[
+            SetParameter('use_sim_time', use_sim_time),
+            PushROSNamespace(
+                condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
+                namespace=namespace),
+            LoadComposableNodes(
+                target_container=container_name,
+                composable_node_descriptions=[
+                    ComposableNode(
+                        package='nav2_map_server',
+                        plugin='nav2_map_server::VectorObjectServer',
+                        name='vector_object_server',
+                        parameters=[configured_params],
+                        remappings=remappings),
+                    ComposableNode(
+                        package='nav2_map_server',
+                        plugin='nav2_map_server::CostmapFilterInfoServer',
+                        name='costmap_filter_info_server',
+                        parameters=[configured_params],
+                        remappings=remappings),
+                    ComposableNode(
+                        package='nav2_lifecycle_manager',
+                        plugin='nav2_lifecycle_manager::LifecycleManager',
+                        name='lifecycle_manager_vo_server',
+                        parameters=[{'autostart': autostart},
+                                    {'node_names': lifecycle_nodes}],
+                        remappings=remappings)
+                ],
+            )
+        ]
+    )
+
+    ld = LaunchDescription()
+
+    # Launch arguments
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
+
+    # Node launching commands
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld
diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml
index ffb3d3d1fce..44045314fa3 100644
--- a/nav2_map_server/package.xml
+++ b/nav2_map_server/package.xml
@@ -22,12 +22,15 @@
   <depend>yaml_cpp_vendor</depend>
   <depend>launch_ros</depend>
   <depend>launch_testing</depend>
+  <depend>geometry_msgs</depend>
   <depend>tf2</depend>
+  <depend>tf2_ros</depend>
   <depend>nav2_msgs</depend>
   <depend>nav2_util</depend>
   <depend>graphicsmagick</depend>
   <depend>lifecycle_msgs</depend>
   <depend>nav2_ros_common</depend>
+  <depend>uuid</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_map_server/params/vector_object_server_params.yaml b/nav2_map_server/params/vector_object_server_params.yaml
new file mode 100644
index 00000000000..6a6a5fc2b68
--- /dev/null
+++ b/nav2_map_server/params/vector_object_server_params.yaml
@@ -0,0 +1,30 @@
+vector_object_server:
+  ros__parameters:
+    map_topic: "vo_map"
+    global_frame_id: "map"
+    resolution: 0.05
+    default_value: -1
+    overlay_type: 0
+    update_frequency: 1.0
+    transform_tolerance: 0.1
+    shapes: ["Poly", "Circle"]
+    Poly:
+      type: "polygon"
+      frame_id: "map"
+      closed: True
+      value: 100
+      points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2]
+    Circle:
+      type: "circle"
+      frame_id: "map"
+      fill: True
+      value: 100
+      center: [1.5, 0.5]
+      radius: 0.7
+costmap_filter_info_server:
+  ros__parameters:
+    type: 0
+    filter_info_topic: "vo_costmap_filter_info"
+    mask_topic: "vo_map"
+    base: 0.0
+    multiplier: 1.0
diff --git a/nav2_map_server/src/vo_server/main.cpp b/nav2_map_server/src/vo_server/main.cpp
new file mode 100644
index 00000000000..d2352b7aad3
--- /dev/null
+++ b/nav2_map_server/src/vo_server/main.cpp
@@ -0,0 +1,28 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// 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 <memory>
+
+#include "nav2_map_server/vector_object_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_map_server::VectorObjectServer>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp
new file mode 100644
index 00000000000..89d7c3c825f
--- /dev/null
+++ b/nav2_map_server/src/vo_server/vector_object_server.cpp
@@ -0,0 +1,558 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_map_server/vector_object_server.hpp"
+
+#include <chrono>
+#include <exception>
+#include <functional>
+#include <limits>
+#include <stdexcept>
+#include <utility>
+
+#include "rclcpp/create_timer.hpp"
+
+#include "tf2_ros/create_timer_ros.hpp"
+
+#include "nav2_util/occ_grid_utils.hpp"
+#include "nav2_util/occ_grid_values.hpp"
+
+using namespace std::placeholders;
+
+namespace nav2_map_server
+{
+
+VectorObjectServer::VectorObjectServer(const rclcpp::NodeOptions & options)
+: nav2::LifecycleNode("vector_object_server", "", options), process_map_(false)
+{}
+
+nav2::CallbackReturn
+VectorObjectServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  // Transform buffer and listener initialization
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+    this->get_node_base_interface(),
+    this->get_node_timers_interface());
+  tf_buffer_->setCreateTimerInterface(timer_interface);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  // Obtaining ROS parameters
+  if (!obtainParams()) {
+    return nav2::CallbackReturn::FAILURE;
+  }
+
+  map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
+    map_topic_,
+    nav2::qos::LatchedPublisherQoS());
+
+  add_shapes_service_ = create_service<nav2_msgs::srv::AddShapes>(
+    "~/add_shapes",
+    std::bind(&VectorObjectServer::addShapesCallback, this, _1, _2, _3));
+
+  get_shapes_service_ = create_service<nav2_msgs::srv::GetShapes>(
+    "~/get_shapes",
+    std::bind(&VectorObjectServer::getShapesCallback, this, _1, _2, _3));
+
+  remove_shapes_service_ = create_service<nav2_msgs::srv::RemoveShapes>(
+    "~/remove_shapes",
+    std::bind(&VectorObjectServer::removeShapesCallback, this, _1, _2, _3));
+
+  return nav2::CallbackReturn::SUCCESS;
+}
+
+nav2::CallbackReturn
+VectorObjectServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  map_pub_->on_activate();
+
+  // Trigger map to be published
+  process_map_ = true;
+  switchMapUpdate();
+
+  // Creating bond connection
+  createBond();
+
+  return nav2::CallbackReturn::SUCCESS;
+}
+
+nav2::CallbackReturn
+VectorObjectServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  if (map_timer_) {
+    map_timer_->cancel();
+    map_timer_.reset();
+  }
+  process_map_ = false;
+
+  map_pub_->on_deactivate();
+
+  // Destroying bond connection
+  destroyBond();
+
+  return nav2::CallbackReturn::SUCCESS;
+}
+
+nav2::CallbackReturn
+VectorObjectServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  add_shapes_service_.reset();
+  get_shapes_service_.reset();
+  remove_shapes_service_.reset();
+
+  map_pub_.reset();
+  map_.reset();
+
+  shapes_.clear();
+
+  tf_listener_.reset();
+  tf_buffer_.reset();
+
+  return nav2::CallbackReturn::SUCCESS;
+}
+
+nav2::CallbackReturn
+VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2::CallbackReturn::SUCCESS;
+}
+
+bool VectorObjectServer::obtainParams()
+{
+  auto node = shared_from_this();
+
+  // Main ROS-parameters
+  map_topic_ = nav2::declare_or_get_parameter(node, "map_topic", std::string{"vo_map"});
+  global_frame_id_ = nav2::declare_or_get_parameter(node, "global_frame_id", std::string{"map"});
+  resolution_ = nav2::declare_or_get_parameter(node, "resolution", 0.05);
+  default_value_ = nav2::declare_or_get_parameter(node, "default_value",
+      static_cast<int>(nav2_util::OCC_GRID_UNKNOWN));
+  overlay_type_ = static_cast<OverlayType>(nav2::declare_or_get_parameter(node, "overlay_type",
+      static_cast<int>(OverlayType::OVERLAY_SEQ)));
+  update_frequency_ = nav2::declare_or_get_parameter(node, "update_frequency", 1.0);
+  transform_tolerance_ = nav2::declare_or_get_parameter(node, "transform_tolerance", 0.1);
+
+  // Shapes
+  auto shape_names = nav2::declare_or_get_parameter(node, "shapes", std::vector<std::string>());
+  for (std::string shape_name : shape_names) {
+    std::string shape_type;
+    try {
+      shape_type = nav2::declare_or_get_parameter<std::string>(node, shape_name + ".type",
+          rclcpp::PARAMETER_STRING);
+    } catch (const std::exception & ex) {
+      RCLCPP_ERROR(
+        get_logger(), "Error while getting shape %s type: %s", shape_name.c_str(), ex.what());
+      return false;
+    }
+
+    if (shape_type == "polygon") {
+      auto polygon = std::make_shared<Polygon>(node);
+      if (!polygon->obtainParams(shape_name)) {
+        return false;
+      }
+      shapes_.push_back(polygon);
+    } else if (shape_type == "circle") {
+      auto circle = std::make_shared<Circle>(node);
+      if (!circle->obtainParams(shape_name)) {
+        return false;
+      }
+      shapes_.push_back(circle);
+    } else {
+      RCLCPP_ERROR(get_logger(),
+        "Please specify the correct type for shape %s. Supported types are 'polygon' and 'circle'",
+        shape_name.c_str());
+      return false;
+    }
+  }
+
+  return true;
+}
+
+std::vector<std::shared_ptr<Shape>>::iterator
+VectorObjectServer::findShape(const unsigned char * uuid)
+{
+  for (auto it = shapes_.begin(); it != shapes_.end(); it++) {
+    if ((*it)->isUUID(uuid)) {
+      return it;
+    }
+  }
+  return shapes_.end();
+}
+
+bool VectorObjectServer::transformVectorObjects()
+{
+  for (auto shape : shapes_) {
+    if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) {
+      // Shape to be updated dynamically
+      if (!shape->toFrame(global_frame_id_, tf_buffer_, transform_tolerance_)) {
+        RCLCPP_ERROR(
+          get_logger(), "Can not transform vector object from %s to %s frame",
+          shape->getFrameID().c_str(), global_frame_id_.c_str());
+        return false;
+      }
+    }
+  }
+
+  return true;
+}
+
+void VectorObjectServer::getMapBoundaries(
+  double & min_x, double & min_y, double & max_x, double & max_y) const
+{
+  min_x = std::numeric_limits<double>::max();
+  min_y = std::numeric_limits<double>::max();
+  max_x = std::numeric_limits<double>::lowest();
+  max_y = std::numeric_limits<double>::lowest();
+
+  double min_p_x, min_p_y, max_p_x, max_p_y;
+  for (auto shape : shapes_) {
+    shape->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y);
+    min_x = std::min(min_x, min_p_x);
+    min_y = std::min(min_y, min_p_y);
+    max_x = std::max(max_x, max_p_x);
+    max_y = std::max(max_y, max_p_y);
+  }
+
+  if (
+    min_x == std::numeric_limits<double>::max() ||
+    min_y == std::numeric_limits<double>::max() ||
+    max_x == std::numeric_limits<double>::lowest() ||
+    max_y == std::numeric_limits<double>::lowest())
+  {
+    throw std::runtime_error("Can not obtain map boundaries");
+  }
+}
+
+void VectorObjectServer::updateMap(
+  const double & min_x, const double & min_y, const double & max_x, const double & max_y)
+{
+  // Calculate size of update map
+  int size_x = static_cast<int>((max_x - min_x) / resolution_) + 1;
+  int size_y = static_cast<int>((max_y - min_y) / resolution_) + 1;
+
+  if (size_x < 0) {
+    throw std::runtime_error("Incorrect map x-size");
+  }
+
+  if (size_y < 0) {
+    throw std::runtime_error("Incorrect map y-size");
+  }
+
+  if (!map_) {
+    map_ = std::make_shared<nav_msgs::msg::OccupancyGrid>();
+  }
+
+  if (
+    map_->info.width != static_cast<unsigned int>(size_x) ||
+    map_->info.height != static_cast<unsigned int>(size_y))
+  {
+    // Map size was changed
+    map_->data = std::vector<int8_t>(size_x * size_y, default_value_);
+    map_->info.width = size_x;
+    map_->info.height = size_y;
+  } else if (size_x > 0 && size_y > 0) {
+    // Map size was not changed
+    memset(map_->data.data(), default_value_, size_x * size_y * sizeof(int8_t));
+  }
+
+  map_->header.frame_id = global_frame_id_;
+  map_->info.resolution = resolution_;
+  map_->info.origin.position.x = min_x;
+  map_->info.origin.position.y = min_y;
+}
+
+void VectorObjectServer::putVectorObjectsOnMap()
+{
+  // Filling the shapes
+  for (auto shape : shapes_) {
+    if (shape->isFill()) {
+      // Put filled shape on map
+      double wx1 = std::numeric_limits<double>::max();
+      double wy1 = std::numeric_limits<double>::max();
+      double wx2 = std::numeric_limits<double>::lowest();
+      double wy2 = std::numeric_limits<double>::lowest();
+      unsigned int mx1 = 0;
+      unsigned int my1 = 0;
+      unsigned int mx2 = 0;
+      unsigned int my2 = 0;
+
+      shape->getBoundaries(wx1, wy1, wx2, wy2);
+      if (
+        !nav2_util::worldToMap(map_, wx1, wy1, mx1, my1) ||
+        !nav2_util::worldToMap(map_, wx2, wy2, mx2, my2))
+      {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Error to get shape boundaries on map (UUID: %s)", shape->getUUID().c_str());
+        return;
+      }
+
+      unsigned int it;
+      for (unsigned int my = my1; my <= my2; my++) {
+        for (unsigned int mx = mx1; mx <= mx2; mx++) {
+          it = my * map_->info.width + mx;
+          double wx, wy;
+          nav2_util::mapToWorld(map_, mx, my, wx, wy);
+          if (shape->isPointInside(wx, wy)) {
+            processVal(map_->data[it], shape->getValue(), overlay_type_);
+          }
+        }
+      }
+    } else {
+      // Put shape borders on map
+      shape->putBorders(map_, overlay_type_);
+    }
+  }
+}
+
+void VectorObjectServer::publishMap()
+{
+  if (map_) {
+    auto map = std::make_unique<nav_msgs::msg::OccupancyGrid>(*map_);
+    map_pub_->publish(std::move(map));
+  }
+}
+
+void VectorObjectServer::processMap()
+{
+  if (!process_map_) {
+    return;
+  }
+
+  try {
+    if (shapes_.size() > 0) {
+      if (!transformVectorObjects()) {
+        return;
+      }
+      double min_x, min_y, max_x, max_y;
+
+      getMapBoundaries(min_x, min_y, max_x, max_y);
+      updateMap(min_x, min_y, max_x, max_y);
+      putVectorObjectsOnMap();
+    } else {
+      updateMap(0.0, 0.0, 0.0, 0.0);
+    }
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(get_logger(), "Can not update map: %s", ex.what());
+    return;
+  }
+
+  publishMap();
+}
+
+void VectorObjectServer::switchMapUpdate()
+{
+  for (auto shape : shapes_) {
+    if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) {
+      if (!map_timer_) {
+        map_timer_ = this->create_timer(
+          std::chrono::duration<double>(1.0 / update_frequency_),
+          std::bind(&VectorObjectServer::processMap, this));
+      }
+      RCLCPP_INFO(get_logger(), "Publishing map dynamically at %f Hz rate", update_frequency_);
+      return;
+    }
+  }
+
+  if (map_timer_) {
+    map_timer_->cancel();
+    map_timer_.reset();
+  }
+  RCLCPP_INFO(get_logger(), "Publishing map once");
+  processMap();
+}
+
+void VectorObjectServer::addShapesCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::AddShapes::Request> request,
+  std::shared_ptr<nav2_msgs::srv::AddShapes::Response> response)
+{
+  // Initialize result with true. If one of the required vector object was not added properly,
+  // set it to false.
+  response->success = true;
+
+  auto node = shared_from_this();
+
+  // Process polygons
+  for (auto req_poly : request->polygons) {
+    nav2_msgs::msg::PolygonObject::SharedPtr new_params =
+      std::make_shared<nav2_msgs::msg::PolygonObject>(req_poly);
+
+    auto it = findShape(new_params->uuid.uuid.data());
+    if (it != shapes_.end()) {
+      // Vector Object with given UUID was found: updating it
+      // Check that found shape has correct type
+      if ((*it)->getType() != POLYGON) {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Shape (UUID: %s) is not a polygon type for a polygon update. Not adding shape.",
+          (*it)->getUUID().c_str());
+        response->success = false;
+        // Do not add this shape
+        continue;
+      }
+
+      std::shared_ptr<Polygon> polygon = std::static_pointer_cast<Polygon>(*it);
+
+      // Preserving old parameters for the case, if new ones to be incorrect
+      nav2_msgs::msg::PolygonObject::SharedPtr old_params = polygon->getParams();
+      if (!polygon->setParams(new_params)) {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Failed to update existing polygon object (UUID: %s) with new params. "
+          "Reverting to old polygon params.",
+          (*it)->getUUID().c_str());
+        // Restore old parameters
+        polygon->setParams(old_params);
+        // ... and set the failure to return
+        response->success = false;
+      }
+    } else {
+      // Vector Object with given UUID was not found: creating a new one
+      std::shared_ptr<Polygon> polygon = std::make_shared<Polygon>(node);
+      if (polygon->setParams(new_params)) {
+        shapes_.push_back(polygon);
+      } else {
+        RCLCPP_ERROR(
+          get_logger(), "Failed to create a new polygon object using the provided params.");
+        response->success = false;
+      }
+    }
+  }
+
+  // Process circles
+  for (auto req_crcl : request->circles) {
+    nav2_msgs::msg::CircleObject::SharedPtr new_params =
+      std::make_shared<nav2_msgs::msg::CircleObject>(req_crcl);
+
+    auto it = findShape(new_params->uuid.uuid.data());
+    if (it != shapes_.end()) {
+      // Vector object with given UUID was found: updating it
+      // Check that found shape has correct type
+      if ((*it)->getType() != CIRCLE) {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Shape (UUID: %s) is not a circle type for a circle update. Not adding shape.",
+          (*it)->getUUID().c_str());
+        response->success = false;
+        // Do not add this shape
+        continue;
+      }
+
+      std::shared_ptr<Circle> circle = std::static_pointer_cast<Circle>(*it);
+
+      // Preserving old parameters for the case, if new ones to be incorrect
+      nav2_msgs::msg::CircleObject::SharedPtr old_params = circle->getParams();
+      if (!circle->setParams(new_params)) {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Failed to update existing circle object (UUID: %s) with new params. "
+          "Reverting to old circle params.",
+          (*it)->getUUID().c_str());
+        // Restore old parameters
+        circle->setParams(old_params);
+        // ... and set the failure to return
+        response->success = false;
+      }
+    } else {
+      // Vector Object with given UUID was not found: creating a new one
+      std::shared_ptr<Circle> circle = std::make_shared<Circle>(node);
+      if (circle->setParams(new_params)) {
+        shapes_.push_back(circle);
+      } else {
+        RCLCPP_ERROR(
+          get_logger(), "Failed to create a new circle object using the provided params.");
+        response->success = false;
+      }
+    }
+  }
+
+  switchMapUpdate();
+}
+
+void VectorObjectServer::getShapesCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::GetShapes::Request>/*request*/,
+  std::shared_ptr<nav2_msgs::srv::GetShapes::Response> response)
+{
+  std::shared_ptr<Polygon> polygon;
+  std::shared_ptr<Circle> circle;
+
+  for (auto shape : shapes_) {
+    switch (shape->getType()) {
+      case POLYGON:
+        polygon = std::static_pointer_cast<Polygon>(shape);
+        response->polygons.push_back(*(polygon->getParams()));
+        break;
+      case CIRCLE:
+        circle = std::static_pointer_cast<Circle>(shape);
+        response->circles.push_back(*(circle->getParams()));
+        break;
+      default:
+        RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape->getUUID().c_str());
+    }
+  }
+}
+
+void VectorObjectServer::removeShapesCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::RemoveShapes::Request> request,
+  std::shared_ptr<nav2_msgs::srv::RemoveShapes::Response> response)
+{
+  // Initialize result with true. If one of the required vector object was not found,
+  // set it to false.
+  response->success = true;
+
+  if (request->all_objects) {
+    // Clear all objects
+    shapes_.clear();
+  } else {
+    // Find objects to remove
+    for (auto req_uuid : request->uuids) {
+      auto it = findShape(req_uuid.uuid.data());
+      if (it != shapes_.end()) {
+        // Shape with given UUID was found: remove it
+        (*it).reset();
+        shapes_.erase(it);
+      } else {
+        // Required vector object was not found
+        RCLCPP_ERROR(
+          get_logger(),
+          "Can not find shape to remove with UUID: %s",
+          unparseUUID(req_uuid.uuid.data()).c_str());
+        response->success = false;
+      }
+    }
+  }
+
+  switchMapUpdate();
+}
+
+}  // namespace nav2_map_server
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::VectorObjectServer)
diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp
new file mode 100644
index 00000000000..72466486c01
--- /dev/null
+++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp
@@ -0,0 +1,559 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_map_server/vector_object_shapes.hpp"
+
+#include <uuid/uuid.h>
+#include <cmath>
+#include <exception>
+#include <limits>
+#include <stdexcept>
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "nav2_util/occ_grid_utils.hpp"
+#include "nav2_util/occ_grid_values.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/raytrace_line_2d.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace nav2_map_server
+{
+
+// ---------- Shape ----------
+
+Shape::Shape(const nav2::LifecycleNode::WeakPtr & node)
+: type_(UNKNOWN), node_(node)
+{}
+
+Shape::~Shape()
+{}
+
+ShapeType Shape::getType()
+{
+  return type_;
+}
+
+bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  try {
+    // Try to get shape UUID from ROS-parameters
+    std::string uuid_str = nav2::declare_or_get_parameter<std::string>(
+      node, shape_name + ".uuid", rclcpp::PARAMETER_STRING);
+    if (uuid_parse(uuid_str.c_str(), out_uuid) != 0) {
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "[%s] Can not parse UUID string for shape: %s",
+        shape_name.c_str(), uuid_str.c_str());
+      return false;
+    }
+  } catch (const std::exception &) {
+    // If no UUID was specified, generate a new one
+    uuid_generate(out_uuid);
+
+    char uuid_str[37];
+    uuid_unparse(out_uuid, uuid_str);
+    RCLCPP_INFO(
+      node->get_logger(),
+      "[%s] No UUID is specified for shape. Generating a new one: %s",
+      shape_name.c_str(), uuid_str);
+  }
+
+  return true;
+}
+
+// ---------- Polygon ----------
+
+Polygon::Polygon(
+  const nav2::LifecycleNode::WeakPtr & node)
+: Shape::Shape(node)
+{
+  type_ = POLYGON;
+}
+
+int8_t Polygon::getValue() const
+{
+  return params_->value;
+}
+
+std::string Polygon::getFrameID() const
+{
+  return params_->header.frame_id;
+}
+
+std::string Polygon::getUUID() const
+{
+  return unparseUUID(params_->uuid.uuid.data());
+}
+
+bool Polygon::isUUID(const unsigned char * uuid) const
+{
+  return uuid_compare(params_->uuid.uuid.data(), uuid) == 0;
+}
+
+bool Polygon::isFill() const
+{
+  return params_->closed;
+}
+
+bool Polygon::obtainParams(const std::string & shape_name)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!params_) {
+    params_ = std::make_shared<nav2_msgs::msg::PolygonObject>();
+  }
+  if (!polygon_) {
+    polygon_ = std::make_shared<geometry_msgs::msg::Polygon>();
+  }
+
+  params_->header.frame_id = nav2::declare_or_get_parameter(
+    node, shape_name + ".frame_id", std::string{"map"});
+  params_->value = nav2::declare_or_get_parameter(
+    node, shape_name + ".value", static_cast<int>(nav2_util::OCC_GRID_OCCUPIED));
+  params_->closed = nav2::declare_or_get_parameter(
+    node, shape_name + ".closed", true);
+
+  std::vector<double> poly_row;
+  try {
+    poly_row = nav2::declare_or_get_parameter<std::vector<double>>(
+      node, shape_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY);
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[%s] Error while getting polygon parameters: %s",
+      shape_name.c_str(), ex.what());
+    return false;
+  }
+  // Check for points format correctness
+  if (poly_row.size() < 6 || poly_row.size() % 2 != 0) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[%s] Polygon has incorrect points description",
+      shape_name.c_str());
+    return false;
+  }
+
+  // Obtain polygon vertices
+  geometry_msgs::msg::Point32 point;
+  bool first = true;
+  for (double val : poly_row) {
+    if (first) {
+      point.x = val;
+    } else {
+      point.y = val;
+      params_->points.push_back(point);
+    }
+    first = !first;
+  }
+
+  // Filling the polygon_ with obtained points in map's frame
+  polygon_->points = params_->points;
+
+  // Getting shape UUID
+  return obtainShapeUUID(shape_name, params_->uuid.uuid.data());
+}
+
+nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const
+{
+  return params_;
+}
+
+bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params)
+{
+  params_ = params;
+
+  if (!polygon_) {
+    polygon_ = std::make_shared<geometry_msgs::msg::Polygon>();
+  }
+  polygon_->points = params_->points;
+
+  // If no UUID was specified, generate a new one
+  if (uuid_is_null(params_->uuid.uuid.data())) {
+    uuid_generate(params_->uuid.uuid.data());
+  }
+
+  return checkConsistency();
+}
+
+bool Polygon::toFrame(
+  const std::string & to_frame,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const double transform_tolerance)
+{
+  geometry_msgs::msg::PoseStamped from_pose, to_pose;
+  from_pose.header = params_->header;
+  for (unsigned int i = 0; i < params_->points.size(); i++) {
+    from_pose.pose.position.x = params_->points[i].x;
+    from_pose.pose.position.y = params_->points[i].y;
+    from_pose.pose.position.z = params_->points[i].z;
+    if (
+      nav2_util::transformPoseInTargetFrame(
+        from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance))
+    {
+      polygon_->points[i].x = to_pose.pose.position.x;
+      polygon_->points[i].y = to_pose.pose.position.y;
+      polygon_->points[i].z = to_pose.pose.position.z;
+    } else {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y)
+{
+  min_x = std::numeric_limits<double>::max();
+  min_y = std::numeric_limits<double>::max();
+  max_x = std::numeric_limits<double>::lowest();
+  max_y = std::numeric_limits<double>::lowest();
+
+  for (auto point : polygon_->points) {
+    min_x = std::min(min_x, static_cast<double>(point.x));
+    min_y = std::min(min_y, static_cast<double>(point.y));
+    max_x = std::max(max_x, static_cast<double>(point.x));
+    max_y = std::max(max_y, static_cast<double>(point.y));
+  }
+}
+
+bool Polygon::isPointInside(const double px, const double py) const
+{
+  return nav2_util::geometry_utils::isPointInsidePolygon(px, py, polygon_->points);
+}
+
+void Polygon::putBorders(
+  nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type)
+{
+  unsigned int mx0, my0, mx1, my1;
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!nav2_util::worldToMap(map, polygon_->points[0].x, polygon_->points[0].y, mx1, my1)) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[UUID: %s] Can not convert (%f, %f) point to map",
+      getUUID().c_str(), polygon_->points[0].x, polygon_->points[0].y);
+    return;
+  }
+
+  MapAction ma(map, params_->value, overlay_type);
+  for (unsigned int i = 1; i < polygon_->points.size(); i++) {
+    mx0 = mx1;
+    my0 = my1;
+    if (!nav2_util::worldToMap(map, polygon_->points[i].x, polygon_->points[i].y, mx1, my1)) {
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "[UUID: %s] Can not convert (%f, %f) point to map",
+        getUUID().c_str(), polygon_->points[i].x, polygon_->points[i].y);
+      return;
+    }
+    nav2_util::raytraceLine(ma, mx0, my0, mx1, my1, map->info.width);
+  }
+}
+
+bool Polygon::checkConsistency()
+{
+  if (params_->points.size() < 3) {
+    auto node = node_.lock();
+    if (!node) {
+      throw std::runtime_error{"Failed to lock node"};
+    }
+
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[UUID: %s] Polygon has incorrect number of vertices: %li",
+      getUUID().c_str(), params_->points.size());
+    return false;
+  }
+
+  return true;
+}
+
+// ---------- Circle ----------
+
+Circle::Circle(
+  const nav2::LifecycleNode::WeakPtr & node)
+: Shape::Shape(node)
+{
+  type_ = CIRCLE;
+}
+
+int8_t Circle::getValue() const
+{
+  return params_->value;
+}
+
+std::string Circle::getFrameID() const
+{
+  return params_->header.frame_id;
+}
+
+std::string Circle::getUUID() const
+{
+  return unparseUUID(params_->uuid.uuid.data());
+}
+
+bool Circle::isUUID(const unsigned char * uuid) const
+{
+  return uuid_compare(params_->uuid.uuid.data(), uuid) == 0;
+}
+
+bool Circle::isFill() const
+{
+  return params_->fill;
+}
+
+bool Circle::obtainParams(const std::string & shape_name)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  if (!params_) {
+    params_ = std::make_shared<nav2_msgs::msg::CircleObject>();
+  }
+  if (!center_) {
+    center_ = std::make_shared<geometry_msgs::msg::Point32>();
+  }
+
+  params_->header.frame_id = nav2::declare_or_get_parameter(
+    node, shape_name + ".frame_id", std::string{"map"});
+  params_->value = nav2::declare_or_get_parameter(
+    node, shape_name + ".value", static_cast<int>(nav2_util::OCC_GRID_OCCUPIED));
+  params_->fill = nav2::declare_or_get_parameter(
+    node, shape_name + ".fill", true);
+
+  std::vector<double> center_row;
+  try {
+    center_row = nav2::declare_or_get_parameter<std::vector<double>>(
+      node, shape_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY);
+    params_->radius = nav2::declare_or_get_parameter<double>(
+      node, shape_name + ".radius", rclcpp::PARAMETER_DOUBLE);
+    if (params_->radius < 0) {
+      RCLCPP_ERROR(
+        node->get_logger(),
+        "[%s] Circle has incorrect radius less than zero",
+        shape_name.c_str());
+      return false;
+    }
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[%s] Error while getting circle parameters: %s",
+      shape_name.c_str(), ex.what());
+    return false;
+  }
+  // Check for points format correctness
+  if (center_row.size() != 2) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[%s] Circle has incorrect center description",
+      shape_name.c_str());
+    return false;
+  }
+
+  // Obtain circle center
+  params_->center.x = center_row[0];
+  params_->center.y = center_row[1];
+  // Setting the center_ with obtained circle center in map's frame
+  *center_ = params_->center;
+
+  // Getting shape UUID
+  return obtainShapeUUID(shape_name, params_->uuid.uuid.data());
+}
+
+nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const
+{
+  return params_;
+}
+
+bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params)
+{
+  params_ = params;
+
+  if (!center_) {
+    center_ = std::make_shared<geometry_msgs::msg::Point32>();
+  }
+  *center_ = params_->center;
+
+  // If no UUID was specified, generate a new one
+  if (uuid_is_null(params_->uuid.uuid.data())) {
+    uuid_generate(params_->uuid.uuid.data());
+  }
+
+  return checkConsistency();
+}
+
+bool Circle::toFrame(
+  const std::string & to_frame,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const double transform_tolerance)
+{
+  geometry_msgs::msg::PoseStamped from_pose, to_pose;
+  from_pose.header = params_->header;
+  from_pose.pose.position.x = params_->center.x;
+  from_pose.pose.position.y = params_->center.y;
+  from_pose.pose.position.z = params_->center.z;
+  if (
+    nav2_util::transformPoseInTargetFrame(
+      from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance))
+  {
+    center_->x = to_pose.pose.position.x;
+    center_->y = to_pose.pose.position.y;
+    center_->z = to_pose.pose.position.z;
+  } else {
+    return false;
+  }
+
+  return true;
+}
+
+void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y)
+{
+  min_x = center_->x - params_->radius;
+  min_y = center_->y - params_->radius;
+  max_x = center_->x + params_->radius;
+  max_y = center_->y + params_->radius;
+}
+
+bool Circle::isPointInside(const double px, const double py) const
+{
+  return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <=
+         params_->radius * params_->radius;
+}
+
+void Circle::putBorders(
+  nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type)
+{
+  unsigned int mcx, mcy;
+  if (!centerToMap(map, mcx, mcy)) {
+    return;
+  }
+
+  // Implementation of the circle generation algorithm, based on the following work:
+  // Berthold K.P. Horn "Circle generators for display devices"
+  // Computer Graphics and Image Processing 5.2 (1976): 280-288.
+
+  // Inputs initialization
+  const int r = static_cast<int>(std::round(params_->radius / map->info.resolution));
+  int x = r;
+  int y = 1;
+
+  // Error initialization
+  int s = -r;
+
+  // Calculation algorithm
+  while (x > y) {  // Calculating only first circle octant
+    // Put 8 points in each octant reflecting symmetrically
+    putPoint(mcx + x, mcy + y, map, overlay_type);
+    putPoint(mcx + y, mcy + x, map, overlay_type);
+    putPoint(mcx - x + 1, mcy + y, map, overlay_type);
+    putPoint(mcx + y, mcy - x + 1, map, overlay_type);
+    putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type);
+    putPoint(mcx - y + 1, mcy - x + 1, map, overlay_type);
+    putPoint(mcx + x, mcy - y + 1, map, overlay_type);
+    putPoint(mcx - y + 1, mcy + x, map, overlay_type);
+
+    s = s + 2 * y + 1;
+    y++;
+    if (s > 0) {
+      s = s - 2 * x + 2;
+      x--;
+    }
+  }
+
+  // Corner case for x == y: do not put end points twice
+  if (x == y) {
+    putPoint(mcx + x, mcy + y, map, overlay_type);
+    putPoint(mcx - x + 1, mcy + y, map, overlay_type);
+    putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type);
+    putPoint(mcx + x, mcy - y + 1, map, overlay_type);
+  }
+}
+
+bool Circle::checkConsistency()
+{
+  if (params_->radius < 0.0) {
+    auto node = node_.lock();
+    if (!node) {
+      throw std::runtime_error{"Failed to lock node"};
+    }
+
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[UUID: %s] Circle has incorrect radius less than zero",
+      getUUID().c_str());
+    return false;
+  }
+  return true;
+}
+
+bool Circle::centerToMap(
+  nav_msgs::msg::OccupancyGrid::ConstSharedPtr map,
+  unsigned int & mcx, unsigned int & mcy)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  // Get center of circle in map coordinates
+  if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[UUID: %s] Can not convert (%f, %f) circle center to map",
+      getUUID().c_str(), center_->x, center_->y);
+    return false;
+  }
+  // We need the circle center to be always shifted one cell less its logical center
+  // and to avoid any FP-accuracy losing on small values, so we are using another
+  // than nav2_util::worldToMap() approach
+  mcx = static_cast<unsigned int>(
+    std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1;
+  mcy = static_cast<unsigned int>(
+    std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1;
+  if (mcx >= map->info.width || mcy >= map->info.height) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "[UUID: %s] Can not convert (%f, %f) point to map",
+      getUUID().c_str(), center_->x, center_->y);
+    return false;
+  }
+
+  return true;
+}
+
+inline void Circle::putPoint(
+  unsigned int mx, unsigned int my,
+  nav_msgs::msg::OccupancyGrid::SharedPtr map,
+  const OverlayType overlay_type)
+{
+  processCell(map, my * map->info.width + mx, params_->value, overlay_type);
+}
+
+}  // namespace nav2_map_server
diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt
index 4140b14d6f0..06a4b2228a0 100644
--- a/nav2_map_server/test/unit/CMakeLists.txt
+++ b/nav2_map_server/test/unit/CMakeLists.txt
@@ -18,3 +18,25 @@ target_link_libraries(test_costmap_filter_info_server
   ${library_name}
   ${map_io_library_name}
 )
+
+# test_vector_object_shapes unit test
+ament_add_gtest(test_vector_object_shapes
+  test_vector_object_shapes.cpp
+)
+
+target_link_libraries(test_vector_object_shapes ${vo_server_dependencies})
+
+target_link_libraries(test_vector_object_shapes
+  ${vo_library_name}
+)
+
+# test_vector_object_server unit test
+ament_add_gtest(test_vector_object_server
+  test_vector_object_server.cpp
+)
+
+target_link_libraries(test_vector_object_server ${vo_server_dependencies})
+
+target_link_libraries(test_vector_object_server
+  ${vo_library_name}
+)
diff --git a/nav2_map_server/test/unit/test_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp
new file mode 100644
index 00000000000..88bfd697d0a
--- /dev/null
+++ b/nav2_map_server/test/unit/test_vector_object_server.cpp
@@ -0,0 +1,1349 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// 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 <gtest/gtest.h>
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "unique_identifier_msgs/msg/uuid.hpp"
+
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
+
+#include "nav2_msgs/srv/add_shapes.hpp"
+#include "nav2_msgs/srv/get_shapes.hpp"
+#include "nav2_msgs/srv/remove_shapes.hpp"
+#include "nav2_msgs/msg/polygon_object.hpp"
+#include "nav2_msgs/msg/circle_object.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_util/occ_grid_utils.hpp"
+
+#include "nav2_map_server/vector_object_server.hpp"
+
+using namespace std::chrono_literals;
+
+static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
+
+static const char POLYGON_NAME[]{"Polygon"};
+static const char CIRCLE_NAME[]{"Circle"};
+static const char GLOBAL_FRAME_ID[]{"map"};
+static const char SHAPE_FRAME_ID[]{"shape"};
+static const double UPDATE_FREQUENCY{10.0};
+static const int8_t POLYGON_VAL{nav2_util::OCC_GRID_OCCUPIED};
+static const int8_t CIRCLE_VAL{nav2_util::OCC_GRID_OCCUPIED / 2};
+
+class VOServerWrapper : public nav2_map_server::VectorObjectServer
+{
+public:
+  void start()
+  {
+    ASSERT_EQ(on_configure(get_current_state()), nav2::CallbackReturn::SUCCESS);
+    ASSERT_EQ(on_activate(get_current_state()), nav2::CallbackReturn::SUCCESS);
+  }
+
+  void configureFail()
+  {
+    ASSERT_EQ(on_configure(get_current_state()), nav2::CallbackReturn::FAILURE);
+  }
+
+  void stop()
+  {
+    ASSERT_EQ(on_deactivate(get_current_state()), nav2::CallbackReturn::SUCCESS);
+    ASSERT_EQ(on_cleanup(get_current_state()), nav2::CallbackReturn::SUCCESS);
+    ASSERT_EQ(on_shutdown(get_current_state()), nav2::CallbackReturn::SUCCESS);
+  }
+
+  void cleanup()
+  {
+    ASSERT_EQ(on_cleanup(get_current_state()), nav2::CallbackReturn::SUCCESS);
+    ASSERT_EQ(on_shutdown(get_current_state()), nav2::CallbackReturn::SUCCESS);
+  }
+
+  void setProcessMap(const bool process_map)
+  {
+    process_map_ = process_map;
+  }
+
+  void getMapBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) const
+  {
+    VectorObjectServer::getMapBoundaries(min_x, min_y, max_x, max_y);
+  }
+
+  void updateMap(
+    const double & min_x, const double & min_y, const double & max_x, const double & max_y)
+  {
+    VectorObjectServer::updateMap(min_x, min_y, max_x, max_y);
+  }
+
+  void putVectorObjectsOnMap()
+  {
+    VectorObjectServer::putVectorObjectsOnMap();
+  }
+};  // VOServerWrapper
+
+class Tester : public ::testing::Test
+{
+public:
+  Tester();
+  ~Tester();
+
+  void setVOServerParams();
+  void setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid);
+  void setPolygonParams(const std::string & uuid);
+  void setCircleParams(const std::string & uuid);
+
+  nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject(
+    const std::vector<unsigned char> & uuid);
+  nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject(
+    const std::vector<unsigned char> & uuid);
+
+  void sendTransform(const double frame_shift);
+
+  template<class T>
+  typename T::Response::SharedPtr sendRequest(
+    typename nav2::ServiceClient<T>::SharedPtr client,
+    typename T::Request::SharedPtr request,
+    const std::chrono::nanoseconds & timeout);
+
+  void mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map);
+  bool waitMap(const std::chrono::nanoseconds & timeout);
+  void verifyMap(bool is_circle, double poly_x_end = 1.0, double circle_cx = 3.0);
+
+  void comparePolygonObjects(
+    nav2_msgs::msg::PolygonObject::SharedPtr p1,
+    nav2_msgs::msg::PolygonObject::SharedPtr p2);
+  void compareCircleObjects(
+    nav2_msgs::msg::CircleObject::SharedPtr c1,
+    nav2_msgs::msg::CircleObject::SharedPtr c2);
+
+protected:
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  // Service clients for calling AddShapes.srv, GetShapes.srv, RemoveShapes.srv
+  nav2::ServiceClient<nav2_msgs::srv::AddShapes>::SharedPtr add_shapes_client_;
+  nav2::ServiceClient<nav2_msgs::srv::GetShapes>::SharedPtr get_shapes_client_;
+  nav2::ServiceClient<nav2_msgs::srv::RemoveShapes>::SharedPtr remove_shapes_client_;
+
+  // Output map subscriber
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr vo_map_sub_;
+  // Output map published by VectorObjectServer
+  nav_msgs::msg::OccupancyGrid::SharedPtr map_;
+
+  // Vector Object server node
+  std::shared_ptr<VOServerWrapper> vo_server_;
+};  // Tester
+
+Tester::Tester()
+: map_(nullptr)
+{
+  vo_server_ = std::make_shared<VOServerWrapper>();
+
+  add_shapes_client_ = vo_server_->create_client<nav2_msgs::srv::AddShapes>(
+    std::string(vo_server_->get_name()) + "/add_shapes");
+
+  get_shapes_client_ = vo_server_->create_client<nav2_msgs::srv::GetShapes>(
+    std::string(vo_server_->get_name()) + "/get_shapes");
+
+  remove_shapes_client_ = vo_server_->create_client<nav2_msgs::srv::RemoveShapes>(
+    std::string(vo_server_->get_name()) + "/remove_shapes");
+
+  vo_map_sub_ = vo_server_->create_subscription<nav_msgs::msg::OccupancyGrid>(
+    "vo_map", std::bind(&Tester::mapCallback, this, std::placeholders::_1),
+    nav2::qos::LatchedSubscriptionQoS());
+
+  // Transform buffer and listener initialization
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(vo_server_->get_clock());
+  tf_buffer_->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+}
+
+Tester::~Tester()
+{
+  vo_map_sub_.reset();
+
+  add_shapes_client_.reset();
+  get_shapes_client_.reset();
+  remove_shapes_client_.reset();
+
+  vo_server_.reset();
+
+  tf_listener_.reset();
+  tf_buffer_.reset();
+}
+
+void Tester::setVOServerParams()
+{
+  vo_server_->declare_parameter(
+    "map_topic", rclcpp::ParameterValue("vo_map"));
+  vo_server_->set_parameter(
+    rclcpp::Parameter("map_topic", "vo_map"));
+
+  vo_server_->declare_parameter(
+    "global_frame_id", rclcpp::ParameterValue("map"));
+  vo_server_->set_parameter(
+    rclcpp::Parameter("global_frame_id", "map"));
+
+  vo_server_->declare_parameter(
+    "resolution", rclcpp::ParameterValue(0.1));
+  vo_server_->set_parameter(
+    rclcpp::Parameter("resolution", 0.1));
+
+  vo_server_->declare_parameter(
+    "default_value", rclcpp::ParameterValue(nav2_util::OCC_GRID_UNKNOWN));
+  vo_server_->set_parameter(
+    rclcpp::Parameter("default_value", nav2_util::OCC_GRID_UNKNOWN));
+
+  vo_server_->declare_parameter(
+    "overlay_type",
+    rclcpp::ParameterValue(static_cast<int>(nav2_map_server::OverlayType::OVERLAY_SEQ)));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(
+      "overlay_type",
+      static_cast<int>(nav2_map_server::OverlayType::OVERLAY_SEQ)));
+
+  vo_server_->declare_parameter(
+    "update_frequency", rclcpp::ParameterValue(UPDATE_FREQUENCY));
+  vo_server_->set_parameter(
+    rclcpp::Parameter("update_frequency", UPDATE_FREQUENCY));
+
+  vo_server_->declare_parameter(
+    "transform_tolerance", rclcpp::ParameterValue(0.1));
+  vo_server_->set_parameter(
+    rclcpp::Parameter("transform_tolerance", 0.1));
+}
+
+void Tester::setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid)
+{
+  vo_server_->declare_parameter(
+    "shapes", rclcpp::ParameterValue(std::vector<std::string>{POLYGON_NAME, CIRCLE_NAME}));
+  vo_server_->set_parameter(
+    rclcpp::Parameter("shapes", std::vector<std::string>{POLYGON_NAME, CIRCLE_NAME}));
+
+  setPolygonParams(poly_uuid);
+  setCircleParams(circle_uuid);
+}
+
+void Tester::setPolygonParams(const std::string & uuid)
+{
+  const std::string polygon_name(POLYGON_NAME);
+
+  vo_server_->declare_parameter(
+    polygon_name + ".type", rclcpp::ParameterValue("polygon"));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".type", "polygon"));
+
+  vo_server_->declare_parameter(
+    polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID));
+
+  vo_server_->declare_parameter(
+    polygon_name + ".closed", rclcpp::ParameterValue(true));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".closed", true));
+
+  vo_server_->declare_parameter(
+    polygon_name + ".value", rclcpp::ParameterValue(POLYGON_VAL));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".value", POLYGON_VAL));
+
+  std::vector<double> points{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0};
+  vo_server_->declare_parameter(
+    polygon_name + ".points", rclcpp::ParameterValue(points));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".points", points));
+
+  if (!uuid.empty()) {
+    vo_server_->declare_parameter(
+      polygon_name + ".uuid", rclcpp::ParameterValue(uuid));
+    vo_server_->set_parameter(
+      rclcpp::Parameter(polygon_name + ".uuid", uuid));
+  }
+}
+
+void Tester::setCircleParams(const std::string & uuid)
+{
+  const std::string circle_name(CIRCLE_NAME);
+
+  vo_server_->declare_parameter(
+    circle_name + ".type", rclcpp::ParameterValue("circle"));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(circle_name + ".type", "circle"));
+
+  vo_server_->declare_parameter(
+    circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID));
+
+  vo_server_->declare_parameter(
+    circle_name + ".fill", rclcpp::ParameterValue(true));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(circle_name + ".fill", true));
+
+  vo_server_->declare_parameter(
+    circle_name + ".value", rclcpp::ParameterValue(CIRCLE_VAL));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(circle_name + ".value", CIRCLE_VAL));
+
+  vo_server_->declare_parameter(
+    circle_name + ".center", rclcpp::ParameterValue(std::vector<double>{3.0, 0.0}));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(circle_name + ".center", std::vector<double>{3.0, 0.0}));
+
+  vo_server_->declare_parameter(
+    circle_name + ".radius", rclcpp::ParameterValue(1.0));
+  vo_server_->set_parameter(
+    rclcpp::Parameter(circle_name + ".radius", 1.0));
+
+  if (!uuid.empty()) {
+    vo_server_->declare_parameter(
+      circle_name + ".uuid", rclcpp::ParameterValue(uuid));
+    vo_server_->set_parameter(
+      rclcpp::Parameter(circle_name + ".uuid", uuid));
+  }
+}
+
+void Tester::sendTransform(const double frame_shift)
+{
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster =
+    std::make_shared<tf2_ros::TransformBroadcaster>(vo_server_);
+
+  geometry_msgs::msg::TransformStamped transform;
+
+  // global_frame -> shape_frame transform
+  transform.header.frame_id = GLOBAL_FRAME_ID;
+  transform.child_frame_id = SHAPE_FRAME_ID;
+
+  transform.header.stamp = vo_server_->now();
+  transform.transform.translation.x = frame_shift;
+  transform.transform.translation.y = 0.0;
+  transform.transform.translation.z = 0.0;
+  transform.transform.rotation.x = 0.0;
+  transform.transform.rotation.y = 0.0;
+  transform.transform.rotation.z = 0.0;
+  transform.transform.rotation.w = 1.0;
+
+  tf_broadcaster->sendTransform(transform);
+}
+
+nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject(
+  const std::vector<unsigned char> & uuid)
+{
+  nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared<nav2_msgs::msg::PolygonObject>();
+  po->header.frame_id = GLOBAL_FRAME_ID;
+  if (uuid.size() == 16) {
+    // uuid could be optionally specified
+    std::array<unsigned char, 16> uuid_array;
+    std::copy(uuid.begin(), uuid.end(), uuid_array.begin());
+    po->uuid.uuid = uuid_array;
+  }
+  geometry_msgs::msg::Point32 p;
+  p.x = 1.0;
+  p.y = 1.0;
+  po->points.push_back(p);
+  p.x = -1.0;
+  po->points.push_back(p);
+  p.y = -1.0;
+  po->points.push_back(p);
+  p.x = 1.0;
+  po->points.push_back(p);
+  po->closed = true;
+  po->value = POLYGON_VAL;
+
+  return po;
+}
+
+nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject(
+  const std::vector<unsigned char> & uuid)
+{
+  nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared<nav2_msgs::msg::CircleObject>();
+  co->header.frame_id = GLOBAL_FRAME_ID;
+  if (uuid.size() == 16) {
+    // uuid could be optionally specified
+    std::array<unsigned char, 16> uuid_array;
+    std::copy(uuid.begin(), uuid.end(), uuid_array.begin());
+    co->uuid.uuid = uuid_array;
+  }
+  co->center.x = 3.0;
+  co->center.y = 0.0;
+  co->radius = 1.0;
+  co->fill = true;
+  co->value = CIRCLE_VAL;
+
+  return co;
+}
+
+template<class T>
+typename T::Response::SharedPtr Tester::sendRequest(
+  typename nav2::ServiceClient<T>::SharedPtr client,
+  typename T::Request::SharedPtr request,
+  const std::chrono::nanoseconds & timeout)
+{
+  auto result_future = client->async_call(request);
+
+  rclcpp::Time start_time = vo_server_->now();
+  while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) {
+    std::future_status status = result_future.wait_for(10ms);
+    if (status == std::future_status::ready) {
+      return result_future.get();
+    }
+    rclcpp::spin_some(vo_server_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+  return nullptr;
+}
+
+void Tester::mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map)
+{
+  map_ = map;
+}
+
+bool Tester::waitMap(const std::chrono::nanoseconds & timeout)
+{
+  rclcpp::Time start_time = vo_server_->now();
+  while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) {
+    if (map_) {
+      return true;
+    }
+    rclcpp::spin_some(vo_server_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+  return false;
+}
+
+void Tester::verifyMap(bool is_circle, double poly_x_end, double circle_cx)
+{
+  // Wait for map to be received
+  const std::chrono::nanoseconds timeout =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(1s * 1.5 / UPDATE_FREQUENCY);
+  ASSERT_TRUE(waitMap(timeout));
+
+  // Map should contain polygon and circle and will look like:
+  //
+  // polygon {x1, y1} (and map's origin): should be always {-1.0, -1.0}
+  // V
+  // xxxxxx....xxx.
+  // xxxxxx...xxxxx < circle is optionally placed on map
+  // xxxxxx...xxxxx
+  // xxxxxx....xxx.
+  //      ^
+  //      polygon {x2, y2}. x2 = poly_x_end; y2 - is always == 1.0
+
+  // Polygon {x2, y2} in map coordinates
+  const unsigned int m_poly_x2 = 9 + poly_x_end * 10;
+  const unsigned int m_poly_y2 = 19;
+
+  // Center of the circle in map coordinates (expressed in floating-point for best precision)
+  const double m_circle_cx = 9 + circle_cx * 10 + 0.5;  // cell's origin + 0.5 center of cell
+  const double m_circle_cy = 9 + 0.5;  // cell's origin + 0.5 center of cell
+
+  // Radius of the circle in map coordinates
+  const double m_circle_rad = 10.0;
+
+  for (unsigned int my = 0; my < map_->info.height; my++) {
+    for (unsigned int mx = 0; mx < map_->info.width; mx++) {
+      if (mx <= m_poly_x2 && my <= m_poly_y2) {
+        // Point belongs to the polygon
+        ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL);
+      } else if (is_circle && std::hypot(m_circle_cx - mx, m_circle_cy - my) <= m_circle_rad) {
+        // Point belongs to the circle
+        ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL);
+      } else {
+        // Point does not belong to any shape
+        ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN);
+      }
+    }
+  }
+}
+
+void Tester::comparePolygonObjects(
+  nav2_msgs::msg::PolygonObject::SharedPtr p1,
+  nav2_msgs::msg::PolygonObject::SharedPtr p2)
+{
+  ASSERT_EQ(p1->header.frame_id, p2->header.frame_id);
+  ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid);
+  ASSERT_EQ(p1->points.size(), p2->points.size());
+  for (unsigned int i = 0; i < p1->points.size(); i++) {
+    ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON);
+    ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON);
+  }
+  ASSERT_EQ(p1->closed, p2->closed);
+  ASSERT_EQ(p1->value, p2->value);
+}
+
+void Tester::compareCircleObjects(
+  nav2_msgs::msg::CircleObject::SharedPtr c1,
+  nav2_msgs::msg::CircleObject::SharedPtr c2)
+{
+  ASSERT_EQ(c1->header.frame_id, c2->header.frame_id);
+  ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid);
+  ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON);
+  ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON);
+  ASSERT_NEAR(c1->radius, c2->radius, EPSILON);
+  ASSERT_EQ(c1->fill, c2->fill);
+  ASSERT_EQ(c1->value, c2->value);
+}
+
+// ---------- ROS-parameters tests ----------
+TEST_F(Tester, testObtainParams)
+{
+  setVOServerParams();
+  setShapesParams(
+    "01010101-0101-0101-0101-010101010101",
+    "01010101-0101-0101-0101-010101010102");
+  vo_server_->start();
+
+  verifyMap(true);
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testObtainNoShapes)
+{
+  setVOServerParams();
+  // Set shapes array, but does not set shapes themselves
+  vo_server_->declare_parameter(
+    "shapes", rclcpp::ParameterValue(std::vector<std::string>{POLYGON_NAME, CIRCLE_NAME}));
+  vo_server_->set_parameter(
+    rclcpp::Parameter("shapes", std::vector<std::string>{POLYGON_NAME, CIRCLE_NAME}));
+
+  // Configured that way, vo_server_ should give the failure on configuration stage
+  vo_server_->configureFail();
+
+  // Cleaning-up vo_server_ w/o deactivation
+  vo_server_->cleanup();
+}
+
+TEST_F(Tester, testIncorrectShapeType)
+{
+  setVOServerParams();
+  setShapesParams("", "");
+  // Setting incorrect type of circle
+  vo_server_->set_parameter(
+    rclcpp::Parameter(std::string(CIRCLE_NAME) + ".type", "incorrect_type"));
+
+  // Configured that way, vo_server_ should give the failure on configuration stage
+  vo_server_->configureFail();
+
+  // Cleaning-up vo_server_ w/o deactivation
+  vo_server_->cleanup();
+}
+
+TEST_F(Tester, testIncorrectPolygonParams)
+{
+  setVOServerParams();
+  // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test
+  setShapesParams(
+    "incorrect_polygon_uuid",
+    "01010101-0101-0101-0101-010101010102");
+
+  // Set that way, vo_server_ should give the failure on configuration stage
+  vo_server_->configureFail();
+
+  // Cleaning-up vo_server_ w/o deactivation
+  vo_server_->cleanup();
+}
+
+TEST_F(Tester, testIncorrectCircleParams)
+{
+  setVOServerParams();
+  // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test
+  setShapesParams(
+    "01010101-0101-0101-0101-010101010101",
+    "incorrect_circle_uuid");
+
+  // Set that way, vo_server_ should give the failure on configuration stage
+  vo_server_->configureFail();
+
+  // Cleaning-up vo_server_ w/o deactivation
+  vo_server_->cleanup();
+}
+
+// ---------- Service call tests ----------
+TEST_F(Tester, testAddShapes)
+{
+  setVOServerParams();
+  vo_server_->start();
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Check the map has been updated correctly
+  verifyMap(true);
+
+  // Check that getShapes.srv will return correct shapes
+  auto get_shapes_msg = std::make_shared<nav2_msgs::srv::GetShapes::Request>();
+  auto get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  // Verify obtained polygon
+  ASSERT_EQ(get_shapes_result->polygons.size(), 1u);
+  auto p_check = std::make_shared<nav2_msgs::msg::PolygonObject>(get_shapes_result->polygons[0]);
+  comparePolygonObjects(p_check, po_msg);
+  // Verify obtained circle
+  ASSERT_EQ(get_shapes_result->circles.size(), 1u);
+  auto c_check = std::make_shared<nav2_msgs::msg::CircleObject>(get_shapes_result->circles[0]);
+  compareCircleObjects(c_check, co_msg);
+
+  // Now move X-coordinate of polygon's border and
+  // add a new circle fully placed inside first one
+  // through AddShapes.srv:
+  // Update polygon x2-coordinate to 1.5
+  po_msg->points[0].x = 1.5;
+  po_msg->points[3].x = 1.5;
+  // Prepare second circle object (fully placed inside first circle)
+  auto co2_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3});
+  co2_msg->radius = 0.5;
+
+  add_shapes_msg->polygons.clear();
+  add_shapes_msg->circles.clear();
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  add_shapes_msg->circles.push_back(*co2_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Check the map has been updated correctly
+  verifyMap(true, 1.5);
+
+  // Check that getShapes.srv will return correct shapes
+  get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  // Verify obtained polygon
+  ASSERT_EQ(get_shapes_result->polygons.size(), 1u);
+  p_check = std::make_shared<nav2_msgs::msg::PolygonObject>(get_shapes_result->polygons[0]);
+  comparePolygonObjects(p_check, po_msg);
+  // Verify obtained circles
+  ASSERT_EQ(get_shapes_result->circles.size(), 2u);
+  c_check = std::make_shared<nav2_msgs::msg::CircleObject>(get_shapes_result->circles[0]);
+  compareCircleObjects(c_check, co_msg);
+  c_check = std::make_shared<nav2_msgs::msg::CircleObject>(get_shapes_result->circles[1]);
+  compareCircleObjects(c_check, co2_msg);
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testRemoveShapes)
+{
+  setVOServerParams();
+  vo_server_->start();
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Check that getShapes.srv will return correct shapes
+  auto get_shapes_msg = std::make_shared<nav2_msgs::srv::GetShapes::Request>();
+  auto get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  // Verify obtained polygon
+  ASSERT_EQ(get_shapes_result->polygons.size(), 1u);
+  auto p_check = std::make_shared<nav2_msgs::msg::PolygonObject>(get_shapes_result->polygons[0]);
+  comparePolygonObjects(p_check, po_msg);
+  // Verify obtained circle
+  ASSERT_EQ(get_shapes_result->circles.size(), 1u);
+  auto c_check = std::make_shared<nav2_msgs::msg::CircleObject>(get_shapes_result->circles[0]);
+  compareCircleObjects(c_check, co_msg);
+
+  // Now remove circle from map
+  auto remove_shapes_msg = std::make_shared<nav2_msgs::srv::RemoveShapes::Request>();
+  remove_shapes_msg->all_objects = false;
+  unique_identifier_msgs::msg::UUID uuid;
+  uuid.uuid = std::array<unsigned char, 16>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2};
+  remove_shapes_msg->uuids.push_back(uuid);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto remove_shapes_result =
+    sendRequest<nav2_msgs::srv::RemoveShapes>(remove_shapes_client_, remove_shapes_msg, 2s);
+  ASSERT_NE(remove_shapes_result, nullptr);
+  ASSERT_TRUE(remove_shapes_result->success);
+
+  // Check that there is only polygon remained on map
+  verifyMap(false);
+
+  // Check that getShapes.srv will return only polygon
+  get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  // Verify obtained polygon
+  ASSERT_EQ(get_shapes_result->polygons.size(), 1u);
+  p_check = std::make_shared<nav2_msgs::msg::PolygonObject>(get_shapes_result->polygons[0]);
+  comparePolygonObjects(p_check, po_msg);
+  // Verify obtained circle
+  ASSERT_EQ(get_shapes_result->circles.size(), 0u);
+
+  // Then call RemoveShapes.srv with enabled all_objects parameter
+  remove_shapes_msg->all_objects = true;
+  remove_shapes_msg->uuids.clear();
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  remove_shapes_result =
+    sendRequest<nav2_msgs::srv::RemoveShapes>(remove_shapes_client_, remove_shapes_msg, 2s);
+  ASSERT_NE(remove_shapes_result, nullptr);
+  ASSERT_TRUE(remove_shapes_result->success);
+
+  // Check that map is empty
+  ASSERT_EQ(map_->info.width, 1);
+  ASSERT_EQ(map_->info.height, 1);
+  ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN);
+
+  // getShapes.srv should return empty vectors
+  get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  ASSERT_EQ(get_shapes_result->polygons.size(), 0u);
+  ASSERT_EQ(get_shapes_result->circles.size(), 0u);
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testAddIncorrectShapes)
+{
+  setVOServerParams();
+  vo_server_->start();
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Check that getShapes.srv will return correct shapes
+  auto get_shapes_msg = std::make_shared<nav2_msgs::srv::GetShapes::Request>();
+  auto get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  // Verify obtained polygon
+  ASSERT_EQ(get_shapes_result->polygons.size(), 1u);
+  auto p_check = std::make_shared<nav2_msgs::msg::PolygonObject>(get_shapes_result->polygons[0]);
+  comparePolygonObjects(p_check, po_msg);
+  // Verify obtained circle
+  ASSERT_EQ(get_shapes_result->circles.size(), 1u);
+  auto c_check = std::make_shared<nav2_msgs::msg::CircleObject>(get_shapes_result->circles[0]);
+  compareCircleObjects(c_check, co_msg);
+
+  // Then try to update polygon with circle's uuid
+  po_msg->uuid.uuid[15] = 2;
+  add_shapes_msg->polygons.clear();
+  add_shapes_msg->circles.clear();
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_FALSE(add_shapes_result->success);
+  po_msg->uuid.uuid[15] = 1;  // Restoring back for further usage
+
+  // Then try to update circle with polygon's uuid
+  co_msg->uuid.uuid[15] = 1;
+  add_shapes_msg->polygons.clear();
+  add_shapes_msg->circles.clear();
+  add_shapes_msg->circles.push_back(*co_msg);
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_FALSE(add_shapes_result->success);
+  co_msg->uuid.uuid[15] = 2;  // Restoring back for further usage
+
+  // Try to update polygon with incorrect number of points
+  po_msg->points.resize(2);
+  add_shapes_msg->polygons.clear();
+  add_shapes_msg->circles.clear();
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_FALSE(add_shapes_result->success);
+
+  // Try to add new incorrect polygon
+  po_msg->uuid.uuid[15] = 100;
+  add_shapes_msg->polygons.clear();
+  add_shapes_msg->circles.clear();
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_FALSE(add_shapes_result->success);
+  // Restoring back for further usage
+  po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+
+  // Try to update circle with incorrect radius
+  co_msg->radius = -1.0;
+  add_shapes_msg->polygons.clear();
+  add_shapes_msg->circles.clear();
+  add_shapes_msg->circles.push_back(*co_msg);
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_FALSE(add_shapes_result->success);
+
+  // Try to add new incorrect circle
+  co_msg->uuid.uuid[15] = 100;
+  add_shapes_msg->polygons.clear();
+  add_shapes_msg->circles.clear();
+  add_shapes_msg->circles.push_back(*co_msg);
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_FALSE(add_shapes_result->success);
+  co_msg->radius = 1.0;  // Restoring back for further usage
+  co_msg->uuid.uuid[15] = 2;  // Restoring back for further usage
+
+  // Verify that map did not corrupted after all false-manipulations
+  verifyMap(true);
+
+  // Check that getShapes.srv will return correct shapes after all false-manipulations
+  get_shapes_msg = std::make_shared<nav2_msgs::srv::GetShapes::Request>();
+  get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  // Verify obtained polygon
+  ASSERT_EQ(get_shapes_result->polygons.size(), 1u);
+  p_check = std::make_shared<nav2_msgs::msg::PolygonObject>(get_shapes_result->polygons[0]);
+  comparePolygonObjects(p_check, po_msg);
+  // Verify obtained circle
+  ASSERT_EQ(get_shapes_result->circles.size(), 1u);
+  c_check = std::make_shared<nav2_msgs::msg::CircleObject>(get_shapes_result->circles[0]);
+  compareCircleObjects(c_check, co_msg);
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testRemoveIncorrectShapes)
+{
+  setVOServerParams();
+  vo_server_->start();
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Check that getShapes.srv will return correct shapes
+  auto get_shapes_msg = std::make_shared<nav2_msgs::srv::GetShapes::Request>();
+  auto get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  // Verify obtained polygon
+  ASSERT_EQ(get_shapes_result->polygons.size(), 1u);
+  auto p_check = std::make_shared<nav2_msgs::msg::PolygonObject>(get_shapes_result->polygons[0]);
+  comparePolygonObjects(p_check, po_msg);
+  // Verify obtained circle
+  ASSERT_EQ(get_shapes_result->circles.size(), 1u);
+  auto c_check = std::make_shared<nav2_msgs::msg::CircleObject>(get_shapes_result->circles[0]);
+  compareCircleObjects(c_check, co_msg);
+
+  // Then try to remove two shapes: non-existing shape and circle
+  auto remove_shapes_msg = std::make_shared<nav2_msgs::srv::RemoveShapes::Request>();
+  remove_shapes_msg->all_objects = false;
+  unique_identifier_msgs::msg::UUID uuid;
+  uuid.uuid = std::array<unsigned char, 16>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 100};
+  remove_shapes_msg->uuids.push_back(uuid);
+  uuid.uuid = std::array<unsigned char, 16>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2};
+  remove_shapes_msg->uuids.push_back(uuid);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto remove_shapes_result =
+    sendRequest<nav2_msgs::srv::RemoveShapes>(remove_shapes_client_, remove_shapes_msg, 2s);
+  ASSERT_NE(remove_shapes_result, nullptr);
+  ASSERT_FALSE(remove_shapes_result->success);
+
+  // Check that despite on the error, the circle was removed from map
+  verifyMap(false);
+
+  // Check that getShapes.srv will return only polygon
+  get_shapes_result =
+    sendRequest<nav2_msgs::srv::GetShapes>(get_shapes_client_, get_shapes_msg, 2s);
+  ASSERT_NE(get_shapes_result, nullptr);
+  // Verify obtained polygon
+  ASSERT_EQ(get_shapes_result->polygons.size(), 1u);
+  p_check = std::make_shared<nav2_msgs::msg::PolygonObject>(get_shapes_result->polygons[0]);
+  comparePolygonObjects(p_check, po_msg);
+  // Verify obtained circle
+  ASSERT_EQ(get_shapes_result->circles.size(), 0u);
+
+  vo_server_->stop();
+}
+
+// ---------- Overlay tests ----------
+TEST_F(Tester, testOverlayMax)
+{
+  setVOServerParams();
+  vo_server_->set_parameter(
+    rclcpp::Parameter(
+      "overlay_type",
+      static_cast<int>(nav2_map_server::OverlayType::OVERLAY_MAX)));
+  vo_server_->start();
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+  // Make circle to be overlapped with polygon
+  co_msg->center.x = 1.0;
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Wait for the map
+  waitMap(500ms);
+
+  // Verify that overlap on map generated correctly
+  double my = 9;
+  double mx = 1;  // inside polygon
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL);
+  mx = 14;  // in the overlapping area
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL);
+  mx = 20;  // inside circle
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL);
+  my = 18;  // outside any shape
+  mx = 28;  // outside any shape
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN);
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testOverlayMin)
+{
+  setVOServerParams();
+  vo_server_->set_parameter(
+    rclcpp::Parameter(
+      "overlay_type",
+      static_cast<int>(nav2_map_server::OverlayType::OVERLAY_MIN)));
+  vo_server_->start();
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+  // Make circle to be overlapped with polygon
+  co_msg->center.x = 1.0;
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Wait for the map
+  waitMap(500ms);
+
+  // Verify that overlap on map generated correctly
+  double my = 9;
+  double mx = 1;  // inside polygon
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL);
+  mx = 14;  // in the overlapping area
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL);
+  mx = 20;  // inside circle
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL);
+  my = 18;  // outside any shape
+  mx = 28;  // outside any shape
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN);
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testOverlaySeq)
+{
+  setVOServerParams();
+  vo_server_->set_parameter(
+    rclcpp::Parameter(
+      "overlay_type",
+      static_cast<int>(nav2_map_server::OverlayType::OVERLAY_SEQ)));
+  vo_server_->start();
+
+  // Sequentially add polygon and then circle overlapped with it on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+
+  // Prepare first polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  add_shapes_msg->polygons.push_back(*po_msg);
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Then prepare circle object to add over the polygon
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+  // Make circle to be overlapped with polygon
+  co_msg->center.x = 1.0;
+  // Also check that putBorders part works correctly
+  co_msg->fill = false;
+  add_shapes_msg->polygons.clear();
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Wait for the map
+  waitMap(500ms);
+
+  // Verify that overlap on map generated correctly
+  double my = 9;
+  double mx = 1;  // inside polygon
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL);
+  mx = 10;  // on the circle border laying over the polygon
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL);
+  mx = 14;  // inside circle and polygon
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL);
+  mx = 24;  // inside circle only
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN);
+  mx = 29;  // on the circle border laying outside the polygon
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL);
+  my = 18;  // outside any shape
+  mx = 28;  // outside any shape
+  ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN);
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testOverlayUnknown)
+{
+  setVOServerParams();
+  vo_server_->set_parameter(
+    rclcpp::Parameter("overlay_type", static_cast<int>(1000)));
+  vo_server_->start();
+
+  // Try to add polygon on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  auto po_msg = makePolygonObject(std::vector<unsigned char>());
+  add_shapes_msg->polygons.push_back(*po_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Wait for the map
+  waitMap(500ms);
+
+  // Check that polygon was not added on map: map is empty
+  ASSERT_EQ(map_->info.width, 1);
+  ASSERT_EQ(map_->info.height, 1);
+  ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN);
+
+  vo_server_->stop();
+}
+
+// ---------- Transform/dynamic tests ----------
+TEST_F(Tester, testShapesMove)
+{
+  setVOServerParams();
+  vo_server_->start();
+
+  // Wait for the initial map to be received to not get it in later updates
+  const std::chrono::nanoseconds timeout =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(1s * 1.5 / UPDATE_FREQUENCY);
+  ASSERT_TRUE(waitMap(timeout));
+
+  // No shift between polygon and map
+  sendTransform(0.0);
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Polygon and circle to be in another moving frame
+  po_msg->header.frame_id = SHAPE_FRAME_ID;
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Check that polygon and circle are in correct positions on map
+  verifyMap(true);
+
+  // Move frame with polygon
+  sendTransform(0.5);
+
+  // Map is being published dynamically. Wait for the map to be published one more time
+  map_.reset();
+  // Check that polygon and circle are in correct positions on map
+  verifyMap(true, 1.0, 2.5);  // Polygon is moved 0.5m towards to the circle
+  // Check that map's origin was updated in accordance to polygon movement
+  ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON);  // -1.0 + 0.5
+  ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON);
+
+  // Then try to publish polygon in incorrect frame
+  add_shapes_msg->polygons[0].header.frame_id = "incorrect_frame";
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Having incorrect transform, map should not be published
+  ASSERT_FALSE(waitMap(timeout));
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testSwitchDynamicStatic)
+{
+  setVOServerParams();
+  vo_server_->start();
+
+  // Wait for the initial map to be received to not get it in later updates
+  const std::chrono::nanoseconds timeout =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(1s * 1.5 / UPDATE_FREQUENCY);
+  ASSERT_TRUE(waitMap(timeout));
+
+  // 0.5m shift between polygon and map
+  sendTransform(0.5);
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Polygon to be in different frame
+  po_msg->header.frame_id = SHAPE_FRAME_ID;
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Check that polygon and circle are in correct positions on map
+  verifyMap(true, 1.0, 2.5);  // Polygon is moved 0.5m towards to the circle
+  // Check that map's origin was updated in accordance to polygon movement
+  ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON);  // -1.0 + 0.5
+  ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON);
+
+  // Then return to the static model and ensure the everything works correctly
+  add_shapes_msg->polygons[0].header.frame_id = GLOBAL_FRAME_ID;
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  verifyMap(true);
+
+  vo_server_->stop();
+}
+
+// ---------- Corner cases ----------
+TEST_F(Tester, switchProcessMap)
+{
+  setVOServerParams();
+  vo_server_->start();
+
+  // Wait for the initial map to be received to not get it in later updates
+  const std::chrono::nanoseconds timeout =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(1s * 1.5 / UPDATE_FREQUENCY);
+  ASSERT_TRUE(waitMap(timeout));
+
+  // Check that received default map is empty
+  ASSERT_EQ(map_->header.frame_id, "map");
+  ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON);
+  ASSERT_EQ(map_->info.width, 1);
+  ASSERT_EQ(map_->info.height, 1);
+
+  // Disable map processing and trying to obtain the map
+  vo_server_->setProcessMap(false);
+
+  // Switching map update by calling remove service
+  auto remove_shapes_msg = std::make_shared<nav2_msgs::srv::RemoveShapes::Request>();
+  remove_shapes_msg->all_objects = true;
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto remove_shapes_result =
+    sendRequest<nav2_msgs::srv::RemoveShapes>(remove_shapes_client_, remove_shapes_msg, 2s);
+  ASSERT_NE(remove_shapes_result, nullptr);
+  ASSERT_TRUE(remove_shapes_result->success);
+
+  // Having process_map_ disabled, map should not be published
+  ASSERT_FALSE(waitMap(timeout));
+
+  // Then enable map processing and trying to obtain the map
+  vo_server_->setProcessMap(true);
+
+  // Switching map update by calling remove service
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  remove_shapes_result =
+    sendRequest<nav2_msgs::srv::RemoveShapes>(remove_shapes_client_, remove_shapes_msg, 2s);
+  ASSERT_NE(remove_shapes_result, nullptr);
+  ASSERT_TRUE(remove_shapes_result->success);
+
+  // Ensure that map is being updated
+  ASSERT_TRUE(waitMap(timeout));
+
+  // ... and it is empty
+  ASSERT_EQ(map_->header.frame_id, "map");
+  ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON);
+  ASSERT_EQ(map_->info.width, 1);
+  ASSERT_EQ(map_->info.height, 1);
+
+  vo_server_->stop();
+}
+
+TEST_F(Tester, testIncorrectMapBoundaries) {
+  setVOServerParams();
+  vo_server_->start();
+
+  // Set min_x > max_x
+  EXPECT_THROW(vo_server_->updateMap(1.0, 0.1, 0.1, 1.0), std::runtime_error);
+
+  // Set min_y > max_y
+  EXPECT_THROW(vo_server_->updateMap(0.1, 1.0, 1.0, 0.1), std::runtime_error);
+}
+
+TEST_F(Tester, testIncorrectMapBoundariesWhenNoShapes) {
+  setVOServerParams();
+  vo_server_->start();
+  double min_x, min_y, max_x, max_y;
+  EXPECT_THROW(vo_server_->getMapBoundaries(min_x, min_y, max_x, max_y), std::runtime_error);
+}
+
+TEST_F(Tester, testShapeOutsideMap) {
+  setVOServerParams();
+  vo_server_->start();
+
+  // Add polygon and circle on map
+  auto add_shapes_msg = std::make_shared<nav2_msgs::srv::AddShapes::Request>();
+  // Prepare polygon object to add
+  auto po_msg = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
+  // Prepare circle object to add
+  auto co_msg = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2});
+
+  add_shapes_msg->polygons.push_back(*po_msg);
+  add_shapes_msg->circles.push_back(*co_msg);
+  map_.reset();  // Resetting the map to ensure that updated one is received later
+  auto add_shapes_result =
+    sendRequest<nav2_msgs::srv::AddShapes>(add_shapes_client_, add_shapes_msg, 2s);
+  ASSERT_NE(add_shapes_result, nullptr);
+  ASSERT_TRUE(add_shapes_result->success);
+
+  // Check the map has been updated correctly
+  verifyMap(true);
+
+  // Modify the map to have a shape outside the map
+  vo_server_->updateMap(2.0, 2.0, 4.0, 4.0);
+
+  // Try to put the shapes back in the map
+  vo_server_->putVectorObjectsOnMap();
+
+  // Verify that map did not corrupted after all false-manipulations
+  verifyMap(true);
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize the system
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  // Actual testing
+  bool test_result = RUN_ALL_TESTS();
+
+  // Shutdown
+  rclcpp::shutdown();
+
+  return test_result;
+}
diff --git a/nav2_map_server/test/unit/test_vector_object_shapes.cpp b/nav2_map_server/test/unit/test_vector_object_shapes.cpp
new file mode 100644
index 00000000000..43a6f43a58d
--- /dev/null
+++ b/nav2_map_server/test/unit/test_vector_object_shapes.cpp
@@ -0,0 +1,805 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// 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 <gtest/gtest.h>
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/point32.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/transform_broadcaster.hpp"
+
+#include "nav2_msgs/msg/polygon_object.hpp"
+#include "nav2_msgs/msg/circle_object.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_util/occ_grid_utils.hpp"
+
+#include "nav2_map_server/vector_object_shapes.hpp"
+#include "nav2_map_server/vector_object_utils.hpp"
+
+static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
+
+static const char POLYGON_NAME[]{"Polygon"};
+static const char CIRCLE_NAME[]{"Circle"};
+static const char GLOBAL_FRAME_ID[]{"map"};
+static const char SHAPE_FRAME_ID[]{"shape"};
+static double FRAME_SHIFT = 0.1;
+static std::vector<double> POINTS{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0};
+static std::vector<double> CENTER{0.0, 0.0};
+
+class PolygonWrapper : public nav2_map_server::Polygon
+{
+public:
+  explicit PolygonWrapper(const nav2::LifecycleNode::WeakPtr & node)
+  : Polygon(node)
+  {}
+
+  geometry_msgs::msg::Polygon::SharedPtr getPoly()
+  {
+    return polygon_;
+  }
+};  // PolygonWrapper
+
+class CircleWrapper : public nav2_map_server::Circle
+{
+public:
+  explicit CircleWrapper(const nav2::LifecycleNode::WeakPtr & node)
+  : Circle(node)
+  {}
+
+  geometry_msgs::msg::Point32::SharedPtr getCenter()
+  {
+    return center_;
+  }
+};  // CircleWrapper
+
+class Tester : public ::testing::Test
+{
+public:
+  Tester();
+  ~Tester();
+
+protected:
+  void setPolygonParams(const std::string & uuid);
+  void setCircleParams(const std::string & uuid);
+  nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject(
+    const std::vector<unsigned char> & uuid);
+  nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject(
+    const std::vector<unsigned char> & uuid);
+
+  void sendTransform();
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr makeMap();
+  void verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map);
+  void verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map);
+  void verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map);
+
+  void comparePolygonObjects(
+    nav2_msgs::msg::PolygonObject::SharedPtr p1,
+    nav2_msgs::msg::PolygonObject::SharedPtr p2);
+  void compareCircleObjects(
+    nav2_msgs::msg::CircleObject::SharedPtr c1,
+    nav2_msgs::msg::CircleObject::SharedPtr c2);
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  std::shared_ptr<PolygonWrapper> polygon_;
+  std::shared_ptr<CircleWrapper> circle_;
+
+  nav2::LifecycleNode::SharedPtr node_;
+};  // Tester
+
+Tester::Tester()
+{
+  node_ = std::make_shared<nav2::LifecycleNode>("test_node");
+
+  // Create shapes
+  polygon_ = std::make_shared<PolygonWrapper>(node_);
+  circle_ = std::make_shared<CircleWrapper>(node_);
+
+  // Transform buffer and listener initialization
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_buffer_->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+}
+
+Tester::~Tester()
+{
+  polygon_.reset();
+  circle_.reset();
+
+  node_.reset();
+
+  tf_listener_.reset();
+  tf_buffer_.reset();
+}
+
+void Tester::setPolygonParams(const std::string & uuid)
+{
+  const std::string polygon_name(POLYGON_NAME);
+
+  node_->declare_parameter(
+    polygon_name + ".type", rclcpp::ParameterValue("polygon"));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".type", "polygon"));
+
+  node_->declare_parameter(
+    polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID));
+
+  node_->declare_parameter(
+    polygon_name + ".closed", rclcpp::ParameterValue(true));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".closed", true));
+
+  node_->declare_parameter(
+    polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED));
+
+  node_->declare_parameter(
+    polygon_name + ".points", rclcpp::ParameterValue(POINTS));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".points", POINTS));
+
+  if (!uuid.empty()) {
+    node_->declare_parameter(
+      polygon_name + ".uuid", rclcpp::ParameterValue(uuid));
+    node_->set_parameter(
+      rclcpp::Parameter(polygon_name + ".uuid", uuid));
+  }
+}
+
+void Tester::setCircleParams(const std::string & uuid)
+{
+  const std::string circle_name(CIRCLE_NAME);
+
+  node_->declare_parameter(
+    circle_name + ".type", rclcpp::ParameterValue("circle"));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".type", "circle"));
+
+  node_->declare_parameter(
+    circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID));
+
+  node_->declare_parameter(
+    circle_name + ".fill", rclcpp::ParameterValue(true));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".fill", true));
+
+  node_->declare_parameter(
+    circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED));
+
+  node_->declare_parameter(
+    circle_name + ".center", rclcpp::ParameterValue(CENTER));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".center", CENTER));
+
+  node_->declare_parameter(
+    circle_name + ".radius", rclcpp::ParameterValue(1.0));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".radius", 1.0));
+
+  if (!uuid.empty()) {
+    node_->declare_parameter(
+      circle_name + ".uuid", rclcpp::ParameterValue(uuid));
+    node_->set_parameter(
+      rclcpp::Parameter(circle_name + ".uuid", uuid));
+  }
+}
+
+nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject(
+  const std::vector<unsigned char> & uuid)
+{
+  nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared<nav2_msgs::msg::PolygonObject>();
+  po->header.frame_id = GLOBAL_FRAME_ID;
+  if (uuid.size() == 16) {
+    // uuid could be optionally specified
+    std::array<unsigned char, 16> uuid_array;
+    std::copy(uuid.begin(), uuid.end(), uuid_array.begin());
+    po->uuid.uuid = uuid_array;
+  }
+  geometry_msgs::msg::Point32 p;
+  p.x = 1.0;
+  p.y = 1.0;
+  po->points.push_back(p);
+  p.x = -1.0;
+  po->points.push_back(p);
+  p.y = -1.0;
+  po->points.push_back(p);
+  p.x = 1.0;
+  po->points.push_back(p);
+  po->closed = true;
+  po->value = nav2_util::OCC_GRID_OCCUPIED;
+
+  return po;
+}
+
+nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject(
+  const std::vector<unsigned char> & uuid)
+{
+  nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared<nav2_msgs::msg::CircleObject>();
+  co->header.frame_id = GLOBAL_FRAME_ID;
+  if (uuid.size() == 16) {
+    // uuid could be optionally specified
+    std::array<unsigned char, 16> uuid_array;
+    std::copy(uuid.begin(), uuid.end(), uuid_array.begin());
+    co->uuid.uuid = uuid_array;
+  }
+  co->center.x = 0.0;
+  co->center.y = 0.0;
+  co->radius = 1.0;
+  co->fill = true;
+  co->value = nav2_util::OCC_GRID_OCCUPIED;
+
+  return co;
+}
+
+void Tester::sendTransform()
+{
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster =
+    std::make_shared<tf2_ros::TransformBroadcaster>(node_);
+
+  geometry_msgs::msg::TransformStamped transform;
+
+  // global_frame -> shape_frame transform
+  transform.header.frame_id = GLOBAL_FRAME_ID;
+  transform.child_frame_id = SHAPE_FRAME_ID;
+
+  transform.header.stamp = node_->now();
+  transform.transform.translation.x = FRAME_SHIFT;
+  transform.transform.translation.y = FRAME_SHIFT;
+  transform.transform.translation.z = 0.0;
+  transform.transform.rotation.x = 0.0;
+  transform.transform.rotation.y = 0.0;
+  transform.transform.rotation.z = 0.0;
+  transform.transform.rotation.w = 1.0;
+
+  tf_broadcaster->sendTransform(transform);
+}
+
+nav_msgs::msg::OccupancyGrid::SharedPtr Tester::makeMap()
+{
+  nav_msgs::msg::OccupancyGrid::SharedPtr map = std::make_shared<nav_msgs::msg::OccupancyGrid>();
+  map->header.frame_id = GLOBAL_FRAME_ID;
+  map->info.resolution = 0.1;
+  map->info.width = 40;
+  map->info.height = 40;
+  map->info.origin.position.x = -2.0;
+  map->info.origin.position.y = -2.0;
+  map->data = std::vector<int8_t>(400 * 400, nav2_util::OCC_GRID_FREE);
+
+  return map;
+}
+
+void Tester::verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map)
+{
+  // Map is expected to be as follows:
+  // 0 0 0 0 0 0 0 0
+  // 0 0 x x x x 0 0
+  // 0 0 x 0 0 0 0 0
+  // 0 0 x 0 0 0 0 0
+  // 0 0 x x x x 0 0
+  // 0 0 0 0 0 0 0 0
+  const unsigned int map_center_x = 19;
+  const unsigned int map_center_y = 19;
+
+  for (unsigned int my = 0; my < map->info.height; my++) {
+    for (unsigned int mx = 0; mx < map->info.width; mx++) {
+      if (my == map_center_y - 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) {
+        // 1st border
+        ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED);
+      } else if (mx == map_center_x - 10 && my >= map_center_y - 10 && my <= map_center_y + 10) {
+        // 2nd border
+        ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED);
+      } else if (my == map_center_y + 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) {
+        // 3rd border
+        ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED);
+      } else {
+        // Does not belong to any border
+        ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE);
+      }
+    }
+  }
+}
+
+void Tester::verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map)
+{
+  // Circle center in cell coordinates
+  const double circle_center_x = 19.5;  // 19 cell's origin + 0.5 center of cell
+  const double circle_center_y = 19.5;  // 19 cell's origin + 0.5 center of cell
+
+  double radius;
+  for (unsigned int my = 0; my < map->info.height; my++) {
+    for (unsigned int mx = 0; mx < map->info.width; mx++) {
+      if (map->data[my * map->info.width + mx] == nav2_util::OCC_GRID_OCCUPIED) {
+        radius = std::hypot(circle_center_x - mx, circle_center_y - my);
+        ASSERT_NEAR(radius, 10.0, 1.0);  // Border drift no more than once cell
+      }
+    }
+  }
+}
+
+void Tester::verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map)
+{
+  for (unsigned int my = 0; my < map->info.height; my++) {
+    for (unsigned int mx = 0; mx < map->info.width; mx++) {
+      ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE);
+    }
+  }
+}
+
+void Tester::comparePolygonObjects(
+  nav2_msgs::msg::PolygonObject::SharedPtr p1,
+  nav2_msgs::msg::PolygonObject::SharedPtr p2)
+{
+  ASSERT_EQ(p1->header.frame_id, p2->header.frame_id);
+  ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid);
+  ASSERT_EQ(p1->points.size(), p2->points.size());
+  for (unsigned int i = 0; i < p1->points.size(); i++) {
+    ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON);
+    ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON);
+  }
+  ASSERT_EQ(p1->closed, p2->closed);
+  ASSERT_EQ(p1->value, p2->value);
+}
+
+void Tester::compareCircleObjects(
+  nav2_msgs::msg::CircleObject::SharedPtr c1,
+  nav2_msgs::msg::CircleObject::SharedPtr c2)
+{
+  ASSERT_EQ(c1->header.frame_id, c2->header.frame_id);
+  ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid);
+  ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON);
+  ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON);
+  ASSERT_NEAR(c1->radius, c2->radius, EPSILON);
+  ASSERT_EQ(c1->fill, c2->fill);
+  ASSERT_EQ(c1->value, c2->value);
+}
+
+//---------- Polygon testcases ----------
+
+TEST_F(Tester, testPolygonObtainParams)
+{
+  setPolygonParams("01010101-0101-0101-0101-010101010101");
+  ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME));
+
+  ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON);
+  ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED);
+  ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID);
+  ASSERT_EQ(polygon_->isFill(), true);
+  ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-010101010101");
+}
+
+TEST_F(Tester, testPolygonObtainIncorrectParams)
+{
+  // Setting polygon parameters w/o points
+  const std::string polygon_name(POLYGON_NAME);
+
+  node_->declare_parameter(
+    polygon_name + ".type", rclcpp::ParameterValue("polygon"));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".type", "polygon"));
+
+  node_->declare_parameter(
+    polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID));
+
+  node_->declare_parameter(
+    polygon_name + ".closed", rclcpp::ParameterValue(true));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".closed", true));
+
+  node_->declare_parameter(
+    polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED));
+
+  node_->declare_parameter(
+    polygon_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY);
+
+  // Check that points is mandatory parameter for the polygon
+  ASSERT_FALSE(polygon_->obtainParams(polygon_name));
+
+  // Setting incorrect number of points
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".points", std::vector{1.0, 2.0, 3.0}));
+  ASSERT_FALSE(polygon_->obtainParams(polygon_name));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".points", POINTS));
+
+  // Setting incorrect UUID
+  node_->declare_parameter(
+    polygon_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid"));
+  node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".uuid", "incorrect-uuid"));
+  ASSERT_FALSE(polygon_->obtainParams(polygon_name));
+}
+
+TEST_F(Tester, testPolygonSetParams)
+{
+  nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2});
+
+  // Check that parameters were set correctly
+  ASSERT_TRUE(polygon_->setParams(po));
+  ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON);
+  ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED);
+  ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID);
+  ASSERT_EQ(polygon_->isFill(), true);
+  ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-020202020202");
+
+  // Verify that getting the parameters also works correctly
+  nav2_msgs::msg::PolygonObject::SharedPtr params = polygon_->getParams();
+  comparePolygonObjects(params, po);
+}
+
+TEST_F(Tester, testPolygonSetIncorrectParams)
+{
+  nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject(
+    std::vector<unsigned char>());
+  // Setting incorrect number of vertices
+  po->points.resize(2);
+
+  // And check that it is failed to set these parameters
+  ASSERT_FALSE(polygon_->setParams(po));
+}
+
+TEST_F(Tester, testPolygonBoundaries)
+{
+  setPolygonParams("");
+  ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME));
+
+  double min_x, min_y, max_x, max_y;
+  polygon_->getBoundaries(min_x, min_y, max_x, max_y);
+
+  ASSERT_NEAR(min_x, -1.0, EPSILON);
+  ASSERT_NEAR(min_y, -1.0, EPSILON);
+  ASSERT_NEAR(max_x, 1.0, EPSILON);
+  ASSERT_NEAR(max_y, 1.0, EPSILON);
+}
+
+TEST_F(Tester, testPolygonPoints)
+{
+  setPolygonParams("");
+  ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME));
+
+  ASSERT_TRUE(polygon_->isPointInside(0.0, 0.0));
+  ASSERT_FALSE(polygon_->isPointInside(-2.0, -2.0));
+  ASSERT_FALSE(polygon_->isPointInside(2.0, 2.0));
+}
+
+TEST_F(Tester, testPolygonBorders)
+{
+  setPolygonParams("");
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false));
+  ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME));
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap();
+
+  polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ);
+
+  verifyPolygonBorders(map);
+}
+
+TEST_F(Tester, testPolygonBordersOutOfBoundaries)
+{
+  setPolygonParams("");
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false));
+  // Set polygon to be out of map
+  node_->set_parameter(
+    rclcpp::Parameter(
+      std::string(POLYGON_NAME) + ".points",
+      std::vector<double>{5.0, 5.0, 6.0, 5.0, 5.0, 6.0}));
+  ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME));
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap();
+
+  polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ);
+
+  verifyMapEmpty(map);
+}
+
+TEST_F(Tester, testPolygonBordersOnePointInsideBoundaries)
+{
+  setPolygonParams("");
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false));
+  // Now, set the first point inside the map and the others out of the map
+  node_->set_parameter(
+    rclcpp::Parameter(
+      std::string(POLYGON_NAME) + ".points",
+      std::vector<double>{0.5, 0.5, 6.0, 5.0, 5.0, 6.0}));
+  ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME));
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap();
+
+  polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ);
+
+  verifyMapEmpty(map);
+}
+
+TEST_F(Tester, testPolygonDifferentFrame)
+{
+  setPolygonParams("");
+  // Change shape's frame ID to differ from global one
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + ".frame_id", SHAPE_FRAME_ID));
+  ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME));
+
+  sendTransform();
+
+  // Check that shape coordinates are not transformed
+  geometry_msgs::msg::Polygon::SharedPtr poly = polygon_->getPoly();
+  ASSERT_NEAR(poly->points[0].x, 1.0, EPSILON);
+  ASSERT_NEAR(poly->points[0].y, 1.0, EPSILON);
+  ASSERT_NEAR(poly->points[1].x, -1.0, EPSILON);
+  ASSERT_NEAR(poly->points[1].y, 1.0, EPSILON);
+  ASSERT_NEAR(poly->points[2].x, -1.0, EPSILON);
+  ASSERT_NEAR(poly->points[2].y, -1.0, EPSILON);
+  ASSERT_NEAR(poly->points[3].x, 1.0, EPSILON);
+  ASSERT_NEAR(poly->points[3].y, -1.0, EPSILON);
+
+  // Transform shape coordinates to global frame
+  ASSERT_TRUE(polygon_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0));
+
+  // Verify that shape coordinates were transformed to global frame successfully
+  poly = polygon_->getPoly();
+  ASSERT_NEAR(poly->points[0].x, 1.0 + FRAME_SHIFT, EPSILON);
+  ASSERT_NEAR(poly->points[0].y, 1.0 + FRAME_SHIFT, EPSILON);
+  ASSERT_NEAR(poly->points[1].x, -1.0 + FRAME_SHIFT, EPSILON);
+  ASSERT_NEAR(poly->points[1].y, 1.0 + FRAME_SHIFT, EPSILON);
+  ASSERT_NEAR(poly->points[2].x, -1.0 + FRAME_SHIFT, EPSILON);
+  ASSERT_NEAR(poly->points[2].y, -1.0 + FRAME_SHIFT, EPSILON);
+  ASSERT_NEAR(poly->points[3].x, 1.0 + FRAME_SHIFT, EPSILON);
+  ASSERT_NEAR(poly->points[3].y, -1.0 + FRAME_SHIFT, EPSILON);
+
+  // Try to transform to incorrect frame
+  ASSERT_FALSE(polygon_->toFrame("incorrect_frame", tf_buffer_, 0.1));
+}
+
+//---------- Circles testcases ----------
+
+TEST_F(Tester, testCircleObtainParams)
+{
+  setCircleParams("01010101-0101-0101-0101-010101010101");
+  ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME));
+
+  ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE);
+  ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED);
+  ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID);
+  ASSERT_EQ(circle_->isFill(), true);
+  ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-010101010101");
+}
+
+TEST_F(Tester, testCircleObtainIncorrectParams)
+{
+  const std::string circle_name(CIRCLE_NAME);
+
+  // Setting circle parameters w/o center and radius
+  node_->declare_parameter(
+    circle_name + ".type", rclcpp::ParameterValue("circle"));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".type", "circle"));
+
+  node_->declare_parameter(
+    circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID));
+
+  node_->declare_parameter(
+    circle_name + ".fill", rclcpp::ParameterValue(true));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".fill", true));
+
+  node_->declare_parameter(
+    circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED));
+
+  node_->declare_parameter(
+    circle_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY);
+  node_->declare_parameter(
+    circle_name + ".radius", rclcpp::PARAMETER_DOUBLE);
+
+  // Check that center and radius are mandatory parameter for the circle
+  ASSERT_FALSE(circle_->obtainParams(circle_name));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".center", CENTER));
+
+  // Setting incorrect radius
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".radius", -1.0));
+  ASSERT_FALSE(circle_->obtainParams(circle_name));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".radius", 1.0));
+
+  // Setting incorrect center format
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".center", std::vector<double>{0.0}));
+  ASSERT_FALSE(circle_->obtainParams(circle_name));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".center", CENTER));
+
+  // Setting incorrect UUID
+  node_->declare_parameter(
+    circle_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid"));
+  node_->set_parameter(
+    rclcpp::Parameter(circle_name + ".uuid", "incorrect-uuid"));
+  ASSERT_FALSE(circle_->obtainParams(circle_name));
+}
+
+TEST_F(Tester, testCircleSetParams)
+{
+  nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject(
+    std::vector<unsigned char>{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2});
+
+  // Check that parameters were set correctly
+  ASSERT_TRUE(circle_->setParams(co));
+  ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE);
+  ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED);
+  ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID);
+  ASSERT_EQ(circle_->isFill(), true);
+  ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-020202020202");
+
+  // Verify that getting the parameters also works correctly
+  nav2_msgs::msg::CircleObject::SharedPtr params = circle_->getParams();
+  compareCircleObjects(params, co);
+}
+
+TEST_F(Tester, testCircleSetIncorrectParams)
+{
+  nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject(
+    std::vector<unsigned char>());
+  // Setting incorrect radius
+  co->radius = -1.0;
+
+  // And check that it is failed to set these parameters
+  ASSERT_FALSE(circle_->setParams(co));
+}
+
+TEST_F(Tester, testCircleBoundaries)
+{
+  setCircleParams("");
+  ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME));
+
+  double min_x, min_y, max_x, max_y;
+  circle_->getBoundaries(min_x, min_y, max_x, max_y);
+
+  ASSERT_NEAR(min_x, -1.0, EPSILON);
+  ASSERT_NEAR(min_y, -1.0, EPSILON);
+  ASSERT_NEAR(max_x, 1.0, EPSILON);
+  ASSERT_NEAR(max_y, 1.0, EPSILON);
+}
+
+TEST_F(Tester, testCirclePoints)
+{
+  setCircleParams("");
+  ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME));
+
+  ASSERT_TRUE(circle_->isPointInside(0.0, 0.0));
+  ASSERT_FALSE(circle_->isPointInside(-1.0, -1.0));
+  ASSERT_FALSE(circle_->isPointInside(1.0, 1.0));
+}
+
+TEST_F(Tester, testCircleBorders)
+{
+  setCircleParams("");
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false));
+  ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME));
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap();
+
+  circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ);
+
+  verifyCircleBorders(map);
+}
+
+TEST_F(Tester, testCircleBordersOutOfBoundaries)
+{
+  setCircleParams("");
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false));
+  // Set circle to be out of map
+  node_->set_parameter(
+    rclcpp::Parameter(
+      std::string(CIRCLE_NAME) + ".center",
+      std::vector<double>{5.0, 5.0}));
+  ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME));
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap();
+
+  circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ);
+
+  verifyMapEmpty(map);
+}
+
+TEST_F(Tester, testCircleBordersOutsideMap)
+{
+  setCircleParams("");
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false));
+  // Set circle to be out of map
+  node_->set_parameter(
+    rclcpp::Parameter(
+      std::string(CIRCLE_NAME) + ".center",
+      std::vector<double>{-3.0, -3.0}));
+  ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME));
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap();
+
+  circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ);
+
+  verifyMapEmpty(map);
+}
+
+TEST_F(Tester, testCircleDifferentFrame)
+{
+  setCircleParams("");
+  // Change shape's frame ID to differ from global one
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(CIRCLE_NAME) + ".frame_id", SHAPE_FRAME_ID));
+  ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME));
+
+  sendTransform();
+
+  // Check that shape coordinates are not transformed
+  geometry_msgs::msg::Point32::SharedPtr center = circle_->getCenter();
+  ASSERT_NEAR(center->x, 0.0, EPSILON);
+  ASSERT_NEAR(center->y, 0.0, EPSILON);
+  // Transform shape coordinates to global frame
+  ASSERT_TRUE(circle_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0));
+
+  // Verify that shape coordinates were transformed to global frame successfully
+  center = circle_->getCenter();
+  ASSERT_NEAR(center->x, FRAME_SHIFT, EPSILON);
+  ASSERT_NEAR(center->y, FRAME_SHIFT, EPSILON);
+
+  // Try to transform to incorrect frame
+  ASSERT_FALSE(circle_->toFrame("incorrect_frame", tf_buffer_, 0.1));
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize the system
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  // Actual testing
+  bool test_result = RUN_ALL_TESTS();
+
+  // Shutdown
+  rclcpp::shutdown();
+
+  return test_result;
+}
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index 14d1327508b..41311173486 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geographic_msgs)
 find_package(action_msgs REQUIRED)
+find_package(unique_identifier_msgs REQUIRED)
 
 nav2_package()
 
@@ -41,6 +42,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Trajectory.msg"
   "msg/TrajectoryPoint.msg"
   "srv/GetCosts.srv"
+  "msg/PolygonObject.msg"
+  "msg/CircleObject.msg"
   "srv/GetCostmap.srv"
   "srv/IsPathValid.srv"
   "srv/ClearCostmapExceptRegion.srv"
@@ -52,6 +55,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SaveMap.srv"
   "srv/SetInitialPose.srv"
   "srv/ReloadDockDatabase.srv"
+  "srv/AddShapes.srv"
+  "srv/RemoveShapes.srv"
+  "srv/GetShapes.srv"
   "srv/SetRouteGraph.srv"
   "srv/DynamicEdges.srv"
   "action/AssistedTeleop.action"
@@ -72,7 +78,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/UndockRobot.action"
   "action/ComputeRoute.action"
   "action/ComputeAndTrackRoute.action"
-  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs unique_identifier_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)
diff --git a/nav2_msgs/msg/CircleObject.msg b/nav2_msgs/msg/CircleObject.msg
new file mode 100644
index 00000000000..2aa64adab62
--- /dev/null
+++ b/nav2_msgs/msg/CircleObject.msg
@@ -0,0 +1,6 @@
+std_msgs/Header header
+unique_identifier_msgs/UUID uuid
+geometry_msgs/Point32 center
+float32 radius
+bool fill
+int8 value
diff --git a/nav2_msgs/msg/PolygonObject.msg b/nav2_msgs/msg/PolygonObject.msg
new file mode 100644
index 00000000000..b5c9721c629
--- /dev/null
+++ b/nav2_msgs/msg/PolygonObject.msg
@@ -0,0 +1,5 @@
+std_msgs/Header header
+unique_identifier_msgs/UUID uuid
+geometry_msgs/Point32[] points
+bool closed
+int8 value
diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml
index 61e84addc37..33de880662e 100644
--- a/nav2_msgs/package.xml
+++ b/nav2_msgs/package.xml
@@ -21,6 +21,7 @@
   <depend>action_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>geographic_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
 
diff --git a/nav2_msgs/srv/AddShapes.srv b/nav2_msgs/srv/AddShapes.srv
new file mode 100644
index 00000000000..1b3b5753874
--- /dev/null
+++ b/nav2_msgs/srv/AddShapes.srv
@@ -0,0 +1,6 @@
+# Add/update vector objects on map
+
+CircleObject[] circles
+PolygonObject[] polygons
+---
+bool success
diff --git a/nav2_msgs/srv/GetShapes.srv b/nav2_msgs/srv/GetShapes.srv
new file mode 100644
index 00000000000..79b7d6c431c
--- /dev/null
+++ b/nav2_msgs/srv/GetShapes.srv
@@ -0,0 +1,5 @@
+# Get vector objects which are now being applied on map
+
+---
+CircleObject[] circles
+PolygonObject[] polygons
diff --git a/nav2_msgs/srv/RemoveShapes.srv b/nav2_msgs/srv/RemoveShapes.srv
new file mode 100644
index 00000000000..a001f9360ef
--- /dev/null
+++ b/nav2_msgs/srv/RemoveShapes.srv
@@ -0,0 +1,6 @@
+# Remove vector objects from map
+
+bool all_objects
+unique_identifier_msgs/UUID[] uuids
+---
+bool success
diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp
index 3a56cb5feea..d67f17e3c7c 100644
--- a/nav2_util/include/nav2_util/geometry_utils.hpp
+++ b/nav2_util/include/nav2_util/geometry_utils.hpp
@@ -188,6 +188,49 @@ inline int find_next_matching_goal_in_waypoint_statuses(
   return itr - waypoint_statuses.begin();
 }
 
+/**
+ * @brief Checks if point is inside the polygon
+ * @param px X-coordinate of the given point to check
+ * @param py Y-coordinate of the given point to check
+ * @param polygon Polygon to check if the point is inside
+ * @return True if given point is inside polygon, otherwise false
+ */
+template<class PointT>
+inline bool isPointInsidePolygon(
+  const double px, const double py, const std::vector<PointT> & polygon)
+{
+  // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."
+  // Communications of the ACM 5.8 (1962): 434.
+  // Implementation of ray crossings algorithm for point in polygon task solving.
+  // Y coordinate is fixed. Moving the ray on X+ axis starting from given point.
+  // Odd number of intersections with polygon boundaries means the point is inside polygon.
+  const int points_num = polygon.size();
+  int i, j;  // Polygon vertex iterators
+  bool res = false;  // Final result, initialized with already inverted value
+
+  // Starting from the edge where the last point of polygon is connected to the first
+  i = points_num - 1;
+  for (j = 0; j < points_num; j++) {
+    // Checking the edge only if given point is between edge boundaries by Y coordinates.
+    // One of the condition should contain equality in order to exclude the edges
+    // parallel to X+ ray.
+    if ((py <= polygon[i].y) == (py > polygon[j].y)) {
+      // Calculating the intersection coordinate of X+ ray
+      const double x_inter = polygon[i].x +
+        (py - polygon[i].y) *
+        (polygon[j].x - polygon[i].x) /
+        (polygon[j].y - polygon[i].y);
+      // If intersection with checked edge is greater than point x coordinate,
+      // inverting the result
+      if (x_inter > px) {
+        res = !res;
+      }
+    }
+    i = j;
+  }
+  return res;
+}
+
 }  // namespace geometry_utils
 }  // namespace nav2_util
 
diff --git a/nav2_util/include/nav2_util/occ_grid_utils.hpp b/nav2_util/include/nav2_util/occ_grid_utils.hpp
new file mode 100644
index 00000000000..26bb8740f2b
--- /dev/null
+++ b/nav2_util/include/nav2_util/occ_grid_utils.hpp
@@ -0,0 +1,100 @@
+// Copyright (c) 2008, 2013, Willow Garage, Inc.
+// Copyright (c) 2023 Samsung R&D Institute Russia
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// 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 <ORGANIZATION> 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 OWNER 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.
+//
+// Author: Eitan Marder-Eppstein
+//         David V. Lu!!
+//         Alexey Merzlyakov
+
+#ifndef NAV2_UTIL__OCC_GRID_UTILS_HPP_
+#define NAV2_UTIL__OCC_GRID_UTILS_HPP_
+
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+namespace nav2_util
+{
+
+/**
+ * @brief: Convert from world coordinates to map coordinates.
+   Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s.
+ * @param  map OccupancyGrid map on which to convert
+ * @param  wx The x world coordinate
+ * @param  wy The y world coordinate
+ * @param  mx Will be set to the associated map x coordinate
+ * @param  my Will be set to the associated map y coordinate
+ * @return True if the conversion was successful (legal bounds) false otherwise
+ */
+inline bool worldToMap(
+  nav_msgs::msg::OccupancyGrid::ConstSharedPtr map,
+  const double wx, const double wy, unsigned int & mx, unsigned int & my)
+{
+  const double origin_x = map->info.origin.position.x;
+  const double origin_y = map->info.origin.position.y;
+  const double resolution = map->info.resolution;
+  const unsigned int size_x = map->info.width;
+  const unsigned int size_y = map->info.height;
+
+  if (wx < origin_x || wy < origin_y) {
+    return false;
+  }
+
+  mx = static_cast<unsigned int>((wx - origin_x) / resolution);
+  my = static_cast<unsigned int>((wy - origin_y) / resolution);
+  if (mx >= size_x || my >= size_y) {
+    return false;
+  }
+
+  return true;
+}
+
+/**
+ * @brief  Convert from map coordinates to world coordinates
+ * @param  mx The x map coordinate
+ * @param  my The y map coordinate
+ * @param  wx Will be set to the associated world x coordinate
+ * @param  wy Will be set to the associated world y coordinate
+ */
+inline void mapToWorld(
+  nav_msgs::msg::OccupancyGrid::ConstSharedPtr map,
+  const unsigned int mx, const unsigned int my, double & wx, double & wy)
+{
+  const double origin_x = map->info.origin.position.x;
+  const double origin_y = map->info.origin.position.y;
+  const double resolution = map->info.resolution;
+
+  wx = origin_x + (mx + 0.5) * resolution;
+  wy = origin_y + (my + 0.5) * resolution;
+}
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__OCC_GRID_UTILS_HPP_
diff --git a/nav2_util/include/nav2_util/raytrace_line_2d.hpp b/nav2_util/include/nav2_util/raytrace_line_2d.hpp
new file mode 100644
index 00000000000..b44d1e79a8d
--- /dev/null
+++ b/nav2_util/include/nav2_util/raytrace_line_2d.hpp
@@ -0,0 +1,146 @@
+// Copyright (c) 2008, 2013, Willow Garage, Inc.
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// 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 Willow Garage, Inc. 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 OWNER 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.
+//
+// Author: Eitan Marder-Eppstein
+//         David V. Lu!!
+
+#ifndef NAV2_UTIL__RAYTRACE_LINE_2D_HPP_
+#define NAV2_UTIL__RAYTRACE_LINE_2D_HPP_
+
+#include <limits.h>
+#include <algorithm>
+#include <cmath>
+
+namespace nav2_util
+{
+
+/**
+ * @brief get the sign of an int
+ */
+inline int sign(int x)
+{
+  return x > 0 ? 1 : -1;
+}
+
+/**
+ * @brief  A 2D implementation of Bresenham's raytracing algorithm...
+ * applies an action at each step
+ */
+template<class ActionType>
+inline void bresenham2D(
+  ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b,
+  int offset_a, int offset_b, unsigned int offset,
+  unsigned int max_length)
+{
+  unsigned int end = std::min(max_length, abs_da);
+  for (unsigned int i = 0; i < end; ++i) {
+    at(offset);
+    offset += offset_a;
+    error_b += abs_db;
+    if ((unsigned int)error_b >= abs_da) {
+      offset += offset_b;
+      error_b -= abs_da;
+    }
+  }
+  at(offset);
+}
+
+/**
+ * @brief  Raytrace a line and apply some action at each step
+ * @param  at The action to take... a functor
+ * @param  x0 The starting x coordinate
+ * @param  y0 The starting y coordinate
+ * @param  x1 The ending x coordinate
+ * @param  y1 The ending y coordinate
+ * @param  step_x OX-step on map
+ * @param  max_length The maximum desired length of the segment...
+ * allows you to not go all the way to the endpoint
+ * @param  min_length The minimum desired length of the segment
+ */
+template<class ActionType>
+inline void raytraceLine(
+  ActionType at, unsigned int x0, unsigned int y0, unsigned int x1,
+  unsigned int y1, unsigned int step_x,
+  unsigned int max_length = UINT_MAX, unsigned int min_length = 0)
+{
+  int dx_full = x1 - x0;
+  int dy_full = y1 - y0;
+
+  // we need to chose how much to scale our dominant dimension,
+  // based on the maximum length of the line
+  double dist = std::hypot(dx_full, dy_full);
+  if (dist < min_length) {
+    return;
+  }
+
+  unsigned int min_x0, min_y0;
+  if (dist > 0.0) {
+    // Adjust starting point and offset to start from min_length distance
+    min_x0 = (unsigned int)(x0 + dx_full / dist * min_length);
+    min_y0 = (unsigned int)(y0 + dy_full / dist * min_length);
+  } else {
+    // dist can be 0 if [x0, y0]==[x1, y1].
+    // In this case only this cell should be processed.
+    min_x0 = x0;
+    min_y0 = y0;
+  }
+  unsigned int offset = min_y0 * step_x + min_x0;
+
+  int dx = x1 - min_x0;
+  int dy = y1 - min_y0;
+
+  unsigned int abs_dx = abs(dx);
+  unsigned int abs_dy = abs(dy);
+
+  int offset_dx = sign(dx);
+  int offset_dy = sign(dy) * step_x;
+
+  double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
+  // if x is dominant
+  if (abs_dx >= abs_dy) {
+    int error_y = abs_dx / 2;
+
+    bresenham2D(
+      at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
+    return;
+  }
+
+  // otherwise y is dominant
+  int error_x = abs_dy / 2;
+
+  bresenham2D(
+    at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
+}
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__RAYTRACE_LINE_2D_HPP_
diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt
index 09d7d4a38c0..c8447eaab6e 100644
--- a/nav2_util/test/CMakeLists.txt
+++ b/nav2_util/test/CMakeLists.txt
@@ -1,3 +1,5 @@
+add_subdirectory(regression)
+
 ament_add_gtest(test_execution_timer test_execution_timer.cpp)
 target_link_libraries(test_execution_timer ${library_name})
 
diff --git a/nav2_util/test/regression/CMakeLists.txt b/nav2_util/test/regression/CMakeLists.txt
new file mode 100644
index 00000000000..f216fbe7751
--- /dev/null
+++ b/nav2_util/test/regression/CMakeLists.txt
@@ -0,0 +1,3 @@
+# Bresenham2D corner cases test
+ament_add_gtest(map_bresenham_2d map_bresenham_2d.cpp)
+target_link_libraries(map_bresenham_2d ${library_name})
diff --git a/nav2_util/test/regression/map_bresenham_2d.cpp b/nav2_util/test/regression/map_bresenham_2d.cpp
new file mode 100644
index 00000000000..220962948e5
--- /dev/null
+++ b/nav2_util/test/regression/map_bresenham_2d.cpp
@@ -0,0 +1,170 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// 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 <ORGANIZATION> 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 OWNER 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.
+//
+// Author: Alexey Merzlyakov
+
+#include <string.h>
+
+#include <gtest/gtest.h>
+
+#include "nav2_util/raytrace_line_2d.hpp"
+
+// MapAction - is a functor class used to cover raytraceLine algorithm.
+// It contains char map inside, which is an abstract one and not related
+// to any concrete representation (like Costmap2D or OccupancyGrid).
+class MapAction
+{
+public:
+  explicit MapAction(
+    char * map, unsigned int size, char mark_val = 100)
+  : map_(map), size_(size), mark_val_(mark_val)
+  {
+  }
+
+  inline void operator()(unsigned int off)
+  {
+    ASSERT_TRUE(off < size_);
+    map_[off] = mark_val_;
+  }
+
+  inline unsigned int get(unsigned int off)
+  {
+    return map_[off];
+  }
+
+private:
+  char * map_;
+  unsigned int size_;
+  char mark_val_;
+};
+
+class MapTest
+{
+public:
+  MapTest(
+    unsigned int size_x, unsigned int size_y,
+    char default_val = 0)
+  : size_x_(size_x), size_y_(size_y)
+  {
+    map_ = new char[size_x * size_y];
+    memset(map_, default_val, size_x * size_y);
+  }
+
+  char * getMap()
+  {
+    return map_;
+  }
+
+  unsigned int getSize()
+  {
+    return size_x_ * size_y_;
+  }
+
+  void raytraceLine(
+    MapAction ma, unsigned int x0, unsigned int y0, unsigned int x1,
+    unsigned int y1,
+    unsigned int max_length = UINT_MAX, unsigned int min_length = 0)
+  {
+    nav2_util::raytraceLine(ma, x0, y0, x1, y1, size_x_, max_length, min_length);
+  }
+
+private:
+  char * map_;
+  unsigned int size_x_, size_y_;
+};
+
+TEST(map_2d, bresenham2DBoundariesCheck)
+{
+  const unsigned int sz_x = 60;
+  const unsigned int sz_y = 60;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 6;
+  MapTest mt(sz_x, sz_y);
+  MapAction ma(mt.getMap(), mt.getSize());
+
+  // Initial point - some asymmetrically standing point in order to cover most corner cases
+  const unsigned int x0 = 2;
+  const unsigned int y0 = 4;
+  // (x1, y1) point will move
+  unsigned int x1, y1;
+
+  // Running on (x, 0) edge
+  y1 = 0;
+  for (x1 = 0; x1 < sz_x; x1++) {
+    mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (x, sz_y) edge
+  y1 = sz_y - 1;
+  for (x1 = 0; x1 < sz_x; x1++) {
+    mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (0, y) edge
+  x1 = 0;
+  for (y1 = 0; y1 < sz_y; y1++) {
+    mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (sz_x, y) edge
+  x1 = sz_x - 1;
+  for (y1 = 0; y1 < sz_y; y1++) {
+    mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length);
+  }
+}
+
+TEST(map_2d, bresenham2DSamePoint)
+{
+  const unsigned int sz_x = 60;
+  const unsigned int sz_y = 60;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 0;
+  MapTest mt(sz_x, sz_y, 0.1);
+  MapAction ma(mt.getMap(), mt.getSize());
+
+  // Initial point
+  const double x0 = 2;
+  const double y0 = 4;
+
+  unsigned int offset = y0 * sz_x + x0;
+  char val_before = ma.get(offset);
+  // Same point to check
+  mt.raytraceLine(ma, x0, y0, x0, y0, max_length, min_length);
+  char val_after = ma.get(offset);
+  ASSERT_FALSE(val_before == val_after);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

From f5b12f3f8dc5e0a4db039627f0c70b238611b630 Mon Sep 17 00:00:00 2001
From: Jakubach <jakubach@gmail.com>
Date: Sun, 4 Jan 2026 14:02:29 +0100
Subject: [PATCH 51/51] Nav2 update

---
 nav2_behavior_tree/CMakeLists.txt             |    3 +
 .../remove_in_collision_goals_action.hpp      |    4 +
 .../action/remove_passed_goals_action.hpp     |    5 +
 .../condition/is_path_valid_condition.hpp     |    2 +-
 .../remove_in_collision_goals_action.cpp      |   52 +-
 .../action/remove_passed_goals_action.cpp     |   63 +-
 nav2_bringup/launch/cbf_supervisor_launch.py  |   27 +
 nav2_bringup/launch/tb4_simulation_launch.py  |    2 +-
 nav2_bringup/params/nav2_params.yaml          |  865 +++-
 ...hrough_poses_w_replanning_and_recovery.xml |   54 -
 ...h_poses_w_replanning_and_recovery_copy.xml |  157 +
 ...ning_and_recovery_copy_test_replanning.xml |  160 +
 ...s_w_replanning_and_recovery_if_invalid.xml |  181 +
 ...ng_and_recovery_if_invalid_copy copy 2.xml |  134 +
 ...ning_and_recovery_if_invalid_copy copy.xml |   88 +
 ...eplanning_and_recovery_if_invalid_copy.xml |  157 +
 .../footprint_collision_checker copy.hpp      |   94 +
 .../nav2_costmap_2d/obstacle_layer.hpp        |    1 +
 .../include/nav2_costmap_2d/static_layer.hpp  |    1 +
 nav2_costmap_2d/plugins/obstacle_layer.cpp    |   11 +-
 nav2_costmap_2d/plugins/static_layer.cpp      |   13 +-
 .../src/footprint_collision_checker copy.cpp  |  268 ++
 .../footprint_collision_checker_test copy.cpp |  793 ++++
 .../opennav_docking/src/docking_server.cpp    |    1 +
 nav2_mppi_controller/CMakeLists copy.txt      |  199 +
 nav2_mppi_controller/CMakeLists.txt           |    3 +
 nav2_mppi_controller/critics.xml              |    6 +
 .../collision_checker.hpp                     |  148 +
 .../nav2_mppi_controller/controller.hpp       |   15 +-
 .../critic_data copy 2.hpp                    |   58 +
 .../nav2_mppi_controller/critic_data.hpp      |    6 +-
 .../nav2_mppi_controller/critic_function.hpp  |    5 +-
 .../nav2_mppi_controller/critic_manager.hpp   |    1 +
 .../critics/cost_critic copy.hpp              |  109 +
 .../critics/cost_critic.hpp                   |   34 +-
 .../critics/obstacles_critic copy 3.hpp       |  108 +
 .../critics/path_align_critic.hpp             |    6 +
 .../critics/path_angle_critic.hpp             |    5 +
 .../critics/path_follow_critic.hpp            |    6 +
 .../critics/waypoint_critic.hpp               |  112 +
 .../fixing_trajectory_validator.hpp           |   71 +
 .../nav2_mppi_controller/optimizer copy 2.hpp |  295 ++
 .../nav2_mppi_controller/optimizer.hpp        |   37 +-
 .../tools/path_handler copy.hpp               |  159 +
 .../tools/path_handler.hpp                    |   10 +-
 .../tools/path_handler_mod.hpp                |  159 +
 .../tools/path_handler_mod_final.hpp          |  159 +
 .../tools/path_handler_original.hpp           |  157 +
 .../tools/trajectory_visualizer copy 2.hpp    |  118 +
 .../tools/trajectory_visualizer.hpp           |   88 +-
 .../tools/utils copy 2.hpp                    |  689 +++
 .../nav2_mppi_controller/tools/utils copy.hpp |  824 ++++
 .../tools/utils copy_mod.hpp                  |  757 ++++
 .../tools/utils copy_original.hpp             |  705 +++
 .../nav2_mppi_controller/tools/utils.hpp      |  132 +-
 .../tools/utils_mod_final.hpp                 |  757 ++++
 .../src/collision_checker.cpp                 |  394 ++
 .../src/controller copy 2.cpp                 |  157 +
 nav2_mppi_controller/src/controller.cpp       |   66 +-
 nav2_mppi_controller/src/critic_manager.cpp   |   38 +-
 .../src/critics/cost_critic copy 2.cpp        |  284 ++
 .../src/critics/cost_critic copy 3.cpp        |  297 ++
 .../src/critics/cost_critic copy.cpp          |  198 +
 .../src/critics/cost_critic.cpp               |   60 +-
 .../src/critics/cost_critic_clear.cpp         |  281 ++
 .../src/critics/obstacles_critic copy 3.cpp   |  249 ++
 .../src/critics/obstacles_critic copy.cpp     |  373 ++
 .../src/critics/obstacles_critic.cpp          |    7 +
 .../src/critics/path_align_critic.cpp         |   28 +
 .../src/critics/path_angle_critic.cpp         |   25 +
 .../src/critics/path_follow_critic.cpp        |   25 +
 .../src/critics/waypoint_critic.cpp           |  207 +
 nav2_mppi_controller/src/optimizer copy 2.cpp |  586 +++
 nav2_mppi_controller/src/optimizer.cpp        |    1 +
 .../src/path_handler copy 2.cpp               |  247 ++
 .../src/path_handler copy.cpp                 |  247 ++
 nav2_mppi_controller/src/path_handler.cpp     |   60 +-
 nav2_mppi_controller/src/path_handler_mod.cpp |  247 ++
 .../src/path_handler_original.cpp             |  205 +
 .../fixing_trajectory_validator.cpp           |  407 ++
 .../src/trajectory_visualizer copy 2.cpp      |  156 +
 .../src/trajectory_visualizer.cpp             |  364 +-
 .../test/critic_manager_test copy 2.cpp       |  160 +
 .../test/critic_manager_test.cpp              |    5 +-
 .../test/critics_tests copy 2.cpp             |  794 ++++
 nav2_mppi_controller/test/critics_tests.cpp   |   38 +-
 .../trajectory_visualizer_tests copy 2.cpp    |  229 +
 .../test/trajectory_visualizer_tests.cpp      |   32 +-
 .../test/utils_test copy 2.cpp                |  614 +++
 nav2_mppi_controller/test/utils_test.cpp      |   17 +-
 .../trajectory_validators.xml                 |    7 +
 .../nav2_simple_commander/robot_navigator.py  |    4 +-
 .../lattice_primitives/config.json            |    4 +-
 .../generate_motion_primitives.py             |    6 +-
 .../lattice_primitives/lattice_generator.py   |    6 +-
 .../lattice_primitives/output.json            | 3864 +++++++++++++++++
 .../0.5m_turning_radius/diff/output1.json     | 3864 +++++++++++++++++
 .../lattice_primitives/trajectory.py          |    2 +-
 .../trajectory_generator.py                   |    2 +-
 .../lattice_primitives/visualizations/0.0.png |  Bin 0 -> 12662 bytes
 .../visualizations/26.56505117707799.png      |  Bin 0 -> 15826 bytes
 .../visualizations/45.0.png                   |  Bin 0 -> 11922 bytes
 .../visualizations/63.43494882292201.png      |  Bin 0 -> 16013 bytes
 .../visualizations/90.0.png                   |  Bin 0 -> 12672 bytes
 .../visualizations/all_trajectories.png       |  Bin 0 -> 42112 bytes
 .../include/nav2_util/controller_utils.hpp    |   15 +
 nav2_util/src/controller_utils.cpp            |   49 +-
 107 files changed, 23563 insertions(+), 432 deletions(-)
 create mode 100644 nav2_bringup/launch/cbf_supervisor_launch.py
 delete mode 100644 nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
 create mode 100644 nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml
 create mode 100644 nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml
 create mode 100644 nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid.xml
 create mode 100644 nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy 2.xml
 create mode 100644 nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy.xml
 create mode 100644 nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml
 create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker copy.hpp
 create mode 100644 nav2_costmap_2d/src/footprint_collision_checker copy.cpp
 create mode 100644 nav2_costmap_2d/test/unit/footprint_collision_checker_test copy.cpp
 create mode 100644 nav2_mppi_controller/CMakeLists copy.txt
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critic_data copy 2.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic copy.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic copy 3.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/waypoint_critic.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/fixing_trajectory_validator.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/optimizer copy 2.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler copy.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod_final.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_original.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer copy 2.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy 2.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_mod.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_original.hpp
 create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/tools/utils_mod_final.hpp
 create mode 100644 nav2_mppi_controller/src/collision_checker.cpp
 create mode 100644 nav2_mppi_controller/src/controller copy 2.cpp
 create mode 100644 nav2_mppi_controller/src/critics/cost_critic copy 2.cpp
 create mode 100644 nav2_mppi_controller/src/critics/cost_critic copy 3.cpp
 create mode 100644 nav2_mppi_controller/src/critics/cost_critic copy.cpp
 create mode 100644 nav2_mppi_controller/src/critics/cost_critic_clear.cpp
 create mode 100644 nav2_mppi_controller/src/critics/obstacles_critic copy 3.cpp
 create mode 100644 nav2_mppi_controller/src/critics/obstacles_critic copy.cpp
 create mode 100644 nav2_mppi_controller/src/critics/waypoint_critic.cpp
 create mode 100644 nav2_mppi_controller/src/optimizer copy 2.cpp
 create mode 100644 nav2_mppi_controller/src/path_handler copy 2.cpp
 create mode 100644 nav2_mppi_controller/src/path_handler copy.cpp
 create mode 100644 nav2_mppi_controller/src/path_handler_mod.cpp
 create mode 100644 nav2_mppi_controller/src/path_handler_original.cpp
 create mode 100644 nav2_mppi_controller/src/trajectory_validators/fixing_trajectory_validator.cpp
 create mode 100644 nav2_mppi_controller/src/trajectory_visualizer copy 2.cpp
 create mode 100644 nav2_mppi_controller/test/critic_manager_test copy 2.cpp
 create mode 100644 nav2_mppi_controller/test/critics_tests copy 2.cpp
 create mode 100644 nav2_mppi_controller/test/trajectory_visualizer_tests copy 2.cpp
 create mode 100644 nav2_mppi_controller/test/utils_test copy 2.cpp
 create mode 100644 nav2_smac_planner/lattice_primitives/output.json
 create mode 100644 nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output1.json
 create mode 100644 nav2_smac_planner/lattice_primitives/visualizations/0.0.png
 create mode 100644 nav2_smac_planner/lattice_primitives/visualizations/26.56505117707799.png
 create mode 100644 nav2_smac_planner/lattice_primitives/visualizations/45.0.png
 create mode 100644 nav2_smac_planner/lattice_primitives/visualizations/63.43494882292201.png
 create mode 100644 nav2_smac_planner/lattice_primitives/visualizations/90.0.png
 create mode 100644 nav2_smac_planner/lattice_primitives/visualizations/all_trajectories.png

diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index 2004b5c62b3..6aa40c5ed05 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -19,6 +19,7 @@ find_package(std_srvs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_ros_common REQUIRED)
+find_package(angles REQUIRED)
 
 nav2_package()
 
@@ -33,6 +34,7 @@ target_include_directories(${library_name}
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${library_name} PUBLIC
   ${action_msgs_TARGETS}
+  angles::angles
   behaviortree_cpp::behaviortree_cpp
   ${geometry_msgs_TARGETS}
   ${nav_msgs_TARGETS}
@@ -275,6 +277,7 @@ foreach(bt_plugin ${plugin_libs})
     ${std_msgs_TARGETS}
     ${std_srvs_TARGETS}
     nav2_ros_common::nav2_ros_common
+    angles::angles
   )
   target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
 endforeach()
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
index a76394e2ffe..d402d37fde5 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
@@ -73,6 +73,9 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
         BT::InputPort<bool>(
           "consider_unknown_as_obstacle", false,
           "Whether to consider unknown cost as obstacle"),
+        BT::InputPort<int>(
+          "nb_goals_to_consider", 5,
+          "Number of goals to consider for collision checking"),
         BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
           "Goals with in-collision goals removed"),
         BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
@@ -85,6 +88,7 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
 private:
   bool use_footprint_;
   bool consider_unknown_as_obstacle_;
+  int nb_goals_to_consider_;
   double cost_threshold_;
   nav_msgs::msg::Goals input_goals_;
 };
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
index a651318379f..d7271c3fbe1 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
@@ -62,6 +62,9 @@ class RemovePassedGoals : public BT::ActionNodeBase
           "Goals with passed viapoints removed"),
       BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
       BT::InputPort<std::string>("robot_base_frame", "Robot base frame"),
+      BT::InputPort<int>("nb_goals_to_consider", 1, "Number of waypoints to consider"),
+      BT::InputPort<double>("yaw", 1.57,
+          "yaw threshold to goal for it to be considered for removal"),
       BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
           "Original waypoint_statuses to mark waypoint status from"),
       BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
@@ -74,10 +77,12 @@ class RemovePassedGoals : public BT::ActionNodeBase
   BT::NodeStatus tick() override;
 
   double viapoint_achieved_radius_;
+  double viapoint_achieved_yaw_;
   double transform_tolerance_;
   nav2::LifecycleNode::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string robot_base_frame_;
+  int nb_goals_to_consider_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
index 334eaefd6f0..f13ed225aaf 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
@@ -73,7 +73,7 @@ class IsPathValidCondition : public BT::ConditionNode
     return {
       BT::InputPort<nav_msgs::msg::Path>("path", "Path to Check"),
       BT::InputPort<std::chrono::milliseconds>("server_timeout"),
-      BT::InputPort<unsigned int>("max_cost", 253, "Maximum cost of the path"),
+      BT::InputPort<unsigned int>("max_cost", 254, "Maximum cost of the path"),
       BT::InputPort<bool>(
           "consider_unknown_as_obstacle", false,
           "Whether to consider unknown cost as obstacle")
diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
index 113c3a2f514..9c52804205b 100644
--- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
+++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
@@ -39,6 +39,7 @@ void RemoveInCollisionGoals::on_tick()
   getInput("cost_threshold", cost_threshold_);
   getInput("input_goals", input_goals_);
   getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
+  getInput("nb_goals_to_consider", nb_goals_to_consider_);
 
   if (input_goals_.goals.empty()) {
     setOutput("output_goals", input_goals_);
@@ -48,8 +49,11 @@ void RemoveInCollisionGoals::on_tick()
   request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
   request_->use_footprint = use_footprint_;
 
-  for (const auto & goal : input_goals_.goals) {
-    request_->poses.push_back(goal);
+  for (size_t i =
+    0; i < static_cast<size_t>(nb_goals_to_consider_) && i < input_goals_.goals.size();
+    ++i)
+  {
+    request_->poses.push_back(input_goals_.goals[i]);
   }
 }
 
@@ -67,35 +71,31 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
   // get the `waypoint_statuses` vector
   std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
   auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses);
-  if (!waypoint_statuses_get_res) {
-    RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!");
-  }
 
-  nav_msgs::msg::Goals valid_goal_poses;
-  for (size_t i = 0; i < response->costs.size(); ++i) {
-    if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
-      response->costs[i] < cost_threshold_)
+  for (int i = static_cast<int>(response->costs.size()) - 1; i >= 0; --i) {
+    if ((response->costs[i] != 255 || consider_unknown_as_obstacle_) &&
+      response->costs[i] >= cost_threshold_)
     {
-      valid_goal_poses.goals.push_back(input_goals_.goals[i]);
-    } else if (waypoint_statuses_get_res) {
-      using namespace nav2_util::geometry_utils;  // NOLINT
-      auto cur_waypoint_index =
-        find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, input_goals_.goals[i]);
-      if (cur_waypoint_index == -1) {
-        RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses");
-        return BT::NodeStatus::FAILURE;
+      if (waypoint_statuses_get_res) {
+        const int cur_waypoint_index =
+          nav2_util::geometry_utils::find_next_matching_goal_in_waypoint_statuses(
+            waypoint_statuses, input_goals_.goals[i]);
+
+        if (cur_waypoint_index >= 0 &&
+            static_cast<size_t>(cur_waypoint_index) < waypoint_statuses.size()) {
+          waypoint_statuses[static_cast<size_t>(cur_waypoint_index)].waypoint_status =
+            nav2_msgs::msg::WaypointStatus::SKIPPED;
+        } else {
+          RCLCPP_WARN(
+            node_->get_logger(),
+            "RemoveInCollisionGoals: No matching waypoint found for goal index %d; "
+            "skipping waypoint_statuses update.", i);
+        }
       }
-      waypoint_statuses[cur_waypoint_index].waypoint_status =
-        nav2_msgs::msg::WaypointStatus::SKIPPED;
+      input_goals_.goals.erase(input_goals_.goals.begin() + i);
     }
   }
-  // Inform if all goals have been removed
-  if (valid_goal_poses.goals.empty()) {
-    RCLCPP_INFO(
-      node_->get_logger(),
-      "All goals are in collision and have been removed from the list");
-  }
-  setOutput("output_goals", valid_goal_poses);
+  setOutput("output_goals", input_goals_);
   // set `waypoint_statuses` output
   setOutput("output_waypoint_statuses", waypoint_statuses);
 
diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
index 335d5e60c37..1fe5a5bd3f6 100644
--- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
+++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
@@ -18,6 +18,8 @@
 
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "tf2/utils.h"
+#include "angles/angles.h"
 
 #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
 
@@ -28,12 +30,14 @@ RemovePassedGoals::RemovePassedGoals(
   const std::string & name,
   const BT::NodeConfiguration & conf)
 : BT::ActionNodeBase(name, conf),
-  viapoint_achieved_radius_(0.5)
+  viapoint_achieved_radius_(0.5),
+  viapoint_achieved_yaw_(1.57)
 {}
 
 void RemovePassedGoals::initialize()
 {
   getInput("radius", viapoint_achieved_radius_);
+  getInput("yaw", viapoint_achieved_yaw_);  // new input for yaw threshold
 
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
   node_ = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
@@ -56,6 +60,9 @@ inline BT::NodeStatus RemovePassedGoals::tick()
     setOutput("output_goals", goal_poses);
     return BT::NodeStatus::SUCCESS;
   }
+  getInput("nb_goals_to_consider", nb_goals_to_consider_);
+  nb_goals_to_consider_ = std::min(nb_goals_to_consider_,
+      static_cast<int>(goal_poses.goals.size()));
 
   using namespace nav2_util::geometry_utils;  // NOLINT
 
@@ -70,33 +77,47 @@ inline BT::NodeStatus RemovePassedGoals::tick()
   // get the `waypoint_statuses` vector
   std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
   auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses);
-  if (!waypoint_statuses_get_res) {
-    RCLCPP_ERROR_ONCE(node_->get_logger(), "Missing [input_waypoint_statuses] port input!");
-  }
-
-  double dist_to_goal;
-  while (goal_poses.goals.size() > 1) {
-    dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose);
 
-    if (dist_to_goal > viapoint_achieved_radius_) {
+  bool goal_reached = false;
+  int reached_goal_index = -1;
+
+  // Iterate over the first `nb_goals_to_consider` goals
+  for (int i = 0; i < nb_goals_to_consider_; ++i) {
+    double dist_to_goal = euclidean_distance(goal_poses.goals[i].pose, current_pose.pose);
+    double yaw_goal = tf2::getYaw(goal_poses.goals[i].pose.orientation);
+    double yaw_current = tf2::getYaw(current_pose.pose.orientation);
+    double yaw_diff = std::fabs(angles::shortest_angular_distance(yaw_current, yaw_goal));
+    if ((dist_to_goal <= viapoint_achieved_radius_ && yaw_diff <= viapoint_achieved_yaw_) 
+    || dist_to_goal <= 0.2) // Added a small tolerance without yaw check to avoid robot stucked near collision (allow to achieve pose without orientation)
+    {
+      reached_goal_index = i;
+      goal_reached = true;
       break;
     }
-
-    // mark waypoint statuses before the goal is erased from goals
+  }
+  if (goal_reached) {
     if (waypoint_statuses_get_res) {
-      auto cur_waypoint_index =
-        find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[0]);
-      if (cur_waypoint_index == -1) {
-        RCLCPP_ERROR_ONCE(node_->get_logger(), "Failed to find matching goal in waypoint_statuses");
-        return BT::NodeStatus::FAILURE;
+      for (int i = 0; i <= reached_goal_index; ++i) {
+        auto cur_waypoint_index =
+          find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[i]);
+        if (cur_waypoint_index >= 0 &&
+            cur_waypoint_index < static_cast<int>(waypoint_statuses.size()))
+        {
+          waypoint_statuses.at(cur_waypoint_index).waypoint_status =
+            (i == reached_goal_index) ? nav2_msgs::msg::WaypointStatus::COMPLETED :
+                                        nav2_msgs::msg::WaypointStatus::SKIPPED;
+        } else {
+          RCLCPP_WARN(node_->get_logger(),
+            "RemovePassedGoals: No matching waypoint found for goal %d", i);
+        }
       }
-      waypoint_statuses[cur_waypoint_index].waypoint_status =
-        nav2_msgs::msg::WaypointStatus::COMPLETED;
     }
-
-    goal_poses.goals.erase(goal_poses.goals.begin());
+    // If the reached goal is NOT the last one, erase all COMPLETED and SKIPPED goals
+    if (reached_goal_index < static_cast<int>(goal_poses.goals.size()) - 1) {
+      goal_poses.goals.erase(goal_poses.goals.begin(),
+          goal_poses.goals.begin() + reached_goal_index + 1);
+    }
   }
-
   setOutput("output_goals", goal_poses);
   // set `waypoint_statuses` output
   setOutput("output_waypoint_statuses", waypoint_statuses);
diff --git a/nav2_bringup/launch/cbf_supervisor_launch.py b/nav2_bringup/launch/cbf_supervisor_launch.py
new file mode 100644
index 00000000000..74dc2f951f9
--- /dev/null
+++ b/nav2_bringup/launch/cbf_supervisor_launch.py
@@ -0,0 +1,27 @@
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, ExecuteProcess
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, EnvironmentVariable
+import os
+
+def generate_launch_description():
+    launchDesc = LaunchDescription()
+    # If the Supervisor package is available on system (debian install)
+    # then include the supervisor launch file
+    pkg = get_package_share_directory("lll_supervisor")
+    launchDesc.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        get_package_share_directory("lll_supervisor"),
+                        "launch",
+                        "supervisor.launch.py",
+                    ]
+                )
+            )
+        )
+    )
+
+    return launchDesc
diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py
index 2dcde8ec82f..e369d8bad7b 100644
--- a/nav2_bringup/launch/tb4_simulation_launch.py
+++ b/nav2_bringup/launch/tb4_simulation_launch.py
@@ -31,7 +31,7 @@
 # Define local map types
 MAP_POSES_DICT = {
     'depot': {
-        'x': -8.00, 'y': 0.00, 'z': 0.01,
+        'x': -7.54, 'y': -0.3, 'z': 0.01,
         'R': 0.00, 'P': 0.00, 'Y': 0.00
     },
     'warehouse': {
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index e66d409478f..a1c33435c18 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -6,6 +6,7 @@ amcl:
     alpha4: 0.2
     alpha5: 0.2
     base_frame_id: "base_footprint"
+    introspection_mode: "disabled"
     beam_skip_distance: 0.5
     beam_skip_error_threshold: 0.9
     beam_skip_threshold: 0.3
@@ -30,7 +31,6 @@ amcl:
     sigma_hit: 0.2
     tf_broadcast: true
     transform_tolerance: 1.0
-    introspection_mode: "disabled"
     update_min_a: 0.2
     update_min_d: 0.25
     z_hit: 0.5
@@ -38,6 +38,15 @@ amcl:
     z_rand: 0.5
     z_short: 0.05
     scan_topic: scan
+    set_initial_pose: true
+    always_reset_initial_pose: true
+    first_map_only: false
+    initial_pose:
+      x: 7.65
+      y: 7.65
+      z: 0.0
+      yaw: 0.0
+
 
 bt_navigator:
   ros__parameters:
@@ -45,8 +54,10 @@ bt_navigator:
     robot_base_frame: base_link
     odom_topic: odom
     bt_loop_duration: 10
+    #enable_stamped_cmd_vel: False
     filter_duration: 0.3
     default_server_timeout: 20
+    goal_reached_tol: 0.8
     wait_for_service_timeout: 1000
     introspection_mode: "disabled"
     navigators: ["navigate_to_pose", "navigate_through_poses"]
@@ -64,10 +75,20 @@ bt_navigator:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
-
+    
+    #default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml
+    default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml
+    #default_nav_through_poses_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml # Pelna obsluga przeszkod
+    
     # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
     # Built-in plugins are added automatically
-    # plugin_lib_names: []
+    plugin_lib_names:
+      - nav2_publish_goals_action_bt_node
+      - nav2_is_collision_free_condition_bt_node
+      - nav2_remove_next_goal_action_bt_node
+      - nav2_pose_reached_condition_bt_node
+      - nav2_goals_truncation_action_bt_node
+      - nav2_goals_has_at_most_point_count_bt_node
 
     error_code_name_prefixes:
       - assisted_teleop
@@ -86,7 +107,8 @@ bt_navigator:
 
 controller_server:
   ros__parameters:
-    controller_frequency: 20.0
+    controller_frequency: 30.0
+    #enable_stamped_cmd_vel: False
     costmap_update_timeout: 0.30
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
@@ -100,8 +122,9 @@ controller_server:
 
     # Progress checker parameters
     progress_checker:
-      plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.5
+      plugin: "nav2_controller::PoseProgressChecker"
+      required_movement_radius: 0.1
+      required_movement_angle: 0.3
       movement_time_allowance: 10.0
     # Goal checker parameters
     #precise_goal_checker:
@@ -112,11 +135,237 @@ controller_server:
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
-    FollowPath:
+      xy_goal_tolerance: 0.25 #0.0 #0.25 #0.0
+      yaw_goal_tolerance: 0.25 #0.0 #0.25 #0.0
+
+    # change general goal checker for path following
+    # FollowPath:
+    #   plugin: "nav2_mppi_controller::MPPIController"
+    #   time_steps: 56
+    #   model_dt: 0.05
+    #   batch_size: 2000
+    #   ax_max: 3.0
+    #   ax_min: -3.0
+    #   ay_max: 3.0
+    #   ay_min: -3.0
+    #   az_max: 3.5
+    #   vx_std: 0.2
+    #   vy_std: 0.2
+    #   wz_std: 0.4 #0.9
+    #   vx_max: 0.3
+    #   vx_min: -0.1
+    #   vy_max: 0.5
+    #   wz_max: 0.3
+    #   iteration_count: 1
+    #   prune_distance: 1.7
+    #   enforce_path_inversion: True
+    #   transform_tolerance: 0.1
+    #   temperature: 0.3
+    #   gamma: 0.015
+    #   motion_model: "DiffDrive"
+    #   #publish_critics_stats: True
+    #   #visualize: true
+    #   #publish_optimal_trajectory: true
+    #   regenerate_noises: true
+    #   Visualization:
+    #     publish_critics_stats: false
+    #     publish_optimal_trajectory_msg: false
+    #     publish_optimal_trajectory: false
+    #     publish_transformed_path: false
+    #     publish_trajectories_with_total_cost: true
+    #     publish_trajectories_with_individual_cost: true
+    #     publish_optimal_footprints: true
+    #     publish_optimal_path: false
+    #     trajectory_step: 5
+    #     time_step: 3
+    #     footprint_downsample_factor: 3
+    #   TrajectoryValidator:
+    #     # The validator can be configured with additional parameters if needed.
+    #     plugin: "mppi::DefaultOptimalTrajectoryValidator"
+    #     collision_lookahead_time: 0.0  # Time to look ahead to check for collisions
+    #     consider_footprint: false  # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
+    #   AckermannConstraints:
+    #     min_turning_r: 0.2
+    #   critics: [ #"ConstraintCritic",
+    #      "CostCritic", "GoalCritic", 
+    #     "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+    #     "PathAngleCritic", "PreferForwardCritic"]
+    #   GoalCritic:
+    #     enabled: true
+    #     cost_power: 1
+    #     cost_weight: 5.0
+    #     threshold_to_consider: 1.4
+    #   GoalAngleCritic:
+    #     enabled: true
+    #     cost_power: 1
+    #     cost_weight: 3.0
+    #     threshold_to_consider: 0.5
+    #   PreferForwardCritic:
+    #     enabled: true
+    #     cost_power: 1
+    #     cost_weight: 5.0
+    #     threshold_to_consider: 0.5
+    #   CostCritic:
+    #     enabled: true
+    #     cost_power: 1
+    #     cost_weight: 3.81
+    #     near_collision_cost: 253
+    #     critical_cost: 300.0
+    #     consider_footprint: true
+    #     collision_cost: 1000000.0
+    #     near_goal_distance: 0.2
+    #     trajectory_point_step: 2
+    #   PathAlignCritic:
+    #     enabled: true
+    #     cost_power: 1
+    #     cost_weight: 14.0
+    #     max_path_occupancy_ratio: 0.05
+    #     trajectory_point_step: 4
+    #     threshold_to_consider: 0.5
+    #     offset_from_furthest: 20
+    #     use_path_orientations: false
+    #   PathFollowCritic:
+    #     enabled: true
+    #     cost_power: 1
+    #     cost_weight: 5.0
+    #     offset_from_furthest: 5
+    #     threshold_to_consider: 1.4
+    #   PathAngleCritic:
+    #     enabled: true
+    #     cost_power: 1
+    #     cost_weight: 2.0
+    #     offset_from_furthest: 4
+    #     threshold_to_consider: 0.5
+    #     max_angle_to_furthest: 1.0
+    #     mode: 0
+    #   WaypointCritic:
+    #     enabled: false
+    #     cost_power: 1
+    #     cost_weight: 5.0
+    #     position_tolerance: 0.1
+    #     yaw_tolerance: 0.1
+    #     waypoint_topic: "truncated_path"
+    #     use_orientation: false
+    #     max_waypoint_distance: 10.0
+    MPPI:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 50
+      model_dt: 0.1
+      batch_size: 2000
+      ax_max: 1.0
+      ax_min: -1.0
+      ay_max: 0.0
+      ay_min: 0.0
+      az_max: 1.0
+      vx_std: 0.2
+      vy_std: 0.0
+      wz_std: 0.9
+      vx_max: 0.3
+      vx_min: -0.1
+      vy_max: 0.0
+      wz_max: 0.3
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      enforce_path_inversion: True
+      enforce_path_rotation: True
+      minimum_rotation_angle: 1.0
+      inversion_xy_tolerance: 0.5
+      inversion_yaw_tolerance: 0.785
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      reset_period: 1.0 # (only in Humble)
+      regenerate_noises: true
+      Visualization:
+        publish_critics_stats: true
+        publish_optimal_trajectory_msg: false
+        publish_optimal_trajectory: false
+        publish_transformed_path: true
+        publish_trajectories_with_total_cost: false
+        publish_trajectories_with_individual_cost: false
+        publish_optimal_footprints: false
+        publish_optimal_path: false
+        trajectory_step: 10
+        time_step: 5
+        footprint_downsample_factor: 3
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      # ObstaclesCritic:
+      #   enabled: true
+      #   cost_power: 1
+      #   repulsion_weight: 1.5
+      #   critical_weight: 20.0
+      #   consider_footprint: false
+      #   collision_cost: 10000.0
+      #   collision_margin_distance: 0.1
+      #   near_goal_distance: 0.5
+      #   inflation_radius: 0.55 # (only in Humble)
+      #   cost_scaling_factor: 10.0 # (only in Humble)
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.5 # PODNIESC WAGE COST CRITISC
+        near_collision_cost: 253
+        critical_cost: 250.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+
+    FollowPathA:
+      # plugin: "nav2_rotation_shim_controller::RotationShimController"
+      # primary_controller: "nav2_mppi_controller::MPPIController"
+      # angular_dist_threshold: 2.4434 # 140 degrees
+      # forward_sampling_distance: 0.5
+      # angular_disengage_threshold: 0.3925
+      # rotate_to_heading_angular_vel: 0.3
+      # max_angular_accel: 1.0
+      # simulate_ahead_time: 1.0
+      # rotate_to_goal_heading: false
       plugin: "nav2_mppi_controller::MPPIController"
-      time_steps: 56
+      time_steps: 35
       model_dt: 0.05
       batch_size: 2000
       ax_max: 3.0
@@ -126,55 +375,389 @@ controller_server:
       az_max: 3.5
       vx_std: 0.2
       vy_std: 0.2
-      wz_std: 0.4
-      vx_max: 0.5
-      vx_min: -0.35
+      wz_std: 0.9
+      vx_max: 0.3
+      vx_min: -0.1
       vy_max: 0.5
-      wz_max: 1.9
+      wz_max: 0.3
       iteration_count: 1
-      prune_distance: 1.7
+      prune_distance: 0.8
+      enforce_path_inversion: True
       transform_tolerance: 0.1
       temperature: 0.3
       gamma: 0.015
       motion_model: "DiffDrive"
-      visualize: true
-      publish_optimal_trajectory: true
-      regenerate_noises: true
-      TrajectoryVisualizer:
-        trajectory_step: 5
-        time_step: 3
+      publish_critics_stats: True
+      #visualize: false
+      #publish_optimal_trajectory: true
+      regenerate_noises: false
+      Visualization:
+        publish_critics_stats: false
+        publish_optimal_trajectory_msg: false
+        publish_optimal_trajectory: false
+        publish_transformed_path: false
+        publish_trajectories_with_total_cost: true
+        publish_trajectories_with_individual_cost: false
+        publish_optimal_footprints: true
+        publish_optimal_path: false
+        trajectory_step: 10
+        time_step: 5
+        footprint_downsample_factor: 3
       TrajectoryValidator:
         # The validator can be configured with additional parameters if needed.
+        # plugin: "mppi::AutoFixingTrajectoryValidator"
+        # #consider_footprint: true
+        # consider_footprint: true
+        # collision_lookahead_time: 0.8
+        # auto_fix_ttc_window: 0.8
+        # auto_fix_safe_clearance: 0.04   # ≈ 0.8 * resolution (np. 5cm → 0.04)
+        # auto_fix_step_gain: 0.6
+        # auto_fix_max_step: 0.05
+        # auto_fix_max_total_displacement: 0.35
+        # auto_fix_max_iters: 6
+        # auto_fix_smooth_iters: 2
         plugin: "mppi::DefaultOptimalTrajectoryValidator"
-        collision_lookahead_time: 2.0  # Time to look ahead to check for collisions
-        consider_footprint: false  # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
+        collision_lookahead_time: 0.0  # Time to look ahead to check for collisions
+        consider_footprint: true  # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
+        
+        #collision_lookahead_time: 0.1  # Time to look ahead to check for collisions
+        #consider_footprint: true  # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
       AckermannConstraints:
         min_turning_r: 0.2
-      critics:
-        [
-          "ConstraintCritic",
-          "CostCritic",
-          "GoalCritic",
-          "GoalAngleCritic",
-          "PathAlignCritic",
-          "PathFollowCritic",
-          "PathAngleCritic",
-          "PreferForwardCritic",
-        ]
+      critics: [ #"ConstraintCritic",
+         "ObstaclesCritic", "GoalCritic", 
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"] #,"WaypointCritic"]
       ConstraintCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
         enabled: true
         cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5 #0.5 # zmienione z 0.2
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.3 #0.3 # zmienione z 0.1
+      PreferForwardCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.5 # PODNIESC WAGE COST CRITISC
+        near_collision_cost: 253
+        critical_cost: 250.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 0.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+        offset_from_furthest: 3
+        threshold_to_consider: 0.0
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.0
+        max_angle_to_furthest: 0.5 #1.2
+        mode: 0
+      ObstaclesCritic:
+        enabled: true
+        cost_power: 1
+        repulsion_weight: 0.5 #0.5 0.8
+        critical_weight: 15.0
+        consider_footprint: true # FALSE # avoid obstacles based on radius not the footprint
+        collision_cost: 10000.0
+        collision_margin_distance: 0.1
+        near_goal_distance: 0.2
+        cost_scaling_factor: 2.5
+        inflation_radius: 1.0
+      WaypointCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 5.0
+        position_tolerance: 0.1
+        yaw_tolerance: 0.1
+        waypoint_topic: "truncated_path"
+        use_orientation: True
+        max_waypoint_distance: 10.0
+      TwirlingCritic:
+        enabled: false
+        twirling_cost_power: 1
+        twirling_cost_weight: 10.0
+        
+    FollowPathB:
+      # plugin: "nav2_rotation_shim_controller::RotationShimController"
+      # primary_controller: "nav2_mppi_controller::MPPIController"
+      # angular_dist_threshold: 2.4434 # 140 degrees
+      # forward_sampling_distance: 0.5
+      # angular_disengage_threshold: 0.3925
+      # rotate_to_heading_angular_vel: 0.3
+      # max_angular_accel: 1.0
+      # simulate_ahead_time: 1.0
+      # rotate_to_goal_heading: false
+      plugin: "nav2_mppi_controller::MPPIController"
+      # Dobrac prune distance, time_stemps i model_dt PRZYSPIESZENIA NIE DZIALA DLA NIZSZEGO MODEL DT
+      time_steps: 35
+      model_dt: 0.15 #0.05
+      batch_size: 2000
+      ax_max: 1.0
+      ax_min: -1.0
+      ay_max: 1.0
+      ay_min: -1.0
+      az_max: 1.0
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.9
+      vx_max: 0.3
+      vx_min: -0.1
+      vy_max: 0.5
+      wz_max: 0.3
+      iteration_count: 1
+      prune_distance: 1.0
+      enforce_path_inversion: True
+      enforce_path_rotation: True
+      minimum_rotation_angle: 1.0
+      inversion_xy_tolerance: 0.5
+      inversion_yaw_tolerance: 0.785
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      publish_critics_stats: false
+      #visualize: false
+      #publish_optimal_trajectory: true
+      regenerate_noises: true
+      Visualization:
+        publish_critics_stats: true
+        publish_optimal_trajectory_msg: false
+        publish_optimal_trajectory: false
+        publish_transformed_path: true
+        publish_trajectories_with_total_cost: false
+        publish_trajectories_with_individual_cost: false
+        publish_optimal_footprints: false
+        publish_optimal_path: false
+        trajectory_step: 10
+        time_step: 5
+        footprint_downsample_factor: 3
+      TrajectoryValidator:
+        # The validator can be configured with additional parameters if needed.
+        # plugin: "mppi::AutoFixingTrajectoryValidator"
+        # #consider_footprint: true
+        # consider_footprint: true
+        # collision_lookahead_time: 0.8
+        # auto_fix_ttc_window: 0.8
+        # auto_fix_safe_clearance: 0.04   # ≈ 0.8 * resolution (np. 5cm → 0.04)
+        # auto_fix_step_gain: 0.6
+        # auto_fix_max_step: 0.05
+        # auto_fix_max_total_displacement: 0.35
+        # auto_fix_max_iters: 6
+        # auto_fix_smooth_iters: 2
+        plugin: "mppi::DefaultOptimalTrajectoryValidator"
+        collision_lookahead_time: 0.0  # Time to look ahead to check for collisions
+        consider_footprint: true  # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
+        
+        #collision_lookahead_time: 0.1  # Time to look ahead to check for collisions
+        #consider_footprint: true  # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [ #"ConstraintCritic",
+         "CostCritic", "GoalCritic", 
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"] #,"WaypointCritic"]
+      ConstraintCritic:
+        enabled: false
+        cost_power: 1
         cost_weight: 4.0
       GoalCritic:
         enabled: true
         cost_power: 1
         cost_weight: 5.0
-        threshold_to_consider: 1.4
+        threshold_to_consider: 0.5 #0.5 # zmienione z 0.2
       GoalAngleCritic:
         enabled: true
         cost_power: 1
         cost_weight: 3.0
+        threshold_to_consider: 0.3 #0.3 # zmienione z 0.1
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
         threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.5 # PODNIESC WAGE COST CRITISC
+        near_collision_cost: 253
+        critical_cost: 250.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 0.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.3
+        offset_from_furthest: 10
+        use_path_orientations: false
+        visualize_furthest_point: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+        offset_from_furthest: 3
+        threshold_to_consider: 0.5
+        visualize_furthest_point: false
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.3
+        max_angle_to_furthest: 0.5 #1.2
+        visualize_furthest_point: false
+        forward_preference: true
+        mode: 0
+      ObstaclesCritic:
+        enabled: true
+        cost_power: 1
+        repulsion_weight: 0.5 #0.5 0.8
+        critical_weight: 15.0
+        consider_footprint: true # FALSE # avoid obstacles based on radius not the footprint
+        collision_cost: 10000.0
+        collision_margin_distance: 0.1
+        near_goal_distance: 0.2
+        cost_scaling_factor: 2.5
+        inflation_radius: 1.0
+      WaypointCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 5.0
+        position_tolerance: 0.1
+        yaw_tolerance: 0.1
+        waypoint_topic: "truncated_path"
+        use_orientation: True
+        max_waypoint_distance: 10.0
+      TwirlingCritic:
+        enabled: false
+        twirling_cost_power: 1
+        twirling_cost_weight: 10.0
+
+    FollowPathC:
+      # plugin: "nav2_rotation_shim_controller::RotationShimController"
+      # primary_controller: "nav2_mppi_controller::MPPIController"
+      # angular_dist_threshold: 2.4434 # 140 degrees
+      # forward_sampling_distance: 0.5
+      # angular_disengage_threshold: 0.3925
+      # rotate_to_heading_angular_vel: 0.3
+      # max_angular_accel: 1.0
+      # simulate_ahead_time: 1.0
+      # rotate_to_goal_heading: false
+      plugin: "nav2_mppi_controller::MPPIController"
+      # Dobrac prune distance, time_stemps i model_dt PRZYSPIESZENIA NIE DZIALA DLA NIZSZEGO MODEL DT
+      time_steps: 35 #35 # 18
+      model_dt: 0.15 #0.15 #0.05
+      batch_size: 2000
+      ax_max: 1.0
+      ax_min: -1.0
+      ay_max: 1.0
+      ay_min: -1.0
+      az_max: 1.0
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.3
+      vx_min: -0.1
+      vy_max: 0.5
+      wz_max: 0.3
+      iteration_count: 1
+      prune_distance: 1.7 #1.7 #1.0
+      enforce_path_inversion: True
+      enforce_path_rotation: True
+      minimum_rotation_angle: 1.0
+      inversion_xy_tolerance: 0.5
+      inversion_yaw_tolerance: 0.785
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      publish_critics_stats: false
+      #visualize: false
+      #publish_optimal_trajectory: true
+      regenerate_noises: true
+      Visualization:
+        publish_critics_stats: true
+        publish_optimal_trajectory_msg: false
+        publish_optimal_trajectory: false
+        publish_transformed_path: true
+        publish_trajectories_with_total_cost: false
+        publish_trajectories_with_individual_cost: false
+        publish_optimal_footprints: false
+        publish_optimal_path: false
+        trajectory_step: 10
+        time_step: 5
+        footprint_downsample_factor: 3
+      TrajectoryValidator:
+        # The validator can be configured with additional parameters if needed.
+        # plugin: "mppi::AutoFixingTrajectoryValidator"
+        # #consider_footprint: true
+        # consider_footprint: true
+        # collision_lookahead_time: 0.8
+        # auto_fix_ttc_window: 0.8
+        # auto_fix_safe_clearance: 0.04   # ≈ 0.8 * resolution (np. 5cm → 0.04)
+        # auto_fix_step_gain: 0.6
+        # auto_fix_max_step: 0.05
+        # auto_fix_max_total_displacement: 0.35
+        # auto_fix_max_iters: 6
+        # auto_fix_smooth_iters: 2
+        plugin: "mppi::DefaultOptimalTrajectoryValidator"
+        collision_lookahead_time: 0.0  # Time to look ahead to check for collisions
+        consider_footprint: true  # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
+        
+        #collision_lookahead_time: 0.1  # Time to look ahead to check for collisions
+        #consider_footprint: true  # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [ #"ConstraintCritic",
+         "CostCritic", "GoalCritic", 
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"] #,"WaypointCritic"]
+      ConstraintCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5 #0.5 # zmienione z 0.2
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.3 #0.3 # zmienione z 0.1
       PreferForwardCritic:
         enabled: true
         cost_power: 1
@@ -183,10 +766,10 @@ controller_server:
       CostCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 3.81
+        cost_weight: 2.5 # PODNIESC WAGE COST CRITISC
         near_collision_cost: 253
-        critical_cost: 300.0
-        consider_footprint: false
+        critical_cost: 250.0
+        consider_footprint: true
         collision_cost: 1000000.0
         near_goal_distance: 1.0
         trajectory_point_step: 2
@@ -196,27 +779,51 @@ controller_server:
         cost_weight: 14.0
         max_path_occupancy_ratio: 0.05
         trajectory_point_step: 4
-        threshold_to_consider: 0.5
-        offset_from_furthest: 20
+        threshold_to_consider: 0.0
+        offset_from_furthest: 10
         use_path_orientations: false
+        visualize_furthest_point: false
       PathFollowCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 5.0
-        offset_from_furthest: 5
-        threshold_to_consider: 1.4
+        cost_weight: 4.0
+        offset_from_furthest: 3
+        threshold_to_consider: 0.0
+        visualize_furthest_point: false
       PathAngleCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 2.0
+        cost_weight: 3.0
         offset_from_furthest: 4
-        threshold_to_consider: 0.5
-        max_angle_to_furthest: 1.0
+        threshold_to_consider: 0.0
+        max_angle_to_furthest: 0.5 #1.2
+        visualize_furthest_point: false
+        forward_preference: true
         mode: 0
-      # TwirlingCritic:
-      #   enabled: true
-      #   twirling_cost_power: 1
-      #   twirling_cost_weight: 10.0
+      ObstaclesCritic:
+        enabled: true
+        cost_power: 1
+        repulsion_weight: 0.5 #0.5 0.8
+        critical_weight: 15.0
+        consider_footprint: true # FALSE # avoid obstacles based on radius not the footprint
+        collision_cost: 10000.0
+        collision_margin_distance: 0.1
+        near_goal_distance: 0.2
+        cost_scaling_factor: 2.5
+        inflation_radius: 1.0
+      WaypointCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 5.0
+        position_tolerance: 0.1
+        yaw_tolerance: 0.1
+        waypoint_topic: "truncated_path"
+        use_orientation: True
+        max_waypoint_distance: 10.0
+      TwirlingCritic:
+        enabled: false
+        twirling_cost_power: 1
+        twirling_cost_weight: 10.0
 
 local_costmap:
   local_costmap:
@@ -229,9 +836,10 @@ local_costmap:
       width: 3
       height: 3
       resolution: 0.05
-      robot_radius: 0.22
-      plugins: ["voxel_layer", "inflation_layer"]
-      filters: ["keepout_filter"]
+      #robot_radius: 0.22
+      footprint: "[ [0.28638, 0.274], [0.28638, -0.274], [-0.39762, -0.274], [-0.39762, 0.274] ]"
+      plugins: ["obstacle_layer", "inflation_layer"]
+      #filters: ["keepout_filter"]
       keepout_filter:
         plugin: "nav2_costmap_2d::KeepoutFilter"
         enabled: KEEPOUT_ZONE_ENABLED
@@ -240,8 +848,31 @@ local_costmap:
         lethal_override_cost: 200
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.70
+        cost_scaling_factor: 2.5
+        inflation_radius: 1.0
+
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        footprint_clearing_enabled: True
+        observation_sources: scan
+        scan:
+          # A relative topic will be appended to the parent of the global_costmap namespace.
+          # For example:
+          #   * User chosen namespace is `tb4`.
+          #   * User chosen topic is `scan`.
+          #   * Topic will be remapped to `/tb4/scan` without `global_costmap`.
+          #   * Use global topic `/scan` if you do not wish the node namespace to apply
+          topic: scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -271,6 +902,8 @@ local_costmap:
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
+        footprint_clearing_enabled: True
+        restore_cleared_footprint: True
       always_send_full_costmap: True
       introspection_mode: "disabled"
 
@@ -281,11 +914,12 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      robot_radius: 0.22
+      #robot_radius: 0.22
+      footprint: "[ [0.28638, 0.274], [0.28638, -0.274], [-0.39762, -0.274], [-0.39762, 0.274] ]"
       resolution: 0.05
       track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
-      filters: ["keepout_filter", "speed_filter"]
+      #filters: ["keepout_filter", "speed_filter"]
       keepout_filter:
         plugin: "nav2_costmap_2d::KeepoutFilter"
         enabled: KEEPOUT_ZONE_ENABLED
@@ -300,6 +934,7 @@ global_costmap:
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
+        footprint_clearing_enabled: True
         observation_sources: scan
         scan:
           # A relative topic will be appended to the parent of the global_costmap namespace.
@@ -323,7 +958,7 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.7
+        inflation_radius: 0.55
       always_send_full_costmap: True
       introspection_mode: "disabled"
 
@@ -369,18 +1004,81 @@ map_saver:
     map_subscribe_transient_local: True
     introspection_mode: "disabled"
 
+# planner_server:
+#   ros__parameters:
+#     expected_planner_frequency: 20.0
+#     planner_plugins: ["GridBased"]
+#     costmap_update_timeout: 1.0
+#     introspection_mode: "disabled"
+#     GridBased:
+#       plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::"
+#       tolerance: 0.5                      # tolerance for planning if unable to reach exact pose, in meters
+#       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: true                   # allow traveling in unknown space
+#       max_iterations: 1000000               # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+#       max_on_approach_iterations: 1000      # maximum number of iterations to attempt to reach goal once in tolerance
+#       max_planning_time: 2.0                # max time in s for planner to plan, smooth
+#       cost_travel_multiplier: 1.0           # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
+#       use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
+#       smoother:
+#         max_iterations: 1000
+#         w_smooth: 0.6
+#         w_data: 0.2
+#         tolerance: 1.0e-10
+
 planner_server:
   ros__parameters:
-    expected_planner_frequency: 20.0
     planner_plugins: ["GridBased"]
-    costmap_update_timeout: 1.0
-    introspection_mode: "disabled"
+
     GridBased:
-      plugin: "nav2_navfn_planner::NavfnPlanner"
-      tolerance: 0.5
-      use_astar: false
-      allow_unknown: true
+      plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::"
+      tolerance: 0.5                      # tolerance for planning if unable to reach exact pose, in meters
+      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: true                   # allow traveling in unknown space
+      max_iterations: 1000000               # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+      max_on_approach_iterations: 1000      # maximum number of iterations to attempt to reach goal once in tolerance
+      max_planning_time: 2.0                # max time in s for planner to plan, smooth
+      cost_travel_multiplier: 1.0           # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
+      use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.6
+        w_data: 0.2
+        tolerance: 1.0e-10
 
+    # GridBased:
+    #   plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::"
+    #   allow_unknown: true                 # Allow traveling in unknown space
+    #   tolerance: 0.25                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
+    #   max_iterations: 1000000             # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+    #   max_on_approach_iterations: 1000    # Maximum number of iterations after within tolerances to continue to try to find exact solution
+    #   max_planning_time: 5.0              # Max time in s for planner to plan, smooth
+    #   analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
+    #   analytic_expansion_max_length: 3.0  # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
+    #   analytic_expansion_max_cost: 200.0  # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal
+    #   analytic_expansion_max_cost_override: false  #  Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
+    #   reverse_penalty: 2.0                # Penalty to apply if motion is reversing, must be => 1
+    #   change_penalty: 0.05                # Penalty to apply if motion is changing directions (L to R), must be >= 0
+    #   non_straight_penalty: 1.05          # Penalty to apply if motion is non-straight, must be => 1
+    #   cost_penalty: 2.0                   # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
+    #   rotation_penalty: 0.0               # Penalty to apply to in-place rotations, if minimum control set contains them
+    #   retrospective_penalty: 0.015
+    #   lattice_filepath: "/home/jakub/ros2_jazzy/src/aros_navigation/aros_navigation/config/lattice_planner.json"                # The filepath to the state lattice graph
+    #   lookup_table_size: 20.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
+    #   cache_obstacle_heuristic: false     # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
+    #   allow_reverse_expansion: false      # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse).
+    #   coarse_search_resolution: 1         # Number of bins to skip when doing a coarse search for the path. Only used for all_direction goal heading mode.
+    #   goal_heading_mode: "DEFAULT"        # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION
+    #   smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path
+    #   smoother:
+    #     max_iterations: 1000
+    #     w_smooth: 0.3
+    #     w_data: 0.2
+    #     tolerance: 1.0e-10
+    #     do_refinement: true
+    #     refinement_num: 2
 smoother_server:
   ros__parameters:
     smoother_plugins: ["simple_smoother", "route_smoother"]
@@ -407,6 +1105,7 @@ behavior_server:
     global_costmap_topic: global_costmap/costmap_raw
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
+    #enable_stamped_cmd_vel: False
     cycle_frequency: 10.0
     behavior_plugins:
       ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
@@ -482,17 +1181,19 @@ velocity_smoother:
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
+    #enable_stamped_cmd_vel: False
 
 collision_monitor:
   ros__parameters:
     base_frame_id: "base_footprint"
     odom_frame_id: "odom"
-    cmd_vel_in_topic: "cmd_vel_smoothed"
+    cmd_vel_in_topic: cmd_vel_smoothed #cmd_vel_smoothed #"cmd_vel_supervised"
     cmd_vel_out_topic: "cmd_vel"
     state_topic: "collision_monitor_state"
     transform_tolerance: 0.2
     source_timeout: 1.0
     base_shift_correction: True
+    #enable_stamped_cmd_vel: False
     stop_pub_timeout: 2.0
     # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
     # and robot footprint for "approach" action type.
@@ -505,14 +1206,14 @@ collision_monitor:
       simulation_time_step: 0.1
       min_points: 6
       visualize: False
-      enabled: True
+      enabled: False
     observation_sources: ["scan"]
     scan:
       type: "scan"
       topic: "scan"
       min_height: 0.15
       max_height: 2.0
-      enabled: True
+      enabled: False
 
 docking_server:
   ros__parameters:
@@ -532,12 +1233,13 @@ docking_server:
     dock_plugins: ["simple_charging_dock"]
     simple_charging_dock:
       plugin: "opennav_docking::SimpleChargingDock"
-      docking_threshold: 0.05
+      docking_threshold: 0.01
       staging_x_offset: -0.7
-      use_external_detection_pose: true
+      use_external_detection_pose: false
       use_battery_status: false # true
       use_stall_detection: false # true
       dock_direction: "forward"
+      rotate_to_dock: False
 
       external_detection_timeout: 1.0
       external_detection_translation_x: -0.18
@@ -547,6 +1249,19 @@ docking_server:
       external_detection_rotation_yaw: 0.0
       filter_coef: 0.1
 
+    docks: ['home_dock','flex_dock1', 'flex_dock2']
+    home_dock:
+      type: 'simple_charging_dock'
+      frame: map
+      pose: [9.65, 7.65, 0.0]
+    flex_dock1:
+      type: 'simple_charging_dock'
+      frame: map
+      pose: [10.0, 10.0, 0.0]
+    flex_dock2:
+      type: 'simple_charging_dock'
+      frame: map
+      pose: [30.0, 30.0, 0.0]
     # Dock instances
     # The following example illustrates configuring dock instances.
     # docks: ['home_dock']  # Input your docks here
@@ -556,10 +1271,10 @@ docking_server:
     #   pose: [0.0, 0.0, 0.0]
 
     controller:
-      k_phi: 3.0
-      k_delta: 2.0
-      v_linear_min: 0.15
-      v_linear_max: 0.15
+      k_phi: 5.0
+      k_delta: 5.0
+      v_linear_min: 0.03
+      v_linear_max: 0.1
       use_collision_detection: true
       costmap_topic: "local_costmap/costmap_raw"
       footprint_topic: "local_costmap/published_footprint"
@@ -567,6 +1282,8 @@ docking_server:
       projection_time: 5.0
       simulation_step: 0.1
       dock_collision_threshold: 0.3
+      rotate_to_heading_angular_vel: 0.5
+      rotate_to_heading_max_angular_accel: 1.0
 
 loopback_simulator:
   ros__parameters:
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
deleted file mode 100644
index 6c32906f4f9..00000000000
--- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
+++ /dev/null
@@ -1,54 +0,0 @@
-
-<!--
-  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
-   and it also has recovery actions specific to planning / control as well as general system issues.
--->
-<root BTCPP_format="4" main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
-    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
-      <PipelineSequence name="NavigateWithReplanning">
-        <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
-        <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
-        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
-        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
-        <RateController hz="0.333">
-          <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
-            <ReactiveSequence>
-              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
-              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
-            </ReactiveSequence>
-            <Sequence>
-              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
-              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
-            </Sequence>
-          </RecoveryNode>
-        </RateController>
-        <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" goal_checker_id="{selected_goal_checker}" progress_checker_id="{selected_progress_checker}"/>
-          <Sequence>
-            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
-            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
-          </Sequence>
-        </RecoveryNode>
-      </PipelineSequence>
-      <Sequence>
-        <Fallback>
-          <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
-          <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
-        </Fallback>
-        <ReactiveFallback name="RecoveryFallback">
-          <GoalUpdated/>
-          <RoundRobin name="RecoveryActions">
-            <Sequence name="ClearingActions">
-              <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
-              <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
-            </Sequence>
-            <Spin name="SpinRecovery" spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
-            <Wait name="WaitRecovery" wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
-            <BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
-          </RoundRobin>
-        </ReactiveFallback>
-      </Sequence>
-    </RecoveryNode>
-  </BehaviorTree>
-</root>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml
new file mode 100644
index 00000000000..01c9d8100d2
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy.xml
@@ -0,0 +1,157 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
+   and it also has recovery actions specific to planning / control as well as general system issues.
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+
+    <Sequence>
+      <RecoveryNode number_of_retries="1000000" name="ComputePathRecovery">
+        <Sequence name="ComputePathSequence">
+          <!-- <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/> -->
+          <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/>
+          <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+          <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+          <SetBlackboard output_key="obstacle_ahead" value="false"/>
+          <GetCurrentPose current_pose="{current_pose}"/>
+          <SetBlackboard output_key="last_stop_pose" value="{current_pose}"/>
+          <SetBlackboard output_key="is_near_last_stop" value="false"/>
+        </Sequence>
+        <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+      </RecoveryNode>
+
+
+      <RetryUntilSuccessful num_attempts="6">
+        <Sequence>
+          <PipelineSequence name="NavigateWithReplanning">
+            <RateController hz="2.0">
+            <Fallback> 
+                <ReactiveSequence>
+                    <Sequence>
+                      <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "1" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+                      <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" /> -->
+                      <!-- <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/> -->
+                      <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/>
+                      <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                      <!-- <Inverter>
+                        <isCollisionFree nav_goal = "{last_removed_goal}" cost_threshold="100.0" radius="1.5"/>
+                      </Inverter> -->
+                    </Sequence>
+                    
+                    <Sequence>
+                      <TruncatePathLocal input_path="{path}" output_path="{path_local}" distance_forward="3.5" distance_backward="0.0" robot_frame="base_link"/>
+                      <!-- <GetPoseFromPath path="{path_local}" index="-1" pose="{last_path_point}"/> -->
+                      <!-- <GetMiddlePoseFromPath path="{path}" pose="{middle_path_point}"/> -->
+                      <Inverter>
+                          <!-- It shoult allow for replanning if close to the (truncated) path end-->
+                          <!-- Distance threshold to last point is set from nav2 param goal_reached_tol -->
+                          <!-- <GoalReached goal="{last_path_point}" robot_base_frame="base_link"/>  -->
+                          <PoseReached path="{path_local}" robot_base_frame="base_link"/>
+                      </Inverter>
+                    </Sequence>
+                    <IsPathValid path="{path}" max_cost="254"/>
+                </ReactiveSequence>
+                <Sequence>
+                  <RecoveryNode number_of_retries="1000000" name="ComputePathRecovery">
+                      <Sequence>
+                          <!-- <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/> -->
+                          <!-- <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/> -->
+                          <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/>
+                          <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                          <!-- isCollisionFree is optional here because ComputePathThroughPoses-->
+                          <Fallback>
+                            <Sequence>
+                              <isCollisionFree nav_goal="{truncated_goals}" cost_threshold="100.0" truncation_samples="6"/>
+                              <SetBlackboard output_key="obstacle_ahead" value="false"/>
+                            </Sequence>
+                            <Sequence>
+                              <!-- <Inverter>
+                                <isCollisionFree nav_goal="{truncated_goals}" cost_threshold="100.0" truncation_samples="6"/>
+                              </Inverter> -->
+                              <SetBlackboard output_key="obstacle_ahead" value="false"/>
+                              <Delay delay_msec="100">
+                                <AlwaysSuccess/>
+                              </Delay>
+                            </Sequence>
+                          </Fallback>
+                          <ScriptCondition code="obstacle_ahead == false"/>
+                          <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+                          <!-- <SetBlackboard output_key="obstacle_ahead" value="false"/> -->
+                      </Sequence>
+                      <Sequence>
+                        <ForceSuccess>
+                          <Sequence>
+                            <ScriptCondition code="obstacle_ahead == false"/>
+                            <!-- <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/> -->
+                          </Sequence>
+                        </ForceSuccess>
+                        <!-- <SetBlackboard output_key="obstacle_ahead" value="true"/> -->
+                        <Delay delay_msec="100">
+                            <AlwaysSuccess/>
+                        </Delay>
+                        <!-- Remove AFTER WAITING or immediately if pose is close -->
+                        <!-- <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/> -->
+                        <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" /> -->
+                        <!-- <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/> -->
+                        <!-- <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/> -->
+                        <!-- <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/> -->
+                        <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/>
+                        <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                      </Sequence>
+                  </RecoveryNode>
+                </Sequence>
+            </Fallback>
+            </RateController>
+
+            <ReactiveSequence name="MonitorAndFollowPath">
+              <RetryUntilSuccessful num_attempts="1000">
+                <Inverter>
+                  <SequenceWithMemory name="CancelingControlAndWait">
+                    <!-- <isObstacleApproaching approaching_obstacles_topic="/dynamic_obstacles/approaching_flag"/> -->
+                    <ScriptCondition code="obstacle_ahead == true"/>
+                    <!-- <GetCurrentPose current_pose="{current_pose}"/> -->
+                    <Fallback name="WaitIfFarFromLastWaitingPose">
+                      <!-- <ArePosesNear ref_pose="{last_stop_pose}" target_pose="{current_pose}" tolerance="1.5"/> -->
+                      <ScriptCondition code="is_near_last_stop == true"/>
+                      <Sequence>
+                        <CancelControl name="ControlCancel"/>
+                        <Wait wait_duration="3"/>
+                        <GetCurrentPose current_pose="{current_pose}"/>
+                        <SetBlackboard output_key="last_stop_pose" value="{current_pose}"/>
+                        <SetBlackboard output_key="is_near_last_stop" value="true"/>
+                      </Sequence>
+                    </Fallback>
+                    <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" /> -->
+                  </SequenceWithMemory>
+                </Inverter>
+              </RetryUntilSuccessful>
+              <!-- <FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="path_follow_goal_checker"/> -->
+              <ForceSuccess>
+                <Sequence>
+                  <GetCurrentPose current_pose="{current_pose}"/>
+                  <Inverter>
+                    <ArePosesNear ref_pose="{last_stop_pose}" target_pose="{current_pose}" tolerance="1.5"/>
+                  </Inverter>
+                  <SetBlackboard output_key="is_near_last_stop" value="false"/>
+                </Sequence>
+              </ForceSuccess> 
+              <FollowPath path="{path}"
+                            controller_id="FollowPath"
+                            error_code_id="{follow_path_error_code}"/>
+            </ReactiveSequence>  
+
+          </PipelineSequence>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>
\ No newline at end of file
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml
new file mode 100644
index 00000000000..210d0f7ec88
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_copy_test_replanning.xml
@@ -0,0 +1,160 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
+   and it also has recovery actions specific to planning / control as well as general system issues.
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+
+    <Sequence>
+      <Sequence name="ComputePathSequence">
+        <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/>
+        <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+        <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+      </Sequence>
+
+      <!-- <RecoveryNode number_of_retries="1000000" name="NavigateRecovery"> -->
+      <RetryUntilSuccessful num_attempts="6">
+        <Sequence>
+
+          <PipelineSequence name="NavigateWithReplanning">
+
+
+          <!-- <IsStopped velocity_threshold="0.001" duration_stopped="10.0"/> -->
+            <!-- <RecoveryNode number_of_retries="2" name="StuckedRecovery">
+              <Inverter>
+                <IsStopped velocity_threshold="0.001" duration_stopped="10.0"/>
+              </Inverter>
+              <ReactiveFallback name="RecoveryFallback">
+                <RoundRobin name="RecoveryActions">
+                  <Sequence name="ClearingActions">
+                    <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+                  </Sequence>
+                  <BackUp backup_dist="0.45" backup_speed="0.05"/>
+                  <Spin spin_dist="1.57"/>
+                </RoundRobin>
+              </ReactiveFallback>
+            </RecoveryNode> -->
+             <!-- Jesli to sie nie powiedzie to zatrzyman nawigacje -->
+
+            <RateController hz="2.0">
+            <Fallback> 
+                <ReactiveSequence>
+                    <Sequence>
+                      <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "1" radius="0.5"/>
+                      <RemovePassedGoals input_goals="{truncated_goals}" output_goals="{truncated_goals}" nb_goals_to_consider = "1" radius="0.5"/>
+                      <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" /> -->
+                      <!-- <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/> -->
+                    </Sequence>
+                    <!-- <RemovePassedGoals radius="0.15" input_goals="{goals}" output_goals="{goals}"/>--> <!-- Remove goals which robot could not get with correct orientation but it is very close to that goals -->
+                    <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" cost_threshold="254.0" 
+                    use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                    input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" 
+                    lookahead_points="6"/> -->
+                    <!-- <RemoveInCollisionGoalsCustom input_goals="{goals}" output_goals="{goals}" lookahead_distance= "6" cost_threshold = "100.0"
+                    filter_sparse_poses= "False" filter_radius="0.25" filter_min_neighbors="3" />  -->
+
+                    <Fallback> 
+                      <!-- Truncated goals wymaga bycia zaktualizowanym po usunieciu passed goasl i goals in cilison-->
+                      <GoalsHasAtMostPointCount goals="{goals}" max_points="1"/>
+                      <Inverter>
+                        <GoalsHasAtMostPointCount goals="{truncated_goals}" max_points="1"/>
+                      </Inverter>
+                    </Fallback>
+                    <!-- <IsPathValid path="{path}" max_cost="254"/> -->
+
+                    <!-- <Fallback>
+                      <IsPathValid path="{path}" max_cost="254"/>
+                      <AlwaysFailure>
+                        <PublishSaveStateFlag topic_name="/dynamic_obstacles/approaching_flag"/>
+                      </AlwaysFailure>
+                    </Fallback> -->
+                    
+                </ReactiveSequence>
+                <Sequence>
+                  
+                  <!-- <RetryUntilSuccessful num_attempts="1000000">
+                  <Sequence name="PopAndPlan">
+                    <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/>
+                    <TruncateGoals input_goals="{goals}" truncation_samples="15" truncated_goals="{truncated_goals}"/>
+                    <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                    <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+                    <IsPathValid path="{path}"/>
+                  </Sequence>
+                </RetryUntilSuccessful> -->
+
+                  <RecoveryNode number_of_retries="1000000" name="ComputePathRecovery">
+                      <Sequence>
+                          <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/>
+                          <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                          <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+                      </Sequence>
+                      <Sequence>
+                        <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/>
+                        <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/>
+                        <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                      </Sequence>
+                  </RecoveryNode>
+                </Sequence>
+            </Fallback>
+            </RateController>
+
+            
+            <FollowPath path="{path}"
+                            controller_id="FollowPath"
+                            error_code_id="{follow_path_error_code}"/>
+            
+            <!-- <ReactiveSequence name="MonitorAndFollowPath">
+              <RetryUntilSuccessful num_attempts="1000">
+                <Inverter>
+                  <SequenceWithMemory name="CancelingControlAndWait">
+                    <isObstacleApproaching approaching_obstacles_topic="/dynamic_obstacles/approaching_flag"/>
+                    <CancelControl name="ControlCancel"/>
+                    <Wait wait_duration="3"/> 
+                  </SequenceWithMemory>
+                </Inverter>
+              </RetryUntilSuccessful>
+              <FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="path_follow_goal_checker"/>
+            </ReactiveSequence> -->
+
+              <!-- <Wait wait_duration="5"/> -->
+              <!-- <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/> -->
+              <!-- <Fallback>
+                <IsStopped velocity_threshold="0.01" duration_stopped="0.5"/>
+                <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/>
+              </Fallback> -->
+            <!-- </RecoveryNode> -->
+            
+          </PipelineSequence>
+        </Sequence>
+
+
+
+        <!-- <Fallback name="RecoveryFallback"> 
+          <SequenceWithMemory>
+            <CancelControl/>
+            <Inverter> -->
+              <!-- <IsStopped linear_velocity_threshold="0.01" angular_velocity_threshold="0.1" duration_stopped="10.0"/> -->
+              <!-- <IsStopped velocity_threshold="0.01" duration_stopped="10.0"/>
+            </Inverter>
+          </SequenceWithMemory>
+
+          <Sequence name="RecoveryActionSteps">  -->
+            <!-- <Sequence name="ClearingActions">
+              <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+            </Sequence> -->
+          <!-- <ClearCostmapAroundRobot name="ClearLocalCostmap-Subtree" reset_distance = "1.2" service_name="local_costmap/clear_around_local_costmap"/> -->
+          <!-- <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/> -->
+          <!-- <BackUpCustom backup_dist="0.45" backup_speed="0.05" cmd_vel_topic="cmd_vel" range_topics="/proximity/back/middle/range;/proximity/back/left/range;/proximity/back/right/range"/> --> 
+          <!--Back up without collision checking-->
+          <!-- <GoalsTruncation input_goals="{goals}" truncation_samples="10" truncated_goals="{truncated_goals}"/>
+          <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+          <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/> -->
+        <!-- </Sequence>
+        </Fallback> -->
+      <!-- </RecoveryNode> -->
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>
\ No newline at end of file
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid.xml
new file mode 100644
index 00000000000..91192bf0480
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid.xml
@@ -0,0 +1,181 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
+   and it also has recovery actions specific to planning / control as well as general system issues.
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+      <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
+      <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
+      <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
+      <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
+      <GetNextFewGoals num_goals="5" input_goals="{goals}" output_goals="{planning_goals}"/>
+      <ComputePathThroughPoses goals="{planning_goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+      <GetCurrentPose current_pose="{initial_pose}"/>
+      <PublishGoals input_goals="{goals}" topic_name= "debug_goals"/> 
+      <SetBlackboard output_key="last_pose" value="{initial_pose}"/>
+
+      <PipelineSequence name="NavigateLoop">
+        <ForceSuccess>
+          <Fallback name="ReplanIfNearLocalPathEnd">
+            <Sequence>
+              
+              <!-- <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.5"/>
+              <GetNextFewGoals num_goals="5" input_goals="{goals}" output_goals="{planning_goals}"/>
+              <ComputePathThroughPoses goals="{planning_goals}" path="{path}" planner_id="GridBased"/>
+              <PublishGoals input_goals="{planning_goals}" topic_name= "truncated_goals"/> -->
+
+
+              <!-- <Sequence>
+                <TruncatePathLocal input_path="{path}" output_path="{path_local}" distance_forward="3.5" distance_backward="0.0" robot_frame="base_link"/>
+                <GetPoseFromPath path="{path_local}" index="-1" pose="{last_path_point}"/>
+                <Inverter> -->
+                    <!-- It shoult allow for replanning if close to the (truncated) path end-->
+                    <!-- Distance threshold to last point is set from nav2 param goal_reached_tol -->
+                  <!-- <GoalReached goal="{last_path_point}" robot_base_frame="base_link"/>  -->
+                  <!-- Tutaj zastosowac wersje z Aros (PoseReached) -->
+                <!-- </Inverter>
+              </Sequence> -->
+
+
+                <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+                <GetCurrentPose current_pose="{current_pose}"/>
+                <GetPoseFromPath path="{path}" index="-1" pose="{last_path_point}"/>
+                <Inverter>
+                  <ArePosesNear ref_pose="{current_pose}" target_pose="{last_path_point}" tolerance="0.8"/>
+                </Inverter>
+            </Sequence>
+            <Sequence>
+              <GetNextFewGoals num_goals="5" input_goals="{goals}" output_goals="{planning_goals}"/>
+              <ComputePathThroughPoses goals="{planning_goals}" path="{path}" planner_id="GridBased"/>
+              <PublishGoals input_goals="{planning_goals}" topic_name= "truncated_goals"/>
+              <Delay delay_msec="1000">
+              <!-- Prevent replanning more frequent than 1 Hz. Delay used instead of Wait because it doesn't spam with logs -->
+                <AlwaysSuccess/>
+              </Delay>
+            </Sequence>
+          </Fallback>
+        </ForceSuccess>
+
+      <!-- If two poses are near then do not wait again-->
+      <!-- <RetryUntilSuccessful num_attempts="1000">
+        <ForceFailure>
+          <Fallback name="FollowOrWait">
+            <ReactiveSequence>
+              <IsPathValid path="{path}" max_cost="254"/>
+              <FollowPath path="{path}"
+                          controller_id="{selected_controller}"
+                          error_code_id="{follow_path_error_code}"/>
+            </ReactiveSequence>
+
+            <Sequence name="WaitThenReplan">
+              
+              <GetCurrentPose current_pose="{current_pose}"/>
+              <isCollisionFree nav_goal="{goals}" cost_threshold="100.0" radius="1.5"/>
+              <Fallback name="SkipWaitingIfNear">
+                <ArePosesNear ref_pose="{last_pose}" target_pose="{current_pose}" tolerance="1.5"/>
+                <Sequence>
+                  <Wait wait_duration="4.0"/>
+                  <AlwaysSuccess/>
+                </Sequence>
+              </Fallback>
+              <SetBlackboard output_key="last_pose" value="{current_pose}"/>
+              <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "100" cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+              <ComputePathThroughPoses goals="{goals}" path="{path}"
+                                      planner_id="{selected_planner}"
+                                      error_code_id="{compute_path_error_code}"
+                                      error_msg="{compute_path_error_msg}"/> 
+              
+            </Sequence>
+          </Fallback>
+        </ForceFailure>
+      </RetryUntilSuccessful> -->
+
+
+      <RetryUntilSuccessful num_attempts="1000">
+        <ForceFailure>
+          <Fallback name="FollowPathAndAvoidObstacles">
+            <ReactiveSequence>
+              <RateController hz="2.0">
+                <IsPathValid path="{path}" max_cost="254" server_timeout="50.0"/>
+              </RateController>
+              <FollowPath path="{path}"
+                          controller_id="{selected_controller}"
+                          error_code_id="{follow_path_error_code}"/>
+            </ReactiveSequence>
+            <Sequence name="CollisionWaitThenReplan">
+              <GetCurrentPose current_pose="{current_pose}"/>
+              <!-- <isCollisionFree nav_goal="{goals}" cost_threshold="100.0" radius="1.5"/> -->
+              <Fallback name="SkipWaitingIfNearLastWaitingPose">
+                <ArePosesNear ref_pose="{last_pose}" target_pose="{current_pose}" tolerance="1.5"/>
+                <Sequence>
+                  <SetBlackboard output_key="last_pose" value="{current_pose}"/>
+                  <CancelControl/>
+                  <Wait wait_duration="4.0"/>
+                  <AlwaysSuccess/>
+                </Sequence>
+              </Fallback>
+                <RetryUntilSuccessful num_attempts="1000"> 
+                  <Sequence name="Replan">
+                    <!-- <GetNextFewGoals num_goals="5" input_goals="{goals}" output_goals="{planning_goals}"/> -->
+                    <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "5" cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+                    <GetNextFewGoals num_goals="5" input_goals="{goals}" output_goals="{planning_goals}"/>
+                    <!-- It's for checking purpose if planning goals is not empty. If it's empty then this sequence should be repeated-->
+                    <ComputePathThroughPoses goals="{planning_goals}" path="{path}"
+                                    planner_id="{selected_planner}"
+                                    error_code_id="{compute_path_error_code}"
+                                    error_msg="{compute_path_error_msg}"/>
+                    <PublishGoals input_goals="{planning_goals}" topic_name= "truncated_goals"/>               
+                  </Sequence>
+                </RetryUntilSuccessful>
+              <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "100" cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+              <ComputePathThroughPoses goals="{goals}" path="{path}"
+                                      planner_id="{selected_planner}"
+                                      error_code_id="{compute_path_error_code}"
+                                      error_msg="{compute_path_error_msg}"/>  -->
+              <!-- <ConcatenatePaths input_path1="{path}" input_path2="{avoid_path}" output_path="{path}"/> -->
+            </Sequence>
+          </Fallback>
+        </ForceFailure>
+      </RetryUntilSuccessful>
+
+
+
+
+
+      <!-- If all goals has been removed then it will return failure and the procedure will be repeated-->
+      <!-- <ReactiveSequence name="MonitorAndFollowPath">
+      <RetryUntilSuccessful num_attempts="1000">
+        <Inverter>
+          <SequenceWithMemory name="CancelingControlAndWait">
+            <Inverter>
+              <isCollisionFree nav_goal="{goals}" cost_threshold="100.0" radius="1.5"/>
+            </Inverter>
+            <Inverter>
+              <IsPathValid path="{path}"/>
+            </Inverter>
+            <CancelControl name="ControlCancel"/>
+            <Wait wait_duration="3"/>
+            <RetryUntilSuccessful num_attempts="1000"> 
+              <Sequence>
+                <GetNextFewGoals num_goals="5" input_goals="{goal_poses}" output_goals="{planning_goals}"/>
+                <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "5" cost_threshold="254.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+                <GetNextFewGoals num_goals="1" input_goals="{planning_goals}" output_goals="{test_goal}"/>
+              </Sequence>
+            </RetryUntilSuccessful>
+            <ComputePathThroughPoses goals="{planning_goals}" path="{avoid_path}"
+                                planner_id="{selected_planner}"
+                                error_code_id="{compute_path_error_code}"
+                                error_msg="{compute_path_error_msg}"/>
+            <ConcatenatePaths input_path1="{path}" input_path2="{avoid_path}" output_path="{path}"/>
+          </SequenceWithMemory>
+        </Inverter>
+      </RetryUntilSuccessful>
+      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
+    </ReactiveSequence>  -->
+
+      </PipelineSequence>
+    </Sequence>
+  </BehaviorTree>
+</root>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy 2.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy 2.xml
new file mode 100644
index 00000000000..290cb7607d1
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy 2.xml	
@@ -0,0 +1,134 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
+   and it also has recovery actions specific to planning / control as well as general system issues.
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+
+    <Sequence>
+      <RecoveryNode number_of_retries="1000000" name="ComputePathRecovery">
+        <Sequence name="ComputePathSequence">
+          <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/>
+          <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+          <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+          <SetBlackboard output_key="obstacle_ahead" value="false"/>
+          <GetCurrentPose current_pose="{current_pose}"/>
+          <SetBlackboard output_key="last_stop_pose" value="{current_pose}"/>
+          <SetBlackboard output_key="is_near_last_stop" value="false"/>
+        </Sequence>
+        <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+      </RecoveryNode>
+
+
+      <RetryUntilSuccessful num_attempts="6">
+        <Sequence>
+          <PipelineSequence name="NavigateWithReplanning">
+            <RateController hz="2.0">
+            <Fallback> 
+                <ReactiveSequence>
+                    <Sequence>
+                      <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+                      <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" /> -->
+                      <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/>
+                      <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                      <!-- <Inverter>
+                        <isCollisionFree nav_goal = "{last_removed_goal}" cost_threshold="100.0" radius="1.5"/>
+                      </Inverter> -->
+                    </Sequence>
+                    
+                    <Sequence>
+                      <TruncatePathLocal input_path="{path}" output_path="{path_local}" distance_forward="3.5" distance_backward="0.0" robot_frame="base_link"/>
+                      <GetPoseFromPath path="{path_local}" index="-1" pose="{last_path_point}"/>
+                      <!-- <GetMiddlePoseFromPath path="{path}" pose="{middle_path_point}"/> -->
+                      <Inverter>
+                          <!-- It shoult allow for replanning if close to the (truncated) path end-->
+                          <!-- Distance threshold to last point is set from nav2 param goal_reached_tol -->
+                          <GoalReached goal="{last_path_point}" robot_base_frame="base_link"/> 
+
+                      </Inverter>
+                    </Sequence>
+                    <IsPathValid path="{path}" max_cost="254"/>
+                </ReactiveSequence>
+                <Sequence>
+                  <RecoveryNode number_of_retries="1000000" name="ComputePathRecovery">
+                      <Sequence>
+                          <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+                          <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/>
+                          <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                          <!-- isCollisionFree is optional here because ComputePathThroughPoses-->
+                          <isCollisionFree nav_goal = "{goals}" cost_threshold="100.0" radius="1.5"/>
+                          <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+                          <SetBlackboard output_key="obstacle_ahead" value="false"/>
+                      </Sequence>
+                      <Sequence>
+                        <SetBlackboard output_key="obstacle_ahead" value="true"/>
+                        <Delay delay_msec="100">
+                          <!-- Add delay to allow MonitorAndFollowPath catch obstacle_ahead -->
+                            <AlwaysSuccess/>
+                        </Delay>
+                        <!-- Remove AFTER WAITING or immediately if pose is close -->
+                        <!-- <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/> -->
+                        <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" /> -->
+                        <!-- <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/> -->
+                        <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+                        <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/>
+                        <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                      </Sequence>
+                  </RecoveryNode>
+                </Sequence>
+            </Fallback>
+            </RateController>
+
+            <ReactiveSequence name="MonitorAndFollowPath">
+              <RetryUntilSuccessful num_attempts="1000">
+                <Inverter>
+                  <SequenceWithMemory name="CancelingControlAndWait">
+                    <!-- <isObstacleApproaching approaching_obstacles_topic="/dynamic_obstacles/approaching_flag"/> -->
+                    <ScriptCondition code="obstacle_ahead == true"/>
+                    <!-- <GetCurrentPose current_pose="{current_pose}"/> -->
+                    <Fallback name="WaitIfFarFromLastWaitingPose">
+                      <!-- Przerobić ArePosesNear tak, aby mierzona byla dlugosc sciezki a nie odleglosc euklidesowa -->
+                      <!-- <ArePosesNear ref_pose="{last_stop_pose}" target_pose="{current_pose}" tolerance="1.5"/> -->
+                      <ScriptCondition code="is_near_last_stop == true"/>
+                      <Sequence>
+                        <CancelControl name="ControlCancel"/>
+                        <Wait wait_duration="3"/>
+                        <GetCurrentPose current_pose="{current_pose}"/>
+                        <SetBlackboard output_key="last_stop_pose" value="{current_pose}"/>
+                        <SetBlackboard output_key="is_near_last_stop" value="true"/>
+                      </Sequence>
+                    </Fallback>
+                    <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+                  </SequenceWithMemory>
+                </Inverter>
+              </RetryUntilSuccessful>
+              <!-- <FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="path_follow_goal_checker"/> -->
+              <ForceSuccess>
+                <Sequence>
+                  <GetCurrentPose current_pose="{current_pose}"/>
+                  <Inverter>
+                    <ArePosesNear ref_pose="{last_stop_pose}" target_pose="{current_pose}" tolerance="1.5"/>
+                  </Inverter>
+                  <SetBlackboard output_key="is_near_last_stop" value="false"/>
+                </Sequence>
+              </ForceSuccess> 
+              <FollowPath path="{path}"
+                            controller_id="{selected_controller}"
+                            error_code_id="{follow_path_error_code}"/>
+            </ReactiveSequence>  
+
+          </PipelineSequence>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>
\ No newline at end of file
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy.xml
new file mode 100644
index 00000000000..627b183a8da
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy copy.xml	
@@ -0,0 +1,88 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
+   and it also has recovery actions specific to planning / control as well as general system issues.
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+      <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
+      <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
+      <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
+      <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
+      <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+      <GetCurrentPose current_pose="{current_pose}"/>
+      <PublishGoals input_goals="{goals}" topic_name= "debug_goals"/> 
+      <!-- <SetBlackboard output_key="last_pose" value="{initial_pose}"/> -->
+      <SetBlackboard output_key="stopped" value="false"/>
+
+      <PipelineSequence name="NavigateLoop">
+       
+        <RateController hz="1.0">
+          <ForceSuccess>
+            <Sequence>
+              <ScriptCondition code="stopped == false"/>
+              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+              <TruncatePathLocal input_path="{path}" pose="{current_pose}" output_path="{path_local}" distance_forward="1.0" distance_backward="0.0" robot_frame="base_link"/>
+              <GetPoseFromPath path="{path_local}" index="-1" pose="{goal}"/>
+              <AppendGoalPoseToGoals goal_pose="{goal}" input_goals="{goal_poses}" output_goals="{goal_poses}"/>
+              <PublishGoals input_goals="{goal_poses}" topic_name= "goal_poses"/> 
+            </Sequence>
+          </ForceSuccess>
+        </RateController>
+        
+        <RetryUntilSuccessful num_attempts="1000">
+          <ForceFailure>
+            <Fallback name="FollowPathAndAvoidObstacles">
+              <ReactiveSequence>
+                <GetCurrentPose current_pose="{current_pose}"/>
+                <IsPathValid path="{path_local}" max_cost="254"/>
+                <FollowPath path="{path_local}"
+                            controller_id="{selected_controller}"
+                            error_code_id="{follow_path_error_code}"/>
+              </ReactiveSequence>
+              <Sequence>
+                <SetBlackboard output_key="stopped" value="true"/>
+                <!-- <Wait wait_duration="1.0"/> -->
+                <RetryUntilSuccessful num_attempts="1000"> 
+                  <Sequence name="Replan">
+                    <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "5" cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+                    <GetNextFewGoals num_goals="5" input_goals="{goals}" output_goals="{planning_goals}"/>
+                    <ComputePathThroughPoses goals="{planning_goals}" path="{avoid_path}"
+                                    planner_id="{selected_planner}"
+                                    error_code_id="{compute_path_error_code}"
+                                    error_msg="{compute_path_error_msg}"/>
+                    <PublishGoals input_goals="{planning_goals}" topic_name= "truncated_goals"/>
+                    <Delay delay_msec="1000">
+                    <!-- Prevent replanning more frequent than 1 Hz. Delay used instead of Wait because it doesn't spam with logs -->
+                      <AlwaysSuccess/>
+                    </Delay>
+                  </Sequence>
+                </RetryUntilSuccessful>
+                <!-- 
+                <GetPoseFromPath path="{avoid_path}" index="-1" pose="{next_pose}"/>
+                <SetBlackboard output_key="current_pose" value="{next_pose}"/>
+                <TruncatePathLocal input_path="{path}" pose="{current_pose}" output_path="{path_local}" distance_forward="1.0" distance_backward="0.0" robot_frame="base_link"/>
+                <ConcatenatePaths input_path1="{avoid_path}" input_path2="{path_local}" output_path="{path_local}"/>
+
+                -->
+                <GetPoseFromPath path="{avoid_path}" index="-1" pose="{next_pose}"/>
+                <SetBlackboard output_key="current_pose" value="{next_pose}"/>
+                <TruncatePathLocal input_path="{path}" pose="{current_pose}" output_path="{path_local}" distance_forward="1.0" distance_backward="0.0" robot_frame="base_link"/>
+                <TruncatePathLocal input_path="{path}" pose="{current_pose}" output_path="{path}" distance_forward="100000000000.0" distance_backward="0.0" robot_frame="base_link"/>
+                <ConcatenatePaths input_path1="{avoid_path}" input_path2="{path}" output_path="{path}"/>
+                <ConcatenatePaths input_path1="{avoid_path}" input_path2="{path}" output_path="{path_local}"/>
+                <SetBlackboard output_key="stopped" value="false"/>
+              </Sequence>
+            </Fallback>
+          </ForceFailure>
+        </RetryUntilSuccessful>
+      </PipelineSequence>
+    </Sequence>
+  </BehaviorTree>
+</root>
+
+
+
+
+
diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml
new file mode 100644
index 00000000000..1e8079d8035
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery_if_invalid_copy.xml
@@ -0,0 +1,157 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
+   and it also has recovery actions specific to planning / control as well as general system issues.
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+
+    <Sequence>
+      <RecoveryNode number_of_retries="1000000" name="ComputePathRecovery">
+        <Sequence name="ComputePathSequence">
+          <!-- <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/> -->
+          <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/>
+          <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+          <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+          <SetBlackboard output_key="obstacle_ahead" value="false"/>
+          <GetCurrentPose current_pose="{current_pose}"/>
+          <SetBlackboard output_key="last_stop_pose" value="{current_pose}"/>
+          <SetBlackboard output_key="is_near_last_stop" value="false"/>
+        </Sequence>
+        <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+      </RecoveryNode>
+
+
+      <RetryUntilSuccessful num_attempts="6">
+        <Sequence>
+          <PipelineSequence name="NavigateWithReplanning">
+            <RateController hz="2.0">
+            <Fallback> 
+                <ReactiveSequence>
+                    <Sequence>
+                      <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "1" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+                      <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" /> -->
+                      <!-- <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/> -->
+                      <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/>
+                      <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                      <!-- <Inverter>
+                        <isCollisionFree nav_goal = "{last_removed_goal}" cost_threshold="100.0" radius="1.5"/>
+                      </Inverter> -->
+                    </Sequence>
+                    
+                    <Sequence>
+                      <TruncatePathLocal input_path="{path}" output_path="{path_local}" distance_forward="3.5" distance_backward="0.0" robot_frame="base_link"/>
+                      <!-- <GetPoseFromPath path="{path_local}" index="-1" pose="{last_path_point}"/> -->
+                      <!-- <GetMiddlePoseFromPath path="{path}" pose="{middle_path_point}"/> -->
+                      <Inverter>
+                          <!-- It shoult allow for replanning if close to the (truncated) path end-->
+                          <!-- Distance threshold to last point is set from nav2 param goal_reached_tol -->
+                          <!-- <GoalReached goal="{last_path_point}" robot_base_frame="base_link"/>  -->
+                          <PoseReached path="{path_local}" robot_base_frame="base_link"/>
+                      </Inverter>
+                    </Sequence>
+                    <IsPathValid path="{path}" max_cost="254"/>
+                </ReactiveSequence>
+                <Sequence>
+                  <RecoveryNode number_of_retries="1000000" name="ComputePathRecovery">
+                      <Sequence>
+                          <!-- <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/> -->
+                          <!-- <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/> -->
+                          <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/>
+                          <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                          <!-- isCollisionFree is optional here because ComputePathThroughPoses-->
+                          <Fallback>
+                            <Sequence>
+                              <isCollisionFree nav_goal="{truncated_goals}" cost_threshold="100.0" truncation_samples="6"/>
+                              <SetBlackboard output_key="obstacle_ahead" value="false"/>
+                            </Sequence>
+                            <Sequence>
+                              <Inverter>
+                                <isCollisionFree nav_goal="{truncated_goals}" cost_threshold="100.0" truncation_samples="6"/>
+                              </Inverter>
+                              <SetBlackboard output_key="obstacle_ahead" value="true"/>
+                              <Delay delay_msec="100">
+                                <AlwaysSuccess/>
+                              </Delay>
+                            </Sequence>
+                          </Fallback>
+                          <ScriptCondition code="obstacle_ahead == false"/>
+                          <ComputePathThroughPoses goals="{truncated_goals}" path="{path}" planner_id="GridBased"/>
+                          <!-- <SetBlackboard output_key="obstacle_ahead" value="false"/> -->
+                      </Sequence>
+                      <Sequence>
+                        <ForceSuccess>
+                          <Sequence>
+                            <ScriptCondition code="obstacle_ahead == false"/>
+                            <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/>
+                          </Sequence>
+                        </ForceSuccess>
+                        <!-- <SetBlackboard output_key="obstacle_ahead" value="true"/> -->
+                        <Delay delay_msec="100">
+                            <AlwaysSuccess/>
+                        </Delay>
+                        <!-- Remove AFTER WAITING or immediately if pose is close -->
+                        <!-- <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/> -->
+                        <!-- <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" /> -->
+                        <!-- <RemoveNextGoal input_goals="{goals}" output_goals="{goals}"/> -->
+                        <!-- <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" radius="0.5" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/> -->
+                        <!-- <GetNextFewGoals num_goals="6" input_goals="{goals}" output_goals="{truncated_goals}"/> -->
+                        <GoalsTruncation input_goals="{goals}" truncation_samples="6" truncated_goals="{truncated_goals}"/>
+                        <PublishGoals input_goals="{truncated_goals}" topic_name= "truncated_path"/>
+                      </Sequence>
+                  </RecoveryNode>
+                </Sequence>
+            </Fallback>
+            </RateController>
+
+            <ReactiveSequence name="MonitorAndFollowPath">
+              <RetryUntilSuccessful num_attempts="1000">
+                <Inverter>
+                  <SequenceWithMemory name="CancelingControlAndWait">
+                    <!-- <isObstacleApproaching approaching_obstacles_topic="/dynamic_obstacles/approaching_flag"/> -->
+                    <ScriptCondition code="obstacle_ahead == true"/>
+                    <!-- <GetCurrentPose current_pose="{current_pose}"/> -->
+                    <Fallback name="WaitIfFarFromLastWaitingPose">
+                      <!-- <ArePosesNear ref_pose="{last_stop_pose}" target_pose="{current_pose}" tolerance="1.5"/> -->
+                      <ScriptCondition code="is_near_last_stop == true"/>
+                      <Sequence>
+                        <CancelControl name="ControlCancel"/>
+                        <Wait wait_duration="3"/>
+                        <GetCurrentPose current_pose="{current_pose}"/>
+                        <SetBlackboard output_key="last_stop_pose" value="{current_pose}"/>
+                        <SetBlackboard output_key="is_near_last_stop" value="true"/>
+                      </Sequence>
+                    </Fallback>
+                    <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "6" 
+                      cost_threshold="253.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" 
+                      input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+                  </SequenceWithMemory>
+                </Inverter>
+              </RetryUntilSuccessful>
+              <!-- <FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="path_follow_goal_checker"/> -->
+              <ForceSuccess>
+                <Sequence>
+                  <GetCurrentPose current_pose="{current_pose}"/>
+                  <Inverter>
+                    <ArePosesNear ref_pose="{last_stop_pose}" target_pose="{current_pose}" tolerance="1.5"/>
+                  </Inverter>
+                  <SetBlackboard output_key="is_near_last_stop" value="false"/>
+                </Sequence>
+              </ForceSuccess> 
+              <FollowPath path="{path}"
+                            controller_id="FollowPath"
+                            error_code_id="{follow_path_error_code}"/>
+            </ReactiveSequence>  
+
+          </PipelineSequence>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>
\ No newline at end of file
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker copy.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker copy.hpp
new file mode 100644
index 00000000000..36b16755846
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker copy.hpp	
@@ -0,0 +1,94 @@
+// 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.
+//
+// Modified by: Shivang Patel (shivaang14@gmail.com)
+
+#ifndef NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
+#define NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace nav2_costmap_2d
+{
+typedef std::vector<geometry_msgs::msg::Point> Footprint;
+
+/**
+ * @class FootprintCollisionChecker
+ * @brief Checker for collision with a footprint on a costmap
+ */
+template<typename CostmapT>
+class FootprintCollisionChecker
+{
+public:
+  /**
+   * @brief A constructor.
+   */
+  FootprintCollisionChecker();
+  /**
+   * @brief A constructor.
+   */
+  explicit FootprintCollisionChecker(CostmapT costmap);
+  /**
+   * @brief Find the footprint cost in oriented footprint
+   * @param footprint The footprint to check
+   * @param check_full_area If true, checks the full area after perimeter check (default: false)
+   * @return The maximum cost found (perimeter only or full area depending on parameter)
+   */
+  double footprintCost(const Footprint & footprint, bool check_full_area = false);
+  /**
+   * @brief Find the footprint cost a a post with an unoriented footprint
+   */
+  double footprintCostAtPose(
+    double x, double y, double theta, const Footprint & footprint,
+    bool check_full_area = false);
+  /**
+   * @brief Get the cost for a line segment
+   */
+  double lineCost(int x0, int x1, int y0, int y1) const;
+  /**
+   * @brief Get the map coordinates from a world point
+   */
+  bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
+  /**
+   * @brief Get the cost of a point
+   */
+  double pointCost(int x, int y) const;
+  /**
+  * @brief Set the current costmap object to use for collision detection
+  */
+  void setCostmap(CostmapT costmap);
+  /**
+  * @brief Get the current costmap object
+  */
+  CostmapT getCostmap()
+  {
+    return costmap_;
+  }
+
+protected:
+  CostmapT costmap_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 32b17ca5d5b..9e035eb6aeb 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -220,6 +220,7 @@ class ObstacleLayer : public CostmapLayer
 
   std::vector<geometry_msgs::msg::Point> transformed_footprint_;
   bool footprint_clearing_enabled_;
+  double footprint_clearing_padding_;
   /**
    * @brief Clear costmap layer info below the robot's footprint
    */
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 0333f03577b..6ffdbca4980 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
@@ -172,6 +172,7 @@ class StaticLayer : public CostmapLayer
   std::vector<geometry_msgs::msg::Point> transformed_footprint_;
   bool footprint_clearing_enabled_;
   bool restore_cleared_footprint_;
+  double footprint_clearing_padding_;
   /**
    * @brief Clear costmap layer info below the robot's footprint
    */
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index c09d60da61b..352a58f563d 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -89,6 +89,7 @@ void ObstacleLayer::onInitialize()
   declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
   declareParameter("combination_method", rclcpp::ParameterValue(1));
   declareParameter("observation_sources", rclcpp::ParameterValue(std::string("")));
+  declareParameter("footprint_clearing_padding", rclcpp::ParameterValue(0.0));
 
   auto node = node_.lock();
   if (!node) {
@@ -97,6 +98,7 @@ void ObstacleLayer::onInitialize()
 
   node->get_parameter(name_ + "." + "enabled", enabled_);
   node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
+  node->get_parameter(name_ + "." + "footprint_clearing_padding", footprint_clearing_padding_);
   node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
   node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
   node->get_parameter("track_unknown_space", track_unknown_space);
@@ -587,7 +589,14 @@ ObstacleLayer::updateFootprint(
   double * max_y)
 {
   if (!footprint_clearing_enabled_) {return;}
-  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
+  
+  // Get the base footprint and apply padding before transformation
+  std::vector<geometry_msgs::msg::Point> padded_footprint = getFootprint();
+  if (footprint_clearing_padding_ > 0.0) {
+    padFootprint(padded_footprint, footprint_clearing_padding_);
+  }
+  // Transform the padded footprint to the robot's current pose
+  transformFootprint(robot_x, robot_y, robot_yaw, padded_footprint, transformed_footprint_);
 
   for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
     touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index 94aa69f87f8..18a0eadb153 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -139,6 +139,7 @@ StaticLayer::getParameters()
   declareParameter("map_topic", rclcpp::ParameterValue("map"));
   declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false));
   declareParameter("restore_cleared_footprint", rclcpp::ParameterValue(true));
+  declareParameter("footprint_clearing_padding", rclcpp::ParameterValue(0.0));
 
   auto node = node_.lock();
   if (!node) {
@@ -149,6 +150,7 @@ StaticLayer::getParameters()
   node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
   node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
   node->get_parameter(name_ + "." + "restore_cleared_footprint", restore_cleared_footprint_);
+  node->get_parameter(name_ + "." + "footprint_clearing_padding", footprint_clearing_padding_);
   node->get_parameter(name_ + "." + "map_topic", map_topic_);
   map_topic_ = joinWithParentNamespace(map_topic_);
   node->get_parameter(
@@ -386,8 +388,13 @@ StaticLayer::updateFootprint(
   double * max_y)
 {
   if (!footprint_clearing_enabled_) {return;}
-
-  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
+  // Get the base footprint and apply padding before transformation
+  std::vector<geometry_msgs::msg::Point> padded_footprint = getFootprint();
+  if (footprint_clearing_padding_ > 0.0) {
+    padFootprint(padded_footprint, footprint_clearing_padding_);
+  }
+  // Transform the padded footprint to the robot's current pose
+  transformFootprint(robot_x, robot_y, robot_yaw, padded_footprint, transformed_footprint_);
 
   for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
     touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
@@ -511,6 +518,8 @@ StaticLayer::dynamicParametersCallback(
     } else if (param_type == ParameterType::PARAMETER_DOUBLE) {
       if (param_name == name_ + "." + "transform_tolerance") {
         transform_tolerance_ = tf2::durationFromSec(parameter.as_double());
+      } else if (param_name == name_ + "." + "footprint_clearing_padding") {
+        footprint_clearing_padding_ = parameter.as_double();
       }
     } else if (param_type == ParameterType::PARAMETER_BOOL) {
       if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
diff --git a/nav2_costmap_2d/src/footprint_collision_checker copy.cpp b/nav2_costmap_2d/src/footprint_collision_checker copy.cpp
new file mode 100644
index 00000000000..da234601a05
--- /dev/null
+++ b/nav2_costmap_2d/src/footprint_collision_checker copy.cpp	
@@ -0,0 +1,268 @@
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2025 Angsa Robotics
+//
+// 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.
+//
+// Modified by: Shivang Patel (shivaang14@gmail.com)
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/exceptions.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_util/line_iterator.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+// Simple 2D point structure
+struct Point2D
+{
+  int x, y;
+  Point2D(int x_val, int y_val)
+  : x(x_val), y(y_val) {}
+};
+
+// Simple rectangle structure
+struct Rectangle
+{
+  int x, y, width, height;
+  Rectangle(int x_val, int y_val, int w, int h)
+  : x(x_val), y(y_val), width(w), height(h) {}
+};
+
+// Calculate bounding rectangle for a set of points
+Rectangle calculateBoundingRect(const std::vector<Point2D> & points)
+{
+  if (points.empty()) {
+    return Rectangle(0, 0, 0, 0);
+  }
+
+  int min_x = points[0].x;
+  int max_x = points[0].x;
+  int min_y = points[0].y;
+  int max_y = points[0].y;
+
+  for (const auto & pt : points) {
+    min_x = std::min(min_x, pt.x);
+    max_x = std::max(max_x, pt.x);
+    min_y = std::min(min_y, pt.y);
+    max_y = std::max(max_y, pt.y);
+  }
+
+  return Rectangle(min_x, min_y, max_x - min_x + 1, max_y - min_y + 1);
+}
+
+// Point-in-polygon test using ray casting algorithm
+bool isPointInPolygon(int x, int y, const std::vector<Point2D> & polygon)
+{
+  if (polygon.size() < 3) {
+    return false;
+  }
+
+  bool inside = false;
+  size_t j = polygon.size() - 1;
+
+  for (size_t i = 0; i < polygon.size(); i++) {
+    if (((polygon[i].y > y) != (polygon[j].y > y)) &&
+      (x <
+      (polygon[j].x - polygon[i].x) * (y - polygon[i].y) / (polygon[j].y - polygon[i].y) +
+      polygon[i].x))
+    {
+      inside = !inside;
+    }
+    j = i;
+  }
+
+  return inside;
+}
+
+template<typename CostmapT>
+FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker()
+: costmap_(nullptr)
+{
+}
+
+template<typename CostmapT>
+FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(
+  CostmapT costmap)
+: costmap_(costmap)
+{
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::footprintCost(
+  const Footprint & footprint,
+  bool check_full_area)
+{
+  if (footprint.empty()) {
+    return static_cast<double>(NO_INFORMATION);
+  }
+
+  // Pre-convert all footprint points to map coordinates once
+  std::vector<Point2D> polygon_points;
+  polygon_points.reserve(footprint.size());
+
+  for (const auto & point : footprint) {
+    unsigned int mx, my;
+    if (!worldToMap(point.x, point.y, mx, my)) {
+      return static_cast<double>(NO_INFORMATION);
+    }
+    polygon_points.emplace_back(static_cast<int>(mx), static_cast<int>(my));
+  }
+
+  // Check perimeter using pre-converted coordinates
+  double perimeter_cost = 0.0;
+  for (size_t i = 0; i < polygon_points.size(); ++i) {
+    size_t next_i = (i + 1) % polygon_points.size();
+    double line_cost = lineCost(
+      polygon_points[i].x, polygon_points[next_i].x,
+      polygon_points[i].y, polygon_points[next_i].y);
+
+    perimeter_cost = std::max(perimeter_cost, line_cost);
+
+    // Early termination if lethal obstacle found
+    if (perimeter_cost == static_cast<double>(LETHAL_OBSTACLE)) {
+      return perimeter_cost;
+    }
+  }
+
+  // If perimeter check found collision or full area check not requested, return perimeter result
+  if (perimeter_cost == static_cast<double>(LETHAL_OBSTACLE) || !check_full_area) {
+    return perimeter_cost;
+  }
+
+  // If no collision on perimeter and full area check requested, rasterize the full area
+  Rectangle bbox = calculateBoundingRect(polygon_points);
+
+  // Clamp bounding box to costmap dimensions for safety
+  unsigned int costmap_width = costmap_->getSizeInCellsX();
+  unsigned int costmap_height = costmap_->getSizeInCellsY();
+
+  int min_x = std::max(0, bbox.x);
+  int min_y = std::max(0, bbox.y);
+  int max_x = std::min(static_cast<int>(costmap_width - 1), bbox.x + bbox.width - 1);
+  int max_y = std::min(static_cast<int>(costmap_height - 1), bbox.y + bbox.height - 1);
+
+  // Translate polygon points to bounding box coordinates
+  std::vector<Point2D> bbox_polygon;
+  bbox_polygon.reserve(polygon_points.size());
+  for (const auto & pt : polygon_points) {
+    bbox_polygon.emplace_back(pt.x - bbox.x, pt.y - bbox.y);
+  }
+
+  double max_cost = perimeter_cost;
+
+  // Iterate through the clamped bounding box and check costs only for cells inside the polygon
+  for (int y = min_y; y <= max_y; ++y) {
+    for (int x = min_x; x <= max_x; ++x) {
+      // Convert to bounding box coordinates for polygon test
+      int bbox_x = x - bbox.x;
+      int bbox_y = y - bbox.y;
+
+      if (isPointInPolygon(bbox_x, bbox_y, bbox_polygon)) {
+        double cell_cost = pointCost(x, y);
+
+        // Early termination if lethal obstacle found
+        if (cell_cost == static_cast<double>(LETHAL_OBSTACLE)) {
+          return cell_cost;
+        }
+
+        max_cost = std::max(max_cost, cell_cost);
+      }
+    }
+  }
+
+  return max_cost;
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::lineCost(int x0, int x1, int y0, int y1) const
+{
+  double line_cost = 0.0;
+  double point_cost = -1.0;
+
+  for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
+    point_cost = pointCost(line.getX(), line.getY());   // Score the current point
+
+    // if in collision, no need to continue
+    if (point_cost == static_cast<double>(LETHAL_OBSTACLE)) {
+      return point_cost;
+    }
+
+    if (line_cost < point_cost) {
+      line_cost = point_cost;
+    }
+  }
+
+  return line_cost;
+}
+
+template<typename CostmapT>
+bool FootprintCollisionChecker<CostmapT>::worldToMap(
+  double wx, double wy, unsigned int & mx, unsigned int & my)
+{
+  return costmap_->worldToMap(wx, wy, mx, my);
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::pointCost(int x, int y) const
+{
+  // Bounds checking to prevent segmentation faults
+  if (x < 0 || y < 0 ||
+    x >= static_cast<int>(costmap_->getSizeInCellsX()) ||
+    y >= static_cast<int>(costmap_->getSizeInCellsY()))
+  {
+    return static_cast<double>(LETHAL_OBSTACLE);
+  }
+
+  return static_cast<double>(costmap_->getCost(x, y));
+}
+
+template<typename CostmapT>
+void FootprintCollisionChecker<CostmapT>::setCostmap(CostmapT costmap)
+{
+  costmap_ = costmap;
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(
+  double x, double y, double theta, const Footprint & footprint, bool check_full_area)
+{
+  double cos_th = cos(theta);
+  double sin_th = sin(theta);
+  Footprint oriented_footprint;
+  oriented_footprint.reserve(footprint.size());
+  geometry_msgs::msg::Point new_pt;
+  for (unsigned int i = 0; i < footprint.size(); ++i) {
+    new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th);
+    new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th);
+    oriented_footprint.push_back(new_pt);
+  }
+
+  return footprintCost(oriented_footprint, check_full_area);
+}
+
+// declare our valid template parameters
+template class FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>;
+template class FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>;
+
+}  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test copy.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test copy.cpp
new file mode 100644
index 00000000000..c11fbb14d52
--- /dev/null
+++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test copy.cpp	
@@ -0,0 +1,793 @@
+// Copyright (c) 2020 Shivang Patel
+//
+// 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 <string>
+#include <vector>
+#include <memory>
+#include <cmath>
+#include <chrono>
+#include <iostream>
+#include <iomanip>
+
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+
+// Test basic footprint collision checking with no obstacles
+// Verifies that a diamond-shaped footprint placed on an empty costmap returns zero cost
+TEST(collision_footprint, test_basic)
+{
+  // Create a 100x100 costmap with 0.1m resolution, all cells initialized to cost 0
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  // Create a diamond-shaped footprint
+  geometry_msgs::msg::Point p1;
+  p1.x = -0.5;  // Left vertex
+  p1.y = 0.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 0.0;   // Top vertex
+  p2.y = 0.5;
+  geometry_msgs::msg::Point p3;
+  p3.x = 0.5;   // Right vertex
+  p3.y = 0.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = 0.0;   // Bottom vertex
+  p4.y = -0.5;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Check footprint cost at position (5.0, 5.0) with no rotation
+  // Should return 0 since no obstacles are present
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+
+  EXPECT_NEAR(value, 0.0, 0.001);
+}
+
+// Test point cost functionality
+// Verifies that the pointCost method correctly returns the cost value at a specific grid cell
+TEST(collision_footprint, test_point_cost)
+{
+  // Create empty costmap with all cells at cost 0
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test point cost at grid cell (50, 50) - should be 0 for empty costmap
+  auto value = collision_checker.pointCost(50, 50);
+
+  EXPECT_NEAR(value, 0.0, 0.001);
+}
+
+// Test world-to-map coordinate conversion and point cost retrieval
+// Verifies that world coordinates are correctly converted to map coordinates
+// and that costs can be retrieved and set at those locations
+TEST(collision_footprint, test_world_to_map)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  unsigned int x, y;
+
+  // Convert world coordinates (1.0, 1.0) to map coordinates
+  // With 0.1m resolution and origin at (0,0), this should map to grid cell (10, 10)
+  collision_checker.worldToMap(1.0, 1.0, x, y);
+
+  // Check that the cost at the converted coordinates is 0 (empty space)
+  auto value = collision_checker.pointCost(x, y);
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  // Set a cost of 200 at grid cell (50, 50) and verify it can be retrieved
+  costmap_->setCost(50, 50, 200);
+  collision_checker.worldToMap(5.0, 5.0, x, y);  // World (5.0, 5.0) -> Grid (50, 50)
+
+  EXPECT_NEAR(collision_checker.pointCost(x, y), 200.0, 0.001);
+}
+
+// Test footprint collision detection when robot moves into obstacle areas
+// Creates a costmap with obstacles everywhere except a safe zone, then tests
+// footprint placement at different positions to verify collision detection
+TEST(collision_footprint, test_footprint_at_pose_with_movement)
+{
+  // Create costmap with all cells set to cost 254 (high obstacle cost)
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 254);
+
+  // Create a safe zone (cost 0) in the center of the map from grid (40,40) to (60,60)
+  // This corresponds to world coordinates roughly (4.0,4.0) to (6.0,6.0)
+  for (unsigned int i = 40; i <= 60; ++i) {
+    for (unsigned int j = 40; j <= 60; ++j) {
+      costmap_->setCost(i, j, 0);
+    }
+  }
+
+  // Create a 2x2 meter square footprint
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;  // Bottom-left
+  p1.y = 1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;   // Bottom-right
+  p2.y = 1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;   // Top-right
+  p3.y = -1.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;  // Top-left
+  p4.y = -1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test footprint at center of safe zone - should have zero cost
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  // Test footprint moved slightly up - should hit obstacle area (cost 254)
+  auto up_value = collision_checker.footprintCostAtPose(5.0, 4.9, 0.0, footprint);
+  EXPECT_NEAR(up_value, 254.0, 0.001);
+
+  // Test footprint moved slightly down - should hit obstacle area (cost 254)
+  auto down_value = collision_checker.footprintCostAtPose(5.0, 5.2, 0.0, footprint);
+  EXPECT_NEAR(down_value, 254.0, 0.001);
+}
+
+// Test point and line cost detection with strategically placed obstacles
+// Places obstacles at specific locations and verifies they are detected when the
+// footprint perimeter intersects them during different robot movements
+TEST(collision_footprint, test_point_and_line_cost)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.10000, 0, 0.0, 0.0);
+
+  // Place obstacles at strategic locations:
+  costmap_->setCost(62, 50, 254);  // Obstacle to the right of center
+  costmap_->setCost(39, 60, 254);  // Obstacle to the left and up from center
+
+  // Create a 2x2 meter square footprint
+  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};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test footprint at center position - should not hit obstacles
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  // Move footprint left - should detect obstacle at (39, 60)
+  auto left_value = collision_checker.footprintCostAtPose(4.9, 5.0, 0.0, footprint);
+  EXPECT_NEAR(left_value, 254.0, 0.001);
+
+  // Move footprint right - should detect obstacle at (62, 50)
+  auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint);
+  EXPECT_NEAR(right_value, 254.0, 0.001);
+}
+
+// Test utility function for footprints with insufficient points
+// Verifies that distance calculation functions handle edge cases correctly
+// when footprint has fewer than 3 points (minimum for a valid polygon)
+TEST(collision_footprint, not_enough_points)
+{
+  geometry_msgs::msg::Point p1;
+  p1.x = 2.0;
+  p1.y = 2.0;
+
+  geometry_msgs::msg::Point p2;
+  p2.x = -2.0;
+  p2.y = -2.0;
+
+  // Create footprint with only 2 points (insufficient for polygon)
+  std::vector<geometry_msgs::msg::Point> footprint = {p1, p2};
+  double min_dist = 0.0;
+  double max_dist = 0.0;
+
+  // calculateMinAndMaxDistances should handle this gracefully
+  std::tie(min_dist, max_dist) = nav2_costmap_2d::calculateMinAndMaxDistances(footprint);
+  EXPECT_EQ(min_dist, std::numeric_limits<double>::max());
+  EXPECT_EQ(max_dist, 0.0f);
+}
+
+// Test conversion from Point to Point32 message types
+// Verifies that geometry message type conversions preserve coordinate values
+TEST(collision_footprint, to_point_32) {
+  geometry_msgs::msg::Point p;
+  p.x = 123.0;
+  p.y = 456.0;
+  p.z = 789.0;
+
+  // Convert to Point32 format
+  geometry_msgs::msg::Point32 p32;
+  p32 = nav2_costmap_2d::toPoint32(p);
+
+  // Verify all coordinates are preserved
+  EXPECT_NEAR(p.x, p32.x, 1e-5);
+  EXPECT_NEAR(p.y, p32.y, 1e-5);
+  EXPECT_NEAR(p.z, p32.z, 1e-5);
+}
+
+// Test conversion from vector of Points to Polygon message type
+// Verifies that footprint data can be converted to ROS message format
+TEST(collision_footprint, to_polygon) {
+  geometry_msgs::msg::Point p1;
+  p1.x = 1.2;
+  p1.y = 3.4;
+  p1.z = 5.1;
+
+  geometry_msgs::msg::Point p2;
+  p2.x = -5.6;
+  p2.y = -7.8;
+  p2.z = -9.1;
+  std::vector<geometry_msgs::msg::Point> pts = {p1, p2};
+
+  // Convert to Polygon message format
+  geometry_msgs::msg::Polygon poly;
+  poly = nav2_costmap_2d::toPolygon(pts);
+
+  // Verify structure and coordinate preservation
+  EXPECT_EQ(2u, sizeof(poly.points) / sizeof(poly.points[0]));
+  EXPECT_NEAR(poly.points[0].x, p1.x, 1e-5);
+  EXPECT_NEAR(poly.points[0].y, p1.y, 1e-5);
+  EXPECT_NEAR(poly.points[0].z, p1.z, 1e-5);
+  EXPECT_NEAR(poly.points[1].x, p2.x, 1e-5);
+  EXPECT_NEAR(poly.points[1].y, p2.y, 1e-5);
+  EXPECT_NEAR(poly.points[1].z, p2.z, 1e-5);
+}
+
+// Test successful parsing of footprint from string representation
+// Verifies that string-based footprint configuration works correctly
+TEST(collision_footprint, make_footprint_from_string) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+
+  // Parse a valid footprint string with scientific notation
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint);
+
+  EXPECT_EQ(result, true);
+  EXPECT_EQ(4u, footprint.size());
+
+  // Verify all coordinate values are parsed correctly
+  EXPECT_NEAR(footprint[0].x, 1.0, 1e-5);
+  EXPECT_NEAR(footprint[0].y, 2.2, 1e-5);
+  EXPECT_NEAR(footprint[1].x, 0.3, 1e-5);
+  EXPECT_NEAR(footprint[1].y, -4e4, 1e-5);
+  EXPECT_NEAR(footprint[2].x, -0.3, 1e-5);
+  EXPECT_NEAR(footprint[2].y, -4e4, 1e-5);
+  EXPECT_NEAR(footprint[3].x, -1.0, 1e-5);
+  EXPECT_NEAR(footprint[3].y, 2.2, 1e-5);
+}
+
+// Test error handling for malformed footprint strings
+// Verifies that parsing fails gracefully with invalid input
+TEST(collision_footprint, make_footprint_from_string_parse_error) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+
+  // Try to parse a malformed string (missing closing bracket)
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[bad_string", footprint);
+  EXPECT_EQ(result, false);
+}
+
+// Test error handling for footprints with too few points
+// Verifies that polygons with insufficient vertices are rejected
+TEST(collision_footprint, make_footprint_from_string_two_points_error) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+
+  // Try to parse footprint with only 2 points (need at least 3 for polygon)
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4]", footprint);
+  EXPECT_EQ(result, false);
+}
+
+// Test error handling for points with wrong number of coordinates
+// Verifies that points with extra coordinates are rejected
+TEST(collision_footprint, make_footprint_from_string_not_pairs) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+
+  // Try to parse footprint with a point having 3 coordinates (should be 2)
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint);
+  EXPECT_EQ(result, false);
+}
+
+// Test full area checking functionality when no obstacles are present
+// Verifies that both perimeter-only and full-area checking return the same result
+// when the footprint area is completely free of obstacles
+TEST(collision_footprint, test_full_area_check_no_collision) {
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  // Create a 2x2 meter rectangular footprint
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;  // Bottom-left
+  p1.y = -1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;   // Bottom-right
+  p2.y = -1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;   // Top-right
+  p3.y = 1.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;  // Top-left
+  p4.y = 1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test perimeter only check at a valid position (5.0, 5.0) within the 10x10m costmap
+  auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false);
+  EXPECT_NEAR(perimeter_cost, 0.0, 0.001);
+
+  // Test full area check at the same position
+  auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true);
+  EXPECT_NEAR(full_area_cost, 0.0, 0.001);
+
+  // Both should be the same when no obstacles present
+  EXPECT_NEAR(perimeter_cost, full_area_cost, 0.001);
+}
+
+// Test that full area checking detects obstacles inside the footprint
+// Obstacles that are inside the footprint but not on the perimeter
+// should only be detected by full area checking
+TEST(collision_footprint, test_full_area_check_interior_obstacle) {
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  // Create a 2x2 meter rectangular footprint that will be centered at (5.0, 5.0)
+  // This maps to grid coordinates around (50, 50) with the 0.1m resolution
+  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};
+
+  // Place an obstacle in the interior of the footprint (not on perimeter)
+  // Grid cell (50, 50) corresponds to world coordinates (5.0, 5.0) - center of footprint
+  costmap_->setCost(50, 50, 200);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test perimeter only check - should NOT detect interior obstacle
+  auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false);
+  EXPECT_NEAR(perimeter_cost, 0.0, 0.001);
+
+  // Test full area check - SHOULD detect interior obstacle
+  auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true);
+  EXPECT_NEAR(full_area_cost, 200.0, 0.001);
+}
+
+// Test full area checking with lethal obstacles for early termination
+// Verifies that when a lethal obstacle is found during full area checking,
+// the algorithm returns immediately with lethal cost (performance optimization)
+TEST(collision_footprint, test_full_area_check_lethal_obstacle) {
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  // Create a triangular footprint (simpler shape for testing)
+  geometry_msgs::msg::Point p1;
+  p1.x = 0.0;   // Top vertex
+  p1.y = 1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = -1.0;  // Bottom-left vertex
+  p2.y = -1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;   // Bottom-right vertex
+  p3.y = -1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3};
+
+  // Place a lethal obstacle in the interior of the triangle
+  // Grid cell (50, 49) should be inside the triangle when centered at (5.0, 5.0)
+  costmap_->setCost(50, 49, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test full area check with lethal obstacle - should return lethal cost immediately
+  auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true);
+  EXPECT_NEAR(full_area_cost, static_cast<double>(nav2_costmap_2d::LETHAL_OBSTACLE), 0.001);
+}
+
+// Test full area checking with rotated footprints
+// Verifies that the full area checking works correctly when the footprint
+// is rotated, ensuring the OpenCV polygon rasterization handles rotation properly
+TEST(collision_footprint, test_full_area_check_rotated_footprint) {
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  // Create a rectangular footprint that's taller than it is wide
+  // This will test rotation effects more clearly
+  geometry_msgs::msg::Point p1;
+  p1.x = -0.5;  // Left edge
+  p1.y = -1.5;  // Bottom edge (tall rectangle)
+  geometry_msgs::msg::Point p2;
+  p2.x = 0.5;   // Right edge
+  p2.y = -1.5;
+  geometry_msgs::msg::Point p3;
+  p3.x = 0.5;
+  p3.y = 1.5;   // Top edge
+  geometry_msgs::msg::Point p4;
+  p4.x = -0.5;
+  p4.y = 1.5;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  // Place obstacle where it would be inside the footprint when rotated 90 degrees
+  // Grid cell (52, 50) should be inside the rotated footprint
+  costmap_->setCost(52, 50, 150);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test with 90-degree rotation (M_PI/2 radians)
+  // The tall rectangle becomes a wide rectangle, covering the obstacle
+  auto rotated_cost = collision_checker.footprintCostAtPose(5.0, 5.0, M_PI / 2, footprint, true);
+  EXPECT_GT(rotated_cost, 0.0);  // Should detect the obstacle
+}
+
+// Test the lineCost function directly
+// Verifies that line cost calculation works correctly for line segments,
+// including early termination when lethal obstacles are encountered
+TEST(collision_footprint, test_line_cost_function) {
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test line cost with no obstacles - should return 0
+  auto line_cost = collision_checker.lineCost(10, 20, 10, 20);
+  EXPECT_NEAR(line_cost, 0.0, 0.001);
+
+  // Add obstacle in the line path from (10,10) to (20,20)
+  costmap_->setCost(15, 15, 100);
+  line_cost = collision_checker.lineCost(10, 20, 10, 20);
+  EXPECT_NEAR(line_cost, 100.0, 0.001);
+
+  // Test with lethal obstacle - should return immediately without checking rest of line
+  costmap_->setCost(12, 12, nav2_costmap_2d::LETHAL_OBSTACLE);
+  line_cost = collision_checker.lineCost(10, 20, 10, 20);
+  EXPECT_NEAR(line_cost, static_cast<double>(nav2_costmap_2d::LETHAL_OBSTACLE), 0.001);
+}
+
+// Test handling of footprints that extend outside the costmap boundaries
+// Verifies that out-of-bounds footprint coordinates are handled correctly
+// and return lethal cost (indicating collision/invalid placement)
+TEST(collision_footprint, test_out_of_bounds_footprint) {
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  // Create a large footprint that will extend beyond map boundaries
+  geometry_msgs::msg::Point p1;
+  p1.x = -2.0;  // Will extend to negative world coordinates
+  p1.y = -2.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 2.0;
+  p2.y = -2.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 2.0;
+  p3.y = 2.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -2.0;
+  p4.y = 2.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Place footprint so part of it extends outside the map (negative world coordinates)
+  // With costmap starting at origin (0,0), placing at (-1.0, -1.0) will put parts out of bounds
+  auto out_of_bounds_cost = collision_checker.footprintCostAtPose(-1.0, -1.0, 0.0, footprint, true);
+  EXPECT_EQ(out_of_bounds_cost, static_cast<double>(nav2_costmap_2d::LETHAL_OBSTACLE));
+}
+
+// Test the setCostmap and getCostmap functionality
+// Verifies that the collision checker can switch between different costmaps
+// and that cost queries are performed on the correct costmap
+TEST(collision_footprint, test_set_costmap_function) {
+  // Create two different costmaps with different sizes
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap1 =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(50, 50, 0.1, 0, 0, 0);
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap2 =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  // Set different costs at the same grid location in both costmaps
+  costmap1->setCost(25, 25, 100);
+  costmap2->setCost(25, 25, 200);
+
+  // Initialize collision checker with first costmap
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap1);
+
+  // Test with first costmap - should return cost 100
+  auto cost1 = collision_checker.pointCost(25, 25);
+  EXPECT_NEAR(cost1, 100.0, 0.001);
+
+  // Switch to second costmap using setCostmap
+  collision_checker.setCostmap(costmap2);
+  auto cost2 = collision_checker.pointCost(25, 25);
+  EXPECT_NEAR(cost2, 200.0, 0.001);
+
+  // Verify getCostmap function returns the correct costmap reference
+  auto retrieved_costmap = collision_checker.getCostmap();
+  EXPECT_EQ(retrieved_costmap, costmap2);
+}
+
+// Test full area checking with complex polygons (non-rectangular shapes)
+// Verifies that the OpenCV polygon rasterization works correctly for
+// more complex shapes like pentagons, ensuring the implementation is robust
+TEST(collision_footprint, test_complex_polygon_full_area) {
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  // Create a complex polygon (pentagon) to test non-trivial polygon rasterization
+  geometry_msgs::msg::Point p1;
+  p1.x = 0.0;   // Top vertex
+  p1.y = 1.5;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.4;   // Top-right vertex
+  p2.y = 0.5;
+  geometry_msgs::msg::Point p3;
+  p3.x = 0.9;   // Bottom-right vertex
+  p3.y = -1.2;
+  geometry_msgs::msg::Point p4;
+  p4.x = -0.9;  // Bottom-left vertex
+  p4.y = -1.2;
+  geometry_msgs::msg::Point p5;
+  p5.x = -1.4;  // Top-left vertex
+  p5.y = 0.5;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4, p5};
+
+  // Place obstacle in what should be the interior of the pentagon
+  costmap_->setCost(50, 50, 180);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Test that full area check detects interior obstacle in complex polygon
+  auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false);
+  auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true);
+
+  // Perimeter should not detect interior obstacle
+  EXPECT_LT(perimeter_cost, 180.0);
+  // Full area should detect it (demonstrating the value of full area checking)
+  EXPECT_NEAR(full_area_cost, 180.0, 0.001);
+}
+
+// Performance test for footprint collision checking
+// Measures execution time for both perimeter and full-area checking methods
+// Tests deterministic scenarios: no obstacles, vertex obstacle, edge obstacle, interior obstacle
+TEST(collision_footprint, test_performance_benchmark) {
+  // Create a reasonably large costmap for performance testing
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(500, 500, 0.05, 0, 0, 0);
+
+  // Create a rectangular footprint (2m x 1m)
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;  // Left edge
+  p1.y = -0.5;  // Bottom edge
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;   // Right edge
+  p2.y = -0.5;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;
+  p3.y = 0.5;   // Top edge
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;
+  p4.y = 0.5;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  // Fixed test position (center of costmap)
+  const double test_x = 12.5;  // Center of 25m x 25m map
+  const double test_y = 12.5;
+  const double test_theta = 0.0;
+
+  // Convert footprint vertices to grid coordinates for obstacle placement
+  unsigned int grid_x, grid_y;
+
+  const int num_iterations = 1000;
+
+  // Define test scenarios
+  struct TestScenario
+  {
+    std::string name;
+    std::vector<std::pair<unsigned int, unsigned int>> obstacle_positions;
+  };
+
+  std::vector<TestScenario> scenarios;
+
+  // Scenario 0: Obstacle at a vertex of the polygon (fastest)
+  // Place obstacle at the first vertex (p1: left edge, bottom edge)
+  collision_checker.worldToMap(test_x - 1.0, test_y - 0.5, grid_x, grid_y);
+  scenarios.push_back({"Obstacle at vertex", {{grid_x, grid_y}}});
+
+  // Scenario 1: Obstacle on a line segment (edge)
+  // Place obstacle on the last line segment (p4 to p1: top edge, from right to left)
+  collision_checker.worldToMap(test_x - 0.5, test_y + 0.5, grid_x, grid_y);
+  scenarios.push_back({"Obstacle on edge", {{grid_x, grid_y}}});
+
+  // Scenario 2: Obstacle inside the polygon
+  // Place obstacle at the center of the rectangle
+  collision_checker.worldToMap(test_x, test_y, grid_x, grid_y);
+  scenarios.push_back({"Obstacle inside", {{grid_x, grid_y}}});
+
+  // Scenario 3: No obstacles (slowest)
+  scenarios.push_back({"No obstacles", {}});
+
+  std::cout << "\n=== FOOTPRINT COLLISION CHECKER PERFORMANCE BENCHMARK ===" << std::endl;
+  std::cout << "Test configuration:" << std::endl;
+  std::cout << "  - Costmap size: 500x500 cells (25m x 25m @ 0.05m resolution)" << std::endl;
+  std::cout << "  - Footprint: Rectangle 2m x 1m" << std::endl;
+  std::cout << "  - Test position: (" << test_x << ", " << test_y << ", " << test_theta << ")" <<
+    std::endl;
+  std::cout << "  - Iterations per scenario: " << num_iterations << std::endl;
+  std::cout << "  - Obstacle cost: 254" << std::endl;
+
+  // Run tests for each scenario
+  std::vector<double> full_area_times;  // Store timing for early exit verification
+
+  for (const auto & scenario : scenarios) {
+    // Clear costmap
+    for (unsigned int i = 0; i < costmap_->getSizeInCellsX(); ++i) {
+      for (unsigned int j = 0; j < costmap_->getSizeInCellsY(); ++j) {
+        costmap_->setCost(i, j, 0);
+      }
+    }
+
+    // Place obstacles for this scenario
+    for (const auto & pos : scenario.obstacle_positions) {
+      costmap_->setCost(pos.first, pos.second, 254);
+    }
+
+    // Test perimeter-only checking
+    auto start_perimeter = std::chrono::high_resolution_clock::now();
+    double perimeter_result = 0.0;
+    for (int iter = 0; iter < num_iterations; ++iter) {
+      perimeter_result = collision_checker.footprintCostAtPose(
+        test_x, test_y, test_theta, footprint, false);
+    }
+    auto end_perimeter = std::chrono::high_resolution_clock::now();
+    auto perimeter_duration = std::chrono::duration_cast<std::chrono::microseconds>(
+      end_perimeter - start_perimeter);
+
+    // Test full-area checking
+    auto start_full_area = std::chrono::high_resolution_clock::now();
+    double full_area_result = 0.0;
+    for (int iter = 0; iter < num_iterations; ++iter) {
+      full_area_result = collision_checker.footprintCostAtPose(
+        test_x, test_y, test_theta, footprint, true);
+    }
+    auto end_full_area = std::chrono::high_resolution_clock::now();
+    auto full_area_duration = std::chrono::duration_cast<std::chrono::microseconds>(
+      end_full_area - start_full_area);
+
+    // Calculate averages
+    double avg_perimeter_us = static_cast<double>(perimeter_duration.count()) / num_iterations;
+    double avg_full_area_us = static_cast<double>(full_area_duration.count()) / num_iterations;
+
+    // Store full-area timing for early exit verification
+    full_area_times.push_back(avg_full_area_us);
+
+    // Print results for this scenario
+    std::cout << "\nScenario: " << scenario.name << std::endl;
+    std::cout << "  Perimeter checking:" << std::endl;
+    std::cout << "    - Result: " << perimeter_result << std::endl;
+    std::cout << "    - Average time: " << std::fixed << std::setprecision(2)
+              << avg_perimeter_us << " microseconds" << std::endl;
+    std::cout << "  Full-area checking:" << std::endl;
+    std::cout << "    - Result: " << full_area_result << std::endl;
+    std::cout << "    - Average time: " << std::fixed << std::setprecision(2)
+              << avg_full_area_us << " microseconds" << std::endl;
+    std::cout << "  Performance ratio (full-area / perimeter): "
+              << std::fixed << std::setprecision(2)
+              << (avg_perimeter_us >
+    0 ? avg_full_area_us / avg_perimeter_us : 0.0) << "x" << std::endl;
+
+    // Verify expected behavior for each scenario
+    if (scenario.name == "No obstacles") {
+      EXPECT_NEAR(perimeter_result, 0.0, 0.001);
+      EXPECT_NEAR(full_area_result, 0.0, 0.001);
+    } else if (scenario.name == "Obstacle at vertex" || scenario.name == "Obstacle on edge") {
+      // Both methods should detect obstacles on perimeter
+      EXPECT_NEAR(perimeter_result, 254.0, 0.001);
+      EXPECT_NEAR(full_area_result, 254.0, 0.001);
+    } else if (scenario.name == "Obstacle inside") {
+      // Only full-area should detect interior obstacles
+      EXPECT_LT(perimeter_result, 254.0);  // Should not detect interior obstacle
+      EXPECT_NEAR(full_area_result, 254.0, 0.001);  // Should detect interior obstacle
+    }
+
+    // Performance sanity checks
+    EXPECT_LT(avg_perimeter_us, 1000.0);  // Should be less than 1ms per call
+    EXPECT_LT(avg_full_area_us, 5000.0);  // Should be less than 5ms per call
+  }
+
+  // Verify early exit performance ordering for full-area checking
+  // Expected order (fastest to slowest due to early exit):
+  // 0: "Obstacle at vertex" - exits earliest when scanning perimeter (fastest)
+  // 1: "Obstacle on edge" - exits when scanning perimeter
+  // 2: "Obstacle inside" - exits when scanning interior
+  // 3: "No obstacles" - must scan entire area (slowest)
+  std::cout << "\nEarly exit performance verification (full-area checking):" << std::endl;
+  std::cout << "  Obstacle at vertex: " << std::fixed << std::setprecision(2)
+            << full_area_times[0] << " μs (fastest)" << std::endl;
+  std::cout << "  Obstacle on edge: " << std::fixed << std::setprecision(2)
+            << full_area_times[1] << " μs" << std::endl;
+  std::cout << "  Obstacle inside: " << std::fixed << std::setprecision(2)
+            << full_area_times[2] << " μs" << std::endl;
+  std::cout << "  No obstacles: " << std::fixed << std::setprecision(2)
+            << full_area_times[3] << " μs (slowest)" << std::endl;
+
+  // Verify early exit behavior: obstacles found earlier should result in faster execution
+  // Expected order: vertex < edge < inside < no obstacles
+  // Allow some tolerance for measurement noise, but expect clear ordering
+  double tolerance_factor = 1.2;  // Allow 20% tolerance for timing variations
+
+  // Obstacle at vertex should be fastest (first vertex checked)
+  EXPECT_LT(full_area_times[0] * tolerance_factor, full_area_times[1])  // vertex < edge
+    << "Obstacle at vertex (" << full_area_times[0] <<
+    " μs) should be faster than obstacle on edge ("
+    << full_area_times[1] << " μs) due to early exit";
+
+  EXPECT_LT(full_area_times[1] * tolerance_factor, full_area_times[2])  // edge < inside
+    << "Obstacle on edge (" << full_area_times[1] << " μs) should be faster than obstacle inside ("
+    << full_area_times[2] << " μs) due to early exit";
+
+  EXPECT_LT(full_area_times[2] * tolerance_factor, full_area_times[3])  // inside < no obstacles
+    << "Obstacle inside (" << full_area_times[2] << " μs) should be faster than no obstacles ("
+    << full_area_times[3] << " μs) due to early exit";
+
+  std::cout << "  ✓ Early exit performance ordering verified" << std::endl;
+  std::cout << "============================================================\n" << std::endl;
+}
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
index 1208ade5521..de552804640 100644
--- a/nav2_docking/opennav_docking/src/docking_server.cpp
+++ b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -319,6 +319,7 @@ void DockingServer::dockRobot()
         // Approach the dock using control law
         if (approachDock(dock, dock_pose, dock_backward)) {
           // We are docked, wait for charging to begin
+          publishZeroVelocity();
           RCLCPP_INFO(
             get_logger(), "Made contact with dock, waiting for charge to start (if applicable).");
           if (waitForCharge(dock)) {
diff --git a/nav2_mppi_controller/CMakeLists copy.txt b/nav2_mppi_controller/CMakeLists copy.txt
new file mode 100644
index 00000000000..fe98d0d2af7
--- /dev/null
+++ b/nav2_mppi_controller/CMakeLists copy.txt	
@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 3.8)
+project(nav2_mppi_controller)
+
+find_package(ament_cmake REQUIRED)
+find_package(angles REQUIRED)
+find_package(backward_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(nav2_ros_common REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+nav2_package()
+
+include(CheckCXXCompilerFlag)
+
+check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA)
+
+if(COMPILER_SUPPORTS_FMA)
+  add_compile_options(-mfma)
+endif()
+
+# If building one the same hardware to be deployed on, try `-march=native`!
+
+add_library(mppi_controller SHARED
+  src/controller.cpp
+  src/collision_checker.cpp
+  src/critic_manager.cpp
+  src/noise_generator.cpp
+  src/optimizer.cpp
+  src/parameters_handler.cpp
+  src/path_handler.cpp
+  src/trajectory_visualizer.cpp
+)
+target_compile_options(mppi_controller PUBLIC -O3)
+target_include_directories(mppi_controller
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(mppi_controller PUBLIC
+  angles::angles
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
+  nav2_costmap_2d::layers
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
+  ${nav_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${std_msgs_TARGETS}
+  tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
+  tf2_ros::tf2_ros
+  ${visualization_msgs_TARGETS}
+)
+
+add_library(mppi_critics SHARED
+  src/critics/constraint_critic.cpp
+  src/critics/cost_critic.cpp
+  src/critics/goal_critic.cpp
+  src/critics/goal_angle_critic.cpp
+  src/critics/obstacles_critic.cpp
+  src/critics/path_align_critic.cpp
+  src/critics/path_angle_critic.cpp
+  src/critics/path_follow_critic.cpp
+  src/critics/prefer_forward_critic.cpp
+  src/critics/twirling_critic.cpp
+  src/critics/velocity_deadband_critic.cpp
+  src/critics/waypoint_critic.cpp
+)
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE)
+    # Apple Clang: use C++20 and optimization, omit -fconcepts
+    target_compile_features(mppi_critics PUBLIC cxx_std_20)
+    target_compile_options(mppi_critics PUBLIC -O3)
+else()
+    target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
+endif()
+target_include_directories(mppi_critics
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(mppi_critics PUBLIC
+  angles::angles
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
+  nav2_costmap_2d::layers
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${std_msgs_TARGETS}
+  tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
+  tf2_ros::tf2_ros
+  ${visualization_msgs_TARGETS}
+  mppi_controller
+)
+target_link_libraries(mppi_critics PRIVATE
+  pluginlib::pluginlib
+)
+
+add_library(mppi_trajectory_validators SHARED
+  src/trajectory_validators/optimal_trajectory_validator.cpp
+)
+target_compile_options(mppi_trajectory_validators PUBLIC -O3)
+target_include_directories(mppi_trajectory_validators
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+    "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
+target_link_libraries(mppi_trajectory_validators PUBLIC
+  angles::angles
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::layers
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${std_msgs_TARGETS}
+  tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
+  tf2_ros::tf2_ros
+  ${visualization_msgs_TARGETS}
+)
+target_link_libraries(mppi_trajectory_validators PRIVATE
+  pluginlib::pluginlib
+)
+
+install(TARGETS mppi_controller mppi_critics mppi_trajectory_validators
+  EXPORT nav2_mppi_controller
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_find_gtest()
+
+  add_subdirectory(test)
+  add_subdirectory(benchmark)
+endif()
+
+ament_export_libraries(${libraries})
+ament_export_dependencies(
+  angles
+  geometry_msgs
+  nav2_core
+  nav2_costmap_2d
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  std_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+  visualization_msgs
+  nav2_ros_common
+  Eigen3
+)
+ament_export_include_directories(include/${PROJECT_NAME})
+ament_export_targets(nav2_mppi_controller)
+
+pluginlib_export_plugin_description_file(nav2_core mppic.xml)
+pluginlib_export_plugin_description_file(nav2_mppi_controller critics.xml)
+pluginlib_export_plugin_description_file(nav2_mppi_controller trajectory_validators.xml)
+
+ament_package()
diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
index c28c711dc50..f4abe8dd80a 100644
--- a/nav2_mppi_controller/CMakeLists.txt
+++ b/nav2_mppi_controller/CMakeLists.txt
@@ -22,6 +22,7 @@ find_package(visualization_msgs REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(Eigen3 REQUIRED)
 
+
 include_directories(
   include
   ${EIGEN3_INCLUDE_DIR}
@@ -84,6 +85,7 @@ add_library(mppi_critics SHARED
   src/critics/prefer_forward_critic.cpp
   src/critics/twirling_critic.cpp
   src/critics/velocity_deadband_critic.cpp
+  src/critics/waypoint_critic.cpp
 )
 if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE)
     # Apple Clang: use C++20 and optimization, omit -fconcepts
@@ -118,6 +120,7 @@ target_link_libraries(mppi_critics PRIVATE
 
 add_library(mppi_trajectory_validators SHARED
   src/trajectory_validators/optimal_trajectory_validator.cpp
+  src/trajectory_validators/fixing_trajectory_validator.cpp
 )
 target_compile_options(mppi_trajectory_validators PUBLIC -O3)
 target_include_directories(mppi_trajectory_validators
diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml
index deae2c11cb9..e738b3455a4 100644
--- a/nav2_mppi_controller/critics.xml
+++ b/nav2_mppi_controller/critics.xml
@@ -45,5 +45,11 @@
       <description>mppi critic for restricting command velocities in deadband range</description>
     </class>
 
+
+    <class type="mppi::critics::WaypointCritic" base_class_type="mppi::critics::CriticFunction">
+      <description>mppi critic for following sparse waypoints with position and orientation tolerances</description>
+    </class>
+
+
   </library>
 </class_libraries>
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp
new file mode 100644
index 00000000000..09fe82f0649
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/collision_checker.hpp
@@ -0,0 +1,148 @@
+// Copyright (c) 2025, Angsa Robotics GmbH
+//
+// 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 NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_
+#define NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_
+
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+
+namespace nav2_mppi_controller
+{
+
+/**
+ * @struct CollisionResult
+ * @brief Result of collision checking
+ */
+struct CollisionResult
+{
+  bool in_collision;
+  std::vector<float> center_cost;
+  std::vector<float> footprint_cost;
+};
+
+/**
+ * @class nav2_mppi_controller::MPPICollisionChecker
+ * @brief A costmap grid collision checker
+ */
+class MPPICollisionChecker
+  : public nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
+{
+public:
+  /**
+   * @brief A constructor for nav2_mppi_controller::MPPICollisionChecker
+   * @param costmap The costmap to collision check against
+   * @param node Node to extract clock and logger from
+   */
+  MPPICollisionChecker(
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node);
+
+  /**
+   * @brief A constructor for nav2_mppi_controller::MPPICollisionChecker
+   * for use when irregular bin intervals are appropriate
+   * @param costmap The costmap to collision check against
+   * @param angles The vector of possible angle bins to precompute for
+   * orientations for to speed up collision checking, in radians
+   */
+  // MPPICollisionChecker(
+  //   nav2_costmap_2d::Costmap2D * costmap,
+  //   std::vector<float> & angles);
+
+  /**
+   * @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
+   * @param inflation_layer_name Optional name of inflation layer for cost calculation
+   */
+  void setFootprint(
+    const nav2_costmap_2d::Footprint & footprint,
+    const bool & radius,
+    const std::string & inflation_layer_name = "");
+
+  /**
+   * @brief Check if in collision with costmap and footprint at poses (batch processing)
+   * @param x Vector of X coordinates of poses to check against
+   * @param y Vector of Y coordinates of poses to check against
+   * @param yaw Vector of yaw angles of poses to check against in radians
+   * @param traverse_unknown Whether or not to traverse in unknown space
+   * @return CollisionResult struct with collision status and vectors of center cost, footprint cost, and area cost
+   */
+  CollisionResult inCollision(
+    const std::vector<float> & x,
+    const std::vector<float> & y,
+    const std::vector<float> & yaw,
+    const bool & traverse_unknown);
+
+  /**
+   * @brief Get costmap ros object for inflation layer params
+   * @return Costmap ros
+   */
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getCostmapROS() {return costmap_ros_;}
+
+  /**
+   * @brief Check if value outside the range
+   * @param max Maximum value of the range
+   * @param value the value to check if it is within the range
+   * @return boolean if in range or not
+   */
+  bool outsideRange(const unsigned int & max, const float & value) const;
+
+  /**
+   * @brief Create convex hull from a set of points
+   * @param points Input points to create convex hull from
+   * @return Convex hull as a footprint
+   */
+  nav2_costmap_2d::Footprint createConvexHull(
+    const std::vector<geometry_msgs::msg::Point> & points);
+
+  /**
+   * @brief Find the circumscribed cost for collision checking optimization
+   * @param inflation_layer_name Optional name of inflation layer
+   * @return The circumscribed cost value
+   */
+  float findCircumscribedCost(const std::string & inflation_layer_name = "");
+
+private:
+  /**
+   * @brief Transform footprint to given orientation
+   * @param footprint The base footprint to transform
+   * @param yaw The yaw angle to transform to
+   * @return Transformed footprint
+   */
+  nav2_costmap_2d::Footprint transformFootprint(
+    const nav2_costmap_2d::Footprint & footprint,
+    float yaw) const;
+
+protected:
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav2_costmap_2d::Footprint unoriented_footprint_;
+  bool footprint_is_radius_{false};
+  float possible_collision_cost_{-1};
+  float circumscribed_radius_{-1.0f};
+  float circumscribed_cost_{-1.0f};
+  rclcpp::Logger logger_{rclcpp::get_logger("MPPICollisionChecker")};
+  rclcpp::Clock::SharedPtr clock_;
+};
+
+}  // namespace nav2_mppi_controller
+
+#endif  // NAV2_MPPI_CONTROLLER__COLLISION_CHECKER_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
index 1c52df96f68..40bb7613c40 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
@@ -101,31 +101,18 @@ class MPPIController : public nav2_core::Controller
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
 protected:
-  /**
-    * @brief Visualize trajectories
-    * @param transformed_plan Transformed input plan
-    * @param cmd_stamp Command stamp
-    * @param optimal_trajectory Optimal trajectory, if already computed
-    */
-  void visualize(
-    nav_msgs::msg::Path transformed_plan,
-    const builtin_interfaces::msg::Time & cmd_stamp,
-    const Eigen::ArrayXXf & optimal_trajectory);
+
 
   std::string name_;
   nav2::LifecycleNode::WeakPtr parent_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  nav2::Publisher<nav2_msgs::msg::Trajectory>::SharedPtr opt_traj_pub_;
 
   std::unique_ptr<ParametersHandler> parameters_handler_;
   Optimizer optimizer_;
   PathHandler path_handler_;
   TrajectoryVisualizer trajectory_visualizer_;
-
-  bool visualize_;
-  bool publish_optimal_trajectory_;
 };
 
 }  // namespace nav2_mppi_controller
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data copy 2.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data copy 2.hpp
new file mode 100644
index 00000000000..8476dde055c
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data copy 2.hpp	
@@ -0,0 +1,58 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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_MPPI_CONTROLLER__CRITIC_DATA_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
+
+#include <Eigen/Dense>
+
+#include <memory>
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_core/goal_checker.hpp"
+#include "nav2_mppi_controller/models/state.hpp"
+#include "nav2_mppi_controller/models/trajectories.hpp"
+#include "nav2_mppi_controller/models/path.hpp"
+#include "nav2_mppi_controller/motion_models.hpp"
+
+
+namespace mppi
+{
+
+/**
+ * @struct mppi::CriticData
+ * @brief Data to pass to critics for scoring, including state, trajectories,
+ * pruned path, global goal, costs, and important parameters to share
+ */
+struct CriticData
+{
+  const models::State & state;
+  const models::Trajectories & trajectories;
+  const models::Path & path;
+  const geometry_msgs::msg::Pose & goal;
+
+  Eigen::ArrayXf & costs;
+  float & model_dt;
+
+  bool fail_flag;
+  nav2_core::GoalChecker * goal_checker;
+  std::shared_ptr<MotionModel> motion_model;
+  std::optional<std::vector<bool>> path_pts_valid;
+  std::optional<size_t> furthest_reached_path_point;
+};
+
+}  // namespace mppi
+
+#endif  // NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp
index 8476dde055c..db849ec9536 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp
@@ -18,6 +18,8 @@
 #include <Eigen/Dense>
 
 #include <memory>
+#include <string>
+#include <utility>
 #include <vector>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -44,9 +46,11 @@ struct CriticData
   const geometry_msgs::msg::Pose & goal;
 
   Eigen::ArrayXf & costs;
+  std::optional<std::vector<std::pair<std::string, Eigen::ArrayXf>>> individual_critics_cost;
   float & model_dt;
 
   bool fail_flag;
+  std::optional<std::vector<bool>> trajectories_in_collision;
   nav2_core::GoalChecker * goal_checker;
   std::shared_ptr<MotionModel> motion_model;
   std::optional<std::vector<bool>> path_pts_valid;
@@ -55,4 +59,4 @@ struct CriticData
 
 }  // namespace mppi
 
-#endif  // NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
+#endif  // NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp
index 9b7d17aaea5..dda4a14f96e 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp
@@ -70,7 +70,9 @@ class CriticFunction
     ParametersHandler * param_handler)
   {
     parent_ = parent;
-    logger_ = parent_.lock()->get_logger();
+    auto node = parent_.lock();
+    logger_ = node->get_logger();
+    clock_ = node->get_clock();
     name_ = name;
     parent_name_ = parent_name;
     costmap_ros_ = costmap_ros;
@@ -111,6 +113,7 @@ class CriticFunction
 
   ParametersHandler * parameters_handler_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
index 237649bedde..447757d73db 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
@@ -99,6 +99,7 @@ class CriticManager
 
   nav2::Publisher<nav2_msgs::msg::CriticsStats>::SharedPtr critics_effect_pub_;
   bool publish_critics_stats_;
+  bool visualize_per_critic_costs_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic copy.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic copy.hpp
new file mode 100644
index 00000000000..969cd9b94d8
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic copy.hpp	
@@ -0,0 +1,109 @@
+// Copyright (c) 2023 Robocc Brice Renaudeau
+//
+// 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_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_costmap_2d/inflation_layer.hpp"
+
+#include "nav2_mppi_controller/critic_function.hpp"
+#include "nav2_mppi_controller/models/state.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_mppi_controller/collision_checker.hpp"
+
+namespace mppi::critics
+{
+
+/**
+ * @class mppi::critics::CostCritic
+ * @brief Critic objective function for avoiding obstacles using costmap's inflated cost
+ */
+class CostCritic : public CriticFunction
+{
+public:
+  /**
+    * @brief Initialize critic
+    */
+  void initialize() override;
+
+  /**
+   * @brief Evaluate cost related to obstacle avoidance
+   *
+   * @param costs [out] add obstacle cost values to this tensor
+   */
+  void score(CriticData & data) override;
+
+protected:
+  /**
+    * @brief An implementation of worldToMap fully using floats
+    * @param wx Float world X coord
+    * @param wy Float world Y coord
+    * @param mx unsigned int map X coord
+    * @param my unsigned into map Y coord
+    * @return if successful
+    */
+  inline bool worldToMapFloat(float wx, float wy, unsigned int & mx, unsigned int & my) const
+  {
+    if (wx < origin_x_ || wy < origin_y_) {
+      return false;
+    }
+
+    mx = static_cast<unsigned int>((wx - origin_x_) / resolution_);
+    my = static_cast<unsigned int>((wy - origin_y_) / resolution_);
+
+    if (mx < size_x_ && my < size_y_) {
+      return true;
+    }
+    return false;
+  }
+
+  /**
+    * @brief A local implementation of getIndex
+    * @param mx unsigned int map X coord
+    * @param my unsigned into map Y coord
+    * @return Index
+    */
+  inline unsigned int getIndex(unsigned int mx, unsigned int my) const
+  {
+    return my * size_x_ + mx;
+  }
+
+  std::unique_ptr<nav2_mppi_controller::MPPICollisionChecker> collision_checker_;
+  float possible_collision_cost_;
+
+  bool consider_footprint_{true};
+  bool is_tracking_unknown_{true};
+  float collision_cost_{0.0f};
+  float critical_cost_{0.0f};
+  unsigned int near_collision_cost_{253};
+  float weight_{0};
+  unsigned int trajectory_point_step_;
+
+  float origin_x_, origin_y_, resolution_;
+  unsigned int size_x_, size_y_;
+
+  float near_goal_distance_;
+  std::string inflation_layer_name_;
+
+
+
+  unsigned int power_{0};
+};
+
+}  // namespace mppi::critics
+
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
index c285a14b664..3ebaf54400e 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -48,38 +48,6 @@ class CostCritic : public CriticFunction
   void score(CriticData & data) override;
 
 protected:
-  /**
-    * @brief Checks if cost represents a collision
-    * @param cost Point cost at pose center
-    * @param x X of pose
-    * @param y Y of pose
-    * @param theta theta of pose
-    * @return bool if in collision
-    */
-  inline bool inCollision(float cost, float x, float y, float theta)
-  {
-    // If consider_footprint_ check footprint scort for collision
-    float score_cost = cost;
-    if (consider_footprint_ &&
-      (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
-    {
-      score_cost = static_cast<float>(collision_checker_.footprintCostAtPose(
-          static_cast<double>(x), static_cast<double>(y), static_cast<double>(theta),
-          costmap_ros_->getRobotFootprint()));
-    }
-
-    switch (static_cast<unsigned char>(score_cost)) {
-      case (nav2_costmap_2d::LETHAL_OBSTACLE):
-        return true;
-      case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
-        return consider_footprint_ ? false : true;
-      case (nav2_costmap_2d::NO_INFORMATION):
-        return is_tracking_unknown_ ? false : true;
-    }
-
-    return false;
-  }
-
   /**
     * @brief Find the min cost of the inflation decay function for which the robot MAY be
     * in collision in any orientation
@@ -148,4 +116,4 @@ class CostCritic : public CriticFunction
 
 }  // namespace mppi::critics
 
-#endif  // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic copy 3.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic copy 3.hpp
new file mode 100644
index 00000000000..b1c52f361d2
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic copy 3.hpp	
@@ -0,0 +1,108 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_
+
+#include <memory>
+#include <string>
+
+//#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_mppi_controller/critic_function.hpp"
+#include "nav2_mppi_controller/models/state.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_mppi_controller/collision_checker.hpp"
+
+namespace mppi::critics
+{
+
+/**
+ * @class mppi::critics::ConstraintCritic
+ * @brief Critic objective function for avoiding obstacles, allowing it to deviate off
+ * the planned path. This is important to tune in tandem with PathAlign to make a balance
+ * between path-tracking and dynamic obstacle avoidance capabilities as desirable for a
+ * particular application
+ */
+class ObstaclesCritic : public CriticFunction
+{
+public:
+  /**
+    * @brief Initialize critic
+    */
+  void initialize() override;
+
+  /**
+   * @brief Evaluate cost related to obstacle avoidance
+   *
+   * @param costs [out] add obstacle cost values to this tensor
+   */
+  void score(CriticData & data) override;
+
+protected:
+  /**
+    * @brief Checks if cost represents a collision
+    * @param cost Costmap cost
+    * @return bool if in collision
+    */
+  inline bool inCollision(float cost) const;
+
+  /**
+    * @brief cost at a robot pose
+    * @param x X of pose
+    * @param y Y of pose
+    * @param theta theta of pose
+    * @return Collision information at pose
+    */
+  //inline CollisionCost costAtPose(float x, float y, float theta);
+
+  /**
+    * @brief Distance to obstacle from cost
+    * @param cost Costmap cost
+    * @return float Distance to the obstacle represented by cost
+    */
+  inline float distanceToObstacle(const CollisionCost & cost);
+
+  /**
+    * @brief Find the min cost of the inflation decay function for which the robot MAY be
+    * in collision in any orientation
+    * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation)
+    * @return double circumscribed cost, any higher than this and need to do full footprint collision checking
+    * since some element of the robot could be in collision
+    */
+  float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
+
+protected:
+  // nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
+  // collision_checker_{nullptr};
+  std::unique_ptr<nav2_mppi_controller::MPPICollisionChecker> collision_checker_;
+  int trajectory_point_step_{1};
+
+  bool consider_footprint_{true};
+  float collision_cost_{0};
+  float inflation_scale_factor_{0}, inflation_radius_{0};
+
+  float possible_collision_cost_;
+  float collision_margin_distance_;
+  float near_goal_distance_;
+  float circumscribed_cost_{0}, circumscribed_radius_{0};
+
+  unsigned int power_{0};
+  float repulsion_weight_, critical_weight_{0};
+  std::string inflation_layer_name_;
+};
+
+}  // namespace mppi::critics
+
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
index f7ad2fda6a9..ac29e1035a8 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
@@ -18,6 +18,9 @@
 #include "nav2_mppi_controller/critic_function.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_ros_common/publisher.hpp"
 
 namespace mppi::critics
 {
@@ -52,6 +55,9 @@ class PathAlignCritic : public CriticFunction
   bool use_path_orientations_{false};
   unsigned int power_{0};
   float weight_{0};
+  
+  bool visualize_furthest_point_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr furthest_point_pub_;
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp
index b137270aba9..a265c21e864 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp
@@ -20,6 +20,9 @@
 #include "nav2_mppi_controller/models/state.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
 #include "nav2_core/controller_exceptions.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_ros_common/publisher.hpp"
 
 namespace mppi::critics
 {
@@ -81,6 +84,8 @@ class PathAngleCritic : public CriticFunction
 
   unsigned int power_{0};
   float weight_{0};
+  bool visualize_furthest_point_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr furthest_point_pub_;
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp
index 1ccd08c32fe..c1d3e157104 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp
@@ -19,6 +19,10 @@
 #include "nav2_mppi_controller/critic_function.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_ros_common/publisher.hpp"
+
 
 namespace mppi::critics
 {
@@ -53,6 +57,8 @@ class PathFollowCritic : public CriticFunction
 
   unsigned int power_{0};
   float weight_{0};
+  bool visualize_furthest_point_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr furthest_point_pub_;
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/waypoint_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/waypoint_critic.hpp
new file mode 100644
index 00000000000..46fe0e2ed2a
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/waypoint_critic.hpp
@@ -0,0 +1,112 @@
+// Copyright (c) 2025
+//
+// 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_MPPI_CONTROLLER__CRITICS__WAYPOINT_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__WAYPOINT_CRITIC_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <mutex>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/utils.h"
+
+#include "nav2_mppi_controller/critic_function.hpp"
+#include "nav2_mppi_controller/models/state.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+
+namespace mppi::critics
+{
+
+/**
+ * @class mppi::critics::WaypointCritic
+ * @brief Critic objective function for following sparse waypoints
+ * 
+ * This critic subscribes to a nav_msgs/Path topic containing sparse waypoints
+ * and penalizes trajectories that don't pass through these waypoints with
+ * configurable position and orientation tolerances.
+ */
+class WaypointCritic : public CriticFunction
+{
+public:
+  /**
+    * @brief Initialize critic
+    */
+  void initialize() override;
+
+  /**
+   * @brief Evaluate cost related to waypoint following
+   *
+   * @param costs [out] add waypoint cost values to this tensor
+   */
+  void score(CriticData & data) override;
+
+protected:
+  /**
+   * @brief Callback for waypoint path subscription
+   * @param msg Received path message containing waypoints
+   */
+  void waypointCallback(const nav_msgs::msg::Path::SharedPtr msg);
+
+  /**
+   * @brief Get the current target waypoint (always the first one in the list)
+   * @return Index of target waypoint (0 for first), or -1 if no waypoints available
+   */
+  int getTargetWaypoint();
+
+  /**
+   * @brief Calculate distance between two points
+   * @param x1 X coordinate of first point
+   * @param y1 Y coordinate of first point
+   * @param x2 X coordinate of second point
+   * @param y2 Y coordinate of second point
+   * @return Euclidean distance
+   */
+  double calculateDistance(double x1, double y1, double x2, double y2);
+
+  /**
+   * @brief Calculate angular difference between two angles
+   * @param angle1 First angle in radians
+   * @param angle2 Second angle in radians
+   * @return Angular difference in radians (-pi to pi)
+   */
+  double calculateAngularDifference(double angle1, double angle2);
+
+  // ROS subscription
+  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr waypoint_sub_;
+  
+  // Waypoint data
+  std::vector<geometry_msgs::msg::PoseStamped> waypoints_;
+  std::mutex waypoints_mutex_;
+  
+  // Parameters
+  unsigned int power_{1};
+  float weight_{1.0f};
+  float position_tolerance_{0.5f};  // meters
+  float yaw_tolerance_{0.5f};       // radians
+  std::string waypoint_topic_{"truncated_path"};
+  bool use_orientation_{true};      // whether to consider orientation
+  float max_waypoint_distance_{10.0f}; // max distance to consider a waypoint
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::string waypoints_frame_;
+};
+
+}  // namespace mppi::critics
+
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__WAYPOINT_CRITIC_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/fixing_trajectory_validator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/fixing_trajectory_validator.hpp
new file mode 100644
index 00000000000..93cb60f53f9
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/fixing_trajectory_validator.hpp
@@ -0,0 +1,71 @@
+// include/nav2_mppi_controller/validators/auto_fixing_trajectory_validator.hpp
+#ifndef NAV2_MPPI_CONTROLLER__AUTO_FIXING_TRAJECTORY_VALIDATOR_HPP_
+#define NAV2_MPPI_CONTROLLER__AUTO_FIXING_TRAJECTORY_VALIDATOR_HPP_
+
+#include "nav2_mppi_controller/optimal_trajectory_validator.hpp"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include <optional>
+
+namespace mppi {
+
+class AutoFixingTrajectoryValidator : public OptimalTrajectoryValidator
+{
+public:
+  using Ptr = std::shared_ptr<AutoFixingTrajectoryValidator>;
+  AutoFixingTrajectoryValidator() = default;
+  ~AutoFixingTrajectoryValidator() override = default;
+
+  void initialize(
+    const nav2::LifecycleNode::WeakPtr & parent,
+    const std::string & name,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
+    ParametersHandler * param_handler,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    const models::OptimizerSettings & settings) override;
+
+  ValidationResult validateTrajectory(
+    const Eigen::ArrayXXf & optimal_trajectory,
+    const models::ControlSequence & control_sequence,
+    const geometry_msgs::msg::PoseStamped & robot_pose,
+    const geometry_msgs::msg::Twist & robot_speed,
+    const nav_msgs::msg::Path & plan,
+    const geometry_msgs::msg::Pose & goal) override;
+
+  // Po SUCCESS można pobrać naprawioną trajektorię (jeśli była korygowana)
+  bool hasFixedTrajectory() const { return has_fixed_; }
+  const Eigen::ArrayXXf & getFixedTrajectory() const { return fixed_traj_; }
+
+private:
+  // --- narzędzia kolizyjne / naprawcze ---
+  bool isCollisionAt(double x, double y, double theta) const;
+  bool isSafe(double x, double y, double theta) const;
+  double clearanceAt(double x, double y) const;
+  std::optional<Eigen::Vector2d> gradientAt(double x, double y) const;
+
+  bool tryRepair(Eigen::ArrayXXf & traj, int start_idx, int end_idx) const;
+  void laplacianSmoothXY(Eigen::ArrayXXf & traj, const std::vector<bool> &locked, int iters) const;
+  void smoothYaw(Eigen::ArrayXXf & traj) const;
+
+  // --- stan ---
+  nav2_costmap_2d::Costmap2D * costmap_{nullptr};
+  std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D*>> fp_checker_;
+
+  // --- parametry ---
+  bool consider_footprint_{true};
+  double safe_clearance_{0.12};
+  double step_gain_{0.6};
+  double max_step_{0.05};
+  double max_total_disp_{0.25};
+  int    max_iters_{6};
+  int    smooth_iters_{2};
+  float model_dt_{0.f};
+  float auto_fix_ttc_window_{0.8f}; 
+
+  // --- wynik naprawy ---
+  mutable bool has_fixed_{false};
+  mutable Eigen::ArrayXXf fixed_traj_;
+};
+
+}  // namespace mppi
+
+#endif  // NAV2_MPPI_CONTROLLER__AUTO_FIXING_TRAJECTORY_VALIDATOR_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer copy 2.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer copy 2.hpp
new file mode 100644
index 00000000000..1914abde647
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer copy 2.hpp	
@@ -0,0 +1,295 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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_MPPI_CONTROLLER__OPTIMIZER_HPP_
+#define NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_
+
+#include <Eigen/Dense>
+
+#include <string>
+#include <memory>
+#include <tuple>
+
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_core/goal_checker.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "pluginlib/class_loader.hpp"
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_mppi_controller/models/optimizer_settings.hpp"
+#include "nav2_mppi_controller/motion_models.hpp"
+#include "nav2_mppi_controller/critic_manager.hpp"
+#include "nav2_mppi_controller/models/state.hpp"
+#include "nav2_mppi_controller/models/trajectories.hpp"
+#include "nav2_mppi_controller/models/path.hpp"
+#include "nav2_mppi_controller/tools/noise_generator.hpp"
+#include "nav2_mppi_controller/tools/parameters_handler.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_mppi_controller/optimal_trajectory_validator.hpp"
+
+namespace mppi
+{
+
+/**
+ * @class mppi::Optimizer
+ * @brief Main algorithm optimizer of the MPPI Controller
+ */
+class Optimizer
+{
+public:
+  /**
+    * @brief Constructor for mppi::Optimizer
+    */
+  Optimizer() = default;
+
+  /**
+   * @brief Destructor for mppi::Optimizer
+   */
+  ~Optimizer() {shutdown();}
+
+
+  /**
+   * @brief Initializes optimizer on startup
+   * @param parent WeakPtr to node
+   * @param name Name of plugin
+   * @param costmap_ros Costmap2DROS object of environment
+   * @param dynamic_parameter_handler Parameter handler object
+   * @param tf_buffer TF buffer for transformations
+   */
+  void initialize(
+    nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    ParametersHandler * dynamic_parameters_handler);
+
+  /**
+   * @brief Shutdown for optimizer at process end
+   */
+  void shutdown();
+
+  /**
+   * @brief Compute control using MPPI algorithm
+   * @param robot_pose Pose of the robot at given time
+   * @param robot_speed Speed of the robot at given time
+   * @param plan Path plan to track
+   * @param goal Given Goal pose to reach.
+   * @param goal_checker Object to check if goal is completed
+   * @return Tuple of [TwistStamped command, optimal trajectory]
+   */
+  std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> evalControl(
+    const geometry_msgs::msg::PoseStamped & robot_pose,
+    const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan,
+    const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
+
+  /**
+   * @brief Get the trajectories generated in a cycle for visualization
+   * @return Set of trajectories evaluated in cycle
+   */
+  models::Trajectories & getGeneratedTrajectories();
+
+  /**
+   * @brief Get the optimal trajectory for a cycle for visualization
+   * @return Optimal trajectory
+   */
+  Eigen::ArrayXXf getOptimizedTrajectory();
+
+  /**
+   * @brief Get the optimal control sequence for a cycle for visualization
+   * @return Optimal control sequence
+   */
+  const models::ControlSequence & getOptimalControlSequence();
+
+  /**
+   * @brief Set the maximum speed based on the speed limits callback
+   * @param speed_limit Limit of the speed for use
+   * @param percentage Whether the speed limit is absolute or relative
+   */
+  void setSpeedLimit(double speed_limit, bool percentage);
+
+  /**
+   * @brief Reset the optimization problem to initial conditions
+   * @param Whether to reset the constraints to its base values
+   */
+  void reset(bool reset_dynamic_speed_limits = true);
+
+  /**
+   * @brief Get the motion model time step
+   * @return Time step of the model
+   */
+  const models::OptimizerSettings & getSettings() const
+  {
+    return settings_;
+  }
+
+protected:
+  /**
+   * @brief Main function to generate, score, and return trajectories
+   */
+  void optimize();
+
+  /**
+   * @brief Prepare state information on new request for trajectory rollouts
+   * @param robot_pose Pose of the robot at given time
+   * @param robot_speed Speed of the robot at given time
+   * @param plan Path plan to track
+   * @param goal_checker Object to check if goal is completed
+   */
+  void prepare(
+    const geometry_msgs::msg::PoseStamped & robot_pose,
+    const geometry_msgs::msg::Twist & robot_speed,
+    const nav_msgs::msg::Path & plan,
+    const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
+
+  /**
+   * @brief Obtain the main controller's parameters
+   */
+  void getParams();
+
+  /**
+   * @brief Set the motion model of the vehicle platform
+   * @param model Model string to use
+   */
+  void setMotionModel(const std::string & model);
+
+  /**
+   * @brief Shift the optimal control sequence after processing for
+   * next iterations initial conditions after execution
+   */
+  void shiftControlSequence();
+
+  /**
+   * @brief updates generated trajectories with noised trajectories
+   * from the last cycle's optimal control
+   */
+  void generateNoisedTrajectories();
+
+  /**
+   * @brief Apply hard vehicle constraints on control sequence
+   */
+  void applyControlSequenceConstraints();
+
+  /**
+   * @brief  Update velocities in state
+   * @param state fill state with velocities on each step
+   */
+  void updateStateVelocities(models::State & state) const;
+
+  /**
+   * @brief  Update initial velocity in state
+   * @param state fill state
+   */
+  void updateInitialStateVelocities(models::State & state) const;
+
+  /**
+   * @brief predict velocities in state using model
+   * for time horizon equal to timesteps
+   * @param state fill state
+   */
+  void propagateStateVelocitiesFromInitials(models::State & state) const;
+
+  /**
+   * @brief Rollout velocities in state to poses
+   * @param trajectories to rollout
+   * @param state fill state
+   */
+  void integrateStateVelocities(
+    models::Trajectories & trajectories,
+    const models::State & state) const;
+
+  /**
+   * @brief Rollout velocities in state to poses
+   * @param trajectories to rollout
+   * @param state fill state
+   */
+  void integrateStateVelocities(
+    Eigen::Array<float, Eigen::Dynamic, 3> & trajectories,
+    const Eigen::ArrayXXf & state) const;
+
+  /**
+   * @brief Update control sequence with state controls weighted by costs
+   * using softmax function
+   */
+  void updateControlSequence();
+
+  /**
+   * @brief Convert control sequence to a twist command
+   * @param stamp Timestamp to use
+   * @return TwistStamped of command to send to robot base
+   */
+  geometry_msgs::msg::TwistStamped
+  getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time & stamp);
+
+  /**
+   * @brief Whether the motion model is holonomic
+   * @return Bool if holonomic to populate `y` axis of state
+   */
+  bool isHolonomic() const;
+
+  /**
+   * @brief Using control frequencies and time step size, determine if trajectory
+   * offset should be used to populate initial state of the next cycle
+   */
+  void setOffset(double controller_frequency);
+
+  /**
+   * @brief Perform fallback behavior to try to recover from a set of trajectories in collision
+   * @param fail Whether the system failed to recover from
+   */
+  bool fallback(bool fail);
+
+protected:
+  nav2::LifecycleNode::WeakPtr parent_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav2_costmap_2d::Costmap2D * costmap_;
+  std::string name_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+
+  std::shared_ptr<MotionModel> motion_model_;
+
+  ParametersHandler * parameters_handler_;
+  CriticManager critic_manager_;
+  NoiseGenerator noise_generator_;
+
+  std::unique_ptr<pluginlib::ClassLoader<OptimalTrajectoryValidator>> validator_loader_;
+  OptimalTrajectoryValidator::Ptr trajectory_validator_;
+
+  models::OptimizerSettings settings_;
+
+  models::State state_;
+  models::ControlSequence control_sequence_;
+  std::array<mppi::models::Control, 4> control_history_;
+  models::Trajectories generated_trajectories_;
+  models::Path path_;
+  geometry_msgs::msg::Pose goal_;
+  Eigen::ArrayXf costs_;
+
+  CriticData critics_data_ = {
+    state_, generated_trajectories_, path_, goal_,
+    costs_, settings_.model_dt, false, nullptr, nullptr,
+    std::nullopt, std::nullopt};  /// Caution, keep references
+
+  rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+};
+
+}  // namespace mppi
+
+#endif  // NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
index 1914abde647..4da02024bfa 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
@@ -20,6 +20,8 @@
 #include <string>
 #include <memory>
 #include <tuple>
+#include <utility>
+#include <vector>
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -118,6 +120,37 @@ class Optimizer
    */
   const models::ControlSequence & getOptimalControlSequence();
 
+  /**
+   * @brief Get the costs for trajectories for visualization
+   * @return Costs array
+   */
+  const Eigen::ArrayXf & getCosts() const
+  {
+    return costs_;
+  }
+
+  /**
+   * @brief Get the per-critic costs for trajectories for visualization
+   * @return Vector of (critic_name, costs) pairs
+   */
+  const std::vector<std::pair<std::string, Eigen::ArrayXf>> & getCriticCosts() const
+  {
+    if (critics_data_.individual_critics_cost) {
+      return *critics_data_.individual_critics_cost;
+    }
+    static const std::vector<std::pair<std::string, Eigen::ArrayXf>> empty;
+    return empty;
+  }
+
+  /**
+   * @brief Get the collision status for trajectories for visualization
+   * @return Optional vector of collision flags for each trajectory
+   */
+  const std::optional<std::vector<bool>> & getTrajectoriesInCollision() const
+  {
+    return critics_data_.trajectories_in_collision;
+  }
+
   /**
    * @brief Set the maximum speed based on the speed limits callback
    * @param speed_limit Limit of the speed for use
@@ -284,7 +317,7 @@ class Optimizer
 
   CriticData critics_data_ = {
     state_, generated_trajectories_, path_, goal_,
-    costs_, settings_.model_dt, false, nullptr, nullptr,
+    costs_, std::nullopt, settings_.model_dt, false, std::nullopt, nullptr, nullptr,
     std::nullopt, std::nullopt};  /// Caution, keep references
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
@@ -292,4 +325,4 @@ class Optimizer
 
 }  // namespace mppi
 
-#endif  // NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_
+#endif  // NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler copy.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler copy.hpp
new file mode 100644
index 00000000000..bb5320ebe62
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler copy.hpp	
@@ -0,0 +1,159 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+
+#include <vector>
+#include <utility>
+#include <string>
+#include <memory>
+
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
+#include "nav2_mppi_controller/tools/parameters_handler.hpp"
+
+namespace mppi
+{
+
+using PathIterator = std::vector<geometry_msgs::msg::PoseStamped>::iterator;
+using PathRange = std::pair<PathIterator, PathIterator>;
+
+/**
+ * @class mppi::PathHandler
+ * @brief Manager of incoming reference paths for transformation and processing
+ */
+
+class PathHandler
+{
+public:
+  /**
+    * @brief Constructor for mppi::PathHandler
+    */
+  PathHandler() = default;
+
+  /**
+    * @brief Destructor for mppi::PathHandler
+    */
+  ~PathHandler() = default;
+
+  /**
+    * @brief Initialize path handler on bringup
+    * @param parent WeakPtr to node
+    * @param name Name of plugin
+    * @param costmap_ros Costmap2DROS object of environment
+    * @param tf TF buffer for transformations
+    * @param dynamic_parameter_handler Parameter handler object
+    */
+  void initialize(
+    nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS>,
+    std::shared_ptr<tf2_ros::Buffer>, ParametersHandler *);
+
+  /**
+    * @brief Set new reference path
+    * @param Plan Path to use
+    */
+  void setPath(const nav_msgs::msg::Path & plan);
+
+  /**
+    * @brief Get reference path
+    * @return Path
+    */
+  nav_msgs::msg::Path & getPath();
+
+  /**
+   * @brief transform global plan to local applying constraints,
+   * then prune global plan
+   * @param robot_pose Pose of robot
+   * @return global plan in local frame
+   */
+  nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  /**
+   * @brief Get the global goal pose transformed to the local frame
+   * @param stamp Time to get the goal pose at
+   * @return Transformed goal pose
+   */
+  geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
+
+protected:
+  /**
+    * @brief Get largest dimension of costmap (radially)
+    * @return Max distance from center of costmap to edge
+    */
+  double getMaxCostmapDist();
+
+  /**
+    * @brief Transform a pose to the global reference frame
+    * @param pose Current pose
+    * @return output poose in global reference frame
+    */
+  geometry_msgs::msg::PoseStamped
+  transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+    * @brief Get global plan within window of the local costmap size
+    * @param global_pose Robot pose
+    * @return plan transformed in the costmap frame and iterator to the first pose of the global
+    * plan (for pruning)
+    */
+  std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(
+    const geometry_msgs::msg::PoseStamped & global_pose);
+
+  /**
+    * @brief Prune a path to only interesting portions
+    * @param plan Plan to prune
+    * @param end Final path iterator
+    */
+  void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
+
+  /**
+    * @brief Check if the robot pose is within the set inversion or rotation tolerances
+    * @param robot_pose Robot's current pose to check
+    * @return bool If the robot pose is within the tolerances for the next path constraint
+    */
+  bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  std::string name_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  ParametersHandler * parameters_handler_;
+
+  nav_msgs::msg::Path global_plan_;
+  nav_msgs::msg::Path global_plan_up_to_inversion_;
+  rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+
+  double max_robot_pose_search_dist_{0};
+  double prune_distance_{0};
+  double transform_tolerance_{0};
+  float inversion_xy_tolerance_{0.2};
+  float inversion_yaw_tolerance_{0.4};
+  float minimum_rotation_angle_{0.785};
+  bool enforce_path_inversion_{false};
+  bool enforce_path_rotation_{false};
+  unsigned int inversion_locale_{0u};
+};
+}  // namespace mppi
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
index 4233c91ef28..bb5320ebe62 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp
@@ -129,9 +129,9 @@ class PathHandler
   void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
 
   /**
-    * @brief Check if the robot pose is within the set inversion tolerances
+    * @brief Check if the robot pose is within the set inversion or rotation tolerances
     * @param robot_pose Robot's current pose to check
-    * @return bool If the robot pose is within the set inversion tolerances
+    * @return bool If the robot pose is within the tolerances for the next path constraint
     */
   bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
 
@@ -148,10 +148,12 @@ class PathHandler
   double prune_distance_{0};
   double transform_tolerance_{0};
   float inversion_xy_tolerance_{0.2};
-  float inversion_yaw_tolerance{0.4};
+  float inversion_yaw_tolerance_{0.4};
+  float minimum_rotation_angle_{0.785};
   bool enforce_path_inversion_{false};
+  bool enforce_path_rotation_{false};
   unsigned int inversion_locale_{0u};
 };
 }  // namespace mppi
 
-#endif  // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod.hpp
new file mode 100644
index 00000000000..bb5320ebe62
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod.hpp
@@ -0,0 +1,159 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+
+#include <vector>
+#include <utility>
+#include <string>
+#include <memory>
+
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
+#include "nav2_mppi_controller/tools/parameters_handler.hpp"
+
+namespace mppi
+{
+
+using PathIterator = std::vector<geometry_msgs::msg::PoseStamped>::iterator;
+using PathRange = std::pair<PathIterator, PathIterator>;
+
+/**
+ * @class mppi::PathHandler
+ * @brief Manager of incoming reference paths for transformation and processing
+ */
+
+class PathHandler
+{
+public:
+  /**
+    * @brief Constructor for mppi::PathHandler
+    */
+  PathHandler() = default;
+
+  /**
+    * @brief Destructor for mppi::PathHandler
+    */
+  ~PathHandler() = default;
+
+  /**
+    * @brief Initialize path handler on bringup
+    * @param parent WeakPtr to node
+    * @param name Name of plugin
+    * @param costmap_ros Costmap2DROS object of environment
+    * @param tf TF buffer for transformations
+    * @param dynamic_parameter_handler Parameter handler object
+    */
+  void initialize(
+    nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS>,
+    std::shared_ptr<tf2_ros::Buffer>, ParametersHandler *);
+
+  /**
+    * @brief Set new reference path
+    * @param Plan Path to use
+    */
+  void setPath(const nav_msgs::msg::Path & plan);
+
+  /**
+    * @brief Get reference path
+    * @return Path
+    */
+  nav_msgs::msg::Path & getPath();
+
+  /**
+   * @brief transform global plan to local applying constraints,
+   * then prune global plan
+   * @param robot_pose Pose of robot
+   * @return global plan in local frame
+   */
+  nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  /**
+   * @brief Get the global goal pose transformed to the local frame
+   * @param stamp Time to get the goal pose at
+   * @return Transformed goal pose
+   */
+  geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
+
+protected:
+  /**
+    * @brief Get largest dimension of costmap (radially)
+    * @return Max distance from center of costmap to edge
+    */
+  double getMaxCostmapDist();
+
+  /**
+    * @brief Transform a pose to the global reference frame
+    * @param pose Current pose
+    * @return output poose in global reference frame
+    */
+  geometry_msgs::msg::PoseStamped
+  transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+    * @brief Get global plan within window of the local costmap size
+    * @param global_pose Robot pose
+    * @return plan transformed in the costmap frame and iterator to the first pose of the global
+    * plan (for pruning)
+    */
+  std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(
+    const geometry_msgs::msg::PoseStamped & global_pose);
+
+  /**
+    * @brief Prune a path to only interesting portions
+    * @param plan Plan to prune
+    * @param end Final path iterator
+    */
+  void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
+
+  /**
+    * @brief Check if the robot pose is within the set inversion or rotation tolerances
+    * @param robot_pose Robot's current pose to check
+    * @return bool If the robot pose is within the tolerances for the next path constraint
+    */
+  bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  std::string name_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  ParametersHandler * parameters_handler_;
+
+  nav_msgs::msg::Path global_plan_;
+  nav_msgs::msg::Path global_plan_up_to_inversion_;
+  rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+
+  double max_robot_pose_search_dist_{0};
+  double prune_distance_{0};
+  double transform_tolerance_{0};
+  float inversion_xy_tolerance_{0.2};
+  float inversion_yaw_tolerance_{0.4};
+  float minimum_rotation_angle_{0.785};
+  bool enforce_path_inversion_{false};
+  bool enforce_path_rotation_{false};
+  unsigned int inversion_locale_{0u};
+};
+}  // namespace mppi
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod_final.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod_final.hpp
new file mode 100644
index 00000000000..bb5320ebe62
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_mod_final.hpp
@@ -0,0 +1,159 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+
+#include <vector>
+#include <utility>
+#include <string>
+#include <memory>
+
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
+#include "nav2_mppi_controller/tools/parameters_handler.hpp"
+
+namespace mppi
+{
+
+using PathIterator = std::vector<geometry_msgs::msg::PoseStamped>::iterator;
+using PathRange = std::pair<PathIterator, PathIterator>;
+
+/**
+ * @class mppi::PathHandler
+ * @brief Manager of incoming reference paths for transformation and processing
+ */
+
+class PathHandler
+{
+public:
+  /**
+    * @brief Constructor for mppi::PathHandler
+    */
+  PathHandler() = default;
+
+  /**
+    * @brief Destructor for mppi::PathHandler
+    */
+  ~PathHandler() = default;
+
+  /**
+    * @brief Initialize path handler on bringup
+    * @param parent WeakPtr to node
+    * @param name Name of plugin
+    * @param costmap_ros Costmap2DROS object of environment
+    * @param tf TF buffer for transformations
+    * @param dynamic_parameter_handler Parameter handler object
+    */
+  void initialize(
+    nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS>,
+    std::shared_ptr<tf2_ros::Buffer>, ParametersHandler *);
+
+  /**
+    * @brief Set new reference path
+    * @param Plan Path to use
+    */
+  void setPath(const nav_msgs::msg::Path & plan);
+
+  /**
+    * @brief Get reference path
+    * @return Path
+    */
+  nav_msgs::msg::Path & getPath();
+
+  /**
+   * @brief transform global plan to local applying constraints,
+   * then prune global plan
+   * @param robot_pose Pose of robot
+   * @return global plan in local frame
+   */
+  nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  /**
+   * @brief Get the global goal pose transformed to the local frame
+   * @param stamp Time to get the goal pose at
+   * @return Transformed goal pose
+   */
+  geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
+
+protected:
+  /**
+    * @brief Get largest dimension of costmap (radially)
+    * @return Max distance from center of costmap to edge
+    */
+  double getMaxCostmapDist();
+
+  /**
+    * @brief Transform a pose to the global reference frame
+    * @param pose Current pose
+    * @return output poose in global reference frame
+    */
+  geometry_msgs::msg::PoseStamped
+  transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+    * @brief Get global plan within window of the local costmap size
+    * @param global_pose Robot pose
+    * @return plan transformed in the costmap frame and iterator to the first pose of the global
+    * plan (for pruning)
+    */
+  std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(
+    const geometry_msgs::msg::PoseStamped & global_pose);
+
+  /**
+    * @brief Prune a path to only interesting portions
+    * @param plan Plan to prune
+    * @param end Final path iterator
+    */
+  void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
+
+  /**
+    * @brief Check if the robot pose is within the set inversion or rotation tolerances
+    * @param robot_pose Robot's current pose to check
+    * @return bool If the robot pose is within the tolerances for the next path constraint
+    */
+  bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  std::string name_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  ParametersHandler * parameters_handler_;
+
+  nav_msgs::msg::Path global_plan_;
+  nav_msgs::msg::Path global_plan_up_to_inversion_;
+  rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+
+  double max_robot_pose_search_dist_{0};
+  double prune_distance_{0};
+  double transform_tolerance_{0};
+  float inversion_xy_tolerance_{0.2};
+  float inversion_yaw_tolerance_{0.4};
+  float minimum_rotation_angle_{0.785};
+  bool enforce_path_inversion_{false};
+  bool enforce_path_rotation_{false};
+  unsigned int inversion_locale_{0u};
+};
+}  // namespace mppi
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_original.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_original.hpp
new file mode 100644
index 00000000000..4233c91ef28
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler_original.hpp
@@ -0,0 +1,157 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
+
+#include <vector>
+#include <utility>
+#include <string>
+#include <memory>
+
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
+#include "nav2_mppi_controller/tools/parameters_handler.hpp"
+
+namespace mppi
+{
+
+using PathIterator = std::vector<geometry_msgs::msg::PoseStamped>::iterator;
+using PathRange = std::pair<PathIterator, PathIterator>;
+
+/**
+ * @class mppi::PathHandler
+ * @brief Manager of incoming reference paths for transformation and processing
+ */
+
+class PathHandler
+{
+public:
+  /**
+    * @brief Constructor for mppi::PathHandler
+    */
+  PathHandler() = default;
+
+  /**
+    * @brief Destructor for mppi::PathHandler
+    */
+  ~PathHandler() = default;
+
+  /**
+    * @brief Initialize path handler on bringup
+    * @param parent WeakPtr to node
+    * @param name Name of plugin
+    * @param costmap_ros Costmap2DROS object of environment
+    * @param tf TF buffer for transformations
+    * @param dynamic_parameter_handler Parameter handler object
+    */
+  void initialize(
+    nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS>,
+    std::shared_ptr<tf2_ros::Buffer>, ParametersHandler *);
+
+  /**
+    * @brief Set new reference path
+    * @param Plan Path to use
+    */
+  void setPath(const nav_msgs::msg::Path & plan);
+
+  /**
+    * @brief Get reference path
+    * @return Path
+    */
+  nav_msgs::msg::Path & getPath();
+
+  /**
+   * @brief transform global plan to local applying constraints,
+   * then prune global plan
+   * @param robot_pose Pose of robot
+   * @return global plan in local frame
+   */
+  nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  /**
+   * @brief Get the global goal pose transformed to the local frame
+   * @param stamp Time to get the goal pose at
+   * @return Transformed goal pose
+   */
+  geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
+
+protected:
+  /**
+    * @brief Get largest dimension of costmap (radially)
+    * @return Max distance from center of costmap to edge
+    */
+  double getMaxCostmapDist();
+
+  /**
+    * @brief Transform a pose to the global reference frame
+    * @param pose Current pose
+    * @return output poose in global reference frame
+    */
+  geometry_msgs::msg::PoseStamped
+  transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+    * @brief Get global plan within window of the local costmap size
+    * @param global_pose Robot pose
+    * @return plan transformed in the costmap frame and iterator to the first pose of the global
+    * plan (for pruning)
+    */
+  std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(
+    const geometry_msgs::msg::PoseStamped & global_pose);
+
+  /**
+    * @brief Prune a path to only interesting portions
+    * @param plan Plan to prune
+    * @param end Final path iterator
+    */
+  void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
+
+  /**
+    * @brief Check if the robot pose is within the set inversion tolerances
+    * @param robot_pose Robot's current pose to check
+    * @return bool If the robot pose is within the set inversion tolerances
+    */
+  bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
+
+  std::string name_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  ParametersHandler * parameters_handler_;
+
+  nav_msgs::msg::Path global_plan_;
+  nav_msgs::msg::Path global_plan_up_to_inversion_;
+  rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+
+  double max_robot_pose_search_dist_{0};
+  double prune_distance_{0};
+  double transform_tolerance_{0};
+  float inversion_xy_tolerance_{0.2};
+  float inversion_yaw_tolerance{0.4};
+  bool enforce_path_inversion_{false};
+  unsigned int inversion_locale_{0u};
+};
+}  // namespace mppi
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer copy 2.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer copy 2.hpp
new file mode 100644
index 00000000000..f57d9b6b2b2
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer copy 2.hpp	
@@ -0,0 +1,118 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_
+
+#include <Eigen/Dense>
+
+#include <memory>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "nav2_mppi_controller/tools/parameters_handler.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_mppi_controller/models/trajectories.hpp"
+
+namespace mppi
+{
+
+/**
+ * @class mppi::TrajectoryVisualizer
+ * @brief Visualizes trajectories for debugging
+ */
+class TrajectoryVisualizer
+{
+public:
+  /**
+    * @brief Constructor for mppi::TrajectoryVisualizer
+    */
+  TrajectoryVisualizer() = default;
+
+  /**
+    * @brief Configure trajectory visualizer
+    * @param parent WeakPtr to node
+    * @param name Name of plugin
+    * @param frame_id Frame to publish trajectories in
+    * @param dynamic_parameter_handler Parameter handler object
+    */
+  void on_configure(
+    nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+    const std::string & frame_id, ParametersHandler * parameters_handler);
+
+  /**
+    * @brief Cleanup object on shutdown
+    */
+  void on_cleanup();
+
+  /**
+    * @brief Activate object
+    */
+  void on_activate();
+
+  /**
+    * @brief Deactivate object
+    */
+  void on_deactivate();
+
+  /**
+    * @brief Add an optimal trajectory to visualize
+    * @param trajectory Optimal trajectory
+    */
+  void add(
+    const Eigen::ArrayXXf & trajectory, const std::string & marker_namespace,
+    const builtin_interfaces::msg::Time & cmd_stamp);
+
+  /**
+    * @brief Add candidate trajectories to visualize
+    * @param trajectories Candidate trajectories
+    */
+  void add(const models::Trajectories & trajectories, const std::string & marker_namespace);
+
+  /**
+    * @brief Visualize the plan
+    * @param plan Plan to visualize
+    */
+  void visualize(const nav_msgs::msg::Path & plan);
+
+  /**
+    * @brief Reset object
+    */
+  void reset();
+
+protected:
+  std::string frame_id_;
+  nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    trajectories_publisher_;
+  nav2::Publisher<nav_msgs::msg::Path>::SharedPtr transformed_path_pub_;
+  nav2::Publisher<nav_msgs::msg::Path>::SharedPtr optimal_path_pub_;
+
+  std::unique_ptr<nav_msgs::msg::Path> optimal_path_;
+  std::unique_ptr<visualization_msgs::msg::MarkerArray> points_;
+  int marker_id_ = 0;
+
+  ParametersHandler * parameters_handler_;
+
+  size_t trajectory_step_{0};
+  size_t time_step_{0};
+
+  rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+};
+
+}  // namespace mppi
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp
index f57d9b6b2b2..b0a1afadfff 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp
@@ -19,14 +19,20 @@
 
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/trajectory.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 #include "nav2_mppi_controller/tools/parameters_handler.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
 #include "nav2_mppi_controller/models/trajectories.hpp"
+#include "nav2_mppi_controller/models/control_sequence.hpp"
 
 namespace mppi
 {
@@ -78,16 +84,50 @@ class TrajectoryVisualizer
     const builtin_interfaces::msg::Time & cmd_stamp);
 
   /**
-    * @brief Add candidate trajectories to visualize
+    * @brief Add candidate trajectories with costs to visualize
     * @param trajectories Candidate trajectories
+    * @param total_costs Total cost array for each trajectory
+    * @param individual_critics_cost Optional vector of (critic_name, cost_array) pairs for per-critic visualization
+    * @param cmd_stamp Timestamp for the markers
+    * @param trajectories_in_collision Optional collision flags for each trajectory
     */
-  void add(const models::Trajectories & trajectories, const std::string & marker_namespace);
+  void add(
+    const models::Trajectories & trajectories,
+    const Eigen::ArrayXf & total_costs,
+    const std::vector<std::pair<std::string, Eigen::ArrayXf>> & individual_critics_cost = {},
+    const builtin_interfaces::msg::Time & cmd_stamp = builtin_interfaces::msg::Time(),
+    const std::optional<std::vector<bool>> & trajectories_in_collision = std::nullopt);
+
+  /**
+    * @brief Visualize all trajectory data in one call
+    * @param plan Transformed plan to visualize
+    * @param optimal_trajectory Optimal trajectory
+    * @param control_sequence Control sequence for optimal trajectory
+    * @param model_dt Model time step
+    * @param stamp Timestamp for the visualization
+    * @param costmap_ros Costmap ROS pointer
+    * @param candidate_trajectories Generated candidate trajectories
+    * @param costs Total costs for each trajectory
+    * @param critic_costs Per-critic costs for each trajectory
+    * @param trajectories_in_collision Optional collision flags for each trajectory
+    */
+  void visualize(
+    nav_msgs::msg::Path plan,
+    const Eigen::ArrayXXf & optimal_trajectory,
+    const models::ControlSequence & control_sequence,
+    float model_dt,
+    const builtin_interfaces::msg::Time & stamp,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
+    const models::Trajectories & candidate_trajectories,
+    const Eigen::ArrayXf & costs,
+    const std::vector<std::pair<std::string, Eigen::ArrayXf>> & critic_costs,
+    const std::optional<std::vector<bool>> & trajectories_in_collision = std::nullopt);
 
   /**
-    * @brief Visualize the plan
-    * @param plan Plan to visualize
+    * @brief Visualize without optimizer (for testing)
+    * @param plan Transformed plan to visualize
     */
-  void visualize(const nav_msgs::msg::Path & plan);
+  void visualize(nav_msgs::msg::Path plan);
 
   /**
     * @brief Reset object
@@ -95,11 +135,40 @@ class TrajectoryVisualizer
   void reset();
 
 protected:
+  /**
+    * @brief Create trajectory markers with cost-based coloring
+    * @param trajectories Set of trajectories to visualize
+    * @param costs Cost array for each trajectory
+    * @param ns Namespace for the markers
+    * @param cmd_stamp Timestamp for the markers
+    * @param trajectories_in_collision Optional collision flags for each trajectory
+    */
+  void createTrajectoryMarkers(
+    const models::Trajectories & trajectories,
+    const Eigen::ArrayXf & costs,
+    const std::string & ns,
+    const builtin_interfaces::msg::Time & cmd_stamp,
+    const std::optional<std::vector<bool>> & trajectories_in_collision = std::nullopt);
+
+  /**
+    * @brief Create footprint markers from trajectory
+    * @param trajectory Optimal trajectory
+    * @param header Message header
+    * @param costmap_ros Costmap ROS pointer for footprint and frame information
+    * @return MarkerArray containing footprint polygons
+    */
+  visualization_msgs::msg::MarkerArray createFootprintMarkers(
+    const Eigen::ArrayXXf & trajectory,
+    const std_msgs::msg::Header & header,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
+
   std::string frame_id_;
   nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     trajectories_publisher_;
   nav2::Publisher<nav_msgs::msg::Path>::SharedPtr transformed_path_pub_;
   nav2::Publisher<nav_msgs::msg::Path>::SharedPtr optimal_path_pub_;
+  nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr optimal_footprints_pub_;
+  nav2::Publisher<nav2_msgs::msg::Trajectory>::SharedPtr optimal_trajectory_msg_pub_;
 
   std::unique_ptr<nav_msgs::msg::Path> optimal_path_;
   std::unique_ptr<visualization_msgs::msg::MarkerArray> points_;
@@ -109,10 +178,17 @@ class TrajectoryVisualizer
 
   size_t trajectory_step_{0};
   size_t time_step_{0};
+  bool publish_trajectories_with_total_cost_{false};
+  bool publish_trajectories_with_individual_cost_{false};
+  bool publish_optimal_footprints_{false};
+  bool publish_optimal_trajectory_msg_{false};
+  bool publish_transformed_path_{false};
+  bool publish_optimal_path_{false};
+  int footprint_downsample_factor_{3};
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };
 
 }  // namespace mppi
 
-#endif  // NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_
\ No newline at end of file
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy 2.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy 2.hpp
new file mode 100644
index 00000000000..4a3cc6b7c06
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy 2.hpp	
@@ -0,0 +1,689 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+
+#include <Eigen/Dense>
+
+#include <algorithm>
+#include <chrono>
+#include <string>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "angles/angles.h"
+
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/trajectory.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "nav2_ros_common/node_utils.hpp"
+#include "nav2_core/goal_checker.hpp"
+
+#include "nav2_mppi_controller/models/optimizer_settings.hpp"
+#include "nav2_mppi_controller/models/control_sequence.hpp"
+#include "nav2_mppi_controller/models/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_mppi_controller/critic_data.hpp"
+
+#define M_PIF 3.141592653589793238462643383279502884e+00F
+#define M_PIF_2 1.5707963267948966e+00F
+
+namespace mppi::utils
+{
+/**
+ * @brief Convert data into pose
+ * @param x X position
+ * @param y Y position
+ * @param z Z position
+ * @return Pose object
+ */
+inline geometry_msgs::msg::Pose createPose(double x, double y, double z)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = x;
+  pose.position.y = y;
+  pose.position.z = z;
+  pose.orientation.w = 1;
+  pose.orientation.x = 0;
+  pose.orientation.y = 0;
+  pose.orientation.z = 0;
+  return pose;
+}
+
+/**
+ * @brief Convert data into scale
+ * @param x X scale
+ * @param y Y scale
+ * @param z Z scale
+ * @return Scale object
+ */
+inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z)
+{
+  geometry_msgs::msg::Vector3 scale;
+  scale.x = x;
+  scale.y = y;
+  scale.z = z;
+  return scale;
+}
+
+/**
+ * @brief Convert data into color
+ * @param r Red component
+ * @param g Green component
+ * @param b Blue component
+ * @param a Alpha component (transparency)
+ * @return Color object
+ */
+inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a)
+{
+  std_msgs::msg::ColorRGBA color;
+  color.r = r;
+  color.g = g;
+  color.b = b;
+  color.a = a;
+  return color;
+}
+
+/**
+ * @brief Convert data into a Maarker
+ * @param id Marker ID
+ * @param pose Marker pose
+ * @param scale Marker scale
+ * @param color Marker color
+ * @param frame Reference frame to use
+ * @return Visualization Marker
+ */
+inline visualization_msgs::msg::Marker createMarker(
+  int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale,
+  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns)
+{
+  using visualization_msgs::msg::Marker;
+  Marker marker;
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Time(0, 0);
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = Marker::SPHERE;
+  marker.action = Marker::ADD;
+
+  marker.pose = pose;
+  marker.scale = scale;
+  marker.color = color;
+  return marker;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame)
+{
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header.frame_id = frame;
+  twist.header.stamp = stamp;
+  twist.twist.linear.x = vx;
+  twist.twist.angular.z = wz;
+
+  return twist;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param vy Y velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp,
+  const std::string & frame)
+{
+  auto twist = toTwistStamped(vx, wz, stamp, frame);
+  twist.twist.linear.y = vy;
+
+  return twist;
+}
+
+inline std::unique_ptr<nav2_msgs::msg::Trajectory> toTrajectoryMsg(
+  const Eigen::ArrayXXf & trajectory,
+  const models::ControlSequence & control_sequence,
+  const double & model_dt,
+  const std_msgs::msg::Header & header)
+{
+  auto trajectory_msg = std::make_unique<nav2_msgs::msg::Trajectory>();
+  trajectory_msg->header = header;
+  trajectory_msg->points.resize(trajectory.rows());
+
+  for (int i = 0; i < trajectory.rows(); ++i) {
+    auto & curr_pt = trajectory_msg->points[i];
+    curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt);
+    curr_pt.pose.position.x = trajectory(i, 0);
+    curr_pt.pose.position.y = trajectory(i, 1);
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, trajectory(i, 2));
+    curr_pt.pose.orientation = tf2::toMsg(quat);
+    curr_pt.velocity.linear.x = control_sequence.vx(i);
+    curr_pt.velocity.angular.z = control_sequence.wz(i);
+    if (control_sequence.vy.size() > 0) {
+      curr_pt.velocity.linear.y = control_sequence.vy(i);
+    }
+  }
+
+  return trajectory_msg;
+}
+
+/**
+ * @brief Convert path to a tensor
+ * @param path Path to convert
+ * @return Path tensor
+ */
+inline models::Path toTensor(const nav_msgs::msg::Path & path)
+{
+  auto result = models::Path{};
+  result.reset(path.poses.size());
+
+  for (size_t i = 0; i < path.poses.size(); ++i) {
+    result.x(i) = path.poses[i].pose.position.x;
+    result.y(i) = path.poses[i].pose.position.y;
+    result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation);
+  }
+
+  return result;
+}
+
+/**
+ * @brief Get the last pose from a path
+ * @param path Reference to the path
+ * @return geometry_msgs::msg::Pose Last pose in the path
+ */
+inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path)
+{
+  const unsigned int path_last_idx = path.x.size() - 1;
+
+  auto last_orientation = path.yaws(path_last_idx);
+
+  tf2::Quaternion pose_orientation;
+  pose_orientation.setRPY(0.0, 0.0, last_orientation);
+
+  geometry_msgs::msg::Pose pathPose;
+  pathPose.position.x = path.x(path_last_idx);
+  pathPose.position.y = path.y(path_last_idx);
+  pathPose.orientation.x = pose_orientation.x();
+  pathPose.orientation.y = pose_orientation.y();
+  pathPose.orientation.z = pose_orientation.z();
+  pathPose.orientation.w = pose_orientation.w();
+
+  return pathPose;
+}
+
+/**
+  * @brief normalize
+  * Normalizes the angle to be -M_PIF circle to +M_PIF circle
+  * It takes and returns radians.
+  * @param angles Angles to normalize
+  * @return normalized angles
+  */
+template<typename T>
+auto normalize_angles(const T & angles)
+{
+  return (angles + M_PIF).unaryExpr([&](const float x) {
+             float remainder = std::fmod(x, 2.0f * M_PIF);
+             return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;
+             });
+}
+
+/**
+  * @brief shortest_angular_distance
+  *
+  * Given 2 angles, this returns the shortest angular
+  * difference.  The inputs and outputs are of course radians.
+  *
+  * The result
+  * would always be -pi <= result <= pi.  Adding the result
+  * to "from" will always get you an equivalent angle to "to".
+  * @param from Start angle
+  * @param to End angle
+  * @return Shortest distance between angles
+  */
+template<typename F, typename T>
+auto shortest_angular_distance(
+  const F & from,
+  const T & to)
+{
+  return normalize_angles(to - from);
+}
+
+/**
+ * @brief Evaluate furthest point idx of data.path which is
+ * nearest to some trajectory in data.trajectories
+ * @param data Data to use
+ * @return Idx of furthest path point reached by a set of trajectories
+ */
+inline size_t findPathFurthestReachedPoint(const CriticData & data)
+{
+  int traj_cols = data.trajectories.x.cols();
+  const auto traj_x = data.trajectories.x.col(traj_cols - 1);
+  const auto traj_y = data.trajectories.y.col(traj_cols - 1);
+
+  const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x;
+  const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y;
+
+  const auto dists = dx * dx + dy * dy;
+
+  int max_id_by_trajectories = 0, min_id_by_path = 0;
+  float min_distance_by_path = std::numeric_limits<float>::max();
+  size_t n_rows = dists.rows();
+  size_t n_cols = dists.cols();
+  for (size_t i = 0; i != n_rows; i++) {
+    min_id_by_path = 0;
+    min_distance_by_path = std::numeric_limits<float>::max();
+    for (size_t j = max_id_by_trajectories; j != n_cols; j++) {
+      const float cur_dist = dists(i, j);
+      if (cur_dist < min_distance_by_path) {
+        min_distance_by_path = cur_dist;
+        min_id_by_path = j;
+      }
+    }
+    max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path);
+  }
+  return max_id_by_trajectories;
+}
+
+/**
+ * @brief evaluate path furthest point if it is not set
+ * @param data Data to use
+ */
+inline void setPathFurthestPointIfNotSet(CriticData & data)
+{
+  if (!data.furthest_reached_path_point) {
+    data.furthest_reached_path_point = findPathFurthestReachedPoint(data);
+  }
+}
+
+/**
+ * @brief evaluate path costs
+ * @param data Data to use
+ */
+inline void findPathCosts(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  auto * costmap = costmap_ros->getCostmap();
+  unsigned int map_x, map_y;
+  const size_t path_segments_count = data.path.x.size() - 1;
+  data.path_pts_valid = std::vector<bool>(path_segments_count, false);
+  const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown();
+  for (unsigned int idx = 0; idx < path_segments_count; idx++) {
+    if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) {
+      (*data.path_pts_valid)[idx] = false;
+      continue;
+    }
+
+    switch (costmap->getCost(map_x, map_y)) {
+      case (nav2_costmap_2d::LETHAL_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::NO_INFORMATION):
+        (*data.path_pts_valid)[idx] = tracking_unknown ? true : false;
+        continue;
+    }
+
+    (*data.path_pts_valid)[idx] = true;
+  }
+}
+
+/**
+ * @brief evaluate path costs if it is not set
+ * @param data Data to use
+ */
+inline void setPathCostsIfNotSet(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  if (!data.path_pts_valid) {
+    findPathCosts(data, costmap_ros);
+  }
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param forward_preference If reversing direction is valid
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference)
+{
+  float pose_x = pose.position.x;
+  float pose_y = pose.position.y;
+  float pose_yaw = tf2::getYaw(pose.orientation);
+
+  float yaw = atan2f(point_y - pose_y, point_x - pose_x);
+
+  // If no preference for forward, return smallest angle either in heading or 180 of heading
+  if (!forward_preference) {
+    return std::min(
+      fabs(angles::shortest_angular_distance(yaw, pose_yaw)),
+      fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF))));
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param point_yaw Yaw of the point to consider along Z axis
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose,
+  double point_x, double point_y, double point_yaw)
+{
+  float pose_x = static_cast<float>(pose.position.x);
+  float pose_y = static_cast<float>(pose.position.y);
+  float pose_yaw = static_cast<float>(tf2::getYaw(pose.orientation));
+
+  float yaw = atan2f(static_cast<float>(point_y) - pose_y, static_cast<float>(point_x) - pose_x);
+
+  if (fabs(angles::shortest_angular_distance(yaw, static_cast<float>(point_yaw))) > M_PIF_2) {
+    yaw = angles::normalize_angle(yaw + M_PIF);
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief Apply Savisky-Golay filter to optimal trajectory
+ * @param control_sequence Sequence to apply filter to
+ * @param control_history Recent set of controls for edge-case handling
+ * @param Settings Settings to use
+ */
+inline void savitskyGolayFilter(
+  models::ControlSequence & control_sequence,
+  std::array<mppi::models::Control, 4> & control_history,
+  const models::OptimizerSettings & settings)
+{
+  // Savitzky-Golay Quadratic, 9-point Coefficients
+  Eigen::Array<float, 9, 1> filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f,
+    -21.0f};
+  filter /= 231.0f;
+
+  // Too short to smooth meaningfully
+  const unsigned int num_sequences = control_sequence.vx.size() - 1;
+  if (num_sequences < 20) {
+    return;
+  }
+
+  auto applyFilter = [&](const Eigen::Array<float, 9, 1> & data) -> float {
+      return (data * filter).eval().sum();
+    };
+
+  auto applyFilterOverAxis =
+    [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence,
+    const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
+    {
+      float pt_m4 = hist_0;
+      float pt_m3 = hist_1;
+      float pt_m2 = hist_2;
+      float pt_m1 = hist_3;
+      float pt = initial_sequence(0);
+      float pt_p1 = initial_sequence(1);
+      float pt_p2 = initial_sequence(2);
+      float pt_p3 = initial_sequence(3);
+      float pt_p4 = initial_sequence(4);
+
+      for (unsigned int idx = 0; idx != num_sequences; idx++) {
+        sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4});
+        pt_m4 = pt_m3;
+        pt_m3 = pt_m2;
+        pt_m2 = pt_m1;
+        pt_m1 = pt;
+        pt = pt_p1;
+        pt_p1 = pt_p2;
+        pt_p2 = pt_p3;
+        pt_p3 = pt_p4;
+
+        if (idx + 5 < num_sequences) {
+          pt_p4 = initial_sequence(idx + 5);
+        } else {
+          // Return the last point
+          pt_p4 = initial_sequence(num_sequences);
+        }
+      }
+    };
+
+  // Filter trajectories
+  const models::ControlSequence initial_control_sequence = control_sequence;
+  applyFilterOverAxis(
+    control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
+    control_history[1].vx, control_history[2].vx, control_history[3].vx);
+  applyFilterOverAxis(
+    control_sequence.vy, initial_control_sequence.vy, control_history[0].vy,
+    control_history[1].vy, control_history[2].vy, control_history[3].vy);
+  applyFilterOverAxis(
+    control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
+    control_history[1].wz, control_history[2].wz, control_history[3].wz);
+
+  // Update control history
+  unsigned int offset = settings.shift_control_sequence ? 1 : 0;
+  control_history[0] = control_history[1];
+  control_history[1] = control_history[2];
+  control_history[2] = control_history[3];
+  control_history[3] = {
+    control_sequence.vx(offset),
+    control_sequence.vy(offset),
+    control_sequence.wz(offset)};
+}
+
+/**
+ * @brief Find the iterator of the first pose at which there is an inversion on the path,
+ * @param path to check for inversion
+ * @return the first point after the inversion found in the path
+ */
+inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
+{
+  // At least 3 poses for a possible inversion
+  if (path.poses.size() < 3) {
+    return path.poses.size();
+  }
+
+  // Iterating through the path to determine the position of the path inversion
+  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+    // We have two vectors for the dot product OA and AB. Determining the vectors.
+    float oa_x = path.poses[idx].pose.position.x -
+      path.poses[idx - 1].pose.position.x;
+    float oa_y = path.poses[idx].pose.position.y -
+      path.poses[idx - 1].pose.position.y;
+    float ab_x = path.poses[idx + 1].pose.position.x -
+      path.poses[idx].pose.position.x;
+    float ab_y = path.poses[idx + 1].pose.position.y -
+      path.poses[idx].pose.position.y;
+
+    // Checking for the existence of cusp, in the path, using the dot product.
+    float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+    if (dot_product < 0.0f) {
+      return idx + 1;
+    }
+  }
+
+  return path.poses.size();
+}
+
+/**
+ * @brief Find and remove poses after the first inversion in the path
+ * @param path to check for inversion
+ * @return The location of the inversion, return 0 if none exist
+ */
+inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
+{
+  nav_msgs::msg::Path cropped_path = path;
+  const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
+  if (first_after_inversion == path.poses.size()) {
+    return 0u;
+  }
+
+  cropped_path.poses.erase(
+    cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
+  path = cropped_path;
+  return first_after_inversion;
+}
+
+/**
+ * @brief Compare to trajectory points to find closest path point along integrated distances
+ * @param vec Vect to check
+ * @return dist Distance to look for
+ * @return init Starting index to indec from
+ */
+inline unsigned int findClosestPathPt(
+  const std::vector<float> & vec, const float dist, const unsigned int init = 0u)
+{
+  float distim1 = init != 0u ? vec[init] : 0.0f;  // First is 0, no accumulated distance yet
+  float disti = 0.0f;
+  const unsigned int size = vec.size();
+  for (unsigned int i = init + 1; i != size; i++) {
+    disti = vec[i];
+    if (disti > dist) {
+      if (i > 0 && dist - distim1 < disti - dist) {
+        return i - 1;
+      }
+      return i;
+    }
+    distim1 = disti;
+  }
+  return size - 1;
+}
+
+// A struct to hold pose data in floating point resolution
+struct Pose2D
+{
+  float x, y, theta;
+};
+
+/**
+ * @brief Shift the columns of a 2D Eigen Array or scalar values of
+ *    1D Eigen Array by 1 place.
+ * @param e Eigen Array
+ * @param direction direction in which Array will be shifted.
+ *     1 for shift in right direction and -1 for left direction.
+ */
+inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
+{
+  int size = e.size();
+  if(size == 1) {return;}
+  if(abs(direction) != 1) {
+    throw std::logic_error("Invalid direction, only 1 and -1 are valid values.");
+  }
+
+  if((e.cols() == 1 || e.rows() == 1) && size > 1) {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1;
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1;
+    while(start_ptr != end_ptr) {
+      *(start_ptr + direction) = *start_ptr;
+      start_ptr -= direction;
+    }
+    *(start_ptr + direction) = *start_ptr;
+  } else {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows();
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows();
+    auto span = e.rows();
+    while(start_ptr != end_ptr) {
+      std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+      start_ptr -= (direction * span);
+    }
+    std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+  }
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of final yaw angle
+ *    of the trajectory.
+ * @param last_yaws Final yaw angles of the trajectories.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points.
+ */
+inline auto normalize_yaws_between_points(
+  const Eigen::Ref<const Eigen::ArrayXf> & last_yaws,
+  const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  Eigen::ArrayXf yaws = utils::shortest_angular_distance(
+          last_yaws, yaw_between_points).abs();
+  int size = yaws.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of goal angle.
+ * @param goal_yaw Goal yaw angle.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points
+ */
+inline auto normalize_yaws_between_points(
+  const float goal_yaw, const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  int size = yaw_between_points.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = fabs(
+      angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Clamps the input between the given lower and upper bounds.
+ * @param lower_bound Lower bound.
+ * @param upper_bound Upper bound.
+ * @return Clamped output.
+ */
+inline float clamp(
+  const float lower_bound, const float upper_bound, const float input)
+{
+  return std::min(upper_bound, std::max(input, lower_bound));
+}
+
+}  // namespace mppi::utils
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy.hpp
new file mode 100644
index 00000000000..cb6c000f2db
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy.hpp	
@@ -0,0 +1,824 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+
+#include <Eigen/Dense>
+
+#include <algorithm>
+#include <chrono>
+#include <string>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "angles/angles.h"
+
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/trajectory.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "nav2_ros_common/node_utils.hpp"
+#include "nav2_core/goal_checker.hpp"
+
+#include "nav2_mppi_controller/models/optimizer_settings.hpp"
+#include "nav2_mppi_controller/models/control_sequence.hpp"
+#include "nav2_mppi_controller/models/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_mppi_controller/critic_data.hpp"
+
+#define M_PIF 3.141592653589793238462643383279502884e+00F
+#define M_PIF_2 1.5707963267948966e+00F
+
+namespace mppi::utils
+{
+/**
+ * @brief Convert data into pose
+ * @param x X position
+ * @param y Y position
+ * @param z Z position
+ * @return Pose object
+ */
+inline geometry_msgs::msg::Pose createPose(double x, double y, double z)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = x;
+  pose.position.y = y;
+  pose.position.z = z;
+  pose.orientation.w = 1;
+  pose.orientation.x = 0;
+  pose.orientation.y = 0;
+  pose.orientation.z = 0;
+  return pose;
+}
+
+/**
+ * @brief Convert data into scale
+ * @param x X scale
+ * @param y Y scale
+ * @param z Z scale
+ * @return Scale object
+ */
+inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z)
+{
+  geometry_msgs::msg::Vector3 scale;
+  scale.x = x;
+  scale.y = y;
+  scale.z = z;
+  return scale;
+}
+
+/**
+ * @brief Convert data into color
+ * @param r Red component
+ * @param g Green component
+ * @param b Blue component
+ * @param a Alpha component (transparency)
+ * @return Color object
+ */
+inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a)
+{
+  std_msgs::msg::ColorRGBA color;
+  color.r = r;
+  color.g = g;
+  color.b = b;
+  color.a = a;
+  return color;
+}
+
+/**
+ * @brief Convert data into a Maarker
+ * @param id Marker ID
+ * @param pose Marker pose
+ * @param scale Marker scale
+ * @param color Marker color
+ * @param frame Reference frame to use
+ * @return Visualization Marker
+ */
+inline visualization_msgs::msg::Marker createMarker(
+  int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale,
+  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns)
+{
+  using visualization_msgs::msg::Marker;
+  Marker marker;
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Time(0, 0);
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = Marker::SPHERE;
+  marker.action = Marker::ADD;
+
+  marker.pose = pose;
+  marker.scale = scale;
+  marker.color = color;
+  return marker;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame)
+{
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header.frame_id = frame;
+  twist.header.stamp = stamp;
+  twist.twist.linear.x = vx;
+  twist.twist.angular.z = wz;
+
+  return twist;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param vy Y velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp,
+  const std::string & frame)
+{
+  auto twist = toTwistStamped(vx, wz, stamp, frame);
+  twist.twist.linear.y = vy;
+
+  return twist;
+}
+
+inline std::unique_ptr<nav2_msgs::msg::Trajectory> toTrajectoryMsg(
+  const Eigen::ArrayXXf & trajectory,
+  const models::ControlSequence & control_sequence,
+  const double & model_dt,
+  const std_msgs::msg::Header & header)
+{
+  auto trajectory_msg = std::make_unique<nav2_msgs::msg::Trajectory>();
+  trajectory_msg->header = header;
+  trajectory_msg->points.resize(trajectory.rows());
+
+  for (int i = 0; i < trajectory.rows(); ++i) {
+    auto & curr_pt = trajectory_msg->points[i];
+    curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt);
+    curr_pt.pose.position.x = trajectory(i, 0);
+    curr_pt.pose.position.y = trajectory(i, 1);
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, trajectory(i, 2));
+    curr_pt.pose.orientation = tf2::toMsg(quat);
+    curr_pt.velocity.linear.x = control_sequence.vx(i);
+    curr_pt.velocity.angular.z = control_sequence.wz(i);
+    if (control_sequence.vy.size() > 0) {
+      curr_pt.velocity.linear.y = control_sequence.vy(i);
+    }
+  }
+
+  return trajectory_msg;
+}
+
+/**
+ * @brief Convert path to a tensor
+ * @param path Path to convert
+ * @return Path tensor
+ */
+inline models::Path toTensor(const nav_msgs::msg::Path & path)
+{
+  auto result = models::Path{};
+  result.reset(path.poses.size());
+
+  for (size_t i = 0; i < path.poses.size(); ++i) {
+    result.x(i) = path.poses[i].pose.position.x;
+    result.y(i) = path.poses[i].pose.position.y;
+    result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation);
+  }
+
+  return result;
+}
+
+/**
+ * @brief Get the last pose from a path
+ * @param path Reference to the path
+ * @return geometry_msgs::msg::Pose Last pose in the path
+ */
+inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path)
+{
+  const unsigned int path_last_idx = path.x.size() - 1;
+
+  auto last_orientation = path.yaws(path_last_idx);
+
+  tf2::Quaternion pose_orientation;
+  pose_orientation.setRPY(0.0, 0.0, last_orientation);
+
+  geometry_msgs::msg::Pose pathPose;
+  pathPose.position.x = path.x(path_last_idx);
+  pathPose.position.y = path.y(path_last_idx);
+  pathPose.orientation.x = pose_orientation.x();
+  pathPose.orientation.y = pose_orientation.y();
+  pathPose.orientation.z = pose_orientation.z();
+  pathPose.orientation.w = pose_orientation.w();
+
+  return pathPose;
+}
+
+/**
+  * @brief normalize
+  * Normalizes the angle to be -M_PIF circle to +M_PIF circle
+  * It takes and returns radians.
+  * @param angles Angles to normalize
+  * @return normalized angles
+  */
+template<typename T>
+auto normalize_angles(const T & angles)
+{
+  return (angles + M_PIF).unaryExpr([&](const float x) {
+             float remainder = std::fmod(x, 2.0f * M_PIF);
+             return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;
+             });
+}
+
+/**
+  * @brief shortest_angular_distance
+  *
+  * Given 2 angles, this returns the shortest angular
+  * difference.  The inputs and outputs are of course radians.
+  *
+  * The result
+  * would always be -pi <= result <= pi.  Adding the result
+  * to "from" will always get you an equivalent angle to "to".
+  * @param from Start angle
+  * @param to End angle
+  * @return Shortest distance between angles
+  */
+template<typename F, typename T>
+auto shortest_angular_distance(
+  const F & from,
+  const T & to)
+{
+  return normalize_angles(to - from);
+}
+
+/**
+ * @brief Evaluate furthest point idx of data.path which is
+ * nearest to some trajectory in data.trajectories
+ * @param data Data to use
+ * @return Idx of furthest path point reached by a set of trajectories
+ */
+inline size_t findPathFurthestReachedPoint(const CriticData & data)
+{
+  int traj_cols = data.trajectories.x.cols();
+  const auto traj_x = data.trajectories.x.col(traj_cols - 1);
+  const auto traj_y = data.trajectories.y.col(traj_cols - 1);
+
+  const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x;
+  const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y;
+
+  const auto dists = dx * dx + dy * dy;
+
+  int max_id_by_trajectories = 0, min_id_by_path = 0;
+  float min_distance_by_path = std::numeric_limits<float>::max();
+  size_t n_rows = dists.rows();
+  size_t n_cols = dists.cols();
+  for (size_t i = 0; i != n_rows; i++) {
+    min_id_by_path = 0;
+    min_distance_by_path = std::numeric_limits<float>::max();
+    for (size_t j = max_id_by_trajectories; j != n_cols; j++) {
+      const float cur_dist = dists(i, j);
+      if (cur_dist < min_distance_by_path) {
+        min_distance_by_path = cur_dist;
+        min_id_by_path = j;
+      }
+    }
+    max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path);
+  }
+  return max_id_by_trajectories;
+}
+
+/**
+ * @brief evaluate path furthest point if it is not set
+ * @param data Data to use
+ */
+inline void setPathFurthestPointIfNotSet(CriticData & data)
+{
+  if (!data.furthest_reached_path_point) {
+    data.furthest_reached_path_point = findPathFurthestReachedPoint(data);
+  }
+}
+
+/**
+ * @brief evaluate path costs
+ * @param data Data to use
+ */
+inline void findPathCosts(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  auto * costmap = costmap_ros->getCostmap();
+  unsigned int map_x, map_y;
+  const size_t path_segments_count = data.path.x.size() - 1;
+  data.path_pts_valid = std::vector<bool>(path_segments_count, false);
+  const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown();
+  for (unsigned int idx = 0; idx < path_segments_count; idx++) {
+    if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) {
+      (*data.path_pts_valid)[idx] = false;
+      continue;
+    }
+
+    switch (costmap->getCost(map_x, map_y)) {
+      case (nav2_costmap_2d::LETHAL_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::NO_INFORMATION):
+        (*data.path_pts_valid)[idx] = tracking_unknown ? true : false;
+        continue;
+    }
+
+    (*data.path_pts_valid)[idx] = true;
+  }
+}
+
+/**
+ * @brief evaluate path costs if it is not set
+ * @param data Data to use
+ */
+inline void setPathCostsIfNotSet(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  if (!data.path_pts_valid) {
+    findPathCosts(data, costmap_ros);
+  }
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param forward_preference If reversing direction is valid
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference)
+{
+  float pose_x = pose.position.x;
+  float pose_y = pose.position.y;
+  float pose_yaw = tf2::getYaw(pose.orientation);
+
+  float yaw = atan2f(point_y - pose_y, point_x - pose_x);
+
+  // If no preference for forward, return smallest angle either in heading or 180 of heading
+  if (!forward_preference) {
+    return std::min(
+      fabs(angles::shortest_angular_distance(yaw, pose_yaw)),
+      fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF))));
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param point_yaw Yaw of the point to consider along Z axis
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose,
+  double point_x, double point_y, double point_yaw)
+{
+  float pose_x = static_cast<float>(pose.position.x);
+  float pose_y = static_cast<float>(pose.position.y);
+  float pose_yaw = static_cast<float>(tf2::getYaw(pose.orientation));
+
+  float yaw = atan2f(static_cast<float>(point_y) - pose_y, static_cast<float>(point_x) - pose_x);
+
+  if (fabs(angles::shortest_angular_distance(yaw, static_cast<float>(point_yaw))) > M_PIF_2) {
+    yaw = angles::normalize_angle(yaw + M_PIF);
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief Apply Savisky-Golay filter to optimal trajectory
+ * @param control_sequence Sequence to apply filter to
+ * @param control_history Recent set of controls for edge-case handling
+ * @param Settings Settings to use
+ */
+inline void savitskyGolayFilter(
+  models::ControlSequence & control_sequence,
+  std::array<mppi::models::Control, 4> & control_history,
+  const models::OptimizerSettings & settings)
+{
+  // Savitzky-Golay Quadratic, 9-point Coefficients
+  Eigen::Array<float, 9, 1> filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f,
+    -21.0f};
+  filter /= 231.0f;
+
+  // Too short to smooth meaningfully
+  const unsigned int num_sequences = control_sequence.vx.size() - 1;
+  if (num_sequences < 20) {
+    return;
+  }
+
+  auto applyFilter = [&](const Eigen::Array<float, 9, 1> & data) -> float {
+      return (data * filter).eval().sum();
+    };
+
+  auto applyFilterOverAxis =
+    [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence,
+    const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
+    {
+      float pt_m4 = hist_0;
+      float pt_m3 = hist_1;
+      float pt_m2 = hist_2;
+      float pt_m1 = hist_3;
+      float pt = initial_sequence(0);
+      float pt_p1 = initial_sequence(1);
+      float pt_p2 = initial_sequence(2);
+      float pt_p3 = initial_sequence(3);
+      float pt_p4 = initial_sequence(4);
+
+      for (unsigned int idx = 0; idx != num_sequences; idx++) {
+        sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4});
+        pt_m4 = pt_m3;
+        pt_m3 = pt_m2;
+        pt_m2 = pt_m1;
+        pt_m1 = pt;
+        pt = pt_p1;
+        pt_p1 = pt_p2;
+        pt_p2 = pt_p3;
+        pt_p3 = pt_p4;
+
+        if (idx + 5 < num_sequences) {
+          pt_p4 = initial_sequence(idx + 5);
+        } else {
+          // Return the last point
+          pt_p4 = initial_sequence(num_sequences);
+        }
+      }
+    };
+
+  // Filter trajectories
+  const models::ControlSequence initial_control_sequence = control_sequence;
+  applyFilterOverAxis(
+    control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
+    control_history[1].vx, control_history[2].vx, control_history[3].vx);
+  applyFilterOverAxis(
+    control_sequence.vy, initial_control_sequence.vy, control_history[0].vy,
+    control_history[1].vy, control_history[2].vy, control_history[3].vy);
+  applyFilterOverAxis(
+    control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
+    control_history[1].wz, control_history[2].wz, control_history[3].wz);
+
+  // Update control history
+  unsigned int offset = settings.shift_control_sequence ? 1 : 0;
+  control_history[0] = control_history[1];
+  control_history[1] = control_history[2];
+  control_history[2] = control_history[3];
+  control_history[3] = {
+    control_sequence.vx(offset),
+    control_sequence.vy(offset),
+    control_sequence.wz(offset)};
+}
+
+/**
+ * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path
+ * @param path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return the first point after the inversion or rotation found in the path
+ */
+inline unsigned int findFirstPathInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
+{
+  if (path.poses.size() < 2) {
+    RCLCPP_INFO(
+      rclcpp::get_logger("mppi_utils"),
+      "[MPPI PathUtils] Path too short for constraints (size=%zu)",
+      path.poses.size());
+    return path.poses.size();
+  }
+
+  unsigned int first_after_constraint = path.poses.size();
+
+  // TYLKO DO LOGÓW: maksymalna rotacja poniżej progu (żeby wiedzieć, czy było \"prawie\")
+  float max_rotation_below_threshold = 0.0f;
+
+  // Check for in-place rotation first (if enabled)
+  if (rotation_threshold > 0.0f) {
+    for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) {
+      float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x;
+      float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y;
+      float translation = sqrtf(dx * dx + dy * dy);
+
+      // Check if poses are at roughly the same location
+      if (translation < 1e-3) {
+        float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation);
+        float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation);
+        float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2));
+
+        // TYLKO DO LOGÓW: zapamiętaj największą rotację poniżej progu
+        if (rotation < rotation_threshold &&
+            rotation > max_rotation_below_threshold)
+        {
+          max_rotation_below_threshold = rotation;
+        }
+
+        // Check if this meets the minimum rotation threshold
+        if (rotation > rotation_threshold) {
+          // Found start of in-place rotation, now find where it ends
+          unsigned int end_idx = idx + 1;
+
+          // Continue while we have minimal translation (still rotating in place)
+          while (end_idx < path.poses.size() - 1) {
+            float next_dx = path.poses[end_idx + 1].pose.position.x -
+              path.poses[end_idx].pose.position.x;
+            float next_dy = path.poses[end_idx + 1].pose.position.y -
+              path.poses[end_idx].pose.position.y;
+            float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy);
+
+            if (next_translation >= 1e-3) {
+              break;  // End of in-place rotation sequence
+            }
+            end_idx++;
+          }
+
+          first_after_constraint = end_idx;
+
+          // LOG: wykryto rotation constraint
+          RCLCPP_INFO(
+            rclcpp::get_logger("mppi_utils"),
+            "[MPPI PathUtils] Rotation constraint detected: rotation=%.3f rad, "
+            "threshold=%.3f rad, start_idx=%u, constraint_idx=%u",
+            rotation, rotation_threshold, idx, end_idx);
+
+          break;
+        }
+      }
+    }
+  }
+
+  // Check for inversion (at least 3 poses needed)
+  if (path.poses.size() >= 3) {
+    for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+      float oa_x = path.poses[idx].pose.position.x -
+        path.poses[idx - 1].pose.position.x;
+      float oa_y = path.poses[idx].pose.position.y -
+        path.poses[idx - 1].pose.position.y;
+      float ab_x = path.poses[idx + 1].pose.position.x -
+        path.poses[idx].pose.position.x;
+      float ab_y = path.poses[idx + 1].pose.position.y -
+        path.poses[idx].pose.position.y;
+
+      float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+
+      float norm_oa = hypotf(oa_x, oa_y);
+      float norm_ab = hypotf(ab_x, ab_y);
+
+      if (norm_oa < 1e-4f || norm_ab < 1e-4f) {
+        continue;
+      }
+
+      float cos_ang = dot_product / (norm_oa * norm_ab);
+      cos_ang = std::max(-1.0f, std::min(1.0f, cos_ang));
+      float angle = acosf(cos_ang);  // [0, pi]
+
+      if (rotation_threshold > 0.0f) {
+        if (angle > rotation_threshold) {
+          first_after_constraint = std::min(first_after_constraint, idx + 1);
+
+          // LOG: constraint z powodu kąta (cusp/inversion po łuku)
+          RCLCPP_INFO(
+            rclcpp::get_logger("mppi_utils"),
+            "[MPPI PathUtils] Inversion/cusp constraint detected by angle: "
+            "angle=%.3f rad (threshold=%.3f rad), at idx=%u (constraint_idx=%u)",
+            angle, rotation_threshold, idx, first_after_constraint);
+
+          break;
+        }
+      } else {
+        if (dot_product < 0.0f) {
+          first_after_constraint = std::min(first_after_constraint, idx + 1);
+
+          // LOG: klasyczny cusp przez dot_product<0
+          RCLCPP_INFO(
+            rclcpp::get_logger("mppi_utils"),
+            "[MPPI PathUtils] Inversion cusp constraint detected by dot_product<0 "
+            "at idx=%u (constraint_idx=%u, dot=%.6f)",
+            idx, first_after_constraint, dot_product);
+
+          break;
+        }
+      }
+    }
+  }
+
+  // Podsumowanie: nic nie znalazł -> zobacz, czy rotacja była \"prawie\" wystarczająca
+  if (first_after_constraint == path.poses.size()) {
+    if (rotation_threshold > 0.0f && max_rotation_below_threshold > 0.0f) {
+      float ratio = max_rotation_below_threshold / rotation_threshold;
+
+      if (ratio >= 0.9f) {
+        // LOG: rotacja była blisko progu (>= 90%)
+        RCLCPP_INFO(
+          rclcpp::get_logger("mppi_utils"),
+          "[MPPI PathUtils] Rotation almost met threshold: rotation=%.3f rad, "
+          "threshold=%.3f rad (%.1f%% of threshold) – no constraint applied",
+          max_rotation_below_threshold, rotation_threshold, ratio * 100.0f);
+      } else {
+        RCLCPP_INFO(
+          rclcpp::get_logger("mppi_utils"),
+          "[MPPI PathUtils] No constraint found. Max rotation below threshold: "
+          "rotation=%.3f rad, threshold=%.3f rad (%.1f%% of threshold)",
+          max_rotation_below_threshold, rotation_threshold, ratio * 100.0f);
+      }
+    } else {
+      RCLCPP_INFO(
+        rclcpp::get_logger("mppi_utils"),
+        "[MPPI PathUtils] No inversion / rotation constraint found "
+        "(path_size=%zu, rotation_threshold=%.3f)",
+        path.poses.size(), rotation_threshold);
+    }
+  }
+
+  return first_after_constraint;
+}
+
+
+/**
+ * @brief Find and remove poses after the first inversion or in-place rotation in the path
+ * @param path Path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return The location of the inversion/rotation, return 0 if none exist
+ */
+inline unsigned int removePosesAfterFirstInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
+{
+  nav_msgs::msg::Path cropped_path = path;
+
+  const unsigned int first_after_constraint = findFirstPathInversion(cropped_path, rotation_threshold);
+
+  if (first_after_constraint == path.poses.size()) {
+    return 0u;  // No constraint found
+  }
+
+  cropped_path.poses.erase(
+    cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end());
+  path = cropped_path;
+  return first_after_constraint;
+}
+
+/**
+ * @brief Compare to trajectory points to find closest path point along integrated distances
+ * @param vec Vect to check
+ * @return dist Distance to look for
+ * @return init Starting index to indec from
+ */
+inline unsigned int findClosestPathPt(
+  const std::vector<float> & vec, const float dist, const unsigned int init = 0u)
+{
+  float distim1 = init != 0u ? vec[init] : 0.0f;  // First is 0, no accumulated distance yet
+  float disti = 0.0f;
+  const unsigned int size = vec.size();
+  for (unsigned int i = init + 1; i != size; i++) {
+    disti = vec[i];
+    if (disti > dist) {
+      if (i > 0 && dist - distim1 < disti - dist) {
+        return i - 1;
+      }
+      return i;
+    }
+    distim1 = disti;
+  }
+  return size - 1;
+}
+
+// A struct to hold pose data in floating point resolution
+struct Pose2D
+{
+  float x, y, theta;
+};
+
+/**
+ * @brief Shift the columns of a 2D Eigen Array or scalar values of
+ *    1D Eigen Array by 1 place.
+ * @param e Eigen Array
+ * @param direction direction in which Array will be shifted.
+ *     1 for shift in right direction and -1 for left direction.
+ */
+inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
+{
+  int size = e.size();
+  if(size == 1) {return;}
+  if(abs(direction) != 1) {
+    throw std::logic_error("Invalid direction, only 1 and -1 are valid values.");
+  }
+
+  if((e.cols() == 1 || e.rows() == 1) && size > 1) {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1;
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1;
+    while(start_ptr != end_ptr) {
+      *(start_ptr + direction) = *start_ptr;
+      start_ptr -= direction;
+    }
+    *(start_ptr + direction) = *start_ptr;
+  } else {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows();
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows();
+    auto span = e.rows();
+    while(start_ptr != end_ptr) {
+      std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+      start_ptr -= (direction * span);
+    }
+    std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+  }
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of final yaw angle
+ *    of the trajectory.
+ * @param last_yaws Final yaw angles of the trajectories.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points.
+ */
+inline auto normalize_yaws_between_points(
+  const Eigen::Ref<const Eigen::ArrayXf> & last_yaws,
+  const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  Eigen::ArrayXf yaws = utils::shortest_angular_distance(
+          last_yaws, yaw_between_points).abs();
+  int size = yaws.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of goal angle.
+ * @param goal_yaw Goal yaw angle.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points
+ */
+inline auto normalize_yaws_between_points(
+  const float goal_yaw, const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  int size = yaw_between_points.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = fabs(
+      angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Clamps the input between the given lower and upper bounds.
+ * @param lower_bound Lower bound.
+ * @param upper_bound Upper bound.
+ * @return Clamped output.
+ */
+inline float clamp(
+  const float lower_bound, const float upper_bound, const float input)
+{
+  return std::min(upper_bound, std::max(input, lower_bound));
+}
+
+}  // namespace mppi::utils
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_mod.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_mod.hpp
new file mode 100644
index 00000000000..0ad511a351f
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_mod.hpp	
@@ -0,0 +1,757 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+
+#include <Eigen/Dense>
+
+#include <algorithm>
+#include <chrono>
+#include <string>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "angles/angles.h"
+
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/trajectory.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "nav2_ros_common/node_utils.hpp"
+#include "nav2_core/goal_checker.hpp"
+
+#include "nav2_mppi_controller/models/optimizer_settings.hpp"
+#include "nav2_mppi_controller/models/control_sequence.hpp"
+#include "nav2_mppi_controller/models/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_mppi_controller/critic_data.hpp"
+
+#define M_PIF 3.141592653589793238462643383279502884e+00F
+#define M_PIF_2 1.5707963267948966e+00F
+
+namespace mppi::utils
+{
+/**
+ * @brief Convert data into pose
+ * @param x X position
+ * @param y Y position
+ * @param z Z position
+ * @return Pose object
+ */
+inline geometry_msgs::msg::Pose createPose(double x, double y, double z)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = x;
+  pose.position.y = y;
+  pose.position.z = z;
+  pose.orientation.w = 1;
+  pose.orientation.x = 0;
+  pose.orientation.y = 0;
+  pose.orientation.z = 0;
+  return pose;
+}
+
+/**
+ * @brief Convert data into scale
+ * @param x X scale
+ * @param y Y scale
+ * @param z Z scale
+ * @return Scale object
+ */
+inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z)
+{
+  geometry_msgs::msg::Vector3 scale;
+  scale.x = x;
+  scale.y = y;
+  scale.z = z;
+  return scale;
+}
+
+/**
+ * @brief Convert data into color
+ * @param r Red component
+ * @param g Green component
+ * @param b Blue component
+ * @param a Alpha component (transparency)
+ * @return Color object
+ */
+inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a)
+{
+  std_msgs::msg::ColorRGBA color;
+  color.r = r;
+  color.g = g;
+  color.b = b;
+  color.a = a;
+  return color;
+}
+
+/**
+ * @brief Convert data into a Maarker
+ * @param id Marker ID
+ * @param pose Marker pose
+ * @param scale Marker scale
+ * @param color Marker color
+ * @param frame Reference frame to use
+ * @return Visualization Marker
+ */
+inline visualization_msgs::msg::Marker createMarker(
+  int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale,
+  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns)
+{
+  using visualization_msgs::msg::Marker;
+  Marker marker;
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Time(0, 0);
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = Marker::SPHERE;
+  marker.action = Marker::ADD;
+
+  marker.pose = pose;
+  marker.scale = scale;
+  marker.color = color;
+  return marker;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame)
+{
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header.frame_id = frame;
+  twist.header.stamp = stamp;
+  twist.twist.linear.x = vx;
+  twist.twist.angular.z = wz;
+
+  return twist;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param vy Y velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp,
+  const std::string & frame)
+{
+  auto twist = toTwistStamped(vx, wz, stamp, frame);
+  twist.twist.linear.y = vy;
+
+  return twist;
+}
+
+inline std::unique_ptr<nav2_msgs::msg::Trajectory> toTrajectoryMsg(
+  const Eigen::ArrayXXf & trajectory,
+  const models::ControlSequence & control_sequence,
+  const double & model_dt,
+  const std_msgs::msg::Header & header)
+{
+  auto trajectory_msg = std::make_unique<nav2_msgs::msg::Trajectory>();
+  trajectory_msg->header = header;
+  trajectory_msg->points.resize(trajectory.rows());
+
+  for (int i = 0; i < trajectory.rows(); ++i) {
+    auto & curr_pt = trajectory_msg->points[i];
+    curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt);
+    curr_pt.pose.position.x = trajectory(i, 0);
+    curr_pt.pose.position.y = trajectory(i, 1);
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, trajectory(i, 2));
+    curr_pt.pose.orientation = tf2::toMsg(quat);
+    curr_pt.velocity.linear.x = control_sequence.vx(i);
+    curr_pt.velocity.angular.z = control_sequence.wz(i);
+    if (control_sequence.vy.size() > 0) {
+      curr_pt.velocity.linear.y = control_sequence.vy(i);
+    }
+  }
+
+  return trajectory_msg;
+}
+
+/**
+ * @brief Convert path to a tensor
+ * @param path Path to convert
+ * @return Path tensor
+ */
+inline models::Path toTensor(const nav_msgs::msg::Path & path)
+{
+  auto result = models::Path{};
+  result.reset(path.poses.size());
+
+  for (size_t i = 0; i < path.poses.size(); ++i) {
+    result.x(i) = path.poses[i].pose.position.x;
+    result.y(i) = path.poses[i].pose.position.y;
+    result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation);
+  }
+
+  return result;
+}
+
+/**
+ * @brief Get the last pose from a path
+ * @param path Reference to the path
+ * @return geometry_msgs::msg::Pose Last pose in the path
+ */
+inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path)
+{
+  const unsigned int path_last_idx = path.x.size() - 1;
+
+  auto last_orientation = path.yaws(path_last_idx);
+
+  tf2::Quaternion pose_orientation;
+  pose_orientation.setRPY(0.0, 0.0, last_orientation);
+
+  geometry_msgs::msg::Pose pathPose;
+  pathPose.position.x = path.x(path_last_idx);
+  pathPose.position.y = path.y(path_last_idx);
+  pathPose.orientation.x = pose_orientation.x();
+  pathPose.orientation.y = pose_orientation.y();
+  pathPose.orientation.z = pose_orientation.z();
+  pathPose.orientation.w = pose_orientation.w();
+
+  return pathPose;
+}
+
+/**
+  * @brief normalize
+  * Normalizes the angle to be -M_PIF circle to +M_PIF circle
+  * It takes and returns radians.
+  * @param angles Angles to normalize
+  * @return normalized angles
+  */
+template<typename T>
+auto normalize_angles(const T & angles)
+{
+  return (angles + M_PIF).unaryExpr([&](const float x) {
+             float remainder = std::fmod(x, 2.0f * M_PIF);
+             return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;
+             });
+}
+
+/**
+  * @brief shortest_angular_distance
+  *
+  * Given 2 angles, this returns the shortest angular
+  * difference.  The inputs and outputs are of course radians.
+  *
+  * The result
+  * would always be -pi <= result <= pi.  Adding the result
+  * to "from" will always get you an equivalent angle to "to".
+  * @param from Start angle
+  * @param to End angle
+  * @return Shortest distance between angles
+  */
+template<typename F, typename T>
+auto shortest_angular_distance(
+  const F & from,
+  const T & to)
+{
+  return normalize_angles(to - from);
+}
+
+/**
+ * @brief Evaluate furthest point idx of data.path which is
+ * nearest to some trajectory in data.trajectories
+ * @param data Data to use
+ * @return Idx of furthest path point reached by a set of trajectories
+ */
+inline size_t findPathFurthestReachedPoint(const CriticData & data)
+{
+  int traj_cols = data.trajectories.x.cols();
+  const auto traj_x = data.trajectories.x.col(traj_cols - 1);
+  const auto traj_y = data.trajectories.y.col(traj_cols - 1);
+
+  const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x;
+  const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y;
+
+  const auto dists = dx * dx + dy * dy;
+
+  int max_id_by_trajectories = 0, min_id_by_path = 0;
+  float min_distance_by_path = std::numeric_limits<float>::max();
+  size_t n_rows = dists.rows();
+  size_t n_cols = dists.cols();
+  for (size_t i = 0; i != n_rows; i++) {
+    min_id_by_path = 0;
+    min_distance_by_path = std::numeric_limits<float>::max();
+    for (size_t j = max_id_by_trajectories; j != n_cols; j++) {
+      const float cur_dist = dists(i, j);
+      if (cur_dist < min_distance_by_path) {
+        min_distance_by_path = cur_dist;
+        min_id_by_path = j;
+      }
+    }
+    max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path);
+  }
+  return max_id_by_trajectories;
+}
+
+/**
+ * @brief evaluate path furthest point if it is not set
+ * @param data Data to use
+ */
+inline void setPathFurthestPointIfNotSet(CriticData & data)
+{
+  if (!data.furthest_reached_path_point) {
+    data.furthest_reached_path_point = findPathFurthestReachedPoint(data);
+  }
+}
+
+/**
+ * @brief evaluate path costs
+ * @param data Data to use
+ */
+inline void findPathCosts(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  auto * costmap = costmap_ros->getCostmap();
+  unsigned int map_x, map_y;
+  const size_t path_segments_count = data.path.x.size() - 1;
+  data.path_pts_valid = std::vector<bool>(path_segments_count, false);
+  const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown();
+  for (unsigned int idx = 0; idx < path_segments_count; idx++) {
+    if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) {
+      (*data.path_pts_valid)[idx] = false;
+      continue;
+    }
+
+    switch (costmap->getCost(map_x, map_y)) {
+      case (nav2_costmap_2d::LETHAL_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::NO_INFORMATION):
+        (*data.path_pts_valid)[idx] = tracking_unknown ? true : false;
+        continue;
+    }
+
+    (*data.path_pts_valid)[idx] = true;
+  }
+}
+
+/**
+ * @brief evaluate path costs if it is not set
+ * @param data Data to use
+ */
+inline void setPathCostsIfNotSet(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  if (!data.path_pts_valid) {
+    findPathCosts(data, costmap_ros);
+  }
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param forward_preference If reversing direction is valid
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference)
+{
+  float pose_x = pose.position.x;
+  float pose_y = pose.position.y;
+  float pose_yaw = tf2::getYaw(pose.orientation);
+
+  float yaw = atan2f(point_y - pose_y, point_x - pose_x);
+
+  // If no preference for forward, return smallest angle either in heading or 180 of heading
+  if (!forward_preference) {
+    return std::min(
+      fabs(angles::shortest_angular_distance(yaw, pose_yaw)),
+      fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF))));
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param point_yaw Yaw of the point to consider along Z axis
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose,
+  double point_x, double point_y, double point_yaw)
+{
+  float pose_x = static_cast<float>(pose.position.x);
+  float pose_y = static_cast<float>(pose.position.y);
+  float pose_yaw = static_cast<float>(tf2::getYaw(pose.orientation));
+
+  float yaw = atan2f(static_cast<float>(point_y) - pose_y, static_cast<float>(point_x) - pose_x);
+
+  if (fabs(angles::shortest_angular_distance(yaw, static_cast<float>(point_yaw))) > M_PIF_2) {
+    yaw = angles::normalize_angle(yaw + M_PIF);
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief Apply Savisky-Golay filter to optimal trajectory
+ * @param control_sequence Sequence to apply filter to
+ * @param control_history Recent set of controls for edge-case handling
+ * @param Settings Settings to use
+ */
+inline void savitskyGolayFilter(
+  models::ControlSequence & control_sequence,
+  std::array<mppi::models::Control, 4> & control_history,
+  const models::OptimizerSettings & settings)
+{
+  // Savitzky-Golay Quadratic, 9-point Coefficients
+  Eigen::Array<float, 9, 1> filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f,
+    -21.0f};
+  filter /= 231.0f;
+
+  // Too short to smooth meaningfully
+  const unsigned int num_sequences = control_sequence.vx.size() - 1;
+  if (num_sequences < 20) {
+    return;
+  }
+
+  auto applyFilter = [&](const Eigen::Array<float, 9, 1> & data) -> float {
+      return (data * filter).eval().sum();
+    };
+
+  auto applyFilterOverAxis =
+    [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence,
+    const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
+    {
+      float pt_m4 = hist_0;
+      float pt_m3 = hist_1;
+      float pt_m2 = hist_2;
+      float pt_m1 = hist_3;
+      float pt = initial_sequence(0);
+      float pt_p1 = initial_sequence(1);
+      float pt_p2 = initial_sequence(2);
+      float pt_p3 = initial_sequence(3);
+      float pt_p4 = initial_sequence(4);
+
+      for (unsigned int idx = 0; idx != num_sequences; idx++) {
+        sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4});
+        pt_m4 = pt_m3;
+        pt_m3 = pt_m2;
+        pt_m2 = pt_m1;
+        pt_m1 = pt;
+        pt = pt_p1;
+        pt_p1 = pt_p2;
+        pt_p2 = pt_p3;
+        pt_p3 = pt_p4;
+
+        if (idx + 5 < num_sequences) {
+          pt_p4 = initial_sequence(idx + 5);
+        } else {
+          // Return the last point
+          pt_p4 = initial_sequence(num_sequences);
+        }
+      }
+    };
+
+  // Filter trajectories
+  const models::ControlSequence initial_control_sequence = control_sequence;
+  applyFilterOverAxis(
+    control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
+    control_history[1].vx, control_history[2].vx, control_history[3].vx);
+  applyFilterOverAxis(
+    control_sequence.vy, initial_control_sequence.vy, control_history[0].vy,
+    control_history[1].vy, control_history[2].vy, control_history[3].vy);
+  applyFilterOverAxis(
+    control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
+    control_history[1].wz, control_history[2].wz, control_history[3].wz);
+
+  // Update control history
+  unsigned int offset = settings.shift_control_sequence ? 1 : 0;
+  control_history[0] = control_history[1];
+  control_history[1] = control_history[2];
+  control_history[2] = control_history[3];
+  control_history[3] = {
+    control_sequence.vx(offset),
+    control_sequence.vy(offset),
+    control_sequence.wz(offset)};
+}
+
+/**
+ * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path
+ * @param path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return the first point after the inversion or rotation found in the path
+ */
+inline unsigned int findFirstPathInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
+{
+  if (path.poses.size() < 2) {
+    return path.poses.size();
+  }
+
+  unsigned int first_after_constraint = path.poses.size();
+
+  // Check for in-place rotation first (if enabled)
+  if (rotation_threshold > 0.0f) {
+    for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) {
+      float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x;
+      float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y;
+      float translation = sqrtf(dx * dx + dy * dy);
+
+      // Check if poses are at roughly the same location
+      if (translation < 1e-3) {
+        float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation);
+        float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation);
+        float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2));
+
+        // Check if this meets the minimum rotation threshold
+        if (rotation > rotation_threshold) {
+          // Found start of in-place rotation, now find where it ends
+          unsigned int end_idx = idx + 1;
+
+          // Continue while we have minimal translation (still rotating in place)
+          while (end_idx < path.poses.size() - 1) {
+            float next_dx = path.poses[end_idx + 1].pose.position.x -
+              path.poses[end_idx].pose.position.x;
+            float next_dy = path.poses[end_idx + 1].pose.position.y -
+              path.poses[end_idx].pose.position.y;
+            float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy);
+
+            if (next_translation >= 1e-3) {
+              break;  // End of in-place rotation sequence
+            }
+            end_idx++;
+          }
+
+          first_after_constraint = end_idx;
+          break;
+        }
+      }
+    }
+  }
+
+  // Check for inversion (at least 3 poses needed)
+  if (path.poses.size() >= 3) {
+    for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+      float oa_x = path.poses[idx].pose.position.x -
+        path.poses[idx - 1].pose.position.x;
+      float oa_y = path.poses[idx].pose.position.y -
+        path.poses[idx - 1].pose.position.y;
+      float ab_x = path.poses[idx + 1].pose.position.x -
+        path.poses[idx].pose.position.x;
+      float ab_y = path.poses[idx + 1].pose.position.y -
+        path.poses[idx].pose.position.y;
+
+      float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+
+      float norm_oa = hypotf(oa_x, oa_y);
+      float norm_ab = hypotf(ab_x, ab_y);
+
+      if (norm_oa < 1e-4f || norm_ab < 1e-4f) {
+        continue;
+      }
+
+      float cos_ang = dot_product / (norm_oa * norm_ab);
+      cos_ang = std::max(-1.0f, std::min(1.0f, cos_ang));
+      float angle = acosf(cos_ang);  // [0, pi]
+
+      if (rotation_threshold > 0.0f) {
+        if (angle > rotation_threshold) {
+          first_after_constraint = std::min(first_after_constraint, idx + 1);
+          break;
+        }
+      } else {
+        if (dot_product < 0.0f) {
+          first_after_constraint = std::min(first_after_constraint, idx + 1);
+          break;
+        }
+      }
+    }
+  }
+
+  return first_after_constraint;
+}
+
+/**
+ * @brief Find and remove poses after the first inversion or in-place rotation in the path
+ * @param path Path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return The location of the inversion/rotation, return 0 if none exist
+ */
+inline unsigned int removePosesAfterFirstInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
+{
+  nav_msgs::msg::Path cropped_path = path;
+
+  const unsigned int first_after_constraint = findFirstPathInversion(cropped_path, rotation_threshold);
+
+  if (first_after_constraint == path.poses.size()) {
+    return 0u;  // No constraint found
+  }
+
+  cropped_path.poses.erase(
+    cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end());
+  path = cropped_path;
+  return first_after_constraint;
+}
+
+/**
+ * @brief Compare to trajectory points to find closest path point along integrated distances
+ * @param vec Vect to check
+ * @return dist Distance to look for
+ * @return init Starting index to indec from
+ */
+inline unsigned int findClosestPathPt(
+  const std::vector<float> & vec, const float dist, const unsigned int init = 0u)
+{
+  float distim1 = init != 0u ? vec[init] : 0.0f;  // First is 0, no accumulated distance yet
+  float disti = 0.0f;
+  const unsigned int size = vec.size();
+  for (unsigned int i = init + 1; i != size; i++) {
+    disti = vec[i];
+    if (disti > dist) {
+      if (i > 0 && dist - distim1 < disti - dist) {
+        return i - 1;
+      }
+      return i;
+    }
+    distim1 = disti;
+  }
+  return size - 1;
+}
+
+// A struct to hold pose data in floating point resolution
+struct Pose2D
+{
+  float x, y, theta;
+};
+
+/**
+ * @brief Shift the columns of a 2D Eigen Array or scalar values of
+ *    1D Eigen Array by 1 place.
+ * @param e Eigen Array
+ * @param direction direction in which Array will be shifted.
+ *     1 for shift in right direction and -1 for left direction.
+ */
+inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
+{
+  int size = e.size();
+  if(size == 1) {return;}
+  if(abs(direction) != 1) {
+    throw std::logic_error("Invalid direction, only 1 and -1 are valid values.");
+  }
+
+  if((e.cols() == 1 || e.rows() == 1) && size > 1) {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1;
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1;
+    while(start_ptr != end_ptr) {
+      *(start_ptr + direction) = *start_ptr;
+      start_ptr -= direction;
+    }
+    *(start_ptr + direction) = *start_ptr;
+  } else {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows();
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows();
+    auto span = e.rows();
+    while(start_ptr != end_ptr) {
+      std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+      start_ptr -= (direction * span);
+    }
+    std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+  }
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of final yaw angle
+ *    of the trajectory.
+ * @param last_yaws Final yaw angles of the trajectories.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points.
+ */
+inline auto normalize_yaws_between_points(
+  const Eigen::Ref<const Eigen::ArrayXf> & last_yaws,
+  const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  Eigen::ArrayXf yaws = utils::shortest_angular_distance(
+          last_yaws, yaw_between_points).abs();
+  int size = yaws.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of goal angle.
+ * @param goal_yaw Goal yaw angle.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points
+ */
+inline auto normalize_yaws_between_points(
+  const float goal_yaw, const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  int size = yaw_between_points.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = fabs(
+      angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Clamps the input between the given lower and upper bounds.
+ * @param lower_bound Lower bound.
+ * @param upper_bound Upper bound.
+ * @return Clamped output.
+ */
+inline float clamp(
+  const float lower_bound, const float upper_bound, const float input)
+{
+  return std::min(upper_bound, std::max(input, lower_bound));
+}
+
+}  // namespace mppi::utils
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_original.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_original.hpp
new file mode 100644
index 00000000000..e2f6a925d36
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils copy_original.hpp	
@@ -0,0 +1,705 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+
+#include <Eigen/Dense>
+
+#include <algorithm>
+#include <chrono>
+#include <string>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "angles/angles.h"
+
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/trajectory.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "nav2_ros_common/node_utils.hpp"
+#include "nav2_core/goal_checker.hpp"
+
+#include "nav2_mppi_controller/models/optimizer_settings.hpp"
+#include "nav2_mppi_controller/models/control_sequence.hpp"
+#include "nav2_mppi_controller/models/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_mppi_controller/critic_data.hpp"
+
+#define M_PIF 3.141592653589793238462643383279502884e+00F
+#define M_PIF_2 1.5707963267948966e+00F
+
+namespace mppi::utils
+{
+/**
+ * @brief Convert data into pose
+ * @param x X position
+ * @param y Y position
+ * @param z Z position
+ * @return Pose object
+ */
+inline geometry_msgs::msg::Pose createPose(double x, double y, double z)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = x;
+  pose.position.y = y;
+  pose.position.z = z;
+  pose.orientation.w = 1;
+  pose.orientation.x = 0;
+  pose.orientation.y = 0;
+  pose.orientation.z = 0;
+  return pose;
+}
+
+/**
+ * @brief Convert data into scale
+ * @param x X scale
+ * @param y Y scale
+ * @param z Z scale
+ * @return Scale object
+ */
+inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z)
+{
+  geometry_msgs::msg::Vector3 scale;
+  scale.x = x;
+  scale.y = y;
+  scale.z = z;
+  return scale;
+}
+
+/**
+ * @brief Convert data into color
+ * @param r Red component
+ * @param g Green component
+ * @param b Blue component
+ * @param a Alpha component (transparency)
+ * @return Color object
+ */
+inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a)
+{
+  std_msgs::msg::ColorRGBA color;
+  color.r = r;
+  color.g = g;
+  color.b = b;
+  color.a = a;
+  return color;
+}
+
+/**
+ * @brief Convert data into a Maarker
+ * @param id Marker ID
+ * @param pose Marker pose
+ * @param scale Marker scale
+ * @param color Marker color
+ * @param frame Reference frame to use
+ * @return Visualization Marker
+ */
+inline visualization_msgs::msg::Marker createMarker(
+  int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale,
+  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns)
+{
+  using visualization_msgs::msg::Marker;
+  Marker marker;
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Time(0, 0);
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = Marker::SPHERE;
+  marker.action = Marker::ADD;
+
+  marker.pose = pose;
+  marker.scale = scale;
+  marker.color = color;
+  return marker;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame)
+{
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header.frame_id = frame;
+  twist.header.stamp = stamp;
+  twist.twist.linear.x = vx;
+  twist.twist.angular.z = wz;
+
+  return twist;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param vy Y velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp,
+  const std::string & frame)
+{
+  auto twist = toTwistStamped(vx, wz, stamp, frame);
+  twist.twist.linear.y = vy;
+
+  return twist;
+}
+
+inline std::unique_ptr<nav2_msgs::msg::Trajectory> toTrajectoryMsg(
+  const Eigen::ArrayXXf & trajectory,
+  const models::ControlSequence & control_sequence,
+  const double & model_dt,
+  const std_msgs::msg::Header & header)
+{
+  auto trajectory_msg = std::make_unique<nav2_msgs::msg::Trajectory>();
+  trajectory_msg->header = header;
+  trajectory_msg->points.resize(trajectory.rows());
+
+  for (int i = 0; i < trajectory.rows(); ++i) {
+    auto & curr_pt = trajectory_msg->points[i];
+    curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt);
+    curr_pt.pose.position.x = trajectory(i, 0);
+    curr_pt.pose.position.y = trajectory(i, 1);
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, trajectory(i, 2));
+    curr_pt.pose.orientation = tf2::toMsg(quat);
+    curr_pt.velocity.linear.x = control_sequence.vx(i);
+    curr_pt.velocity.angular.z = control_sequence.wz(i);
+    if (control_sequence.vy.size() > 0) {
+      curr_pt.velocity.linear.y = control_sequence.vy(i);
+    }
+  }
+
+  return trajectory_msg;
+}
+
+/**
+ * @brief Convert path to a tensor
+ * @param path Path to convert
+ * @return Path tensor
+ */
+inline models::Path toTensor(const nav_msgs::msg::Path & path)
+{
+  auto result = models::Path{};
+  result.reset(path.poses.size());
+
+  for (size_t i = 0; i < path.poses.size(); ++i) {
+    result.x(i) = path.poses[i].pose.position.x;
+    result.y(i) = path.poses[i].pose.position.y;
+    result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation);
+  }
+
+  return result;
+}
+
+/**
+ * @brief Get the last pose from a path
+ * @param path Reference to the path
+ * @return geometry_msgs::msg::Pose Last pose in the path
+ */
+inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path)
+{
+  const unsigned int path_last_idx = path.x.size() - 1;
+
+  auto last_orientation = path.yaws(path_last_idx);
+
+  tf2::Quaternion pose_orientation;
+  pose_orientation.setRPY(0.0, 0.0, last_orientation);
+
+  geometry_msgs::msg::Pose pathPose;
+  pathPose.position.x = path.x(path_last_idx);
+  pathPose.position.y = path.y(path_last_idx);
+  pathPose.orientation.x = pose_orientation.x();
+  pathPose.orientation.y = pose_orientation.y();
+  pathPose.orientation.z = pose_orientation.z();
+  pathPose.orientation.w = pose_orientation.w();
+
+  return pathPose;
+}
+
+/**
+  * @brief normalize
+  * Normalizes the angle to be -M_PIF circle to +M_PIF circle
+  * It takes and returns radians.
+  * @param angles Angles to normalize
+  * @return normalized angles
+  */
+template<typename T>
+auto normalize_angles(const T & angles)
+{
+  return (angles + M_PIF).unaryExpr([&](const float x) {
+             float remainder = std::fmod(x, 2.0f * M_PIF);
+             return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;
+             });
+}
+
+/**
+  * @brief shortest_angular_distance
+  *
+  * Given 2 angles, this returns the shortest angular
+  * difference.  The inputs and outputs are of course radians.
+  *
+  * The result
+  * would always be -pi <= result <= pi.  Adding the result
+  * to "from" will always get you an equivalent angle to "to".
+  * @param from Start angle
+  * @param to End angle
+  * @return Shortest distance between angles
+  */
+template<typename F, typename T>
+auto shortest_angular_distance(
+  const F & from,
+  const T & to)
+{
+  return normalize_angles(to - from);
+}
+
+/**
+ * @brief Evaluate furthest point idx of data.path which is
+ * nearest to some trajectory in data.trajectories
+ * @param data Data to use
+ * @return Idx of furthest path point reached by a set of trajectories
+ */
+inline size_t findPathFurthestReachedPoint(const CriticData & data)
+{
+  int traj_cols = data.trajectories.x.cols();
+  const auto traj_x = data.trajectories.x.col(traj_cols - 1);
+  const auto traj_y = data.trajectories.y.col(traj_cols - 1);
+
+  const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x;
+  const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y;
+
+  const auto dists = dx * dx + dy * dy;
+
+  int max_id_by_trajectories = 0, min_id_by_path = 0;
+  float min_distance_by_path = std::numeric_limits<float>::max();
+  size_t n_rows = dists.rows();
+  size_t n_cols = dists.cols();
+  for (size_t i = 0; i != n_rows; i++) {
+    // Skip trajectories in collision
+    bool in_collision = data.trajectories_in_collision && 
+                        i < data.trajectories_in_collision->size() && 
+                        (*data.trajectories_in_collision)[i];
+    if (in_collision) {
+      continue;
+    }
+    min_id_by_path = 0;
+    min_distance_by_path = std::numeric_limits<float>::max();
+    // Search all path points to find the true closest point
+    for (size_t j = 0; j != n_cols; j++) {
+      const float cur_dist = dists(i, j);
+      if (cur_dist < min_distance_by_path) {
+        min_distance_by_path = cur_dist;
+        min_id_by_path = j;
+        // If we've found the last path point as closest, can't do better for this trajectory
+        if (min_id_by_path >= static_cast<int>(n_cols - 1)) {
+          break;
+        }
+      }
+    }
+    max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path);
+    // Early exit: if we've reached the last path point, we can't do better
+    if (max_id_by_trajectories >= static_cast<int>(n_cols - 1)) {
+      break;
+    }
+  }
+  return max_id_by_trajectories;
+}
+
+/**
+ * @brief evaluate path furthest point if it is not set
+ * @param data Data to use
+ */
+inline void setPathFurthestPointIfNotSet(CriticData & data)
+{
+  if (!data.furthest_reached_path_point) {
+    data.furthest_reached_path_point = findPathFurthestReachedPoint(data);
+  }
+}
+
+/**
+ * @brief evaluate path costs
+ * @param data Data to use
+ */
+inline void findPathCosts(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  auto * costmap = costmap_ros->getCostmap();
+  unsigned int map_x, map_y;
+  const size_t path_segments_count = data.path.x.size() - 1;
+  data.path_pts_valid = std::vector<bool>(path_segments_count, false);
+  const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown();
+  for (unsigned int idx = 0; idx < path_segments_count; idx++) {
+    if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) {
+      (*data.path_pts_valid)[idx] = false;
+      continue;
+    }
+
+    switch (costmap->getCost(map_x, map_y)) {
+      case (nav2_costmap_2d::LETHAL_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::NO_INFORMATION):
+        (*data.path_pts_valid)[idx] = tracking_unknown ? true : false;
+        continue;
+    }
+
+    (*data.path_pts_valid)[idx] = true;
+  }
+}
+
+/**
+ * @brief evaluate path costs if it is not set
+ * @param data Data to use
+ */
+inline void setPathCostsIfNotSet(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  if (!data.path_pts_valid) {
+    findPathCosts(data, costmap_ros);
+  }
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param forward_preference If reversing direction is valid
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference)
+{
+  float pose_x = pose.position.x;
+  float pose_y = pose.position.y;
+  float pose_yaw = tf2::getYaw(pose.orientation);
+
+  float yaw = atan2f(point_y - pose_y, point_x - pose_x);
+
+  // If no preference for forward, return smallest angle either in heading or 180 of heading
+  if (!forward_preference) {
+    return std::min(
+      fabs(angles::shortest_angular_distance(yaw, pose_yaw)),
+      fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF))));
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param point_yaw Yaw of the point to consider along Z axis
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose,
+  double point_x, double point_y, double point_yaw)
+{
+  float pose_x = static_cast<float>(pose.position.x);
+  float pose_y = static_cast<float>(pose.position.y);
+  float pose_yaw = static_cast<float>(tf2::getYaw(pose.orientation));
+
+  float yaw = atan2f(static_cast<float>(point_y) - pose_y, static_cast<float>(point_x) - pose_x);
+
+  if (fabs(angles::shortest_angular_distance(yaw, static_cast<float>(point_yaw))) > M_PIF_2) {
+    yaw = angles::normalize_angle(yaw + M_PIF);
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief Apply Savisky-Golay filter to optimal trajectory
+ * @param control_sequence Sequence to apply filter to
+ * @param control_history Recent set of controls for edge-case handling
+ * @param Settings Settings to use
+ */
+inline void savitskyGolayFilter(
+  models::ControlSequence & control_sequence,
+  std::array<mppi::models::Control, 4> & control_history,
+  const models::OptimizerSettings & settings)
+{
+  // Savitzky-Golay Quadratic, 9-point Coefficients
+  Eigen::Array<float, 9, 1> filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f,
+    -21.0f};
+  filter /= 231.0f;
+
+  // Too short to smooth meaningfully
+  const unsigned int num_sequences = control_sequence.vx.size() - 1;
+  if (num_sequences < 20) {
+    return;
+  }
+
+  auto applyFilter = [&](const Eigen::Array<float, 9, 1> & data) -> float {
+      return (data * filter).eval().sum();
+    };
+
+  auto applyFilterOverAxis =
+    [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence,
+    const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
+    {
+      float pt_m4 = hist_0;
+      float pt_m3 = hist_1;
+      float pt_m2 = hist_2;
+      float pt_m1 = hist_3;
+      float pt = initial_sequence(0);
+      float pt_p1 = initial_sequence(1);
+      float pt_p2 = initial_sequence(2);
+      float pt_p3 = initial_sequence(3);
+      float pt_p4 = initial_sequence(4);
+
+      for (unsigned int idx = 0; idx != num_sequences; idx++) {
+        sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4});
+        pt_m4 = pt_m3;
+        pt_m3 = pt_m2;
+        pt_m2 = pt_m1;
+        pt_m1 = pt;
+        pt = pt_p1;
+        pt_p1 = pt_p2;
+        pt_p2 = pt_p3;
+        pt_p3 = pt_p4;
+
+        if (idx + 5 < num_sequences) {
+          pt_p4 = initial_sequence(idx + 5);
+        } else {
+          // Return the last point
+          pt_p4 = initial_sequence(num_sequences);
+        }
+      }
+    };
+
+  // Filter trajectories
+  const models::ControlSequence initial_control_sequence = control_sequence;
+  applyFilterOverAxis(
+    control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
+    control_history[1].vx, control_history[2].vx, control_history[3].vx);
+  applyFilterOverAxis(
+    control_sequence.vy, initial_control_sequence.vy, control_history[0].vy,
+    control_history[1].vy, control_history[2].vy, control_history[3].vy);
+  applyFilterOverAxis(
+    control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
+    control_history[1].wz, control_history[2].wz, control_history[3].wz);
+
+  // Update control history
+  unsigned int offset = settings.shift_control_sequence ? 1 : 0;
+  control_history[0] = control_history[1];
+  control_history[1] = control_history[2];
+  control_history[2] = control_history[3];
+  control_history[3] = {
+    control_sequence.vx(offset),
+    control_sequence.vy(offset),
+    control_sequence.wz(offset)};
+}
+
+/**
+ * @brief Find the iterator of the first pose at which there is an inversion on the path,
+ * @param path to check for inversion
+ * @return the first point after the inversion found in the path
+ */
+inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
+{
+  // At least 3 poses for a possible inversion
+  if (path.poses.size() < 3) {
+    return path.poses.size();
+  }
+
+  // Iterating through the path to determine the position of the path inversion
+  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+    // We have two vectors for the dot product OA and AB. Determining the vectors.
+    float oa_x = path.poses[idx].pose.position.x -
+      path.poses[idx - 1].pose.position.x;
+    float oa_y = path.poses[idx].pose.position.y -
+      path.poses[idx - 1].pose.position.y;
+    float ab_x = path.poses[idx + 1].pose.position.x -
+      path.poses[idx].pose.position.x;
+    float ab_y = path.poses[idx + 1].pose.position.y -
+      path.poses[idx].pose.position.y;
+
+    // Checking for the existence of cusp, in the path, using the dot product.
+    float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+    if (dot_product < 0.0f) {
+      return idx + 1;
+    }
+  }
+
+  return path.poses.size();
+}
+
+/**
+ * @brief Find and remove poses after the first inversion in the path
+ * @param path to check for inversion
+ * @return The location of the inversion, return 0 if none exist
+ */
+inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
+{
+  nav_msgs::msg::Path cropped_path = path;
+  const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
+  if (first_after_inversion == path.poses.size()) {
+    return 0u;
+  }
+
+  cropped_path.poses.erase(
+    cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
+  path = cropped_path;
+  return first_after_inversion;
+}
+
+/**
+ * @brief Compare to trajectory points to find closest path point along integrated distances
+ * @param vec Vect to check
+ * @return dist Distance to look for
+ * @return init Starting index to indec from
+ */
+inline unsigned int findClosestPathPt(
+  const std::vector<float> & vec, const float dist, const unsigned int init = 0u)
+{
+  float distim1 = init != 0u ? vec[init] : 0.0f;  // First is 0, no accumulated distance yet
+  float disti = 0.0f;
+  const unsigned int size = vec.size();
+  for (unsigned int i = init + 1; i != size; i++) {
+    disti = vec[i];
+    if (disti > dist) {
+      if (i > 0 && dist - distim1 < disti - dist) {
+        return i - 1;
+      }
+      return i;
+    }
+    distim1 = disti;
+  }
+  return size - 1;
+}
+
+// A struct to hold pose data in floating point resolution
+struct Pose2D
+{
+  float x, y, theta;
+};
+
+/**
+ * @brief Shift the columns of a 2D Eigen Array or scalar values of
+ *    1D Eigen Array by 1 place.
+ * @param e Eigen Array
+ * @param direction direction in which Array will be shifted.
+ *     1 for shift in right direction and -1 for left direction.
+ */
+inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
+{
+  int size = e.size();
+  if(size == 1) {return;}
+  if(abs(direction) != 1) {
+    throw std::logic_error("Invalid direction, only 1 and -1 are valid values.");
+  }
+
+  if((e.cols() == 1 || e.rows() == 1) && size > 1) {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1;
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1;
+    while(start_ptr != end_ptr) {
+      *(start_ptr + direction) = *start_ptr;
+      start_ptr -= direction;
+    }
+    *(start_ptr + direction) = *start_ptr;
+  } else {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows();
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows();
+    auto span = e.rows();
+    while(start_ptr != end_ptr) {
+      std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+      start_ptr -= (direction * span);
+    }
+    std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+  }
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of final yaw angle
+ *    of the trajectory.
+ * @param last_yaws Final yaw angles of the trajectories.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points.
+ */
+inline auto normalize_yaws_between_points(
+  const Eigen::Ref<const Eigen::ArrayXf> & last_yaws,
+  const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  Eigen::ArrayXf yaws = utils::shortest_angular_distance(
+          last_yaws, yaw_between_points).abs();
+  int size = yaws.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of goal angle.
+ * @param goal_yaw Goal yaw angle.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points
+ */
+inline auto normalize_yaws_between_points(
+  const float goal_yaw, const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  int size = yaw_between_points.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = fabs(
+      angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Clamps the input between the given lower and upper bounds.
+ * @param lower_bound Lower bound.
+ * @param upper_bound Upper bound.
+ * @return Clamped output.
+ */
+inline float clamp(
+  const float lower_bound, const float upper_bound, const float input)
+{
+  return std::min(upper_bound, std::max(input, lower_bound));
+}
+
+}  // namespace mppi::utils
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
index 4a3cc6b7c06..0ad511a351f 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -511,56 +511,124 @@ inline void savitskyGolayFilter(
 }
 
 /**
- * @brief Find the iterator of the first pose at which there is an inversion on the path,
- * @param path to check for inversion
- * @return the first point after the inversion found in the path
+ * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path
+ * @param path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return the first point after the inversion or rotation found in the path
  */
-inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
+inline unsigned int findFirstPathInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
 {
-  // At least 3 poses for a possible inversion
-  if (path.poses.size() < 3) {
+  if (path.poses.size() < 2) {
     return path.poses.size();
   }
 
-  // Iterating through the path to determine the position of the path inversion
-  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
-    // We have two vectors for the dot product OA and AB. Determining the vectors.
-    float oa_x = path.poses[idx].pose.position.x -
-      path.poses[idx - 1].pose.position.x;
-    float oa_y = path.poses[idx].pose.position.y -
-      path.poses[idx - 1].pose.position.y;
-    float ab_x = path.poses[idx + 1].pose.position.x -
-      path.poses[idx].pose.position.x;
-    float ab_y = path.poses[idx + 1].pose.position.y -
-      path.poses[idx].pose.position.y;
-
-    // Checking for the existence of cusp, in the path, using the dot product.
-    float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
-    if (dot_product < 0.0f) {
-      return idx + 1;
+  unsigned int first_after_constraint = path.poses.size();
+
+  // Check for in-place rotation first (if enabled)
+  if (rotation_threshold > 0.0f) {
+    for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) {
+      float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x;
+      float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y;
+      float translation = sqrtf(dx * dx + dy * dy);
+
+      // Check if poses are at roughly the same location
+      if (translation < 1e-3) {
+        float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation);
+        float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation);
+        float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2));
+
+        // Check if this meets the minimum rotation threshold
+        if (rotation > rotation_threshold) {
+          // Found start of in-place rotation, now find where it ends
+          unsigned int end_idx = idx + 1;
+
+          // Continue while we have minimal translation (still rotating in place)
+          while (end_idx < path.poses.size() - 1) {
+            float next_dx = path.poses[end_idx + 1].pose.position.x -
+              path.poses[end_idx].pose.position.x;
+            float next_dy = path.poses[end_idx + 1].pose.position.y -
+              path.poses[end_idx].pose.position.y;
+            float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy);
+
+            if (next_translation >= 1e-3) {
+              break;  // End of in-place rotation sequence
+            }
+            end_idx++;
+          }
+
+          first_after_constraint = end_idx;
+          break;
+        }
+      }
     }
   }
 
-  return path.poses.size();
+  // Check for inversion (at least 3 poses needed)
+  if (path.poses.size() >= 3) {
+    for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+      float oa_x = path.poses[idx].pose.position.x -
+        path.poses[idx - 1].pose.position.x;
+      float oa_y = path.poses[idx].pose.position.y -
+        path.poses[idx - 1].pose.position.y;
+      float ab_x = path.poses[idx + 1].pose.position.x -
+        path.poses[idx].pose.position.x;
+      float ab_y = path.poses[idx + 1].pose.position.y -
+        path.poses[idx].pose.position.y;
+
+      float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+
+      float norm_oa = hypotf(oa_x, oa_y);
+      float norm_ab = hypotf(ab_x, ab_y);
+
+      if (norm_oa < 1e-4f || norm_ab < 1e-4f) {
+        continue;
+      }
+
+      float cos_ang = dot_product / (norm_oa * norm_ab);
+      cos_ang = std::max(-1.0f, std::min(1.0f, cos_ang));
+      float angle = acosf(cos_ang);  // [0, pi]
+
+      if (rotation_threshold > 0.0f) {
+        if (angle > rotation_threshold) {
+          first_after_constraint = std::min(first_after_constraint, idx + 1);
+          break;
+        }
+      } else {
+        if (dot_product < 0.0f) {
+          first_after_constraint = std::min(first_after_constraint, idx + 1);
+          break;
+        }
+      }
+    }
+  }
+
+  return first_after_constraint;
 }
 
 /**
- * @brief Find and remove poses after the first inversion in the path
- * @param path to check for inversion
- * @return The location of the inversion, return 0 if none exist
+ * @brief Find and remove poses after the first inversion or in-place rotation in the path
+ * @param path Path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return The location of the inversion/rotation, return 0 if none exist
  */
-inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
+inline unsigned int removePosesAfterFirstInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
 {
   nav_msgs::msg::Path cropped_path = path;
-  const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
-  if (first_after_inversion == path.poses.size()) {
-    return 0u;
+
+  const unsigned int first_after_constraint = findFirstPathInversion(cropped_path, rotation_threshold);
+
+  if (first_after_constraint == path.poses.size()) {
+    return 0u;  // No constraint found
   }
 
   cropped_path.poses.erase(
-    cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
+    cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end());
   path = cropped_path;
-  return first_after_inversion;
+  return first_after_constraint;
 }
 
 /**
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils_mod_final.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils_mod_final.hpp
new file mode 100644
index 00000000000..0ad511a351f
--- /dev/null
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils_mod_final.hpp
@@ -0,0 +1,757 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
+
+#include <Eigen/Dense>
+
+#include <algorithm>
+#include <chrono>
+#include <string>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "angles/angles.h"
+
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/trajectory.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "nav2_ros_common/node_utils.hpp"
+#include "nav2_core/goal_checker.hpp"
+
+#include "nav2_mppi_controller/models/optimizer_settings.hpp"
+#include "nav2_mppi_controller/models/control_sequence.hpp"
+#include "nav2_mppi_controller/models/path.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "nav2_mppi_controller/critic_data.hpp"
+
+#define M_PIF 3.141592653589793238462643383279502884e+00F
+#define M_PIF_2 1.5707963267948966e+00F
+
+namespace mppi::utils
+{
+/**
+ * @brief Convert data into pose
+ * @param x X position
+ * @param y Y position
+ * @param z Z position
+ * @return Pose object
+ */
+inline geometry_msgs::msg::Pose createPose(double x, double y, double z)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = x;
+  pose.position.y = y;
+  pose.position.z = z;
+  pose.orientation.w = 1;
+  pose.orientation.x = 0;
+  pose.orientation.y = 0;
+  pose.orientation.z = 0;
+  return pose;
+}
+
+/**
+ * @brief Convert data into scale
+ * @param x X scale
+ * @param y Y scale
+ * @param z Z scale
+ * @return Scale object
+ */
+inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z)
+{
+  geometry_msgs::msg::Vector3 scale;
+  scale.x = x;
+  scale.y = y;
+  scale.z = z;
+  return scale;
+}
+
+/**
+ * @brief Convert data into color
+ * @param r Red component
+ * @param g Green component
+ * @param b Blue component
+ * @param a Alpha component (transparency)
+ * @return Color object
+ */
+inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a)
+{
+  std_msgs::msg::ColorRGBA color;
+  color.r = r;
+  color.g = g;
+  color.b = b;
+  color.a = a;
+  return color;
+}
+
+/**
+ * @brief Convert data into a Maarker
+ * @param id Marker ID
+ * @param pose Marker pose
+ * @param scale Marker scale
+ * @param color Marker color
+ * @param frame Reference frame to use
+ * @return Visualization Marker
+ */
+inline visualization_msgs::msg::Marker createMarker(
+  int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale,
+  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns)
+{
+  using visualization_msgs::msg::Marker;
+  Marker marker;
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Time(0, 0);
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = Marker::SPHERE;
+  marker.action = Marker::ADD;
+
+  marker.pose = pose;
+  marker.scale = scale;
+  marker.color = color;
+  return marker;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame)
+{
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header.frame_id = frame;
+  twist.header.stamp = stamp;
+  twist.twist.linear.x = vx;
+  twist.twist.angular.z = wz;
+
+  return twist;
+}
+
+/**
+ * @brief Convert data into TwistStamped
+ * @param vx X velocity
+ * @param vy Y velocity
+ * @param wz Angular velocity
+ * @param stamp Timestamp
+ * @param frame Reference frame to use
+ */
+inline geometry_msgs::msg::TwistStamped toTwistStamped(
+  float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp,
+  const std::string & frame)
+{
+  auto twist = toTwistStamped(vx, wz, stamp, frame);
+  twist.twist.linear.y = vy;
+
+  return twist;
+}
+
+inline std::unique_ptr<nav2_msgs::msg::Trajectory> toTrajectoryMsg(
+  const Eigen::ArrayXXf & trajectory,
+  const models::ControlSequence & control_sequence,
+  const double & model_dt,
+  const std_msgs::msg::Header & header)
+{
+  auto trajectory_msg = std::make_unique<nav2_msgs::msg::Trajectory>();
+  trajectory_msg->header = header;
+  trajectory_msg->points.resize(trajectory.rows());
+
+  for (int i = 0; i < trajectory.rows(); ++i) {
+    auto & curr_pt = trajectory_msg->points[i];
+    curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt);
+    curr_pt.pose.position.x = trajectory(i, 0);
+    curr_pt.pose.position.y = trajectory(i, 1);
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, trajectory(i, 2));
+    curr_pt.pose.orientation = tf2::toMsg(quat);
+    curr_pt.velocity.linear.x = control_sequence.vx(i);
+    curr_pt.velocity.angular.z = control_sequence.wz(i);
+    if (control_sequence.vy.size() > 0) {
+      curr_pt.velocity.linear.y = control_sequence.vy(i);
+    }
+  }
+
+  return trajectory_msg;
+}
+
+/**
+ * @brief Convert path to a tensor
+ * @param path Path to convert
+ * @return Path tensor
+ */
+inline models::Path toTensor(const nav_msgs::msg::Path & path)
+{
+  auto result = models::Path{};
+  result.reset(path.poses.size());
+
+  for (size_t i = 0; i < path.poses.size(); ++i) {
+    result.x(i) = path.poses[i].pose.position.x;
+    result.y(i) = path.poses[i].pose.position.y;
+    result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation);
+  }
+
+  return result;
+}
+
+/**
+ * @brief Get the last pose from a path
+ * @param path Reference to the path
+ * @return geometry_msgs::msg::Pose Last pose in the path
+ */
+inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path)
+{
+  const unsigned int path_last_idx = path.x.size() - 1;
+
+  auto last_orientation = path.yaws(path_last_idx);
+
+  tf2::Quaternion pose_orientation;
+  pose_orientation.setRPY(0.0, 0.0, last_orientation);
+
+  geometry_msgs::msg::Pose pathPose;
+  pathPose.position.x = path.x(path_last_idx);
+  pathPose.position.y = path.y(path_last_idx);
+  pathPose.orientation.x = pose_orientation.x();
+  pathPose.orientation.y = pose_orientation.y();
+  pathPose.orientation.z = pose_orientation.z();
+  pathPose.orientation.w = pose_orientation.w();
+
+  return pathPose;
+}
+
+/**
+  * @brief normalize
+  * Normalizes the angle to be -M_PIF circle to +M_PIF circle
+  * It takes and returns radians.
+  * @param angles Angles to normalize
+  * @return normalized angles
+  */
+template<typename T>
+auto normalize_angles(const T & angles)
+{
+  return (angles + M_PIF).unaryExpr([&](const float x) {
+             float remainder = std::fmod(x, 2.0f * M_PIF);
+             return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;
+             });
+}
+
+/**
+  * @brief shortest_angular_distance
+  *
+  * Given 2 angles, this returns the shortest angular
+  * difference.  The inputs and outputs are of course radians.
+  *
+  * The result
+  * would always be -pi <= result <= pi.  Adding the result
+  * to "from" will always get you an equivalent angle to "to".
+  * @param from Start angle
+  * @param to End angle
+  * @return Shortest distance between angles
+  */
+template<typename F, typename T>
+auto shortest_angular_distance(
+  const F & from,
+  const T & to)
+{
+  return normalize_angles(to - from);
+}
+
+/**
+ * @brief Evaluate furthest point idx of data.path which is
+ * nearest to some trajectory in data.trajectories
+ * @param data Data to use
+ * @return Idx of furthest path point reached by a set of trajectories
+ */
+inline size_t findPathFurthestReachedPoint(const CriticData & data)
+{
+  int traj_cols = data.trajectories.x.cols();
+  const auto traj_x = data.trajectories.x.col(traj_cols - 1);
+  const auto traj_y = data.trajectories.y.col(traj_cols - 1);
+
+  const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x;
+  const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y;
+
+  const auto dists = dx * dx + dy * dy;
+
+  int max_id_by_trajectories = 0, min_id_by_path = 0;
+  float min_distance_by_path = std::numeric_limits<float>::max();
+  size_t n_rows = dists.rows();
+  size_t n_cols = dists.cols();
+  for (size_t i = 0; i != n_rows; i++) {
+    min_id_by_path = 0;
+    min_distance_by_path = std::numeric_limits<float>::max();
+    for (size_t j = max_id_by_trajectories; j != n_cols; j++) {
+      const float cur_dist = dists(i, j);
+      if (cur_dist < min_distance_by_path) {
+        min_distance_by_path = cur_dist;
+        min_id_by_path = j;
+      }
+    }
+    max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path);
+  }
+  return max_id_by_trajectories;
+}
+
+/**
+ * @brief evaluate path furthest point if it is not set
+ * @param data Data to use
+ */
+inline void setPathFurthestPointIfNotSet(CriticData & data)
+{
+  if (!data.furthest_reached_path_point) {
+    data.furthest_reached_path_point = findPathFurthestReachedPoint(data);
+  }
+}
+
+/**
+ * @brief evaluate path costs
+ * @param data Data to use
+ */
+inline void findPathCosts(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  auto * costmap = costmap_ros->getCostmap();
+  unsigned int map_x, map_y;
+  const size_t path_segments_count = data.path.x.size() - 1;
+  data.path_pts_valid = std::vector<bool>(path_segments_count, false);
+  const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown();
+  for (unsigned int idx = 0; idx < path_segments_count; idx++) {
+    if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) {
+      (*data.path_pts_valid)[idx] = false;
+      continue;
+    }
+
+    switch (costmap->getCost(map_x, map_y)) {
+      case (nav2_costmap_2d::LETHAL_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (nav2_costmap_2d::NO_INFORMATION):
+        (*data.path_pts_valid)[idx] = tracking_unknown ? true : false;
+        continue;
+    }
+
+    (*data.path_pts_valid)[idx] = true;
+  }
+}
+
+/**
+ * @brief evaluate path costs if it is not set
+ * @param data Data to use
+ */
+inline void setPathCostsIfNotSet(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  if (!data.path_pts_valid) {
+    findPathCosts(data, costmap_ros);
+  }
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param forward_preference If reversing direction is valid
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference)
+{
+  float pose_x = pose.position.x;
+  float pose_y = pose.position.y;
+  float pose_yaw = tf2::getYaw(pose.orientation);
+
+  float yaw = atan2f(point_y - pose_y, point_x - pose_x);
+
+  // If no preference for forward, return smallest angle either in heading or 180 of heading
+  if (!forward_preference) {
+    return std::min(
+      fabs(angles::shortest_angular_distance(yaw, pose_yaw)),
+      fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF))));
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief evaluate angle from pose (have angle) to point (no angle)
+ * @param pose pose
+ * @param point_x Point to find angle relative to X axis
+ * @param point_y Point to find angle relative to Y axis
+ * @param point_yaw Yaw of the point to consider along Z axis
+ * @return Angle between two points
+ */
+inline float posePointAngle(
+  const geometry_msgs::msg::Pose & pose,
+  double point_x, double point_y, double point_yaw)
+{
+  float pose_x = static_cast<float>(pose.position.x);
+  float pose_y = static_cast<float>(pose.position.y);
+  float pose_yaw = static_cast<float>(tf2::getYaw(pose.orientation));
+
+  float yaw = atan2f(static_cast<float>(point_y) - pose_y, static_cast<float>(point_x) - pose_x);
+
+  if (fabs(angles::shortest_angular_distance(yaw, static_cast<float>(point_yaw))) > M_PIF_2) {
+    yaw = angles::normalize_angle(yaw + M_PIF);
+  }
+
+  return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
+}
+
+/**
+ * @brief Apply Savisky-Golay filter to optimal trajectory
+ * @param control_sequence Sequence to apply filter to
+ * @param control_history Recent set of controls for edge-case handling
+ * @param Settings Settings to use
+ */
+inline void savitskyGolayFilter(
+  models::ControlSequence & control_sequence,
+  std::array<mppi::models::Control, 4> & control_history,
+  const models::OptimizerSettings & settings)
+{
+  // Savitzky-Golay Quadratic, 9-point Coefficients
+  Eigen::Array<float, 9, 1> filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f,
+    -21.0f};
+  filter /= 231.0f;
+
+  // Too short to smooth meaningfully
+  const unsigned int num_sequences = control_sequence.vx.size() - 1;
+  if (num_sequences < 20) {
+    return;
+  }
+
+  auto applyFilter = [&](const Eigen::Array<float, 9, 1> & data) -> float {
+      return (data * filter).eval().sum();
+    };
+
+  auto applyFilterOverAxis =
+    [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence,
+    const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
+    {
+      float pt_m4 = hist_0;
+      float pt_m3 = hist_1;
+      float pt_m2 = hist_2;
+      float pt_m1 = hist_3;
+      float pt = initial_sequence(0);
+      float pt_p1 = initial_sequence(1);
+      float pt_p2 = initial_sequence(2);
+      float pt_p3 = initial_sequence(3);
+      float pt_p4 = initial_sequence(4);
+
+      for (unsigned int idx = 0; idx != num_sequences; idx++) {
+        sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4});
+        pt_m4 = pt_m3;
+        pt_m3 = pt_m2;
+        pt_m2 = pt_m1;
+        pt_m1 = pt;
+        pt = pt_p1;
+        pt_p1 = pt_p2;
+        pt_p2 = pt_p3;
+        pt_p3 = pt_p4;
+
+        if (idx + 5 < num_sequences) {
+          pt_p4 = initial_sequence(idx + 5);
+        } else {
+          // Return the last point
+          pt_p4 = initial_sequence(num_sequences);
+        }
+      }
+    };
+
+  // Filter trajectories
+  const models::ControlSequence initial_control_sequence = control_sequence;
+  applyFilterOverAxis(
+    control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
+    control_history[1].vx, control_history[2].vx, control_history[3].vx);
+  applyFilterOverAxis(
+    control_sequence.vy, initial_control_sequence.vy, control_history[0].vy,
+    control_history[1].vy, control_history[2].vy, control_history[3].vy);
+  applyFilterOverAxis(
+    control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
+    control_history[1].wz, control_history[2].wz, control_history[3].wz);
+
+  // Update control history
+  unsigned int offset = settings.shift_control_sequence ? 1 : 0;
+  control_history[0] = control_history[1];
+  control_history[1] = control_history[2];
+  control_history[2] = control_history[3];
+  control_history[3] = {
+    control_sequence.vx(offset),
+    control_sequence.vy(offset),
+    control_sequence.wz(offset)};
+}
+
+/**
+ * @brief Find the iterator of the first pose at which there is an inversion or in-place rotation in the path
+ * @param path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return the first point after the inversion or rotation found in the path
+ */
+inline unsigned int findFirstPathInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
+{
+  if (path.poses.size() < 2) {
+    return path.poses.size();
+  }
+
+  unsigned int first_after_constraint = path.poses.size();
+
+  // Check for in-place rotation first (if enabled)
+  if (rotation_threshold > 0.0f) {
+    for (unsigned int idx = 0; idx < path.poses.size() - 1; ++idx) {
+      float dx = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x;
+      float dy = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y;
+      float translation = sqrtf(dx * dx + dy * dy);
+
+      // Check if poses are at roughly the same location
+      if (translation < 1e-3) {
+        float yaw1 = tf2::getYaw(path.poses[idx].pose.orientation);
+        float yaw2 = tf2::getYaw(path.poses[idx + 1].pose.orientation);
+        float rotation = fabs(angles::shortest_angular_distance(yaw1, yaw2));
+
+        // Check if this meets the minimum rotation threshold
+        if (rotation > rotation_threshold) {
+          // Found start of in-place rotation, now find where it ends
+          unsigned int end_idx = idx + 1;
+
+          // Continue while we have minimal translation (still rotating in place)
+          while (end_idx < path.poses.size() - 1) {
+            float next_dx = path.poses[end_idx + 1].pose.position.x -
+              path.poses[end_idx].pose.position.x;
+            float next_dy = path.poses[end_idx + 1].pose.position.y -
+              path.poses[end_idx].pose.position.y;
+            float next_translation = sqrtf(next_dx * next_dx + next_dy * next_dy);
+
+            if (next_translation >= 1e-3) {
+              break;  // End of in-place rotation sequence
+            }
+            end_idx++;
+          }
+
+          first_after_constraint = end_idx;
+          break;
+        }
+      }
+    }
+  }
+
+  // Check for inversion (at least 3 poses needed)
+  if (path.poses.size() >= 3) {
+    for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+      float oa_x = path.poses[idx].pose.position.x -
+        path.poses[idx - 1].pose.position.x;
+      float oa_y = path.poses[idx].pose.position.y -
+        path.poses[idx - 1].pose.position.y;
+      float ab_x = path.poses[idx + 1].pose.position.x -
+        path.poses[idx].pose.position.x;
+      float ab_y = path.poses[idx + 1].pose.position.y -
+        path.poses[idx].pose.position.y;
+
+      float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+
+      float norm_oa = hypotf(oa_x, oa_y);
+      float norm_ab = hypotf(ab_x, ab_y);
+
+      if (norm_oa < 1e-4f || norm_ab < 1e-4f) {
+        continue;
+      }
+
+      float cos_ang = dot_product / (norm_oa * norm_ab);
+      cos_ang = std::max(-1.0f, std::min(1.0f, cos_ang));
+      float angle = acosf(cos_ang);  // [0, pi]
+
+      if (rotation_threshold > 0.0f) {
+        if (angle > rotation_threshold) {
+          first_after_constraint = std::min(first_after_constraint, idx + 1);
+          break;
+        }
+      } else {
+        if (dot_product < 0.0f) {
+          first_after_constraint = std::min(first_after_constraint, idx + 1);
+          break;
+        }
+      }
+    }
+  }
+
+  return first_after_constraint;
+}
+
+/**
+ * @brief Find and remove poses after the first inversion or in-place rotation in the path
+ * @param path Path to check for inversion or rotation
+ * @param rotation_threshold Minimum rotation angle to consider an in-place rotation (0 to disable rotation check)
+ * @return The location of the inversion/rotation, return 0 if none exist
+ */
+inline unsigned int removePosesAfterFirstInversion(
+  nav_msgs::msg::Path & path,
+  float rotation_threshold = 0.0f)
+{
+  nav_msgs::msg::Path cropped_path = path;
+
+  const unsigned int first_after_constraint = findFirstPathInversion(cropped_path, rotation_threshold);
+
+  if (first_after_constraint == path.poses.size()) {
+    return 0u;  // No constraint found
+  }
+
+  cropped_path.poses.erase(
+    cropped_path.poses.begin() + first_after_constraint, cropped_path.poses.end());
+  path = cropped_path;
+  return first_after_constraint;
+}
+
+/**
+ * @brief Compare to trajectory points to find closest path point along integrated distances
+ * @param vec Vect to check
+ * @return dist Distance to look for
+ * @return init Starting index to indec from
+ */
+inline unsigned int findClosestPathPt(
+  const std::vector<float> & vec, const float dist, const unsigned int init = 0u)
+{
+  float distim1 = init != 0u ? vec[init] : 0.0f;  // First is 0, no accumulated distance yet
+  float disti = 0.0f;
+  const unsigned int size = vec.size();
+  for (unsigned int i = init + 1; i != size; i++) {
+    disti = vec[i];
+    if (disti > dist) {
+      if (i > 0 && dist - distim1 < disti - dist) {
+        return i - 1;
+      }
+      return i;
+    }
+    distim1 = disti;
+  }
+  return size - 1;
+}
+
+// A struct to hold pose data in floating point resolution
+struct Pose2D
+{
+  float x, y, theta;
+};
+
+/**
+ * @brief Shift the columns of a 2D Eigen Array or scalar values of
+ *    1D Eigen Array by 1 place.
+ * @param e Eigen Array
+ * @param direction direction in which Array will be shifted.
+ *     1 for shift in right direction and -1 for left direction.
+ */
+inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
+{
+  int size = e.size();
+  if(size == 1) {return;}
+  if(abs(direction) != 1) {
+    throw std::logic_error("Invalid direction, only 1 and -1 are valid values.");
+  }
+
+  if((e.cols() == 1 || e.rows() == 1) && size > 1) {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1;
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1;
+    while(start_ptr != end_ptr) {
+      *(start_ptr + direction) = *start_ptr;
+      start_ptr -= direction;
+    }
+    *(start_ptr + direction) = *start_ptr;
+  } else {
+    auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows();
+    auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows();
+    auto span = e.rows();
+    while(start_ptr != end_ptr) {
+      std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+      start_ptr -= (direction * span);
+    }
+    std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
+  }
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of final yaw angle
+ *    of the trajectory.
+ * @param last_yaws Final yaw angles of the trajectories.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points.
+ */
+inline auto normalize_yaws_between_points(
+  const Eigen::Ref<const Eigen::ArrayXf> & last_yaws,
+  const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  Eigen::ArrayXf yaws = utils::shortest_angular_distance(
+          last_yaws, yaw_between_points).abs();
+  int size = yaws.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Normalize the yaws between points on the basis of goal angle.
+ * @param goal_yaw Goal yaw angle.
+ * @param yaw_between_points Yaw angles calculated between x and y coordinates of the trajectories.
+ * @return Normalized yaw between points
+ */
+inline auto normalize_yaws_between_points(
+  const float goal_yaw, const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
+{
+  int size = yaw_between_points.size();
+  Eigen::ArrayXf yaws_between_points_corrected(size);
+  for(int i = 0; i != size; i++) {
+    const float & yaw_between_point = yaw_between_points[i];
+    yaws_between_points_corrected[i] = fabs(
+      angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ?
+      yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
+  }
+  return yaws_between_points_corrected;
+}
+
+/**
+ * @brief Clamps the input between the given lower and upper bounds.
+ * @param lower_bound Lower bound.
+ * @param upper_bound Upper bound.
+ * @return Clamped output.
+ */
+inline float clamp(
+  const float lower_bound, const float upper_bound, const float input)
+{
+  return std::min(upper_bound, std::max(input, lower_bound));
+}
+
+}  // namespace mppi::utils
+
+#endif  // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
diff --git a/nav2_mppi_controller/src/collision_checker.cpp b/nav2_mppi_controller/src/collision_checker.cpp
new file mode 100644
index 00000000000..351bc06a575
--- /dev/null
+++ b/nav2_mppi_controller/src/collision_checker.cpp
@@ -0,0 +1,394 @@
+// Copyright (c) 2025, Angsa Robotics GmbH
+//
+// 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 <algorithm>
+#include <cmath>
+
+#include "nav2_mppi_controller/collision_checker.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+
+
+namespace nav2_mppi_controller
+{
+
+MPPICollisionChecker::MPPICollisionChecker(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr)
+{
+  if (node) {
+    clock_ = node->get_clock();
+    logger_ = node->get_logger();
+  }
+
+  if (costmap_ros) {
+    costmap_ros_ = costmap_ros;
+  }
+}
+
+void MPPICollisionChecker::setFootprint(
+  const nav2_costmap_2d::Footprint & footprint,
+  const bool & radius,
+  const std::string & inflation_layer_name)
+{
+  // Calculate the circumscribed cost automatically
+  possible_collision_cost_ = findCircumscribedCost(inflation_layer_name);
+
+  if (possible_collision_cost_ <= 0.0f) {
+    RCLCPP_ERROR_THROTTLE(
+      logger_, *clock_, 1000,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_mppi_controller#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  footprint_is_radius_ = radius;
+
+  // Use radius, no caching required
+  if (radius) {
+    return;
+  }
+
+  // Store the unoriented footprint for on-the-fly transformation
+  unoriented_footprint_ = footprint;
+}
+
+CollisionResult MPPICollisionChecker::inCollision(
+  const std::vector<float> & x,
+  const std::vector<float> & y,
+  const std::vector<float> & yaw,
+  const bool & traverse_unknown)
+{
+  CollisionResult result;
+  result.in_collision = false;
+
+  // Check if all vectors have the same size
+  if (x.size() != y.size() || x.size() != yaw.size()) {
+    RCLCPP_ERROR(logger_, "Unexpected size of input vectors for collision checking");
+    result.in_collision = true;
+    return result;
+  }
+
+  // Initialize result vectors
+  result.center_cost.resize(x.size(), -1.0f);
+  result.footprint_cost.resize(x.size(), -1.0f);
+
+  // Step 1: Check all poses for bounds and get center costs
+  std::vector<bool> needs_footprint_check(x.size(), false);
+
+  for (size_t i = 0; i < x.size(); ++i) {
+    // Check to make sure cell is inside the map
+    if (outsideRange(costmap_->getSizeInCellsX(), x[i]) ||
+      outsideRange(costmap_->getSizeInCellsY(), y[i]))
+    {
+      result.in_collision = true;
+      return result;
+    }
+
+    // Get center cost for this pose
+    float current_center_cost = static_cast<float>(costmap_->getCost(
+      static_cast<unsigned int>(x[i] + 0.5f), static_cast<unsigned int>(y[i] + 0.5f)));
+
+    result.center_cost[i] = current_center_cost;
+
+    if (current_center_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) {
+      result.in_collision = true;
+      return result;
+    }
+
+    if (current_center_cost >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
+      result.in_collision = true;
+      return result;
+    }
+
+    // For footprint-based collision checking, mark poses that need further checking
+    if (!footprint_is_radius_) {
+      // Skip if center cost is below collision threshold
+      if (current_center_cost >= possible_collision_cost_ || possible_collision_cost_ <= 0.0f) {
+        needs_footprint_check[i] = true;
+      }
+    }
+  }
+
+  if (footprint_is_radius_) {
+    return result;  // No further checks needed for radius-based checking
+  }
+
+  // Step 2: If using footprint, check footprint costs for poses that need it
+  std::vector<bool> needs_swept_area_check(x.size(), false);
+  // Cache computed footprints to avoid recomputation in Step 4
+  std::vector<nav2_costmap_2d::Footprint> cached_footprints(x.size());
+
+  for (size_t i = 0; i < x.size(); ++i) {
+    // Skip poses that don't need footprint checking
+    if (!needs_footprint_check[i]) {
+      continue;
+    }
+
+    // Transform footprint to current orientation and world position
+    double wx, wy;
+    costmap_->mapToWorld(static_cast<double>(x[i]), static_cast<double>(y[i]), wx, wy);
+
+    // Transform the footprint to the given orientation
+    nav2_costmap_2d::Footprint oriented_footprint = transformFootprint(unoriented_footprint_,
+        yaw[i]);
+
+    // Translate to world position
+    nav2_costmap_2d::Footprint current_footprint;
+    current_footprint.reserve(oriented_footprint.size());
+    geometry_msgs::msg::Point new_pt;
+    for (unsigned int j = 0; j < oriented_footprint.size(); ++j) {
+      new_pt.x = wx + oriented_footprint[j].x;
+      new_pt.y = wy + oriented_footprint[j].y;
+      current_footprint.push_back(new_pt);
+    }
+
+    // Cache the computed footprint for potential reuse in Step 4
+    cached_footprints[i] = current_footprint;
+
+    // Check footprint perimeter
+    float footprint_cost = static_cast<float>(footprintCost(current_footprint, false));
+
+    // Store footprint cost in result
+    result.footprint_cost[i] = footprint_cost;
+
+    if (footprint_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) {
+      result.in_collision = true;
+      return result;
+    }
+
+    if (footprint_cost >= nav2_costmap_2d::LETHAL_OBSTACLE) {
+      result.in_collision = true;
+      return result;
+    }
+
+    // Mark for swept area checking if footprint cost is INSCRIBED_INFLATED_OBSTACLE
+    if (footprint_cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
+      needs_swept_area_check[i] = true;
+    }
+  }
+
+  // Step 3: Check swept area for consecutive poses with footprint cost INSCRIBED_INFLATED_OBSTACLE
+  // Find consecutive sequences of poses that need swept area checking
+  std::vector<std::vector<size_t>> consecutive_sequences;
+  std::vector<size_t> current_sequence;
+
+  for (size_t i = 0; i < x.size(); ++i) {
+    if (needs_swept_area_check[i]) {
+      current_sequence.push_back(i);
+      // Limit sequence to maximum of 5 poses to avoid an unprecise swept area check
+      if (current_sequence.size() >= 5) {
+        consecutive_sequences.push_back(current_sequence);
+        current_sequence.clear();
+      }
+    } else {
+      if (!current_sequence.empty()) {
+        // Only add sequences with more than one pose
+        if (current_sequence.size() > 1) {
+          consecutive_sequences.push_back(current_sequence);
+        }
+        current_sequence.clear();
+      }
+    }
+  }
+
+  // Don't forget the last sequence if it ends at the last pose
+  if (!current_sequence.empty()) {
+    // Only add sequences with more than one pose
+    if (current_sequence.size() > 1) {
+      consecutive_sequences.push_back(current_sequence);
+    }
+  }
+
+  // Step 4: Check swept area using convex hull for each consecutive sequence
+  for (const auto & sequence : consecutive_sequences) {
+    // Collect all footprint points from consecutive poses using cached footprints
+    std::vector<geometry_msgs::msg::Point> all_points;
+
+    for (size_t seq_idx : sequence) {
+      // Use cached footprint instead of recomputing
+      const nav2_costmap_2d::Footprint & current_footprint = cached_footprints[seq_idx];
+
+      for (const auto & footprint_pt : current_footprint) {
+        all_points.push_back(footprint_pt);
+      }
+    }
+
+    // Create convex hull from all collected points
+    nav2_costmap_2d::Footprint convex_hull = createConvexHull(all_points);
+
+    // Check swept area cost using full area checking
+    float swept_area_cost = static_cast<float>(footprintCost(convex_hull, true));
+
+    if (swept_area_cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) {
+      result.in_collision = true;
+      return result;
+    }
+
+    if (swept_area_cost >= nav2_costmap_2d::LETHAL_OBSTACLE) {
+      result.in_collision = true;
+      return result;
+    }
+  }
+
+  return result;
+}
+
+nav2_costmap_2d::Footprint MPPICollisionChecker::createConvexHull(
+  const std::vector<geometry_msgs::msg::Point> & points)
+{
+  if (points.size() <= 1) {
+    return points;
+  }
+
+  // Create a copy of points for sorting
+  std::vector<geometry_msgs::msg::Point> sorted_points = points;
+
+  // Sort points lexicographically (first by x, then by y)
+  std::sort(sorted_points.begin(), sorted_points.end(),
+    [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) {
+      return a.x < b.x || (a.x == b.x && a.y < b.y);
+    });
+
+  // Remove duplicate points
+  sorted_points.erase(
+    std::unique(sorted_points.begin(), sorted_points.end(),
+    [](const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) {
+      return std::abs(a.x - b.x) < 1e-9 && std::abs(a.y - b.y) < 1e-9;
+      }),
+    sorted_points.end());
+
+  if (sorted_points.size() <= 1) {
+    return sorted_points;
+  }
+
+  // Andrew's monotone chain algorithm
+  std::vector<geometry_msgs::msg::Point> hull;
+
+  // Helper function to compute cross product
+  auto cross = [](const geometry_msgs::msg::Point & O,
+    const geometry_msgs::msg::Point & A,
+    const geometry_msgs::msg::Point & B) {
+      return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x);
+    };
+
+  // Build lower hull
+  for (size_t i = 0; i < sorted_points.size(); ++i) {
+    while (hull.size() >= 2 && cross(hull[hull.size() - 2], hull[hull.size() - 1],
+        sorted_points[i]) <= 0)
+    {
+      hull.pop_back();
+    }
+    hull.push_back(sorted_points[i]);
+  }
+
+  // Build upper hull
+  size_t t = hull.size() + 1;
+  for (int i = static_cast<int>(sorted_points.size()) - 2; i >= 0; --i) {
+    while (hull.size() >= t && cross(hull[hull.size() - 2], hull[hull.size() - 1],
+        sorted_points[i]) <= 0)
+    {
+      hull.pop_back();
+    }
+    hull.push_back(sorted_points[i]);
+  }
+
+  // Remove last point because it's the same as the first one
+  if (hull.size() > 1) {
+    hull.pop_back();
+  }
+
+  return hull;
+}
+
+bool MPPICollisionChecker::outsideRange(const unsigned int & max, const float & value) const
+{
+  return value < 0.0f || value > max;
+}
+
+nav2_costmap_2d::Footprint MPPICollisionChecker::transformFootprint(
+  const nav2_costmap_2d::Footprint & footprint,
+  float yaw) const
+{
+  double sin_th = sin(yaw);
+  double cos_th = cos(yaw);
+  nav2_costmap_2d::Footprint oriented_footprint;
+  oriented_footprint.reserve(footprint.size());
+
+  geometry_msgs::msg::Point new_pt;
+  for (unsigned int i = 0; i < footprint.size(); ++i) {
+    new_pt.x = footprint[i].x * cos_th - footprint[i].y * sin_th;
+    new_pt.y = footprint[i].x * sin_th + footprint[i].y * cos_th;
+    oriented_footprint.push_back(new_pt);
+  }
+
+  return oriented_footprint;
+}
+
+float MPPICollisionChecker::findCircumscribedCost(const std::string & inflation_layer_name)
+{
+  if (!costmap_ros_) {
+    RCLCPP_ERROR(logger_, "Costmap ROS is not available for circumscribed cost calculation");
+    return -1.0f;
+  }
+
+  double result = -1.0;
+  const double circum_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius();
+
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
+  // check if the costmap has an inflation layer
+  const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(
+    costmap_ros_,
+    inflation_layer_name);
+
+  if (inflation_layer != nullptr) {
+    const double resolution = costmap_ros_->getCostmap()->getResolution();
+    double inflation_radius = inflation_layer->getInflationRadius();
+
+    if (inflation_radius < circum_radius) {
+      RCLCPP_ERROR(
+        logger_,
+        "The inflation radius (%f) is smaller than the circumscribed radius (%f) "
+        "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+        "field to speed up collision checking by only checking the full footprint "
+        "when robot is within possibly-inscribed radius of an obstacle. This may "
+        "significantly slow down planning times!",
+        inflation_radius, circum_radius);
+      result = 0.0;
+    } else {
+      result = inflation_layer->computeCost(circum_radius / resolution);
+    }
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times and not avoid anything but absolute collisions!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+}  // namespace nav2_mppi_controller
\ No newline at end of file
diff --git a/nav2_mppi_controller/src/controller copy 2.cpp b/nav2_mppi_controller/src/controller copy 2.cpp
new file mode 100644
index 00000000000..a9fab10c85e
--- /dev/null
+++ b/nav2_mppi_controller/src/controller copy 2.cpp	
@@ -0,0 +1,157 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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 <stdint.h>
+#include <chrono>
+#include "nav2_mppi_controller/controller.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+
+// #define BENCHMARK_TESTING
+
+namespace nav2_mppi_controller
+{
+
+void MPPIController::configure(
+  const nav2::LifecycleNode::WeakPtr & parent,
+  std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  parent_ = parent;
+  costmap_ros_ = costmap_ros;
+  tf_buffer_ = tf;
+  name_ = name;
+  parameters_handler_ = std::make_unique<ParametersHandler>(parent, name_);
+
+  auto node = parent_.lock();
+  // Get high-level controller parameters
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(visualize_, "visualize", false);
+
+  getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false);
+
+  // Configure composed objects
+  optimizer_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get());
+  path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get());
+  trajectory_visualizer_.on_configure(
+    parent_, name_,
+    costmap_ros_->getGlobalFrameID(), parameters_handler_.get());
+
+  if (publish_optimal_trajectory_) {
+    opt_traj_pub_ = node->create_publisher<nav2_msgs::msg::Trajectory>(
+      "~/optimal_trajectory");
+  }
+
+  RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
+}
+
+void MPPIController::cleanup()
+{
+  optimizer_.shutdown();
+  trajectory_visualizer_.on_cleanup();
+  parameters_handler_.reset();
+  opt_traj_pub_.reset();
+  RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str());
+}
+
+void MPPIController::activate()
+{
+  auto node = parent_.lock();
+  trajectory_visualizer_.on_activate();
+  parameters_handler_->start();
+  if (opt_traj_pub_) {
+    opt_traj_pub_->on_activate();
+  }
+  RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
+}
+
+void MPPIController::deactivate()
+{
+  trajectory_visualizer_.on_deactivate();
+  if (opt_traj_pub_) {
+    opt_traj_pub_->on_deactivate();
+  }
+  RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
+}
+
+void MPPIController::reset()
+{
+  optimizer_.reset(false /*Don't reset zone-based speed limits between requests*/);
+}
+
+geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
+  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Twist & robot_speed,
+  nav2_core::GoalChecker * goal_checker)
+{
+#ifdef BENCHMARK_TESTING
+  auto start = std::chrono::system_clock::now();
+#endif
+
+  std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
+  geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose;
+
+  nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
+
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> costmap_lock(*(costmap->getMutex()));
+
+  auto [cmd, optimal_trajectory] =
+    optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker);
+
+#ifdef BENCHMARK_TESTING
+  auto end = std::chrono::system_clock::now();
+  auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
+  RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
+#endif
+
+  if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) {
+    auto trajectory_msg = utils::toTrajectoryMsg(
+      optimal_trajectory,
+      optimizer_.getOptimalControlSequence(),
+      optimizer_.getSettings().model_dt,
+      cmd.header);
+    opt_traj_pub_->publish(std::move(trajectory_msg));
+  }
+
+  if (visualize_) {
+    visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory);
+  }
+
+  return cmd;
+}
+
+void MPPIController::visualize(
+  nav_msgs::msg::Path transformed_plan,
+  const builtin_interfaces::msg::Time & cmd_stamp,
+  const Eigen::ArrayXXf & optimal_trajectory)
+{
+  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
+  trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
+  trajectory_visualizer_.visualize(std::move(transformed_plan));
+}
+
+void MPPIController::setPlan(const nav_msgs::msg::Path & path)
+{
+  path_handler_.setPath(path);
+}
+
+void MPPIController::setSpeedLimit(const double & speed_limit, const bool & percentage)
+{
+  optimizer_.setSpeedLimit(speed_limit, percentage);
+}
+
+}  // namespace nav2_mppi_controller
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_mppi_controller::MPPIController, nav2_core::Controller)
diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp
index a9fab10c85e..1988b65d6c1 100644
--- a/nav2_mppi_controller/src/controller.cpp
+++ b/nav2_mppi_controller/src/controller.cpp
@@ -16,8 +16,9 @@
 #include <chrono>
 #include "nav2_mppi_controller/controller.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
-
-// #define BENCHMARK_TESTING
+#include "nav2_costmap_2d/footprint.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+#include "std_msgs/msg/header.hpp"
 
 namespace nav2_mppi_controller
 {
@@ -36,9 +37,6 @@ void MPPIController::configure(
   auto node = parent_.lock();
   // Get high-level controller parameters
   auto getParam = parameters_handler_->getParamGetter(name_);
-  getParam(visualize_, "visualize", false);
-
-  getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false);
 
   // Configure composed objects
   optimizer_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get());
@@ -47,11 +45,6 @@ void MPPIController::configure(
     parent_, name_,
     costmap_ros_->getGlobalFrameID(), parameters_handler_.get());
 
-  if (publish_optimal_trajectory_) {
-    opt_traj_pub_ = node->create_publisher<nav2_msgs::msg::Trajectory>(
-      "~/optimal_trajectory");
-  }
-
   RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
 }
 
@@ -60,7 +53,6 @@ void MPPIController::cleanup()
   optimizer_.shutdown();
   trajectory_visualizer_.on_cleanup();
   parameters_handler_.reset();
-  opt_traj_pub_.reset();
   RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str());
 }
 
@@ -69,18 +61,12 @@ void MPPIController::activate()
   auto node = parent_.lock();
   trajectory_visualizer_.on_activate();
   parameters_handler_->start();
-  if (opt_traj_pub_) {
-    opt_traj_pub_->on_activate();
-  }
   RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
 }
 
 void MPPIController::deactivate()
 {
   trajectory_visualizer_.on_deactivate();
-  if (opt_traj_pub_) {
-    opt_traj_pub_->on_deactivate();
-  }
   RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
 }
 
@@ -94,9 +80,9 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   const geometry_msgs::msg::Twist & robot_speed,
   nav2_core::GoalChecker * goal_checker)
 {
-#ifdef BENCHMARK_TESTING
+  #ifdef BENCHMARK_TESTING
   auto start = std::chrono::system_clock::now();
-#endif
+  #endif
 
   std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
   geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose;
@@ -109,38 +95,28 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   auto [cmd, optimal_trajectory] =
     optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker);
 
-#ifdef BENCHMARK_TESTING
+
+  #ifdef BENCHMARK_TESTING
   auto end = std::chrono::system_clock::now();
   auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
   RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
-#endif
-
-  if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) {
-    auto trajectory_msg = utils::toTrajectoryMsg(
-      optimal_trajectory,
-      optimizer_.getOptimalControlSequence(),
-      optimizer_.getSettings().model_dt,
-      cmd.header);
-    opt_traj_pub_->publish(std::move(trajectory_msg));
-  }
-
-  if (visualize_) {
-    visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory);
-  }
+  #endif
+
+  trajectory_visualizer_.visualize(
+    std::move(transformed_plan),
+    optimal_trajectory,
+    optimizer_.getOptimalControlSequence(),
+    optimizer_.getSettings().model_dt,
+    cmd.header.stamp,
+    costmap_ros_,
+    optimizer_.getGeneratedTrajectories(),
+    optimizer_.getCosts(),
+    optimizer_.getCriticCosts(),
+    optimizer_.getTrajectoriesInCollision());
 
   return cmd;
 }
 
-void MPPIController::visualize(
-  nav_msgs::msg::Path transformed_plan,
-  const builtin_interfaces::msg::Time & cmd_stamp,
-  const Eigen::ArrayXXf & optimal_trajectory)
-{
-  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
-  trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
-  trajectory_visualizer_.visualize(std::move(transformed_plan));
-}
-
 void MPPIController::setPlan(const nav_msgs::msg::Path & path)
 {
   path_handler_.setPath(path);
@@ -154,4 +130,4 @@ void MPPIController::setSpeedLimit(const double & speed_limit, const bool & perc
 }  // namespace nav2_mppi_controller
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(nav2_mppi_controller::MPPIController, nav2_core::Controller)
+PLUGINLIB_EXPORT_CLASS(nav2_mppi_controller::MPPIController, nav2_core::Controller)
\ No newline at end of file
diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp
index 305140e2e13..aaa5790548c 100644
--- a/nav2_mppi_controller/src/critic_manager.cpp
+++ b/nav2_mppi_controller/src/critic_manager.cpp
@@ -37,7 +37,11 @@ void CriticManager::getParams()
   auto node = parent_.lock();
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(critic_names_, "critics", std::vector<std::string>{}, ParameterType::Static);
-  getParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static);
+  // Read visualization parameters from Visualization namespace
+  auto getVisualizerParam = parameters_handler_->getParamGetter(name_ + ".Visualization");
+  getVisualizerParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static);
+  getVisualizerParam(visualize_per_critic_costs_, "publish_trajectories_with_individual_cost",
+      false, ParameterType::Static);
 }
 
 void CriticManager::loadCritics()
@@ -83,6 +87,15 @@ void CriticManager::evalTrajectoriesScores(
     stats_msg->costs_sum.reserve(critics_.size());
   }
 
+  // Initialize per-critic costs tracking only if requested
+  if (visualize_per_critic_costs_) {
+    if (!data.individual_critics_cost) {
+      data.individual_critics_cost = std::vector<std::pair<std::string, Eigen::ArrayXf>>();
+    }
+    data.individual_critics_cost->clear();
+    data.individual_critics_cost->reserve(critics_.size());
+  }
+
   for (size_t i = 0; i < critics_.size(); ++i) {
     if (data.fail_flag) {
       break;
@@ -90,21 +103,30 @@ void CriticManager::evalTrajectoriesScores(
 
     // Store costs before critic evaluation
     Eigen::ArrayXf costs_before;
-    if (publish_critics_stats_) {
+    if (visualize_per_critic_costs_ || publish_critics_stats_) {
       costs_before = data.costs;
     }
 
     critics_[i]->score(data);
 
-    // Calculate statistics if publishing is enabled
-    if (publish_critics_stats_) {
-      stats_msg->critics.push_back(critic_names_[i]);
+    // Calculate cost contribution from this critic
+    if (visualize_per_critic_costs_ || publish_critics_stats_) {
 
       // Calculate sum of costs added by this individual critic
       Eigen::ArrayXf cost_diff = data.costs - costs_before;
-      float costs_sum = cost_diff.sum();
-      stats_msg->costs_sum.push_back(costs_sum);
-      stats_msg->changed.push_back(costs_sum != 0.0f);
+      if (visualize_per_critic_costs_) {
+        data.individual_critics_cost->emplace_back(critic_names_[i], cost_diff);
+      }
+
+      // Calculate statistics if publishing is enabled
+      if (publish_critics_stats_) {
+        stats_msg->critics.push_back(critic_names_[i]);
+
+        // Calculate sum of costs added by this individual critic
+        float costs_sum = cost_diff.sum();
+        stats_msg->costs_sum.push_back(costs_sum);
+        stats_msg->changed.push_back(costs_sum != 0.0f);
+      }
     }
   }
 
diff --git a/nav2_mppi_controller/src/critics/cost_critic copy 2.cpp b/nav2_mppi_controller/src/critics/cost_critic copy 2.cpp
new file mode 100644
index 00000000000..2ab49793515
--- /dev/null
+++ b/nav2_mppi_controller/src/critics/cost_critic copy 2.cpp	
@@ -0,0 +1,284 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+#include <algorithm> 
+#include <limits>
+
+namespace mppi::critics
+{
+
+void CostCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(consider_footprint_, "consider_footprint", false);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 3.81f);
+  getParam(critical_cost_, "critical_cost", 300.0f);
+  getParam(near_collision_cost_, "near_collision_cost", 253);
+  getParam(collision_cost_, "collision_cost", 1000000.0f);
+  getParam(near_goal_distance_, "near_goal_distance", 0.5f);
+  getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
+  getParam(trajectory_point_step_, "trajectory_point_step", 2);
+  getParam(precollision_lookahead_time_, "precollision_lookahead_time", 1.0f); 
+  getParam(precollision_penalty_, "precollision_penalty", 100.0f);
+
+  // Normalized by cost value to put in same regime as other weights
+  weight_ /= 254.0f;
+
+  // Normalize weight when parameter is changed dynamically as well
+  auto weightDynamicCb = [&](
+    const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) {
+      weight_ = weight.as_double() / 254.0f;
+    };
+  parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb);
+
+  collision_checker_.setCostmap(costmap_);
+  possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possible_collision_cost_ < 1.0f) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+    logger_,
+    "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+    "in both the costmap and the cost critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+      "Considering footprint in collision checking but no robot footprint provided in the "
+      "costmap.");
+    }
+  }
+
+  if(near_collision_cost_ > 253) {
+    RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE");
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "InflationCostCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_cost_, weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+float CostCritic::findCircumscribedCost(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  double result = -1.0;
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
+  // check if the costmap has an inflation layer
+  const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(
+    costmap,
+    inflation_layer_name_);
+  if (inflation_layer != nullptr) {
+    const double resolution = costmap->getCostmap()->getResolution();
+    double inflation_radius = inflation_layer->getInflationRadius();
+    if (inflation_radius < circum_radius) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("computeCircumscribedCost"),
+        "The inflation radius (%f) is smaller than the circumscribed radius (%f) "
+        "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+        "field to speed up collision checking by only checking the full footprint "
+        "when robot is within possibly-inscribed radius of an obstacle. This may "
+        "significantly slow down planning times!",
+        inflation_radius, circum_radius);
+      result = 0.0;
+      return result;
+    }
+    result = inflation_layer->computeCost(circum_radius / resolution);
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times and not avoid anything but absolute collisions!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+void CostCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  // Setup cost information for various parts of the critic
+  is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+  auto * costmap = collision_checker_.getCostmap();
+  origin_x_ = static_cast<float>(costmap->getOriginX());
+  origin_y_ = static_cast<float>(costmap->getOriginY());
+  resolution_ = static_cast<float>(costmap->getResolution());
+  size_x_ = costmap->getSizeInCellsX();
+  size_y_ = costmap->getSizeInCellsY();
+
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
+  // If near the goal, don't apply the preferential term since the goal is near obstacles
+  bool near_goal = false;
+  if (data.state.local_path_length < near_goal_distance_) {
+    near_goal = true;
+  }
+
+  Eigen::ArrayXf repulsive_cost(data.costs.rows());
+  repulsive_cost.setZero();
+  bool all_trajectories_collide = true;
+
+  int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1;
+  int strided_traj_rows = data.trajectories.x.rows();
+  int outer_stride = strided_traj_rows * trajectory_point_step_;
+
+    // Initialize trajectories collision tracking
+  if (!data.trajectories_in_collision) {
+    data.trajectories_in_collision = std::vector<bool>(strided_traj_rows, false);
+  }
+
+  const auto traj_x = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+  const auto traj_y = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+  const auto traj_yaw = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+
+  for (int i = 0; i < strided_traj_rows; ++i) {
+    bool trajectory_collide = false;
+    float pose_cost = 0.0f;
+    float & traj_cost = repulsive_cost(i);
+
+    for (int j = 0; j < strided_traj_cols; j++) {
+      float Tx = traj_x(i, j);
+      float Ty = traj_y(i, j);
+      unsigned int x_i = 0u, y_i = 0u;
+
+      // The getCost doesn't use orientation
+      // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
+      // So the center point has more information than the footprint
+      if (!worldToMapFloat(Tx, Ty, x_i, y_i)) {
+        pose_cost = 255.0f;  // NO_INFORMATION in float
+      } else {
+        pose_cost = static_cast<float>(costmap->getCost(getIndex(x_i, y_i)));
+        if (pose_cost < 1.0f) {
+          continue;  // In free space
+        }
+      }
+
+      if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
+        traj_cost = collision_cost_;
+        trajectory_collide = true;
+        break;
+      }
+
+      // Let near-collision trajectory points be punished severely
+      // Note that we collision check based on the footprint actual,
+      // but score based on the center-point cost regardless
+      if (pose_cost >= static_cast<float>(near_collision_cost_)) {
+        traj_cost += critical_cost_;
+      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
+        traj_cost += pose_cost;
+      }
+    }
+
+    if (!trajectory_collide &&
+        precollision_lookahead_time_ > 0.0f &&
+        precollision_penalty_ > 0.0f)
+    {
+      // how many columns ahead correspond to precollision_lookahead_time_
+      const unsigned int max_preview_cols = std::min(
+        (unsigned int)std::ceil(
+            precollision_lookahead_time_ /
+            (data.model_dt * static_cast<float>(trajectory_point_step_))),
+        (unsigned int)strided_traj_cols);
+
+      bool collide_within_window = false;
+
+      for (unsigned int pj = 0; pj < max_preview_cols; ++pj) {
+        const float Px   = traj_x(i, pj);
+        const float Py   = traj_y(i, pj);
+        const float Pyaw = traj_yaw(i, pj);
+
+        const double fp_cost = collision_checker_.footprintCostAtPose(
+          (double)Px,
+          (double)Py,
+          (double)Pyaw,
+          costmap_ros_->getRobotFootprint());
+
+        // Jeśli footprint w tej próbce jest już w strefie naprawdę niebezpiecznej,
+        // to ta trajektoria "uderzy" w przeszkodę w czasie krótszym niż
+        // precollision_lookahead_time_.
+        // Uznajemy ją za złą.
+        if (fp_cost == (double)nav2_costmap_2d::LETHAL_OBSTACLE ||
+            fp_cost == (double)nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+        {
+          collide_within_window = true;
+          break;
+        }
+      }
+
+      if (collide_within_window) {
+        // kara stała za to, że kolizja jest < precollision_lookahead_time_
+        repulsive_cost(i) += precollision_penalty_;
+      }
+    }
+
+
+    (*data.trajectories_in_collision)[i] = trajectory_collide;
+    all_trajectories_collide &= trajectory_collide;
+  }
+
+  if (power_ > 1u) {
+    data.costs += (repulsive_cost *
+      (weight_ / static_cast<float>(strided_traj_cols))).pow(power_);
+  } else {
+    data.costs += repulsive_cost * (weight_ / static_cast<float>(strided_traj_cols));
+  }
+
+  data.fail_flag = all_trajectories_collide;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::CostCritic,
+  mppi::critics::CriticFunction)
diff --git a/nav2_mppi_controller/src/critics/cost_critic copy 3.cpp b/nav2_mppi_controller/src/critics/cost_critic copy 3.cpp
new file mode 100644
index 00000000000..74ce0df31e0
--- /dev/null
+++ b/nav2_mppi_controller/src/critics/cost_critic copy 3.cpp	
@@ -0,0 +1,297 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
+namespace mppi::critics
+{
+
+void CostCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(consider_footprint_, "consider_footprint", false);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 3.81f);
+  getParam(critical_cost_, "critical_cost", 300.0f);
+  getParam(near_collision_cost_, "near_collision_cost", 253);
+  getParam(collision_cost_, "collision_cost", 1000000.0f);
+  getParam(near_goal_distance_, "near_goal_distance", 0.5f);
+  getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
+  getParam(trajectory_point_step_, "trajectory_point_step", 2);
+
+  // Normalized by cost value to put in same regime as other weights
+  weight_ /= 254.0f;
+
+  // Normalize weight when parameter is changed dynamically as well
+  auto weightDynamicCb = [&](
+    const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) {
+      weight_ = weight.as_double() / 254.0f;
+    };
+  parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb);
+
+  collision_checker_.setCostmap(costmap_);
+  possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possible_collision_cost_ < 1.0f) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+    logger_,
+    "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+    "in both the costmap and the cost critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+      "Considering footprint in collision checking but no robot footprint provided in the "
+      "costmap.");
+    }
+  }
+
+  if(near_collision_cost_ > 253) {
+    RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE");
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "InflationCostCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_cost_, weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+float CostCritic::findCircumscribedCost(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  double result = -1.0;
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius() + 0.2;
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
+  // check if the costmap has an inflation layer
+  const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(
+    costmap,
+    inflation_layer_name_);
+  if (inflation_layer != nullptr) {
+    const double resolution = costmap->getCostmap()->getResolution();
+    double inflation_radius = inflation_layer->getInflationRadius();
+    if (inflation_radius < circum_radius) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("computeCircumscribedCost"),
+        "The inflation radius (%f) is smaller than the circumscribed radius (%f) "
+        "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+        "field to speed up collision checking by only checking the full footprint "
+        "when robot is within possibly-inscribed radius of an obstacle. This may "
+        "significantly slow down planning times!",
+        inflation_radius, circum_radius);
+      result = 0.0;
+      return result;
+    }
+    result = inflation_layer->computeCost(circum_radius / resolution);
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times and not avoid anything but absolute collisions!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+void CostCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  // static bool s_footprint_active = false;
+  // bool any_use_footprint_this_tick = false;
+
+  // Setup cost information for various parts of the critic
+  is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+  auto * costmap = collision_checker_.getCostmap();
+  origin_x_ = static_cast<float>(costmap->getOriginX());
+  origin_y_ = static_cast<float>(costmap->getOriginY());
+  resolution_ = static_cast<float>(costmap->getResolution());
+  size_x_ = costmap->getSizeInCellsX();
+  size_y_ = costmap->getSizeInCellsY();
+
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
+  // If near the goal, don't apply the preferential term since the goal is near obstacles
+  bool near_goal = false;
+  if (data.state.local_path_length < near_goal_distance_) {
+    near_goal = true;
+  }
+
+  Eigen::ArrayXf repulsive_cost(data.costs.rows());
+  repulsive_cost.setZero();
+  bool all_trajectories_collide = true;
+
+  int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1;
+  int strided_traj_rows = data.trajectories.x.rows();
+  int outer_stride = strided_traj_rows * trajectory_point_step_;
+
+    // Initialize trajectories collision tracking
+  if (!data.trajectories_in_collision) {
+    data.trajectories_in_collision = std::vector<bool>(strided_traj_rows, false);
+  }
+
+  const auto traj_x = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+  const auto traj_y = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+  const auto traj_yaw = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+
+  for (int i = 0; i < strided_traj_rows; ++i) {
+    bool trajectory_collide = false;
+    float pose_cost = 0.0f;
+    float & traj_cost = repulsive_cost(i);
+
+    for (int j = 0; j < strided_traj_cols; j++) {
+      float Tx = traj_x(i, j);
+      float Ty = traj_y(i, j);
+      unsigned int x_i = 0u, y_i = 0u;
+
+      // The getCost doesn't use orientation
+      // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
+      // So the center point has more information than the footprint
+      if (!worldToMapFloat(Tx, Ty, x_i, y_i)) {
+        pose_cost = 0.0f;  // NO_INFORMATION in float
+      } else {
+        pose_cost = static_cast<float>(costmap->getCost(getIndex(x_i, y_i)));
+        if (pose_cost < 1.0f) {
+          continue;  // In free space
+        }
+      }
+      int footprint_every_n_points_ = 2;
+      float cost_for_scoring = pose_cost;
+      // bool use_footprint_cost = consider_footprint_ &&
+      //   (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f);
+      bool use_footprint_cost = false;
+      if (consider_footprint_) {
+        const bool near_obstacle = (pose_cost >= possible_collision_cost_) || (possible_collision_cost_ < 1.0f);
+        bool sample_this_point = false;
+        if (footprint_every_n_points_ <= 0) {
+          sample_this_point = (j == strided_traj_cols - 1);            // tylko końcowy punkt
+        } else {
+          sample_this_point = (j % footprint_every_n_points_ == 0) || (j == strided_traj_cols - 1);
+        }
+        use_footprint_cost = near_obstacle && sample_this_point;
+      }
+
+      //any_use_footprint_this_tick = any_use_footprint_this_tick || use_footprint_cost;
+
+      if (use_footprint_cost) {
+        cost_for_scoring = static_cast<float>(collision_checker_.footprintCostAtPose(
+            static_cast<double>(Tx), static_cast<double>(Ty), static_cast<double>(traj_yaw(i, j)),
+            costmap_ros_->getRobotFootprint()));
+      }
+
+      // Check for collision based on the appropriate cost
+      bool in_collision = false;
+
+      switch (static_cast<unsigned char>(cost_for_scoring)) {
+        case (nav2_costmap_2d::LETHAL_OBSTACLE):
+          in_collision = true;
+          break;
+        case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+          in_collision = use_footprint_cost ? false : true;
+          break;
+        case (nav2_costmap_2d::NO_INFORMATION):
+          in_collision = is_tracking_unknown_ ? false : true;
+          break;
+        default:
+          in_collision = false;
+      }
+
+
+      if (in_collision) {
+        traj_cost = collision_cost_;
+        trajectory_collide = true;
+        break;
+      }
+
+      // Let near-collision trajectory points be punished severely
+      // Note that we collision check based on the footprint actual,
+      // but score based on the center-point cost regardless
+      if (cost_for_scoring >= near_collision_cost_ && !near_goal) {
+        traj_cost += critical_cost_;
+      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
+        traj_cost += cost_for_scoring;
+      }
+    }
+
+    (*data.trajectories_in_collision)[i] = trajectory_collide;
+    all_trajectories_collide &= trajectory_collide;
+  }
+
+  if (power_ > 1u) {
+    data.costs += (repulsive_cost *
+      (weight_ / static_cast<float>(strided_traj_cols))).pow(power_);
+  } else {
+    data.costs += repulsive_cost * (weight_ / static_cast<float>(strided_traj_cols));
+  }
+
+  // if (any_use_footprint_this_tick != s_footprint_active) { 
+  //   if (any_use_footprint_this_tick) { 
+  //     RCLCPP_INFO(logger_, "[CostCritic] WLACZONO FOOTPRINT COST");
+  //     RCLCPP_INFO(logger_,
+  //   "[CostCritic] possible_collision_cost_ = %.1f",
+  //   possible_collision_cost_); 
+  //   } else { 
+  //     RCLCPP_INFO(logger_, "[CostCritic] WYLACZONO FOOTPRINT COST"); 
+  //     RCLCPP_INFO(logger_,
+  //   "[CostCritic] possible_collision_cost_ = %.1f",
+  //   possible_collision_cost_); 
+  //   } 
+  //   s_footprint_active = any_use_footprint_this_tick; 
+  // }
+  
+
+  data.fail_flag = all_trajectories_collide;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::CostCritic,
+  mppi::critics::CriticFunction)
diff --git a/nav2_mppi_controller/src/critics/cost_critic copy.cpp b/nav2_mppi_controller/src/critics/cost_critic copy.cpp
new file mode 100644
index 00000000000..5c5d803ca3a
--- /dev/null
+++ b/nav2_mppi_controller/src/critics/cost_critic copy.cpp	
@@ -0,0 +1,198 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "geometry_msgs/msg/point.hpp"
+
+namespace mppi::critics
+{
+
+void CostCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(consider_footprint_, "consider_footprint", false);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 3.81f);
+  getParam(critical_cost_, "critical_cost", 300.0f);
+  getParam(near_collision_cost_, "near_collision_cost", 253);
+  getParam(collision_cost_, "collision_cost", 1000000.0f);
+  getParam(near_goal_distance_, "near_goal_distance", 0.5f);
+  getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
+  getParam(trajectory_point_step_, "trajectory_point_step", 2);
+
+  // Normalized by cost value to put in same regime as other weights
+  weight_ /= 254.0f;
+
+  // Normalize weight when parameter is changed dynamically as well
+  auto weightDynamicCb = [&](
+    const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) {
+      weight_ = weight.as_double() / 254.0f;
+    };
+  parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb);
+
+  collision_checker_ = std::make_unique<nav2_mppi_controller::MPPICollisionChecker>(
+    costmap_ros_, parent_.lock());
+
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+    logger_,
+    "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+    "in both the costmap and the cost critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+      "Considering footprint in collision checking but no robot footprint provided in the "
+      "costmap.");
+    }
+  }
+
+  if(near_collision_cost_ > 253) {
+    RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE");
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "InflationCostCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_cost_, weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+void CostCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  // Setup cost information for various parts of the critic
+  is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+  auto * costmap = collision_checker_->getCostmap();
+  origin_x_ = static_cast<float>(costmap->getOriginX());
+  origin_y_ = static_cast<float>(costmap->getOriginY());
+  resolution_ = static_cast<float>(costmap->getResolution());
+  size_x_ = costmap->getSizeInCellsX();
+  size_y_ = costmap->getSizeInCellsY();
+
+  bool near_goal = false;
+  if (data.state.local_path_length < near_goal_distance_) {
+    near_goal = true;
+  }
+
+  Eigen::ArrayXf repulsive_cost(data.costs.rows());
+  repulsive_cost.setZero();
+  bool all_trajectories_collide = true;
+
+  int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1;
+  int strided_traj_rows = data.trajectories.x.rows();
+  int outer_stride = strided_traj_rows * trajectory_point_step_;
+
+  // Initialize trajectories collision tracking
+  if (!data.trajectories_in_collision) {
+    data.trajectories_in_collision = std::vector<bool>(strided_traj_rows, false);
+  }
+
+  const auto traj_x = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+  const auto traj_y = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+  const auto traj_yaw = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+
+  collision_checker_->setFootprint(
+    costmap_ros_->getRobotFootprint(),
+    costmap_ros_->getUseRadius(),
+    inflation_layer_name_);
+
+  for (int i = 0; i < strided_traj_rows; ++i) {
+    bool trajectory_collide = false;
+    float & traj_cost = repulsive_cost(i);
+    // Prepare vectors for batch collision checking
+    std::vector<float> x_coords, y_coords, yaw_angles;
+    x_coords.reserve(strided_traj_cols);
+    y_coords.reserve(strided_traj_cols);
+    yaw_angles.reserve(strided_traj_cols);
+
+    // Convert world coordinates to map coordinates and collect yaw angles
+    for (int j = 0; j < strided_traj_cols; j++) {
+      float Tx = traj_x(i, j);
+      float Ty = traj_y(i, j);
+      unsigned int x_i = 0u, y_i = 0u;
+
+      if (!worldToMapFloat(Tx, Ty, x_i, y_i)) {
+        // If any point is outside the map, mark trajectory as colliding
+        traj_cost = collision_cost_;
+        trajectory_collide = true;
+        break;
+      }
+
+      x_coords.push_back(static_cast<float>(x_i));
+      y_coords.push_back(static_cast<float>(y_i));
+      yaw_angles.push_back(traj_yaw(i, j));
+    }
+    // Skip batch collision checking if trajectory already marked as colliding
+    if (trajectory_collide) {
+      continue;
+    }
+
+    // Perform batch collision checking
+    auto collision_result = collision_checker_->inCollision(
+      x_coords, y_coords, yaw_angles, is_tracking_unknown_);
+
+    if (collision_result.in_collision) {
+      traj_cost = collision_cost_;
+      trajectory_collide = true;
+    } else {
+      for (size_t k = 0; k < collision_result.footprint_cost.size(); ++k) {
+        // For optimization purposes, we don't always have the footprint cost
+        // but we still use it when available to staw away from collision more conservatively
+        float cost = collision_result.footprint_cost[k] != -1.0f ?
+          collision_result.footprint_cost[k] : collision_result.center_cost[k];
+
+        if (cost >= static_cast<float>(near_collision_cost_)) {
+          traj_cost += critical_cost_;
+        } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
+          traj_cost += cost;
+        }
+      }
+    }
+
+
+    (*data.trajectories_in_collision)[i] = trajectory_collide;
+    all_trajectories_collide &= trajectory_collide;
+  }
+
+  if (power_ > 1u) {
+    data.costs += (repulsive_cost *
+      (weight_ / static_cast<float>(strided_traj_cols))).pow(power_);
+  } else {
+    data.costs += repulsive_cost * (weight_ / static_cast<float>(strided_traj_cols));
+  }
+
+  data.fail_flag = all_trajectories_collide;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::CostCritic,
+  mppi::critics::CriticFunction)
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index 6b15d992ca8..6d2e869614e 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -85,7 +85,7 @@ float CostCritic::findCircumscribedCost(
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
 {
   double result = -1.0;
-  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius() /*+ 0.2*/;
   if (static_cast<float>(circum_radius) == circumscribed_radius_) {
     // early return if footprint size is unchanged
     return circumscribed_cost_;
@@ -133,6 +133,9 @@ void CostCritic::score(CriticData & data)
     return;
   }
 
+  static bool s_footprint_active = false;
+  bool any_use_footprint_this_tick = false;
+
   // Setup cost information for various parts of the critic
   is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
   auto * costmap = collision_checker_.getCostmap();
@@ -161,6 +164,11 @@ void CostCritic::score(CriticData & data)
   int strided_traj_rows = data.trajectories.x.rows();
   int outer_stride = strided_traj_rows * trajectory_point_step_;
 
+    // Initialize trajectories collision tracking
+  if (!data.trajectories_in_collision) {
+    data.trajectories_in_collision = std::vector<bool>(strided_traj_rows, false);
+  }
+
   const auto traj_x = Eigen::Map<const Eigen::ArrayXXf, 0,
       Eigen::Stride<-1, -1>>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols,
       Eigen::Stride<-1, -1>(outer_stride, 1));
@@ -185,7 +193,7 @@ void CostCritic::score(CriticData & data)
       // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
       // So the center point has more information than the footprint
       if (!worldToMapFloat(Tx, Ty, x_i, y_i)) {
-        pose_cost = 255.0f;  // NO_INFORMATION in float
+        pose_cost = 0.0f;  // NO_INFORMATION in float
       } else {
         pose_cost = static_cast<float>(costmap->getCost(getIndex(x_i, y_i)));
         if (pose_cost < 1.0f) {
@@ -193,7 +201,36 @@ void CostCritic::score(CriticData & data)
         }
       }
 
-      if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
+      float cost_for_scoring = pose_cost;
+      bool use_footprint_cost = consider_footprint_ &&
+        (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f);
+      any_use_footprint_this_tick = any_use_footprint_this_tick || use_footprint_cost;
+
+      if (use_footprint_cost) {
+        cost_for_scoring = static_cast<float>(collision_checker_.footprintCostAtPose(
+            static_cast<double>(Tx), static_cast<double>(Ty), static_cast<double>(traj_yaw(i, j)),
+            costmap_ros_->getRobotFootprint()));
+      }
+
+      // Check for collision based on the appropriate cost
+      bool in_collision = false;
+
+      switch (static_cast<unsigned char>(cost_for_scoring)) {
+        case (nav2_costmap_2d::LETHAL_OBSTACLE):
+          in_collision = true;
+          break;
+        case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+          in_collision = use_footprint_cost ? false : true;
+          break;
+        case (nav2_costmap_2d::NO_INFORMATION):
+          in_collision = false;
+          break;
+        default:
+          in_collision = false;
+      }
+
+
+      if (in_collision) {
         traj_cost = collision_cost_;
         trajectory_collide = true;
         break;
@@ -202,13 +239,14 @@ void CostCritic::score(CriticData & data)
       // Let near-collision trajectory points be punished severely
       // Note that we collision check based on the footprint actual,
       // but score based on the center-point cost regardless
-      if (pose_cost >= static_cast<float>(near_collision_cost_)) {
+      if (cost_for_scoring >= near_collision_cost_ && !near_goal) {
         traj_cost += critical_cost_;
       } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
-        traj_cost += pose_cost;
+        traj_cost += cost_for_scoring;
       }
     }
 
+    (*data.trajectories_in_collision)[i] = trajectory_collide;
     all_trajectories_collide &= trajectory_collide;
   }
 
@@ -219,6 +257,18 @@ void CostCritic::score(CriticData & data)
     data.costs += repulsive_cost * (weight_ / static_cast<float>(strided_traj_cols));
   }
 
+  if (any_use_footprint_this_tick != s_footprint_active) { 
+    static constexpr const char* RESET = "\033[0m";
+    static constexpr const char* GREEN = "\033[32;1m";
+    if (any_use_footprint_this_tick) { 
+      RCLCPP_INFO(logger_, "%s[CostCritic] TURN ON FOOTPRINT COST%s",GREEN, RESET); 
+    } else { 
+      RCLCPP_INFO(logger_, "%s[CostCritic] TURN OFF FOOTPRINT COST%s",GREEN, RESET); 
+    } 
+    s_footprint_active = any_use_footprint_this_tick; 
+  }
+  
+
   data.fail_flag = all_trajectories_collide;
 }
 
diff --git a/nav2_mppi_controller/src/critics/cost_critic_clear.cpp b/nav2_mppi_controller/src/critics/cost_critic_clear.cpp
new file mode 100644
index 00000000000..9ab364dcd0b
--- /dev/null
+++ b/nav2_mppi_controller/src/critics/cost_critic_clear.cpp
@@ -0,0 +1,281 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
+namespace mppi::critics
+{
+
+void CostCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(consider_footprint_, "consider_footprint", false);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 3.81f);
+  getParam(critical_cost_, "critical_cost", 300.0f);
+  getParam(near_collision_cost_, "near_collision_cost", 253);
+  getParam(collision_cost_, "collision_cost", 1000000.0f);
+  getParam(near_goal_distance_, "near_goal_distance", 0.5f);
+  getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
+  getParam(trajectory_point_step_, "trajectory_point_step", 2);
+
+  // Normalized by cost value to put in same regime as other weights
+  weight_ /= 254.0f;
+
+  // Normalize weight when parameter is changed dynamically as well
+  auto weightDynamicCb = [&](
+    const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) {
+      weight_ = weight.as_double() / 254.0f;
+    };
+  parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb);
+
+  collision_checker_.setCostmap(costmap_);
+  possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possible_collision_cost_ < 1.0f) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+    logger_,
+    "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+    "in both the costmap and the cost critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+      "Considering footprint in collision checking but no robot footprint provided in the "
+      "costmap.");
+    }
+  }
+
+  if(near_collision_cost_ > 253) {
+    RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE");
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "InflationCostCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_cost_, weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+float CostCritic::findCircumscribedCost(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  double result = -1.0;
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
+  // check if the costmap has an inflation layer
+  const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(
+    costmap,
+    inflation_layer_name_);
+  if (inflation_layer != nullptr) {
+    const double resolution = costmap->getCostmap()->getResolution();
+    double inflation_radius = inflation_layer->getInflationRadius();
+    if (inflation_radius < circum_radius) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("computeCircumscribedCost"),
+        "The inflation radius (%f) is smaller than the circumscribed radius (%f) "
+        "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+        "field to speed up collision checking by only checking the full footprint "
+        "when robot is within possibly-inscribed radius of an obstacle. This may "
+        "significantly slow down planning times!",
+        inflation_radius, circum_radius);
+      result = 0.0;
+      return result;
+    }
+    result = inflation_layer->computeCost(circum_radius / resolution);
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times and not avoid anything but absolute collisions!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+void CostCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  static bool s_footprint_active = false;
+  bool any_use_footprint_this_tick = false;
+
+  // Setup cost information for various parts of the critic
+  is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+  auto * costmap = collision_checker_.getCostmap();
+  origin_x_ = static_cast<float>(costmap->getOriginX());
+  origin_y_ = static_cast<float>(costmap->getOriginY());
+  resolution_ = static_cast<float>(costmap->getResolution());
+  size_x_ = costmap->getSizeInCellsX();
+  size_y_ = costmap->getSizeInCellsY();
+
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
+  // If near the goal, don't apply the preferential term since the goal is near obstacles
+  bool near_goal = false;
+  if (data.state.local_path_length < near_goal_distance_) {
+    near_goal = true;
+  }
+
+  Eigen::ArrayXf repulsive_cost(data.costs.rows());
+  repulsive_cost.setZero();
+  bool all_trajectories_collide = true;
+
+  int strided_traj_cols = floor((data.trajectories.x.cols() - 1) / trajectory_point_step_) + 1;
+  int strided_traj_rows = data.trajectories.x.rows();
+  int outer_stride = strided_traj_rows * trajectory_point_step_;
+
+    // Initialize trajectories collision tracking
+  if (!data.trajectories_in_collision) {
+    data.trajectories_in_collision = std::vector<bool>(strided_traj_rows, false);
+  }
+
+  const auto traj_x = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+  const auto traj_y = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+  const auto traj_yaw = Eigen::Map<const Eigen::ArrayXXf, 0,
+      Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
+      Eigen::Stride<-1, -1>(outer_stride, 1));
+
+  for (int i = 0; i < strided_traj_rows; ++i) {
+    bool trajectory_collide = false;
+    float pose_cost = 0.0f;
+    float & traj_cost = repulsive_cost(i);
+
+    for (int j = 0; j < strided_traj_cols; j++) {
+      float Tx = traj_x(i, j);
+      float Ty = traj_y(i, j);
+      unsigned int x_i = 0u, y_i = 0u;
+
+      // The getCost doesn't use orientation
+      // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
+      // So the center point has more information than the footprint
+      if (!worldToMapFloat(Tx, Ty, x_i, y_i)) {
+        pose_cost = 0.0f;  // NO_INFORMATION in float
+      } else {
+        pose_cost = static_cast<float>(costmap->getCost(getIndex(x_i, y_i)));
+        if (pose_cost < 1.0f) {
+          continue;  // In free space
+        }
+      }
+
+      float cost_for_scoring = pose_cost;
+      bool use_footprint_cost = consider_footprint_ &&
+        (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f);
+      any_use_footprint_this_tick = any_use_footprint_this_tick || use_footprint_cost;
+
+      if (use_footprint_cost) {
+        cost_for_scoring = static_cast<float>(collision_checker_.footprintCostAtPose(
+            static_cast<double>(Tx), static_cast<double>(Ty), static_cast<double>(traj_yaw(i, j)),
+            costmap_ros_->getRobotFootprint()));
+      }
+
+      // Check for collision based on the appropriate cost
+      bool in_collision = false;
+
+      switch (static_cast<unsigned char>(cost_for_scoring)) {
+        case (nav2_costmap_2d::LETHAL_OBSTACLE):
+          in_collision = true;
+          break;
+        case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+          in_collision = use_footprint_cost ? false : true;
+          break;
+        case (nav2_costmap_2d::NO_INFORMATION):
+          in_collision = is_tracking_unknown_ ? false : true;
+          break;
+        default:
+          in_collision = false;
+      }
+
+
+      if (in_collision) {
+        traj_cost = collision_cost_;
+        trajectory_collide = true;
+        break;
+      }
+
+      // Let near-collision trajectory points be punished severely
+      // Note that we collision check based on the footprint actual,
+      // but score based on the center-point cost regardless
+      if (cost_for_scoring >= near_collision_cost_ && !near_goal) {
+        traj_cost += critical_cost_;
+      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
+        traj_cost += cost_for_scoring;
+      }
+    }
+
+    (*data.trajectories_in_collision)[i] = trajectory_collide;
+    all_trajectories_collide &= trajectory_collide;
+  }
+
+  if (power_ > 1u) {
+    data.costs += (repulsive_cost *
+      (weight_ / static_cast<float>(strided_traj_cols))).pow(power_);
+  } else {
+    data.costs += repulsive_cost * (weight_ / static_cast<float>(strided_traj_cols));
+  }
+
+  if (any_use_footprint_this_tick != s_footprint_active) { 
+    static constexpr const char* RESET = "\033[0m";
+    static constexpr const char* GREEN = "\033[32;1m";
+    if (any_use_footprint_this_tick) { 
+      RCLCPP_INFO(logger_, "%s[CostCritic] TURN ON FOOTPRINT COST%s",GREEN, RESET); 
+    } else { 
+      RCLCPP_INFO(logger_, "%s[CostCritic] TURN OFF FOOTPRINT COST%s",GREEN, RESET); 
+    } 
+    s_footprint_active = any_use_footprint_this_tick; 
+  }
+  
+
+  data.fail_flag = all_trajectories_collide;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::CostCritic,
+  mppi::critics::CriticFunction)
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic copy 3.cpp b/nav2_mppi_controller/src/critics/obstacles_critic copy 3.cpp
new file mode 100644
index 00000000000..0577db0a76f
--- /dev/null
+++ b/nav2_mppi_controller/src/critics/obstacles_critic copy 3.cpp	
@@ -0,0 +1,249 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/obstacles_critic.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
+namespace mppi::critics
+{
+
+void ObstaclesCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(consider_footprint_, "consider_footprint", false);
+  getParam(power_, "cost_power", 1);
+  getParam(repulsion_weight_, "repulsion_weight", 1.5f);
+  getParam(critical_weight_, "critical_weight", 20.0f);
+  getParam(collision_cost_, "collision_cost", 100000.0f);
+  getParam(collision_margin_distance_, "collision_margin_distance", 0.10f);
+  getParam(near_goal_distance_, "near_goal_distance", 0.5f);
+  getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
+
+  collision_checker_.setCostmap(costmap_);
+  possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possible_collision_cost_ < 1.0f) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+    logger_,
+    "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+    "in both the costmap and the obstacle critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+      "Considering footprint in collision checking but no robot footprint provided in the "
+      "costmap.");
+    }
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "ObstaclesCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_weight_, repulsion_weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+float ObstaclesCritic::findCircumscribedCost(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  double result = -1.0;
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
+  // check if the costmap has an inflation layer
+  const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(
+    costmap,
+    inflation_layer_name_);
+  if (inflation_layer != nullptr) {
+    const double resolution = costmap->getCostmap()->getResolution();
+    result = inflation_layer->computeCost(circum_radius / resolution);
+    inflation_scale_factor_ = static_cast<float>(inflation_layer->getCostScalingFactor());
+    inflation_radius_ = static_cast<float>(inflation_layer->getInflationRadius());
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times and not avoid anything but absolute collisions!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost)
+{
+  const float scale_factor = inflation_scale_factor_;
+  const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius();
+  float dist_to_obj = (scale_factor * min_radius - log(cost.cost) + log(253.0f)) / scale_factor;
+
+  // If not footprint collision checking, the cost is using the center point cost and
+  // needs the radius subtracted to obtain the closest distance to the object
+  if (!cost.using_footprint) {
+    dist_to_obj -= min_radius;
+  }
+
+  return dist_to_obj;
+}
+
+void ObstaclesCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
+  // If near the goal, don't apply the preferential term since the goal is near obstacles
+  bool near_goal = false;
+  if (data.state.local_path_length < near_goal_distance_) {
+    near_goal = true;
+  }
+
+  Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size());
+  Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size());
+
+  const unsigned int traj_len = data.trajectories.x.cols();
+  const unsigned int batch_size = data.trajectories.x.rows();
+  bool all_trajectories_collide = true;
+
+  for(unsigned int i = 0; i != batch_size; i++) {
+    bool trajectory_collide = false;
+    float traj_cost = 0.0f;
+    const auto & traj = data.trajectories;
+    CollisionCost pose_cost;
+    raw_cost(i) = 0.0f;
+    repulsive_cost(i) = 0.0f;
+
+    for(unsigned int j = 0; j != traj_len; j++) {
+      pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j));
+      if (pose_cost.cost < 1.0f) {continue;}  // In free space
+
+      if (inCollision(pose_cost.cost)) {
+        trajectory_collide = true;
+        break;
+      }
+
+      // Cannot process repulsion if inflation layer does not exist
+      if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {
+        continue;
+      }
+
+      const float dist_to_obj = distanceToObstacle(pose_cost);
+
+      // Let near-collision trajectory points be punished severely
+      if (dist_to_obj < collision_margin_distance_) {
+        traj_cost += (collision_margin_distance_ - dist_to_obj);
+      }
+
+      // Generally prefer trajectories further from obstacles
+      if (!near_goal) {
+        repulsive_cost[i] += inflation_radius_ - dist_to_obj;
+      }
+    }
+
+    if (!trajectory_collide) {all_trajectories_collide = false;}
+    raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost;
+  }
+
+  // Normalize repulsive cost by trajectory length & lowest score to not overweight importance
+  // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors
+  auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len;
+
+  if (power_ > 1u) {
+    data.costs +=
+      ((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized)).pow(power_);
+  } else {
+    data.costs += (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized);
+  }
+
+  data.fail_flag = all_trajectories_collide;
+}
+
+/**
+  * @brief Checks if cost represents a collision
+  * @param cost Costmap cost
+  * @return bool if in collision
+  */
+bool ObstaclesCritic::inCollision(float cost) const
+{
+  bool is_tracking_unknown =
+    costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+
+  using namespace nav2_costmap_2d; // NOLINT
+  switch (static_cast<unsigned char>(cost)) {
+    case (LETHAL_OBSTACLE):
+      return true;
+    case (INSCRIBED_INFLATED_OBSTACLE):
+      return consider_footprint_ ? false : true;
+    case (NO_INFORMATION):
+      return is_tracking_unknown ? false : true;
+  }
+
+  return false;
+}
+
+CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
+{
+  CollisionCost collision_cost;
+  float & cost = collision_cost.cost;
+  collision_cost.using_footprint = false;
+  unsigned int x_i, y_i;
+  if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
+    cost = nav2_costmap_2d::NO_INFORMATION;
+    return collision_cost;
+  }
+  cost = collision_checker_.pointCost(x_i, y_i);
+
+  if (consider_footprint_ &&
+    (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
+  {
+    cost = static_cast<float>(collision_checker_.footprintCostAtPose(
+        x, y, theta, costmap_ros_->getRobotFootprint()));
+    collision_cost.using_footprint = true;
+  }
+
+  return collision_cost;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::ObstaclesCritic,
+  mppi::critics::CriticFunction)
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic copy.cpp b/nav2_mppi_controller/src/critics/obstacles_critic copy.cpp
new file mode 100644
index 00000000000..387dfd64776
--- /dev/null
+++ b/nav2_mppi_controller/src/critics/obstacles_critic copy.cpp	
@@ -0,0 +1,373 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/obstacles_critic.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
+namespace mppi::critics
+{
+
+void ObstaclesCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(consider_footprint_, "consider_footprint", false);
+  getParam(power_, "cost_power", 1);
+  getParam(repulsion_weight_, "repulsion_weight", 1.5f);
+  getParam(critical_weight_, "critical_weight", 20.0f);
+  getParam(collision_cost_, "collision_cost", 100000.0f);
+  getParam(collision_margin_distance_, "collision_margin_distance", 0.10f);
+  getParam(near_goal_distance_, "near_goal_distance", 0.5f);
+  getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
+  getParam(trajectory_point_step_, "trajectory_point_step", 2);
+
+  //collision_checker_.setCostmap(costmap_);
+  collision_checker_ = std::make_unique<nav2_mppi_controller::MPPICollisionChecker>(
+    costmap_ros_, parent_.lock());
+  possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possible_collision_cost_ < 1.0f) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+    logger_,
+    "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+    "in both the costmap and the obstacle critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+      "Considering footprint in collision checking but no robot footprint provided in the "
+      "costmap.");
+    }
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "ObstaclesCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_weight_, repulsion_weight_, consider_footprint_ ?
+    "footprint" : "circular");
+
+  RCLCPP_WARN(logger_,
+  "Inflation: radius=%.3f scale=%.3f, possible_collision_cost=%.1f, inscribed=%.3f",
+  inflation_radius_, inflation_scale_factor_, possible_collision_cost_,
+  costmap_ros_->getLayeredCostmap()->getInscribedRadius());
+
+}
+
+float ObstaclesCritic::findCircumscribedCost(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  double result = -1.0;
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
+  // check if the costmap has an inflation layer
+  const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(
+    costmap,
+    inflation_layer_name_);
+  if (inflation_layer != nullptr) {
+    const double resolution = costmap->getCostmap()->getResolution();
+    result = inflation_layer->computeCost(circum_radius / resolution);
+    inflation_scale_factor_ = static_cast<float>(inflation_layer->getCostScalingFactor());
+    inflation_radius_ = static_cast<float>(inflation_layer->getInflationRadius());
+  } else {
+    RCLCPP_WARN(
+      logger_,
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times and not avoid anything but absolute collisions!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost)
+{
+  const float scale_factor = inflation_scale_factor_;
+  const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius();
+  float dist_to_obj = (scale_factor * min_radius - log(cost.cost) + log(253.0f)) / scale_factor;
+
+  // If not footprint collision checking, the cost is using the center point cost and
+  // needs the radius subtracted to obtain the closest distance to the object
+  if (!cost.using_footprint) {
+    dist_to_obj -= min_radius;
+  }
+
+  return dist_to_obj;
+}
+
+void ObstaclesCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  collision_checker_->setFootprint(
+    costmap_ros_->getRobotFootprint(),
+    costmap_ros_->getUseRadius(),
+    inflation_layer_name_);
+
+  const auto & fp = costmap_ros_->getRobotFootprint();
+  RCLCPP_WARN(logger_,
+    "[DBG] setFootprint: use_radius=%d, fp_pts=%zu, infl_radius=%.3f, infl_scale=%.3f, circ_cost=%.1f, inscribed=%.3f",
+    costmap_ros_->getUseRadius(), fp.size(),
+    inflation_radius_, inflation_scale_factor_, possible_collision_cost_,
+    costmap_ros_->getLayeredCostmap()->getInscribedRadius());
+  if (!fp.empty()) {
+    float minx=fp[0].x, maxx=fp[0].x, miny=fp[0].y, maxy=fp[0].y;
+    for (auto &p: fp) { minx=std::min(minx,(float)p.x); maxx=std::max(maxx,(float)p.x);
+                        miny=std::min(miny,(float)p.y); maxy=std::max(maxy,(float)p.y); }
+    RCLCPP_WARN(logger_, "[DBG] footprint bounds: x[%.3f, %.3f], y[%.3f, %.3f]", minx, maxx, miny, maxy);
+  }
+
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
+  Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size());
+  Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size());
+
+  //const unsigned int traj_len = data.trajectories.x.cols();
+  //const unsigned int batch_size = data.trajectories.x.rows();
+  const int traj_cols_full = data.trajectories.x.cols();
+  const int traj_rows = data.trajectories.x.rows();
+  const int strided_cols = std::floor((traj_cols_full - 1) / trajectory_point_step_) + 1;
+
+  bool all_trajectories_collide = true;
+
+    // Initialize trajectories collision tracking
+  // if (!data.trajectories_in_collision) {
+  //   data.trajectories_in_collision = std::vector<bool>(batch_size, false);
+  // }
+  if (data.trajectories_in_collision && data.trajectories_in_collision->size() != (size_t)traj_rows) {
+    data.trajectories_in_collision->assign(traj_rows, false);
+  }
+
+
+
+  // for(unsigned int i = 0; i != batch_size; i++) {
+  for (int i = 0; i < traj_rows; ++i) {
+    bool trajectory_collide = false;
+    // float traj_cost = 0.0f;
+    // const auto & traj = data.trajectories;
+    // CollisionCost pose_cost;
+    // raw_cost(i) = 0.0f;
+    // repulsive_cost(i) = 0.0f;
+    float& near_collision_acc = raw_cost(i);
+    float& repulse_acc        = repulsive_cost(i);
+    near_collision_acc = 0.0f;
+    repulse_acc        = 0.0f;
+    
+
+
+    // for(unsigned int j = 0; j != traj_len; j++) {
+    //   pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j));
+    //   if (pose_cost.cost < 1.0f) {continue;}  // In free space
+
+    //   if (inCollision(pose_cost.cost)) {
+    //     trajectory_collide = true;
+    //     break;
+    //   }
+
+    //   // Cannot process repulsion if inflation layer does not exist
+    //   if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {
+    //     continue;
+    //   }
+
+    //   const float dist_to_obj = distanceToObstacle(pose_cost);
+
+    //   // Let near-collision trajectory points be punished severely
+    //   if (dist_to_obj < collision_margin_distance_) {
+    //     traj_cost += (collision_margin_distance_ - dist_to_obj);
+    //   }
+
+    //   // Generally prefer trajectories further from obstacles
+    //   if (!near_goal) {
+    //     repulsive_cost[i] += inflation_radius_ - dist_to_obj;
+    //   }
+    // }
+
+          // 1) Zbierz próbki w stride (dokładnie jak w CostCritic)
+      std::vector<float> xs; xs.reserve(strided_cols);
+      std::vector<float> ys; ys.reserve(strided_cols);
+      std::vector<float> yaws; yaws.reserve(strided_cols);
+  
+      for (int j = 0; j < strided_cols; ++j) {
+        const int col = j * trajectory_point_step_;
+        if (col >= traj_cols_full) break;
+        xs.push_back(data.trajectories.x(i, col));
+        ys.push_back(data.trajectories.y(i, col));
+        yaws.push_back(data.trajectories.yaws(i, col));
+      }
+  
+      // 2) Jedno batched sprawdzenie
+      auto result = collision_checker_->inCollision(
+          xs, ys, yaws,
+          /* treat_unknown_as_obstacle = */ false);
+        
+      if (i == 0) {
+        auto vminmax = [](const std::vector<float>& v){
+          if (v.empty()) return std::make_pair(0.f, 0.f);
+          float mn=v[0], mx=v[0];
+          for (auto& z: v) { mn=std::min(mn,z); mx=std::max(mx,z); }
+          return std::make_pair(mn,mx);
+        };
+        auto cc = vminmax(result.center_cost);
+        auto fc = vminmax(result.footprint_cost);
+        RCLCPP_WARN(logger_,
+          "[DBG] inCollision: in=%d, N=%zu, center[min=%.1f max=%.1f], footprint[min=%.1f max=%.1f]",
+          (int)result.in_collision, result.center_cost.size(),
+          cc.first, cc.second, fc.first, fc.second);
+
+        // pokaż też kilka próbek z trajektorii
+        for (size_t s = 0; s < std::min<size_t>(5, xs.size()); ++s) {
+          RCLCPP_WARN(logger_, "[DBG] sample[%zu]: x=%.3f y=%.3f yaw=%.3f", s, xs[s], ys[s], yaws[s]);
+        }
+      }
+  
+      if (result.in_collision) {
+        trajectory_collide = true;
+        near_collision_acc = collision_cost_;
+      } else {
+        // 3) „Near-collision”    preferencja dystansu – identycznie jak u Ciebie,
+        //    ale karmione danymi z 'result'
+        for (size_t k = 0; k < result.center_cost.size(); ++k) {
+          const bool have_fp = (k < result.footprint_cost.size() && result.footprint_cost[k] >= 0.0f);
+          float cost = have_fp ? result.footprint_cost[k] : result.center_cost[k];
+          if (cost < 1.0f) continue; // free
+  
+          float dist = distanceToObstacle(CollisionCost{cost, have_fp});
+  
+          // „twardy” element: kara tuż przed kolizją
+          if (dist < collision_margin_distance_) {
+            near_collision_acc += (collision_margin_distance_ - dist);
+          }
+          // „miękki” element: delikatna preferencja dalej od przeszkód
+          if (inflation_radius_ > 0.f && inflation_scale_factor_ > 0.f && data.state.local_path_length >= near_goal_distance_) {
+            repulse_acc += std::max(0.f, inflation_radius_ - dist);
+          }
+        }
+      }
+
+    // if (!trajectory_collide) {all_trajectories_collide = false;}
+    // (*data.trajectories_in_collision)[i] = trajectory_collide;
+    // raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost;
+      if (!trajectory_collide) { all_trajectories_collide = false; }
+        if (data.trajectories_in_collision) {
+          (*data.trajectories_in_collision)[i] = trajectory_collide;
+      }
+  }
+
+  // Normalize repulsive cost by trajectory length & lowest score to not overweight importance
+  // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors
+  //auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len;
+  auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / std::max(1, strided_cols);
+
+  if (power_ > 1u) {
+    data.costs +=
+      ((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized)).pow(power_);
+  } else {
+    data.costs += (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized);
+  }
+
+  static int dbg_counter = 0;
+  if (++dbg_counter % 20 == 0) {
+    int coll_count = 0;
+    for (int i = 0; i < traj_rows; ++i) {
+      if (data.trajectories_in_collision && (*data.trajectories_in_collision)[i]) coll_count++;
+    }
+    RCLCPP_WARN(logger_,
+      "MPPI ObstaclesCritic: rows=%d, samples/row≈%d, collide_rows=%d, raw_cost_min=%.3f max=%.3f rep_min=%.3f max=%.3f",
+      traj_rows, strided_cols,
+      coll_count,
+      raw_cost.minCoeff(), raw_cost.maxCoeff(),
+      repulsive_cost.minCoeff(), repulsive_cost.maxCoeff());
+  }
+
+  data.fail_flag = all_trajectories_collide;
+}
+
+/**
+  * @brief Checks if cost represents a collision
+  * @param cost Costmap cost
+  * @return bool if in collision
+  */
+bool ObstaclesCritic::inCollision(float cost) const
+{
+  bool is_tracking_unknown =
+    costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+
+  using namespace nav2_costmap_2d; // NOLINT
+  switch (static_cast<unsigned char>(cost)) {
+    case (LETHAL_OBSTACLE):
+      return true;
+    case (INSCRIBED_INFLATED_OBSTACLE):
+      return consider_footprint_ ? false : true;
+    case (NO_INFORMATION):
+      return is_tracking_unknown ? false : true;
+  }
+
+  return false;
+}
+
+// CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
+// {
+//   CollisionCost collision_cost;
+//   float & cost = collision_cost.cost;
+//   collision_cost.using_footprint = false;
+//   unsigned int x_i, y_i;
+//   if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
+//     cost = nav2_costmap_2d::NO_INFORMATION;
+//     return collision_cost;
+//   }
+//   cost = collision_checker_.pointCost(x_i, y_i);
+
+//   if (consider_footprint_ &&
+//     (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
+//   {
+//     cost = static_cast<float>(collision_checker_.footprintCostAtPose(
+//         x, y, theta, costmap_ros_->getRobotFootprint()));
+//     collision_cost.using_footprint = true;
+//   }
+
+//   return collision_cost;
+// }
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::ObstaclesCritic,
+  mppi::critics::CriticFunction)
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
index 0577db0a76f..2a02150e9e1 100644
--- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp
+++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -141,6 +141,12 @@ void ObstaclesCritic::score(CriticData & data)
   const unsigned int batch_size = data.trajectories.x.rows();
   bool all_trajectories_collide = true;
 
+    // Initialize trajectories collision tracking
+  if (!data.trajectories_in_collision) {
+    data.trajectories_in_collision = std::vector<bool>(batch_size, false);
+  }
+
+
   for(unsigned int i = 0; i != batch_size; i++) {
     bool trajectory_collide = false;
     float traj_cost = 0.0f;
@@ -177,6 +183,7 @@ void ObstaclesCritic::score(CriticData & data)
     }
 
     if (!trajectory_collide) {all_trajectories_collide = false;}
+    (*data.trajectories_in_collision)[i] = trajectory_collide;
     raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost;
   }
 
diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
index befa9e42eb9..028130de611 100644
--- a/nav2_mppi_controller/src/critics/path_align_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -30,6 +30,17 @@ void PathAlignCritic::initialize()
     threshold_to_consider_,
     "threshold_to_consider", 0.5f);
   getParam(use_path_orientations_, "use_path_orientations", false);
+  getParam(visualize_furthest_point_, "visualize_furthest_point", false);
+
+  if (visualize_furthest_point_) {
+    auto node = parent_.lock();
+    if (node) {
+      furthest_point_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+        "PathAlignCritic/furthest_reached_path_point", 1);
+      furthest_point_pub_->on_activate();
+    }
+  }
+
 
   RCLCPP_INFO(
     logger_,
@@ -48,6 +59,23 @@ void PathAlignCritic::score(CriticData & data)
   // Up to furthest only, closest path point is always 0 from path handler
   const size_t path_segments_count = *data.furthest_reached_path_point;
   float path_segments_flt = static_cast<float>(path_segments_count);
+
+  // Visualize target pose if enabled
+  if (visualize_furthest_point_ && path_segments_count > 0 &&
+    furthest_point_pub_->get_subscription_count() > 0)
+  {
+    auto furthest_point = std::make_unique<geometry_msgs::msg::PoseStamped>();
+    furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID();
+    furthest_point->header.stamp = clock_->now();
+    furthest_point->pose.position.x = data.path.x(path_segments_count);
+    furthest_point->pose.position.y = data.path.y(path_segments_count);
+    furthest_point->pose.position.z = 0.0;
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, data.path.yaws(path_segments_count));
+    furthest_point->pose.orientation = tf2::toMsg(quat);
+    furthest_point_pub_->publish(std::move(furthest_point));
+  }
+
   if (path_segments_count < offset_from_furthest_) {
     return;
   }
diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
index 6f42d33b042..68901f1c2af 100644
--- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -41,6 +41,7 @@ void PathAngleCritic::initialize()
   getParam(
     max_angle_to_furthest_,
     "max_angle_to_furthest", 0.785398f);
+  getParam(visualize_furthest_point_, "visualize_furthest_point", false);
 
   int mode = 0;
   getParam(mode, "mode", mode);
@@ -53,6 +54,15 @@ void PathAngleCritic::initialize()
       "don't allow for reversing! Setting mode to forward preference.");
   }
 
+  if (visualize_furthest_point_) {
+    auto node = parent_.lock();
+    if (node) {
+      furthest_point_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+      "PathAngleCritic/furthest_reached_path_point", 1);
+      furthest_point_pub_->on_activate();
+    }
+  }
+
   RCLCPP_INFO(
     logger_,
     "PathAngleCritic instantiated with %d power and %f weight. Mode set to: %s",
@@ -74,6 +84,21 @@ void PathAngleCritic::score(CriticData & data)
   const float goal_y = data.path.y(offsetted_idx);
   const float goal_yaw = data.path.yaws(offsetted_idx);
   const geometry_msgs::msg::Pose & pose = data.state.pose.pose;
+  
+  // Visualize target pose if enabled
+  if (visualize_furthest_point_ && furthest_point_pub_->get_subscription_count() > 0) {
+    auto furthest_point = std::make_unique<geometry_msgs::msg::PoseStamped>();
+    furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID();
+    furthest_point->header.stamp = clock_->now();
+    furthest_point->pose.position.x = goal_x;
+    furthest_point->pose.position.y = goal_y;
+    furthest_point->pose.position.z = 0.0;
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, goal_yaw);
+    furthest_point->pose.orientation = tf2::toMsg(quat);
+    furthest_point_pub_->publish(std::move(furthest_point));
+  }
+
 
   switch (mode_) {
     case PathAngleMode::FORWARD_PREFERENCE:
diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp
index 240c49725d6..e62a465a270 100644
--- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp
@@ -29,6 +29,17 @@ void PathFollowCritic::initialize()
   getParam(offset_from_furthest_, "offset_from_furthest", 6);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 5.0f);
+  getParam(visualize_furthest_point_, "visualize_furthest_point", false);
+
+  if (visualize_furthest_point_) {
+    auto node = parent_.lock();
+    if (node) {
+      furthest_point_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(
+        "PathFollowCritic/furthest_reached_path_point", 1);
+      furthest_point_pub_->on_activate();
+    }
+  }
+
 }
 
 void PathFollowCritic::score(CriticData & data)
@@ -60,6 +71,20 @@ void PathFollowCritic::score(CriticData & data)
 
   const auto path_x = data.path.x(offsetted_idx);
   const auto path_y = data.path.y(offsetted_idx);
+  // Visualize target pose if enabled
+  if (visualize_furthest_point_ && furthest_point_pub_->get_subscription_count() > 0) {
+    auto furthest_point = std::make_unique<geometry_msgs::msg::PoseStamped>();
+    furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID();
+    furthest_point->header.stamp = clock_->now();
+    furthest_point->pose.position.x = path_x;
+    furthest_point->pose.position.y = path_y;
+    furthest_point->pose.position.z = 0.0;
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, data.path.yaws(offsetted_idx));
+    furthest_point->pose.orientation = tf2::toMsg(quat);
+    furthest_point_pub_->publish(std::move(furthest_point));
+  }
+
 
   const int && rightmost_idx = data.trajectories.x.cols() - 1;
   const auto last_x = data.trajectories.x.col(rightmost_idx);
diff --git a/nav2_mppi_controller/src/critics/waypoint_critic.cpp b/nav2_mppi_controller/src/critics/waypoint_critic.cpp
new file mode 100644
index 00000000000..ca8d5f1c3b6
--- /dev/null
+++ b/nav2_mppi_controller/src/critics/waypoint_critic.cpp
@@ -0,0 +1,207 @@
+// Copyright (c) 2025
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/waypoint_critic.hpp"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2/time.h>
+
+namespace mppi::critics
+{
+
+void WaypointCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 1.0f);
+  getParam(position_tolerance_, "position_tolerance", 0.1f);
+  getParam(yaw_tolerance_, "yaw_tolerance", 0.1f);
+  getParam(waypoint_topic_, "waypoint_topic", std::string("truncated_path"));
+  getParam(use_orientation_, "use_orientation", true);
+  getParam(max_waypoint_distance_, "max_waypoint_distance", 10.0f);
+
+  RCLCPP_INFO(
+    logger_, 
+    "WaypointCritic instantiated with %d power, %f weight, "
+    "position tolerance: %f, yaw tolerance: %f, topic: %s",
+    power_, weight_, position_tolerance_, yaw_tolerance_, waypoint_topic_.c_str());
+
+  // Create subscription to waypoint topic
+  auto node = parent_.lock();
+  if (node) {
+    waypoint_sub_ = node->create_subscription<nav_msgs::msg::Path>(
+      waypoint_topic_,
+      std::bind(&WaypointCritic::waypointCallback, this, std::placeholders::_1),
+      rclcpp::QoS(10).reliable());
+      tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+      tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, node, false);
+
+    RCLCPP_INFO(logger_, "WaypointCritic subscribed to topic: %s", waypoint_topic_.c_str());
+  } else {
+    RCLCPP_ERROR(logger_, "Failed to get parent node for WaypointCritic subscription");
+  }
+}
+
+void WaypointCritic::waypointCallback(const nav_msgs::msg::Path::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(waypoints_mutex_);
+  waypoints_ = msg->poses;
+  waypoints_frame_ = msg->header.frame_id;
+  RCLCPP_INFO(
+    logger_, 
+    "WaypointCritic received %zu waypoints", 
+    waypoints_.size());
+}
+
+void WaypointCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  std::lock_guard<std::mutex> lock(waypoints_mutex_);
+  
+  // If no waypoints available, don't apply any cost
+  if (waypoints_.empty()) {
+    return;
+  }
+
+  // Get current robot pose
+  const auto & robot_x = data.state.pose.pose.position.x;
+  const auto & robot_y = data.state.pose.pose.position.y;
+  
+  // Always target the first waypoint (assuming completed waypoints are removed by publisher)
+  int target_waypoint_idx = getTargetWaypoint();
+  
+  if (target_waypoint_idx < 0) {
+    return;
+  }
+
+  const std::string target_frame = data.state.pose.header.frame_id;
+  geometry_msgs::msg::PoseStamped wp_in = waypoints_[target_waypoint_idx];
+  if (wp_in.header.frame_id.empty()) {
+    wp_in.header.frame_id = waypoints_frame_;
+  }
+  geometry_msgs::msg::PoseStamped wp_tf;
+  if (wp_in.header.frame_id == target_frame || !tf_buffer_) {
+    wp_tf = wp_in;
+  } else {
+    if (!tf_buffer_->canTransform(
+        target_frame, wp_in.header.frame_id, wp_in.header.stamp,
+        tf2::durationFromSec(0.0)))
+    {
+    RCLCPP_WARN(logger_, "TF %s->%s not available yet", 
+                wp_in.header.frame_id.c_str(), target_frame.c_str());
+    return;
+    }
+    tf_buffer_->transform(wp_in, wp_tf, target_frame, tf2::durationFromSec(0.0));
+  }
+  const float target_x = static_cast<float>(wp_tf.pose.position.x);
+  const float target_y = static_cast<float>(wp_tf.pose.position.y);
+  const float target_yaw = static_cast<float>(tf2::getYaw(wp_tf.pose.orientation));
+
+
+//   const auto & plan = data.path;
+//   if (plan.x.size() == 0) {
+//     return;
+//   }
+//   RCLCPP_INFO(
+//   logger_,
+//   "data.path: size=%zu frame=%s",
+//   static_cast<size_t>(plan.x.size()),
+//   data.state.pose.header.frame_id.c_str());
+//   const float target_x   = plan.x(0);
+//   const float target_y   = plan.y(0);
+//   const float target_yaw = (plan.yaws.size() > 0) ? plan.yaws(0) : 0.0f;
+
+
+  // Check if waypoint is within reasonable distance
+  double waypoint_distance = calculateDistance(robot_x, robot_y, target_x, target_y);
+  if (waypoint_distance > max_waypoint_distance_) {
+    return;
+  }
+
+
+  // Calculate position costs for all trajectories
+  const auto delta_x = data.trajectories.x - target_x;
+  const auto delta_y = data.trajectories.y - target_y;
+    // RCLCPP_INFO(
+    // logger_,
+    // "Traj mean: x=%.3f y=%.3f | target=(%.3f, %.3f)",
+    // static_cast<double>(data.trajectories.x.mean()),
+    // static_cast<double>(data.trajectories.y.mean()),
+    // target_x, target_y);
+  
+  // Calculate euclidean distance cost
+  auto position_distances = (delta_x.square() + delta_y.square()).sqrt();
+  
+  // Apply position tolerance - penalize trajectories that are farther than tolerance
+  auto position_costs = (position_distances - position_tolerance_).cwiseMax(0.0f);
+
+  // If orientation is considered, add yaw cost
+  Eigen::ArrayXf total_costs = position_costs;
+  
+  if (use_orientation_) {
+    // Calculate yaw differences for all trajectories using utils function  
+    auto yaw_differences = utils::shortest_angular_distance(data.trajectories.yaws, target_yaw);
+    
+    // Apply yaw tolerance
+    auto yaw_costs = (yaw_differences.abs() - yaw_tolerance_).cwiseMax(0.0f);
+    
+    // Combine position and yaw costs
+    total_costs = position_costs + yaw_costs;
+  }
+
+  // Apply power and weight
+  if (power_ > 1u) {
+    data.costs += (total_costs.rowwise().mean() * weight_).pow(power_);
+  } else {
+    data.costs += (total_costs.rowwise().mean() * weight_).eval();
+  }
+}
+
+int WaypointCritic::getTargetWaypoint()
+{
+  if (waypoints_.empty()) {
+    return -1;
+  }
+
+  // Always target the first waypoint in the list
+  // (assuming the publisher removes completed waypoints)
+  return 0;
+}
+
+double WaypointCritic::calculateDistance(double x1, double y1, double x2, double y2)
+{
+  double dx = x2 - x1;
+  double dy = y2 - y1;
+  return sqrt(dx * dx + dy * dy);
+}
+
+double WaypointCritic::calculateAngularDifference(double angle1, double angle2)
+{
+  double diff = angle2 - angle1;
+  while (diff > M_PI) diff -= 2.0 * M_PI;
+  while (diff < -M_PI) diff += 2.0 * M_PI;
+  return diff;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(mppi::critics::WaypointCritic, mppi::critics::CriticFunction)
\ No newline at end of file
diff --git a/nav2_mppi_controller/src/optimizer copy 2.cpp b/nav2_mppi_controller/src/optimizer copy 2.cpp
new file mode 100644
index 00000000000..013b29de502
--- /dev/null
+++ b/nav2_mppi_controller/src/optimizer copy 2.cpp	
@@ -0,0 +1,586 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2025 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/optimizer.hpp"
+
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+#include <cmath>
+#include <chrono>
+
+#include "nav2_core/controller_exceptions.hpp"
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_ros_common/node_utils.hpp"
+
+namespace mppi
+{
+
+void Optimizer::initialize(
+  nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  ParametersHandler * param_handler)
+{
+  parent_ = parent;
+  name_ = name;
+  costmap_ros_ = costmap_ros;
+  costmap_ = costmap_ros_->getCostmap();
+  parameters_handler_ = param_handler;
+  tf_buffer_ = tf_buffer;
+
+  auto node = parent_.lock();
+  logger_ = node->get_logger();
+
+  getParams();
+
+  critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_);
+  noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_);
+
+  // This may throw an exception if not valid and fail initialization
+  nav2::declare_parameter_if_not_declared(
+    node, name_ + ".TrajectoryValidator.plugin",
+    rclcpp::ParameterValue("mppi::DefaultOptimalTrajectoryValidator"));
+  std::string validator_plugin_type = nav2::get_plugin_type_param(
+    node, name_ + ".TrajectoryValidator");
+  validator_loader_ = std::make_unique<pluginlib::ClassLoader<OptimalTrajectoryValidator>>(
+    "nav2_mppi_controller", "mppi::OptimalTrajectoryValidator");
+  trajectory_validator_ = validator_loader_->createUniqueInstance(validator_plugin_type);
+  trajectory_validator_->initialize(
+    parent_, name_ + ".TrajectoryValidator",
+    costmap_ros_, parameters_handler_, tf_buffer, settings_);
+  RCLCPP_INFO(logger_, "Loaded trajectory validator plugin: %s", validator_plugin_type.c_str());
+
+  reset();
+}
+
+void Optimizer::shutdown()
+{
+  noise_generator_.shutdown();
+}
+
+void Optimizer::getParams()
+{
+  std::string motion_model_name;
+
+  auto & s = settings_;
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  auto getParentParam = parameters_handler_->getParamGetter("");
+  getParam(s.model_dt, "model_dt", 0.05f);
+  getParam(s.time_steps, "time_steps", 56);
+  getParam(s.batch_size, "batch_size", 1000);
+  getParam(s.iteration_count, "iteration_count", 1);
+  getParam(s.temperature, "temperature", 0.3f);
+  getParam(s.gamma, "gamma", 0.015f);
+  getParam(s.base_constraints.vx_max, "vx_max", 0.5f);
+  getParam(s.base_constraints.vx_min, "vx_min", -0.35f);
+  getParam(s.base_constraints.vy, "vy_max", 0.5f);
+  getParam(s.base_constraints.wz, "wz_max", 1.9f);
+  getParam(s.base_constraints.ax_max, "ax_max", 3.0f);
+  getParam(s.base_constraints.ax_min, "ax_min", -3.0f);
+  getParam(s.base_constraints.ay_max, "ay_max", 3.0f);
+  getParam(s.base_constraints.ay_min, "ay_min", -3.0f);
+  getParam(s.base_constraints.az_max, "az_max", 3.5f);
+  getParam(s.sampling_std.vx, "vx_std", 0.2f);
+  getParam(s.sampling_std.vy, "vy_std", 0.2f);
+  getParam(s.sampling_std.wz, "wz_std", 0.4f);
+  getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
+
+  s.base_constraints.ax_max = fabs(s.base_constraints.ax_max);
+  if (s.base_constraints.ax_min > 0.0) {
+    s.base_constraints.ax_min = -1.0 * s.base_constraints.ax_min;
+    RCLCPP_WARN(
+      logger_,
+      "Sign of the parameter ax_min is incorrect, consider setting it negative.");
+  }
+
+  if (s.base_constraints.ay_min > 0.0) {
+    s.base_constraints.ay_min = -1.0 * s.base_constraints.ay_min;
+    RCLCPP_WARN(
+      logger_,
+      "Sign of the parameter ay_min is incorrect, consider setting it negative.");
+  }
+
+
+  getParam(motion_model_name, "motion_model", std::string("DiffDrive"));
+
+  s.constraints = s.base_constraints;
+
+  setMotionModel(motion_model_name);
+  parameters_handler_->addPostCallback([this]() {reset();});
+
+  double controller_frequency;
+  getParentParam(controller_frequency, "controller_frequency", 0.0, ParameterType::Static);
+  setOffset(controller_frequency);
+}
+
+void Optimizer::setOffset(double controller_frequency)
+{
+  const double controller_period = 1.0 / controller_frequency;
+  constexpr double eps = 1e-6;
+
+  if ((controller_period + eps) < settings_.model_dt) {
+    RCLCPP_WARN(
+      logger_,
+      "Controller period is less then model dt, consider setting it equal");
+  } else if (abs(controller_period - settings_.model_dt) < eps) {
+    RCLCPP_INFO(
+      logger_,
+      "Controller period is equal to model dt. Control sequence "
+      "shifting is ON");
+    settings_.shift_control_sequence = true;
+  } else {
+    throw nav2_core::ControllerException(
+            "Controller period more then model dt, set it equal to model dt");
+  }
+}
+
+void Optimizer::reset(bool reset_dynamic_speed_limits)
+{
+  state_.reset(settings_.batch_size, settings_.time_steps);
+  control_sequence_.reset(settings_.time_steps);
+  control_history_[0] = {0.0f, 0.0f, 0.0f};
+  control_history_[1] = {0.0f, 0.0f, 0.0f};
+  control_history_[2] = {0.0f, 0.0f, 0.0f};
+  control_history_[3] = {0.0f, 0.0f, 0.0f};
+
+  if (reset_dynamic_speed_limits) {
+    settings_.constraints = settings_.base_constraints;
+  }
+
+  costs_.setZero(settings_.batch_size);
+  generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);
+
+  noise_generator_.reset(settings_, isHolonomic());
+  motion_model_->initialize(settings_.constraints, settings_.model_dt);
+  trajectory_validator_->initialize(
+    parent_, name_ + ".TrajectoryValidator",
+    costmap_ros_, parameters_handler_, tf_buffer_, settings_);
+
+  RCLCPP_INFO(logger_, "Optimizer reset");
+}
+
+bool Optimizer::isHolonomic() const
+{
+  return motion_model_->isHolonomic();
+}
+
+std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalControl(
+  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Twist & robot_speed,
+  const nav_msgs::msg::Path & plan,
+  const geometry_msgs::msg::Pose & goal,
+  nav2_core::GoalChecker * goal_checker)
+{
+  prepare(robot_pose, robot_speed, plan, goal, goal_checker);
+  Eigen::ArrayXXf optimal_trajectory;
+  bool trajectory_valid = true;
+
+  do {
+    optimize();
+    optimal_trajectory = getOptimizedTrajectory();
+    switch (trajectory_validator_->validateTrajectory(
+      optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal))
+    {
+      case mppi::ValidationResult::SOFT_RESET:
+        trajectory_valid = false;
+        RCLCPP_WARN(logger_, "Soft reset triggered by trajectory validator");
+        break;
+      case mppi::ValidationResult::FAILURE:
+        throw nav2_core::NoValidControl(
+          "Trajectory validator failed to validate trajectory, hard reset triggered.");
+      case mppi::ValidationResult::SUCCESS:
+      default:
+        trajectory_valid = true;
+        break;
+    }
+  } while (fallback(critics_data_.fail_flag || !trajectory_valid));
+
+  utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
+  auto control = getControlFromSequenceAsTwist(plan.header.stamp);
+
+  if (settings_.shift_control_sequence) {
+    shiftControlSequence();
+  }
+
+  return std::make_tuple(control, optimal_trajectory);
+}
+
+void Optimizer::optimize()
+{
+  for (size_t i = 0; i < settings_.iteration_count; ++i) {
+    generateNoisedTrajectories();
+    critic_manager_.evalTrajectoriesScores(critics_data_);
+    updateControlSequence();
+  }
+}
+
+bool Optimizer::fallback(bool fail)
+{
+  static size_t counter = 0;
+
+  if (!fail) {
+    counter = 0;
+    return false;
+  }
+
+  reset();
+
+  if (++counter > settings_.retry_attempt_limit) {
+    counter = 0;
+    throw nav2_core::NoValidControl("Optimizer fail to compute path");
+  }
+
+  return true;
+}
+
+void Optimizer::prepare(
+  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Twist & robot_speed,
+  const nav_msgs::msg::Path & plan,
+  const geometry_msgs::msg::Pose & goal,
+  nav2_core::GoalChecker * goal_checker)
+{
+  state_.pose = robot_pose;
+  state_.speed = robot_speed;
+  state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan);
+  path_ = utils::toTensor(plan);
+  costs_.setZero();
+  goal_ = goal;
+
+  critics_data_.fail_flag = false;
+  critics_data_.goal_checker = goal_checker;
+  critics_data_.motion_model = motion_model_;
+  critics_data_.furthest_reached_path_point.reset();
+  critics_data_.path_pts_valid.reset();
+}
+
+void Optimizer::shiftControlSequence()
+{
+  auto size = control_sequence_.vx.size();
+  utils::shiftColumnsByOnePlace(control_sequence_.vx, -1);
+  utils::shiftColumnsByOnePlace(control_sequence_.wz, -1);
+  control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);
+  control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);
+
+  if (isHolonomic()) {
+    utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);
+    control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);
+  }
+}
+
+void Optimizer::generateNoisedTrajectories()
+{
+  noise_generator_.setNoisedControls(state_, control_sequence_);
+  noise_generator_.generateNextNoises();
+  updateStateVelocities(state_);
+  integrateStateVelocities(generated_trajectories_, state_);
+}
+
+void Optimizer::applyControlSequenceConstraints()
+{
+  auto & s = settings_;
+
+  float max_delta_vx = s.model_dt * s.constraints.ax_max;
+  float min_delta_vx = s.model_dt * s.constraints.ax_min;
+  float max_delta_vy = s.model_dt * s.constraints.ay_max;
+  float min_delta_vy = s.model_dt * s.constraints.ay_min;
+  float max_delta_wz = s.model_dt * s.constraints.az_max;
+  float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, control_sequence_.vx(0));
+  float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0));
+  control_sequence_.vx(0) = vx_last;
+  control_sequence_.wz(0) = wz_last;
+  float vy_last = 0;
+  if (isHolonomic()) {
+    vy_last = utils::clamp(-s.constraints.vy, s.constraints.vy, control_sequence_.vy(0));
+    control_sequence_.vy(0) = vy_last;
+  }
+
+  for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) {
+    float & vx_curr = control_sequence_.vx(i);
+    vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr);
+    if(vx_last > 0) {
+      vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr);
+    } else {
+      vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr);
+    }
+    vx_last = vx_curr;
+
+    float & wz_curr = control_sequence_.wz(i);
+    wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr);
+    wz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, wz_curr);
+    wz_last = wz_curr;
+
+    if (isHolonomic()) {
+      float & vy_curr = control_sequence_.vy(i);
+      vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr);
+      if(vy_last > 0) {
+        vy_curr = utils::clamp(vy_last + min_delta_vy, vy_last + max_delta_vy, vy_curr);
+      } else {
+        vy_curr = utils::clamp(vy_last - max_delta_vy, vy_last - min_delta_vy, vy_curr);
+      }
+      vy_last = vy_curr;
+    }
+  }
+
+  motion_model_->applyConstraints(control_sequence_);
+}
+
+void Optimizer::updateStateVelocities(
+  models::State & state) const
+{
+  updateInitialStateVelocities(state);
+  propagateStateVelocitiesFromInitials(state);
+}
+
+void Optimizer::updateInitialStateVelocities(
+  models::State & state) const
+{
+  state.vx.col(0) = static_cast<float>(state.speed.linear.x);
+  state.wz.col(0) = static_cast<float>(state.speed.angular.z);
+
+  if (isHolonomic()) {
+    state.vy.col(0) = static_cast<float>(state.speed.linear.y);
+  }
+}
+
+void Optimizer::propagateStateVelocitiesFromInitials(
+  models::State & state) const
+{
+  motion_model_->predict(state);
+}
+
+void Optimizer::integrateStateVelocities(
+  Eigen::Array<float, Eigen::Dynamic, 3> & trajectory,
+  const Eigen::ArrayXXf & sequence) const
+{
+  float initial_yaw = static_cast<float>(tf2::getYaw(state_.pose.pose.orientation));
+
+  const auto vx = sequence.col(0);
+  const auto wz = sequence.col(1);
+
+  auto traj_x = trajectory.col(0);
+  auto traj_y = trajectory.col(1);
+  auto traj_yaws = trajectory.col(2);
+
+  const size_t n_size = traj_yaws.size();
+  if (n_size == 0) {
+    return;
+  }
+
+  float last_yaw = initial_yaw;
+  for(size_t i = 0; i != n_size; i++) {
+    last_yaw += wz(i) * settings_.model_dt;
+    traj_yaws(i) = last_yaw;
+  }
+
+  Eigen::ArrayXf yaw_cos = traj_yaws.cos();
+  Eigen::ArrayXf yaw_sin = traj_yaws.sin();
+  utils::shiftColumnsByOnePlace(yaw_cos, 1);
+  utils::shiftColumnsByOnePlace(yaw_sin, 1);
+  yaw_cos(0) = cosf(initial_yaw);
+  yaw_sin(0) = sinf(initial_yaw);
+
+  auto dx = (vx * yaw_cos).eval();
+  auto dy = (vx * yaw_sin).eval();
+
+  if (isHolonomic()) {
+    auto vy = sequence.col(2);
+    dx = (dx - vy * yaw_sin).eval();
+    dy = (dy + vy * yaw_cos).eval();
+  }
+
+  float last_x = state_.pose.pose.position.x;
+  float last_y = state_.pose.pose.position.y;
+  for(size_t i = 0; i != n_size; i++) {
+    last_x += dx(i) * settings_.model_dt;
+    last_y += dy(i) * settings_.model_dt;
+    traj_x(i) = last_x;
+    traj_y(i) = last_y;
+  }
+}
+
+void Optimizer::integrateStateVelocities(
+  models::Trajectories & trajectories,
+  const models::State & state) const
+{
+  auto initial_yaw = static_cast<float>(tf2::getYaw(state.pose.pose.orientation));
+  const size_t n_cols = trajectories.yaws.cols();
+
+  Eigen::ArrayXf last_yaws = Eigen::ArrayXf::Constant(trajectories.yaws.rows(), initial_yaw);
+  for (size_t i = 0; i != n_cols; i++) {
+    last_yaws += state.wz.col(i) * settings_.model_dt;
+    trajectories.yaws.col(i) = last_yaws;
+  }
+
+  Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos();
+  Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin();
+  utils::shiftColumnsByOnePlace(yaw_cos, 1);
+  utils::shiftColumnsByOnePlace(yaw_sin, 1);
+  yaw_cos.col(0) = cosf(initial_yaw);
+  yaw_sin.col(0) = sinf(initial_yaw);
+
+  auto dx = (state.vx * yaw_cos).eval();
+  auto dy = (state.vx * yaw_sin).eval();
+
+  if (isHolonomic()) {
+    dx -= state.vy * yaw_sin;
+    dy += state.vy * yaw_cos;
+  }
+
+  Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant(trajectories.x.rows(),
+    state.pose.pose.position.x);
+  Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant(trajectories.y.rows(),
+    state.pose.pose.position.y);
+
+  for (size_t i = 0; i != n_cols; i++) {
+    last_x += dx.col(i) * settings_.model_dt;
+    last_y += dy.col(i) * settings_.model_dt;
+    trajectories.x.col(i) = last_x;
+    trajectories.y.col(i) = last_y;
+  }
+}
+
+Eigen::ArrayXXf Optimizer::getOptimizedTrajectory()
+{
+  const bool is_holo = isHolonomic();
+  Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2);
+  Eigen::Array<float, Eigen::Dynamic, 3> trajectories =
+    Eigen::Array<float, Eigen::Dynamic, 3>(settings_.time_steps, 3);
+
+  sequence.col(0) = control_sequence_.vx;
+  sequence.col(1) = control_sequence_.wz;
+
+  if (is_holo) {
+    sequence.col(2) = control_sequence_.vy;
+  }
+
+  integrateStateVelocities(trajectories, sequence);
+  return trajectories;
+}
+
+const models::ControlSequence & Optimizer::getOptimalControlSequence()
+{
+  return control_sequence_;
+}
+
+void Optimizer::updateControlSequence()
+{
+  const bool is_holo = isHolonomic();
+  auto & s = settings_;
+
+  auto vx_T = control_sequence_.vx.transpose();
+  auto bounded_noises_vx = state_.cvx.rowwise() - vx_T;
+  const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx);
+  costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval();
+
+  if (s.sampling_std.wz > 0.0f) {
+    auto wz_T = control_sequence_.wz.transpose();
+    auto bounded_noises_wz = state_.cwz.rowwise() - wz_T;
+    const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz);
+    costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval();
+  }
+
+  if (is_holo) {
+    auto vy_T = control_sequence_.vy.transpose();
+    auto bounded_noises_vy = state_.cvy.rowwise() - vy_T;
+    const float gamma_vy = s.gamma / (s.sampling_std.vy * s.sampling_std.vy);
+    costs_ += (gamma_vy * (bounded_noises_vy.rowwise() * vy_T).rowwise().sum()).eval();
+  }
+
+  auto costs_normalized = costs_ - costs_.minCoeff();
+  const float inv_temp = 1.0f / s.temperature;
+  auto softmaxes = (-inv_temp * costs_normalized).exp().eval();
+  softmaxes /= softmaxes.sum();
+
+  auto softmax_mat = softmaxes.matrix();
+  control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat;
+  control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat;
+
+  if (is_holo) {
+    control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat;
+  }
+
+  applyControlSequenceConstraints();
+}
+
+geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist(
+  const builtin_interfaces::msg::Time & stamp)
+{
+  unsigned int offset = settings_.shift_control_sequence ? 1 : 0;
+
+  auto vx = control_sequence_.vx(offset);
+  auto wz = control_sequence_.wz(offset);
+
+  if (isHolonomic()) {
+    auto vy = control_sequence_.vy(offset);
+    return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID());
+  }
+
+  return utils::toTwistStamped(vx, wz, stamp, costmap_ros_->getBaseFrameID());
+}
+
+void Optimizer::setMotionModel(const std::string & model)
+{
+  if (model == "DiffDrive") {
+    motion_model_ = std::make_shared<DiffDriveMotionModel>();
+  } else if (model == "Omni") {
+    motion_model_ = std::make_shared<OmniMotionModel>();
+  } else if (model == "Ackermann") {
+    motion_model_ = std::make_shared<AckermannMotionModel>(parameters_handler_, name_);
+  } else {
+    throw nav2_core::ControllerException(
+            std::string(
+              "Model " + model + " is not valid! Valid options are DiffDrive, Omni, "
+              "or Ackermann"));
+  }
+  motion_model_->initialize(settings_.constraints, settings_.model_dt);
+}
+
+void Optimizer::setSpeedLimit(double speed_limit, bool percentage)
+{
+  auto & s = settings_;
+  if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
+    s.constraints.vx_max = s.base_constraints.vx_max;
+    s.constraints.vx_min = s.base_constraints.vx_min;
+    s.constraints.vy = s.base_constraints.vy;
+    s.constraints.wz = s.base_constraints.wz;
+  } else {
+    if (percentage) {
+      // Speed limit is expressed in % from maximum speed of robot
+      double ratio = speed_limit / 100.0;
+      s.constraints.vx_max = s.base_constraints.vx_max * ratio;
+      s.constraints.vx_min = s.base_constraints.vx_min * ratio;
+      s.constraints.vy = s.base_constraints.vy * ratio;
+      s.constraints.wz = s.base_constraints.wz * ratio;
+    } else {
+      // Speed limit is expressed in absolute value
+      double ratio = speed_limit / s.base_constraints.vx_max;
+      s.constraints.vx_max = s.base_constraints.vx_max * ratio;
+      s.constraints.vx_min = s.base_constraints.vx_min * ratio;
+      s.constraints.vy = s.base_constraints.vy * ratio;
+      s.constraints.wz = s.base_constraints.wz * ratio;
+    }
+  }
+}
+
+models::Trajectories & Optimizer::getGeneratedTrajectories()
+{
+  return generated_trajectories_;
+}
+
+}  // namespace mppi
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index 013b29de502..b12c7dd6925 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -267,6 +267,7 @@ void Optimizer::prepare(
   critics_data_.motion_model = motion_model_;
   critics_data_.furthest_reached_path_point.reset();
   critics_data_.path_pts_valid.reset();
+  critics_data_.trajectories_in_collision.reset();
 }
 
 void Optimizer::shiftControlSequence()
diff --git a/nav2_mppi_controller/src/path_handler copy 2.cpp b/nav2_mppi_controller/src/path_handler copy 2.cpp
new file mode 100644
index 00000000000..b49e8991306
--- /dev/null
+++ b/nav2_mppi_controller/src/path_handler copy 2.cpp	
@@ -0,0 +1,247 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/tools/path_handler.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace mppi
+{
+
+void PathHandler::initialize(
+  nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
+  std::shared_ptr<tf2_ros::Buffer> buffer, ParametersHandler * param_handler)
+{
+  name_ = name;
+  costmap_ = costmap;
+  tf_buffer_ = buffer;
+  auto node = parent.lock();
+  logger_ = node->get_logger();
+  parameters_handler_ = param_handler;
+
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist());
+  getParam(prune_distance_, "prune_distance", 1.5);
+  getParam(transform_tolerance_, "transform_tolerance", 0.1);
+  getParam(enforce_path_inversion_, "enforce_path_inversion", false);
+  getParam(enforce_path_rotation_, "enforce_path_rotation", false);
+
+  if (enforce_path_inversion_ || enforce_path_rotation_) {
+    getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
+    getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4);
+    getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785);
+    inversion_locale_ = 0u;
+  }
+}
+
+std::pair<nav_msgs::msg::Path, PathIterator>
+PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
+  const geometry_msgs::msg::PoseStamped & global_pose)
+{
+  using nav2_util::geometry_utils::euclidean_distance;
+
+  auto begin = global_plan_up_to_inversion_.poses.begin();
+
+  // Limit the search for the closest pose up to max_robot_pose_search_dist on the path
+  auto closest_pose_upper_bound =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(),
+    max_robot_pose_search_dist_);
+
+  // Find closest point to the robot
+  auto closest_point = nav2_util::geometry_utils::min_by(
+    begin, closest_pose_upper_bound,
+    [&global_pose](const geometry_msgs::msg::PoseStamped & ps) {
+      return euclidean_distance(global_pose, ps);
+    });
+
+  nav_msgs::msg::Path transformed_plan;
+  transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
+  transformed_plan.header.stamp = global_pose.header.stamp;
+
+  auto pruned_plan_end =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_);
+
+  unsigned int mx, my;
+  // Find the furthest relevant pose on the path to consider within costmap
+  // bounds
+  // Transforming it to the costmap frame in the same loop
+  for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end;
+    ++global_plan_pose)
+  {
+    // Transform from global plan frame to costmap frame
+    geometry_msgs::msg::PoseStamped costmap_plan_pose;
+    global_plan_pose->header.stamp = global_pose.header.stamp;
+    global_plan_pose->header.frame_id = global_plan_.header.frame_id;
+    nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_,
+        costmap_->getGlobalFrameID(), transform_tolerance_);
+
+    // Check if pose is inside the costmap
+    if (!costmap_->getCostmap()->worldToMap(
+        costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
+    {
+      return {transformed_plan, closest_point};
+    }
+
+    // Filling the transformed plan to return with the transformed pose
+    transformed_plan.poses.push_back(costmap_plan_pose);
+  }
+
+  return {transformed_plan, closest_point};
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
+  const geometry_msgs::msg::PoseStamped & pose)
+{
+  if (global_plan_up_to_inversion_.poses.empty()) {
+    throw nav2_core::InvalidPath("Received plan with zero length");
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_,
+      global_plan_up_to_inversion_.header.frame_id, transform_tolerance_))
+  {
+    throw nav2_core::ControllerTFError(
+            "Unable to transform robot pose into global plan's frame");
+  }
+
+  return robot_pose;
+}
+
+nav_msgs::msg::Path PathHandler::transformPath(
+  const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Find relevant bounds of path to use
+  geometry_msgs::msg::PoseStamped global_pose =
+    transformToGlobalPlanFrame(robot_pose);
+  auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
+
+  prunePlan(global_plan_up_to_inversion_, lower_bound);
+
+  if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) {
+    if (isWithinInversionTolerances(global_pose)) {
+      // Robot has reached the inversion/rotation point, unlock the rest of the path
+      prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
+      global_plan_up_to_inversion_ = global_plan_;
+
+        double max_yaw_delta = 0.0;
+
+        for (size_t i = 1; i < global_plan_.poses.size(); i++) {
+          double yaw_prev = tf2::getYaw(global_plan_.poses[i-1].pose.orientation);
+          double yaw_curr = tf2::getYaw(global_plan_.poses[i].pose.orientation);
+          double diff = fabs(angles::shortest_angular_distance(yaw_prev, yaw_curr));
+          if (diff > max_yaw_delta) {
+            max_yaw_delta = diff;
+          }
+        }
+
+      // Recompute locale on the updated path
+      float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+      inversion_locale_ = utils::removePosesAfterFirstInversion(
+        global_plan_up_to_inversion_, rotation_threshold);
+
+      RCLCPP_INFO(
+    logger_,
+    "[MPPI PathHandler] setPath: rotation_threshold=%.3f, inversion_locale=%u, path_size=%zu",
+    rotation_threshold,
+    inversion_locale_,
+    global_plan_up_to_inversion_.poses.size());
+
+    }
+  }
+  else{
+  //   RCLCPP_INFO(
+  // logger_,
+  // "[MPPI PathHandler] step: enforce_path_inversion=%s, enforce_path_rotation=%s, inversion_locale=%u",
+  // enforce_path_inversion_ ? "true" : "false",
+  // enforce_path_rotation_ ? "true" : "false",
+  // inversion_locale_);
+
+  }
+
+  if (transformed_plan.poses.empty()) {
+    throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");
+  }
+
+  return transformed_plan;
+}
+
+double PathHandler::getMaxCostmapDist()
+{
+  const auto & costmap = costmap_->getCostmap();
+  return static_cast<double>(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) *
+         costmap->getResolution() * 0.50;
+}
+
+void PathHandler::setPath(const nav_msgs::msg::Path & plan)
+{
+  global_plan_ = plan;
+  global_plan_up_to_inversion_ = plan;
+  if (!enforce_path_inversion_ && !enforce_path_rotation_) {
+    inversion_locale_ = 0u;
+    return;
+  }
+
+  // Find and restrict to the first rotation or inversion constraint
+  float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+  inversion_locale_ = utils::removePosesAfterFirstInversion(
+    global_plan_up_to_inversion_, rotation_threshold);
+}
+
+nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
+
+void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
+{
+  plan.poses.erase(plan.poses.begin(), end);
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
+  const builtin_interfaces::msg::Time & stamp)
+{
+  auto goal = global_plan_.poses.back();
+  goal.header.frame_id = global_plan_.header.frame_id;
+  goal.header.stamp = stamp;
+  if (goal.header.frame_id.empty()) {
+    throw nav2_core::ControllerTFError("Goal pose has an empty frame_id");
+  }
+  geometry_msgs::msg::PoseStamped transformed_goal;
+  if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_,
+      costmap_->getGlobalFrameID(), transform_tolerance_))
+  {
+    throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame");
+  }
+  return transformed_goal;
+}
+
+bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Keep full path if we are within tolerance of the inversion pose
+  const auto last_pose = global_plan_up_to_inversion_.poses.back();
+  float distance = hypotf(
+    robot_pose.pose.position.x - last_pose.pose.position.x,
+    robot_pose.pose.position.y - last_pose.pose.position.y);
+
+  float angle_distance = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(last_pose.pose.orientation));
+
+  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_;
+}
+
+}  // namespace mppi
\ No newline at end of file
diff --git a/nav2_mppi_controller/src/path_handler copy.cpp b/nav2_mppi_controller/src/path_handler copy.cpp
new file mode 100644
index 00000000000..b49e8991306
--- /dev/null
+++ b/nav2_mppi_controller/src/path_handler copy.cpp	
@@ -0,0 +1,247 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/tools/path_handler.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace mppi
+{
+
+void PathHandler::initialize(
+  nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
+  std::shared_ptr<tf2_ros::Buffer> buffer, ParametersHandler * param_handler)
+{
+  name_ = name;
+  costmap_ = costmap;
+  tf_buffer_ = buffer;
+  auto node = parent.lock();
+  logger_ = node->get_logger();
+  parameters_handler_ = param_handler;
+
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist());
+  getParam(prune_distance_, "prune_distance", 1.5);
+  getParam(transform_tolerance_, "transform_tolerance", 0.1);
+  getParam(enforce_path_inversion_, "enforce_path_inversion", false);
+  getParam(enforce_path_rotation_, "enforce_path_rotation", false);
+
+  if (enforce_path_inversion_ || enforce_path_rotation_) {
+    getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
+    getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4);
+    getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785);
+    inversion_locale_ = 0u;
+  }
+}
+
+std::pair<nav_msgs::msg::Path, PathIterator>
+PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
+  const geometry_msgs::msg::PoseStamped & global_pose)
+{
+  using nav2_util::geometry_utils::euclidean_distance;
+
+  auto begin = global_plan_up_to_inversion_.poses.begin();
+
+  // Limit the search for the closest pose up to max_robot_pose_search_dist on the path
+  auto closest_pose_upper_bound =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(),
+    max_robot_pose_search_dist_);
+
+  // Find closest point to the robot
+  auto closest_point = nav2_util::geometry_utils::min_by(
+    begin, closest_pose_upper_bound,
+    [&global_pose](const geometry_msgs::msg::PoseStamped & ps) {
+      return euclidean_distance(global_pose, ps);
+    });
+
+  nav_msgs::msg::Path transformed_plan;
+  transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
+  transformed_plan.header.stamp = global_pose.header.stamp;
+
+  auto pruned_plan_end =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_);
+
+  unsigned int mx, my;
+  // Find the furthest relevant pose on the path to consider within costmap
+  // bounds
+  // Transforming it to the costmap frame in the same loop
+  for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end;
+    ++global_plan_pose)
+  {
+    // Transform from global plan frame to costmap frame
+    geometry_msgs::msg::PoseStamped costmap_plan_pose;
+    global_plan_pose->header.stamp = global_pose.header.stamp;
+    global_plan_pose->header.frame_id = global_plan_.header.frame_id;
+    nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_,
+        costmap_->getGlobalFrameID(), transform_tolerance_);
+
+    // Check if pose is inside the costmap
+    if (!costmap_->getCostmap()->worldToMap(
+        costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
+    {
+      return {transformed_plan, closest_point};
+    }
+
+    // Filling the transformed plan to return with the transformed pose
+    transformed_plan.poses.push_back(costmap_plan_pose);
+  }
+
+  return {transformed_plan, closest_point};
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
+  const geometry_msgs::msg::PoseStamped & pose)
+{
+  if (global_plan_up_to_inversion_.poses.empty()) {
+    throw nav2_core::InvalidPath("Received plan with zero length");
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_,
+      global_plan_up_to_inversion_.header.frame_id, transform_tolerance_))
+  {
+    throw nav2_core::ControllerTFError(
+            "Unable to transform robot pose into global plan's frame");
+  }
+
+  return robot_pose;
+}
+
+nav_msgs::msg::Path PathHandler::transformPath(
+  const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Find relevant bounds of path to use
+  geometry_msgs::msg::PoseStamped global_pose =
+    transformToGlobalPlanFrame(robot_pose);
+  auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
+
+  prunePlan(global_plan_up_to_inversion_, lower_bound);
+
+  if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) {
+    if (isWithinInversionTolerances(global_pose)) {
+      // Robot has reached the inversion/rotation point, unlock the rest of the path
+      prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
+      global_plan_up_to_inversion_ = global_plan_;
+
+        double max_yaw_delta = 0.0;
+
+        for (size_t i = 1; i < global_plan_.poses.size(); i++) {
+          double yaw_prev = tf2::getYaw(global_plan_.poses[i-1].pose.orientation);
+          double yaw_curr = tf2::getYaw(global_plan_.poses[i].pose.orientation);
+          double diff = fabs(angles::shortest_angular_distance(yaw_prev, yaw_curr));
+          if (diff > max_yaw_delta) {
+            max_yaw_delta = diff;
+          }
+        }
+
+      // Recompute locale on the updated path
+      float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+      inversion_locale_ = utils::removePosesAfterFirstInversion(
+        global_plan_up_to_inversion_, rotation_threshold);
+
+      RCLCPP_INFO(
+    logger_,
+    "[MPPI PathHandler] setPath: rotation_threshold=%.3f, inversion_locale=%u, path_size=%zu",
+    rotation_threshold,
+    inversion_locale_,
+    global_plan_up_to_inversion_.poses.size());
+
+    }
+  }
+  else{
+  //   RCLCPP_INFO(
+  // logger_,
+  // "[MPPI PathHandler] step: enforce_path_inversion=%s, enforce_path_rotation=%s, inversion_locale=%u",
+  // enforce_path_inversion_ ? "true" : "false",
+  // enforce_path_rotation_ ? "true" : "false",
+  // inversion_locale_);
+
+  }
+
+  if (transformed_plan.poses.empty()) {
+    throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");
+  }
+
+  return transformed_plan;
+}
+
+double PathHandler::getMaxCostmapDist()
+{
+  const auto & costmap = costmap_->getCostmap();
+  return static_cast<double>(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) *
+         costmap->getResolution() * 0.50;
+}
+
+void PathHandler::setPath(const nav_msgs::msg::Path & plan)
+{
+  global_plan_ = plan;
+  global_plan_up_to_inversion_ = plan;
+  if (!enforce_path_inversion_ && !enforce_path_rotation_) {
+    inversion_locale_ = 0u;
+    return;
+  }
+
+  // Find and restrict to the first rotation or inversion constraint
+  float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+  inversion_locale_ = utils::removePosesAfterFirstInversion(
+    global_plan_up_to_inversion_, rotation_threshold);
+}
+
+nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
+
+void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
+{
+  plan.poses.erase(plan.poses.begin(), end);
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
+  const builtin_interfaces::msg::Time & stamp)
+{
+  auto goal = global_plan_.poses.back();
+  goal.header.frame_id = global_plan_.header.frame_id;
+  goal.header.stamp = stamp;
+  if (goal.header.frame_id.empty()) {
+    throw nav2_core::ControllerTFError("Goal pose has an empty frame_id");
+  }
+  geometry_msgs::msg::PoseStamped transformed_goal;
+  if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_,
+      costmap_->getGlobalFrameID(), transform_tolerance_))
+  {
+    throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame");
+  }
+  return transformed_goal;
+}
+
+bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Keep full path if we are within tolerance of the inversion pose
+  const auto last_pose = global_plan_up_to_inversion_.poses.back();
+  float distance = hypotf(
+    robot_pose.pose.position.x - last_pose.pose.position.x,
+    robot_pose.pose.position.y - last_pose.pose.position.y);
+
+  float angle_distance = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(last_pose.pose.orientation));
+
+  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_;
+}
+
+}  // namespace mppi
\ No newline at end of file
diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp
index 55931f6bc62..b49e8991306 100644
--- a/nav2_mppi_controller/src/path_handler.cpp
+++ b/nav2_mppi_controller/src/path_handler.cpp
@@ -39,9 +39,12 @@ void PathHandler::initialize(
   getParam(prune_distance_, "prune_distance", 1.5);
   getParam(transform_tolerance_, "transform_tolerance", 0.1);
   getParam(enforce_path_inversion_, "enforce_path_inversion", false);
-  if (enforce_path_inversion_) {
+  getParam(enforce_path_rotation_, "enforce_path_rotation", false);
+
+  if (enforce_path_inversion_ || enforce_path_rotation_) {
     getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
-    getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4);
+    getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4);
+    getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785);
     inversion_locale_ = 0u;
   }
 }
@@ -131,13 +134,46 @@ nav_msgs::msg::Path PathHandler::transformPath(
 
   prunePlan(global_plan_up_to_inversion_, lower_bound);
 
-  if (enforce_path_inversion_ && inversion_locale_ != 0u) {
+  if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) {
     if (isWithinInversionTolerances(global_pose)) {
+      // Robot has reached the inversion/rotation point, unlock the rest of the path
       prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
       global_plan_up_to_inversion_ = global_plan_;
-      inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
+
+        double max_yaw_delta = 0.0;
+
+        for (size_t i = 1; i < global_plan_.poses.size(); i++) {
+          double yaw_prev = tf2::getYaw(global_plan_.poses[i-1].pose.orientation);
+          double yaw_curr = tf2::getYaw(global_plan_.poses[i].pose.orientation);
+          double diff = fabs(angles::shortest_angular_distance(yaw_prev, yaw_curr));
+          if (diff > max_yaw_delta) {
+            max_yaw_delta = diff;
+          }
+        }
+
+      // Recompute locale on the updated path
+      float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+      inversion_locale_ = utils::removePosesAfterFirstInversion(
+        global_plan_up_to_inversion_, rotation_threshold);
+
+      RCLCPP_INFO(
+    logger_,
+    "[MPPI PathHandler] setPath: rotation_threshold=%.3f, inversion_locale=%u, path_size=%zu",
+    rotation_threshold,
+    inversion_locale_,
+    global_plan_up_to_inversion_.poses.size());
+
     }
   }
+  else{
+  //   RCLCPP_INFO(
+  // logger_,
+  // "[MPPI PathHandler] step: enforce_path_inversion=%s, enforce_path_rotation=%s, inversion_locale=%u",
+  // enforce_path_inversion_ ? "true" : "false",
+  // enforce_path_rotation_ ? "true" : "false",
+  // inversion_locale_);
+
+  }
 
   if (transformed_plan.poses.empty()) {
     throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");
@@ -156,10 +192,16 @@ double PathHandler::getMaxCostmapDist()
 void PathHandler::setPath(const nav_msgs::msg::Path & plan)
 {
   global_plan_ = plan;
-  global_plan_up_to_inversion_ = global_plan_;
-  if (enforce_path_inversion_) {
-    inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
+  global_plan_up_to_inversion_ = plan;
+  if (!enforce_path_inversion_ && !enforce_path_rotation_) {
+    inversion_locale_ = 0u;
+    return;
   }
+
+  // Find and restrict to the first rotation or inversion constraint
+  float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+  inversion_locale_ = utils::removePosesAfterFirstInversion(
+    global_plan_up_to_inversion_, rotation_threshold);
 }
 
 nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
@@ -199,7 +241,7 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam
     tf2::getYaw(robot_pose.pose.orientation),
     tf2::getYaw(last_pose.pose.orientation));
 
-  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance;
+  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_;
 }
 
-}  // namespace mppi
+}  // namespace mppi
\ No newline at end of file
diff --git a/nav2_mppi_controller/src/path_handler_mod.cpp b/nav2_mppi_controller/src/path_handler_mod.cpp
new file mode 100644
index 00000000000..b49e8991306
--- /dev/null
+++ b/nav2_mppi_controller/src/path_handler_mod.cpp
@@ -0,0 +1,247 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/tools/path_handler.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace mppi
+{
+
+void PathHandler::initialize(
+  nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
+  std::shared_ptr<tf2_ros::Buffer> buffer, ParametersHandler * param_handler)
+{
+  name_ = name;
+  costmap_ = costmap;
+  tf_buffer_ = buffer;
+  auto node = parent.lock();
+  logger_ = node->get_logger();
+  parameters_handler_ = param_handler;
+
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist());
+  getParam(prune_distance_, "prune_distance", 1.5);
+  getParam(transform_tolerance_, "transform_tolerance", 0.1);
+  getParam(enforce_path_inversion_, "enforce_path_inversion", false);
+  getParam(enforce_path_rotation_, "enforce_path_rotation", false);
+
+  if (enforce_path_inversion_ || enforce_path_rotation_) {
+    getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
+    getParam(inversion_yaw_tolerance_, "inversion_yaw_tolerance", 0.4);
+    getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785);
+    inversion_locale_ = 0u;
+  }
+}
+
+std::pair<nav_msgs::msg::Path, PathIterator>
+PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
+  const geometry_msgs::msg::PoseStamped & global_pose)
+{
+  using nav2_util::geometry_utils::euclidean_distance;
+
+  auto begin = global_plan_up_to_inversion_.poses.begin();
+
+  // Limit the search for the closest pose up to max_robot_pose_search_dist on the path
+  auto closest_pose_upper_bound =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(),
+    max_robot_pose_search_dist_);
+
+  // Find closest point to the robot
+  auto closest_point = nav2_util::geometry_utils::min_by(
+    begin, closest_pose_upper_bound,
+    [&global_pose](const geometry_msgs::msg::PoseStamped & ps) {
+      return euclidean_distance(global_pose, ps);
+    });
+
+  nav_msgs::msg::Path transformed_plan;
+  transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
+  transformed_plan.header.stamp = global_pose.header.stamp;
+
+  auto pruned_plan_end =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_);
+
+  unsigned int mx, my;
+  // Find the furthest relevant pose on the path to consider within costmap
+  // bounds
+  // Transforming it to the costmap frame in the same loop
+  for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end;
+    ++global_plan_pose)
+  {
+    // Transform from global plan frame to costmap frame
+    geometry_msgs::msg::PoseStamped costmap_plan_pose;
+    global_plan_pose->header.stamp = global_pose.header.stamp;
+    global_plan_pose->header.frame_id = global_plan_.header.frame_id;
+    nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_,
+        costmap_->getGlobalFrameID(), transform_tolerance_);
+
+    // Check if pose is inside the costmap
+    if (!costmap_->getCostmap()->worldToMap(
+        costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
+    {
+      return {transformed_plan, closest_point};
+    }
+
+    // Filling the transformed plan to return with the transformed pose
+    transformed_plan.poses.push_back(costmap_plan_pose);
+  }
+
+  return {transformed_plan, closest_point};
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
+  const geometry_msgs::msg::PoseStamped & pose)
+{
+  if (global_plan_up_to_inversion_.poses.empty()) {
+    throw nav2_core::InvalidPath("Received plan with zero length");
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_,
+      global_plan_up_to_inversion_.header.frame_id, transform_tolerance_))
+  {
+    throw nav2_core::ControllerTFError(
+            "Unable to transform robot pose into global plan's frame");
+  }
+
+  return robot_pose;
+}
+
+nav_msgs::msg::Path PathHandler::transformPath(
+  const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Find relevant bounds of path to use
+  geometry_msgs::msg::PoseStamped global_pose =
+    transformToGlobalPlanFrame(robot_pose);
+  auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
+
+  prunePlan(global_plan_up_to_inversion_, lower_bound);
+
+  if ((enforce_path_inversion_ || enforce_path_rotation_) && inversion_locale_ != 0u) {
+    if (isWithinInversionTolerances(global_pose)) {
+      // Robot has reached the inversion/rotation point, unlock the rest of the path
+      prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
+      global_plan_up_to_inversion_ = global_plan_;
+
+        double max_yaw_delta = 0.0;
+
+        for (size_t i = 1; i < global_plan_.poses.size(); i++) {
+          double yaw_prev = tf2::getYaw(global_plan_.poses[i-1].pose.orientation);
+          double yaw_curr = tf2::getYaw(global_plan_.poses[i].pose.orientation);
+          double diff = fabs(angles::shortest_angular_distance(yaw_prev, yaw_curr));
+          if (diff > max_yaw_delta) {
+            max_yaw_delta = diff;
+          }
+        }
+
+      // Recompute locale on the updated path
+      float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+      inversion_locale_ = utils::removePosesAfterFirstInversion(
+        global_plan_up_to_inversion_, rotation_threshold);
+
+      RCLCPP_INFO(
+    logger_,
+    "[MPPI PathHandler] setPath: rotation_threshold=%.3f, inversion_locale=%u, path_size=%zu",
+    rotation_threshold,
+    inversion_locale_,
+    global_plan_up_to_inversion_.poses.size());
+
+    }
+  }
+  else{
+  //   RCLCPP_INFO(
+  // logger_,
+  // "[MPPI PathHandler] step: enforce_path_inversion=%s, enforce_path_rotation=%s, inversion_locale=%u",
+  // enforce_path_inversion_ ? "true" : "false",
+  // enforce_path_rotation_ ? "true" : "false",
+  // inversion_locale_);
+
+  }
+
+  if (transformed_plan.poses.empty()) {
+    throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");
+  }
+
+  return transformed_plan;
+}
+
+double PathHandler::getMaxCostmapDist()
+{
+  const auto & costmap = costmap_->getCostmap();
+  return static_cast<double>(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) *
+         costmap->getResolution() * 0.50;
+}
+
+void PathHandler::setPath(const nav_msgs::msg::Path & plan)
+{
+  global_plan_ = plan;
+  global_plan_up_to_inversion_ = plan;
+  if (!enforce_path_inversion_ && !enforce_path_rotation_) {
+    inversion_locale_ = 0u;
+    return;
+  }
+
+  // Find and restrict to the first rotation or inversion constraint
+  float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+  inversion_locale_ = utils::removePosesAfterFirstInversion(
+    global_plan_up_to_inversion_, rotation_threshold);
+}
+
+nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
+
+void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
+{
+  plan.poses.erase(plan.poses.begin(), end);
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
+  const builtin_interfaces::msg::Time & stamp)
+{
+  auto goal = global_plan_.poses.back();
+  goal.header.frame_id = global_plan_.header.frame_id;
+  goal.header.stamp = stamp;
+  if (goal.header.frame_id.empty()) {
+    throw nav2_core::ControllerTFError("Goal pose has an empty frame_id");
+  }
+  geometry_msgs::msg::PoseStamped transformed_goal;
+  if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_,
+      costmap_->getGlobalFrameID(), transform_tolerance_))
+  {
+    throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame");
+  }
+  return transformed_goal;
+}
+
+bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Keep full path if we are within tolerance of the inversion pose
+  const auto last_pose = global_plan_up_to_inversion_.poses.back();
+  float distance = hypotf(
+    robot_pose.pose.position.x - last_pose.pose.position.x,
+    robot_pose.pose.position.y - last_pose.pose.position.y);
+
+  float angle_distance = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(last_pose.pose.orientation));
+
+  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_;
+}
+
+}  // namespace mppi
\ No newline at end of file
diff --git a/nav2_mppi_controller/src/path_handler_original.cpp b/nav2_mppi_controller/src/path_handler_original.cpp
new file mode 100644
index 00000000000..55931f6bc62
--- /dev/null
+++ b/nav2_mppi_controller/src/path_handler_original.cpp
@@ -0,0 +1,205 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/tools/path_handler.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace mppi
+{
+
+void PathHandler::initialize(
+  nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
+  std::shared_ptr<tf2_ros::Buffer> buffer, ParametersHandler * param_handler)
+{
+  name_ = name;
+  costmap_ = costmap;
+  tf_buffer_ = buffer;
+  auto node = parent.lock();
+  logger_ = node->get_logger();
+  parameters_handler_ = param_handler;
+
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist());
+  getParam(prune_distance_, "prune_distance", 1.5);
+  getParam(transform_tolerance_, "transform_tolerance", 0.1);
+  getParam(enforce_path_inversion_, "enforce_path_inversion", false);
+  if (enforce_path_inversion_) {
+    getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
+    getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4);
+    inversion_locale_ = 0u;
+  }
+}
+
+std::pair<nav_msgs::msg::Path, PathIterator>
+PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
+  const geometry_msgs::msg::PoseStamped & global_pose)
+{
+  using nav2_util::geometry_utils::euclidean_distance;
+
+  auto begin = global_plan_up_to_inversion_.poses.begin();
+
+  // Limit the search for the closest pose up to max_robot_pose_search_dist on the path
+  auto closest_pose_upper_bound =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(),
+    max_robot_pose_search_dist_);
+
+  // Find closest point to the robot
+  auto closest_point = nav2_util::geometry_utils::min_by(
+    begin, closest_pose_upper_bound,
+    [&global_pose](const geometry_msgs::msg::PoseStamped & ps) {
+      return euclidean_distance(global_pose, ps);
+    });
+
+  nav_msgs::msg::Path transformed_plan;
+  transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
+  transformed_plan.header.stamp = global_pose.header.stamp;
+
+  auto pruned_plan_end =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_);
+
+  unsigned int mx, my;
+  // Find the furthest relevant pose on the path to consider within costmap
+  // bounds
+  // Transforming it to the costmap frame in the same loop
+  for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end;
+    ++global_plan_pose)
+  {
+    // Transform from global plan frame to costmap frame
+    geometry_msgs::msg::PoseStamped costmap_plan_pose;
+    global_plan_pose->header.stamp = global_pose.header.stamp;
+    global_plan_pose->header.frame_id = global_plan_.header.frame_id;
+    nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_,
+        costmap_->getGlobalFrameID(), transform_tolerance_);
+
+    // Check if pose is inside the costmap
+    if (!costmap_->getCostmap()->worldToMap(
+        costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
+    {
+      return {transformed_plan, closest_point};
+    }
+
+    // Filling the transformed plan to return with the transformed pose
+    transformed_plan.poses.push_back(costmap_plan_pose);
+  }
+
+  return {transformed_plan, closest_point};
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
+  const geometry_msgs::msg::PoseStamped & pose)
+{
+  if (global_plan_up_to_inversion_.poses.empty()) {
+    throw nav2_core::InvalidPath("Received plan with zero length");
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_,
+      global_plan_up_to_inversion_.header.frame_id, transform_tolerance_))
+  {
+    throw nav2_core::ControllerTFError(
+            "Unable to transform robot pose into global plan's frame");
+  }
+
+  return robot_pose;
+}
+
+nav_msgs::msg::Path PathHandler::transformPath(
+  const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Find relevant bounds of path to use
+  geometry_msgs::msg::PoseStamped global_pose =
+    transformToGlobalPlanFrame(robot_pose);
+  auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
+
+  prunePlan(global_plan_up_to_inversion_, lower_bound);
+
+  if (enforce_path_inversion_ && inversion_locale_ != 0u) {
+    if (isWithinInversionTolerances(global_pose)) {
+      prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
+      global_plan_up_to_inversion_ = global_plan_;
+      inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
+    }
+  }
+
+  if (transformed_plan.poses.empty()) {
+    throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");
+  }
+
+  return transformed_plan;
+}
+
+double PathHandler::getMaxCostmapDist()
+{
+  const auto & costmap = costmap_->getCostmap();
+  return static_cast<double>(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) *
+         costmap->getResolution() * 0.50;
+}
+
+void PathHandler::setPath(const nav_msgs::msg::Path & plan)
+{
+  global_plan_ = plan;
+  global_plan_up_to_inversion_ = global_plan_;
+  if (enforce_path_inversion_) {
+    inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
+  }
+}
+
+nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
+
+void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
+{
+  plan.poses.erase(plan.poses.begin(), end);
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
+  const builtin_interfaces::msg::Time & stamp)
+{
+  auto goal = global_plan_.poses.back();
+  goal.header.frame_id = global_plan_.header.frame_id;
+  goal.header.stamp = stamp;
+  if (goal.header.frame_id.empty()) {
+    throw nav2_core::ControllerTFError("Goal pose has an empty frame_id");
+  }
+  geometry_msgs::msg::PoseStamped transformed_goal;
+  if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_,
+      costmap_->getGlobalFrameID(), transform_tolerance_))
+  {
+    throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame");
+  }
+  return transformed_goal;
+}
+
+bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Keep full path if we are within tolerance of the inversion pose
+  const auto last_pose = global_plan_up_to_inversion_.poses.back();
+  float distance = hypotf(
+    robot_pose.pose.position.x - last_pose.pose.position.x,
+    robot_pose.pose.position.y - last_pose.pose.position.y);
+
+  float angle_distance = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(last_pose.pose.orientation));
+
+  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance;
+}
+
+}  // namespace mppi
diff --git a/nav2_mppi_controller/src/trajectory_validators/fixing_trajectory_validator.cpp b/nav2_mppi_controller/src/trajectory_validators/fixing_trajectory_validator.cpp
new file mode 100644
index 00000000000..7c17b4e5a3e
--- /dev/null
+++ b/nav2_mppi_controller/src/trajectory_validators/fixing_trajectory_validator.cpp
@@ -0,0 +1,407 @@
+// src/validators/auto_fixing_trajectory_validator.cpp
+#include "nav2_mppi_controller/fixing_trajectory_validator.hpp"
+#include <algorithm>
+#include <cmath>
+
+namespace mppi {
+
+void AutoFixingTrajectoryValidator::initialize(
+  const nav2::LifecycleNode::WeakPtr & parent,
+  const std::string & name,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
+  ParametersHandler * param_handler,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  const models::OptimizerSettings & settings)
+{
+  OptimalTrajectoryValidator::initialize(parent, name, costmap, param_handler, tf_buffer, settings);
+  auto node = node_.lock();
+  auto get = param_handler_->getParamGetter(name_);
+
+  model_dt_ = settings.model_dt;
+  get(auto_fix_ttc_window_, "auto_fix_ttc_window", 0.8f);
+
+  costmap_ = costmap_ros_->getCostmap();
+
+  get(consider_footprint_, "consider_footprint", true);
+  get(safe_clearance_, "auto_fix_safe_clearance", 0.12);
+  get(step_gain_, "auto_fix_step_gain", 0.6);
+  get(max_step_, "auto_fix_max_step", 0.05);
+  get(max_total_disp_, "auto_fix_max_total_displacement", 0.25);
+  get(max_iters_, "auto_fix_max_iters", 6);
+  get(smooth_iters_, "auto_fix_smooth_iters", 2);
+
+  // Clamping parametrów względem rozdzielczości costmapy – odporność na złe YAML
+  const double r = costmap_->getResolution();
+  auto clamp = [&](double &v, double lo, double hi) { v = std::clamp(v, lo, hi); };
+  clamp(safe_clearance_, 0.5 * r, 1.5 * r);
+  clamp(max_step_,       0.25 * r, 1.5 * r);
+  clamp(max_total_disp_, 1.5 * r, 10.0 * r);
+  step_gain_ = std::clamp(step_gain_, 0.2, 0.9);
+
+  if (consider_footprint_) {
+    fp_checker_ = std::make_unique<nav2_costmap_2d::FootprintCollisionChecker<
+      nav2_costmap_2d::Costmap2D *>>(costmap_);
+  }
+
+  RCLCPP_INFO(node->get_logger(),
+    "[%s] AutoFixingTrajectoryValidator initialized: safe=%.2f step_gain=%.2f "
+    "max_step=%.2f max_disp=%.2f iters=%d smooth=%d lookahead=%.2fs (samples=%u)",
+    name_.c_str(), safe_clearance_, step_gain_, max_step_, max_total_disp_,
+    max_iters_, smooth_iters_, collision_lookahead_time_, traj_samples_to_evaluate_);
+}
+
+bool AutoFixingTrajectoryValidator::isCollisionAt(double x, double y, double theta) const
+{
+  if (consider_footprint_) {
+    const double c = fp_checker_->footprintCostAtPose(
+      x, y, theta, costmap_ros_->getRobotFootprint());
+    // W tym Nav2 checkerze kolizja/OOB ➜ LETHAL (254). Traktuj też INSCRIBED (253) jako nieakceptowalne.
+    return (c >= static_cast<double>(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE));
+  }
+
+  unsigned int mx, my;
+  if (!costmap_->worldToMap(x, y, mx, my)) {
+    return true;  // poza mapą = niebezpieczne
+  }
+  const unsigned char c = costmap_->getCost(mx, my);
+  return (c >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
+}
+
+double AutoFixingTrajectoryValidator::clearanceAt(double x, double y) const
+{
+  // Prosty promieniowy skan (jeśli masz EDT/SDF – podmień)
+  static const int K = 12;
+  static const double MAX_R = 0.6;
+  double best = MAX_R;
+  for (int k = 0; k < K; ++k) {
+    const double ang = 2.0 * M_PI * k / K;
+    const double cs = std::cos(ang), sn = std::sin(ang);
+    double r = 0.0;
+    while (r < MAX_R) {
+      const double px = x + r * cs;
+      const double py = y + r * sn;
+      unsigned int mx, my;
+      if (!costmap_->worldToMap(px, py, mx, my)) break;
+      const unsigned char c = costmap_->getCost(mx, my);
+      if (c == nav2_costmap_2d::LETHAL_OBSTACLE ||
+          c == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
+        best = std::min(best, r);
+        break;
+      }
+      r += costmap_->getResolution();
+    }
+  }
+  return best;
+}
+
+bool AutoFixingTrajectoryValidator::isSafe(double x, double y, double theta) const
+{
+  if (isCollisionAt(x, y, theta)) return false;
+  return clearanceAt(x, y) >= safe_clearance_;
+}
+
+std::optional<Eigen::Vector2d> AutoFixingTrajectoryValidator::gradientAt(double x, double y) const
+{
+  unsigned int mx, my;
+  if (!costmap_->worldToMap(x, y, mx, my)) return std::nullopt;
+
+  auto sample = [&](int dx, int dy) -> double {
+    const int ix = static_cast<int>(mx) + dx;
+    const int iy = static_cast<int>(my) + dy;
+    if (ix < 0 || iy < 0 ||
+        ix >= static_cast<int>(costmap_->getSizeInCellsX()) ||
+        iy >= static_cast<int>(costmap_->getSizeInCellsY())) {
+      return 0.0;
+    }
+    return static_cast<double>(costmap_->getCost(ix, iy));
+  };
+
+  const double cx1 = sample(+1, 0), cx0 = sample(-1, 0);
+  const double cy1 = sample(0, +1), cy0 = sample(0, -1);
+  const double dx_cost = (cx1 - cx0) / (2.0 * costmap_->getResolution());
+  const double dy_cost = (cy1 - cy0) / (2.0 * costmap_->getResolution());
+
+  Eigen::Vector2d g(dx_cost, dy_cost);
+  if (g.norm() < 1e-6) return std::nullopt;
+  return g; // kierunek wzrostu kosztu ⇒ „odpychamy” w +grad
+}
+
+void AutoFixingTrajectoryValidator::laplacianSmoothXY(
+  Eigen::ArrayXXf & traj, const std::vector<bool> &locked, int iters) const
+{
+  const int N = traj.rows();
+  if (N < 3 || iters <= 0) return;
+  for (int it = 0; it < iters; ++it) {
+    for (int i = 1; i < N - 1; ++i) {
+      if (locked[i]) continue;
+      const Eigen::Vector2f prev = traj.row(i - 1).segment<2>(0);
+      const Eigen::Vector2f curr = traj.row(i).segment<2>(0);
+      const Eigen::Vector2f next = traj.row(i + 1).segment<2>(0);
+      const Eigen::Vector2f lap = (prev - 2.f * curr + next);
+      traj(i, 0) += 0.25f * lap.x();
+      traj(i, 1) += 0.25f * lap.y();
+    }
+  }
+}
+
+void AutoFixingTrajectoryValidator::smoothYaw(Eigen::ArrayXXf & traj) const
+{
+  const int N = traj.rows();
+  if (N < 3) return;
+  for (int i = 1; i < N - 1; ++i) {
+    const float t_prev = traj(i - 1, 2), t_cur = traj(i, 2), t_next = traj(i + 1, 2);
+    traj(i, 2) = 0.25f * t_prev + 0.5f * t_cur + 0.25f * t_next;
+  }
+}
+
+bool AutoFixingTrajectoryValidator::tryRepair(Eigen::ArrayXXf & traj, int start_idx, int end_idx) const
+{
+  const int Ncap = static_cast<int>(std::min<unsigned int>(traj.rows(), traj_samples_to_evaluate_));
+  const int s = std::clamp(start_idx, 0, Ncap);
+  const int e = std::clamp(end_idx,   0, Ncap);
+  if (e - s <= 0) return true;
+
+  std::vector<double> accum(e - s, 0.0);
+  std::vector<bool>   locked(e - s, false);
+
+  auto safe_idx = [&](int i) { return isSafe(traj(i, 0), traj(i, 1), traj(i, 2)); };
+
+  for (int i = s; i < e; ++i) {
+    locked[i - s] = safe_idx(i);
+  }
+
+  for (int it = 0; it < max_iters_; ++it) {
+    bool improved = false;
+
+    for (int i = s; i < e; ++i) {
+      if (locked[i - s]) continue;
+
+      const double x = traj(i, 0), y = traj(i, 1), th = traj(i, 2);
+      double clr = clearanceAt(x, y);
+
+      // kierunek: gradient kosztu lub fallback
+      Eigen::Vector2d dir(0.0, 0.0);
+      if (auto g = gradientAt(x, y)) {
+        dir = -(*g);
+      } else {
+        const int K = 16;
+        double best_gain = -1e9;
+        for (int k = 0; k < K; ++k) {
+          const double ang = 2.0 * M_PI * k / K;
+          const Eigen::Vector2d cand(std::cos(ang), std::sin(ang));
+          const double nx = x + 0.02 * cand.x();
+          const double ny = y + 0.02 * cand.y();
+          const double gain = clearanceAt(nx, ny) - clr;
+          if (gain > best_gain) { best_gain = gain; dir = cand; }
+        }
+      }
+
+      const double nrm = dir.norm();
+      if (nrm < 1e-6) continue;
+      dir /= nrm;
+
+        const bool in_collision = isCollisionAt(x, y, th);
+        double step = 0.0;
+        if (in_collision) {
+        // Silniejszy krok startowy, żeby wyjść poza INSCRIBED
+        const double min_step = std::max({ 0.5 * costmap_->getResolution(),
+                                            0.25 * safe_clearance_,
+                                            safe_clearance_ + 0.5 * costmap_->getResolution() });
+        step = std::min(std::max(min_step, step_gain_ * (safe_clearance_ + costmap_->getResolution())),
+                        max_step_);
+        } else {
+        clr = clearanceAt(x, y);
+        const double deficit = std::max(0.0, safe_clearance_ - clr);
+        step = std::min(max_step_, step_gain_ * deficit);
+        }
+      double room = std::max(0.0, max_total_disp_ - accum[i - s]);
+        if (room <= 1e-6) continue;
+        if (step > room) step = room;
+
+
+      // limit przemieszczenia per-punkt (względem zakresu [s,e))
+      if (accum[i - s] + step > max_total_disp_) {
+        step = std::max(0.0, max_total_disp_ - accum[i - s]);
+      }
+      if (step <= 1e-6) continue;
+
+      double nx = x + step * dir.x();
+      double ny = y + step * dir.y();
+
+      // krótki line-search: nie wchodź w kolizję
+      int ls = 0;
+      while (ls < 4 && isCollisionAt(nx, ny, th)) {
+        step *= 0.5; nx = x + step * dir.x(); ny = y + step * dir.y(); ++ls;
+      }
+
+      // jeśli footprint nadal „zbyt blisko”, wysuń delikatnie w górę
+      if (consider_footprint_) {
+        int ls_up = 0;
+        while (ls_up < 4) {
+          const double c = fp_checker_->footprintCostAtPose(nx, ny, th, costmap_ros_->getRobotFootprint());
+          if (c < static_cast<double>(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) break;
+          double extra = 0.5 * step;
+
+          // nie przekraczaj max_total_disp_
+          room = std::max(0.0, max_total_disp_ - accum[i - s] - step);
+          if (room <= 1e-6) break;
+          if (extra > room) extra = room;
+
+          nx += extra * dir.x();
+          ny += extra * dir.y();
+          step += extra;
+          ++ls_up;
+        }
+      }
+
+      if (step <= 1e-6) continue; // nic nie zrobiliśmy
+
+      traj(i, 0) = static_cast<float>(nx);
+      traj(i, 1) = static_cast<float>(ny);
+      accum[i - s] += step;
+      improved = true;
+    }
+
+    // smoothing z pełnym wektorem locków w przestrzeni [0..rows)
+    std::vector<bool> locked_full(traj.rows(), false);
+    for (int i = s; i < e; ++i) locked_full[i] = locked[i - s];
+    laplacianSmoothXY(traj, locked_full, smooth_iters_);
+    smoothYaw(traj);
+
+    // zaktualizuj maskę bezpiecznych w zakresie
+    bool all_safe = true;
+    for (int i = s; i < e; ++i) {
+      if (!safe_idx(i)) { all_safe = false; break; }
+      locked[i - s] = true;
+    }
+    if (all_safe) return true;
+    if (!improved) break;
+  }
+
+  // częściowo lub wcale się nie udało
+  return false;
+}
+
+ValidationResult AutoFixingTrajectoryValidator::validateTrajectory(
+  const Eigen::ArrayXXf & optimal_trajectory,
+  const models::ControlSequence & control_sequence,
+  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Twist & robot_speed,
+  const nav_msgs::msg::Path & plan,
+  const geometry_msgs::msg::Pose & goal)
+{
+  (void)control_sequence;
+  (void)robot_pose;
+  (void)robot_speed;
+  (void)plan;
+  (void)goal;
+
+  auto node = node_.lock();
+  RCLCPP_DEBUG_THROTTLE(node->get_logger(), *node->get_clock(), 1000,
+    "[%s] validateTrajectory called. samples=%u", name_.c_str(), traj_samples_to_evaluate_);
+
+  has_fixed_ = false;
+  fixed_traj_ = optimal_trajectory; // kopia robocza
+
+  // 1) szybka weryfikacja
+  int first_coll_idx = -1;
+  const size_t limit = std::min<size_t>(traj_samples_to_evaluate_, fixed_traj_.rows());
+  for (size_t i = 0; i < limit; ++i) {
+    const double x = fixed_traj_(i, 0), y = fixed_traj_(i, 1), th = fixed_traj_(i, 2);
+    if (!isSafe(x, y, th)) { first_coll_idx = static_cast<int>(i); break; }
+  }
+  
+  if (first_coll_idx == 0) {
+  const double old_disp = max_total_disp_;
+  const int    old_iters = max_iters_;
+  const double old_step  = max_step_;
+
+  max_total_disp_ = std::max(max_total_disp_, 0.8);  // było 0.5
+  max_iters_      = std::max(max_iters_, 14);        // było 10
+  max_step_       = std::max(max_step_, 0.08);       // ≥ r
+
+  const int window_samples = std::max(1,
+    static_cast<int>(std::ceil(auto_fix_ttc_window_ / std::max(1e-6f, model_dt_))));
+  const bool ok0 = tryRepair(fixed_traj_, 0, std::min<int>(limit, window_samples));
+
+  // Przywróć
+  max_total_disp_ = old_disp;
+  max_iters_ = old_iters;
+  max_step_  = old_step;
+
+  if (!ok0) {
+    RCLCPP_WARN_THROTTLE(node->get_logger(), *node->get_clock(), 1000,
+      "[%s] Collision at sample 0: requesting soft reset for re-sampling.",
+      name_.c_str());
+    return ValidationResult::SOFT_RESET;
+  }
+
+  // Drugi pass: potwierdź brak kolizji po naprawie TTC=0 na całym oknie
+  bool clean = true;
+  for (size_t j = 0; j < limit; ++j) {
+    if (!isSafe(fixed_traj_(j,0), fixed_traj_(j,1), fixed_traj_(j,2))) { clean = false; break; }
+  }
+  if (clean) {
+    has_fixed_ = true;
+    RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000,
+      "[%s] Auto-fix SUCCESS at TTC=0: using repaired trajectory.", name_.c_str());
+    return ValidationResult::SUCCESS;  // ważne: NIE resetuj, pozwól kontrolerowi użyć fixed_traj_
+  }
+
+  // Inaczej wpadnij do standardowej ścieżki naprawy dalej w kodzie
+}
+
+
+
+  if (first_coll_idx > 0) {
+    const double ttc = static_cast<double>(first_coll_idx) * static_cast<double>(model_dt_);
+    RCLCPP_WARN_THROTTLE(
+      node->get_logger(), *node->get_clock(), 1000,
+      "[%s] Collision predicted in %.2f s at sample %d (%.2f, %.2f). Attempting auto-fix...",
+      name_.c_str(), ttc, first_coll_idx, fixed_traj_(first_coll_idx, 0), fixed_traj_(first_coll_idx, 1));
+
+    // zakres naprawy: [first - pre, first + window]
+    const int window_samples = std::max(1, static_cast<int>(std::ceil(
+      auto_fix_ttc_window_ / std::max(1e-6f, model_dt_))));
+    const int pre       = std::min(6, first_coll_idx); // ~ 6 próbek wstecz
+    const int start_idx = first_coll_idx - pre;
+    const int end_idx   = std::min<int>(static_cast<int>(limit), first_coll_idx + window_samples);
+
+    // 2) próba naprawy
+    const bool ok = tryRepair(fixed_traj_, start_idx, end_idx);
+    if (!ok) {
+      RCLCPP_ERROR_THROTTLE(
+        node->get_logger(), *node->get_clock(), 1000,
+        "[%s] Auto-fix FAILED: trajectory still in collision after repair attempts.",
+        name_.c_str());
+      return ValidationResult::SOFT_RESET;
+    }
+
+    // 3) potwierdź brak kolizji po naprawie
+    for (size_t j = 0; j < limit; ++j) {
+      if (!isSafe(fixed_traj_(j, 0), fixed_traj_(j, 1), fixed_traj_(j, 2))) {
+        RCLCPP_ERROR_THROTTLE(
+          node->get_logger(), *node->get_clock(), 1000,
+          "[%s] Auto-fix incomplete: remaining collision at sample %zu.",
+          name_.c_str(), j);
+        return ValidationResult::SOFT_RESET;
+      }
+    }
+
+    has_fixed_ = true;
+    RCLCPP_INFO_THROTTLE(
+      node->get_logger(), *node->get_clock(), 1000,
+      "[%s] Auto-fix SUCCESS: repaired up to %.2f s window; TTC was %.2f s.",
+      name_.c_str(), (first_coll_idx + window_samples) * model_dt_, ttc);
+
+    return ValidationResult::SUCCESS;
+  }
+
+  // już było dobrze
+  return ValidationResult::SUCCESS;
+}
+
+} // namespace mppi
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(mppi::AutoFixingTrajectoryValidator, mppi::OptimalTrajectoryValidator)
diff --git a/nav2_mppi_controller/src/trajectory_visualizer copy 2.cpp b/nav2_mppi_controller/src/trajectory_visualizer copy 2.cpp
new file mode 100644
index 00000000000..6960b5676c6
--- /dev/null
+++ b/nav2_mppi_controller/src/trajectory_visualizer copy 2.cpp	
@@ -0,0 +1,156 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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 <memory>
+#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
+
+namespace mppi
+{
+
+void TrajectoryVisualizer::on_configure(
+  nav2::LifecycleNode::WeakPtr parent, const std::string & name,
+  const std::string & frame_id, ParametersHandler * parameters_handler)
+{
+  auto node = parent.lock();
+  logger_ = node->get_logger();
+  frame_id_ = frame_id;
+  trajectories_publisher_ =
+    node->create_publisher<visualization_msgs::msg::MarkerArray>("~/candidate_trajectories");
+  transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>(
+    "~/transformed_global_plan");
+  optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("~/optimal_path");
+  parameters_handler_ = parameters_handler;
+
+  auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");
+
+  getParam(trajectory_step_, "trajectory_step", 5);
+  getParam(time_step_, "time_step", 3);
+
+  reset();
+}
+
+void TrajectoryVisualizer::on_cleanup()
+{
+  trajectories_publisher_.reset();
+  transformed_path_pub_.reset();
+  optimal_path_pub_.reset();
+}
+
+void TrajectoryVisualizer::on_activate()
+{
+  trajectories_publisher_->on_activate();
+  transformed_path_pub_->on_activate();
+  optimal_path_pub_->on_activate();
+}
+
+void TrajectoryVisualizer::on_deactivate()
+{
+  trajectories_publisher_->on_deactivate();
+  transformed_path_pub_->on_deactivate();
+  optimal_path_pub_->on_deactivate();
+}
+
+void TrajectoryVisualizer::add(
+  const Eigen::ArrayXXf & trajectory,
+  const std::string & marker_namespace,
+  const builtin_interfaces::msg::Time & cmd_stamp)
+{
+  size_t size = trajectory.rows();
+  if (!size) {
+    return;
+  }
+
+  auto add_marker = [&](auto i) {
+      float component = static_cast<float>(i) / static_cast<float>(size);
+
+      auto pose = utils::createPose(trajectory(i, 0), trajectory(i, 1), 0.06);
+      auto scale =
+        i != size - 1 ?
+        utils::createScale(0.03, 0.03, 0.07) :
+        utils::createScale(0.07, 0.07, 0.09);
+      auto color = utils::createColor(0, component, component, 1);
+      auto marker = utils::createMarker(
+        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
+      points_->markers.push_back(marker);
+
+      // populate optimal path
+      geometry_msgs::msg::PoseStamped pose_stamped;
+      pose_stamped.header.frame_id = frame_id_;
+      pose_stamped.pose = pose;
+
+      tf2::Quaternion quaternion_tf2;
+      quaternion_tf2.setRPY(0., 0., trajectory(i, 2));
+      pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2);
+
+      optimal_path_->poses.push_back(pose_stamped);
+    };
+
+  optimal_path_->header.stamp = cmd_stamp;
+  optimal_path_->header.frame_id = frame_id_;
+  for (size_t i = 0; i < size; i++) {
+    add_marker(i);
+  }
+}
+
+void TrajectoryVisualizer::add(
+  const models::Trajectories & trajectories, const std::string & marker_namespace)
+{
+  size_t n_rows = trajectories.x.rows();
+  size_t n_cols = trajectories.x.cols();
+  const float shape_1 = static_cast<float>(n_cols);
+  points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_));
+
+  for (size_t i = 0; i < n_rows; i += trajectory_step_) {
+    for (size_t j = 0; j < n_cols; j += time_step_) {
+      const float j_flt = static_cast<float>(j);
+      float blue_component = 1.0f - j_flt / shape_1;
+      float green_component = j_flt / shape_1;
+
+      auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03);
+      auto scale = utils::createScale(0.03, 0.03, 0.03);
+      auto color = utils::createColor(0, green_component, blue_component, 1);
+      auto marker = utils::createMarker(
+        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
+
+      points_->markers.push_back(marker);
+    }
+  }
+}
+
+void TrajectoryVisualizer::reset()
+{
+  marker_id_ = 0;
+  points_ = std::make_unique<visualization_msgs::msg::MarkerArray>();
+  optimal_path_ = std::make_unique<nav_msgs::msg::Path>();
+}
+
+void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
+{
+  if (trajectories_publisher_->get_subscription_count() > 0) {
+    trajectories_publisher_->publish(std::move(points_));
+  }
+
+  if (optimal_path_pub_->get_subscription_count() > 0) {
+    optimal_path_pub_->publish(std::move(optimal_path_));
+  }
+
+  reset();
+
+  if (transformed_path_pub_->get_subscription_count() > 0) {
+    auto plan_ptr = std::make_unique<nav_msgs::msg::Path>(plan);
+    transformed_path_pub_->publish(std::move(plan_ptr));
+  }
+}
+
+}  // namespace mppi
diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp
index 6960b5676c6..28141994791 100644
--- a/nav2_mppi_controller/src/trajectory_visualizer.cpp
+++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp
@@ -13,6 +13,8 @@
 // limitations under the License.
 
 #include <memory>
+#include <vector>
+#include <algorithm>
 #include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
 
 namespace mppi
@@ -25,17 +27,38 @@ void TrajectoryVisualizer::on_configure(
   auto node = parent.lock();
   logger_ = node->get_logger();
   frame_id_ = frame_id;
-  trajectories_publisher_ =
-    node->create_publisher<visualization_msgs::msg::MarkerArray>("~/candidate_trajectories");
-  transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>(
-    "~/transformed_global_plan");
-  optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("~/optimal_path");
   parameters_handler_ = parameters_handler;
-
-  auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");
-
+  auto getParam = parameters_handler->getParamGetter(name + ".Visualization");
   getParam(trajectory_step_, "trajectory_step", 5);
   getParam(time_step_, "time_step", 3);
+  getParam(publish_trajectories_with_total_cost_, "publish_trajectories_with_total_cost", false);
+  getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost",
+      false);
+  getParam(publish_optimal_footprints_, "publish_optimal_footprints", false);
+  getParam(publish_optimal_trajectory_msg_, "publish_optimal_trajectory_msg", false);
+  getParam(publish_transformed_path_, "publish_transformed_path", false);
+  getParam(publish_optimal_path_, "publish_optimal_path", false);
+  getParam(footprint_downsample_factor_, "footprint_downsample_factor", 3);
+
+  if (publish_trajectories_with_total_cost_ || publish_trajectories_with_individual_cost_) {
+    trajectories_publisher_ =
+      node->create_publisher<visualization_msgs::msg::MarkerArray>("~/candidate_trajectories");
+  }
+  if (publish_transformed_path_) {
+    transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>(
+      "~/transformed_global_plan");
+  }
+  if (publish_optimal_path_) {
+    optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("~/optimal_path");
+  }
+  if (publish_optimal_footprints_) {
+    optimal_footprints_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "~/optimal_footprints");
+  }
+  if (publish_optimal_trajectory_msg_) {
+    optimal_trajectory_msg_pub_ = node->create_publisher<nav2_msgs::msg::Trajectory>(
+      "~/optimal_trajectory");
+  }
 
   reset();
 }
@@ -45,20 +68,46 @@ void TrajectoryVisualizer::on_cleanup()
   trajectories_publisher_.reset();
   transformed_path_pub_.reset();
   optimal_path_pub_.reset();
+  optimal_footprints_pub_.reset();
+  optimal_trajectory_msg_pub_.reset();
 }
 
 void TrajectoryVisualizer::on_activate()
 {
-  trajectories_publisher_->on_activate();
-  transformed_path_pub_->on_activate();
-  optimal_path_pub_->on_activate();
+  if (trajectories_publisher_) {
+    trajectories_publisher_->on_activate();
+  }
+  if (transformed_path_pub_) {
+    transformed_path_pub_->on_activate();
+  }
+  if (optimal_path_pub_) {
+    optimal_path_pub_->on_activate();
+  }
+  if (optimal_footprints_pub_) {
+    optimal_footprints_pub_->on_activate();
+  }
+  if (optimal_trajectory_msg_pub_) {
+    optimal_trajectory_msg_pub_->on_activate();
+  }
 }
 
 void TrajectoryVisualizer::on_deactivate()
 {
-  trajectories_publisher_->on_deactivate();
-  transformed_path_pub_->on_deactivate();
-  optimal_path_pub_->on_deactivate();
+  if (trajectories_publisher_) {
+    trajectories_publisher_->on_deactivate();
+  }
+  if (transformed_path_pub_) {
+    transformed_path_pub_->on_deactivate();
+  }
+  if (optimal_path_pub_) {
+    optimal_path_pub_->on_deactivate();
+  }
+  if (optimal_footprints_pub_) {
+    optimal_footprints_pub_->on_deactivate();
+  }
+  if (optimal_trajectory_msg_pub_) {
+    optimal_trajectory_msg_pub_->on_deactivate();
+  }
 }
 
 void TrajectoryVisualizer::add(
@@ -104,27 +153,132 @@ void TrajectoryVisualizer::add(
 }
 
 void TrajectoryVisualizer::add(
-  const models::Trajectories & trajectories, const std::string & marker_namespace)
+  const models::Trajectories & trajectories,
+  const Eigen::ArrayXf & total_costs,
+  const std::vector<std::pair<std::string, Eigen::ArrayXf>> & individual_critics_cost,
+  const builtin_interfaces::msg::Time & cmd_stamp,
+  const std::optional<std::vector<bool>> & trajectories_in_collision)
+{
+  // Check if we should visualize per-critic costs
+  bool visualize_per_critic = !individual_critics_cost.empty() &&
+    publish_trajectories_with_individual_cost_ &&
+    trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0;
+
+  size_t n_rows = trajectories.x.rows();
+  points_->markers.reserve(n_rows / trajectory_step_);
+
+  // Visualize total costs if requested
+  if (publish_trajectories_with_total_cost_) {
+    createTrajectoryMarkers(trajectories, total_costs, "Total Costs", cmd_stamp, trajectories_in_collision);
+  }
+
+  // Visualize each critic's contribution if requested
+  if (visualize_per_critic) {
+    for (const auto & [critic_name, costs] : individual_critics_cost) {
+      createTrajectoryMarkers(trajectories, costs, critic_name, cmd_stamp, trajectories_in_collision);
+    }
+  }
+}
+
+void TrajectoryVisualizer::createTrajectoryMarkers(
+  const models::Trajectories & trajectories,
+  const Eigen::ArrayXf & costs,
+  const std::string & ns,
+  const builtin_interfaces::msg::Time & cmd_stamp,
+  const std::optional<std::vector<bool>> & trajectories_in_collision)
 {
   size_t n_rows = trajectories.x.rows();
   size_t n_cols = trajectories.x.cols();
-  const float shape_1 = static_cast<float>(n_cols);
-  points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_));
+
+  // Find min/max cost for normalization, excluding trajectories in collision
+  float min_cost = std::numeric_limits<float>::max();
+  float max_cost = std::numeric_limits<float>::lowest();
+
+  for (Eigen::Index i = 0; i < costs.size(); ++i) {
+    // Skip trajectories in collision when computing min/max for normalization
+    bool in_collision = trajectories_in_collision && 
+                        i < static_cast<Eigen::Index>(trajectories_in_collision->size()) && 
+                        (*trajectories_in_collision)[i];
+    
+    if (!in_collision) {
+      min_cost = std::min(min_cost, costs(i));
+      max_cost = std::max(max_cost, costs(i));
+    }
+  }
+
+  float cost_range = max_cost - min_cost;
+
+  // Avoid division by zero
+  if (cost_range < 1e-6f) {
+    cost_range = 1.0f;
+  }
 
   for (size_t i = 0; i < n_rows; i += trajectory_step_) {
-    for (size_t j = 0; j < n_cols; j += time_step_) {
-      const float j_flt = static_cast<float>(j);
-      float blue_component = 1.0f - j_flt / shape_1;
-      float green_component = j_flt / shape_1;
+    float red_component, green_component, blue_component;
 
-      auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03);
-      auto scale = utils::createScale(0.03, 0.03, 0.03);
-      auto color = utils::createColor(0, green_component, blue_component, 1);
-      auto marker = utils::createMarker(
-        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
+    // Check if trajectory is in collision
+    bool in_collision = trajectories_in_collision && 
+                        i < trajectories_in_collision->size() && 
+                        (*trajectories_in_collision)[i];
 
-      points_->markers.push_back(marker);
+    if (in_collision) {
+      // Fuschia color for trajectories in collision (magenta/pink)
+      red_component = 1.0f;
+      green_component = 0.0f;
+      blue_component = 1.0f;
+    } else {
+      // Check if cost is zero (no contribution from this critic)
+      bool zero_cost = std::abs(costs(i)) < 1e-6f;
+
+      if (zero_cost) {
+        // Gray color for zero cost (no contribution)
+        red_component = 0.5f;
+        green_component = 0.5f;
+        blue_component = 0.5f;
+      } else {
+        // Normal gradient for trajectories
+        float normalized_cost = (costs(i) - min_cost) / cost_range;
+        normalized_cost = std::clamp(normalized_cost, 0.0f, 1.0f);
+
+        // Color scheme: Green (low cost) -> Yellow -> Red (high cost)
+        blue_component = 0.0f;
+        if (normalized_cost < 0.5f) {
+          // Green to Yellow (0.0 - 0.5)
+          red_component = normalized_cost * 2.0f;
+          green_component = 1.0f;
+        } else {
+          // Yellow to Red (0.5 - 1.0)
+          red_component = 1.0f;
+          green_component = 2.0f * (1.0f - normalized_cost);
+        }
+      }
     }
+
+    // Create line strip marker for this trajectory
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = frame_id_;
+    marker.header.stamp = cmd_stamp;
+    marker.ns = ns;
+    marker.id = marker_id_++;
+    marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.pose.orientation.w = 1.0;
+    marker.scale.x = 0.01;  // Line width
+    marker.color.r = red_component;
+    marker.color.g = green_component;
+    marker.color.b = blue_component;
+    marker.color.a = 1.0;
+
+    // Add all points in this trajectory to the line strip
+    for (size_t j = 0; j < n_cols; j += time_step_) {
+      geometry_msgs::msg::Point point;
+      point.x = trajectories.x(i, j);
+      point.y = trajectories.y(i, j);
+      point.z = 0.0f;
+      marker.points.push_back(point);
+    }
+
+    points_->markers.push_back(marker);
   }
 }
 
@@ -135,22 +289,168 @@ void TrajectoryVisualizer::reset()
   optimal_path_ = std::make_unique<nav_msgs::msg::Path>();
 }
 
-void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
+void TrajectoryVisualizer::visualize(
+  nav_msgs::msg::Path plan,
+  const Eigen::ArrayXXf & optimal_trajectory,
+  const models::ControlSequence & control_sequence,
+  float model_dt,
+  const builtin_interfaces::msg::Time & stamp,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
+  const models::Trajectories & candidate_trajectories,
+  const Eigen::ArrayXf & costs,
+  const std::vector<std::pair<std::string, Eigen::ArrayXf>> & critic_costs,
+  const std::optional<std::vector<bool>> & trajectories_in_collision)
 {
-  if (trajectories_publisher_->get_subscription_count() > 0) {
+  // Create header with frame from costmap
+  std_msgs::msg::Header header;
+  header.stamp = stamp;
+  header.frame_id = costmap_ros->getGlobalFrameID();
+
+  // Visualize trajectories with total costs
+  if (publish_trajectories_with_total_cost_ ||
+    (!publish_trajectories_with_individual_cost_ || critic_costs.empty()))
+  {
+    add(candidate_trajectories, costs, {}, stamp, trajectories_in_collision);
+  }
+
+  // Visualize trajectories with individual critic costs
+  if (publish_trajectories_with_individual_cost_ && !critic_costs.empty()) {
+    add(candidate_trajectories, costs, critic_costs, stamp, trajectories_in_collision);
+  }
+
+  // Add optimal trajectory to populate optimal_path_
+  if (publish_optimal_path_ && optimal_trajectory.rows() > 0) {
+    add(optimal_trajectory, "Optimal Trajectory", stamp);
+  }
+
+  // Publish trajectories
+  if (trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0) {
     trajectories_publisher_->publish(std::move(points_));
   }
 
-  if (optimal_path_pub_->get_subscription_count() > 0) {
+  // Publish optimal path if enabled
+  if (publish_optimal_path_ && optimal_path_pub_ &&
+    optimal_path_pub_->get_subscription_count() > 0)
+  {
     optimal_path_pub_->publish(std::move(optimal_path_));
   }
 
+  // Publish optimal footprints if enabled
+  if (publish_optimal_footprints_ && optimal_footprints_pub_ &&
+    optimal_footprints_pub_->get_subscription_count() > 0 &&
+    costmap_ros != nullptr && optimal_trajectory.rows() > 0)
+  {
+    auto footprints_msg = createFootprintMarkers(optimal_trajectory, header, costmap_ros);
+    optimal_footprints_pub_->publish(std::move(footprints_msg));
+  }
+
+  // Publish optimal trajectory message if enabled
+  if (publish_optimal_trajectory_msg_ && optimal_trajectory_msg_pub_ &&
+    optimal_trajectory_msg_pub_->get_subscription_count() > 0)
+  {
+    auto trajectory_msg = utils::toTrajectoryMsg(
+      optimal_trajectory,
+      control_sequence,
+      model_dt,
+      header);
+    optimal_trajectory_msg_pub_->publish(std::move(trajectory_msg));
+  }
+
   reset();
 
-  if (transformed_path_pub_->get_subscription_count() > 0) {
+  // Publish transformed path
+  if (transformed_path_pub_ && transformed_path_pub_->get_subscription_count() > 0) {
     auto plan_ptr = std::make_unique<nav_msgs::msg::Path>(plan);
     transformed_path_pub_->publish(std::move(plan_ptr));
   }
 }
 
-}  // namespace mppi
+void TrajectoryVisualizer::visualize(nav_msgs::msg::Path plan)
+{
+  // Simplified version for testing that only publishes what's been added
+  if (trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0) {
+    trajectories_publisher_->publish(std::move(points_));
+  }
+
+  if (publish_optimal_path_ && optimal_path_pub_ &&
+    optimal_path_pub_->get_subscription_count() > 0)
+  {
+    optimal_path_pub_->publish(std::move(optimal_path_));
+  }
+
+  reset();
+
+  if (transformed_path_pub_ && transformed_path_pub_->get_subscription_count() > 0) {
+    auto plan_ptr = std::make_unique<nav_msgs::msg::Path>(plan);
+    transformed_path_pub_->publish(std::move(plan_ptr));
+  }
+}
+
+visualization_msgs::msg::MarkerArray TrajectoryVisualizer::createFootprintMarkers(
+  const Eigen::ArrayXXf & trajectory,
+  const std_msgs::msg::Header & header,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (trajectory.rows() == 0) {
+    return marker_array;
+  }
+
+  // Get robot footprint
+  std::vector<geometry_msgs::msg::Point> robot_footprint =
+    costmap_ros->getRobotFootprint();
+
+  // Skip if footprint is empty or very small
+  if (robot_footprint.size() < 3) {
+    return marker_array;
+  }
+
+  // Create header for markers
+  std_msgs::msg::Header costmap_header;
+  costmap_header.frame_id = costmap_ros->getGlobalFrameID();
+  costmap_header.stamp = header.stamp;
+
+  int marker_id = 0;
+  for (int i = 0; i < trajectory.rows(); i += footprint_downsample_factor_) {
+    double x = trajectory(i, 0);
+    double y = trajectory(i, 1);
+    double theta = trajectory(i, 2);
+
+    // Create oriented footprint
+    geometry_msgs::msg::PolygonStamped oriented_footprint;
+    oriented_footprint.header = costmap_header;
+    nav2_costmap_2d::transformFootprint(x, y, theta, robot_footprint, oriented_footprint);
+    // Create marker for this footprint
+    visualization_msgs::msg::Marker marker;
+    marker.header = costmap_header;
+    marker.ns = "optimal_footprints";
+    marker.id = marker_id++;
+    marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.pose.orientation.w = 1.0;
+    // Set marker scale and color
+    marker.scale.x = 0.02;  // Line width
+    marker.color.r = 0.0;
+    marker.color.g = 1.0;
+    marker.color.b = 0.0;
+    marker.color.a = 0.8;
+    // Add footprint points to marker
+    for (const auto & point : oriented_footprint.polygon.points) {
+      geometry_msgs::msg::Point p;
+      p.x = point.x;
+      p.y = point.y;
+      p.z = 0.0;
+      marker.points.push_back(p);
+    }
+    // Close the polygon by adding the first point again
+    if (!marker.points.empty()) {
+      marker.points.push_back(marker.points[0]);
+    }
+
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+
+}  // namespace mppi
\ No newline at end of file
diff --git a/nav2_mppi_controller/test/critic_manager_test copy 2.cpp b/nav2_mppi_controller/test/critic_manager_test copy 2.cpp
new file mode 100644
index 00000000000..ff0116ed7de
--- /dev/null
+++ b/nav2_mppi_controller/test/critic_manager_test copy 2.cpp	
@@ -0,0 +1,160 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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 <chrono>
+#include <thread>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_mppi_controller/critic_manager.hpp"
+
+// Tests critic manager
+
+using namespace mppi;  // NOLINT
+using namespace mppi::critics;  // NOLINT
+
+class DummyCritic : public CriticFunction
+{
+public:
+  virtual void initialize() {initialized_ = true;}
+  virtual void score(CriticData & /*data*/) {scored_ = true;}
+  bool initialized_{false}, scored_{false};
+};
+
+class CriticManagerWrapper : public CriticManager
+{
+public:
+  CriticManagerWrapper()
+  : CriticManager() {}
+
+  virtual ~CriticManagerWrapper() = default;
+
+  virtual void loadCritics()
+  {
+    critics_.clear();
+    auto instance = std::unique_ptr<critics::CriticFunction>(new DummyCritic);
+    critics_.push_back(std::move(instance));
+    critics_.back()->on_configure(
+      parent_, name_, name_ + "." + "DummyCritic", costmap_ros_,
+      parameters_handler_);
+  }
+
+  std::string getFullNameWrapper(const std::string & name)
+  {
+    return getFullName(name);
+  }
+
+  bool getDummyCriticInitialized()
+  {
+    const auto critic = critics_[0].get();
+    if (critic == nullptr) {
+      return false;
+    }
+    const auto dummy_critic = dynamic_cast<DummyCritic *>(critic);
+    return dummy_critic == nullptr ? false : dummy_critic->initialized_;
+  }
+
+  bool getDummyCriticScored()
+  {
+    const auto critic = critics_[0].get();
+    if (critic == nullptr) {
+      return false;
+    }
+    const auto dummy_critic = dynamic_cast<DummyCritic *>(critic);
+    return dummy_critic == nullptr ? false : dummy_critic->scored_;
+  }
+};
+
+class CriticManagerWrapperEnum : public CriticManager
+{
+public:
+  CriticManagerWrapperEnum()
+  : CriticManager() {}
+
+  virtual ~CriticManagerWrapperEnum() = default;
+
+  unsigned int getCriticNum()
+  {
+    return critics_.size();
+  }
+};
+
+TEST(CriticManagerTests, BasicCriticOperations)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  // Configuration should get parameters and initialize critic functions
+  CriticManagerWrapper critic_manager;
+  critic_manager.on_configure(node, "critic_manager", costmap_ros, &param_handler);
+  EXPECT_TRUE(critic_manager.getDummyCriticInitialized());
+
+  // Evaluation of critics should score them, but only if failure flag is not set
+  models::State state;
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  Eigen::ArrayXf costs;
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+    std::nullopt, std::nullopt};
+
+  data.fail_flag = true;
+  EXPECT_FALSE(critic_manager.getDummyCriticScored());
+  data.fail_flag = false;
+  critic_manager.evalTrajectoriesScores(data);
+  EXPECT_TRUE(critic_manager.getDummyCriticScored());
+
+  // This should get the full namespaced name of the critics
+  EXPECT_EQ(critic_manager.getFullNameWrapper("name"), std::string("mppi::critics::name"));
+}
+
+TEST(CriticManagerTests, CriticLoadingTest)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  node->declare_parameter(
+    "critic_manager.critics",
+    rclcpp::ParameterValue(std::vector<std::string>{"ConstraintCritic", "PreferForwardCritic"}));
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State state;
+  costmap_ros->on_configure(state);
+
+  // This should grab the critics parameter and load the 2 requested plugins
+  CriticManagerWrapperEnum critic_manager;
+  critic_manager.on_configure(node, "critic_manager", costmap_ros, &param_handler);
+  EXPECT_EQ(critic_manager.getCriticNum(), 2u);
+}
+
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  rclcpp::init(0, nullptr);
+
+  int result = RUN_ALL_TESTS();
+
+  rclcpp::shutdown();
+
+  return result;
+}
diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp
index ff0116ed7de..f29b0ac2d41 100644
--- a/nav2_mppi_controller/test/critic_manager_test.cpp
+++ b/nav2_mppi_controller/test/critic_manager_test.cpp
@@ -114,7 +114,8 @@ TEST(CriticManagerTests, BasicCriticOperations)
   Eigen::ArrayXf costs;
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt,
+    nullptr, nullptr,
     std::nullopt, std::nullopt};
 
   data.fail_flag = true;
@@ -157,4 +158,4 @@ int main(int argc, char **argv)
   rclcpp::shutdown();
 
   return result;
-}
+}
\ No newline at end of file
diff --git a/nav2_mppi_controller/test/critics_tests copy 2.cpp b/nav2_mppi_controller/test/critics_tests copy 2.cpp
new file mode 100644
index 00000000000..88e823019a7
--- /dev/null
+++ b/nav2_mppi_controller/test/critics_tests copy 2.cpp	
@@ -0,0 +1,794 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <thread>
+#include <random>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_mppi_controller/motion_models.hpp"
+#include "nav2_mppi_controller/critics/constraint_critic.hpp"
+#include "nav2_mppi_controller/critics/goal_angle_critic.hpp"
+#include "nav2_mppi_controller/critics/goal_critic.hpp"
+#include "nav2_mppi_controller/critics/obstacles_critic.hpp"
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
+#include "nav2_mppi_controller/critics/path_align_critic.hpp"
+#include "nav2_mppi_controller/critics/path_angle_critic.hpp"
+#include "nav2_mppi_controller/critics/path_follow_critic.hpp"
+#include "nav2_mppi_controller/critics/prefer_forward_critic.hpp"
+#include "nav2_mppi_controller/critics/twirling_critic.hpp"
+#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp"
+#include "utils_test.cpp"  // NOLINT
+
+// Tests the various critic plugin functions
+
+// ROS lock used from utils_test.cpp
+
+using namespace mppi;  // NOLINT
+using namespace mppi::critics;  // NOLINT
+using namespace mppi::utils;  // NOLINT
+
+class PathAngleCriticWrapper : public PathAngleCritic
+{
+public:
+  PathAngleCriticWrapper()
+  : PathAngleCritic()
+  {
+  }
+
+  void setMode(int mode)
+  {
+    mode_ = static_cast<PathAngleMode>(mode);
+  }
+};
+
+TEST(CriticTests, ConstraintsCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  // provide velocities in constraints, should not have any costs
+  state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30);
+  state.vy = Eigen::ArrayXXf::Zero(1000, 30);
+  state.wz = Eigen::ArrayXXf::Ones(1000, 30);
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+
+  // Initialization testing
+
+  // Make sure initializes correctly and that defaults are reasonable
+  ConstraintCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+  EXPECT_TRUE(critic.getMaxVelConstraint() > 0.0);
+  EXPECT_TRUE(critic.getMinVelConstraint() < 0.0);
+
+  // Scoring testing
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0, 1e-6);
+
+  // provide out of maximum velocity constraint
+  state.vx.row(999).setConstant(0.60f);
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0);
+  // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2
+  EXPECT_NEAR(costs(999), 1.2, 0.01);
+  costs.setZero();
+
+  // provide out of minimum velocity constraint
+  state.vx.row(1).setConstant(-0.45f);
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0);
+  // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2
+  EXPECT_NEAR(costs(1), 1.2, 0.01);
+  costs.setZero();
+
+  // Now with ackermann, all in constraint so no costs to score
+  state.vx.setConstant(0.40f);
+  state.wz.setConstant(1.5f);
+  data.motion_model = std::make_shared<AckermannMotionModel>(&param_handler, node->get_name());
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0, 1e-6);
+
+  // Now violating the ackermann constraints
+  state.wz.setConstant(2.5f);
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0);
+  // 4.0 weight * 0.1 model_dt * (0.2 - 0.4/2.5) * 30 timesteps = 0.48
+  EXPECT_NEAR(costs(1), 0.48, 0.01);
+}
+
+TEST(CriticTests, ObstacleCriticMisalignedParams) {
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", true);
+
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  ObstaclesCritic critic;
+  // Expect throw when settings mismatched
+  EXPECT_THROW(
+    critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler),
+    nav2_core::ControllerException
+  );
+}
+
+TEST(CriticTests, ObstacleCriticAlignedParams) {
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", false);
+
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  ObstaclesCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+}
+
+
+TEST(CriticTests, CostCriticMisAlignedParams) {
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+   auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", true);
+  costmap_ros->on_configure(lstate);
+
+  CostCritic critic;
+  // Expect throw when settings mismatched
+  EXPECT_THROW(
+    critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler),
+    nav2_core::ControllerException
+  );
+}
+
+TEST(CriticTests, CostCriticAlignedParams) {
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+   auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", false);
+  costmap_ros->on_configure(lstate);
+
+  CostCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+}
+
+TEST(CriticTests, GoalAngleCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(10);
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+
+  // Initialization testing
+
+  // Make sure initializes correctly
+  GoalAngleCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing
+
+  // provide state poses and path too far from `threshold_to_consider` to consider
+  state.pose.pose.position.x = 1.0;
+  path.x(9) = 10.0;
+  path.y(9) = 0.0;
+  path.yaws(9) = 3.14;
+  goal.position.x = 10.0;
+  goal.position.y = 0.0;
+  goal.orientation.x = 0.0;
+  goal.orientation.y = 0.0;
+  goal.orientation.z = 1.0;
+  goal.orientation.w = 0.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0, 1e-6);
+
+  // Let's move it even closer, just to be sure it still doesn't trigger
+  state.pose.pose.position.x = 9.2;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0, 1e-6);
+
+  // provide state pose and path below `threshold_to_consider` to consider
+  state.pose.pose.position.x = 9.7;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0);
+  EXPECT_NEAR(costs(0), 9.42, 0.02);  // (3.14 - 0.0) * 3.0 weight
+}
+
+TEST(CriticTests, GoalCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(10);
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+
+  // Initialization testing
+
+  // Make sure initializes correctly
+  GoalCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing with all trajectories set to 0
+
+  // provide state poses and path far, should not trigger
+  state.pose.pose.position.x = 1.0;
+  path.x(9) = 10.0;
+  path.y(9) = 0.0;
+  goal.position.x = 10.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs(2), 0.0, 1e-6);  // (0 * 5.0 weight
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);  // Should all be 0 * 1000
+  costs.setZero();
+
+  // provide state pose and path close
+  path.x(9) = 0.5;
+  path.y(9) = 0.0;
+  goal.position.x = 0.5;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs(2), 2.5, 1e-6);  // (sqrt(10.0 * 10.0) * 5.0 weight
+  EXPECT_NEAR(costs.sum(), 2500.0, 1e-3);  // should be 2.5 * 1000
+}
+
+TEST(CriticTests, PathAngleCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  state.reset(1000, 30);
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(10);
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+  TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
+
+  // Initialization testing
+
+  // Make sure initializes correctly
+  PathAngleCriticWrapper critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing
+
+  // provide state poses and path close, within pose tolerance so won't do anything
+  state.pose.pose.position.x = 0.0;
+  state.pose.pose.position.y = 0.0;
+  path.x(9) = 0.15;
+  goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path close but outside of tol. with less than PI/2 angular diff.
+  path.x(9) = 0.95;
+  goal.position.x = 0.95;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  data.furthest_reached_path_point = 2;  // So it grabs the 2 + offset_from_furthest_ = 6th point
+  path.x(6) = 1.0;  // angle between path point and pose = 0 < max_angle_to_furthest_
+  path.y(6) = 0.0;
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path close but outside of tol. with more than PI/2 angular diff.
+  path.x(6) = -1.0;  // angle between path point and pose > max_angle_to_furthest_
+  path.y(6) = 4.0;
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0.0);
+  EXPECT_NEAR(costs(0), 3.9947, 1e-2);  // atan2(4,-1) [1.81] * 2.2 weight
+
+  // Set mode to no directional preferences + reset costs
+  critic.setMode(1);
+  costs.setZero();
+
+  // provide state pose and path close but outside of tol. with more than PI/2 angular diff.
+  path.x(6) = 1.0;  // angle between path point and pose < max_angle_to_furthest_
+  path.y(6) = 0.0;
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path close but outside of tol. with more than PI/2 angular diff.
+  path.x(6) = -1.0;  // angle between path pt and pose < max_angle_to_furthest_ IF non-directional
+  path.y(6) = 0.0;
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path close but outside of tol. with more than PI/2 angular diff.
+  path.x(6) = -1.0;  // angle between path point and pose < max_angle_to_furthest_
+  path.y(6) = 4.0;
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0.0);
+  // should use reverse orientation as the closer angle in no dir preference mode
+  EXPECT_NEAR(costs(0), 2.9167, 1e-2);
+
+  // Set mode to consider path directionality + reset costs
+  critic.setMode(2);
+  costs.setZero();
+
+  // provide state pose and path close but outside of tol. with more than PI/2 angular diff.
+  path.x(6) = 1.0;  // angle between path point and pose < max_angle_to_furthest_
+  path.y(6) = 0.0;
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path close but outside of tol. with more than PI/2 angular diff.
+  path.x(6) = -1.0;  // angle between path pt and pose < max_angle_to_furthest_ IF non-directional
+  path.y(6) = 0.0;
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path close but outside of tol. with more than PI/2 angular diff.
+  path.x(6) = -1.0;  // angle between path point and pose < max_angle_to_furthest_
+  path.y(6) = 4.0;
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0.0);
+  // should use reverse orientation as the closer angle in no dir preference mode
+  EXPECT_NEAR(costs(0), 2.9167, 1e-2);
+
+  PathAngleMode mode;
+  mode = PathAngleMode::FORWARD_PREFERENCE;
+  EXPECT_EQ(modeToStr(mode), std::string("Forward Preference"));
+  mode = PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS;
+  EXPECT_EQ(modeToStr(mode), std::string("Consider Feasible Path Orientations"));
+  mode = PathAngleMode::NO_DIRECTIONAL_PREFERENCE;
+  EXPECT_EQ(modeToStr(mode), std::string("No Directional Preference"));
+  mode = static_cast<PathAngleMode>(4);
+  EXPECT_EQ(modeToStr(mode), std::string("Invalid mode!"));
+}
+
+TEST(CriticTests, PreferForwardCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  state.reset(1000, 30);
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(10);
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+  TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
+
+  // Initialization testing
+
+  // Make sure initializes correctly
+  PreferForwardCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing
+
+  // provide state poses and path far away, not within positional tolerances
+  state.pose.pose.position.x = 1.0;
+  path.x(9) = 10.0;
+  goal.position.x = 10.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f);
+
+  // provide state pose and path close to trigger behavior but with all forward motion
+  path.x(9) = 0.15;
+  goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  state.vx.setOnes();
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f);
+
+  // provide state pose and path close to trigger behavior but with all reverse motion
+  state.vx.setConstant(-1.0f);
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0.0f);
+  EXPECT_NEAR(costs(0), 15.0f, 1e-3f);  // 1.0 * 0.1 model_dt * 5.0 weight * 30 length
+}
+
+TEST(CriticTests, TwirlingCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  state.reset(1000, 30);
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(10);
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+  TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
+  data.goal_checker = &goal_checker;
+
+  // Initialization testing
+
+  // Make sure initializes correctly
+  TwirlingCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing
+
+  // provide state poses and path far away, not within positional tolerances
+  state.pose.pose.position.x = 1.0;
+  path.x(9) = 10.0;
+  goal.position.x = 10.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path close to trigger behavior but with no angular variation
+  path.x(9) = 0.15;
+  goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  state.wz.setZero();
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // Provide nearby with some motion
+  state.wz.row(0).setConstant(10.0f);
+  critic.score(data);
+  EXPECT_NEAR(costs(0), 100.0, 1e-6);  // (mean(10.0) * 10.0 weight
+  costs.setZero();
+
+  // Now try again with some wiggling noise
+  std::mt19937 engine;
+  std::normal_distribution<float> normal_dist = std::normal_distribution(0.0f, 0.5f);
+  state.wz.row(0) = Eigen::ArrayXf::NullaryExpr(30, [&] () {return normal_dist(engine);});
+  critic.score(data);
+  EXPECT_NEAR(costs(0), 2.581, 4e-1);  // (mean of noise with mu=0, sigma=0.5 * 10.0 weight
+}
+
+TEST(CriticTests, PathFollowCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  state.reset(1000, 30);
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(6);
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+  TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
+  data.goal_checker = &goal_checker;
+
+  // Initialization testing
+
+  // Make sure initializes correctly
+  PathFollowCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing
+
+  // provide state poses and goal close within positional tolerances
+  state.pose.pose.position.x = 2.0;
+  path.x(5) = 1.8;
+  goal.position.x = 1.8;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path far enough to enable
+  // pose differential is (0, 0) and (0.15, 0)
+  path.x(5) = 0.15;
+  goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 750.0, 1e-2);  // 0.15 * 5 weight * 1000
+}
+
+TEST(CriticTests, PathAlignCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  state.reset(1000, 30);
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(10);
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+  TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
+  data.goal_checker = &goal_checker;
+
+  // Initialization testing
+
+  // Make sure initializes correctly
+  PathAlignCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing
+
+  // provide state poses and path close within positional tolerances
+  state.pose.pose.position.x = 1.0;
+  path.x(9) = 0.85;
+  goal.position.x = 0.85;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path far enough to enable
+  // but data furthest point reached is 0 and offset default is 20, so returns
+  path.x(9) = 0.15;
+  goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path far enough to enable, with data to pass condition
+  // but with empty trajectories and paths, should still be zero
+  *data.furthest_reached_path_point = 21;
+  path.x(9) = 0.15;
+  goal.position.x = 0.15;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+
+  // provide state pose and path far enough to enable, with data to pass condition
+  // and with a valid path to pass invalid path condition
+  state.pose.pose.position.x = 0.0;
+  data.path_pts_valid.reset();  // Recompute on new path
+  path.reset(22);
+  path.x(0) = 0;
+  path.x(1) = 0.1;
+  path.x(2) = 0.2;
+  path.x(3) = 0.3;
+  path.x(4) = 0.4;
+  path.x(5) = 0.5;
+  path.x(6) = 0.6;
+  path.x(7) = 0.7;
+  path.x(8) = 0.8;
+  path.x(9) = 0.9;
+  path.x(10) = 0.9;
+  path.x(11) = 0.9;
+  path.x(12) = 0.9;
+  path.x(13) = 0.9;
+  path.x(14) = 0.9;
+  path.x(15) = 0.9;
+  path.x(16) = 0.9;
+  path.x(17) = 0.9;
+  path.x(18) = 0.9;
+  path.x(19) = 0.9;
+  path.x(20) = 0.9;
+  path.x(21) = 0.9;
+  goal.position.x = 0.9;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  generated_trajectories.x.setConstant(0.66f);
+  critic.score(data);
+  // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term
+  EXPECT_NEAR(costs.sum(), 6600.0, 1e-2);
+
+  // provide state pose and path far enough to enable, with data to pass condition
+  // but path is blocked in collision
+  auto * costmap = costmap_ros->getCostmap();
+  // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution
+  for (unsigned int i = 11; i <= 30; ++i) {  // 1.1m-3m
+    for (unsigned int j = 11; j <= 30; ++j) {  // 1.1m-3m
+      costmap->setCost(i, j, 254);
+    }
+  }
+
+  data.path_pts_valid.reset();  // Recompute on new path
+  costs.setZero();
+  path.x.setConstant(1.5f);
+  path.y.setConstant(1.5f);
+  goal.position.x = 1.5;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0.0, 1e-6);
+}
+
+TEST(CriticTests, VelocityDeadbandCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  auto getParam = param_handler.getParamGetter("critic");
+  std::vector<double> deadband_velocities_;
+  getParam(deadband_velocities_, "deadband_velocities", std::vector<double>{0.08, 0.08, 0.08});
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  state.reset(1000, 30);
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<OmniMotionModel>();
+
+  // Initialization testing
+
+  // Make sure initializes correctly and that defaults are reasonable
+  VelocityDeadbandCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing
+
+  // provide velocities out of deadband bounds, should not have any costs
+  state.vx.setConstant(0.80f);
+  state.vy.setConstant(0.60f);
+  state.wz.setConstant(0.80f);
+  critic.score(data);
+  EXPECT_NEAR(costs.sum(), 0, 1e-6);
+
+  // Test cost value
+  state.vx.setConstant(0.01f);
+  state.vy.setConstant(0.02f);
+  state.wz.setConstant(0.021f);
+  critic.score(data);
+  // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7
+  EXPECT_NEAR(costs(1), 19.845, 0.01);
+}
diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp
index 88e823019a7..7c54114b24b 100644
--- a/nav2_mppi_controller/test/critics_tests.cpp
+++ b/nav2_mppi_controller/test/critics_tests.cpp
@@ -79,8 +79,8 @@ TEST(CriticTests, ConstraintsCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<DiffDriveMotionModel>();
 
   // Initialization testing
@@ -227,8 +227,8 @@ TEST(CriticTests, GoalAngleCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<DiffDriveMotionModel>();
 
   // Initialization testing
@@ -290,8 +290,8 @@ TEST(CriticTests, GoalCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<DiffDriveMotionModel>();
 
   // Initialization testing
@@ -346,8 +346,8 @@ TEST(CriticTests, PathAngleCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<DiffDriveMotionModel>();
   TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
 
@@ -467,8 +467,8 @@ TEST(CriticTests, PreferForwardCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<DiffDriveMotionModel>();
   TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
 
@@ -526,8 +526,8 @@ TEST(CriticTests, TwirlingCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<DiffDriveMotionModel>();
   TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
   data.goal_checker = &goal_checker;
@@ -593,8 +593,8 @@ TEST(CriticTests, PathFollowCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<DiffDriveMotionModel>();
   TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
   data.goal_checker = &goal_checker;
@@ -647,8 +647,8 @@ TEST(CriticTests, PathAlignCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<DiffDriveMotionModel>();
   TestGoalChecker goal_checker;  // from utils_tests tolerance of 0.25 positionally
   data.goal_checker = &goal_checker;
@@ -764,8 +764,8 @@ TEST(CriticTests, VelocityDeadbandCritic)
   Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt,
-    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt,
+    false, std::nullopt, nullptr, nullptr, std::nullopt, std::nullopt};
   data.motion_model = std::make_shared<OmniMotionModel>();
 
   // Initialization testing
@@ -791,4 +791,4 @@ TEST(CriticTests, VelocityDeadbandCritic)
   critic.score(data);
   // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7
   EXPECT_NEAR(costs(1), 19.845, 0.01);
-}
+}
\ No newline at end of file
diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests copy 2.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests copy 2.cpp
new file mode 100644
index 00000000000..7e710fc7375
--- /dev/null
+++ b/nav2_mppi_controller/test/trajectory_visualizer_tests copy 2.cpp	
@@ -0,0 +1,229 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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 <chrono>
+#include <thread>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
+
+// Tests trajectory visualization
+
+using namespace mppi;  // NOLINT
+
+TEST(TrajectoryVisualizerTests, StateTransition)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  std::string name = "test";
+  auto parameters_handler = std::make_unique<ParametersHandler>(node, name);
+
+  TrajectoryVisualizer vis;
+  vis.on_configure(node, "my_name", "map", parameters_handler.get());
+  vis.on_activate();
+  vis.on_deactivate();
+  vis.on_cleanup();
+}
+
+TEST(TrajectoryVisualizerTests, VisPathRepub)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  std::string name = "test";
+  auto parameters_handler = std::make_unique<ParametersHandler>(node, name);
+  nav_msgs::msg::Path received_path;
+  nav_msgs::msg::Path pub_path;
+  pub_path.header.frame_id = "fake_frame";
+  pub_path.poses.resize(5);
+
+  auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
+    "~/transformed_global_plan",
+    [&](const nav_msgs::msg::Path msg) {received_path = msg;});
+
+  TrajectoryVisualizer vis;
+  vis.on_configure(node, "my_name", "map", parameters_handler.get());
+  vis.on_activate();
+  vis.visualize(pub_path);
+
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_EQ(received_path.poses.size(), 5u);
+  EXPECT_EQ(received_path.header.frame_id, "fake_frame");
+}
+
+TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  std::string name = "test";
+  auto parameters_handler = std::make_unique<ParametersHandler>(node, name);
+
+  visualization_msgs::msg::MarkerArray received_msg;
+  auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
+    "~/candidate_trajectories",
+    [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;});
+
+  // optimal_trajectory empty, should fail to publish
+  Eigen::ArrayXXf optimal_trajectory;
+  TrajectoryVisualizer vis;
+  vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
+  vis.on_activate();
+  builtin_interfaces::msg::Time bogus_stamp;
+  vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
+  nav_msgs::msg::Path bogus_path;
+  vis.visualize(bogus_path);
+
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_EQ(received_msg.markers.size(), 0u);
+
+  // Now populated with content, should publish
+  optimal_trajectory = Eigen::ArrayXXf::Ones(20, 3);
+  vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
+  vis.visualize(bogus_path);
+
+  rclcpp::spin_some(node->get_node_base_interface());
+
+  // Should have 20 trajectory points in the map frame
+  EXPECT_EQ(received_msg.markers.size(), 20u);
+  EXPECT_EQ(received_msg.markers[0].header.frame_id, "fkmap");
+
+  // Check IDs are properly populated
+  EXPECT_EQ(received_msg.markers[0].id, 0);
+  EXPECT_EQ(received_msg.markers[1].id, 1);
+  EXPECT_EQ(received_msg.markers[10].id, 10);
+
+  // Check poses are correct
+  EXPECT_EQ(received_msg.markers[0].pose.position.x, 1);
+  EXPECT_EQ(received_msg.markers[0].pose.position.y, 1);
+  EXPECT_EQ(received_msg.markers[0].pose.position.z, 0.06);
+
+  // Check that scales are rational
+  EXPECT_EQ(received_msg.markers[0].scale.x, 0.03);
+  EXPECT_EQ(received_msg.markers[0].scale.y, 0.03);
+  EXPECT_EQ(received_msg.markers[0].scale.z, 0.07);
+
+  EXPECT_EQ(received_msg.markers[19].scale.x, 0.07);
+  EXPECT_EQ(received_msg.markers[19].scale.y, 0.07);
+  EXPECT_EQ(received_msg.markers[19].scale.z, 0.09);
+
+  // Check that the colors are rational
+  for (unsigned int i = 0; i != received_msg.markers.size() - 1; i++) {
+    EXPECT_LT(received_msg.markers[i].color.g, received_msg.markers[i + 1].color.g);
+    EXPECT_LT(received_msg.markers[i].color.b, received_msg.markers[i + 1].color.b);
+    EXPECT_EQ(received_msg.markers[i].color.r, received_msg.markers[i + 1].color.r);
+    EXPECT_EQ(received_msg.markers[i].color.a, received_msg.markers[i + 1].color.a);
+  }
+}
+
+TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  std::string name = "test";
+  auto parameters_handler = std::make_unique<ParametersHandler>(node, name);
+
+  visualization_msgs::msg::MarkerArray received_msg;
+  auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
+    "~/candidate_trajectories",
+    [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;});
+
+  models::Trajectories candidate_trajectories;
+  candidate_trajectories.x = Eigen::ArrayXXf::Ones(200, 12);
+  candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12);
+  candidate_trajectories.yaws = Eigen::ArrayXXf::Ones(200, 12);
+
+  TrajectoryVisualizer vis;
+  vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
+  vis.on_activate();
+  vis.add(candidate_trajectories, "Candidate Trajectories");
+  nav_msgs::msg::Path bogus_path;
+  vis.visualize(bogus_path);
+
+  rclcpp::spin_some(node->get_node_base_interface());
+  // 40 * 4, for 5 trajectory steps + 3 point steps
+  EXPECT_EQ(received_msg.markers.size(), 160u);
+}
+
+TEST(TrajectoryVisualizerTests, VisOptimalPath)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  std::string name = "test";
+  auto parameters_handler = std::make_unique<ParametersHandler>(node, name);
+  builtin_interfaces::msg::Time cmd_stamp;
+  cmd_stamp.sec = 5;
+  cmd_stamp.nanosec = 10;
+
+  nav_msgs::msg::Path received_path;
+  auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
+    "~/optimal_path",
+    [&](const nav_msgs::msg::Path msg) {received_path = msg;});
+
+  // optimal_trajectory empty, should fail to publish
+  Eigen::ArrayXXf optimal_trajectory;
+  TrajectoryVisualizer vis;
+  vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
+  vis.on_activate();
+  vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
+  nav_msgs::msg::Path bogus_path;
+  vis.visualize(bogus_path);
+
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_EQ(received_path.poses.size(), 0u);
+
+  // Now populated with content, should publish
+  optimal_trajectory.resize(20, 3);
+  for (unsigned int i = 0; i != optimal_trajectory.rows() - 1; i++) {
+    optimal_trajectory(i, 0) = static_cast<float>(i);
+    optimal_trajectory(i, 1) = static_cast<float>(i);
+    optimal_trajectory(i, 2) = static_cast<float>(i);
+  }
+  vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
+  vis.visualize(bogus_path);
+
+  rclcpp::spin_some(node->get_node_base_interface());
+
+  // Should have a 20 points path in the map frame and with same stamp than velocity command
+  EXPECT_EQ(received_path.poses.size(), 20u);
+  EXPECT_EQ(received_path.header.frame_id, "fkmap");
+  EXPECT_EQ(received_path.header.stamp.sec, cmd_stamp.sec);
+  EXPECT_EQ(received_path.header.stamp.nanosec, cmd_stamp.nanosec);
+
+  tf2::Quaternion quat;
+  for (unsigned int i = 0; i != received_path.poses.size() - 1; i++) {
+    // Poses should be in map frame too
+    EXPECT_EQ(received_path.poses[i].header.frame_id, "fkmap");
+
+    // Check positions are correct
+    EXPECT_EQ(received_path.poses[i].pose.position.x, static_cast<float>(i));
+    EXPECT_EQ(received_path.poses[i].pose.position.y, static_cast<float>(i));
+    EXPECT_EQ(received_path.poses[i].pose.position.z, 0.06);
+
+    // Check orientations are correct
+    quat.setRPY(0., 0., optimal_trajectory(i, 2));
+    auto expected_orientation = tf2::toMsg(quat);
+    EXPECT_EQ(received_path.poses[i].pose.orientation.x, expected_orientation.x);
+    EXPECT_EQ(received_path.poses[i].pose.orientation.y, expected_orientation.y);
+    EXPECT_EQ(received_path.poses[i].pose.orientation.z, expected_orientation.z);
+    EXPECT_EQ(received_path.poses[i].pose.orientation.w, expected_orientation.w);
+  }
+}
+
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  rclcpp::init(0, nullptr);
+
+  int result = RUN_ALL_TESTS();
+
+  rclcpp::shutdown();
+
+  return result;
+}
diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
index 7e710fc7375..64aefddd874 100644
--- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
+++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
@@ -50,12 +50,15 @@ TEST(TrajectoryVisualizerTests, VisPathRepub)
     "~/transformed_global_plan",
     [&](const nav_msgs::msg::Path msg) {received_path = msg;});
 
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node->get_node_base_interface());
+
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "map", parameters_handler.get());
   vis.on_activate();
   vis.visualize(pub_path);
 
-  rclcpp::spin_some(node->get_node_base_interface());
+  executor.spin_some();
   EXPECT_EQ(received_path.poses.size(), 5u);
   EXPECT_EQ(received_path.header.frame_id, "fake_frame");
 }
@@ -70,6 +73,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
   auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
     "~/candidate_trajectories",
     [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;});
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node->get_node_base_interface());
 
   // optimal_trajectory empty, should fail to publish
   Eigen::ArrayXXf optimal_trajectory;
@@ -81,7 +86,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
   nav_msgs::msg::Path bogus_path;
   vis.visualize(bogus_path);
 
-  rclcpp::spin_some(node->get_node_base_interface());
+  executor.spin_some();
   EXPECT_EQ(received_msg.markers.size(), 0u);
 
   // Now populated with content, should publish
@@ -89,7 +94,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
   vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
   vis.visualize(bogus_path);
 
-  rclcpp::spin_some(node->get_node_base_interface());
+  executor.spin_some();
 
   // Should have 20 trajectory points in the map frame
   EXPECT_EQ(received_msg.markers.size(), 20u);
@@ -128,25 +133,33 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
   auto node = std::make_shared<nav2::LifecycleNode>("my_node");
   std::string name = "test";
   auto parameters_handler = std::make_unique<ParametersHandler>(node, name);
+  builtin_interfaces::msg::Time cmd_stamp;
+  cmd_stamp.sec = 5;
+  cmd_stamp.nanosec = 10;
 
   visualization_msgs::msg::MarkerArray received_msg;
   auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
     "~/candidate_trajectories",
     [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;});
 
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node->get_node_base_interface());
+
   models::Trajectories candidate_trajectories;
   candidate_trajectories.x = Eigen::ArrayXXf::Ones(200, 12);
   candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12);
   candidate_trajectories.yaws = Eigen::ArrayXXf::Ones(200, 12);
 
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Random(200);
+
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
   vis.on_activate();
-  vis.add(candidate_trajectories, "Candidate Trajectories");
+  vis.add(candidate_trajectories, costs, {}, cmd_stamp);
   nav_msgs::msg::Path bogus_path;
   vis.visualize(bogus_path);
 
-  rclcpp::spin_some(node->get_node_base_interface());
+  executor.spin_some();
   // 40 * 4, for 5 trajectory steps + 3 point steps
   EXPECT_EQ(received_msg.markers.size(), 160u);
 }
@@ -165,6 +178,9 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
     "~/optimal_path",
     [&](const nav_msgs::msg::Path msg) {received_path = msg;});
 
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node->get_node_base_interface());
+
   // optimal_trajectory empty, should fail to publish
   Eigen::ArrayXXf optimal_trajectory;
   TrajectoryVisualizer vis;
@@ -174,7 +190,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
   nav_msgs::msg::Path bogus_path;
   vis.visualize(bogus_path);
 
-  rclcpp::spin_some(node->get_node_base_interface());
+  executor.spin_some();
   EXPECT_EQ(received_path.poses.size(), 0u);
 
   // Now populated with content, should publish
@@ -187,7 +203,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
   vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
   vis.visualize(bogus_path);
 
-  rclcpp::spin_some(node->get_node_base_interface());
+  executor.spin_some();
 
   // Should have a 20 points path in the map frame and with same stamp than velocity command
   EXPECT_EQ(received_path.poses.size(), 20u);
@@ -226,4 +242,4 @@ int main(int argc, char **argv)
   rclcpp::shutdown();
 
   return result;
-}
+}
\ No newline at end of file
diff --git a/nav2_mppi_controller/test/utils_test copy 2.cpp b/nav2_mppi_controller/test/utils_test copy 2.cpp
new file mode 100644
index 00000000000..70733e37db4
--- /dev/null
+++ b/nav2_mppi_controller/test/utils_test copy 2.cpp	
@@ -0,0 +1,614 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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 <chrono>
+#include <thread>
+#include <random>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_mppi_controller/models/path.hpp"
+
+// Tests noise generator object
+
+using namespace mppi::utils;  // NOLINT
+using namespace mppi;  // NOLINT
+
+class TestGoalChecker : public nav2_core::GoalChecker
+{
+public:
+  TestGoalChecker() {}
+
+  virtual void initialize(
+    const nav2::LifecycleNode::WeakPtr & /*parent*/,
+    const std::string & /*plugin_name*/,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/) {}
+
+  virtual void reset() {}
+
+  virtual bool isGoalReached(
+    const geometry_msgs::msg::Pose & /*query_pose*/,
+    const geometry_msgs::msg::Pose & /*goal_pose*/,
+    const geometry_msgs::msg::Twist & /*velocity*/) {return false;}
+
+  virtual bool getTolerances(
+    geometry_msgs::msg::Pose & pose_tolerance,
+    geometry_msgs::msg::Twist & /*vel_tolerance*/)
+  {
+    pose_tolerance.position.x = 0.25;
+    pose_tolerance.position.y = 0.25;
+    return true;
+  }
+};
+
+TEST(UtilsTests, MarkerPopulationUtils)
+{
+  auto pose = createPose(1.0, 2.0, 3.0);
+  EXPECT_EQ(pose.position.x, 1.0);
+  EXPECT_EQ(pose.position.y, 2.0);
+  EXPECT_EQ(pose.position.z, 3.0);
+  EXPECT_EQ(pose.orientation.w, 1.0);
+
+  auto scale = createScale(1.0, 2.0, 3.0);
+  EXPECT_EQ(scale.x, 1.0);
+  EXPECT_EQ(scale.y, 2.0);
+  EXPECT_EQ(scale.z, 3.0);
+
+  auto color = createColor(1.0, 2.0, 3.0, 0.0);
+  EXPECT_EQ(color.r, 1.0);
+  EXPECT_EQ(color.g, 2.0);
+  EXPECT_EQ(color.b, 3.0);
+  EXPECT_EQ(color.a, 0.0);
+
+  auto marker = createMarker(999, pose, scale, color, "map", "ns");
+  EXPECT_EQ(marker.header.frame_id, "map");
+  EXPECT_EQ(marker.id, 999);
+  EXPECT_EQ(marker.pose, pose);
+  EXPECT_EQ(marker.scale, scale);
+  EXPECT_EQ(marker.color, color);
+  EXPECT_EQ(marker.ns, "ns");
+}
+
+TEST(UtilsTests, ConversionTests)
+{
+  geometry_msgs::msg::TwistStamped output;
+  builtin_interfaces::msg::Time time;
+
+  // Check population is correct
+  output = toTwistStamped(0.5, 0.3, time, "map");
+  EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6);
+  EXPECT_NEAR(output.twist.linear.y, 0.0, 1e-6);
+  EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6);
+  EXPECT_EQ(output.header.frame_id, "map");
+  EXPECT_EQ(output.header.stamp, time);
+
+  output = toTwistStamped(0.5, 0.4, 0.3, time, "map");
+  EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6);
+  EXPECT_NEAR(output.twist.linear.y, 0.4, 1e-6);
+  EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6);
+  EXPECT_EQ(output.header.frame_id, "map");
+  EXPECT_EQ(output.header.stamp, time);
+
+  nav_msgs::msg::Path path;
+  path.poses.resize(5);
+  path.poses[2].pose.position.x = 5;
+  path.poses[2].pose.position.y = 50;
+  models::Path path_t = toTensor(path);
+
+  // Check population is correct
+  EXPECT_EQ(path_t.x.rows(), 5u);
+  EXPECT_EQ(path_t.y.rows(), 5u);
+  EXPECT_EQ(path_t.yaws.rows(), 5u);
+  EXPECT_EQ(path_t.x(2), 5);
+  EXPECT_EQ(path_t.y(2), 50);
+  EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6);
+}
+
+TEST(UtilsTests, AnglesTests)
+{
+  // Test angle normalization by creating insane angles
+  Eigen::ArrayXf angles(100);
+  angles.setConstant(1.0f);
+
+  for (unsigned int i = 0; i != angles.size(); i++) {
+    angles(i) = i * i;
+    if (i % 2 == 0) {
+      angles(i) *= -1;
+    }
+  }
+
+  auto norm_ang = normalize_angles(angles).eval();
+  for (unsigned int i = 0; i != norm_ang.size(); i++) {
+    EXPECT_TRUE((norm_ang(i) >= -M_PIF) && (norm_ang(i) <= M_PIF));
+  }
+
+  // Test shortest angular distance
+  Eigen::ArrayXf zero_angles(100);
+  zero_angles.setZero();
+  auto ang_dist = shortest_angular_distance(angles, zero_angles).eval();
+  for (unsigned int i = 0; i != ang_dist.size(); i++) {
+    EXPECT_TRUE((ang_dist(i) >= -M_PIF) && (ang_dist(i) <= M_PIF));
+  }
+
+  // Test point-pose angle
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 0.0;
+  pose.position.y = 0.0;
+  pose.orientation.w = 1.0;
+  double point_x = 1.0, point_y = 0.0;
+  bool forward_preference = true;
+  EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
+  forward_preference = false;
+  EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
+  point_x = -1.0;
+  EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
+  forward_preference = true;
+  EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), M_PI, 1e-6);
+
+  // Test point-pose angle with goal yaws
+  point_x = 1.0;
+  double point_yaw = 0.0;
+  EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), 0.0, 1e-6);
+  point_yaw = M_PI;
+  EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), M_PI, 1e-6);
+  point_yaw = 0.1;
+  EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), 0.0, 1e-3);
+  point_yaw = 3.04159;
+  EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), M_PI, 1e-3);
+}
+
+TEST(UtilsTests, FurthestAndClosestReachedPoint)
+{
+  models::State state;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(100, 2);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(10);
+  Eigen::ArrayXf costs;
+  float model_dt = 0.1;
+
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+    std::nullopt, std::nullopt};  /// Caution, keep references
+
+  // Attempt to set furthest point if notionally set, should not change
+  data.furthest_reached_path_point = 99999;
+  setPathFurthestPointIfNotSet(data);
+  EXPECT_EQ(data.furthest_reached_path_point, 99999);
+
+  // Attempt to set if not set already with no other information, should fail
+  CriticData data2 =
+  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+    std::nullopt, std::nullopt};  /// Caution, keep references
+  setPathFurthestPointIfNotSet(data2);
+  EXPECT_EQ(data2.furthest_reached_path_point, 0);
+
+  // Test the actual computation of the path point reached
+  generated_trajectories.x = Eigen::ArrayXXf::Ones(100, 2);
+  generated_trajectories.y = Eigen::ArrayXXf::Zero(100, 2);
+  generated_trajectories.yaws = Eigen::ArrayXXf::Zero(100, 2);
+
+  nav_msgs::msg::Path plan;
+  plan.poses.resize(10);
+  for (unsigned int i = 0; i != plan.poses.size(); i++) {
+    plan.poses[i].pose.position.x = 0.2 * i;
+    plan.poses[i].pose.position.y = 0.0;
+  }
+  path = toTensor(plan);
+
+  CriticData data3 =
+  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+    std::nullopt, std::nullopt};  /// Caution, keep references
+  EXPECT_EQ(findPathFurthestReachedPoint(data3), 5);
+}
+
+TEST(UtilsTests, findPathCosts)
+{
+  models::State state;
+  models::Trajectories generated_trajectories;
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(50);
+  Eigen::ArrayXf costs;
+  float model_dt = 0.1;
+
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+    std::nullopt, std::nullopt};  /// Caution, keep references
+
+  // Test not set if already set, should not change
+  data.path_pts_valid = std::vector<bool>(10, false);
+  for (unsigned int i = 0; i != 10; i++) {
+    (*data.path_pts_valid)[i] = false;
+  }
+  EXPECT_TRUE(data.path_pts_valid);
+  setPathCostsIfNotSet(data, nullptr);
+  EXPECT_EQ(data.path_pts_valid->size(), 10u);
+
+  CriticData data3 =
+  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+    std::nullopt, std::nullopt};  /// Caution, keep references
+
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+  auto * costmap = costmap_ros->getCostmap();
+  // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution
+  for (unsigned int i = 10; i <= 30; ++i) {  // 1m-3m
+    for (unsigned int j = 10; j <= 30; ++j) {  // 1m-3m
+      costmap->setCost(i, j, 254);
+    }
+  }
+  for (unsigned int i = 40; i <= 45; ++i) {  // 4m-4.5m
+    for (unsigned int j = 45; j <= 45; ++j) {  // 4m-4.5m
+      costmap->setCost(i, j, 253);
+    }
+  }
+
+  path.x(1) = 999999999;  // OFF COSTMAP
+  path.y(1) = 999999999;
+  path.x(10) = 1.5;  // IN LETHAL
+  path.y(10) = 1.5;
+  path.x(20) = 4.2;  // IN INFLATED
+  path.y(20) = 4.2;
+
+  // This should be evaluated and have real outputs now
+  setPathCostsIfNotSet(data3, costmap_ros);
+  EXPECT_TRUE(data3.path_pts_valid.has_value());
+  for (unsigned int i = 0; i != path.x.size() - 1; i++) {
+    if (i == 1 || i == 10) {
+      EXPECT_FALSE((*data3.path_pts_valid)[i]);
+    } else {
+      EXPECT_TRUE((*data3.path_pts_valid)[i]);
+    }
+  }
+}
+
+TEST(UtilsTests, SmootherTest)
+{
+  models::ControlSequence noisey_sequence, sequence_init;
+  noisey_sequence.vx = 0.2 * Eigen::ArrayXf::Ones(30);
+  noisey_sequence.vy = 0.0 * Eigen::ArrayXf::Ones(30);
+  noisey_sequence.wz = 0.3 * Eigen::ArrayXf::Ones(30);
+
+  // Make the sequence noisy
+  std::mt19937 engine;
+  std::normal_distribution<float> normal_dist = std::normal_distribution(0.0f, 0.2f);
+  auto noises = Eigen::ArrayXf::NullaryExpr(
+    30, [&] () {return normal_dist(engine);});
+  noisey_sequence.vx += noises;
+  noisey_sequence.vy += noises;
+  noisey_sequence.wz += noises;
+  sequence_init = noisey_sequence;
+
+  std::array<mppi::models::Control, 4> history, history_init;
+  history[3].vx = 0.1;
+  history[3].vy = 0.0;
+  history[3].wz = 0.3;
+  history[2].vx = 0.1;
+  history[2].vy = 0.0;
+  history[2].wz = 0.3;
+  history[1].vx = 0.1;
+  history[1].vy = 0.0;
+  history[1].wz = 0.3;
+  history[0].vx = 0.0;
+  history[0].vy = 0.0;
+  history[0].wz = 0.0;
+  history_init = history;
+
+  models::OptimizerSettings settings;
+  settings.shift_control_sequence = false;  // so result stores 0th value in history
+
+  savitskyGolayFilter(noisey_sequence, history, settings);
+
+  // Check history is propagated backward
+  EXPECT_NEAR(history_init[3].vx, history[2].vx, 0.02);
+  EXPECT_NEAR(history_init[3].vy, history[2].vy, 0.02);
+  EXPECT_NEAR(history_init[3].wz, history[2].wz, 0.02);
+
+  // Check history element is updated for first command
+  EXPECT_NEAR(history[3].vx, 0.2, 0.05);
+  EXPECT_NEAR(history[3].vy, 0.0, 0.035);
+  EXPECT_NEAR(history[3].wz, 0.23, 0.02);
+
+  // Check that path is smoother
+  float smoothed_val{0}, original_val{0};
+  for (unsigned int i = 1; i != noisey_sequence.vx.size() - 1; i++) {
+    smoothed_val += fabs(noisey_sequence.vx(i) - 0.2);
+    smoothed_val += fabs(noisey_sequence.vy(i) - 0.0);
+    smoothed_val += fabs(noisey_sequence.wz(i) - 0.3);
+
+    original_val += fabs(sequence_init.vx(i) - 0.2);
+    original_val += fabs(sequence_init.vy(i) - 0.0);
+    original_val += fabs(sequence_init.wz(i) - 0.3);
+  }
+
+  EXPECT_LT(smoothed_val, original_val);
+}
+
+TEST(UtilsTests, FindPathInversionTest)
+{
+  // Straight path, no inversions to be found
+  nav_msgs::msg::Path path;
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  EXPECT_EQ(utils::findFirstPathInversion(path), 10u);
+
+  // To short to process
+  path.poses.erase(path.poses.begin(), path.poses.begin() + 7);
+  EXPECT_EQ(utils::findFirstPathInversion(path), 3u);
+
+  // Has inversion at index 10, so should return 11 for the first point afterwards
+  // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1
+  path.poses.clear();
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = 10 - i;
+    path.poses.push_back(pose);
+  }
+  EXPECT_EQ(utils::findFirstPathInversion(path), 11u);
+}
+
+TEST(UtilsTests, RemovePosesAfterPathInversionTest)
+{
+  nav_msgs::msg::Path path;
+  // straight path
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u);
+
+  // try empty path
+  path.poses.clear();
+  EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u);
+
+  // cusping path
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = i;
+    path.poses.push_back(pose);
+  }
+  for (unsigned int i = 0; i != 10; i++) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = 10 - i;
+    path.poses.push_back(pose);
+  }
+  EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u);
+  // Check to see if removed
+  EXPECT_EQ(path.poses.size(), 11u);
+  EXPECT_EQ(path.poses.back().pose.position.x, 10);
+}
+
+TEST(UtilsTests, ShiftColumnsByOnePlaceTest)
+{
+  // Try with scalar value
+  Eigen::ArrayXf scalar_val(1);
+  scalar_val(0) = 5;
+  utils::shiftColumnsByOnePlace(scalar_val, 1);
+  EXPECT_EQ(scalar_val.size(), 1);
+  EXPECT_EQ(scalar_val(0), 5);
+
+  // Try with one dimensional array, shift right
+  Eigen::ArrayXf array_1d(4);
+  array_1d << 1, 2, 3, 4;
+  utils::shiftColumnsByOnePlace(array_1d, 1);
+  EXPECT_EQ(array_1d.size(), 4);
+  EXPECT_EQ(array_1d(0), 1);
+  EXPECT_EQ(array_1d(1), 1);
+  EXPECT_EQ(array_1d(2), 2);
+  EXPECT_EQ(array_1d(3), 3);
+
+  // Try with one dimensional array, shift left
+  array_1d(1) = 5;
+  utils::shiftColumnsByOnePlace(array_1d, -1);
+  EXPECT_EQ(array_1d.size(), 4);
+  EXPECT_EQ(array_1d(0), 5);
+  EXPECT_EQ(array_1d(1), 2);
+  EXPECT_EQ(array_1d(2), 3);
+  EXPECT_EQ(array_1d(2), 3);
+
+  // Try with two dimensional array, shift right
+  // 1 2 3 4        1 1 2 3
+  // 5 6 7 8    ->  5 5 6 7
+  // 9 10 11 12     9 9 10 11
+  Eigen::ArrayXXf array_2d(3, 4);
+  array_2d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;
+  utils::shiftColumnsByOnePlace(array_2d, 1);
+  EXPECT_EQ(array_2d.rows(), 3);
+  EXPECT_EQ(array_2d.cols(), 4);
+  EXPECT_EQ(array_2d(0, 0), 1);
+  EXPECT_EQ(array_2d(1, 0), 5);
+  EXPECT_EQ(array_2d(2, 0), 9);
+  EXPECT_EQ(array_2d(0, 1), 1);
+  EXPECT_EQ(array_2d(1, 1), 5);
+  EXPECT_EQ(array_2d(2, 1), 9);
+  EXPECT_EQ(array_2d(0, 2), 2);
+  EXPECT_EQ(array_2d(1, 2), 6);
+  EXPECT_EQ(array_2d(2, 2), 10);
+  EXPECT_EQ(array_2d(0, 3), 3);
+  EXPECT_EQ(array_2d(1, 3), 7);
+  EXPECT_EQ(array_2d(2, 3), 11);
+
+  array_2d.col(0).setZero();
+
+  // Try with two dimensional array, shift left
+  // 0 1 2 3      1 2 3 3
+  // 0 5 6 7   -> 5 6 7 7
+  // 0 9 10 11    9 10 11 11
+  utils::shiftColumnsByOnePlace(array_2d, -1);
+  EXPECT_EQ(array_2d.rows(), 3);
+  EXPECT_EQ(array_2d.cols(), 4);
+  EXPECT_EQ(array_2d(0, 0), 1);
+  EXPECT_EQ(array_2d(1, 0), 5);
+  EXPECT_EQ(array_2d(2, 0), 9);
+  EXPECT_EQ(array_2d(0, 1), 2);
+  EXPECT_EQ(array_2d(1, 1), 6);
+  EXPECT_EQ(array_2d(2, 1), 10);
+  EXPECT_EQ(array_2d(0, 2), 3);
+  EXPECT_EQ(array_2d(1, 2), 7);
+  EXPECT_EQ(array_2d(2, 2), 11);
+  EXPECT_EQ(array_2d(0, 3), 3);
+  EXPECT_EQ(array_2d(1, 3), 7);
+  EXPECT_EQ(array_2d(2, 3), 11);
+
+  // Try with invalid direction value.
+  EXPECT_THROW(utils::shiftColumnsByOnePlace(array_2d, -2), std::logic_error);
+}
+
+TEST(UtilsTests, NormalizeYawsBetweenPointsTest)
+{
+  Eigen::ArrayXf last_yaws(10);
+  last_yaws.setZero();
+
+  Eigen::ArrayXf yaw_between_points(10);
+  yaw_between_points.setZero(10);
+
+  // Try with both angles 0
+  Eigen::ArrayXf yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+    yaw_between_points);
+  EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));
+
+  // Try with yaw between points as pi/4
+  yaw_between_points.setConstant(M_PIF_2 / 2);
+  yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+    yaw_between_points);
+  EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));
+
+  // Try with yaw between points as pi/2
+  yaw_between_points.setConstant(M_PIF_2);
+  yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+    yaw_between_points);
+  EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));
+
+  // Try with a few yaw between points  more than pi/2
+  yaw_between_points[1] = 1.2 * M_PIF_2;
+  yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
+    yaw_between_points);
+  EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3);
+  EXPECT_NEAR(yaws_between_points_corrected[0], yaw_between_points[0], 1e-3);
+  EXPECT_NEAR(yaws_between_points_corrected[9], yaw_between_points[9], 1e-3);
+
+  // Try with goal angle 0
+  float goal_angle = 0;
+  yaws_between_points_corrected = utils::normalize_yaws_between_points(goal_angle,
+    yaw_between_points);
+  EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3);
+}
+
+TEST(UtilsTests, toTrajectoryMsgTest)
+{
+  Eigen::ArrayXXf trajectory(5, 3);
+  trajectory <<
+    0.0, 0.0, 0.0,
+    1.0, 1.0, 1.0,
+    2.0, 2.0, 2.0,
+    3.0, 3.0, 3.0,
+    4.0, 4.0, 4.0;
+
+  models::ControlSequence control_sequence;
+  control_sequence.vx = Eigen::ArrayXf::Ones(5);
+  control_sequence.wz = Eigen::ArrayXf::Ones(5);
+  control_sequence.vy = Eigen::ArrayXf::Zero(5);
+
+  std_msgs::msg::Header header;
+  header.frame_id = "map";
+  header.stamp = rclcpp::Time(100, 0, RCL_ROS_TIME);
+
+  auto trajectory_msg = utils::toTrajectoryMsg(
+    trajectory, control_sequence, 1.0, header);
+
+  EXPECT_EQ(trajectory_msg->header.frame_id, "map");
+  EXPECT_EQ(trajectory_msg->header.stamp, header.stamp);
+  EXPECT_EQ(trajectory_msg->points.size(), 5u);
+  EXPECT_EQ(trajectory_msg->points[0].pose.position.x, 0.0);
+  EXPECT_EQ(trajectory_msg->points[0].pose.position.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[1].pose.position.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].pose.position.y, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].pose.position.x, 2.0);
+  EXPECT_EQ(trajectory_msg->points[2].pose.position.y, 2.0);
+  EXPECT_EQ(trajectory_msg->points[3].pose.position.x, 3.0);
+  EXPECT_EQ(trajectory_msg->points[3].pose.position.y, 3.0);
+  EXPECT_EQ(trajectory_msg->points[4].pose.position.x, 4.0);
+  EXPECT_EQ(trajectory_msg->points[4].pose.position.y, 4.0);
+
+  EXPECT_EQ(trajectory_msg->points[0].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[0].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[0].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.angular.z, 1.0);
+
+  EXPECT_EQ(trajectory_msg->points[0].time_from_start, rclcpp::Duration(0, 0));
+  EXPECT_EQ(trajectory_msg->points[1].time_from_start, rclcpp::Duration(1, 0));
+  EXPECT_EQ(trajectory_msg->points[2].time_from_start, rclcpp::Duration(2, 0));
+  EXPECT_EQ(trajectory_msg->points[3].time_from_start, rclcpp::Duration(3, 0));
+  EXPECT_EQ(trajectory_msg->points[4].time_from_start, rclcpp::Duration(4, 0));
+}
+
+TEST(UtilsTests, getLastPathPoseTest)
+{
+  nav_msgs::msg::Path path;
+  path.poses.resize(10);
+  path.poses[9].pose.position.x = 5.0;
+  path.poses[9].pose.position.y = 50.0;
+  path.poses[9].pose.orientation.x = 0.0;
+  path.poses[9].pose.orientation.y = 0.0;
+  path.poses[9].pose.orientation.z = 1.0;
+  path.poses[9].pose.orientation.w = 0.0;
+
+  models::Path path_t = toTensor(path);
+  geometry_msgs::msg::Pose last_path_pose = utils::getLastPathPose(path_t);
+
+  EXPECT_EQ(last_path_pose.position.x, 5);
+  EXPECT_EQ(last_path_pose.position.y, 50);
+  EXPECT_NEAR(last_path_pose.orientation.x, 0.0, 1e-3);
+  EXPECT_NEAR(last_path_pose.orientation.y, 0.0, 1e-3);
+  EXPECT_NEAR(last_path_pose.orientation.z, 1.0, 1e-3);
+  EXPECT_NEAR(last_path_pose.orientation.w, 0.0, 1e-3);
+}
+
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  rclcpp::init(0, nullptr);
+
+  int result = RUN_ALL_TESTS();
+
+  rclcpp::shutdown();
+
+  return result;
+}
diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp
index 70733e37db4..792ae839b22 100644
--- a/nav2_mppi_controller/test/utils_test.cpp
+++ b/nav2_mppi_controller/test/utils_test.cpp
@@ -181,7 +181,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint)
   float model_dt = 0.1;
 
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr,
+    nullptr,
     std::nullopt, std::nullopt};  /// Caution, keep references
 
   // Attempt to set furthest point if notionally set, should not change
@@ -191,7 +192,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint)
 
   // Attempt to set if not set already with no other information, should fail
   CriticData data2 =
-  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr,
+    nullptr,
     std::nullopt, std::nullopt};  /// Caution, keep references
   setPathFurthestPointIfNotSet(data2);
   EXPECT_EQ(data2.furthest_reached_path_point, 0);
@@ -210,7 +212,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint)
   path = toTensor(plan);
 
   CriticData data3 =
-  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr,
+    nullptr,
     std::nullopt, std::nullopt};  /// Caution, keep references
   EXPECT_EQ(findPathFurthestReachedPoint(data3), 5);
 }
@@ -226,7 +229,8 @@ TEST(UtilsTests, findPathCosts)
   float model_dt = 0.1;
 
   CriticData data =
-  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr,
+    nullptr,
     std::nullopt, std::nullopt};  /// Caution, keep references
 
   // Test not set if already set, should not change
@@ -239,7 +243,8 @@ TEST(UtilsTests, findPathCosts)
   EXPECT_EQ(data.path_pts_valid->size(), 10u);
 
   CriticData data3 =
-  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
+  {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, std::nullopt, nullptr,
+    nullptr,
     std::nullopt, std::nullopt};  /// Caution, keep references
 
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
@@ -611,4 +616,4 @@ int main(int argc, char **argv)
   rclcpp::shutdown();
 
   return result;
-}
+}
\ No newline at end of file
diff --git a/nav2_mppi_controller/trajectory_validators.xml b/nav2_mppi_controller/trajectory_validators.xml
index 5a4da246123..fb02d0dac9a 100644
--- a/nav2_mppi_controller/trajectory_validators.xml
+++ b/nav2_mppi_controller/trajectory_validators.xml
@@ -3,5 +3,12 @@
     <class name="mppi::DefaultOptimalTrajectoryValidator" type="mppi::OptimalTrajectoryValidator" base_class_type="mppi::OptimalTrajectoryValidator">
       <description>Default trajectory validator for MPPI controller</description>
     </class>
+
+    <class name="mppi::AutoFixingTrajectoryValidator"
+           type="mppi::AutoFixingTrajectoryValidator"
+           base_class_type="mppi::OptimalTrajectoryValidator">
+      <description>Trajectory validator with automatic collision repair before rejection.</description>
+    </class>
+
   </library>
 </class_libraries>
diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
index ea93fc61711..f25c7774293 100644
--- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
+++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
@@ -41,7 +41,7 @@
 from rclpy.node import Node
 from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 from rclpy.task import Future
-from rclpy.type_support import GetResultServiceResponse
+#from rclpy.type_support import GetResultServiceResponse
 
 
 # Task Result enum for the result of the task being executed
@@ -291,7 +291,7 @@ def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> Optional[Runn
         goal_msg.poses = poses
         goal_msg.behavior_tree = behavior_tree
 
-        self.info(f'Navigating with {len(goal_msg.poses)} goals....')
+        self.info(f'Navigating with {len(goal_msg.poses.goals)} goals....')
         send_goal_future = self.nav_through_poses_client.send_goal_async(
             goal_msg, self._feedbackCallback
         )
diff --git a/nav2_smac_planner/lattice_primitives/config.json b/nav2_smac_planner/lattice_primitives/config.json
index 12a8470bdb3..531bbdccad1 100644
--- a/nav2_smac_planner/lattice_primitives/config.json
+++ b/nav2_smac_planner/lattice_primitives/config.json
@@ -1,6 +1,6 @@
 {
-    "motion_model": "ackermann",
-    "turning_radius": 0.5,
+    "motion_model": "diff",
+    "turning_radius": 0.00,
     "grid_resolution": 0.05,
     "stopping_threshold": 5,
     "num_of_headings": 16
diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py
index bf9e64559ab..7e278a9313b 100644
--- a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py
+++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py
@@ -21,9 +21,9 @@
 from typing import Any, cast, Dict, List, TypedDict
 
 import matplotlib.pyplot as plt
-from nav2_smac_planner.lattice_primitives import constants
-from nav2_smac_planner.lattice_primitives.lattice_generator import ConfigDict, LatticeGenerator
-from nav2_smac_planner.lattice_primitives.trajectory import Trajectory
+from lattice_primitives import constants
+from lattice_primitives.lattice_generator import ConfigDict, LatticeGenerator
+from lattice_primitives.trajectory import Trajectory
 import numpy as np
 
 logging.basicConfig(level=logging.INFO)
diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py
index 7aacd36e156..1d909366f95 100644
--- a/nav2_smac_planner/lattice_primitives/lattice_generator.py
+++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py
@@ -16,10 +16,10 @@
 from enum import Enum
 from typing import Any, Dict, List, Tuple, TypedDict
 
-from nav2_smac_planner.lattice_primitives.helper import angle_difference, interpolate_yaws
-from nav2_smac_planner.lattice_primitives.trajectory import (AnyFloat, FloatNDArray, Path,
+from lattice_primitives.helper import angle_difference, interpolate_yaws
+from lattice_primitives.trajectory import (AnyFloat, FloatNDArray, Path,
                                                              Trajectory, TrajectoryParameters)
-from nav2_smac_planner.lattice_primitives.trajectory_generator import TrajectoryGenerator
+from lattice_primitives.trajectory_generator import TrajectoryGenerator
 import numpy as np
 from rtree import index
 
diff --git a/nav2_smac_planner/lattice_primitives/output.json b/nav2_smac_planner/lattice_primitives/output.json
new file mode 100644
index 00000000000..b5766a0c2df
--- /dev/null
+++ b/nav2_smac_planner/lattice_primitives/output.json
@@ -0,0 +1,3864 @@
+{
+	"version": 1.0,
+	"date_generated": "2025-12-03",
+	"lattice_metadata": {
+		"motion_model": "diff",
+		"turning_radius": 0.08,
+		"grid_resolution": 0.05,
+		"stopping_threshold": 5,
+		"num_of_headings": 16,
+		"heading_angles": [
+			0.0,
+			0.4636476090008061,
+			0.7853981633974483,
+			1.1071487177940904,
+			1.5707963267948966,
+			2.0344439357957027,
+			2.356194490192345,
+			2.677945044588987,
+			3.141592653589793,
+			3.6052402625905993,
+			3.9269908169872414,
+			4.2487413713838835,
+			4.71238898038469,
+			5.176036589385496,
+			5.497787143782138,
+			5.81953769817878
+		],
+		"number_of_trajectories": 136
+	},
+	"primitives": [
+		{
+			"trajectory_id": 0,
+			"start_angle_index": 0,
+			"end_angle_index": 13,
+			"left_turn": false,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					0.04725,
+					-0.01523,
+					5.659486377865489
+				],
+				[
+					0.07743,
+					-0.05487,
+					5.176036589385496
+				],
+				[
+					0.1,
+					-0.1,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 1,
+			"start_angle_index": 0,
+			"end_angle_index": 14,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.05558,
+					-0.01356,
+					5.80469930592716
+				],
+				[
+					0.1,
+					-0.05,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 2,
+			"start_angle_index": 0,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.0
+				],
+				[
+					0.0,
+					0.0,
+					6.128636104179318
+				],
+				[
+					0.0,
+					0.0,
+					5.9740869011790485
+				],
+				[
+					0.0,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 3,
+			"start_angle_index": 0,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.05,
+			"arc_length": 0.0,
+			"straight_length": 0.05,
+			"poses": [
+				[
+					0.05,
+					0.0,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 4,
+			"start_angle_index": 0,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.0
+				],
+				[
+					0.0,
+					0.0,
+					0.1545492030002687
+				],
+				[
+					0.0,
+					0.0,
+					0.3090984060005374
+				],
+				[
+					0.0,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 5,
+			"start_angle_index": 0,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.05558,
+					0.01356,
+					0.47848600125242624
+				],
+				[
+					0.1,
+					0.05,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 6,
+			"start_angle_index": 0,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					0.04725,
+					0.01523,
+					0.6236989293140972
+				],
+				[
+					0.07743,
+					0.05487,
+					1.1071487177940904
+				],
+				[
+					0.1,
+					0.1,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 7,
+			"start_angle_index": 1,
+			"end_angle_index": 14,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.04916,
+					0.02356,
+					0.32653252396015897
+				],
+				[
+					0.10294,
+					0.02617,
+					6.053752487460941
+				],
+				[
+					0.15,
+					0.0,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 8,
+			"start_angle_index": 1,
+			"end_angle_index": 15,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					0.0118,
+					0.0
+				],
+				[
+					0.1,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 9,
+			"start_angle_index": 1,
+			"end_angle_index": 15,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04862,
+					0.01763,
+					0.23182380450040305
+				],
+				[
+					0.1,
+					0.02361,
+					0.0
+				],
+				[
+					0.15138,
+					0.01763,
+					6.051361502679184
+				],
+				[
+					0.2,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 10,
+			"start_angle_index": 1,
+			"end_angle_index": 15,
+			"left_turn": false,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04797,
+					0.01951,
+					0.3090984060005374
+				],
+				[
+					0.09837,
+					0.03141,
+					0.1545492030002687
+				],
+				[
+					0.15,
+					0.03541,
+					0.0
+				],
+				[
+					0.20163,
+					0.03141,
+					6.128636104179318
+				],
+				[
+					0.25203,
+					0.01951,
+					5.9740869011790485
+				],
+				[
+					0.3,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 11,
+			"start_angle_index": 1,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.4636476090008061
+				],
+				[
+					0.0,
+					0.0,
+					0.23182380450040305
+				],
+				[
+					0.0,
+					0.0,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 12,
+			"start_angle_index": 1,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					0.05,
+					0.025,
+					0.4636476090008061
+				],
+				[
+					0.1,
+					0.05,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 13,
+			"start_angle_index": 1,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.4636476090008061
+				],
+				[
+					0.0,
+					0.0,
+					0.6245228861991272
+				],
+				[
+					0.0,
+					0.0,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 14,
+			"start_angle_index": 1,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					0.05,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 15,
+			"start_angle_index": 1,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04028,
+					0.02587,
+					0.6781479785985676
+				],
+				[
+					0.07413,
+					0.05972,
+					0.892648348196329
+				],
+				[
+					0.1,
+					0.1,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 16,
+			"start_angle_index": 1,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.3618,
+			"trajectory_length": 0.42696,
+			"arc_length": 0.40057,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					0.04092,
+					0.02394,
+					0.5947686722333784
+				],
+				[
+					0.07836,
+					0.05302,
+					0.7258897354659507
+				],
+				[
+					0.11168,
+					0.08674,
+					0.8570107986985229
+				],
+				[
+					0.1403,
+					0.12453,
+					0.9881318619310953
+				],
+				[
+					0.16374,
+					0.16574,
+					1.1192529251636676
+				],
+				[
+					0.18159,
+					0.20965,
+					1.25037398839624
+				],
+				[
+					0.19354,
+					0.25553,
+					1.3814950516288125
+				],
+				[
+					0.19939,
+					0.30257,
+					1.512616114861385
+				],
+				[
+					0.2,
+					0.35,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 17,
+			"start_angle_index": 2,
+			"end_angle_index": 15,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.04706,
+					0.02617,
+					0.2294328197186447
+				],
+				[
+					0.10084,
+					0.02356,
+					5.956652783219427
+				],
+				[
+					0.15,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 18,
+			"start_angle_index": 2,
+			"end_angle_index": 0,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.04442,
+					0.03644,
+					0.4784860012524262
+				],
+				[
+					0.1,
+					0.05,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 19,
+			"start_angle_index": 2,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.7853981633974483
+				],
+				[
+					0.0,
+					0.0,
+					0.6245228861991272
+				],
+				[
+					0.0,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 20,
+			"start_angle_index": 2,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.07071,
+			"arc_length": 0.0,
+			"straight_length": 0.07071,
+			"poses": [
+				[
+					0.05,
+					0.05,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 21,
+			"start_angle_index": 2,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.7853981633974483
+				],
+				[
+					0.0,
+					0.0,
+					0.9462734405957693
+				],
+				[
+					0.0,
+					0.0,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 22,
+			"start_angle_index": 2,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.03644,
+					0.04442,
+					1.0923103255424704
+				],
+				[
+					0.05,
+					0.1,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 23,
+			"start_angle_index": 2,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.02617,
+					0.04706,
+					1.341363507076252
+				],
+				[
+					0.02356,
+					0.10084,
+					1.8973288507550554
+				],
+				[
+					0.0,
+					0.15,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 24,
+			"start_angle_index": 3,
+			"end_angle_index": 0,
+			"left_turn": false,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					0.02393,
+					0.04093,
+					0.9760278785613808
+				],
+				[
+					0.05301,
+					0.07837,
+					0.8449070393286713
+				],
+				[
+					0.08673,
+					0.11169,
+					0.7137862000959616
+				],
+				[
+					0.12452,
+					0.14031,
+					0.582665360863252
+				],
+				[
+					0.16573,
+					0.16375,
+					0.4515445216305424
+				],
+				[
+					0.20965,
+					0.18159,
+					0.3204236823978327
+				],
+				[
+					0.25553,
+					0.19355,
+					0.18930284316512325
+				],
+				[
+					0.30257,
+					0.1994,
+					0.05818200393241346
+				],
+				[
+					0.35,
+					0.2,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 25,
+			"start_angle_index": 3,
+			"end_angle_index": 1,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					0.05,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 26,
+			"start_angle_index": 3,
+			"end_angle_index": 1,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.02587,
+					0.04028,
+					0.892648348196329
+				],
+				[
+					0.05972,
+					0.07413,
+					0.6781479785985676
+				],
+				[
+					0.1,
+					0.1,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 27,
+			"start_angle_index": 3,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					1.1071487177940904
+				],
+				[
+					0.0,
+					0.0,
+					0.9998985329952097
+				],
+				[
+					0.0,
+					0.0,
+					0.892648348196329
+				],
+				[
+					0.0,
+					0.0,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 28,
+			"start_angle_index": 3,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					0.025,
+					0.05,
+					1.1071487177940904
+				],
+				[
+					0.05,
+					0.1,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 29,
+			"start_angle_index": 3,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					1.1071487177940904
+				],
+				[
+					0.0,
+					0.0,
+					1.261697920794359
+				],
+				[
+					0.0,
+					0.0,
+					1.416247123794628
+				],
+				[
+					0.0,
+					0.0,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 30,
+			"start_angle_index": 3,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0118,
+					0.05,
+					1.5707963267948966
+				],
+				[
+					0.0,
+					0.1,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 31,
+			"start_angle_index": 3,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.01763,
+					0.04862,
+					1.3389725222944935
+				],
+				[
+					0.02361,
+					0.1,
+					1.5707963267948966
+				],
+				[
+					0.01763,
+					0.15138,
+					1.8026201312952996
+				],
+				[
+					0.0,
+					0.2,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 32,
+			"start_angle_index": 3,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.01951,
+					0.04797,
+					1.261697920794359
+				],
+				[
+					0.03141,
+					0.09837,
+					1.416247123794628
+				],
+				[
+					0.03541,
+					0.15,
+					1.5707963267948966
+				],
+				[
+					0.03141,
+					0.20163,
+					1.7253455297951654
+				],
+				[
+					0.01951,
+					0.25203,
+					1.879894732795434
+				],
+				[
+					0.0,
+					0.3,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 33,
+			"start_angle_index": 3,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.02356,
+					0.04916,
+					1.2442638028347375
+				],
+				[
+					0.02617,
+					0.10294,
+					1.8002291465135412
+				],
+				[
+					0.0,
+					0.15,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 34,
+			"start_angle_index": 4,
+			"end_angle_index": 1,
+			"left_turn": false,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					0.01523,
+					0.04725,
+					0.9470973974807994
+				],
+				[
+					0.05487,
+					0.07743,
+					0.4636476090008061
+				],
+				[
+					0.1,
+					0.1,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 35,
+			"start_angle_index": 4,
+			"end_angle_index": 2,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.01356,
+					0.05558,
+					1.0923103255424704
+				],
+				[
+					0.05,
+					0.1,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 36,
+			"start_angle_index": 4,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					1.5707963267948966
+				],
+				[
+					0.0,
+					0.0,
+					1.416247123794628
+				],
+				[
+					0.0,
+					0.0,
+					1.261697920794359
+				],
+				[
+					0.0,
+					0.0,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 37,
+			"start_angle_index": 4,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.05,
+			"arc_length": 0.0,
+			"straight_length": 0.05,
+			"poses": [
+				[
+					0.0,
+					0.05,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 38,
+			"start_angle_index": 4,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					1.5707963267948966
+				],
+				[
+					0.0,
+					0.0,
+					1.7253455297951652
+				],
+				[
+					0.0,
+					0.0,
+					1.879894732795434
+				],
+				[
+					0.0,
+					0.0,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 39,
+			"start_angle_index": 4,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.01356,
+					0.05558,
+					2.049282328047323
+				],
+				[
+					-0.05,
+					0.1,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 40,
+			"start_angle_index": 4,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					-0.01523,
+					0.04725,
+					2.194495256108994
+				],
+				[
+					-0.05487,
+					0.07743,
+					2.677945044588987
+				],
+				[
+					-0.1,
+					0.1,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 41,
+			"start_angle_index": 5,
+			"end_angle_index": 2,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.02356,
+					0.04916,
+					1.8973288507550556
+				],
+				[
+					-0.02617,
+					0.10294,
+					1.341363507076252
+				],
+				[
+					0.0,
+					0.15,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 42,
+			"start_angle_index": 5,
+			"end_angle_index": 3,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.0118,
+					0.05,
+					1.5707963267948966
+				],
+				[
+					0.0,
+					0.1,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 43,
+			"start_angle_index": 5,
+			"end_angle_index": 3,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.01763,
+					0.04862,
+					1.8026201312952996
+				],
+				[
+					-0.02361,
+					0.1,
+					1.5707963267948966
+				],
+				[
+					-0.01763,
+					0.15138,
+					1.3389725222944935
+				],
+				[
+					0.0,
+					0.2,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 44,
+			"start_angle_index": 5,
+			"end_angle_index": 3,
+			"left_turn": false,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.01951,
+					0.04797,
+					1.879894732795434
+				],
+				[
+					-0.03141,
+					0.09837,
+					1.7253455297951652
+				],
+				[
+					-0.03541,
+					0.15,
+					1.5707963267948966
+				],
+				[
+					-0.03141,
+					0.20163,
+					1.4162471237946277
+				],
+				[
+					-0.01951,
+					0.25203,
+					1.261697920794359
+				],
+				[
+					0.0,
+					0.3,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 45,
+			"start_angle_index": 5,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.0344439357957027
+				],
+				[
+					0.0,
+					0.0,
+					1.8026201312952996
+				],
+				[
+					0.0,
+					0.0,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 46,
+			"start_angle_index": 5,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					-0.025,
+					0.05,
+					2.0344439357957027
+				],
+				[
+					-0.05,
+					0.1,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 47,
+			"start_angle_index": 5,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.0344439357957027
+				],
+				[
+					0.0,
+					0.0,
+					2.1953192129940238
+				],
+				[
+					0.0,
+					0.0,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 48,
+			"start_angle_index": 5,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					0.05,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 49,
+			"start_angle_index": 5,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.02587,
+					0.04028,
+					2.248944305393464
+				],
+				[
+					-0.05972,
+					0.07413,
+					2.4634446749912255
+				],
+				[
+					-0.1,
+					0.1,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 50,
+			"start_angle_index": 5,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					-0.02393,
+					0.04093,
+					2.1655647750284124
+				],
+				[
+					-0.05301,
+					0.07837,
+					2.296685614261122
+				],
+				[
+					-0.08673,
+					0.11169,
+					2.4278064534938313
+				],
+				[
+					-0.12452,
+					0.14031,
+					2.558927292726541
+				],
+				[
+					-0.16573,
+					0.16375,
+					2.6900481319592506
+				],
+				[
+					-0.20965,
+					0.18159,
+					2.8211689711919603
+				],
+				[
+					-0.25553,
+					0.19355,
+					2.95228981042467
+				],
+				[
+					-0.30257,
+					0.1994,
+					3.0834106496573797
+				],
+				[
+					-0.35,
+					0.2,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 51,
+			"start_angle_index": 6,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.04706,
+					0.02617,
+					2.9121598338711485
+				],
+				[
+					-0.10084,
+					0.02356,
+					3.468125177549952
+				],
+				[
+					-0.15,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 52,
+			"start_angle_index": 6,
+			"end_angle_index": 3,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.02617,
+					0.04706,
+					1.8002291465135412
+				],
+				[
+					-0.02356,
+					0.10084,
+					1.2442638028347377
+				],
+				[
+					0.0,
+					0.15,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 53,
+			"start_angle_index": 6,
+			"end_angle_index": 4,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.03644,
+					0.04442,
+					2.049282328047323
+				],
+				[
+					-0.05,
+					0.1,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 54,
+			"start_angle_index": 6,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.356194490192345
+				],
+				[
+					0.0,
+					0.0,
+					2.1953192129940238
+				],
+				[
+					0.0,
+					0.0,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 55,
+			"start_angle_index": 6,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.07071,
+			"arc_length": 0.0,
+			"straight_length": 0.07071,
+			"poses": [
+				[
+					-0.05,
+					0.05,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 56,
+			"start_angle_index": 6,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.356194490192345
+				],
+				[
+					0.0,
+					0.0,
+					2.517069767390666
+				],
+				[
+					0.0,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 57,
+			"start_angle_index": 6,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.04442,
+					0.03644,
+					2.6631066523373668
+				],
+				[
+					-0.1,
+					0.05,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 58,
+			"start_angle_index": 7,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					0.0118,
+					3.141592653589793
+				],
+				[
+					-0.1,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 59,
+			"start_angle_index": 7,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04862,
+					0.01763,
+					2.90976884908939
+				],
+				[
+					-0.1,
+					0.02361,
+					3.141592653589793
+				],
+				[
+					-0.15138,
+					0.01763,
+					3.373416458090196
+				],
+				[
+					-0.2,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 60,
+			"start_angle_index": 7,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04797,
+					0.01951,
+					2.832494247589256
+				],
+				[
+					-0.09837,
+					0.03141,
+					2.9870434505895243
+				],
+				[
+					-0.15,
+					0.03541,
+					3.141592653589793
+				],
+				[
+					-0.20163,
+					0.03141,
+					3.296141856590062
+				],
+				[
+					-0.25203,
+					0.01951,
+					3.450691059590331
+				],
+				[
+					-0.3,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 61,
+			"start_angle_index": 7,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.04916,
+					0.02356,
+					2.815060129629634
+				],
+				[
+					-0.10294,
+					0.02617,
+					3.3710254733084377
+				],
+				[
+					-0.15,
+					0.0,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 62,
+			"start_angle_index": 7,
+			"end_angle_index": 4,
+			"left_turn": false,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					-0.04093,
+					0.02393,
+					2.5468242053562773
+				],
+				[
+					-0.07837,
+					0.05301,
+					2.4157033661235676
+				],
+				[
+					-0.11169,
+					0.08673,
+					2.2845825268908584
+				],
+				[
+					-0.14031,
+					0.12452,
+					2.1534616876581487
+				],
+				[
+					-0.16375,
+					0.16573,
+					2.022340848425439
+				],
+				[
+					-0.18159,
+					0.20965,
+					1.8912200091927294
+				],
+				[
+					-0.19355,
+					0.25553,
+					1.7600991699600197
+				],
+				[
+					-0.1994,
+					0.30257,
+					1.62897833072731
+				],
+				[
+					-0.2,
+					0.35,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 63,
+			"start_angle_index": 7,
+			"end_angle_index": 5,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					0.05,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 64,
+			"start_angle_index": 7,
+			"end_angle_index": 5,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04028,
+					0.02587,
+					2.4634446749912255
+				],
+				[
+					-0.07413,
+					0.05972,
+					2.248944305393464
+				],
+				[
+					-0.1,
+					0.1,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 65,
+			"start_angle_index": 7,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.677945044588987
+				],
+				[
+					0.0,
+					0.0,
+					2.5706948597901063
+				],
+				[
+					0.0,
+					0.0,
+					2.4634446749912255
+				],
+				[
+					0.0,
+					0.0,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 66,
+			"start_angle_index": 7,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					-0.05,
+					0.025,
+					2.677945044588987
+				],
+				[
+					-0.1,
+					0.05,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 67,
+			"start_angle_index": 7,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.677945044588987
+				],
+				[
+					0.0,
+					0.0,
+					2.832494247589256
+				],
+				[
+					0.0,
+					0.0,
+					2.9870434505895243
+				],
+				[
+					0.0,
+					0.0,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 68,
+			"start_angle_index": 8,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.141592653589793
+				],
+				[
+					0.0,
+					0.0,
+					3.296141856590062
+				],
+				[
+					0.0,
+					0.0,
+					3.4506910595903304
+				],
+				[
+					0.0,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 69,
+			"start_angle_index": 8,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.05558,
+					-0.01356,
+					3.6200786548422195
+				],
+				[
+					-0.1,
+					-0.05,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 70,
+			"start_angle_index": 8,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					-0.04725,
+					-0.01523,
+					3.7652915829038904
+				],
+				[
+					-0.07743,
+					-0.05487,
+					4.2487413713838835
+				],
+				[
+					-0.1,
+					-0.1,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 71,
+			"start_angle_index": 8,
+			"end_angle_index": 5,
+			"left_turn": false,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					-0.04725,
+					0.01523,
+					2.517893724275696
+				],
+				[
+					-0.07743,
+					0.05487,
+					2.0344439357957027
+				],
+				[
+					-0.1,
+					0.1,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 72,
+			"start_angle_index": 8,
+			"end_angle_index": 6,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.05558,
+					0.01356,
+					2.6631066523373668
+				],
+				[
+					-0.1,
+					0.05,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 73,
+			"start_angle_index": 8,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.141592653589793
+				],
+				[
+					0.0,
+					0.0,
+					2.9870434505895243
+				],
+				[
+					0.0,
+					0.0,
+					2.832494247589256
+				],
+				[
+					0.0,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 74,
+			"start_angle_index": 8,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.05,
+			"arc_length": 0.0,
+			"straight_length": 0.05,
+			"poses": [
+				[
+					-0.05,
+					0.0,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 75,
+			"start_angle_index": 9,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					-0.05,
+					-0.025,
+					3.6052402625905993
+				],
+				[
+					-0.1,
+					-0.05,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 76,
+			"start_angle_index": 9,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.6052402625905993
+				],
+				[
+					0.0,
+					0.0,
+					3.7661155397889203
+				],
+				[
+					0.0,
+					0.0,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 77,
+			"start_angle_index": 9,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					-0.05,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 78,
+			"start_angle_index": 9,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04028,
+					-0.02587,
+					3.8197406321883607
+				],
+				[
+					-0.07413,
+					-0.05972,
+					4.034241001786122
+				],
+				[
+					-0.1,
+					-0.1,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 79,
+			"start_angle_index": 9,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					-0.04093,
+					-0.02393,
+					3.736361101823309
+				],
+				[
+					-0.07837,
+					-0.05301,
+					3.8674819410560186
+				],
+				[
+					-0.11169,
+					-0.08673,
+					3.998602780288728
+				],
+				[
+					-0.14031,
+					-0.12452,
+					4.129723619521437
+				],
+				[
+					-0.16375,
+					-0.16573,
+					4.260844458754147
+				],
+				[
+					-0.18159,
+					-0.20965,
+					4.391965297986857
+				],
+				[
+					-0.19355,
+					-0.25553,
+					4.5230861372195665
+				],
+				[
+					-0.1994,
+					-0.30257,
+					4.654206976452276
+				],
+				[
+					-0.2,
+					-0.35,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 80,
+			"start_angle_index": 9,
+			"end_angle_index": 6,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.04916,
+					-0.02356,
+					3.468125177549952
+				],
+				[
+					-0.10294,
+					-0.02617,
+					2.9121598338711485
+				],
+				[
+					-0.15,
+					0.0,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 81,
+			"start_angle_index": 9,
+			"end_angle_index": 7,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					-0.0118,
+					3.141592653589793
+				],
+				[
+					-0.1,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 82,
+			"start_angle_index": 9,
+			"end_angle_index": 7,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04862,
+					-0.01763,
+					3.373416458090196
+				],
+				[
+					-0.1,
+					-0.02361,
+					3.141592653589793
+				],
+				[
+					-0.15138,
+					-0.01763,
+					2.90976884908939
+				],
+				[
+					-0.2,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 83,
+			"start_angle_index": 9,
+			"end_angle_index": 7,
+			"left_turn": false,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04797,
+					-0.01951,
+					3.4506910595903304
+				],
+				[
+					-0.09837,
+					-0.03141,
+					3.296141856590062
+				],
+				[
+					-0.15,
+					-0.03541,
+					3.141592653589793
+				],
+				[
+					-0.20163,
+					-0.03141,
+					2.9870434505895243
+				],
+				[
+					-0.25203,
+					-0.01951,
+					2.8324942475892554
+				],
+				[
+					-0.3,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 84,
+			"start_angle_index": 9,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.6052402625905993
+				],
+				[
+					0.0,
+					0.0,
+					3.373416458090196
+				],
+				[
+					0.0,
+					0.0,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 85,
+			"start_angle_index": 10,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.9269908169872414
+				],
+				[
+					0.0,
+					0.0,
+					3.7661155397889203
+				],
+				[
+					0.0,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 86,
+			"start_angle_index": 10,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.07071,
+			"arc_length": 0.0,
+			"straight_length": 0.07071,
+			"poses": [
+				[
+					-0.05,
+					-0.05,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 87,
+			"start_angle_index": 10,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.9269908169872414
+				],
+				[
+					0.0,
+					0.0,
+					4.0878660941855625
+				],
+				[
+					0.0,
+					0.0,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 88,
+			"start_angle_index": 10,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.03644,
+					-0.04442,
+					4.233902979132264
+				],
+				[
+					-0.05,
+					-0.1,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 89,
+			"start_angle_index": 10,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.02617,
+					-0.04706,
+					4.4829561606660455
+				],
+				[
+					-0.02356,
+					-0.10084,
+					5.038921504344849
+				],
+				[
+					0.0,
+					-0.15,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 90,
+			"start_angle_index": 10,
+			"end_angle_index": 7,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.04706,
+					-0.02617,
+					3.3710254733084377
+				],
+				[
+					-0.10084,
+					-0.02356,
+					2.815060129629634
+				],
+				[
+					-0.15,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 91,
+			"start_angle_index": 10,
+			"end_angle_index": 8,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.04442,
+					-0.03644,
+					3.6200786548422195
+				],
+				[
+					-0.1,
+					-0.05,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 92,
+			"start_angle_index": 11,
+			"end_angle_index": 9,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					-0.05,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 93,
+			"start_angle_index": 11,
+			"end_angle_index": 9,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.02587,
+					-0.04028,
+					4.034241001786122
+				],
+				[
+					-0.05972,
+					-0.07413,
+					3.8197406321883607
+				],
+				[
+					-0.1,
+					-0.1,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 94,
+			"start_angle_index": 11,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					4.2487413713838835
+				],
+				[
+					0.0,
+					0.0,
+					4.141491186585003
+				],
+				[
+					0.0,
+					0.0,
+					4.034241001786122
+				],
+				[
+					0.0,
+					0.0,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 95,
+			"start_angle_index": 11,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					-0.025,
+					-0.05,
+					4.2487413713838835
+				],
+				[
+					-0.05,
+					-0.1,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 96,
+			"start_angle_index": 11,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					4.2487413713838835
+				],
+				[
+					0.0,
+					0.0,
+					4.403290574384152
+				],
+				[
+					0.0,
+					0.0,
+					4.557839777384421
+				],
+				[
+					0.0,
+					0.0,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 97,
+			"start_angle_index": 11,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.0118,
+					-0.05,
+					4.71238898038469
+				],
+				[
+					0.0,
+					-0.1,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 98,
+			"start_angle_index": 11,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.01763,
+					-0.04862,
+					4.480565175884287
+				],
+				[
+					-0.02361,
+					-0.1,
+					4.71238898038469
+				],
+				[
+					-0.01763,
+					-0.15138,
+					4.944212784885092
+				],
+				[
+					0.0,
+					-0.2,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 99,
+			"start_angle_index": 11,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.01951,
+					-0.04797,
+					4.403290574384152
+				],
+				[
+					-0.03141,
+					-0.09837,
+					4.557839777384421
+				],
+				[
+					-0.03541,
+					-0.15,
+					4.71238898038469
+				],
+				[
+					-0.03141,
+					-0.20163,
+					4.866938183384958
+				],
+				[
+					-0.01951,
+					-0.25203,
+					5.021487386385227
+				],
+				[
+					0.0,
+					-0.3,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 100,
+			"start_angle_index": 11,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.02356,
+					-0.04916,
+					4.385856456424531
+				],
+				[
+					-0.02617,
+					-0.10294,
+					4.941821800103334
+				],
+				[
+					0.0,
+					-0.15,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 101,
+			"start_angle_index": 11,
+			"end_angle_index": 8,
+			"left_turn": false,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					-0.02393,
+					-0.04093,
+					4.117620532151173
+				],
+				[
+					-0.05301,
+					-0.07837,
+					3.986499692918464
+				],
+				[
+					-0.08673,
+					-0.11169,
+					3.855378853685755
+				],
+				[
+					-0.12452,
+					-0.14031,
+					3.7242580144530453
+				],
+				[
+					-0.16573,
+					-0.16375,
+					3.5931371752203356
+				],
+				[
+					-0.20965,
+					-0.18159,
+					3.462016335987626
+				],
+				[
+					-0.25553,
+					-0.19355,
+					3.3308954967549163
+				],
+				[
+					-0.30257,
+					-0.1994,
+					3.1997746575222066
+				],
+				[
+					-0.35,
+					-0.2,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 102,
+			"start_angle_index": 12,
+			"end_angle_index": 9,
+			"left_turn": false,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					-0.01523,
+					-0.04725,
+					4.088690051070593
+				],
+				[
+					-0.05487,
+					-0.07743,
+					3.6052402625905993
+				],
+				[
+					-0.1,
+					-0.1,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 103,
+			"start_angle_index": 12,
+			"end_angle_index": 10,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.01356,
+					-0.05558,
+					4.233902979132264
+				],
+				[
+					-0.05,
+					-0.1,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 104,
+			"start_angle_index": 12,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					4.71238898038469
+				],
+				[
+					0.0,
+					0.0,
+					4.557839777384421
+				],
+				[
+					0.0,
+					0.0,
+					4.403290574384152
+				],
+				[
+					0.0,
+					0.0,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 105,
+			"start_angle_index": 12,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.05,
+			"arc_length": 0.0,
+			"straight_length": 0.05,
+			"poses": [
+				[
+					0.0,
+					-0.05,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 106,
+			"start_angle_index": 12,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					4.71238898038469
+				],
+				[
+					0.0,
+					0.0,
+					4.866938183384958
+				],
+				[
+					0.0,
+					0.0,
+					5.021487386385227
+				],
+				[
+					0.0,
+					0.0,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 107,
+			"start_angle_index": 12,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.01356,
+					-0.05558,
+					5.190874981637116
+				],
+				[
+					0.05,
+					-0.1,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 108,
+			"start_angle_index": 12,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					0.01523,
+					-0.04725,
+					5.3360879096987865
+				],
+				[
+					0.05487,
+					-0.07743,
+					5.81953769817878
+				],
+				[
+					0.1,
+					-0.1,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 109,
+			"start_angle_index": 13,
+			"end_angle_index": 10,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.02356,
+					-0.04916,
+					5.038921504344849
+				],
+				[
+					0.02617,
+					-0.10294,
+					4.4829561606660455
+				],
+				[
+					0.0,
+					-0.15,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 110,
+			"start_angle_index": 13,
+			"end_angle_index": 11,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0118,
+					-0.05,
+					4.71238898038469
+				],
+				[
+					0.0,
+					-0.1,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 111,
+			"start_angle_index": 13,
+			"end_angle_index": 11,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.01763,
+					-0.04862,
+					4.944212784885092
+				],
+				[
+					0.02361,
+					-0.1,
+					4.71238898038469
+				],
+				[
+					0.01763,
+					-0.15138,
+					4.480565175884287
+				],
+				[
+					0.0,
+					-0.2,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 112,
+			"start_angle_index": 13,
+			"end_angle_index": 11,
+			"left_turn": false,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.01951,
+					-0.04797,
+					5.021487386385227
+				],
+				[
+					0.03141,
+					-0.09837,
+					4.866938183384958
+				],
+				[
+					0.03541,
+					-0.15,
+					4.71238898038469
+				],
+				[
+					0.03141,
+					-0.20163,
+					4.557839777384421
+				],
+				[
+					0.01951,
+					-0.25203,
+					4.403290574384152
+				],
+				[
+					0.0,
+					-0.3,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 113,
+			"start_angle_index": 13,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.176036589385496
+				],
+				[
+					0.0,
+					0.0,
+					4.944212784885092
+				],
+				[
+					0.0,
+					0.0,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 114,
+			"start_angle_index": 13,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					0.025,
+					-0.05,
+					5.176036589385496
+				],
+				[
+					0.05,
+					-0.1,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 115,
+			"start_angle_index": 13,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.176036589385496
+				],
+				[
+					0.0,
+					0.0,
+					5.336911866583817
+				],
+				[
+					0.0,
+					0.0,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 116,
+			"start_angle_index": 13,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					-0.05,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 117,
+			"start_angle_index": 13,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.02587,
+					-0.04028,
+					5.390536958983257
+				],
+				[
+					0.05972,
+					-0.07413,
+					5.605037328581019
+				],
+				[
+					0.1,
+					-0.1,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 118,
+			"start_angle_index": 13,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					0.02393,
+					-0.04093,
+					5.307157428618205
+				],
+				[
+					0.05301,
+					-0.07837,
+					5.438278267850915
+				],
+				[
+					0.08673,
+					-0.11169,
+					5.569399107083624
+				],
+				[
+					0.12452,
+					-0.14031,
+					5.7005199463163345
+				],
+				[
+					0.16573,
+					-0.16375,
+					5.831640785549044
+				],
+				[
+					0.20965,
+					-0.18159,
+					5.962761624781754
+				],
+				[
+					0.25553,
+					-0.19355,
+					6.093882464014463
+				],
+				[
+					0.30257,
+					-0.1994,
+					6.225003303247172
+				],
+				[
+					0.35,
+					-0.2,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 119,
+			"start_angle_index": 14,
+			"end_angle_index": 11,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.02617,
+					-0.04706,
+					4.941821800103334
+				],
+				[
+					0.02356,
+					-0.10084,
+					4.385856456424531
+				],
+				[
+					0.0,
+					-0.15,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 120,
+			"start_angle_index": 14,
+			"end_angle_index": 12,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.03644,
+					-0.04442,
+					5.190874981637116
+				],
+				[
+					0.05,
+					-0.1,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 121,
+			"start_angle_index": 14,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.497787143782138
+				],
+				[
+					0.0,
+					0.0,
+					5.336911866583817
+				],
+				[
+					0.0,
+					0.0,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 122,
+			"start_angle_index": 14,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.07071,
+			"arc_length": 0.0,
+			"straight_length": 0.07071,
+			"poses": [
+				[
+					0.05,
+					-0.05,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 123,
+			"start_angle_index": 14,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.497787143782138
+				],
+				[
+					0.0,
+					0.0,
+					5.658662420980459
+				],
+				[
+					0.0,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 124,
+			"start_angle_index": 14,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.04442,
+					-0.03644,
+					5.80469930592716
+				],
+				[
+					0.1,
+					-0.05,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 125,
+			"start_angle_index": 14,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.04706,
+					-0.02617,
+					6.053752487460941
+				],
+				[
+					0.10084,
+					-0.02356,
+					0.32653252396015886
+				],
+				[
+					0.15,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 126,
+			"start_angle_index": 15,
+			"end_angle_index": 12,
+			"left_turn": false,
+			"trajectory_radius": 0.3618,
+			"trajectory_length": 0.42696,
+			"arc_length": 0.40057,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					0.04092,
+					-0.02394,
+					5.688416634946208
+				],
+				[
+					0.07836,
+					-0.05302,
+					5.557295571713635
+				],
+				[
+					0.11168,
+					-0.08674,
+					5.426174508481063
+				],
+				[
+					0.1403,
+					-0.12453,
+					5.295053445248491
+				],
+				[
+					0.16374,
+					-0.16574,
+					5.163932382015918
+				],
+				[
+					0.18159,
+					-0.20965,
+					5.032811318783346
+				],
+				[
+					0.19354,
+					-0.25553,
+					4.901690255550774
+				],
+				[
+					0.19939,
+					-0.30257,
+					4.770569192318201
+				],
+				[
+					0.2,
+					-0.35,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 127,
+			"start_angle_index": 15,
+			"end_angle_index": 13,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					-0.05,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 128,
+			"start_angle_index": 15,
+			"end_angle_index": 13,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04028,
+					-0.02587,
+					5.605037328581019
+				],
+				[
+					0.07413,
+					-0.05972,
+					5.390536958983257
+				],
+				[
+					0.1,
+					-0.1,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 129,
+			"start_angle_index": 15,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.81953769817878
+				],
+				[
+					0.0,
+					0.0,
+					5.712287513379899
+				],
+				[
+					0.0,
+					0.0,
+					5.605037328581019
+				],
+				[
+					0.0,
+					0.0,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 130,
+			"start_angle_index": 15,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					0.05,
+					-0.025,
+					5.81953769817878
+				],
+				[
+					0.1,
+					-0.05,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 131,
+			"start_angle_index": 15,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.81953769817878
+				],
+				[
+					0.0,
+					0.0,
+					5.9740869011790485
+				],
+				[
+					0.0,
+					0.0,
+					6.128636104179318
+				],
+				[
+					0.0,
+					0.0,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 132,
+			"start_angle_index": 15,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					-0.0118,
+					0.0
+				],
+				[
+					0.1,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 133,
+			"start_angle_index": 15,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04862,
+					-0.01763,
+					6.051361502679184
+				],
+				[
+					0.1,
+					-0.02361,
+					0.0
+				],
+				[
+					0.15138,
+					-0.01763,
+					0.23182380450040302
+				],
+				[
+					0.2,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 134,
+			"start_angle_index": 15,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04797,
+					-0.01951,
+					5.9740869011790485
+				],
+				[
+					0.09837,
+					-0.03141,
+					6.128636104179318
+				],
+				[
+					0.15,
+					-0.03541,
+					0.0
+				],
+				[
+					0.20163,
+					-0.03141,
+					0.1545492030002687
+				],
+				[
+					0.25203,
+					-0.01951,
+					0.30909840600053734
+				],
+				[
+					0.3,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 135,
+			"start_angle_index": 15,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.04916,
+					-0.02356,
+					5.956652783219427
+				],
+				[
+					0.10294,
+					-0.02617,
+					0.22943281971864465
+				],
+				[
+					0.15,
+					0.0,
+					0.7853981633974483
+				]
+			]
+		}
+	]
+}
\ No newline at end of file
diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output1.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output1.json
new file mode 100644
index 00000000000..dfff481543d
--- /dev/null
+++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output1.json
@@ -0,0 +1,3864 @@
+{
+	"version": 1.0,
+	"date_generated": "2025-10-30",
+	"lattice_metadata": {
+		"motion_model": "diff",
+		"turning_radius": 0.08,
+		"grid_resolution": 0.05,
+		"stopping_threshold": 5,
+		"num_of_headings": 16,
+		"heading_angles": [
+			0.0,
+			0.4636476090008061,
+			0.7853981633974483,
+			1.1071487177940904,
+			1.5707963267948966,
+			2.0344439357957027,
+			2.356194490192345,
+			2.677945044588987,
+			3.141592653589793,
+			3.6052402625905993,
+			3.9269908169872414,
+			4.2487413713838835,
+			4.71238898038469,
+			5.176036589385496,
+			5.497787143782138,
+			5.81953769817878
+		],
+		"number_of_trajectories": 136
+	},
+	"primitives": [
+		{
+			"trajectory_id": 0,
+			"start_angle_index": 0,
+			"end_angle_index": 13,
+			"left_turn": false,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					0.04725,
+					-0.01523,
+					5.659486377865489
+				],
+				[
+					0.07743,
+					-0.05487,
+					5.176036589385496
+				],
+				[
+					0.1,
+					-0.1,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 1,
+			"start_angle_index": 0,
+			"end_angle_index": 14,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.05558,
+					-0.01356,
+					5.80469930592716
+				],
+				[
+					0.1,
+					-0.05,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 2,
+			"start_angle_index": 0,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.0
+				],
+				[
+					0.0,
+					0.0,
+					6.128636104179318
+				],
+				[
+					0.0,
+					0.0,
+					5.9740869011790485
+				],
+				[
+					0.0,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 3,
+			"start_angle_index": 0,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.05,
+			"arc_length": 0.0,
+			"straight_length": 0.05,
+			"poses": [
+				[
+					0.05,
+					0.0,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 4,
+			"start_angle_index": 0,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.0
+				],
+				[
+					0.0,
+					0.0,
+					0.1545492030002687
+				],
+				[
+					0.0,
+					0.0,
+					0.3090984060005374
+				],
+				[
+					0.0,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 5,
+			"start_angle_index": 0,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.05558,
+					0.01356,
+					0.47848600125242624
+				],
+				[
+					0.1,
+					0.05,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 6,
+			"start_angle_index": 0,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					0.04725,
+					0.01523,
+					0.6236989293140972
+				],
+				[
+					0.07743,
+					0.05487,
+					1.1071487177940904
+				],
+				[
+					0.1,
+					0.1,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 7,
+			"start_angle_index": 1,
+			"end_angle_index": 14,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.04916,
+					0.02356,
+					0.32653252396015897
+				],
+				[
+					0.10294,
+					0.02617,
+					6.053752487460941
+				],
+				[
+					0.15,
+					0.0,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 8,
+			"start_angle_index": 1,
+			"end_angle_index": 15,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					0.0118,
+					0.0
+				],
+				[
+					0.1,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 9,
+			"start_angle_index": 1,
+			"end_angle_index": 15,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04862,
+					0.01763,
+					0.23182380450040305
+				],
+				[
+					0.1,
+					0.02361,
+					0.0
+				],
+				[
+					0.15138,
+					0.01763,
+					6.051361502679184
+				],
+				[
+					0.2,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 10,
+			"start_angle_index": 1,
+			"end_angle_index": 15,
+			"left_turn": false,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04797,
+					0.01951,
+					0.3090984060005374
+				],
+				[
+					0.09837,
+					0.03141,
+					0.1545492030002687
+				],
+				[
+					0.15,
+					0.03541,
+					0.0
+				],
+				[
+					0.20163,
+					0.03141,
+					6.128636104179318
+				],
+				[
+					0.25203,
+					0.01951,
+					5.9740869011790485
+				],
+				[
+					0.3,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 11,
+			"start_angle_index": 1,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.4636476090008061
+				],
+				[
+					0.0,
+					0.0,
+					0.23182380450040305
+				],
+				[
+					0.0,
+					0.0,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 12,
+			"start_angle_index": 1,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					0.05,
+					0.025,
+					0.4636476090008061
+				],
+				[
+					0.1,
+					0.05,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 13,
+			"start_angle_index": 1,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.4636476090008061
+				],
+				[
+					0.0,
+					0.0,
+					0.6245228861991272
+				],
+				[
+					0.0,
+					0.0,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 14,
+			"start_angle_index": 1,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					0.05,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 15,
+			"start_angle_index": 1,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04028,
+					0.02587,
+					0.6781479785985676
+				],
+				[
+					0.07413,
+					0.05972,
+					0.892648348196329
+				],
+				[
+					0.1,
+					0.1,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 16,
+			"start_angle_index": 1,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.3618,
+			"trajectory_length": 0.42696,
+			"arc_length": 0.40057,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					0.04092,
+					0.02394,
+					0.5947686722333784
+				],
+				[
+					0.07836,
+					0.05302,
+					0.7258897354659507
+				],
+				[
+					0.11168,
+					0.08674,
+					0.8570107986985229
+				],
+				[
+					0.1403,
+					0.12453,
+					0.9881318619310953
+				],
+				[
+					0.16374,
+					0.16574,
+					1.1192529251636676
+				],
+				[
+					0.18159,
+					0.20965,
+					1.25037398839624
+				],
+				[
+					0.19354,
+					0.25553,
+					1.3814950516288125
+				],
+				[
+					0.19939,
+					0.30257,
+					1.512616114861385
+				],
+				[
+					0.2,
+					0.35,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 17,
+			"start_angle_index": 2,
+			"end_angle_index": 15,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.04706,
+					0.02617,
+					0.2294328197186447
+				],
+				[
+					0.10084,
+					0.02356,
+					5.956652783219427
+				],
+				[
+					0.15,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 18,
+			"start_angle_index": 2,
+			"end_angle_index": 0,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.04442,
+					0.03644,
+					0.4784860012524262
+				],
+				[
+					0.1,
+					0.05,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 19,
+			"start_angle_index": 2,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.7853981633974483
+				],
+				[
+					0.0,
+					0.0,
+					0.6245228861991272
+				],
+				[
+					0.0,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 20,
+			"start_angle_index": 2,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.07071,
+			"arc_length": 0.0,
+			"straight_length": 0.07071,
+			"poses": [
+				[
+					0.05,
+					0.05,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 21,
+			"start_angle_index": 2,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					0.7853981633974483
+				],
+				[
+					0.0,
+					0.0,
+					0.9462734405957693
+				],
+				[
+					0.0,
+					0.0,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 22,
+			"start_angle_index": 2,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.03644,
+					0.04442,
+					1.0923103255424704
+				],
+				[
+					0.05,
+					0.1,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 23,
+			"start_angle_index": 2,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.02617,
+					0.04706,
+					1.341363507076252
+				],
+				[
+					0.02356,
+					0.10084,
+					1.8973288507550554
+				],
+				[
+					0.0,
+					0.15,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 24,
+			"start_angle_index": 3,
+			"end_angle_index": 0,
+			"left_turn": false,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					0.02393,
+					0.04093,
+					0.9760278785613808
+				],
+				[
+					0.05301,
+					0.07837,
+					0.8449070393286713
+				],
+				[
+					0.08673,
+					0.11169,
+					0.7137862000959616
+				],
+				[
+					0.12452,
+					0.14031,
+					0.582665360863252
+				],
+				[
+					0.16573,
+					0.16375,
+					0.4515445216305424
+				],
+				[
+					0.20965,
+					0.18159,
+					0.3204236823978327
+				],
+				[
+					0.25553,
+					0.19355,
+					0.18930284316512325
+				],
+				[
+					0.30257,
+					0.1994,
+					0.05818200393241346
+				],
+				[
+					0.35,
+					0.2,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 25,
+			"start_angle_index": 3,
+			"end_angle_index": 1,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					0.05,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 26,
+			"start_angle_index": 3,
+			"end_angle_index": 1,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.02587,
+					0.04028,
+					0.892648348196329
+				],
+				[
+					0.05972,
+					0.07413,
+					0.6781479785985676
+				],
+				[
+					0.1,
+					0.1,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 27,
+			"start_angle_index": 3,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					1.1071487177940904
+				],
+				[
+					0.0,
+					0.0,
+					0.9998985329952097
+				],
+				[
+					0.0,
+					0.0,
+					0.892648348196329
+				],
+				[
+					0.0,
+					0.0,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 28,
+			"start_angle_index": 3,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					0.025,
+					0.05,
+					1.1071487177940904
+				],
+				[
+					0.05,
+					0.1,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 29,
+			"start_angle_index": 3,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					1.1071487177940904
+				],
+				[
+					0.0,
+					0.0,
+					1.261697920794359
+				],
+				[
+					0.0,
+					0.0,
+					1.416247123794628
+				],
+				[
+					0.0,
+					0.0,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 30,
+			"start_angle_index": 3,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0118,
+					0.05,
+					1.5707963267948966
+				],
+				[
+					0.0,
+					0.1,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 31,
+			"start_angle_index": 3,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.01763,
+					0.04862,
+					1.3389725222944935
+				],
+				[
+					0.02361,
+					0.1,
+					1.5707963267948966
+				],
+				[
+					0.01763,
+					0.15138,
+					1.8026201312952996
+				],
+				[
+					0.0,
+					0.2,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 32,
+			"start_angle_index": 3,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.01951,
+					0.04797,
+					1.261697920794359
+				],
+				[
+					0.03141,
+					0.09837,
+					1.416247123794628
+				],
+				[
+					0.03541,
+					0.15,
+					1.5707963267948966
+				],
+				[
+					0.03141,
+					0.20163,
+					1.7253455297951654
+				],
+				[
+					0.01951,
+					0.25203,
+					1.879894732795434
+				],
+				[
+					0.0,
+					0.3,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 33,
+			"start_angle_index": 3,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.02356,
+					0.04916,
+					1.2442638028347375
+				],
+				[
+					0.02617,
+					0.10294,
+					1.8002291465135412
+				],
+				[
+					0.0,
+					0.15,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 34,
+			"start_angle_index": 4,
+			"end_angle_index": 1,
+			"left_turn": false,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					0.01523,
+					0.04725,
+					0.9470973974807994
+				],
+				[
+					0.05487,
+					0.07743,
+					0.4636476090008061
+				],
+				[
+					0.1,
+					0.1,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 35,
+			"start_angle_index": 4,
+			"end_angle_index": 2,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.01356,
+					0.05558,
+					1.0923103255424704
+				],
+				[
+					0.05,
+					0.1,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 36,
+			"start_angle_index": 4,
+			"end_angle_index": 3,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					1.5707963267948966
+				],
+				[
+					0.0,
+					0.0,
+					1.416247123794628
+				],
+				[
+					0.0,
+					0.0,
+					1.261697920794359
+				],
+				[
+					0.0,
+					0.0,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 37,
+			"start_angle_index": 4,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.05,
+			"arc_length": 0.0,
+			"straight_length": 0.05,
+			"poses": [
+				[
+					0.0,
+					0.05,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 38,
+			"start_angle_index": 4,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					1.5707963267948966
+				],
+				[
+					0.0,
+					0.0,
+					1.7253455297951652
+				],
+				[
+					0.0,
+					0.0,
+					1.879894732795434
+				],
+				[
+					0.0,
+					0.0,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 39,
+			"start_angle_index": 4,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.01356,
+					0.05558,
+					2.049282328047323
+				],
+				[
+					-0.05,
+					0.1,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 40,
+			"start_angle_index": 4,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					-0.01523,
+					0.04725,
+					2.194495256108994
+				],
+				[
+					-0.05487,
+					0.07743,
+					2.677945044588987
+				],
+				[
+					-0.1,
+					0.1,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 41,
+			"start_angle_index": 5,
+			"end_angle_index": 2,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.02356,
+					0.04916,
+					1.8973288507550556
+				],
+				[
+					-0.02617,
+					0.10294,
+					1.341363507076252
+				],
+				[
+					0.0,
+					0.15,
+					0.7853981633974483
+				]
+			]
+		},
+		{
+			"trajectory_id": 42,
+			"start_angle_index": 5,
+			"end_angle_index": 3,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.0118,
+					0.05,
+					1.5707963267948966
+				],
+				[
+					0.0,
+					0.1,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 43,
+			"start_angle_index": 5,
+			"end_angle_index": 3,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.01763,
+					0.04862,
+					1.8026201312952996
+				],
+				[
+					-0.02361,
+					0.1,
+					1.5707963267948966
+				],
+				[
+					-0.01763,
+					0.15138,
+					1.3389725222944935
+				],
+				[
+					0.0,
+					0.2,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 44,
+			"start_angle_index": 5,
+			"end_angle_index": 3,
+			"left_turn": false,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.01951,
+					0.04797,
+					1.879894732795434
+				],
+				[
+					-0.03141,
+					0.09837,
+					1.7253455297951652
+				],
+				[
+					-0.03541,
+					0.15,
+					1.5707963267948966
+				],
+				[
+					-0.03141,
+					0.20163,
+					1.4162471237946277
+				],
+				[
+					-0.01951,
+					0.25203,
+					1.261697920794359
+				],
+				[
+					0.0,
+					0.3,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 45,
+			"start_angle_index": 5,
+			"end_angle_index": 4,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.0344439357957027
+				],
+				[
+					0.0,
+					0.0,
+					1.8026201312952996
+				],
+				[
+					0.0,
+					0.0,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 46,
+			"start_angle_index": 5,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					-0.025,
+					0.05,
+					2.0344439357957027
+				],
+				[
+					-0.05,
+					0.1,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 47,
+			"start_angle_index": 5,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.0344439357957027
+				],
+				[
+					0.0,
+					0.0,
+					2.1953192129940238
+				],
+				[
+					0.0,
+					0.0,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 48,
+			"start_angle_index": 5,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					0.05,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 49,
+			"start_angle_index": 5,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.02587,
+					0.04028,
+					2.248944305393464
+				],
+				[
+					-0.05972,
+					0.07413,
+					2.4634446749912255
+				],
+				[
+					-0.1,
+					0.1,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 50,
+			"start_angle_index": 5,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					-0.02393,
+					0.04093,
+					2.1655647750284124
+				],
+				[
+					-0.05301,
+					0.07837,
+					2.296685614261122
+				],
+				[
+					-0.08673,
+					0.11169,
+					2.4278064534938313
+				],
+				[
+					-0.12452,
+					0.14031,
+					2.558927292726541
+				],
+				[
+					-0.16573,
+					0.16375,
+					2.6900481319592506
+				],
+				[
+					-0.20965,
+					0.18159,
+					2.8211689711919603
+				],
+				[
+					-0.25553,
+					0.19355,
+					2.95228981042467
+				],
+				[
+					-0.30257,
+					0.1994,
+					3.0834106496573797
+				],
+				[
+					-0.35,
+					0.2,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 51,
+			"start_angle_index": 6,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.04706,
+					0.02617,
+					2.9121598338711485
+				],
+				[
+					-0.10084,
+					0.02356,
+					3.468125177549952
+				],
+				[
+					-0.15,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 52,
+			"start_angle_index": 6,
+			"end_angle_index": 3,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.02617,
+					0.04706,
+					1.8002291465135412
+				],
+				[
+					-0.02356,
+					0.10084,
+					1.2442638028347377
+				],
+				[
+					0.0,
+					0.15,
+					1.1071487177940904
+				]
+			]
+		},
+		{
+			"trajectory_id": 53,
+			"start_angle_index": 6,
+			"end_angle_index": 4,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.03644,
+					0.04442,
+					2.049282328047323
+				],
+				[
+					-0.05,
+					0.1,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 54,
+			"start_angle_index": 6,
+			"end_angle_index": 5,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.356194490192345
+				],
+				[
+					0.0,
+					0.0,
+					2.1953192129940238
+				],
+				[
+					0.0,
+					0.0,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 55,
+			"start_angle_index": 6,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.07071,
+			"arc_length": 0.0,
+			"straight_length": 0.07071,
+			"poses": [
+				[
+					-0.05,
+					0.05,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 56,
+			"start_angle_index": 6,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.356194490192345
+				],
+				[
+					0.0,
+					0.0,
+					2.517069767390666
+				],
+				[
+					0.0,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 57,
+			"start_angle_index": 6,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.04442,
+					0.03644,
+					2.6631066523373668
+				],
+				[
+					-0.1,
+					0.05,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 58,
+			"start_angle_index": 7,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					0.0118,
+					3.141592653589793
+				],
+				[
+					-0.1,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 59,
+			"start_angle_index": 7,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04862,
+					0.01763,
+					2.90976884908939
+				],
+				[
+					-0.1,
+					0.02361,
+					3.141592653589793
+				],
+				[
+					-0.15138,
+					0.01763,
+					3.373416458090196
+				],
+				[
+					-0.2,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 60,
+			"start_angle_index": 7,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04797,
+					0.01951,
+					2.832494247589256
+				],
+				[
+					-0.09837,
+					0.03141,
+					2.9870434505895243
+				],
+				[
+					-0.15,
+					0.03541,
+					3.141592653589793
+				],
+				[
+					-0.20163,
+					0.03141,
+					3.296141856590062
+				],
+				[
+					-0.25203,
+					0.01951,
+					3.450691059590331
+				],
+				[
+					-0.3,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 61,
+			"start_angle_index": 7,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.04916,
+					0.02356,
+					2.815060129629634
+				],
+				[
+					-0.10294,
+					0.02617,
+					3.3710254733084377
+				],
+				[
+					-0.15,
+					0.0,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 62,
+			"start_angle_index": 7,
+			"end_angle_index": 4,
+			"left_turn": false,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					-0.04093,
+					0.02393,
+					2.5468242053562773
+				],
+				[
+					-0.07837,
+					0.05301,
+					2.4157033661235676
+				],
+				[
+					-0.11169,
+					0.08673,
+					2.2845825268908584
+				],
+				[
+					-0.14031,
+					0.12452,
+					2.1534616876581487
+				],
+				[
+					-0.16375,
+					0.16573,
+					2.022340848425439
+				],
+				[
+					-0.18159,
+					0.20965,
+					1.8912200091927294
+				],
+				[
+					-0.19355,
+					0.25553,
+					1.7600991699600197
+				],
+				[
+					-0.1994,
+					0.30257,
+					1.62897833072731
+				],
+				[
+					-0.2,
+					0.35,
+					1.5707963267948966
+				]
+			]
+		},
+		{
+			"trajectory_id": 63,
+			"start_angle_index": 7,
+			"end_angle_index": 5,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					0.05,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 64,
+			"start_angle_index": 7,
+			"end_angle_index": 5,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04028,
+					0.02587,
+					2.4634446749912255
+				],
+				[
+					-0.07413,
+					0.05972,
+					2.248944305393464
+				],
+				[
+					-0.1,
+					0.1,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 65,
+			"start_angle_index": 7,
+			"end_angle_index": 6,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.677945044588987
+				],
+				[
+					0.0,
+					0.0,
+					2.5706948597901063
+				],
+				[
+					0.0,
+					0.0,
+					2.4634446749912255
+				],
+				[
+					0.0,
+					0.0,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 66,
+			"start_angle_index": 7,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					-0.05,
+					0.025,
+					2.677945044588987
+				],
+				[
+					-0.1,
+					0.05,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 67,
+			"start_angle_index": 7,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					2.677945044588987
+				],
+				[
+					0.0,
+					0.0,
+					2.832494247589256
+				],
+				[
+					0.0,
+					0.0,
+					2.9870434505895243
+				],
+				[
+					0.0,
+					0.0,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 68,
+			"start_angle_index": 8,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.141592653589793
+				],
+				[
+					0.0,
+					0.0,
+					3.296141856590062
+				],
+				[
+					0.0,
+					0.0,
+					3.4506910595903304
+				],
+				[
+					0.0,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 69,
+			"start_angle_index": 8,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.05558,
+					-0.01356,
+					3.6200786548422195
+				],
+				[
+					-0.1,
+					-0.05,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 70,
+			"start_angle_index": 8,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					-0.04725,
+					-0.01523,
+					3.7652915829038904
+				],
+				[
+					-0.07743,
+					-0.05487,
+					4.2487413713838835
+				],
+				[
+					-0.1,
+					-0.1,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 71,
+			"start_angle_index": 8,
+			"end_angle_index": 5,
+			"left_turn": false,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					-0.04725,
+					0.01523,
+					2.517893724275696
+				],
+				[
+					-0.07743,
+					0.05487,
+					2.0344439357957027
+				],
+				[
+					-0.1,
+					0.1,
+					2.0344439357957027
+				]
+			]
+		},
+		{
+			"trajectory_id": 72,
+			"start_angle_index": 8,
+			"end_angle_index": 6,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.05558,
+					0.01356,
+					2.6631066523373668
+				],
+				[
+					-0.1,
+					0.05,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 73,
+			"start_angle_index": 8,
+			"end_angle_index": 7,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.141592653589793
+				],
+				[
+					0.0,
+					0.0,
+					2.9870434505895243
+				],
+				[
+					0.0,
+					0.0,
+					2.832494247589256
+				],
+				[
+					0.0,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 74,
+			"start_angle_index": 8,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.05,
+			"arc_length": 0.0,
+			"straight_length": 0.05,
+			"poses": [
+				[
+					-0.05,
+					0.0,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 75,
+			"start_angle_index": 9,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					-0.05,
+					-0.025,
+					3.6052402625905993
+				],
+				[
+					-0.1,
+					-0.05,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 76,
+			"start_angle_index": 9,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.6052402625905993
+				],
+				[
+					0.0,
+					0.0,
+					3.7661155397889203
+				],
+				[
+					0.0,
+					0.0,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 77,
+			"start_angle_index": 9,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					-0.05,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 78,
+			"start_angle_index": 9,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04028,
+					-0.02587,
+					3.8197406321883607
+				],
+				[
+					-0.07413,
+					-0.05972,
+					4.034241001786122
+				],
+				[
+					-0.1,
+					-0.1,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 79,
+			"start_angle_index": 9,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					-0.04093,
+					-0.02393,
+					3.736361101823309
+				],
+				[
+					-0.07837,
+					-0.05301,
+					3.8674819410560186
+				],
+				[
+					-0.11169,
+					-0.08673,
+					3.998602780288728
+				],
+				[
+					-0.14031,
+					-0.12452,
+					4.129723619521437
+				],
+				[
+					-0.16375,
+					-0.16573,
+					4.260844458754147
+				],
+				[
+					-0.18159,
+					-0.20965,
+					4.391965297986857
+				],
+				[
+					-0.19355,
+					-0.25553,
+					4.5230861372195665
+				],
+				[
+					-0.1994,
+					-0.30257,
+					4.654206976452276
+				],
+				[
+					-0.2,
+					-0.35,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 80,
+			"start_angle_index": 9,
+			"end_angle_index": 6,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.04916,
+					-0.02356,
+					3.468125177549952
+				],
+				[
+					-0.10294,
+					-0.02617,
+					2.9121598338711485
+				],
+				[
+					-0.15,
+					0.0,
+					2.356194490192345
+				]
+			]
+		},
+		{
+			"trajectory_id": 81,
+			"start_angle_index": 9,
+			"end_angle_index": 7,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					-0.0118,
+					3.141592653589793
+				],
+				[
+					-0.1,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 82,
+			"start_angle_index": 9,
+			"end_angle_index": 7,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04862,
+					-0.01763,
+					3.373416458090196
+				],
+				[
+					-0.1,
+					-0.02361,
+					3.141592653589793
+				],
+				[
+					-0.15138,
+					-0.01763,
+					2.90976884908939
+				],
+				[
+					-0.2,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 83,
+			"start_angle_index": 9,
+			"end_angle_index": 7,
+			"left_turn": false,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.04797,
+					-0.01951,
+					3.4506910595903304
+				],
+				[
+					-0.09837,
+					-0.03141,
+					3.296141856590062
+				],
+				[
+					-0.15,
+					-0.03541,
+					3.141592653589793
+				],
+				[
+					-0.20163,
+					-0.03141,
+					2.9870434505895243
+				],
+				[
+					-0.25203,
+					-0.01951,
+					2.8324942475892554
+				],
+				[
+					-0.3,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 84,
+			"start_angle_index": 9,
+			"end_angle_index": 8,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.6052402625905993
+				],
+				[
+					0.0,
+					0.0,
+					3.373416458090196
+				],
+				[
+					0.0,
+					0.0,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 85,
+			"start_angle_index": 10,
+			"end_angle_index": 9,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.9269908169872414
+				],
+				[
+					0.0,
+					0.0,
+					3.7661155397889203
+				],
+				[
+					0.0,
+					0.0,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 86,
+			"start_angle_index": 10,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.07071,
+			"arc_length": 0.0,
+			"straight_length": 0.07071,
+			"poses": [
+				[
+					-0.05,
+					-0.05,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 87,
+			"start_angle_index": 10,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					3.9269908169872414
+				],
+				[
+					0.0,
+					0.0,
+					4.0878660941855625
+				],
+				[
+					0.0,
+					0.0,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 88,
+			"start_angle_index": 10,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.03644,
+					-0.04442,
+					4.233902979132264
+				],
+				[
+					-0.05,
+					-0.1,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 89,
+			"start_angle_index": 10,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.02617,
+					-0.04706,
+					4.4829561606660455
+				],
+				[
+					-0.02356,
+					-0.10084,
+					5.038921504344849
+				],
+				[
+					0.0,
+					-0.15,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 90,
+			"start_angle_index": 10,
+			"end_angle_index": 7,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.04706,
+					-0.02617,
+					3.3710254733084377
+				],
+				[
+					-0.10084,
+					-0.02356,
+					2.815060129629634
+				],
+				[
+					-0.15,
+					0.0,
+					2.677945044588987
+				]
+			]
+		},
+		{
+			"trajectory_id": 91,
+			"start_angle_index": 10,
+			"end_angle_index": 8,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.04442,
+					-0.03644,
+					3.6200786548422195
+				],
+				[
+					-0.1,
+					-0.05,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 92,
+			"start_angle_index": 11,
+			"end_angle_index": 9,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.05,
+					-0.05,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 93,
+			"start_angle_index": 11,
+			"end_angle_index": 9,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.02587,
+					-0.04028,
+					4.034241001786122
+				],
+				[
+					-0.05972,
+					-0.07413,
+					3.8197406321883607
+				],
+				[
+					-0.1,
+					-0.1,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 94,
+			"start_angle_index": 11,
+			"end_angle_index": 10,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					4.2487413713838835
+				],
+				[
+					0.0,
+					0.0,
+					4.141491186585003
+				],
+				[
+					0.0,
+					0.0,
+					4.034241001786122
+				],
+				[
+					0.0,
+					0.0,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 95,
+			"start_angle_index": 11,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					-0.025,
+					-0.05,
+					4.2487413713838835
+				],
+				[
+					-0.05,
+					-0.1,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 96,
+			"start_angle_index": 11,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					4.2487413713838835
+				],
+				[
+					0.0,
+					0.0,
+					4.403290574384152
+				],
+				[
+					0.0,
+					0.0,
+					4.557839777384421
+				],
+				[
+					0.0,
+					0.0,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 97,
+			"start_angle_index": 11,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.0118,
+					-0.05,
+					4.71238898038469
+				],
+				[
+					0.0,
+					-0.1,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 98,
+			"start_angle_index": 11,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.01763,
+					-0.04862,
+					4.480565175884287
+				],
+				[
+					-0.02361,
+					-0.1,
+					4.71238898038469
+				],
+				[
+					-0.01763,
+					-0.15138,
+					4.944212784885092
+				],
+				[
+					0.0,
+					-0.2,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 99,
+			"start_angle_index": 11,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					-0.01951,
+					-0.04797,
+					4.403290574384152
+				],
+				[
+					-0.03141,
+					-0.09837,
+					4.557839777384421
+				],
+				[
+					-0.03541,
+					-0.15,
+					4.71238898038469
+				],
+				[
+					-0.03141,
+					-0.20163,
+					4.866938183384958
+				],
+				[
+					-0.01951,
+					-0.25203,
+					5.021487386385227
+				],
+				[
+					0.0,
+					-0.3,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 100,
+			"start_angle_index": 11,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					-0.02356,
+					-0.04916,
+					4.385856456424531
+				],
+				[
+					-0.02617,
+					-0.10294,
+					4.941821800103334
+				],
+				[
+					0.0,
+					-0.15,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 101,
+			"start_angle_index": 11,
+			"end_angle_index": 8,
+			"left_turn": false,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					-0.02393,
+					-0.04093,
+					4.117620532151173
+				],
+				[
+					-0.05301,
+					-0.07837,
+					3.986499692918464
+				],
+				[
+					-0.08673,
+					-0.11169,
+					3.855378853685755
+				],
+				[
+					-0.12452,
+					-0.14031,
+					3.7242580144530453
+				],
+				[
+					-0.16573,
+					-0.16375,
+					3.5931371752203356
+				],
+				[
+					-0.20965,
+					-0.18159,
+					3.462016335987626
+				],
+				[
+					-0.25553,
+					-0.19355,
+					3.3308954967549163
+				],
+				[
+					-0.30257,
+					-0.1994,
+					3.1997746575222066
+				],
+				[
+					-0.35,
+					-0.2,
+					3.141592653589793
+				]
+			]
+		},
+		{
+			"trajectory_id": 102,
+			"start_angle_index": 12,
+			"end_angle_index": 9,
+			"left_turn": false,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					-0.01523,
+					-0.04725,
+					4.088690051070593
+				],
+				[
+					-0.05487,
+					-0.07743,
+					3.6052402625905993
+				],
+				[
+					-0.1,
+					-0.1,
+					3.6052402625905993
+				]
+			]
+		},
+		{
+			"trajectory_id": 103,
+			"start_angle_index": 12,
+			"end_angle_index": 10,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					-0.01356,
+					-0.05558,
+					4.233902979132264
+				],
+				[
+					-0.05,
+					-0.1,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 104,
+			"start_angle_index": 12,
+			"end_angle_index": 11,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					4.71238898038469
+				],
+				[
+					0.0,
+					0.0,
+					4.557839777384421
+				],
+				[
+					0.0,
+					0.0,
+					4.403290574384152
+				],
+				[
+					0.0,
+					0.0,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 105,
+			"start_angle_index": 12,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.05,
+			"arc_length": 0.0,
+			"straight_length": 0.05,
+			"poses": [
+				[
+					0.0,
+					-0.05,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 106,
+			"start_angle_index": 12,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					4.71238898038469
+				],
+				[
+					0.0,
+					0.0,
+					4.866938183384958
+				],
+				[
+					0.0,
+					0.0,
+					5.021487386385227
+				],
+				[
+					0.0,
+					0.0,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 107,
+			"start_angle_index": 12,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.01356,
+					-0.05558,
+					5.190874981637116
+				],
+				[
+					0.05,
+					-0.1,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 108,
+			"start_angle_index": 12,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.0809,
+			"trajectory_length": 0.15137,
+			"arc_length": 0.08957,
+			"straight_length": 0.0618,
+			"poses": [
+				[
+					0.01523,
+					-0.04725,
+					5.3360879096987865
+				],
+				[
+					0.05487,
+					-0.07743,
+					5.81953769817878
+				],
+				[
+					0.1,
+					-0.1,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 109,
+			"start_angle_index": 13,
+			"end_angle_index": 10,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.02356,
+					-0.04916,
+					5.038921504344849
+				],
+				[
+					0.02617,
+					-0.10294,
+					4.4829561606660455
+				],
+				[
+					0.0,
+					-0.15,
+					3.9269908169872414
+				]
+			]
+		},
+		{
+			"trajectory_id": 110,
+			"start_angle_index": 13,
+			"end_angle_index": 11,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0118,
+					-0.05,
+					4.71238898038469
+				],
+				[
+					0.0,
+					-0.1,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 111,
+			"start_angle_index": 13,
+			"end_angle_index": 11,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.01763,
+					-0.04862,
+					4.944212784885092
+				],
+				[
+					0.02361,
+					-0.1,
+					4.71238898038469
+				],
+				[
+					0.01763,
+					-0.15138,
+					4.480565175884287
+				],
+				[
+					0.0,
+					-0.2,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 112,
+			"start_angle_index": 13,
+			"end_angle_index": 11,
+			"left_turn": false,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.01951,
+					-0.04797,
+					5.021487386385227
+				],
+				[
+					0.03141,
+					-0.09837,
+					4.866938183384958
+				],
+				[
+					0.03541,
+					-0.15,
+					4.71238898038469
+				],
+				[
+					0.03141,
+					-0.20163,
+					4.557839777384421
+				],
+				[
+					0.01951,
+					-0.25203,
+					4.403290574384152
+				],
+				[
+					0.0,
+					-0.3,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 113,
+			"start_angle_index": 13,
+			"end_angle_index": 12,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.176036589385496
+				],
+				[
+					0.0,
+					0.0,
+					4.944212784885092
+				],
+				[
+					0.0,
+					0.0,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 114,
+			"start_angle_index": 13,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					0.025,
+					-0.05,
+					5.176036589385496
+				],
+				[
+					0.05,
+					-0.1,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 115,
+			"start_angle_index": 13,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.176036589385496
+				],
+				[
+					0.0,
+					0.0,
+					5.336911866583817
+				],
+				[
+					0.0,
+					0.0,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 116,
+			"start_angle_index": 13,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					-0.05,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 117,
+			"start_angle_index": 13,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.02587,
+					-0.04028,
+					5.390536958983257
+				],
+				[
+					0.05972,
+					-0.07413,
+					5.605037328581019
+				],
+				[
+					0.1,
+					-0.1,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 118,
+			"start_angle_index": 13,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.36181,
+			"trajectory_length": 0.42697,
+			"arc_length": 0.40058,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					0.02393,
+					-0.04093,
+					5.307157428618205
+				],
+				[
+					0.05301,
+					-0.07837,
+					5.438278267850915
+				],
+				[
+					0.08673,
+					-0.11169,
+					5.569399107083624
+				],
+				[
+					0.12452,
+					-0.14031,
+					5.7005199463163345
+				],
+				[
+					0.16573,
+					-0.16375,
+					5.831640785549044
+				],
+				[
+					0.20965,
+					-0.18159,
+					5.962761624781754
+				],
+				[
+					0.25553,
+					-0.19355,
+					6.093882464014463
+				],
+				[
+					0.30257,
+					-0.1994,
+					6.225003303247172
+				],
+				[
+					0.35,
+					-0.2,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 119,
+			"start_angle_index": 14,
+			"end_angle_index": 11,
+			"left_turn": false,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.02617,
+					-0.04706,
+					4.941821800103334
+				],
+				[
+					0.02356,
+					-0.10084,
+					4.385856456424531
+				],
+				[
+					0.0,
+					-0.15,
+					4.2487413713838835
+				]
+			]
+		},
+		{
+			"trajectory_id": 120,
+			"start_angle_index": 14,
+			"end_angle_index": 12,
+			"left_turn": false,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.03644,
+					-0.04442,
+					5.190874981637116
+				],
+				[
+					0.05,
+					-0.1,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 121,
+			"start_angle_index": 14,
+			"end_angle_index": 13,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.497787143782138
+				],
+				[
+					0.0,
+					0.0,
+					5.336911866583817
+				],
+				[
+					0.0,
+					0.0,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 122,
+			"start_angle_index": 14,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.07071,
+			"arc_length": 0.0,
+			"straight_length": 0.07071,
+			"poses": [
+				[
+					0.05,
+					-0.05,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 123,
+			"start_angle_index": 14,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.497787143782138
+				],
+				[
+					0.0,
+					0.0,
+					5.658662420980459
+				],
+				[
+					0.0,
+					0.0,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 124,
+			"start_angle_index": 14,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.12071,
+			"trajectory_length": 0.11552,
+			"arc_length": 0.09481,
+			"straight_length": 0.02071,
+			"poses": [
+				[
+					0.04442,
+					-0.03644,
+					5.80469930592716
+				],
+				[
+					0.1,
+					-0.05,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 125,
+			"start_angle_index": 14,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.04706,
+					-0.02617,
+					6.053752487460941
+				],
+				[
+					0.10084,
+					-0.02356,
+					0.32653252396015886
+				],
+				[
+					0.15,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 126,
+			"start_angle_index": 15,
+			"end_angle_index": 12,
+			"left_turn": false,
+			"trajectory_radius": 0.3618,
+			"trajectory_length": 0.42696,
+			"arc_length": 0.40057,
+			"straight_length": 0.02639,
+			"poses": [
+				[
+					0.04092,
+					-0.02394,
+					5.688416634946208
+				],
+				[
+					0.07836,
+					-0.05302,
+					5.557295571713635
+				],
+				[
+					0.11168,
+					-0.08674,
+					5.426174508481063
+				],
+				[
+					0.1403,
+					-0.12453,
+					5.295053445248491
+				],
+				[
+					0.16374,
+					-0.16574,
+					5.163932382015918
+				],
+				[
+					0.18159,
+					-0.20965,
+					5.032811318783346
+				],
+				[
+					0.19354,
+					-0.25553,
+					4.901690255550774
+				],
+				[
+					0.19939,
+					-0.30257,
+					4.770569192318201
+				],
+				[
+					0.2,
+					-0.35,
+					4.71238898038469
+				]
+			]
+		},
+		{
+			"trajectory_id": 127,
+			"start_angle_index": 15,
+			"end_angle_index": 13,
+			"left_turn": false,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.07194,
+			"arc_length": 0.07194,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					-0.05,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 128,
+			"start_angle_index": 15,
+			"end_angle_index": 13,
+			"left_turn": false,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.14389,
+			"arc_length": 0.14389,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04028,
+					-0.02587,
+					5.605037328581019
+				],
+				[
+					0.07413,
+					-0.05972,
+					5.390536958983257
+				],
+				[
+					0.1,
+					-0.1,
+					5.176036589385496
+				]
+			]
+		},
+		{
+			"trajectory_id": 129,
+			"start_angle_index": 15,
+			"end_angle_index": 14,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.81953769817878
+				],
+				[
+					0.0,
+					0.0,
+					5.712287513379899
+				],
+				[
+					0.0,
+					0.0,
+					5.605037328581019
+				],
+				[
+					0.0,
+					0.0,
+					5.497787143782138
+				]
+			]
+		},
+		{
+			"trajectory_id": 130,
+			"start_angle_index": 15,
+			"end_angle_index": 15,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.1118,
+			"arc_length": 0.0,
+			"straight_length": 0.1118,
+			"poses": [
+				[
+					0.05,
+					-0.025,
+					5.81953769817878
+				],
+				[
+					0.1,
+					-0.05,
+					5.81953769817878
+				]
+			]
+		},
+		{
+			"trajectory_id": 131,
+			"start_angle_index": 15,
+			"end_angle_index": 0,
+			"left_turn": true,
+			"trajectory_radius": 0.0,
+			"trajectory_length": 0.0,
+			"arc_length": 0.0,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.0,
+					0.0,
+					5.81953769817878
+				],
+				[
+					0.0,
+					0.0,
+					5.9740869011790485
+				],
+				[
+					0.0,
+					0.0,
+					6.128636104179318
+				],
+				[
+					0.0,
+					0.0,
+					0.0
+				]
+			]
+		},
+		{
+			"trajectory_id": 132,
+			"start_angle_index": 15,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.1118,
+			"trajectory_length": 0.10367,
+			"arc_length": 0.10367,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.05,
+					-0.0118,
+					0.0
+				],
+				[
+					0.1,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 133,
+			"start_angle_index": 15,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.22361,
+			"trajectory_length": 0.20735,
+			"arc_length": 0.20735,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04862,
+					-0.01763,
+					6.051361502679184
+				],
+				[
+					0.1,
+					-0.02361,
+					0.0
+				],
+				[
+					0.15138,
+					-0.01763,
+					0.23182380450040302
+				],
+				[
+					0.2,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 134,
+			"start_angle_index": 15,
+			"end_angle_index": 1,
+			"left_turn": true,
+			"trajectory_radius": 0.33541,
+			"trajectory_length": 0.31102,
+			"arc_length": 0.31102,
+			"straight_length": 0.0,
+			"poses": [
+				[
+					0.04797,
+					-0.01951,
+					5.9740869011790485
+				],
+				[
+					0.09837,
+					-0.03141,
+					6.128636104179318
+				],
+				[
+					0.15,
+					-0.03541,
+					0.0
+				],
+				[
+					0.20163,
+					-0.03141,
+					0.1545492030002687
+				],
+				[
+					0.25203,
+					-0.01951,
+					0.30909840600053734
+				],
+				[
+					0.3,
+					0.0,
+					0.4636476090008061
+				]
+			]
+		},
+		{
+			"trajectory_id": 135,
+			"start_angle_index": 15,
+			"end_angle_index": 2,
+			"left_turn": true,
+			"trajectory_radius": 0.09811,
+			"trajectory_length": 0.16364,
+			"arc_length": 0.12254,
+			"straight_length": 0.04109,
+			"poses": [
+				[
+					0.04916,
+					-0.02356,
+					5.956652783219427
+				],
+				[
+					0.10294,
+					-0.02617,
+					0.22943281971864465
+				],
+				[
+					0.15,
+					0.0,
+					0.7853981633974483
+				]
+			]
+		}
+	]
+}
\ No newline at end of file
diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py
index 0ba9141411c..637e2d993e1 100644
--- a/nav2_smac_planner/lattice_primitives/trajectory.py
+++ b/nav2_smac_planner/lattice_primitives/trajectory.py
@@ -15,7 +15,7 @@
 from dataclasses import dataclass
 from typing import Any, Union
 
-from nav2_smac_planner.lattice_primitives.helper import angle_difference, normalize_angle
+from lattice_primitives.helper import angle_difference, normalize_angle
 import numpy as np
 from numpy.typing import NDArray
 
diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py
index f576b14c188..5bf36a7efe2 100644
--- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py
+++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py
@@ -15,7 +15,7 @@
 import logging
 from typing import Any, List, Tuple, TypedDict, Union
 
-from nav2_smac_planner.lattice_primitives.trajectory import (FloatNDArray, Path, Trajectory,
+from lattice_primitives.trajectory import (FloatNDArray, Path, Trajectory,
                                                              TrajectoryFloat, TrajectoryParameters)
 import numpy as np
 
diff --git a/nav2_smac_planner/lattice_primitives/visualizations/0.0.png b/nav2_smac_planner/lattice_primitives/visualizations/0.0.png
new file mode 100644
index 0000000000000000000000000000000000000000..a0cb1b53329d71809cd2df83ea1aba4c2beb54b0
GIT binary patch
literal 12662
zcmeHtcT`jBw(kN2QN#`?T@X|daEpL|G#gDs={+jFgLIGpin>uLQkAYW>AhD)ML;@)
zPADQZpaKaH65fpap8Y)Mo_*fE<NbB+dt=1Ga3yQ4ugq`GUztHSt}8O`X5Eb-2&1yn
zWeo(O_D2w^ae6wqBGxtX1^!67UeR;abhL2wxP8|gQM>KxWasE=XMKm;-TbbLwWEWm
zu(+`3X>Kc5S0@)K5fS@8e<195*HWZot!@n-vdc+H-vvST-KPAbdLx%%jUZ>HlrJl2
zdB)>L{fu<hNZa2}Ow!UtWFJo3dh)s_aO>t?(ax8Z&1uo4>PbSPtQeuftrtvPI^m@S
zYcGAv&i4Oka*ZwK`j}y!EiU)I6g`g!1%`^L5gK0~JgoNCY=SRwnQMmTsXR@<f>AtS
z3SI5BAmuo=(<I-6Wn(dZA^wnsel#D9>_|#n4=w!VuAujaKaY+f)Nt_;LXRM~j!<(W
zi2v>X|Hc2!lDt^D&e9o`aKTH1h{tJmB+4)`Gk<lJM37{r)2-cIDT;mhC^@$Ui?4-e
zsBcLx4wtKc|Ni}Y3>AX3H#Bki<&a54Y2!z{^!~WcVoP;pJljcr{*MzjQaarGZzw1P
z+>=eZz!};zy(3|cq?$uTiW{we&stHr9UIH~p{YrNEaSifk3AQU4-N`)Hb<U4d7^Rc
z+Oq}pcf-b2G9SjD+_r?e!+32L`z6%zrPm}If;8OPArFzzxF3Nr?74*OfKc5-wI9AX
z^KbS=-fsWa7?J*Dg$KPUiu?FZoUmQ_+LjStZQjZp%q3@1>qEfkTPCEve}7p9KVYhm
z!whpgI+`GyobqLQ`jnucD&_9=?^A0FsBMFubhV!L1ZlBb4gMA@DJdype&l<>tYRu%
zU0p*(=1(0er<t!PDlUADb97Ipfnzi7)OMhOPc_;Kzc|?P`LmXWMtHQaan|~}XRco1
z>kk_#ilG>{e&dXZ#>axkjw#4(P3#jhZ;9;T6oUnKqS0lix`5uQP*G7Sx%Kuw|CDT!
z`oo70b2mBD($WM@pYFZ1PsX)H(#_2+D?8hEBrh}b`EgomDq89Q(Ki=d#a5@v>I_<c
z+2E0pdNv;$LERk<4|eX$s;e_`c6L_#I5jz`rLXTb<Vz$h=VxUJB%F7?SXo&K!@V#)
z^5H3qZP)87Z66tS?>@}L<Yw{V@%eh}vg@vpv$cLfV#$8R#27sq2|UWsZ~-p5TtV#i
zGc#k%hKq76Oi`s?D;B=wjTr%9Va$D6hI3}0_8WVQx(T(nH8<aMcXyw1S5pg~oSK?m
z**JXYP!E~zq7wK1Fzm9ffkEs)4d_Rrp8W8RXTibRo}Qk?DZV@RvRk=peaog}WIe9T
zDtR{*zl_Zv85z-}Yxwj@>B`ls;%o%Z3wgw8tMRX62ERu8$Uyz@rsSc#3i{(bJQtgr
zo7?tk5=rP_Hi^7GY*SGVf6{l@K__0=gUHmyMLTmdGpo-pep69X>&w#4)-o~4t8|%B
zDN@IxH|H`8>e02wtp|(DS<l?^KX~X6-ynhZ-k0Z!uEX}_k{Ec7{Fw@V#KZ?zD&;yG
z#(D3Xg0_~H)%)KM43$_%(CuYEshsuZ%?&Rvuc@tw6Y2sdPAK{imb3{=qib!BXw7|c
zTLqNR$}oT<Nx!>)hrDvEgk$2@wz&SueoU@mzZtP?*kQ_w3l3?xpezlfZGQ~!|MVR0
ztc@7e-K&J1Pvw;HJ}D`wA1C1`J3RtBC?F)H=DWFi6Ox>Ursmpwc0OGpW$i3s6Tbd&
zsu@^>wXCf(`LDf5w(d#Ktf{H_u;ExwD(mj<F0t<6kBW@6>P}Uz`q3h&ZD7Fqr3KR9
z{Z=t5r{-|JSQ($Pv!bH#lhicT7y(&X6G5FED`#?dnyMRN)MH`tsR*X?)upxKB+|7@
zkN5To=GPqj;{n;#J+!WYQ%#}Ssi~J4m`_`sU0T`D($gErT6X^NMVUj!TdUT4O=>#6
zs!FP_uTRXOzqeN@oI1dtI-udG7H1Xfrn4a`*W7#ipsoD21wo{c&S41~EK?dKNIm}c
z!A@-yD!-zlf`1CmbyiM}T_3@7wM)@$ap1SM7r#wKx-g5F<Xm~mEXF2s^5hLIEv?}v
zOFUcX`X0XXEx%@o0Y7{DT)5cIL{*@mr>7TT-&zzKTx!<%h_R)utxr{?Hnq-|n0VLZ
z`t^{a;$pUPTN|6hTwEea9T_iPybu++umh6N1_=#Gs(I@BdE@JyFUO!Bb=D6;aSDhd
zI-Y-Ec@~n;z<}Pv$B!kXhdjm`0xT41>jgwa)Qc@UFe@E$`OOh0lH484%u;!HdE<h8
zE^zjf?w)gUam>B_>+Bb?_N4e-x<)$=9ysvo%a<<}k26W=`j5eE9xWtsyMbgj$C{fR
zvc#OcipeMK!iNCi9Iu$9Pd;vLX(`Cdd&%_Uvzs<HSvt7}X+M74ZA*~W4fox|_o&PD
zZLTfw4Nf>>a`ZQ+Mf@@&BO^biJhn`5i>@)rH}!>3aux62(O{cjtMhm-NB4t$vTvR|
z*&Tf3w~KM&_T3EuG<?cTOiXG^yVfaH<HYs&&fM~`EJMfIQs=ElY!01OZVUH~Wg#te
zLPV-`*Lba*a~!K-uE+P?AW(`2S^-{h>=RQG71ROwg#-WgRrvn^8Y%=3VN{*b13E^H
zP)?c1`1qW*IEkdY4p*)`?C$9ivl}?$wt!L7*Vlh5co^!lLf$TaNR}-vE#2=PGQa`N
z+I5KE3UVn#kfSr&*f?QxbuQ&01Izq729Z5?OJOnFoz)PdBC{Zij>O#0pRe)p@mbGx
zr#%S`?SH}~Xb_(i7M6@6|A>hdvxO~o49lkV@3hpvu8SlWyLwdlhd6DmJ5zGP+xxU?
zAAMz$b~7+YFV6J#^=0Pe<vCZUr(X>}Ouqv@Qj0Tz_aC80FuJxsqyEL%_CkKY?c4ji
zSkH_HUJY@rVzqoh=5%t|c38~JH9COo<Gk=@bJP37hYw;$L!Um?G&eVYOp5=9y4!bK
z?nLA|37w(p6iI=20YRkgm86N5me^Dh(Z_n?U2Uwi$KxJ8$(HwohM3C2?V%w9_-(u(
z^{od51qJ-W(dJ@owxRLy>=5}9l(cdZ(iXsp-+udz1MhNC;h&Y=fMycbSy2Lq<;s)w
zwORWT;XFS|yqlqVH?Vn`s(F+o5&u^7CA`7TF6UiiV;l!g+G$+AIORU>z|o_bpFVxE
znBoj=a;s-&92qgReL+b9FEFqes*GF-iE{PVYJE_4TH8Xu;bgvPJN)HAp1Globe6Kq
z2#mT$kT$=@Ex1iQ)NW_qk=*lNKFDkYfN>GWDL^DED@#LP|0NF(kLz1nmh)ZI$ocuD
z>a<YV@Ph|aKRi{RKUc0n+xemLU)Jj8y6-Ex%e&5dug-q#hd}bi4_b=B)P2_{9=|=!
zfw0dIR0Va1^853myz#DOoJax`jvL`nt~0z=zLu_+LM)516<0JfA#c*J#>xz%W&G;S
zS%<$jK8tv`e6WPWZn>0Kof;|^O}8(a6Fo3zgY2LI@VzHGNrd!O0>5w~De`qnLhep^
zC&1?oHxxsZUcSUHNH2v6*Y<YZMRpvZ{5UE>$agdIoZ0WI#YvLJjQ+QH5NiJb^1321
z(oye4i9^Dd?hJkE^R&S4j(q)g+q#{dxR&6dGSwt)xX*ta;Di6|qN3C%yXUr6(jIf(
ztzt#4bHhai6STfTy~#XAZ4>91R@{EKisz3nj_u_Lcc}b;_U$*;P<c|khB)n|48xg-
z(=bnqd~s4{J-Me3aXLu3_-*oOcX0=Kq1Q2#^E#9QWylXsl@E5NRl4B%2_wA091C_b
zNW3EOj+i((l$4V^tE`dtI8{bA)&AW;MaS=S$cao(D^E<|8IyQ<6Y}WgPf`Y_iW}r@
zi?gh6Z+`yqnKLZ?s|P@0g-dK>&N;s(ZPY&%4qvLT$6*_bq-sth$L>?UTp<(P*RyjX
zuZBeK-ktjU?`6KB{U!{W?)b~N(P2q3YA0qGu~BNywW`IlnOQ2VG6vpLjG5|5m}S(>
zW5yl{=j;tUaHBqDoZcsEidAUGxnV&oE-MGECM*xAZ&j?{nJev29ji4gUk*CdJr%0j
z_(-`!<^+&E>&?}A+vf1ib|F@O-YRKhV1uMGhp6;x&$F^|iZ#<SswR#L1XAdBn2IV6
zS7@4sBFzU12#m$_ODC^9p&LIEf>6t|5N705t*zbHiTLs~U6j4?I&o2yIM|Sx>Ao&a
zw6mU8$t>uU_1)7@d!CveF&-FpRE<Fib4XkG;g_RyQyuS3trGR3OVG8eQ(VJIOp&Q<
z4q<4rOQs{rhxn!Wx)7UxBdmAlm0EI@>vWWceeOB0qSbCy4Xxfeg4eb$am+tXVtBam
zG0-H{7?YumHEMc}cxpO-;Wx?hRg=4=x6~2@YPWvOHDYIyR6VfvNkgMGgNekIq~Q$o
zE<b;-pQHMmuGZbBtJ)MgtcWD}k$Xcui%-uH>SLBFE^V%x`Yg_8r=_W;Q5j*Wjy#1e
zGybG1qTFx9OH1t@=Cf3(iT$#{wH@YA-M05xcw`tiX<oN`bxuK$MgI56!)FlJ10prt
zr172Z>(yUbPl@DDf78`HA+s7r+Ne{DJ07ZNP;q0k6-Dod*s)XX@E7J5QLiF<XV#gh
zsK=m*hM7JL+q8Z-F(OZ>jL#x&`Ce1qC)0s~aSQ`@iSv&A$i<N!_2F0fbz^nLt($9Y
zji@URWh{S~NpB_i<gG3icPEiAuhVq0LkL|Qh)`E4wiqN#w|<Vq<VC%!U-cZ@^d`Fa
z1_gCLV$>*iz|W9n*Bj93a6DZ~Y3Pk9>$tzvCW@50w;8Q9d`NwOD*tVnefLP^ws+w0
z2NqR99i`^xxA{!+{qGLQBlHTb(ImmGQFL|f=zj8EuCfOxm6m$WiLyHnwX`UskfKa*
zf3a((>r4WEp?WDC{hfiR<)9ks6ere7_HN`IL6XH_xA$M9d=bPE9vQ()n(-T&SQhas
zRHu=+@D6vMVKLk1#C6v^9mJR=!=H-#G6aup3~4i9Cz**XZWkt8HH!*$BIa)wsgn+p
zk@!=TApM=5AwnlVx{e@2RDYl<66L*G;P}1BHS?TPvMC?6(=IqMfnndirBA4bld;<o
zV}42FJCmm><_~}SG2JR=)ycg!+jR*j+R(_i+b=ae7jnxf5CKrX)wl7t29<Zs!vtvk
z%Z_bP%_{v%WI!IP^>N#nj<J0gxC=N!UoBxQd)TqwSfu9L#W2C0K(2%V)%^>;E~uN=
z>rX{f;yR}RiT{+SsHOzeB~Gs|&$)H?BFKKDy!Acf-=~_iH8ft+GBBSzO4zsuMBUCg
z>Gf-#jeB+crkd&3x&UMVQuy-SGZv?*tD8|(RrL`EQjM|(s5#vGtsL2orM==RU+XhS
zr$GEwM|Z&c<6p_61Db}FJP79n&l_uNYfqj&wLQzp!jb_fh?$THaNn66bm)ZZw{e98
zk1?+{ync;mA+wM{uU$=Yvi!}PH*vGvu7Jw_1|j~Y68$|^$iJ{Q@{_tt)Te!?la!Qf
zX>WI+C<{05-Kzna>CT-y?Inw0l;IPIQPI(u$*vpaqQ6Q31gsNXhglxIe_>(a?tS|l
z-Ak4M54VN<5Iav8aWRa?*7|s?Esa)x@C9VgboA(;-N@AUilvy8fH)sp#vY2Cuqjau
z+Jckmw!^~AwTeleu-|{D2jNCS`WlWv6muA<U?wQ5so9Iy!^8l>PXq%wGiqak8Vht}
zEe^YN)wVycFVDCd;@l&vrpCx+raj6msV+gxR%dDpX#0FhsH}#&dzm|KD6Q1C-=-~A
zY_lsA{Y8rbK@f$BCz=i6uDH_vd0=tg-`?)jzIpT20p6=tp#HF~`+2D6zIkJHSG_^u
z0Uc0BwW^hN=~BDFvug|e>Y;Mvin?vmvUDFc60d=-a~(9t^G+biwMK^!KI^d>FW~Ja
zPf>>D4#vfWuv<1D2PPOXAtfemrK__k-6Vo4W75@Af)RNY7g^6K8I)a%J^P{SAXSCG
z?R6>|d0k6=N}iM>Wu7yA<hoTMgPsj@U8?NHZVRg=*!bak)Dv<c8C*r+m>*!~C#^SW
zn5C`~v8vnT<!%;~5|)$)hZR;u!?R*_YenPE>(v3%?w~{0bs3Wd0UpTHzZ6&Vo@OEz
zqy4s0RvowbNh?Vj*PfM_1!-yUq@}AQ`3aj_sx~)wpW^h~fn;2!yeJ}?-+t)$l`G2m
zbppm-X}#aZ?=y%*mf1ar6Ysqi?!FlIF*N3^C{FVO(#Z;So?an>e^8f<o?`2^%~y^=
zIdH8-q&kc)HkA96yY{Hz2V%v#3vT1()DvCv91vp)@VVF<xJyGJz;~07TfWF3U$?a*
zdq7Y6oBbe9FuOGu)#uM8gyF)*$||ZrSxPD?=+$aoj%k-Q++0cGOH!A*n>rW&eG1n>
zC%gH^_kBUZjZI8>Dx|*<PPf-K$g2=2DaRZZ^@!)3@=dxix!qryyDgK%5~jWnusmEG
z;)B|u;alt5rKAzQT8$kne?jF>2lJSvu3HnDTkFU5)ba7*b8qkEw3E&E^COD?7ALO!
zVCOJ_q2?<qaelI%B(&~EL$5};g-5vNhWCovDdBds)TdPC?f@F0js#fpZQr>Qa?g2q
z7PEsCpM^0C6)g+VH`r0`xcF#a`K58MnL(=MqSn+WPoifM-J&_x$NaE$rPquTBI9gP
zXEbi?fB%{)kR9gk`dd&CUi&SHIGnUmy;#&E$LcV9gGhhNB$0!C8!tihde?8#$~%o{
z0;No5V3q3K8$d&+<bzi_bV6l%IwO&b*+TBp`h$I`Ij4yx_qR#d3mSXRFu@j7y?Of}
ztsEOo#~4l~dr0p<uTRVof`STZP|87(;Xm>fTY``BGBMGn*&)aLDK&baAprlp;n=0c
zZys}L?nkzySd7iEx=wupbMY>z+a#3r$a#RAoTqla`1y0Ado>)?j~OFm$pd=>sy+Nj
z=+UadfaTWENMf7g;&QQr@sZF-8lz_v#MYUg#6c}mAwN{!QFq2{rX7PD&IlzbRkp?9
z3_q|-9Ur~t;vr<TZVwNyI7|U=Ji_gDS3bYF2|$#C8taq4M)V^NcjIy0yrD{6UD58i
z`P@?54O?m_UUV^4!<Eg|!lWZ-y-T#T&9SGea&p#)J{Rz!fmomAKz?<yQe|cQPP&ob
zDI@8rE+;LHKNTrcpSd1~!81s25@Nn~i1Z9vdZ+{;>~Ihf7O5F_h{o{n#h$n2*d^C1
z$i-chF|{m2VRDP}@xFL{>#3_5g4Fv3ega$5CKbYnX?>z}KiP;Iq2T*zPEUZpBYXW;
zW~2ef-;k%E;-}9d0iB$>FoGs8uXIQoA3!o-OB)b4o3(_Y{F%0hz3ff^fZHjk|JYJ>
zN1gKO+XwFSC8yz4<A)(sj_~ljs}rd3@tAwal3H`&&&y`3Rz#)b<D=f$zM=BSSMY-T
zZ!gvAR?YIv6?ZHeHaZC$IkOLDum{eEF0IdnZdH*%i6FXL%#>$Z^`3-CE<|D7<Fsy0
z0EWL$0c?+|W;luXGmrPFmXEy4ywz|VBKu5alPQXc{lKx>A5T_$HGu_i>%jutDmcKK
zdgbY2e>qkd=nK#Y`n{9~FDc4}e+-Gr%1)DXInT7`XV5%a7#o#kVn2F{)UiE~PgM~I
zGXo_(>iP3y`Tc_V{n^i+?ctET`=x|vH&SsED(_7SRekqPOGk$}(twKlFdv^X<WiE9
zpen#j#a61$W+3D}Jw4hwI_X<m_oioNvU75-9X)z9V{~Sq(3DjUUBd~1&#k=+c_k!m
zsG|Jb#wNAWrBRY;PlcXG27^_;akcv?IXM)t`KmABs>hYHb@O#VVRBy{%e~$BsI&rg
zNq*~F<Gu&l0!U!VHz(ri@)Z_kw4Vechtv&SqX}1SMCww^KL0CMuKe(>4F)`<p`r0*
z&0AYf&$9*b<e#6P*V5JPuQ1ls<W=6igBm_R>H8%)hlT9YYs(ZD2j}<g)?9`hE}d)p
zypg)Bq*3{(`$g<ldXKhbV=qCTMuM>XF>xrXb!S6Mi^>{qxO~tPTj#4i_*PvKMS%=r
zBO(*3yMN04eXryMwf<gq*vEOAkI&bz+I<aE<Hb}p6`}!xKxEKzfQXCfc4=wp^_cOB
zFh+kc5QHA(iugbQKCXQG_U*Ph3w@oZQ@H055h;s{i$C!WSW~StE2sGR(=Yi&_khX?
zS|rG?fQ#aVPt5)?nPtD(&O-(T<6H*Sc%!v>x5}RB^Y2y({(q>5MewGTnU4O?FGba*
zPk@0U?Y`IvURj)PZdR5GsGuAMntvrB-m=*S+}$QopJ7rv8N9gNdd+v=x&Oc%fp$MF
z20OdO{&rP0HElPy(lYzu^g=kB;bjh^S@ZKY6rZ5D71X)eyH#8Ex5C52&$-QOwY?No
zguSQu9r2Pb=RkwzXYoJI&p-cx1!XrvUM%i_*h^24_D~6@ghWwM(cbM5Zjkui%>4lP
zlIZpIXfV5^1_(3oNx2%_t%>r=+5ohqZEQSRLHf}uV!JxqRW-T2<b1~E>$PA`83TZ%
zdu(tBg1lue%J3h4t($M+4UUAmbkk-Fhni!}_p76xQ^5&(Oz;+~yB~ljAV>)(oZo*x
zD-W{9f7OzF_7|xja)l<qf2neYld{a<+RgXpCt0+`ruh=_wE0%>_{SAINP0#F<=k&<
zY_xQA=z?B!_Uzf~lJ#X}I>Y6T{Uic5A5M+!)W_hVV$0CHs`)H#9YzEhe?~ERO{(2-
z8fbJKz;m}|z8Ix&oqUtr-d-&nC;%X!+m?m)FGOLN9na#kQ6v@9rVwo}uS&W-NA^b!
z0g8?9oTj;T^X7AqAWhdJQX?Y|d)1nNS6p@kG1M?N&Mhc7li-UV%!vOx-he+fEzj*_
z!5H}F7vb?wx!uE5-M$YVYi^#liN_7w)i`47{%{A@!Knaakvc6SLy({UYvjs58UzbZ
zmoC<Mr}m!vm56*K+uGVD2)}hc#)i1KxESs+MB&?|mzBaHv&Q<i73DBD)eG#WxBB||
zrIgo?)MybDdZ_Y0F$t{o15@2UX#{_0)0C^Eg5|H;{!;zKp2X&h;z)av97@!({Q@pN
zFmMNMpzsnHm5%sQuqxrGCAe?Pb0;4awXj+qtMw+W_1BBP2is5J?AfBhil!;&Q4vMR
zco^2dpTQ`m<0btda)ga7dthK7KOrVImU5`=hs)GpOQMe3k5*}e0mCZcFmj4uOYi4L
zT0pIRWOIDuu{_41BMnFC<id~g0A&@G?%qsIG0S##CKi_LiV8iKub;bC=X&Cdknk5T
z-mswV-3)h~e7_4Iq+Dn9a$UA%qSsX*Z?OJaupc2@a!Xo*5OoS&(=^G)WmZ0mQanuH
zS@5|U%$k;w=$+LZKU(dPT~%cezS?{uX?8jWg*lOQ@AXsRDkaFSCR0i8-d#NL(;18n
zlc6191_!AdEDYZvNf3SOt}7{76+v{~ItWJQ|08AsaAetgdQi5ZhQg=i)2Ghoyo%SW
z)g2u2z?@~`;K+4pmsA9&6$9?+1I;rC;{V{UEC1F?NB<%93$F-cKI(>Lb~i6xybs$0
zQu2K*F}PC{q&s;Z+)$}1Bn(RC=Zi7oQ8uXcE(Pzg)Q8}}$R?iifdoEAoSle%(Pdjq
zPFDzr^y=s`y1ZwB4m@U+zpVJ{=1rko{opqP1_0rk1xz<EHvk2&QcSnw$^r870rJTl
z#(|0djgx-?SLe6h$aM}1NFN3>U$fF40hT)yyHhhFCN+9bnT2e`O>W#se)Hx{84H&7
zm-_#Ynt{<Lx&|=YnHw=NwZMYIQ^9MnKoy_P{_tHS4is&WCMbGDu1T$dsKw__SZu>R
zk)SI56AbWm!C+qFpJISn>>~82G_?Kn8}#9g8!I!mkX$knBEYoakaoA)CX?FgbhNe8
z!2Il4;)3lxcIlDTr?6ux%F129;Y%&rSf{^yDLF59_N>P9=g-?d%5a7*pe3P2rW|wT
zSMdVBT!wDH4DW!f(32QqANH4d!6*)$ZmzE;I|ZuI_*G-F_!(to<ya}VEErkJ<m4U5
z2e$3;lCiSB-VqG)5Or&iY*oTx<I{bXYjq)WZO*2ImiQ9g%`GhEBh+QP#QKe<{u{~Q
zFehi;KG}77>^7O;2G)ixSY8m5-C#3_-CKA1>x{6}mGW5JVPs&?912e0@&9RQfnwL?
zeJ(n!tO30xjZv;OP#(Yy>+9|P^iR#`b)V{qW#>Z&kl~5hu9Wap=*@X72rFanwmN$g
zs&7WC)8!ZDcem^_AYa%3Ql1|y&VW)EFPw1+9!l}ixcs&XK^XfJwf-5ghS6!z;gf}`
zN6%y0KC3r6?<n8^>o@&pJJ;2WC(w;GDuOor>5ctIy*4CDtbIb!h?<H(YIzHU=x00L
z>njkp-s1r~4<9``=XxsS*P!hodPoaX!-&u9QT%J4_?WWPz4ZbBGLT88*Svuvz2a32
ziFZ#L88PDH=NFKYGMI^X3E@{y%*iz@r^^_>-iZ;T-$7k9ljw8q?iVF!Ys!XsQ2bE<
z5iB*Yr8{5N?1VUab4oM)q`0^afP4W#K?apv4)cv4)2!#L!oVK`Pu|RatY)KR1?#;a
z1R7qpVkjr;1<wMVJfowjD$<IEoB7cA;_Y4ah><PbVYCXIIzi8e?yt*o-hzt{0%N+Y
z?<O?6UCqqQ^j_=JeV}LGWUIOjX}Ak|vihn$9249y`YPA1^_qstFu%9ydQR^jb{qiB
ze+F>X9UC)A`6m)RFD};-63grcUjv-jxF@TvtxcObAJT^rdy|{n2a|K#CJ}5wm>a5a
zPJzCyl<d4Jf#0<~;zhmou}iyOgZ7R7#(i?^sRGj1O-vF2!<RTsT!f+8fT|oTZXesy
z+G-eI@3CxRjNX`zJ#+iMimIv`ymKBu?AXsBQnUX{i}Y1PVt#hEW|FKg<z{2giH8b#
zWiBOdjALk`Jv2^Gghhc<r@B&=Bfxgi%_=E5_bJNbSs9*^-5>!j;L5QWio%KYCCt?k
zmwKQ^+&7Yx3Z$LFja2r8n?e4O2!S31{qP*f63Km7keP?V%}8_Ve)G@HN^2|Tf|D?O
zS{b9r<=cDI+&vbLrwmy&9NhGwAZ_;?AUg)2o7$g^e(}OeNUtFI!-faew>;{_ivhF6
zXhVlWV5U;JAKQB|<a)cw_dysAkC86rC|Y$NOfq<@Mz@3vO5UWXGCT>H-G&C7GBlJN
zcUNnA>Ja4OAM}>OkXf7lB)F6&7;xEh4N6r^^2=2@Ic9$s3rhu51}$%Mz?_x>1*Fv#
z*Ra~`a%dw^-+ye}qge6SQ$!&F0UL3@mdI1y(0;=N9(*pQ-HD%6t)`gs7Wzmi2YGG2
zFI!Y)QH!I?54wK9y%%$yeBXZZKmrI#!1l8M6SsYYJ<o=c)w4u=EVreAUH>RSX&I!n
z1^ytCo&*Quax|pXDANO6_X_0wlQJ^3tup6pc{i6;_Wr$%FKu2Q6%{4aF6TTM2wx3Y
zm%2X^ox|Eo-C^YPUA+la>l#$V*=Gl(wS6|1vGw&JhKpHzJ~HaPkPDr6i=Q>Rl<ED2
zqvPVTp{Dkby<ld1KZ!KH{MYgd%|mZ8H8X5JK07J}7Iem{3;+E>x^Q2GPr!-Ypr=R(
zRaetUJg@n7zCL@?#u3%;sJddvx-2j>RL_<$$7(lFpg?K%Yi+gt?w45gM0Cy%+9N6J
zSbBF_MaZ~Hi%r~a_Vd9mWr{&5)v>j)A_f?@r(Ib|sS}D>iQhKa0+pSWWrO#7jG`c`
ze_ndC@gWDFQSwRbZ>Ep%OQYHSC_kCh5HfAbb{-{rOiWC)g}AbxcN&i*C4#3X)DCdI
z+8a9H{<d;M#dOf}LFv6p^dozzC0?i$ZCyNvoAc{TRtQf8d}jkNfYP4?{gnOIrLSG~
zBgUu2#mlraOf@w%fj{R0y0=~&C=5@91eXPP8VH<iPe;jVRcN5J)N-_(T@<tW-%vl$
zXus2>OefRj%a<v=W$1d8F3Q(!ssg**PpBi*{<I({tzF$_Ie*v?&zq*^dcCEGUal^S
zTUOD*x^=2ajHUwmn|&gfM}IXl;dROw%_}_iUP*waZW4FNwFId?Rxz7MXazK*oK(oi
zCF$x(V`l9Iw;OXnU@(X_*9SYaGAbg%l=1^E^!h7smhQAA$_zhw_N-*gZ`=2TfmL}~
zo&xv&0wAIS*{~ftKv==C=vvRn%WDr}9!kq48Ah9PNV$Fj%d3>U-Xv!bo&#CEo$_A+
z0fGC*=r|d$W}=&>3ZdW0l#)N-6coKqNy&xI!_K19tp)|R0-$1ao!{o#UVjWjF(37s
zNx(oSY_6CsMi1INHUZ7fXffcrF=V0EB1m|--Q603l9Gce>gwf`|JH9!i(HSy9viBn
zqcmjofeT~{{gP^EY$&_r<R5n?c~0(H0De;jS=SUdSlnb#?VihY=#Z(9QAKAabLZY2
z@_Nzc+gH#)^9*Ht@Fm{(MQdX}(i#R-zqrrO&15#dM8KM1<hD1|7br;w+PEyQT)wOa
zL@Cj&Pg_@ixXhjb{hcE<*S6DXAtIVw^Zg^6LVkTM90#}$VA$ZD?R^)%$=|S*(`=0v
zDu&)nE6M|~U>W3;k9lW*x0M)idO*n@{43adD{2%yM(6V7z^JgWThP<X^NGnLrwotn
zu3reC*-2MEyB8BJ_Ts$rou{@=jf@gI>c8#njwS<OaJF6lHld@X6-k*N=r}_L(8~x7
ze9Q9FR{3XIH|y1ZqXCJA-iUS&khg#5Cj5<U>jPU-j$Do5fMpQnE7vdQUA%Swe*rc|
BdkO#m

literal 0
HcmV?d00001

diff --git a/nav2_smac_planner/lattice_primitives/visualizations/26.56505117707799.png b/nav2_smac_planner/lattice_primitives/visualizations/26.56505117707799.png
new file mode 100644
index 0000000000000000000000000000000000000000..f66c187cb6a6fb9630605d42726fd530936247e5
GIT binary patch
literal 15826
zcmeHuXH-+&x^4gk3y2L6P!JUh2#7S55-dm)Dbf*8kY1&?5JUl$rU=q|2O;#H1my#y
z2I)N@BE2Y(Kthr`7vHzf-uvFY&$wsYG42@mjQb-a$y#&Gw&r}^=Y8L2zR`H7$a0A5
z5DW%mQC7OA1%uHCz+iM!2N=N>iEraSz}HPT1%0<iPL^(-PhBivYERvq?Va50ZOqPk
zSh%>_I5~<5OA3o!J!|de=InY)M8x4g3J5#7Sc%}aFk7IKgU(6@t}qzuQ|LpN2hX*E
z!DQje_ik%@C9O<&d+7|ITYgDB6TfosiBj}&MvIe2ZEED#1)3A40xBkT?}^)rXk0N<
za1=aS8R*bF_BmbeMc@rVEw<dZHfB1n?#Q3La_+zprIW`GTt4_N>U5MlO`ql6i+A!a
z9%Vzs4Vk&=q-2DPKXxF?+t?R{z8fZbA9Tt2z98x__?CA*2BU+)PTz&?0~eX;j>2G#
zqWk2*oz|oO|Hl8xlsq-z=!w$N*0vrhHnyFptGsjPPJ=KL4A%cr^gh#((;6X1&h!_=
zB)UG`*VtELHzdr@&+lyvR^E;Mrv5wGw>*~T-8oav!cu|<i;Psu;PR!Vr966lHS{k6
zu8WDqeEgWRzV3|bICyJ_9D9m^<XMk=Bf9QNYi@2<QBk=?@*GzH^=fzzOG!!&#?oE8
zb;}S`fFU=dSpVSx=X7MOI^JcYQ3?hNXgUx8UV8)|j2?6%7<K>#n>s~*7ThrZuihB&
zqt?1Lv84Mfmtss}>BZk%gVtJF?Uk$1)ZekXDoQsxmb3ycer5)>51hVfMq@cFHpxye
zehg}EU>`jJuUm8=VExkG-_wb7pVq66roDmIV_q<){Pr2JN&{@;=w9KWwVRi79TpsX
zo#6oT9O$_QOGv=9C$xYIrjK6RjR3=tmxvJyI87gHx>x$=@Xdfu*q2mjOxjJ%Xa6el
zi9Yzx+Jf&ku1oJV?0n#qzarQcX<SF>n%4HpKf1~A)g3miw^x6D65Wr7XV(S)xp0Dc
zsQit6%*fw5uq0{E;yRC&g1o%T{Fg4-HNR!MH10Xu9#gme<*@tDc&o)Qv2G2N33_wt
zB`ag8Axz%28T3cEbBUg-BZ^;D+@k$tSF*Gohly7@b$1TF^t(f3=x0~rK>nj#E^_N1
ze^~$i(RPcryTNwiL}rYj-r`3iN6iNh!u)@KNcHNW1M~gl*n!<{DX{=nne~8{aPHh+
zGp&W5Ym+|n=^@e4(OKRbKL_e)0dx#*6YfL#>Zx*zz1g9THPejYr$jTl4er7Mxb^qZ
zV>g<apBNa#528#_3w^nIBPn3*=@jY?EcyQU@xywcK!?=j<nQ_gp`xm)V`%8J>HhqA
z>4hst4_wyRNez|L9Erq}3Pk?4=x7u5clj%|+%}I{wKn454F1dc>3+vx#MOzy-@je>
zFJ9CgNr{Wg3k(drg&!+5S7c*nU;O(1e1e4C$CgN5<QK$TXME;Jxz*5cN_>3&{QUeQ
zK}>u{d;9OYed@K7*gsD7U=u0G_0RlKhJsfX0?N%b+9D~j(a~PUfpef8!Jt>Kw7ng|
z@=J^A*aUUwr#`=I@9rMlT<9ApGLka(oN%vPl{Rl-R!hFMHNW)zyBXM(SA>Mrz~0Hl
z%ZnX9urE3$CcD31J18W?*Z5)2Urp9iVn3?v-%r4q=RRagI?d3Ahcm<|u0Q@63OmKi
zi|Xm=;VMV?5|;Bt1@(;q9`GwuNLziY6ZH~ygP%??Hh%r8q;UWK;4lvl&uYDCgo^)a
zk=)}oQ#0myJemrp>I;a(e*YdeS|z*u<J4(fB!yg*`e-^-;q0+v$5b9ZEEZKu2E9tZ
z8!|{HpE55<nqh)D;}m&EkT_}F`~{}>0qh+dVgXU&?u**lxw&>zjW1M`mA?h<KO7w!
zoBQU(C6%%eC(qUK{M=N3!>G;q%*cXAxepfy3S%$$8aKjsr&;S(s>WoN=wMT5bGn|f
zTLC#g5@B8|l|#0845E%W=^K-piozhswj*U0i{rKB7p}nJrrzG(ibPaiUQZaOSOtRY
z5fl`(Fz$*Qpb+sWBgfk2LUtIetMQjuK!EAaMVLx&mhuO2o8Bx#EaArobzN`7m_xPb
z7!xe->4qiQX>Ibpg~j9`y*Qg`5(C)M=9h(H<S6}eR*Xme?}I%$g|Bj~6bu-vc;78I
zwqzTS1KG1Fe1s|N%b_RRoDg6j3uS01bzY|>SOV9d|C8u0z`pEl>|JiUqs3<de55T3
z2B3@fTiZfA{Fxq1PrHQvl;hr=f)Jzu9W(YkD0Q}*_R<Ifg!y6qCpT!U!cc*;mau7d
zfCY`-A*{ScbPr&!4nsxebc$h)W4gDNuA&O*5_mw%Qw`&f$s$Bpu;AW(^kx)=7^T`!
z#BgBmaT_v0nUj;S<38kVbol61(2;;X6MutSF8AuD=<_|DKsA9c0jxQXZs5j{tr1&Q
zN6yAUHCZ9HVm7H{!cOIo4|wtc^kn(RtM9|!3)d+v_P2p)nS#JQsnV;I65Gt{%mQ^L
z%V8{QYbm?r+CqX|Lowq4VQ2`-G3Chod@a9*>PpyWXnt?Bp3<d#ae%6Hb#Yndo65Vq
zK*kg6xXy0x4MMw{8qURE>A2z`6u<#p$<@|Y5@~pnT2A69hHyEl%q*zFHTNXPC1d~E
zp#j(=G-XLzwIpH)Jo?;uFz4%spzef~U0-PCSaAidSwO9cT^GwnVLx`?QYEc=qKg^V
zkL;CdZ^90l2nGd%fll#5Jw6z6L>Yy#-D?keMM>)|+*-<S+`4b}rrkEtWAF-vrtHpa
zb1JI-;h}@Ss@NOtp!(MkJK53ipF+>A8~kg;Ug*tMQB%_)6IR%`xeKGCPf}K#kcZ7G
zVF6Ee22fe@@^#y03k@4!^43$F=%@aM+(|bd-Mo3TZayQ_ZD%%CP(%bZNWmj%i0qsk
zXrCBj{n5d!yv2pZbt<6T?c&q0o@lvU4WGp;<Xs|`SjC=JyRHWFn|i?{?l^uUoEL6_
z_9YJ9WtGj!%DT{%C<UtH^_l0J@SaO1Mbfre-Pb0Ki4&fwb)IX*Qj=cizs4}Y5_&6a
zqC|TohDx>>@=pGArwdR2^y$IU5NZe@UZK6Zs%l?>PT}H~tbl-k+f*Z6rQOi#D{}P5
zj|CMKn|t`DtabLN#@VAcg4z>N?f$!idWgRu4^Bc7Cyt`GbmP8!3A4n>Wdv8u#ERa~
z@n6M9%1T6w&`(XGeRWJsivRfI4}q~0Cr;>SXlS6;ezn4>nt=NB&VS2PufZkVbQK4<
z$TcP=CdPK=%tT3s(d$p2uA)@eqn9YEVRV%0ajKv|mg%lpT$Uu4s9AlL16gCNN@AB>
z@k7*_*?$?EmG#HcahbVLnEsWl(K>(g2jLEt1`S@NmR$+iLngjz2M->+H15<A#+h)-
zeev7SpZrM6&@jQw+*}>KZh09PDnUWj{QUewtgOVkq%>DFU{Yzck9gW)kyrEOxG(*6
zl>jT1ZSivXx7UbGr;?7qiKgEBa2A*Y0l$9zI>f?KBsx}Yg-hWo4;&COD7!b)65bao
zg2_`AL05Dm&YwTe#>r`5?(5}Mv*v$OV5hd4<os6~BS@QkH=HbCr^1GBv1hv9*E-+4
zM}K!1iznS=!|3lje(YE}FtS{I{RRdGT({ij58|u6ewj`9ZFKoicjnw@WAr55<6!}3
z<E7L`F~+yZ_BQP&A)Mhg?F5k8pZ_Q(C8hWU1Cszhzrtbms~RRIDHk6mU2fySOw_vO
zrudU=lqGj3yk^3Ewx>fRo_&3nSsr4Q^gQ_c_wO=G3@~RO!dk39#g4o?>6;yQO1*<F
zfgiuG(GG>y1>2zyO`I&;-SyjA8Pn_dDD-HqJDFd3XJ<#J_~7;&oMxaHar#?P^<;sO
z6MAc&kmy*atS-0ntfYepCLTqjm@X`#R`#blVMYKj{cMw>(}=pLmH@24#Q{>~V)`nm
z+>R6ogWcYmjqSb5Ix<JZiv;Z~qE7dC`8_{!76ywl-hRGaf}S1w`W|d>aZ_kJi}-D)
zYkQZ>5wWq0&)Kdu8th$k?vP5O0#piMUrYAPs-7&H+XWj1pimbtK$D&f9c?AgNt;yl
zq|5G0KOJP{Rpnv(U=`NLozMz@G<f}P#0k(s1v98jK2*n0V8T5XA?J{w-MTVPcRB<B
zi*;$uEVr}mUK2Jn{{<;*DXw;wyXV^86K+Gsy%@y!Z<ZTud;iOyy^-A&95KQ^2mN}w
z_t5N2WcII_m5ut<D6L26A|~rMdzWW=_!bo5QAc>9gIQxOafG^aX%-Ng8tyX(6O|Py
z7X^UPhw6QVLZAB<>c=s5p{NvpL)!rrpXvSF^mIvF036dsI@;b}RIkFV)vI$!c8>W3
ze1v$h^&oh;#kDPyTjoEM7C5l$p!Qh^I}?P=vv_Ixq-C*uSF45m+-1Heu-p*S^Dz1H
zt*Mv2g(#I{$9h#olvhT*jgL*SLSri5j6^E+<&4a+%3WqTJ?|hJkP6ZDu7^+4-Bv1l
z-=FVwA5w->@`u>ysCU5RiVM!p_HGTSN_(oHeUuNgEAjD}=<VM=484v|yQCp4-H#{F
zIN-?S!Rm(M1M9qdqxjkxy^0=^ONE;*_O*`rz_8F%Bm|)_Wg-6I@BM$DxFS2T^cF^g
zF3WHn7*^o*C8{tmDzU<s-4z0`5dBX{lruK0RCya(K1WedfV%Glx*w4HQ8<Rjt5FX5
zTX`YLRYr)(nI1AT@#^r(YM&2TskFzW&#cg8M(FaJGtyZV;`ufDBpfYzwH^oB^n^xx
zyBbk1ZXZV>j=N^_t7ZZBtdM^EHW=-`0jt!J^=k8wesq0>Uu}Q-%OM>)N5~cjT8>F_
zQ!%@}lRq7HTo%UgggSvnS!hNz{B}z>C)P%NcT+r5HYa_1N348}*`_W+GtuTSY2Re(
zkLgFHZm!mAnPq_LR`({OKSxb+cV$v&YfS{b5fS}v%3UX53|dMTH*iRmrV!Pjpk7am
zd6TJM19)crU;8LhfW4WwM(E&DWC6PuRExjP?-fKJz#wV=PPq1uNxbKj{K86gw#Q0C
z@u7epnXsB?ZO1_&1u?5`?S~H|gzM(d8T;?dws&+Cy3=}lwJM$PCeS?^mGbhj5Nvy_
z&b^n2xrCrpihGqr$M9D=EV=8z1ON+^TW@6Gl5$J}R38P*$5MI-kCv8J6mWX<mZ-*c
zZr_?%;h(sj8Z@6je}21|9hQK7`}UD9K0%^4J645ERJ|Lb5+ARXC>d_R8PLb}@9Z7d
zitGR4s~kNbJ~}7P6hqlALXlh`cy>n9*aX|RHd0w1Me->2XDU+g)jsa8??*5XykOu1
zKHMQR)u*=m#$DStP4gsNKq{GNuFf(W3ajy4Gl_|f6)YW<*nfP_?)6xwg7Kg3N|CK^
zA#`>=P*7Bq0<B#Fpl%X7b!E>t;vnO2k%JXB9ECMC;f`2Dh-1A_waFTJKGTD=g?i++
z?fjZV5bFQYW>;aR;N8d0y-%3R4EX~8%DGu_>x>h<3rM^ysjj9*Us+ih4ET|~y?rL)
zq3hAVJ2BE;FCXr;u1BVk>=wR<HfcXTo#Bd`KsASs6{;l)J|tmgJMM0d;e!{~r;i%D
zC<lEMcF3HcKXyB>W}${8rqPC@=<$4ZjTfPR;^)_J=k8s*PQuDad2Y+6sfE{JVcO50
zJqur%o}R{#XY+o{O)fTVQTbaMv#F}8+21l(Rz{<u!YsQxbrU7=(l#_CtzO(VdJ~0I
zwy^;)Q`O69wf*y?Ypy?|?My3`GKkAxZ+5c2ckflyhYxlS9$8sf*v$8&^XTk{35AIE
zzB<@TUQ-3Z5){Vg-pXi9$|fQ{zW3{U$z5ZQ_{~u}P}{zAfMm`8yS@-<-q*#|m0y{O
zl}G#O({%8g4cOz2ZfRU7JFHQRI4LN*5q6SG05Ev8@mQ?sASES?d4UODA{L@&as|Cx
zrrmM-IIQa(Acd7fCWQzxVPVpDO~>CKLn5pcT6BEOcb)5wS`JDrG(F+m7(l0(iZaET
z;NY}|Tb`?*L@?Xu;<6b6a#OtTCrEV2rApy=T4YDgwMpTlsY}=_K$OY&*pFBFcIkxP
zIMJtFP)iU8##60e%MLQL;SMcv7mE+T5~_)lA6ml8ak*;cAupD+H2Emgti8sb%CobQ
z`-DxdD%=zI&WCZmM?t_sm80s{k!+>MJi8=%5ySyB4TI=alNuR;E32r-0^4#UmB-rK
zuk^q^SkJYfAVLG0Le7pp32a1`uKTXS*Y1yu%q7zz*R>MhcudGTQw8LNICo;H7Pv9;
zt}DRSMUz^)Fc9$QRp1y=;G3vZF4R-49zQ4X-9ndcA7@AaT_YbG8{yjO;(*pzmB)P)
zf;>u;^}6;LPSVVq^lHLjesV38wq8gzCh|BR2Vi{qx@*qn_fSI5iXch}?K6PE*SYfn
zsuxFCw#j~dVBrN5FqCmy47-7M8(~wggAQkt*Q#+GLQ%mig|(5Qy);UyxV?TJVfl^b
zQ1SgjUFGUYm2HOISDc{V@|HQ9eW{~!#Bp=IbzME}3*o#@BKw4btiQf9_MvPMZ1E{E
zJOOwB{f%>y4pIJ_=Y1zI+>Rx5C3P&^%ZLCtxs!(nL!-JRyLL0#_WgW2#lf7`_WZyp
znBP^nY2Eij3ypMO&#jz`m|>meG8b8hiZW|5o$KyxkLh)&)^YyIu`UYU%65PH-CiBN
zk&f!k`KKI+)5!WN2Bxf1^Of}Jvg`G0!7RYTnH*p~djsJ7)Lrme$BRrwnp;&nJ^~AU
zhD>S+?`?e1dssv%J)J;s1k}x)7e;Nq2?~p~DA6|Hy-TPuPT)>`=$0i84}q8LSg&@=
zPk(V()3EFw*RB4s1iF13V5<FoOx^ZHW2{n2QK|kU{TgYn9-dVM!GKKq-LWz>GlR}&
z3-E;caDrQEG^W9*dX!9(+kA%*QszZG)1yc3mdgGhC*lFsG9XfrblXs!hBwTY6;hnA
z6Fh?yLUnd?=nBP`f1&Nt%BLwTmEV2-365O2%LSAAh4+W`1hR3AHS<)Dn>FFt^pt9A
zC8rRi{LPoVN^oi^3)03$>*0}URv1+4r=-(YZjN-%2L&l($;q27+#b`dn+sa0avpR_
z)_HDg=w2fyW#AalFifl}>hLoe@nRytboCMG&TfO?9ZV=pw0FD~@Mi)Ek$h|9=nbzD
z<9<=|U~P>jL^ah9JboIt)*9-tRtiCoWqzbSV)QvC`jztAp>o_uS7Z6c5HJlrfy;QO
zwITBF4#HNRX734grLijIyTMAleov}BmoCb#2Z+05EjDDf=BAGJ5I6g@OeRZLCh9m@
zcp4O{@sq|{){iO~8qb3bWX&hHEni14v>|)=VkhyG?&4)Mg<u}*VKZS^M|NDWf*1M>
zMb5CXDFepJ;!oRdEnRv3yf;o1Y+dEHHpR_Bqtv`dxvOgUQvqp!;K_?wg>9}k5z2&K
z1oT%HCYKsv9xskO61Y-Y?gyq4puLJ!vd)HYbgZfq*5NK*5H-C2d=#3w^Yt;wP2lDy
zKM$VZIj|1}hkXX|(E!WkC6q&nFU6fq!csemm(|o`G234pxo`C|{_%pr4ukAq=OzIV
z8?9U#CTw(naKsqYQGI-EPQf4wBrjI!Qz?5PG4_(ez2UKX3oBKrNoy>1#Yq)QLr=_Y
zZ|q{Iqr!*07q8f?jGB*)RhGwwm4<jN$w?jEqjYKU`FZcC+lmXt^;zc?m-+B77_xzL
zL9PT53J&bM9JLQ-X3M@2r{7<{Za)t?S$|8vEH<F_`yl{wvoV+CM6T%z2xKuZEqlL?
ziTwV3)Rgze6ISKc<`G5v<<hxIhr=@>TpCWhbuf9cGEY=;x=~dphT83!g_HYW;`a^0
zqx$VkRSvSx%zZFQ--n}U@-T4$j}OL_nlE{-U`dw+rJw6de_L7{J964O$<X%18Dp^v
zDa-C#4w{;sVP9CF7kp9o)4SsOhoiN9#lOQNV<qiNOqcNZlsPH;IBk9FRQOhO!Q;GI
zgzrKyw|&4e9{V&Ejl@>ieAt{0A(T28<#{aE=wr7PhQGn|YMrx0@7+t1$*Xj9SbO49
z_QXKT3~QzQ2BB@j@Jstu2yY?3q~rrdz+zNIf(nqxd{a322HF13IYT*sHC~@G?$iMX
zMh{z7=#JeI@|B9%_2Z^Tl8x%;&!2A+hIwzO`F_<adtuRknVTeB--#(!HHhE*)!G_p
zGCR{DQn)svqBUlSRebP(V%fKN$Wv1@_oj90!F>@ypM$%Vc;I3-VH6WGVI<__xqP&w
z!!3{aGi@oV2Aqt7ya4JO4^^B5^ReEOBI_|IEZBi@9B2>5g4oocQ8wTM9EgW11<d?|
zjV}mC*TC|Qw;J5Vv?xh?^fUVyRr4H<9N_048Tc&g3&NX<n>%xf;!osF`7Z|Nn_|tU
zi)xeYICvae)2g25q23;nUTqEe13`|MM-yEJ>b77Glcz~*@tXBtEq7eVH%wxMC0;&_
z<hjH;8^Dn;f)a(*m^U%j%g=1P62Wrm*Io!byo~nc_1_*I8m*2ku50h!Om_b$D^ur^
zEqh_N?Gl0ho3}T?MWo)7*XV$jv2~s?vlizA0fFVtp;*(=Ry_6M2%MGUGy>;|Ij@%R
zj8#*r8a8~&u(E6#<ZBq8Dj(VxCLrLk_*Rf4C^J6EGQ0lN*Cu)Kv(J#QuR)3kri5r-
zeDWCI)}oHsDUao^PoH^pgkW1>J=fiqFC#X(N{1%u$H)YBb`=^0@4>$21!(_F<XG#%
zWf8c?<oKHrR;K}#>lg2FOAZ_LO!yR{=hkENMmLyG9AC~v_;P#f3Ja)kU2Bw!sCF_P
zaJ?+$Sb<{YslZ7&eyT$zN)k$b?ab-PhMnf%sL-9d0WF&ge7Cuzx{IeMARsp8SkAYY
zpKBY8{qQENV(w@2AH(mCbJ|#u+S~1irQN;;zbA?ODCv8|#->yK@uSKb)`@qZ(!uxY
z(Tf+9-n_Zwe}-cY#aSYTt2pcy%pzS?l=1zKkZIytb$PX*o-Rc$`PM*^(DF!MxO4av
z@Oc74sOwA%DZchMqO#nVmeU7Mym?&`E1l2m8FFMKP1Dep*A$8PjSG97C!2N#TjVen
zAZhQVP9d`n4Vi}6;COuXy}P)4)xRf8+xg?2Fr|@2M(QSay|&xzl$1(B>Wmf(#&nC8
zB~I6SR33I0i1|2(IXYK}TISnrkg4y<)q0lY*2Yipee52ChTU+kgUnz0yfrzmzt(jk
z#Hyp+7G8}qu0Mfm(805F`~H?^JD{|)!_Rf2r@!v?Q?iH6XL>+qmMahQbE}A8hPwkR
z?W|Z0)<!Zjr%Qi(KQE<Fr9B6EE)*_}kEC$KCkP<cd~>UZGjF~Qs|djrnik9@Iu-ET
zFZMghF}86V@1sguP+!@J%e+6*x}T$>)0ue<IK63{Huy=#<k^muWj*5%ZNcj1RwiV&
z%}{Y~(oGM-@9fyDTGy4LrAfq)9EngV77u`=t0VKYoPE>uuk@0tH2oc))+5~FyuO{m
z$v=LWzi^({SX*=K)gP^j5w80xC%}0gxT72&8>n??wy9tif~eg`*~ra;!e#UA^%iu)
zk=<|*Z0PFbc2WZT-q>LbmC6#><^J)KK)XrH&tUFlh5Wg$@;PzG(Ps=($)2OW9JI|`
z$&<R6OIH?#ZO7_P-+G3V>TEBIR7+fb`rCcU9u^)^RUQGt(}oHCx#}Bd3LhOiZsxn#
zr<fo&5&T^WU-9&sSRapMiWE-H%q;8JdHYihzg5V*CmP>W{H}w+<aOS^C&)FPzpr%W
z)-e1B@QLB%_zLIr<o&n2dX@z2z`EyQpRDaO!Q@z}8_kW^N`5`UeNo83<$Fh8|HX^<
z>vmlLt6ZM$NtRwY7Z2#(=bEfPOwN*#)K>qNYODjpoEUESunk%5PT=MYuPUm+Opgb<
zlOz5-gL3nxo04H&zrU=gCI}r=5F;ncvZq7eP9ZxXJJ?^1LU-c<8-Q=Fp{;mZmqsfI
zhgp3Q$rwoodwukZr0<RGZ;BwODP84rLsT>2+HF#s`l8ffuZAOM>>KNU-RHIifGR0n
z<5$aqz{!~NMxAM`5SPB|^{ZJ2S8czLk+JN40af!Qc$tMe;*3Cd+k5ISR&t!2@#0`t
zH**9Rzp)n|M{Y3A5(WZ!g~i78ASkTT*4AcNYNn7(7BR&<27lhZecO9;E-7<oV~#0b
z3zj!ljvF1QU}7C@N>-QQQ%kme*=b-kf~az;MlJ)Z;j8#g4zV0IvY%tvsFkwn=`r1?
zXX-7+aPqjB9tyKthL-=0^0%<Vqm4ikaXDG67-Q%|UP*{e%wHp}4dEg>jGRUkPJ(sA
zBGcAJSW1)}8ENVesh%4)c6lS~*5EjikFa#r@pk<2$w2bJ&)E?TuMrE60V0@0y9Y;j
z9j%U`u#?>_>}=k~H)YKIz6WubTm#Oi<@A>z%Z;C1KM7+_Qlg@wmaP$IDpzScbE%2*
z^VY89mFh$gU{4G>%#i?+>>D7~H9!^%efu`-!Xs_%etidVZVL;Gp96ZP65!h;_WQvh
zvQ)6B^(uvfoeQ{!Ei$_$qBE`BVqPVn@|NK|y^U{GMWP(W&pNY|TRKynI^oo(4~%I;
z1KO0>7|xAY?5T~3HQgeVBC9!%Ur!tg>)3!3xGHYhc3c{QyVD;$R>C-CN8^GHhwpq3
z9kq3sgfndEJht!b5aD+1Q)~I7RqxEpl56G|qAb}J*gYNmTrAaYW)6$rnoWe=u<ki4
z_WV1D!uxD3TI8swnk-R0C%tnQ1`73J9ID4%LOHKLQ$oZ0aBz8l>YhLV8qFI@Y%KT2
zh*e7isX!D<tz?lx`*NS)^7;N`xVBvMydsyFd9WxB?$jB=JNk}2j0z`xjcfZ5ZZkpB
zm$gcI+FA3w<&!DCCRIoCY3E>3i_GuWqIeocw6s*BHN)4F`<<zGa~y?i^b^<E1|$HT
z?J+G65-pLE1(CP$7=qQ)EnD*=yaZEP%<e9+9&;zmW+ylJV@%8yAY=fZaYiwQw3s>O
zgr#`Q{`_g{y(R6TTDSc@RN_e^OxABaT+-BgmMxq|))3@x520rFy#Zk;pqb!77(age
z3FJ>!ChGM-PB(l!rWyz;%F4>36A}!U^tY3ttiy{`l88{{VLC~NL*H`L_(=2T$!p!o
zl_Ie$tWt~Ln&vyjJQuCtb|}rvJBbyTmrTHIF|wI#&>gK<&{uoOs574nFY4JElA@;O
zpI6Lcl{@fl%xSE3w31Ii1Wp)9Prpz5#mlxhzH_206|XERDnz!Lx+h1`_SxvtB`ulW
zt#Gjz5Vgin&hKt)i2Hg3GP3+0ih2QZ(ao)`g?V{jJ(1X@4h~u<NFVFX&d#cQYKW#{
zLFT+5EiG-3c=F`Q{yN(G^NJN943&%^j7UuWbL@${(D{AlnT3V7abWIVj(N8qS?}S&
z_eV3w*bozIuw84!FcNMEzVv5jn+vioJmy85Ja<LexZXdjq(nWjp~?wq1VU2TLPnzt
zMb&k-TZ=~w>#JhgJBWD>V-jkFTCh`}&uhd<I=mf%(?(2cYYzhIZ`9s>WFR-yqt|!M
zVrQ}U`hfeI@Wk^F@uzoc22tugOuR|iEZoO+^iLH7-e1nhYLbcewWAr0YQ-e&S`K6W
zO7J-4iRQFaeONIaBdCa3cJ_b_M<k{=iSB`_>c#}}_O=aSM!N=*0o06QMYT*!<Y~KB
zwI+xy*9n)7n_M?a6no@o?Sk1LpgW4Lf2=O^RNTwCA($0=?s6)kOQJPpZgzikyg@oE
zd_1Q@+|!@&eq<HxV~p||+EagHZms`}=jNlw<DLydpWd@^aQObIDVUj=VdLU5G_P_3
z0aFRf9f4%`L4(RwUUv2zF?HwEbSSw$=+d?Wpaf4%eVOW8_@@MigS<A_HR&&Ob@>F#
z>EVeFRB4asRgCmJuWx_toXSiqQ-%G=k)ROb^Hr;}GUFz4b6?-9y$xOZQU6yY%;L$p
zbLXOyk{-`!sOjo{3_fzkaLE^hOKq1^fiwaneSW_L$}Yby4v<;Nd6-<#%H#VW5>BnD
zvLDHiUah$c67&f-o@ZR}-6phfJIU11{&;hu(!BKu&_3uGRoQ==iG&v`gt9MyR8Fm)
z5DW&}7bJ3oPPO+xiDhMhY!FzIXC|<AL&b(w_VKGVGe=}e^O=i1=`X39x$uP$_-^`N
zmHn%jRZ}oaUoZ=ILX7|n7VxuK3Iv2;Fbz2H3I2Nv(m*N^@3#@&+t&xyRc=kq4M?W}
z;^pobE0BH$iO*Nx!74j`JY#%(Jgpxn7N;ak_cfN9w+cu}>2Y#$wtxMq;pkWlo?r6A
z0U?OnbP&fPmAuBP6$56j>u%i8{-k0dqpHj2&)*#x8R3%gD!E_-tRSQV0s69max0u6
zUXSxA7GUU;7*K!D)rxIcjn9@XNSK~sK}Lx``+5}ov;hf$OUwbe85I@U+1c5OrW_m`
zs!;$!>0ii0v1N^aR{Q*^NrN;#hJLWCmoIDFxpOCc1*BvJE?>6p$0dkaMOflejLm&5
zEpsw5Gr70Q6GHzf{aZf)_VRWqEh_3b$O%6#X9*~`>?)A3KeP6dRqovdpyuh{P7uu*
zT*)>6*C}35Wdr-)1K$6n+8Nu!dNN8TaIVaf8+J)O<&&s+OMk6;tgvy#^O^fAlj!=E
zp+koc$Hc`A=ScvCj==TnWi13~BU4<of4e$uP5aP}o6FN2i6Wwm{vh5%NqP%vf)Ih1
z7uibzyHQ(PI}4=MZF|ygM`Z!IPhMW0(%q0F4vW{p$~H`5M{>qNnU1m!;l5V~Pedmt
z7cDQ_tHoWreO@u*QyDBv4`t#5(!3EEDrpH7lbBctn2BmsmE%MKSPJ*yPC&4kR%`PM
z?C})e)tg&OL+Um*HXz#H2iOeCD#bgeUCyZ5QAbnr+qWSUmRiTid@4;4)u0t6ZrOQ?
z<<vF#jX4~v?Fv{Wp9k$(>7?$}d#r3Om*WOObTp@ZDbWdIAcgmHJ)yfFEDD(&Ge*!L
zYzxxG{kN8j&V<6YyQJ`!Wo3;)uIX)P=p!GWI>H(r`GvypA|OXCF~^@mx&qvRGq>D&
z!lfqjjO#u2qpZQ)L~fshMVyh=t*xzvcIEy1!60z}((wLYD5@$d6;GQOClJJm(UPYx
zKMETcIE+=<^yg{b{=@}?HF5*ZsB;~J&tYNA7FJev>r<bjvH%T*()Z9?1O~>g;gYcM
zLy$SrsiNRP^qP@{E32VF#(lB>%H>K17|biZnW+&lJ3(RLRqO2!S$ubcFM|gPb&Col
zV1qee_3`oX+4kjh9V7x_n~sgm!k#E1YMF0V{;abti}<5|$bqFHt08qMoVeDS!c_n&
z4h*E{zi^>{IOW~DG@#M*K}0r%u-cI}7r3N7K0%6#8vOqx=`f~c7`<B*%PxMtr+N6R
zLV~H5)_3=VDRxzK&V!!}Duat=5bK|rqO#t-d&gxe*3w^xu9H8T2DW2fM#deGCMj~o
zbBbA7FZSo}VUFOHD<DT?<IdRn@!tqI{?_w7rAC(|>|u(G0l<iS*HmHSI%^=&7)ASf
zhNFPKP(W0)w42C8PY3bEv1+Fw>9q#e-lH6xocUkBe&rf#YHC6dJ2cyH=z)+wMJa!w
z5)!-RDv;m+3yX}@^YhzPCaNkcYieq~A1pR5y#10<ISPy{Gb7_1%b8o7uht5gk*R*a
z-%KKi#>w74<HL2OfiLPh_+5mduin$i*ki;Z;o8%{vhwm6fB^?jT(ti2^}P;zz~<rr
z@F9P{y|4gekfd*s{vgURxKKNn^2pn}_Ryh2+8P>ZWo6e<h5$|uyk+Nw@$cQl`cr_6
zC#Rx9JQ+#d@d0Yng5`4eC0#ZUC5{5zSr85IgtS<Qa{IiWV;<~xIg);*Z4AeromVTD
z-8X)8Wwf#nTsNANYl+Lm)w?Zd%}M~Ptm-EU6nL-z5+oJ)KNJf8*QLe(H=p>W0NeeU
z=oAekG{7PQ>vsj90hzdl+K>fSNDC-;PXgs_mBZK~<RJoyYT%=5U}8dm&J;M5h2!Io
zEuKAtEG~co6?Jv+n5@rc&-qB13|9EAhM-=tHZU~MTl(_l%UK;3VAR8bQ2K^3AYYmQ
zHo#Jl@LGSO-1{Zya7<KGMv+l<0Z<uLVw{$TOLa`(AXjc!Vf|jxaXfDg#D&yMAQ877
zFtm@+K9xYg6A=to=DPmuHTYR!J5T^*rZX`yF*ctA_d{?7$c1{1sf|rOuoaOQVPWBq
z7xvTB?WcdyH=nVSzufAC7>NDH)n1E)6Z$oT<Y4S4PhRHZOK%~(dbN)YI2${YSn3vv
zOUyD12+!RJBUXYyP6c#Pm&TFZNjDQ@eLccg;^X7rMnpJz{SuM~Om&Fgs~t)M2T&(g
zfyQ-r7^$rM23#y2VQe%gw;bTAiXeVw;#G<QY{AyA>(rwq!VC+q++#3}qxGIBcHzf)
z-!c{Lx)LNR8C8@MKwbvW+dp;3QBhIQgboi6f2i!w*hfzX9P}#^5<0-?H;FyrRt(wc
z%{32Tw5R5%f{|0TpFo>!A1>K}e7XB-U$!cT!+4EmJ&@4-s{Hch4)Cf5cc^i3r@)?5
zo2YZ|d&hUTAK-?NVTBTKKl0pZ|A}&U1<G{)snz{v&j8Zne#s%`g4tPF5Cs7Icp&Ze
z%L~dt01ROUVDF=8IPn(ko2{(C5=0vAT0HR~t`>C&*FQLM@qvbiM-{Xlo_^UEe)?uv
zJN-Kow9i6}p2_<QzyK)$-Eu6r4Y7BJ;gZ`kt&u}|rig4ipzbNu1vV)#qnSxw*=MzO
zt{-Iz2gD81G|#0XF)09v1jzOibvg<P3Q<`-JsODJU#+BRR%9NJtPcc10#~p0zhx5y
zkv3itLr~R$1WVroHGHF$cDYumh^lTxn5bf^qJn}A;HhfHK+`K<{v#A*Pr*S4NL=;b
z;WWtuz@96fwVYDc0?PK*7;y}+On{IArY<NWGrr^!*!_>Ic^o;5UunnA2%iIIHyD_X
zD1kK@B~uLel6XIG%1&}{%z=X)2SkNi@qOBwn(2Tz<b&mP=-|O$bxHSi{)|r`fMoP-
zc=*<T=n?;B&YZ%&fl5F|n5ois_UDLmCul^s$S@!1bxdNvWdRe6Aco?P{^LR)q8caf
z6#lSW1`N*m)ZgAoKj5-jK&cdY?)-VQaPx_Oe8Pw3-+B(>d9BwVsQ)*|K$Lp{G0fcD
zbc2u)P>h9zg;6XLnY<Pc#QGrAlnwSxrNfwRtcg!Pu!JX)0u$x$ww^tDz<RX8W^o9W
zYG!5z4Fr-Og5{*Q{?_qc?r*UYU}(SrggcP5^58*DM@L5uTZgNF0_XtYJTPC`KYlzb
z(l5>Ero@Vv7K1-v<pN<r_qW}hjhEUseb!1eph1NAhz}6lgSoM3kG}F*u;QS^GiHc!
z0r5U0NCMFx$oauj3DswH$F{k8Y3(tB?vUHmDTeFkv`R{cb{K#mX0u9bAY)L9YWc5Y
z`a})aEk+Q6g4o_FhU^G!PSsf90bn!4ZTs`i>P~n)oVA+jNx!>yCWJ;IGUd0ol>cXX
zs+F<orMH5`JqjGgSxHH!!O0C{(g@B!C)U*e=GPdhh+u-Lp<w}GWz6npXZ#`_i+oq;
zz{PUrB@vG_1tM3-DThvSOgDur6%-b0J$(4^ouJccmK8YQvL7y~50clgDBvj0$6|N0
z{dYP{ZGpW2;^gozjoyh;PLfd%JUj^B918VNl2M(lyvWCEYak~2>_U7s-oO4zyS-Mp
z1S7yUiKtG%ffF)1FS^!IobZ?lrRzZCtfp@<!zFJ0F8;=Ivls`o&(`(6Ty>N?4G^wA
zz;g=QtDu7c%$(P=+HCDrSXO9WARZtFYW6*$!!DC6#u(Ery(S|D1j8-DLYKKRX@w}v
z=(uweOSmytU;wAQ_15A5ux)+dR7a#E_uow)ps9c&3F()}11>M*;*loWA|C}J*(ZTR
zE$J{Semh1jx0K{`rM<nq!k^*;@KFak<>K+hA9!EJ=Coty?^^@(KeUftP1o+xjTkB=
zN3<mk5L4(J7x0%7GXA(YaEdbb=W!S=NqhZA(XTJwhrI&)Ec)F$4RhZ+cLKrhMZYyu
z8c7eaWP}HPVKk55`fbQQ$;n~zRUEEE<2%AD_X!-bu>%%fXtiY_{2NlW>@PVPV5#i{
zAf`S+Fak>IB`Tz@1)gK#r@<U&0SZ~oJnS<sZCepC2bqeIj&F}A_AhMW!4VqZx7vau
zA9Y)UMiTzJ+c{U+)Ly~n%^?b-q@<J_3BX5r&(t>H|IySM0y-#MzkD5H#bCy5qWm8!
zTx*dpJA4Ot+L>?P9s*~Rs=twM=zLzpW*1D3wULS(XrootCs|hTkq_OO*+on~0c&p4
z7R6slZGEKrT!4boFTJxgSkwo0>Q>KPR-0eno7D`?B`5)WO<Q>IZwdO~HmYe60t;|N
z2QtKWgPAqKfuTz4Z|Az=L}w~&ovwa9^%8(G#0r2e*=H`<Ghtxa^cu<O4HPzkf^#|M
zz<0W|O#D>dUEoeDDY*u5@V~vypKdKrKsO=fp^4uw1!JFuY&8SR4>yyk;N=P^Ls;dp
zHdzElvrxBGY*%LZP5eNB96K+sDXOXT%9Sf1QPK+#OacVjm3sjXgnWJwgpr&V3l|j-
z5|YjKNxbtY^0vh`860f^ip@32`m<BbIC<5hd-`q=jI~$*R9vo37EmWVjY_mbsH>>F
zodm!rFa}QhNjOi*2Ze-4*`UK$v`NIoM`vC(hjJ`oX=H3Akb@h0&&GVdT&XNRh?93E
z)BMB(^a|B`8Xg0e;^*!LP72zr&<C7365ER^sDrua>djUS7i~E$<JAMWPtzw8^r+vj
zW)4eG$a1{q`Niwkd+kRn+CteM8ts90oMz_zcxX=pTKge26}hQtXt=0wI5#_;!foCO
z4*xs=(ogS=E~)Sf8($8K!~tvq#dw#0b1(lb>+&r_0m`~q0zAHW@nQ?qT+1nQkdSZ$
zajVF`=U-aSc*a5b7Z7}m|11CEJ@CcG#+uf0>cfW*<}NNS4_sWzI03|ijFc!ZV)H%6
ztkm1!h|e1!9|6d8XJ(|F;VcmWIIc;hvmw_g4Uld#6C}mG++j=-`fO~^L?#Xo>x;Sm
ze1I}+v^fj=a>|_c&J;=6$^lCZ`uqg(7GU5u)M5}9;xDCmfdW9twk6`aHK3Px<jk$O
z*RKz_0=k<BO$><eFk-jw_x6b9HEb$7M0(;8QuAA>#lZxpFi~}xU-ybAcvqb4?IMac
z6b{V+=Ir9Rb6@=Cr17F1u_AC%C6LFrF>_0PYNxKDi7A}do&>yk{rVcvyo!P;JM7XN
zUYJf9B4FJ?ZtkU#+j+aMr=Ze<qBRRSiGikG=WUc|NK#+wgH$=h12?ydH{szD42;az
zKtmEBBFm#4(Z^=iiN%>Q0w>{w{(k0vYu5kYxUqvg+JekCJd%7B&=i=m!oz#Tcb)|P
E54LcP%m4rY

literal 0
HcmV?d00001

diff --git a/nav2_smac_planner/lattice_primitives/visualizations/45.0.png b/nav2_smac_planner/lattice_primitives/visualizations/45.0.png
new file mode 100644
index 0000000000000000000000000000000000000000..2d6cc75e11094ca9c1fcf3fa6b319725c135baba
GIT binary patch
literal 11922
zcmeHtcTm%7x9$&YSh1sYrFT&T0V#@zQHu0V1f=&CIzh#PC`eanN|hF*cY*~FQ0btw
zpwfE_Aduv)xc52ZKHuJF&Ye5=kNeGa;sC$AdGnUFp7pF}t>EjbN_%%P?m`e`FY3xA
z4FsVLL=c)Gy6y1fY+L^@{E&24)N|K#vUK+}b+tfLOx>OBo!sqh%-As&u5LC?j%S3$
zh0h4GTf4hEyGe<NIQ-`TVJBBBkrrYN5hmH`d_~_4LFi4X|7l*yW!NBy5CwJVf|ghO
z%mCJ1i<m%J=#l5%iE88xx}-<@Msa8K4IAErdNol($K~5%CQqhsmpRBE(peXe<4(4F
zEfazfN*0g{O2*-z6xSPjd==}^R6K{0XXmS6cQ3u6bud7P^NQG>MiN<6I{fMWMRQ9>
zSGkr3Ny#&-pT{f*=jeA<t@W3m5pFIJq(P9{WB${^uu3Fc5!s3$r}oh7hli)O%EO~~
z`w<wiN8$g!@qcn8m%bXyJ9T$<rgA8SzQR(-uH{CU*ewVmRa+pKAnA5l;octgBw0(`
z-2+JFw-4bQS5c_^$~_3ux-BVSJ@ei^7F*BbCF}@BfkGy6Z3_rIc<`V-j=N|x0DtxB
z)nTH~2&QsInCH9l9NKK(IANO1HCo$9>gnp@<KjyDNpLv?)1L6^l{j(YO*qXdVPVys
zJ9ma|)XmX#Y{Wj@vMy1H4VFCh5*vxGu^0_@f?L0H;NRrd&+#KT{ld!}mmgiXx6e6!
z`gD;)Ur9*lK#hMTw_0M(tt~sU(D;C>*RFL$s>vRIV+*!EeZaz#=4FBRlBJMwg_d1+
zPTO!}Byqa1#kM;~zqiyW>E2$ZZd*S;zp5BP{p5V}27XytlfF`?o&irn?%J9cX*ol#
z_$6F!;0NcAyM~&g1&VLHp&c0=wOt->Gbpx=*3i%>!p!uFH-s@ssFn_3;&A1ojA`lV
zlPi5rJ&%r`=`}tJUg%7#l!GKnd#YA?&gldM1cZ=G@^sVE(pc76j~qF|FC^3@g8kVX
z5^7^@eO*C8VF_bwoWR*Qx7s|n>iKdpQQ%8h$ND7iZx`(BN>QE8EMz`_sKuXqSzmvd
zi-)K5OHzD%&hzKbF<~rH?yo}G<Zw%4E!nSMUoS5&XJDx`tJ@`nuBqxGM?@T~tE=<K
zU1R+`w)EBbcJA+{7EJ2t>FG1RJp*&!F%s4dacC|unAu2&qn-#6sk!d$UB0-qbk=qF
z$^fR%!68(Lm-o7fiOH`e!(o8^Mn8GdO4{zTA5RcTYRSmS5iB2YtZWWG_t)jWIpf9c
zb_r2dR@Nfa_#5V~ut;Nay0V5O9v>6E7!j42h)Yol%X<BKKAGkeH}_>xQPJF$$^pj)
zlIvE<^3u>#)nMi?u!U5c=UJI63aEH{fZ^69B_#n4j+Cy`#VunUsi;V)O@F~=+XY5B
za04FK_q$$jj*N`(@$e}9Xb97_v`p{LL+52>@e3IgUC|g?o*7HR;uz3=>JddgMBBYg
z;>tEQHW9RQyVu?yQX99D+j!<60~1}kZB9FsTRNB($)h&ero^W0;Za=w=y-~dVTtwH
z{I`hI$w^ynZSAvRPPVoh7cSh|$0C*b>J=|(1Wit3^_><S@LwIk6vIUO4}|Uvj41M2
zFcUJVD*M!6;`QzB(LTA2uPu5rU~-pT-`i0=x7Fp}9=t<$1yX(U!2yoLOibC29_;~Z
zn@KhgdH66YIy&vbo$Y)lPF#5Q?AiLjTmZu#j{WT^Que1?H^&HRDk|PhSALePUuc<`
zm6i2rVGd8JDJUo?vS>a!Ggxidk*?D5SYq(i!-o&)boas0>%-9t=;AtWZ`&Oe7nj|Z
zqGZ>Vr43Ns)>~}X$Beh_;vc|LNb7^EH5u8simQ~hFv=_HFHU$8CP-ihf;`CKt=vs_
z*KO>x&FbtxQ%j3eK4FTORa8`@KuF2VL|J`~ZwsW^A}p2u^5uoSd-s0o^YHM9-~_M0
z69{*kd~rAARHw32e)qjbl5~E?XV*)}d6(}&j7&_?@$vm;SXb8qTU%SHWtjld6nbj<
zYvFTg&soKW9KHOD&9Nd*#rV|JOD--h2?7<d^FRGCu{Q*M_cSvKkw}Dx(n?!v*<+i#
z%=!v)bT)bmY&y7)96dToppXdBQBm$~F59+kYie$`8^mR2YhJzju*xJ40#k*F7ya<+
z#>mo7xCpM&J_k?916DqigKqK^%T*E49jdc(UfWx4hSCKF2M13AaAtSam^XwL8ywk8
z{}d--_vZFC9}=<WtpEBlrlJ1*dtPqt^sa!?zL_#~C_B$;_knckEXz(9m~!YnIiXuk
z_uTiBPsYcsEpFZ{GDu(f>GxdBrd=_tugE5HwrWYWdVMmx;@dr@x5nHEA|E#N;D|g5
zmaVC(8d@L5miO_aaxNNg0`6x}WF4MbJJHu7M==tqSxZipUyGB&)+G8&@~On1%l9U}
zhFG!ab-siMF1Fth%%OD9CtEe~e5MkczgAaQ7vmYrM<w=OlfS*Ye=Q;_JKL~|XQ;46
zHXAT?D$#egO=h*fqlywLJ=?llq-y`fB206#UX<o~#8DBGoVpN((w|=>6b5|fsu{?)
z!P$M-m8!M_6+>8*{_oMRGP>r1N?L@Do|YZ)d#8&=(aZHKW$XC&%xWO)f^>n%{9ZP!
zpv=N0Hiv=d$)yC$SL1z15HnE3ns3sC@EBo-2+jdtCv;bLuOU4W#0YQn&eKy`Qw4Bq
z7n88F8W(O2bRqKAz$jkqC9R`Y`<IkZ?Y+hAB@W^YTago&kgb6sSJdQQR?hB<-}U*^
zUL$5&L{%Q1V$QGVrQpggN_kyh9}$_sZAUKNLFkZ~vgy+{#K~Gx16%CMY;iF&Lbnsv
zZuB+`%e*FqH8FV2?z|hRJ`M|!8hU2M8K%@*e9dRwrZSr*aP|a3E5Bn8D|J6ZWCeN@
zmRwj(MC3hz<-WLcRPaf<N@UHZM#YAstiUn(IiSOV`&`RYiBoA`^d?sEvE&i-G>#6b
zxCiS<IOUpJ<mWvUV3O#*{E9k@E_Ig3#l=*YAE}xdslIc?m5j*vd03pghE`OOOG;;H
z-f|v=znT~`aD+yFj5>UG{o}|0@=`4cPcqCb=^pe!UYvnLZ&eICf6z;1D4athpoV&W
z4b<}sStmw{+tc-<OI3X+{=QQ*fuSd1LlS<JjrO;9Z*A;mxQh0-u#-euPXPXpr=v;y
z^QaR$=&$0}?!qLsX_9|WhjMW^9So-A@?*v{S^stGiT1Q`fy03*Mwn+sJ|08lyZ|oJ
zLRd-679@7~zlTu3EJiWwCow{X>2`V5;%~cXou5m(rN%kMd`^mWb=y*kv}#InBRHdS
zjon8DuRA*8E{Cz^YG>;JQB}0-!8y43+1h4N9qv<-9GW$ZhdM*DRpQQ^?X$gHGQ=qA
zF6tD4P4mY8NsY)7gZJP30#UJ>Egh|`FYevDH#$>MA%%({%*7Js=)W#4Z+#OG->s|k
zY!LsR**%SNYfD-Ko9D!AWy<pWyO_~W(s`+=I2nopJ+ZAZQdr2O|JCv`&OdDS6sZOH
zbbLSlknf}*VBsMi)mQoX>Wz(!LMuExJlW~#+&5basT_$ufNp#nQ1QbI3=9Qh6DU8~
zqyerkP0K*UUlfvfxKNFa9UjZpRf*Vu`|TZSm~}}erZlUhgv{RFV`-^TQypqyp6k9r
z4=1qXU!XQMH8msr1_Ky*K{z(MuEat=cf~^qh{59G;`7p}$;qgQh?99e*+D@;Ph`~8
zhW-w2TNB7ikH)i8l+x5wxF^@Aa#iJ4yK{xRx-U;GHVctR1gsiH8UM#O4wLU#%`1;S
zK%PE*`l`l%qr_lF8Z)hzt^H>9L2$4-Fs^KpV`-lYp8106Y%a&I094UV8HwHD*b*N+
zso}Lzirw8$9#;~stNSXwnojw7?c=RO8=eMo#@`-<$S0?yM2CmB**@zXTe4|SMTu)~
ztJQjgc&)pSg*n(pJbXyI>Qv5klZ!@vcc>msRBBb#`H|sa>&>-=rjH*x*NIaA&S{Dv
zM^U@tYe<_*EyV93BI0bC`ucpRb@<hNbInCtM~7cZ%78##DV?#V2~^m+WhhfKL9Qf6
zISUtao^@h}9G!zPvLEc%Ybv*N5-7dTa+{L2v2m`bdA&8z0Y+KBvea58IqZ@QO~YfR
z9zQIEXdhyi?pePzXBJt%a}jA<oS!xe@9xKNA@D7?`r;fTdcEp)B0)jsv@bY;kLiUl
zh&V3gGnjb2?2>Js+zctC8Pcfm*;SS~F@=1XKqKFVAM}8~s<n4%*0TAr`>#A(&UHeW
zIE_!y2C@T81nKE+x#3K7TCwL77cV+XQ+pSnV87X$9t1Ob__c-0I)nT$fCl^fiUL|a
z^%E?zf^~JMLd&~pf{68Hz^eUISCKt9vN6`5eJic}Ie-lI_qQ_wnHW<=@KwuR%@HP8
z8E499+&i74h!_W~<N<L`g!mJWH<n(Ms->5UB5&Zg%x!qS+iNPc7vZFSet3O(qCf7!
z4mHZ>_~Ue85cU9B=<a;pS}QR~*Y8Rc3A{qZ)MM_;9r7zPyZy=s+8;~Ys}@9_P=WpR
z_;_^fn=ZLL-9@jrhKG=?)G7B!F(P;JC_g6ANH}%SsqqgPx0uiK&I|}BDx%~zO*yk@
z0}<+RTu{<M?qpyo>v?H3Ti&Ao^Ye$0ayW|AUy})Y>{@F*0eq^~cRLv10sdVGSuJW{
z8xnZP*mm2gvULfMEHUfrH*G7T(zz$*O((*zQT+VtAI3Hk&CHFTi@uv}`Xg$+9nzpV
z<weXh&cL?Bhs&<>?q6A%-P<3qW_klFk47nI>2zN~wlY5e=r?U0<Qr0xi^fbpF%q`<
z96#Uf@-55c)Hv*b)Iuqek(Dy27-h8i_>{YRfRe`Kq^!xlht~PvuPvD+i`hSoifCWY
zPfo6#L6f4zyqw1qm)f&^g!Udk`Itq(*mtI<R2R*%rgadHS2rx#)HiQPzl)PcWVS;{
zdpp!5kftVUvys2JSnHsxM!|S!&%J#ifw5;#r9E{BMx31P(oMU4yLMN9n!A;@$g$<y
z**?gA>Os@FmD9eNzL+yf^MDi9vW(ol+b$B&wqmYuuV|kSJRaZo>v4|}N8S6ajv{-g
z&**midgeBIMS>uxxKMgJkZvds!WvC-$_x$(L{Wp)ooRwdSI;$w?k+C3`i2<tes0;%
zPmMqqVuYiwUhNGK<j=BT-?Wt9Ga{amOk?#!ml^SS0x*nU+da`Nf+z2i!phE*u?)WS
zwXLqrGl{uvDI2`fhE1OxH#Aj{Wz}DEnfMovlvtq9{#~c{clV(EDyti$3jAmLds=2z
zSGxwQZ>@3&@;)IGD3OkMnfaaEZ(5|bw@QPNyx8GZULz~SwDa-fTkLweV?EMyRT=5&
zHa;`Oybc3dqj|ow;f`e|_HWO_n>>8{SlI!&m=OjRg!qKk_@_DfUt<)za#a=Ab9-CN
zps&b$d3~4+_v7P-^tk+%KGpVh8@b#qa(=ly=zB};3{9&vI4BYecf4G?<yOlH@(;GO
zjk(x+`=V{_(g>TEpNp$7%AP-EIcHLtz0H{qHYT6(h($n#>~%wXe!w%W->C6Nxa@}V
zdRJZz`EVp~99dH5tSwE^U9h|H2wDHsIV`&O^@Xg=@Ifz*`C#t0wp-C}H#cjWOr1Yw
zYGQQvuv0B`yaWhKgIo+itEuz5?KYF0wdRJUSLH}HL&V#1WEXbc0Ah4Ckv82;3o$CR
ze0=%Qv6o-I=mCSa?kjPa2_;l`WP@5lO|1m-sx+p$b3?ADAwx#7Xx4K=e&6aiCMMA6
z^RCT8w@w{nepkmKKMY~raj;7NTC8wafkm^Hy825#&2(#!Odz+ib=!QYaNiwRy<Jlj
zU)HNvt#{~UUjfh6b*fspOihi(cA1<n@N}9AQ4)9DT>l=vF*o-80DA=UAw(Bvl(}v9
zQC<lNUC8dcv$V4z_9YkF^^`~k3{-e9irYWc$u&rWY)k-#cn)~PCVG7jre<uGW%J*|
zR|oC97e2_5G>5*2FE7_V4cR2H$Uivu{qc*I1nG={N>2efIec9J#M1WmcJVMi0fES#
zDTjO~$j*DqTv9+`>v!;*^ifc_!^p_^EA(=r0`TYnJigLlnfEtd(TerjND||lFLq5^
zPNGb-r>poES%h3;^B>j-2pFP`;M08W-oT>rT#m5o+TPf=ErD(uo@QzZBM%U-@9%e>
z6&L51lq|0!4-en$>g+7?R#s6_iTlx#B$o%Q!+L(`O>U@uzS-Rv5%i!f7KIA_{{4GJ
z+l3a$vZjUK==_lqv60E<W~tJ1QI6$xxdjDpdMi9$$*x;JDRG!expyz+s&fDF6~^9o
zErCnxd3kxIm0SBHPa@mPjc`Z%Vl0Y#Ju?>^X6X6<R-EZ7@TI1qzr>jwab1Ok&s1&`
zNS>L%&i2>NWF)N@d%Lmuk4fJV^Is<(@FnkFI5X7$PpqQmg#fNB`O|{Bx;o=I=NoYi
z@BcZ?Q={mb@EtjPY15|fJ$2>(#3>wDBcDC{YU>AQ16f@Q{dSPh#FV!FO7%LN0+yH{
z`Eu*UWAU?<bF#oEY}%3)BWN?6##)jH=L6u6_2EdhsMy#H(#Gmk`B-8{{bMHLQWB*f
zcNBp%?d^e#K(2{yL#M9BNFL&NN?2A!g@Jkfqc-5WV!#kk8jz}otZro%H4CDqfE%-w
zYT3GZ-A_&^EH8JcO^wDmm1F~?1U_(9Mxs(7*|S^tUgsdY@^-gKQLlu@q}I&WLY|M>
zkP<EnhDvVR14Bb0vLeVaYO;acI0i6`1eyZeBFMdemp5EMxA2&Za&>hbUhslfRAgjI
zM}}Idud=#&xaG&EFW<kvRBj{;<nD5+4s5&<4A#=hD!ZUS1LR14VPPE$i!}IjugoY_
z<T&qL0BTu2o%T+Uz|K%bch4CD9!(u8BGTj0W6Xo<eQkQ@^qDgrL$x%I8P8^Y`f;0P
zVgB2@aQ-6*@<M5xNb@=;N0~J&Lvj<<{yzB7$by%afq_pO^5qERHn!H*)>N@0lJh4i
z;3tVp<=*?J8*KbNq9#b!KGKll1|Y*YSB~Xb>C~^5ML_ZA<>SljBB!Quf>#GEm+R!{
zy&P$dE$u~XXe5`Fl|7NwRr~h45C4a(@n;4gBaR$BdbH$tiSY899c))3KwZsl0DaO8
z>v{O_VO|lDJnu>zPL+?3Z_%gv>Xj>W6K-EWfe1Xd1W0xbDEqAI$v<87)9kNg20One
zvC+pKF)FwjJo+iFXZC4lr`F6srLIyKs{`@N!jA@!keJ(_o0*v%K6I$V7T-66=@(IY
z66rhjd%3{bjG{V~ApjFm<ybSb?_3QQmX=v1B|4xqbq4J`#EF6o;<|%FF1O0_!?UNs
zu6+_rDB`E6ApY^$nPTT}7o%fiv#Y9%b~6Zf!tEIvArUb#=@JCLLhlVMWfiNfub=6^
zv4V=&$9(QZQ&ZCuyd#JqFES>GpqQ9=j~q$M$Y`D-Hcb(2r@!R;Or*scJd$)C3beJe
ziz_AgObI;(eO;p8`3|SeV3jwcgkyYnt|3EPFI9EPZAhe@eMGZm`}jyK+R^iog}zY9
z9B4%Y-eU=-IZzOMVFlVRcpM@X`tR$0f51rn0h;;FOwFdo#_)z8@KOh1Of^Z?zij~m
z9sT+F!1j)gBW!GWClu~klQ(}#fsQ}?5a9K7FbC}okPY|+1hkEea=w2zqb6ZIJY&f}
zz<P_VT3BT^=il`fT7`jx>OMboVZgmZ<v<Q26w+ALF$}H78>@4=E-v|?#d!>P^q?Zd
z?Z3PLHIC(+vwX_s$F?4iU^y*gcKLHjl%GjwP2X;%!uFTfrUx=vV2*v@U@_6rjXxH|
z?#rQ`pL0n*&l(1moW4DQ4w<>R(aj_67O}>dHqqZ5_wc#2>D7(OZh876|8u`}ANTcx
zL=f~^UDz&2oWfF$bq|ji)cAR4leoCJ9)^TWd9_u4kkGxVV*b0*IgPKW8Js<6-($Yz
zhN-D(oaxj>Hoy5>oQ)%{#XcVka>Q4$k%YOPD(|IEs12UA>n@TmdGSKt+uQq>@(a>(
zIFJ#Eb1RqOCS;S;^!4>!D+e&Khg^QwK>tYIyUn)@MQ~jca~iC|G^D1d+s+MEOZhBY
zXE#u*I>W=mn6T9i66nA<YH>ks^BaBa9v3K)JPZ!DDJINdq-SF}J~UQ)1pSU=`Pd9%
z#}NP_Zf<UVQPG8nAAp_OhKAV*(w??&gLX!wLJ8&-IDI6yO2xR#oB7oYITC8FW&^i<
zAM>#Ss;l`F{13feGs2y`={*3P66Ew)s(%A1%1TeaeD>^FDbLw><6Q8@OPBKK10$B3
z1oC)gWQ?IA1}_kgyRL~;cba{Ka-No<p#W+fs&&f{t0zEw?eMHwV%f`jzU*gqQBh~;
zuV9}3FW&!YRZ~;b(0&VoJbiyaAP~N3{M^4|!2Jtx07*+TLzP<FVhy9dBqb%K{MX$p
z8x9{m3I#@$g@uKR&BbO=;@xMy<a3<f2FLT|!vUI8($a>}(b3fK3+@Z<9~~LlF)ONz
z_AgHK9zE{A_U(Qz6j(JhHAjd(9-!jTY3~IF69&bPaU9UA?6NXFD5_P=R!niCn8fX`
zLs^ZA@#-m;i@l977);1aw1BSdkNU^Gm7aM4^LN;SsvY4<Dwo$0JslnHu!at1GN;Y=
z_rV~>!$PaAk*d~mx3QuCl8^U!R!E?W8CZJ$RSKND%pjxonk|FarT$m{N%*2^MTEIE
zHB(YkQ*|~XBO<gkH9KbWAtZ!Ckjz+L_W={q0~dsv1p^EIzt%--)=X;uo4P1ONkKtD
z`!n{&<B<3~;RMR96Q&>2HA>#C5+|+#XDJ2M8!G(4E^He@Ssb=sMg1)icg~FQIr1Mw
z{GY3&Q~5N`jS*bWC7etu&EX!3J?BhVWPFOc%p1bkbb&BcHo!l-;Sb@AlCG)Ts`0<-
zR7?_%S{E){i2GswzY+WGSKWVIAVl3_w=q?fr>8DN6qz^d>+0^d1*tE=M-_^}RrU0M
z*ug%ezS>8uG6rX?K6k#oyIb4D1h=^8Dr8i4z0j(~(S_!Ufx+`Zza<q^``1D%;?N!X
zET9KTev82?(_d5i`;GipzFMuv;eRq!8UV69cwoYYa7{$(nyCBuR6y|wUiG0Y*|8=*
znRQRM?AS*Iriji`C$#_OLPN#R(Kru4aXXkxDqyp$#Gx<U4p(0P&xl}dZmzARb$GS_
zOwWfCs_{U^U;OxYA>hf=!Y?EW1+PVD>FQd!alIT{-Oxs(^FTQN``SCU^bRF7?g8>;
zFH|YRInz>8zZe0{9?yZx{P1Jpi}5`nqnO5p(m3cQa3B6~fH+4$3&%Fkx?arsgP^WM
z8U%Iz8~;)cB>bfZriL-}NUh^|M@HBmZ7-(qnV~ZD=I7tb_ZcfrlTGL+&+-|-tZsma
z@~cs)E&b`!V={{$I3T9wR~|NeO4a*Y@n3<O-<lvvvoPF(amVf*`Exj!(0}b~x)Pz9
zAe9FE7hnWlAVQBXPjt+%YU=5|Dk>7IYiO8SY!>N(XT=b8J{)G=o{+EwPsrQB>Louh
zG4U`gtOpW4F)Z1SMbeeiRq_6H-GJ@De#Fe{_i%A@H;uSv78IO5S$wbQ`B^(Lv-fm$
z_4WK5915Vh*m-UOJq4MZs>2w2k3<`kIvNM@?+yFBj0Tl@dfFcHm8(z`ZEH`v#-VF{
z%)@dY%>1{-ZiT!vBqu_RF?1Vr*WTLFm4}vt&XI{mo}@=Nis`s$**Q^Pzkbci$<YG*
z2KGW_+VS!6n6Ppvl@dRmX&HY<FW0#;(--~xIX_4v(E6bCYO-~5nz^&5$F3z&#s?st
zO7Nf)Wn$>for;O{xRZJw<L)C?Lx0J`j!VT!ICaB<i|xOj24}Jc`3H6$1Da4TGj_c7
zgfW&xe16*EAwNI=GWh%CpzmB4lT+1!Z%qm*g(L|vG3(YB7Ys`5pW&)kdg*kTSQ8TN
ztP-$jU_!kG79p6q!Cae;^oe>VCoM%q#UKIt$Ddc&fU9y{iy0@a5_-s_4PvVT{lv@r
ztnE-o;4qTB`1bY!?92*P0K7A96|h%7A}36aT=tHTQBu+!ag#(|#nUT2Gd+?e`K8<~
zgR7A$47^ph{Ni3pQMkVkM#Hh6(m8H$h+FyfQ6Ym)Gc>?fbW##wbZuh}PXG!EloeEl
z62R4CC5-Pf3rT3}$<EAf0%?G7H?!cw#AoKM{u_OS5>RtkC)-b+JWOhNcBY|%w9=<w
z%y=^Ji2SQxNvKkfv8VA;P4FR}rXLGnf6yiHqrcqE03T3&o>eW+h=s0>`HBXlSVqFJ
z!j;nnr?igCzMRCDo&&Z7CnJ>C(`Sb7nX0zNTUc1+*0($rRj9<wKv|65F!x()G&zC$
zuv;q-Pw-&t+CZ9byrMZ5+py3U0b4lhG1=)H_23i-auN6!l(uY(<@oqe@cRtw#%Jld
zaYMp-jG4JP5e}ybvQ1!;Mrrp{JV)8bv-4^|@~SB)Ec6}Nk#RqnGB^yGw5`3p76hIU
z5|qyvW=hl3(?2bM_R<5pd#hu2&OHsAv9peObG--O4d8#)bGF|(>X^>J1X#&S@aML*
zg&!U*GOM5iO2ns~RHh5ur=zFG3*8B`Jvie^P88J1x)wmjDyLk8&J9h_1*p4l8&&?4
zAgv1_b@w$n;{(BxPiPr}KE_F?fxKo=XsPmHV?65$kznXwSbB}v*S%J<vU)UOh7epK
zwIfd4LBrJarMS~z|0p)mCj_^QcjyzJfnqYKp&g(@7J=nC$2>R{Tu_MwwfrOZ9-PG9
zCmaTw4Chd?g@{YdRB#oO{0{wP>d-(jkR_vaQgjCpd}s^l0-1FAzTD<ZrChQi92~Ex
zs5+#;4#n**km81{_|_fKEs~Vw=f4W!ZO@;#m@e)1n%dT)_gX#=Pp(K;OJe<e`dzv-
zDeSLpQP-i@%<zk?qSt<eMNm{UUm?T%%s;aE$xb(em^7%R3U@K>`<8g;#@a?l5;@hA
z;9uIXjYc|PyU(3JIxapyN$+#K<fjdP*`$EAcWfoz%0TA^12$%kZh+Z{1#Ehi56r&q
zX0kP&AlUqIuI<L218Kl~ChjpgKDd-=dLS$5R^p1MtkfG^wpwf9$9TyOMriGVbkdUQ
zpMb30RDs~p@w2v3fFd{bV=Vj^>KJsOAW9tr)wwUd)xM?FK05(!?)r6*=ccV+_DM;@
zu@b1@lmq8#08{CJNVlX5VlIo2`!%bLQo&{^@5NMYb#+rx19tO1S0j`JdcePTWkVcn
zAMxykDz+uWLp$K2-B1_JNxK$n0JOLpIy{7$3vxC${RDV;GQ2mFD4SLS!or^`Aot0E
z8`B3aRn+fTO6(($yKzS=(h5n2F&40PJ%CJ|%a`v(CnPvf|D+Jn6bq%FC)&PBdI3Ah
zg|06<;$)Xl06{HD)<JjoK>Js65|C;rS`HTMV?A%pa^9=eXSRGSCn#vE-PZyOOmUzi
zJMy+5r&exQ?q{P~4nUqEknGZc#mFSR4Alf%(aRA;D4N3e>!SmHrk%C=%xw4J$q5~I
z_oB02^Eal_;~Wbxv;D_^epg5>bldMY6<Ja<2n}ZsdI-kg3k8BxKgW`s3Mopg-kizB
zo?Vk&*}1TP!?w1zZcqG%u^srsr?3H+29|D+5ey$;=c-q8Tz=H+!vGy9=NCh>Bb_d@
zLBb3^S8fVEsZ=t)f9fN%a)MOxC&{uOq0%SOUJu;UDH%xty%nR+?Q+V>&Iv%TDpj&6
zw*17HouA)!uc^&gxOp9~D+)<SEu9fnApL(@6a7iyd-YUpQIMk-NC|>cRK0||c;oi}
E0`;BYC;$Ke

literal 0
HcmV?d00001

diff --git a/nav2_smac_planner/lattice_primitives/visualizations/63.43494882292201.png b/nav2_smac_planner/lattice_primitives/visualizations/63.43494882292201.png
new file mode 100644
index 0000000000000000000000000000000000000000..a8a7428be7739e0d04f90ee625df48802144592b
GIT binary patch
literal 16013
zcmeHuXH-+sy6vV2f+8Yfp{jsV1XOwl6=_nWNCyS!O?oFFDmFlo-jOa+q{YyJ6@}13
zlNt~Rh8B8BNb*)Z=iYnHx#zt5#(3|~z2l9c14!9>?X|u&zxmB?uBV3jnk>wm%n$^z
zXlq?Jf*{%`2%?!f$N*lE?i-&4Kji&2%>9kMo&1CD`Z_{-cm3~sc>8;}+Mfw@^!0P~
z_L3Br5tqDp#>L<NzMq1Ggy+A1K-}BcS)ymFbqidC>AseQ9|W=9rT(WWRV{IaAZ3E~
zb#;^AtmTO?Yg5P8_BA~A+%u-4gHDN-wxVK;lJ9-q-MD02`#jyk{I$Z~73Xhql7b@|
znd$G0(`}mXIrIy6DRsG6$O~k5NL|-GE869@p2$3DWGMWzO#!A<t1){dv*oU8?#|*@
zcYMpXWhTb^EFqv`OQ~~CE}kC@$(;W?<^cHF20s`Dex#g*Xu*q*p@R@~=QQmZ2#Rw2
z|8M-?OiB7t*va0c<fJ4QOlySO;M-(t@l<3d4G63$x%hY<#p3PdF;SuK-41u@+d_ZB
zy>smy97r}G@=9a)B6rn!6#Q3%PDiRk?_zIPSZZH^MV&i$Zr?gC2r=PRVcTN)^yyP(
z7M86bc@^g{$}wUTd3nXoo{!#_5)u;fEG|xY5C0+VVAR%>{4y6C8{12oOG--C;1`;m
z=XUH3I}-0%e`8k2g4@w7q_Ob5l<W(wU}P6>K=cq)eVFDbh>I)qY9KB;k3!TpuKo9V
zgWZtk{$z8=!Z@NfL)N?U!w0zvo2CYIz;K0CNv8zT+H<t-*7Fl$0|lC_%B0PHm3Jq=
z1FK%$w~2yZv}(9%T%teN9J1lRH<!Bj=%j6LZ~nFVfW_O{VJnEaA@7lzqT-wBZzfy9
z${jmXEa6_+D~*fd^&TTeNYb)5sTa&wls7$!t!<>%*?oU^E#uPN2cg?bW%7PlAsLz5
z8}mI?_em-dgx@JvUVY&5rTW<lxP~#W{+DO3y;X8M*d`u7gJ*{F?$Uo@Yy0%+Dj2MG
zf3cout<vSow{SR|G~opbb+f3XWKjaPr(3jm{P^+Gyu7K|#pY%u?ZmO=l(FT?Yjbg_
zCAn&g<;5O&yP7b***R?r34<nV`RLrNtAhPl$wkI46VJRPvVO^N6Foyam47YP#M=5@
zT3XtbP6vDYq}Q*F=Ndo^_I`|EG&M0PJT2+u^64Q{a;2jC(8aDS`P%XI!E!TEC8cJs
zud=?buC8S9k5$xr`SC4Hggwp)U)M=a^6$-(U-X}T6ic#6#QfadP|XJSJWA~8>DdWl
ziadFgruXxlZ*s`8oIb*_#HuN<BA!pR^2Lk8dYLkMHjVy$V(@XJ=|*8;VI6&al|6v2
z2Jhd$*SUGq<ofmNBrcch<6dwy8lCj=WnOQuAuM&q?@wS~k$k=dM;^d+$<A!GtEQA@
zAa!WtmsMd;BaVJ2g>_GjTs%X2$HXM*K}<|1&$8nEDK$jFgOWuMD*|O^Rkou@7}5!q
zcFPqyeCUv|xw$!FE<Zn?W~XzS?R(4M!)220_DW1Fudn;suX7Ou*C)3t4-b{zB3FJ_
z^&su#c5Fu+eL`ZiR1IJ|8l#pW?Oy!h!w28kvVJsL>P}k}CoFzO&arZh?oL(Ua^Yen
zQ9m&$@f^nq0^$s``X)+_R!IBDj~_ZZIz|9AIQaOg9vwRAwzD#kA?2DV;716=Pqu{4
zC7TIo`^|R`BIa|-d6h%8u58e%?9IL=OyDEu;k=NPIKO6O39IT-CBD_qb^5An$8ne$
zJv7O7iDng>={+?4Id<zie}a&rqSZ+o|5pHAiU1txB-Yf{)`#t^yk1OAN)i$j)KCmu
zGSt#~(ydAfSKj{V(70IHv_D82b)#70xxMo4ou|b&nTAm5=np<dQ8p`w+Kgz}Awk#m
z*&i87A-<3BE&J=8j<%GQAmoe-Uz8gOD=Qefg1F;}@hW~?W?w7>xm=<B#i<6l2<7La
z9TL*#Yn^q8+k0UAs|+Chm^jR8;nq8gaw8LYBY$EBAvF>kM3cuUo+?ig)s|=xh}cSo
zEz(30&Ox+lEQe2>nYbTgVstJ$a9A61@dcU7@LH{t_Q#KCMZA{#&~6haRH_IrJ75U+
zN^~wRRV8Wra#M!rpa2bsKFTuq_gUNv$wC9w<jLUA)SFEJ*vmU%Z;$FzeQNcE#-)*Z
zMcp4iu(lsDFbOg9DZCq$xBcfu=d80`S*|&hy&fa4O6rhCsYBu!4=EWai1q(%h~tii
zdQGU;J971EVLP8}k8@AT_R^ZSGGxdFFt)USjla;2Y(&^|_F`kqa|MZ5jR#?kW;aNi
zmq1>O*ekN@q>19>2QMpc;w9oZzl!~Wq{OJ-EYakX9;R<4R&Gwmk}DaYx0>K#LKjBs
zH&6KVD9vCjp}g~8j8g~LFJ!duOup97TJ;u#40*sr1gqmGjpn<ZuCIpS_o%bO2|&NO
zZ7#7AIjLm#{u8nuQVXDdS1<?K?N$V^LcN*@xa(@-Zm_U|r9!9);Wt0bj1Ec$LyJnK
zXVm4_%$Uz&VCYQ)E7kT~sU#W`-hcB53lzsjz4q_7<%LfU3M*QTe(0l4@mK2AHSXR;
zSKr0%t<RAw4nQK*G$WxHpm3^vjXF>^>Oe<&^RZe}K^Sejh&k%4EB$%#w6hiy6IQ+;
znnvmd7?cY%&Oxu{0P<H9!zKHN$Bzp*I_W3|5x|oPJqMhICC}5!bxLhxze|>6$qXfd
z8$~gEa=in^ae%RLeh(N<Zx2{3jA8uqt~&Ky^!tM2C!M{R;J!u3a+)Y3>Rr@w`rH28
z=qmLSvrJKVq{Q6Z9{dEfdax9d^PW@+SQ)qI%9J&;w$2)=w6Vf9cBV?0UA_8Xw869d
z$&-U$+uIjgb|yFO)<Yh>wIu@;nI`5!q(wXG>b;0!it0=e>mMjHmX09!J32YJO|?aH
zD)^UzZErJ8;DhbIKX&X`o!g)YH6@tYh7{d+aVj6=0zR`NXI>(#Ux2yarN0SfDTg(D
zOW><wVn1K%+>?_b=^PKZg*1%3&pez9{#a?*v9$@s6@fr0=Z)CB6|g*N>Ww5dB#1H|
z{jEfEe^8_>uMGr1l)$6iCdgap;>Cety-X=^(<`0q?Ci-oIkp8oLc+p*?;Y9)8Uq$}
zhJ}TM3_l9P!sbcy4|w5YV(>AW*bXiU1J&puGrf^X<h>o0-8D^ISxMpMW~+`?DU6b8
zeXib{?@a{J+OnxZD~3USOQI%vWPf9)I)pDv?(Fi2aiQm!rKl?M`hf!)BH}6Bnj+#d
z{A0R0))#Wkw#`RsIwnQM1Czt>kGXXGCjur(+3(k!LmQ(GM#UcCG4k<wZ*OnkaWUcP
z(_8xb`f(~VWhFW2r|cJ`LN|Zw45v%Gr#N+IS$OR&43=~7^4d1N$$U~*Ck0ELG6N}O
z1i`wEXB{>+AXy3u&bzFPOD_y9yR@iEDk#_zw>1`*;qc#Ip7Jf&6(&%VnoZsk9b8dM
zGvP7r`>RVl%ACaonwgaOkPS-n5yd^ieC{=YM>?Fp@il@G1x{TJVO#7Q8yhWz5WQP!
z^lGfBou=6O8M(O>(|b5{QO0G5$CIIpVj7y7W{tKwI?obczcy&??(8)7@OXEOot=Y?
z?bgmGuCws53NT(l0RbT?sR7K~ZWc&OOEC$m;(dvN>Xx-wU%hNgN&&0rZF*5r-Dl4l
z&4;b~%gG01E{(y+M*Tk-H8h+O*Rr^AZ8WLd_*7qC)C`$aRK?2uGM>APjQMO_?=mfP
z?wnn=g0$K7hq~(OA{!js*5o~0g1Wl?fljyW(>e30HbWXDVlIA=l+ylkzwKbuty{O4
zj~o#Xs>8QM)6K;*=Xj0OIC82+Hctl6u(0qdedq}I{OQy26DMwag}Au56p%XeTWJyS
zjOrHHkI9IaiNA*2tK>s>CXRYeh}eYtkW{L!F>ZW-!K$xbi&?bGOZZ9HP)%0dQTKVj
zxeP~RyU;0O=+=nyOLMbY#GTC5`GH@Q3;Fp*O@T%?UxaLin{Fyxe5XYr3=<o%8Sb<z
zJ!*Md#wTgmpv*r>S+1z42p8HjICLd;Su>v70vD*8Dn6JUG+9=po9gzn`fjTbhZ=id
zMdoIw-OAu-J<sMaQinoBZV{XPx-Tb$?F=Dh=!-vHbzQv8zI{cq%i(s4$;3!U-Wg$q
zMyLFhceSYRn;BDA5N#)p(ikR!WdxN#VQU0YL`B8+&`I%um5C;4SG0gOc5@jp$*m62
zDy=Ki-s2y9Lb2)214tO<wI-`oolE}r?^?($n!L4B(hFP_He>3P-Pk(M%=G@M%2Bv%
z0==5n`2o)nBhF13@08Yk^uxCPN3ogwy%B`8u|qk$3vb?_%8c@4XwO5F;Tv5F;k(tm
zA+v0b&d!T(o{OX3nwAf|chJ-}3JeTPm#47{o<6|A!GT+8!VgUZVIq9@mf@JnRJ+KL
z;W;{&hYEp8#jp5vH;1g$LpN{X2F(`97CHP;#O;t087UqF0;`pC;kTnx2E{NbDalUu
ziV@+fgbtFhd0Df7DR{L3pj)C;$of6K93{l39evgSJ^($RPtxc8XrhqqTefb$v<=e>
z-Mw3;o7xx=KY#F!emtV`IjoniKbo#cb4w|KLJs=6ctcay$LIWP2(4X-zO<^wp#e>`
z10W|gs6-Ur{ahG?Np|W=JR$aWZ2rrav|cl7kE@?XoWtPyyD2kTVcSW{kD5aK=1O)I
z121xIf|0=(4u-9s1PR4tCYtdk6h6m+%a28=V(U4$Jt{{=n})+dxa4GUZ_H0E!;1`5
zULE0i8{FbG(ZSFHZYbj!ZYBCBo0=5BV6LloaHO50cD__>SNYg{owxTVzF`^}mU#Z<
z@e!R`rvt8oBCekw*Wei$bx+!wZ-1ZsdQ!K-a<9ZTPlie)=vbBR&`!P4z#`@|T1VyP
zxpDmlr<umnI8Nxe52Y1^i<&)l8LlkvXolg6&`;Qm?B6hjb!8IE4D)jSd_9Iin~qV(
zeOSjp>8vuze_1uA><(wi=hz|`N!Jo#6pB|a+dR)EF71B%-NzW4#+Al1EL^wYQ>=gT
zXK6ko@5{%BU)ct$>u6stkaMSgIl;U2ax@-1c)((Z{*Pnq4)N<OJeGxT-k8|HNJk$%
zX4tDT&jaa`ChzgFV}+5V!Z@y?k=mkm-tvj<ah5NzXU__^7Im*-wwE0-_=&NHt?pIH
zgl*}d;rmTI6_%z;xeuZls2Q8n7lFFf2HwPWC$F@axlJ@;$HMgaeCc~-*Vj?P6L5@7
zK@doOn=QSDxM!lM9^c*jk`&%M>|o&rKCMn;e^Gi=Q*%OYWXRWI$9(Clr<X_PPPd%}
z@ijsmY4GyI`g33Yl+}^&WYsD?3Pa_QCzLd4yc>Vex6R)3BUWata?>dJUMJu#lxaMF
zT;6{op5Jp+pk=R^DblJTDFPX`gEnqjTQfRHS)fUzPPV)3_?fLG*OhsNph|xVNqS8D
ziV-tw>iR~P>(oY%_fG}7ksejuDx|pbP8NbLR@3k$K#`0#S^J0SNvG994!hGvSu!&x
z790E_gYkS-BZ1;4jXw?ySK2ty2gV*jdq+BavNKPUa!cA=D96f#xesYkw(K6N{$XIH
zF`3>ejsBPYzPGGi?Seaqq>cJ<<6>9~7pqdc6?<N^s)YX>PsFda>!n4`uz=F?!yKof
zF_kdJynKl^Mj9yHt;CfxF+3O;Uz<Vi@1ZqLY9q<@i0-Pw!p%J-#q)+eJ0WV6$~bu@
z0yE{A>tRTQhPB>(<`1EmNKw6Pw@-lPqlYStI#aNFW_IaT>v7z!D_;-gc~K{aslALq
z>NRv7mD<=}YcG26U_te@aT|xI@MXU(79O|Ci7@PG$ppLMO6=Zbi&I<Ifs?yUMrSU8
zIGU33f`jQ)2Ze&lU}ac454LnNM;TjpFF=mgeCQRw(;2<tN~id*116H@-!}FatBNp`
zR;=+dR7zE>valOHApT-DKQhiQB%4{AR*p3z05>QaGo1%;JoAcwtNeW#@z)_-WfRI}
zl0uNc5%EQmJyJbL(hv$Kd+*6<AC@-aE~s((H<v|~_wPRy!0jMehlVOlt1tzzA^U{I
z1yy1j&BiHq;{@8w(l_UDe3VP@JBt%mmLGaiPk)xDhO{~-Y|g$$B^=zP1v^9JJ|;r2
zarYNpM^BLJrVG1?h;b(LTYhU%p2H)Cj<FC)HeDDW8CQ4g;*^YX7!can2aoWSy-ST1
zYNe7;X8Hr#JL9z$%cJsy1yNiOvOL_z0NP(^s@ch@c;dVo>wHP;3~|%gbzDwZW@uc+
zyN9>T(@OH`c6YYZN~;~I!+L}9kHBa6RI=w{uV~eAQ6a5RoS^I4uV=%G^W1)$9m_Jq
zHKt!5^GBtpqfO1UNrdz!zvCx#Qf)<A)}L*DW<}eP?JG%iQ>xVX`NUx+7yA3`MmW0g
znZX9b0qu#V%=y7=h9k4oJfIykDg5=THeaMm@ejnZjO%9x*O?o_!av3vSfVI@sy6^}
z@h;hNXXAKn)@)SWLV5B4&YwBZwjYk`fyr#_kVtqZ@)s+w%LO5J5rf=J^t~Ub@WKh|
zxI{rC|GM<ndkX{Wivt2sN>UOp&Ebc}ZGwW>y5OA^^flJ>C~iK93Ok9G?F@u2bJVw|
zfcH!HR%SCri>Id7%x(>Lw%Wahf_LxZ&b+Ga@k&#OaGX#mgTeePgL-(`w{2lIgjx1l
zZnc>|n8c~{^TnNHRGlCu(uy!yMpIp5^*uzNazit7aVFyp9w}>#ILz&GW3RzYuq(rn
z(7NYzW0O|z09bUl>FL<60m1f!%taa)dE_E~bnszrfu`<-FclkPUMMkBbRw*3ptd9(
zB`>5!SoYRCm?P6Q&UQeppSt9h2bnGp!!<OtEiAA@j)_ca*@~I-$=M8E&*;<upzduH
ziQ5!j$hl<PC?JqFX&3ylTQLKQjsnkG@82!&<X<k(mFaw5D&;NmNOc(tG}0_@>Em-Y
zL&-wH|NG|7N)tK^pFRfvYemawX6LCNp@ZICLnA3R5s-(7CWIvDa9%!Vo}hHB%5%)x
zt^Y<SImET|1TMq<LpSR|dTJOHs8TE%_kD3cFZ(G^?agP-4IXIw7D~T5rS7_Z5BKYN
z(az){BQ33`D~J+v02mVyLH#@*X!DwZA__rzrdm(x{LB1*2m%ggRHE+`5>OJwHOAKV
zfEG96+$A;&Fu!K8Qnc^oSw5HjU3H*xZhJ>V&GGyITUg(}$MV7x>%tKs<Mo~6JTv}r
z%Lnsj(%4#*!!p=fLqgCJkpTB!Z6YaF?`nHgrt!yUq>fVaM1S>NG#|w!r^))fdA5RP
z-n|<=yh<)PvLNzvBb<tB5xDcN?MK*kDDyDXcw`L2UXudE5(lQ$cJ+4w_Q<Im?L6du
z%@T2B1pD4XvP_Q)GZgst%{j@`yU0ka7q6IQ+At=ZKaR^8yF+($ch{5=k~qjB4ZrF>
zgldi?@73UQ6%M>W-soEy;U^J>(fkQ%c?yB=<c;JV9uL$M!Q5!4_-Htw`L4{8?rg>K
zkPYm!=g<2`>)l0dWPwoQ&>n9JWFU^?#|z6iCMI|ww|oapT#6NH<HcduxeoNszVZQ-
zB0lY-2p{ZXw$J0b;P(i)kAoZ!OWO`0f8u`?H8*kn$?553(R7S%zkBmXTf%}KEuIAO
zq32jbhDt=Ec<Zh`pV^@^h7-XKxDCaXOLSKSC@he$(90EsCg<g=3gJHwum5beXxzGu
z8KTMaVM>GK!nQr)_*8A#&)+oc$x)Ve?&kjb?VB5Ue~+UQQ)YZYc>^WMDeIMWj-Nlm
zS}aO!F8#2Akd}h)7gX+r!ARy)J@pT_)+U0Saz4rXp%zv5)pZPHcf=U(z@yR_tE;OG
zGi5x{QayG1lvinKrJI}nRS~%l9?*h$$O_Ni$z`eq5+L|D+-l%3g}cy8cq=?cOkJ8}
zB<|<}J!x^+?(M^eqg$B#nB$zB=ip=3zu^Jp{O~3%)%~Lvg|(JPIXaic%eMV;A{Df9
zBd}=6p5vk-Z?~13$&I=}7@l_y?bdD+qkYdMPPl(L_*$f#_$nnyt1~tA_jD`_h-S}$
zwpmXhN4H@yG{V*YNlWct(cxuhFUZQ`2BWs9b#ld`(&tjDfOtPWJ#9-=iPN^U%s}d{
zic-fs&WJ#CZK5R2$_ooyPDtn*-@JL?HNo`kxpTK-S>Z1Q5Y`CGdbhaNec!0UhEb%d
zary1#3SXjfSa)v>t8M<LPiMY<39tMZBifo&bWWAR{qCgQQW2<E;u8xGiOUtv&gwBA
zAzPH+Hf{0rd9TX{3y&x7^>~femLwNmnh3+|Pekm@OOwgb-@jp^jStvU_BiwgT+=~4
z&0E?3ct}3p*vlfKu&_`_UViV7(hOEv1nAMAr9gd>=&|M7;9-C~aola1lnaDld%pc;
zZ$Zl@8FS6}!+_5iZy+@z3EG6Ub~MsbG2mJ6)W_l9+hbidnO=?8BZ#*nCqU6I<ziUP
zd?+UArQw^BoJKA2VGU`*q+R&giBR_^t>jU`)XqNAPO1xGQ|r+~Yty+vRm#Pdb)A<v
zp(0v}foZn<{5*o^BMUC%JTWD14-fn%GIEZ=7-<o(p3_R12d+aeo;@?VfB!zBAcFA-
zw@{EtwaTZzDM;H$zQOdq-qLr&D2Xk^sm8@;^??<e-HK-$@vRwl;w_$;h{!69L+lJz
z@FVEP0QHWp<k9-q;!PI|I}qDBk(Q$$ZasVQ@`M<(QLWP;%)9;73Gt+2ed)U^OSMIU
zy4H>B9e1>oG2fPVeQ=LX7MA3QWS3NXS_#WrWL(PE$_-zyABC&-bmUUXJV-pN2fvW%
zDsmeSK2bl@l_70xZEcPCJOE^N&P#Wqa^KSSCfS5lAoTIC4XhRJ=V8Y}R_K-xUU-yb
z5r-&h;}w5-j!VemZQMu3t_`rSJh5;6HcS0Cy3mE%=l5B7L>wJ~d}OgFqe^zc(k}h%
z8~-SvIA-0nyHtzr&PLa{9&?z90)?w4Bq%H|Z=tQNy)Xu^`ZY6Ce}DQaC|9jspVv#g
zr;h<OtXWmC*Ql^CP#F^a_ji4Gl!6T{EG*oH-ldCwn4*Q%L3wPrpSKk7%>@K9QL^5*
zNb2s@u7(YIwI0*6uU<KID{D7SKkk5wid^ac2585ywL^}XGawGAQ|zNuxs+GZxYgvu
z6+d4T+qaMnAjkfA0n(*j<8lL9`yCB^E2t`EG2yjG_*C9$@`hX=85wE({pLAUBMVq^
z5r5d--7PMA;ZCkfguV)<&h@oz*s^rM($8CpK`Wb%IEb@sch@6=LQ1!M|7M$}`q62+
zH#6;BrN6#wfjUD3v-0!;Dk7MlzY911R@_cp_;QIS9*LoE@6PVB(IRY>2a{}?Q1cR&
z_u`M~7~v<Kh_?y~r#gC$(H%7W&tmvoiu`1n%OSP41&pedZp!8W5w9B(0xH|LznG$O
z47VnNus*x19SPMy@kQ`e-P8YkTiR?Ynhw1T*DWi<ZvE~%f9bY=M<6lK&*c7^J^OJz
z1N0^isHfy6K^};5=!rXEN@x9l-@j?oq#$z9q~5qx3;TV1dl?jzlvKO$yS!D%)?c3E
zUVe?k%Q8WWS~_p{H&l1k8h7p4$r-Y;AMt=8#UfER*$;zC**0?IA@XBP*ESA!{>3pe
z)I{ZP$HY^md<#z>{&o8Mt>l)lqWy3`<s9ekT{^~_=1qi!XYi1HAJ_i#IKZ}_j6Zu|
zaqHD-?kmC7h<N;iLnz#@r|g}Tlphzc-zPzmZdrTGln)eR;<1vV^ystns?agOo!$bz
zjy0ToYg#niWBSdH0M|V!sW8lc`Ig&A%|0O)=5=F62kqCBj<eF=`OJF$18Gj;e6L>G
zolh7IN;+q+PvKyB*VZHCeo>jEkQS%n?a>;I&<#CYO{ZJ`Wr&N)C7C0L3T~Lm!`NLU
zr92d{>7~BU<BUHG-t;3&atJLe3hY1icLvRz?zA@s7?s~fql?hbC3L2zvHly+IIyI%
z*9xWG3wsU08W00re;flHqsBm%GriR5VOCc^pQk;T%dkz~<~4!JWBYNK;q>J<`;7@I
zZrDZl{u`FndHZD}sGF|6XZ$+gW)d5rs8fC=FTXzTs^Ri_sX|1(kjqgpC6L4XZ0@9r
zZ@HX`;-D5(EIn+^$)b8t`HKrsPM8<2CYny~yP~O6v3!vzXawn$V5)`qKWA+#hnqKU
z8sQ2O3O9OWhfmoCB%c<l2`E3EtQ66hj-A7mblvNQ4B4p+?pvO^)5~ukW1R9d9F=IQ
zZJN-IB?e?`bC+TtVUa6-tAL<ylfZ*Mq!*S2(%}x7XtX%wG>aEZ)%pv;psHa_L78Vp
z8Y;|2>F-dLdJ}%Bft%UqOd~nvBn(F3Rr|g@IEMsP%E(O?b`HmQa(3v#PRkC;Wg2X;
zCzt^fztEsjDZl6NYq5nuOzFz(66hYUWKpxBP9+2A9+<ro%AUcS<m^#K>m_4k1lT9_
zTXONmdgyl+*uCZuBY%Hfn0JdH0K(I*N*-yO+hf?{s~l!&W%dF<!s=3rv}p>hmxfOK
zr9*Vuo^}FM*Q?)f`aCrIeVW+4GMcKqwk(2}c#2o6MI){ly^|vL+N-%Qi<ehg$Fg-C
zH8xz5z!c8L=46YU&z1FKrd5mhGqz8kbuai;{+6n~00pjChwdb7bc;s7us0<ubp7j?
ze0e319>r0@;xaLc<at<O(s4hj)pow3)`+gWSGNi{3&o(6KSK)9r<1U=B`X_17ez#P
zgqKdUH2XN<T*nU_LT$kk<yrPKF=_IoNTQXW>Xc|zl2eZi-XPnJAv=pxfToQLG^-&<
zisN%^=2$c6g#k&_vFhsveOtDoZfbT&F`^hBuXyu&)7B9BDCiBMbIJE|z_)MgfBTx=
zhjh+yjXxaEJ<`iVQ_Z0X=KFECLg(R*mmN6_;v!d~4X^O~8`w~$V~ae;P*X3RbEWCD
zctZ-M_9udJ5nSI(Z=K)Y!h2U<7TBk8(V~XaBV$dvu%(}OuUgkS0h!&Y`pLZ&#{JpX
z*u6s%X=-VS_E+--Cj1gkFI^6*3?Y@MriYLf0yniq|2Ph!ELXYHJHHEM@AUW)c3w!L
z3g$VH!*;SIk?nDkB?*+(ShPD$d)#aCwyfWmYCSGoc2FUO>@7Ke29lEcBLT_JYo*#a
z<si3GLN@8!KzKO>e+WVScr^-7%?a%|ydJzFFS)i+6)6ZxTK}aq#I}TJCyK-fwNl$S
zmI@^5N}f28G1frZG{!Y<%xCh$o}bWGSI0)S@WDJLb+uoq?7s}#rCn9NK|`k|7Q)~W
zys|-@2r9zIodT^2he{@H8Pp4`&HJm&M`ZY)z;-{5HvtUknngoXtkn4_+UQB;ji}q)
zV&v0uAI^{`GO%1J34jrP(0>^)VJAO(-Zji&rlp&bX8akNlAwO0Wa<w4l9KTkZdI?9
z*|q6dYzFd|{l@N847JeRqG`(oz%Zo|?{H(k(!!XaaQ;T<vNY~(`Mwnw6nyC*J=FYt
zE^(#R>e-V^RgA1DDwM2M#ylU;T8v7a@PB=J=~aeXtG)d`*lS5&h3|m&85`S+^#i@Z
zKJ@?3tRFYk4zz(XrVMnd-6oq6zkdCy3tS!*8Mdo+iUWTPG>7;BwLQeL`c8C$2yh~;
z=KdG`FVK(fl^(V(0M(0#q@?i<28N8J5HX9nl>HLWx&l|I09UAW?it!$n~}cziGFKo
z#Mlpux&>M%*q&UwX~Lls7kjBqbKu1I(-;HBjFsjMePysgR4Tb4;Dw@EKx9FS!VMTM
zGGsh0)8~NCM#sRQ>VD?d;;`uD%kNP&XRkeW>3gH$+cW`6Q!%)nx_UG-GqXOmOY>R)
zxN{H_D};u=(qIBpU8`HiHZa-Hkj5sEUsMjxlIh-;j50rgf`9j(<v4j#^!)i^)OuRl
zskzx%lMi~JQw7Q~Aix8^i2`w{w$oGcAI%oLJW{s*VYaa6>qSmb8$W7Gs;a6OjNq=#
z-?x<Dks6)KMn*<SSy|!h0k*a|pbwY~3aKYgo;0RR2!X+ZE{O2iv-zJ2@}s-|eyGaM
zs~(_taY;l(=i|qZbMf0ea`*GbZ>c1JRW$t&37i3=b*{$KGI#IZWj=IB6s{yCB{dZ(
zuk_cJ&eI4rN+8u|MEjniwpb!gYkyR$)~W0G$&+R)mjXboz;O7K;k3-!+S=TZT}x;^
zVSBlg?=S=%?WH{j=@|L?){wX1NL*2>ExN&T%)du@$K`K)5TH+TVK^6<BEVpZW@qn#
z0#;9=WfMsMmW8V1wFI&oaN)FFp#62cT43V8?tB0`_|CNaHZCV@rEeVJH`p2xrVJLK
zaqSxyX%Lveq6|P=HL38&>kIBc`Q&KbS?`ocwUTk^J>v;NJGvZv#KoExd;AgvL2m!!
zJ#^=RU+5LTCg}1Mg6@>YjT<7u!rGb%ycNEgbDgQ~-(Q}6_Uu_==bX{)+bITa)U6E%
zbtF~7052FB8z*OEynXlXqM)FlDWVHBW&ilB-na8^+)4)X7kBLm@=z(zo*IOurA<$t
zKHa!f(<z`W(YpJFT7g6m0|S5y=F_qgjn8~{cB=>n1m)dwS*N*AB5XZ-@j~e|S)dF?
zA%kB{LPJA~XsX4*?ldtpy#Hqd2>e(M>|72ETP8{|<V4a|RAtn(0B6v@#VUfLwp7-T
z5e}h#{J7=s?~j=4>+9?I@ndaa2Y7k@u4%k7h#xdBEGpV-qDTJSHQrP9_rpOeIVesB
z>Q1{{@TofBt$`}ZByrXk(a|z5r~jyPkstVLfYIlI-Td5~FffGylzH~#$?C$+y6it~
z?a~q8P5)tFde_I$CTv{nf=at=TG<a&7SE9y4dM=7VrLTAwK&+>X9hF=MP>URPC<K0
z^pJs#)F^a{gQKXlwA5|*c|wAjmsh2X*LZ0piJ+Y*<MA_ZeB7Gl)TO-qPGAgl@y*zm
zx}+SoJuqlyR{<7IgrEN!SfC6!pEr+L`D~patpr}>I{UBBURS~P*EAg+9bFfPDiRBq
z$Kax(qMeJwEMTKZ$>^y-IzZ3W9Vj=G5IldLDkHRjerkDDtEE#m^|P|FJF|<)qGoRm
za#c8GJkkVQ!ILiZmn59HXg<|H3buB5MPCkWW!zAO6|iA_HLaYH(@!4`8uy7^`7jgD
z@QxY8gb(u*pcq0>sXX8a|D)ZkRFwv_ux}X|B?6g^<MipW!a@Na<<L6x)`S-vbaPd<
z76x_2tQt&++so2k<8ok*dxA9RAZS-xjlK=II7C4GURGA7qo+sRqKS!#A~qnQ*k#Ik
z55=&mlmbJGJFw^wfP2h;xqg^X?FQzRa&Rq(W~<(FOs1%~m>LPGHi78^xUVPeOv1yP
z8@w@<KpsdPJAdVh5y)7MDat$Jehb4@xxma);n$-)e4(B?fNQT=qZWs&%<kQr{jqvM
zn?&3JzZRnV`$Z2&as24&O75IPr#t6Z;1muVI3OY<B=^S#m?&Sg6xg&lq{ct@@2cNV
z#!c@a;Dr6Fsj>VAV;fN3fRQ5AikF9{A}cG4GvZs1D&?JPf3fS*&-Y-zmHM~*qbw&a
z(a0oE(kk=(BVqlg^jg3YCFz7Jz2uR-&=1^VK$WZX#RG9{dc7Yc)40=;Hvp;u%c*Vh
zV)1_x%iecP{XG8fIaeLH3d!Q_^M7}v6^tK4j}rh6{q!pu4&Pps`DO!ms4K7q0b3#r
zv2%o1sT6oLMc_eA6P#d$1DAg0v#RXojUGLD;zThJOnp1AUAsoNlKPqdWmq{A5ElP^
zT8<J~>mnE40O%+!71GeqFm190>*VX_SMM`(1CSeo#Janm8RoE9x3(ynWUqN97$%Zf
zp|Zbvt=_f&n>%&2q@463In#2K!xk3GtA@TjWlw(fDxd1t%#yqHIhIvI5-J2~!1BHQ
zBZ0EgQmuj;acJk<h+;wAl+q6$%;n|f0g~Rjdp93x3AXU+)AM}5F!&=;5SVaMfwzT9
zOn?<)5rB(WZ8va7Cue4gYLC`BC)}>IE(5UhJ5g7>Zv<R%T~t|lf=>(t;qJ`><pb7S
z6;|+}Utgb>TetgUz3x#ASTMm#gFUZD`|BXs`;c2a(DDCA+uHv(DH%9-OTnrD0i~#0
zVeuj`;+S*Ec&zGu1QQc^{3y+RuopTyI*i=h-coaOa&i%nc}3v3p^D5L7&#<j?dwUZ
z$5|m@q43L>>p<Y^)ikB<T+RQpH2p1}#|jIWcx}B)-#KcYLXx+txSGAd-C?f(I%}_*
zJbLuV^27anfM@|=2*3^XPk45yf8XxE<k@cPG?y-1&;$v5E=5yQQ}n`xk`7Ny$UF~q
zo9^xHQ7LwEN=h-veLBOr|B&5niN7+V{=*XXZ+3b0Guyejxp@4s{YtQrz|@+YYS7=p
zmE`IqT8o<@cDUQi?(daVHm~b|<VWT4p<wLNp_?wAKdbXUsR?WUb2D>*n6$7L+zQ~N
zj+U08i%Ti8b4~tlH}6a0PS(d-3jjaH#W(xXE<3!nE&(p}(Hh6++jxxKN=pnQs|ctj
zI+m>(Jj%pv8cTtjX#s;oHU=)g)p&kFSWvL&)7IbGul`2u{^y*)bPi{pu)7;fMTD7e
zaa8CQ0R9!0j;^l3C>lDqEoGi|VZS&&2vR%aw%*^i392nKyNLSp`dOs_6CCf|bNTh<
zDW^(!a5~Ky<<QOEXB?se+86Y*dPzH&D)5Qr(Ry8KhIz=uUgzAyJK^82N2apHwhMrN
zB^6Rp1K6>tc5i$8H=#Q#71XPv6vp7*RF)zjEGK7f2P4&cHLu+WSRB#=05lBPPkrFB
zrA_ekBeDy%0&wPB7M>b)nP{v(Az^Dhrw~7r6%Gyt_=(xIHs^OzX?xQDG>ka^1#>{k
zF@8oXaeaM#-}>ne$T)NJ^P<2#|Iahd2EJh)<HN-UEl&S@?~qvd^QXD08JHTkMerw}
z(VO5H%CxUXJaWs#$Ot$#x644O<cqKd$*UaP;y4#qIrYE_=mh<fbmqg&qZoJ-5DqiL
zm)T~Yedw!sA3S(4kbjL4mfDvGj<nFOoJ7j}T@GBv4DrKw1Hah<h<FYJw?D^Eu0H~V
zc60SJtLVjxC`{4rBdX!?udMh^7$rJh{S5ec$}|=4|Ha8A0quWEk~%zr+dm)p=u5$A
z4u!>cwNk;WcffZ2D0@IQYIKDT@NwcwlR-=97O=aBQA@)(E?IrRq&95&Y)w*C>43v`
z7#!U=&cOltQ$?V#$sJe_+rV}igYpC%Bk}wC^{Xq0b<;|uQx_-rqYS7bpdqqRcjqH5
zpXul&w|-b)U<1I(azL0wg@m-g#@@2YXDO8--T|zOiU?y3p1}E;4|XFk4~MC|8zQ>(
zQ-Jf9S~-=2(-LldZyo_>Yko;?<Y?^Rr_6&<F9qUw6mNr843*OXXs@ZM0ri^6oXkXH
zK>tRMY89xbt^#rn?v<Sc5--3HLa7`6?Xt@Eo}OWBSLW7K467S>3d`1TMC%@w%O-5O
zUgh0r*uuK=6v+IIz}~<0(;;E6R0nU-;E`UepZylsUm^;G89K%z1Jn~U;4A_2Fy_5O
zY$XEA$-*sXn4X^ANFfuqh!~13Wq)2Z!Wt1Y&_DtGT)-l4_m>z91M;{u<HmKvbLV3_
zAMD-3hx8Se^*YoZaI%?g85{o%AxX*p_T(^4PvXBF!%0vjn}g};2llx<M&4jG;H}*B
znOkLM92#o`z1|-`zJHfPc!1h4USNK)3R$@}Ka0gGBxX_Nqq7|zhLfX=T(VyN0`g6P
z%WhAuaNUd`QpXIwh(dX^iL{j4AtSpBwO@B!T<j}|l>{be=3yjQglrHoNfv>_HbL@$
zw0c)_av6CNRR{CbW3p8Dr&(D{((wb%e8JZDKOX2trAxNhWEcb&P{0T#5z7r@%{H9j
zQT$Q8gsnv?U+)xeTAHmPlZc{#E&t_+4><TEX8pnN)vH%}>%XI9x1$9B5O>V^;!u)3
z9#JA(@o<8-2oPQXIXe%Es1%Ewv@~;9B;e8#a54zTO+b`Dtt)F@!ICddV&wE*ib+g7
zM%|2nEO|XS#+4*X=s}@Cjoiq<!Z~nM@J%!ul>ZNmSp*(2pB?1??X+_#2-;8ruH>%?
z^RY<%N9yD41H%%~$udCnj?H>yt;CTt6R*V{DFbxx^5x5pDeYgrn2dV=t+2&%+x=Rp
zE+zUK7jf!uOxcfx-m4FmP>%2@0J}L@gY7}!K&=1{cBy-1E@$Z5AQ^{mhXLCbL8=g!
zB(;IMAOdfhk>e0o9i)Z^*mcwcQB+#`yRRSs_~sdKCF0@lSp_7|+yliUP$>GqiLT~J
zO`E;9l;VZW$+wH}H(R2XlO`Yn)l7yXs{L_BU1^uEfjwTns%oI8VMDM3Cz7al6Em+?
z13F?Kx=5F+=DO5NU}K4TEa}*B91w}(jsPk%0>zaJg-n{8R0QCGpz!2W0x(%X7d>zE
z$GrjTTJAnvX<6s;3TSZ)rcH2Qv-kkY1zu%Ks;J4x7v5V6+VO8F#X|q4w)t}dwGACP
zR_|^$O*nh@ELD(<85}*jfQIFj^{AMGEl0KZ%T5Mg16$YYJ2h|#_<uOVlyBvQDEjE8
z@%aqIPz`7fXLfwtzpn9bWBPr#fwN+uZ1Qn$FR_iC+)CQEirAfwjT1k(JJX|P-22W#
z5&)R6n3&!$NWDMis10d(-+Pb7zJPNLz$`3Y>j6qlYP0z2wQF2;w%+AnrHd<xcYw9M
zaid$Y9!xR|mkjD;k<3AF(rFhD*nGi>eQ5%HTqj`t%bP0`V;0EBmfA>CXbH&u-K*1|
zJKyyKn~F04LU3}bdu^sYJ1Ob7GuY4Fz$cRpuup!bJs}7rhb#%?KAKN#z{+z6xDf&?
z!J*d@5Q_SspnaX49kE`Xm)8c~*7YO?akt9@C-m#|bX+rlb^r`p!*)SeBqg(#mOSiI
z?cA%7<VJQ*&V`GXb>Bw-ib#Tjq691`^_D!K!~hkfJZOqv7zSvD&t6@<KM#aMp9Zgq
zKi^<tVF>^dBy6|*QD~Vb1q>SipT3h*G5g7r^DlT5hXAtpw*o&OED6t1Kfzc93~TJi
zj{QPqE5cEDVqbwKY4B<b2+R3g*rWx*w`ikF79)oV--z2l>q^LjJlhR?JV2LRh=vZf
z`=C^3;gg}|{clZ0OF&}ou7FQ2aLfA^q5#MtDI`2~+JO8tYYii9QPWnoQ&)yOD54tt
zKq;>Pux~SB&DLlvVk(_b_@;_gb-x!(huNQ5+M^=%T?%-u+Q7Z49N{;w006`vC}G(B
zUni0Ghu&HEt+$MMBPUJ+=SKRLDmW2T0yG<aAoON|x>X*Wb=>j!I%zsa7A05&0iyw2
zBh12c`y)U}k#R;iCb9{CXeV)Wld@FXqj1tTIG>%DcLf~olZzm1`hX-f59DHEzVAS4
xrw`eWLlR{8cWpv<z>oi)$5Q`tiWWto6PBu-yvHdX0G<TW*3iFRdG*eN{{?C2-Q)lO

literal 0
HcmV?d00001

diff --git a/nav2_smac_planner/lattice_primitives/visualizations/90.0.png b/nav2_smac_planner/lattice_primitives/visualizations/90.0.png
new file mode 100644
index 0000000000000000000000000000000000000000..29353676faa470c659f8dc5eff386aa108bdf686
GIT binary patch
literal 12672
zcmeHucT|&m*JS{^Qq+q`R{>EF5CQ3+A|hS7bP?&jOAR)>Hqg*JB1Kw2dXJSzhfqR~
zf`l515ReiwC-1!9x9*+yzO%krv*wSvGp?{$5c1^7`JJ=RK6@XZ+|W^DJiu`Pfj}^-
zU%#r4K+uOH5Om-6?SbD&b&vdjZ?e9sH+>B}?0o}lyzLO$Hol&&9=@(lw@>)ndHXnd
zxJ!s$6qPu4!qL~)(??EB%<Vt_K-9zAL9CP9NQR5-_q=ZEgFrCb(7x#2D(5;O5a&ti
zS1%jhO(Bg2`x;HAQkQkFGu^xDEwn3>{iEvTBQ<9jRq5?y%ZZ4m<&oT<9lHc>uzx;g
zQ1GPJ`hf&r$8-A10|&-;t7cW4x6<ItCDUu|JHwxS;8g16H>gV)s1xkbDy2xILbRjS
zdy5W%!OpFUV=jUIX`yTDTm1K;IvZ2YV7<8L5Qz62qo*w44iO)pA@(5<x7g`VAQ0hp
z`@-Q@aVHV<2*k~Z|Nq4QPDq|S@ujmg4GnEz;#DfNY6v<jBqW5Jgew|*a;XXGrWtyB
zm)81~`G`D64A4Dh<~7#P(8y|q@ROF7vAhLH>dq@szEk_cxw{HX#MFs<&zwAY@?+mF
zlM^gmaU$kFRyKYt-#uliGcY)Y`OMXY8Kr2p-Ampo&d=9}3r%1qE}la8U;5-kc=&La
z$)5Ms)iMJ7{M8F=7%A69>)TAzgoPae;`@jlx5kjxj-v#4-(!4#lVkTXLRh@J{b&y}
zPa%(D&>2NV%i&5l`H2D2aFxr)hx=!Ha-)LQ=JR@c4Mof=&pq#hWWRb%E9>CCI7ZHk
zj^95%GJ{v2)=J30@**t^a&`I|L$@Svy$`pafPWRYKG475kkVLQULI-;t#fZ68-KxK
z=dj8<Uc2NViu;)Hg$Pm0TEjAj&i)ixpP0C~oXOUBVF3YEVH32g*Y<Sx8$Sve@A7Nu
zJBOs>OZWQaYuafF8-)cC@M`yOPg&m&4VjU~8q8~IYP2i2LX=pK9jlyGeERH}(aoE0
zksI+y*@=XtB#yq`Udcvuem;tSH$B~M`g{I{V}Vku6P1l-t!r?!;X9-SIOmZO3}>*j
zu*t6Xw%Eu#G+IYpU0u6UP(UEBzJ9xKYi*HW&C1Fu7rI&gb7e)!p(8~i(z5i<lVf65
zBd%MJXTPzr;Ty;0WibTanIorK{&?j9q@SN(mges=aPd8QdV1Pc@{v8R!+5vhO6_#y
zPR-M&Ph*mk3r?y;OmEH=dw988S*3Y<dvjQu7#W>~a44ng?%%(k+au%X{$2Dxw<s-b
zZ86x1C%&>a4#CtTK9{Ke>tf=2-4`(552dwR97Y^HdX$x$d(^dJ5klB*yc^@b^z8#G
z>lL5c?AKR(EuEdpwut91UKqK$x|Wf6qOg7#F2<1&bC-cX6bR>5fVnd1`_tr0wE>Ih
z)zww|*4{9T^#nG`gS;gZ+4F-#8BRgk`^$k0{>dLd&dAG~+crJdyLK(YxX3IUhtqFQ
zmexhdhHlN|raO~{Do|N!yrG5+dk)=jcFyNtv3S3iH{?bYAuRZF5G@z>;&;8DL)YnG
zqj>S5>!Z1)r59ct7r&A$?a}}K9)p0oot>SKh=?1PwY7C|eZ2y=obPpUaq-%vj|a)`
z8JMU0%{^Wi9)t_od{080QD2$r#BXn`m~RjD=qiu?Ad{*H9nQQ6L<ve5O}7hyxHNYd
z0nmO<1pz;l9HL_Zn7^>=61={Z<!|ytwGaY*^WBvPdzp=+cobvO($J7?sb2F<FF0cp
z6Z4=@g-)Kl%+AgpM5@4lyeR@fxl?@_;XY7u`5>EUZ>H)qLko+-Cr1UHXM6K#q4(PE
zFEXdS6b{y9ePJ|7(g}rb+;nDTV|!fV4QRdOt&Om#_06;IOe>Q0A)XNt(Qb_wGttm^
zYK*oR9RCHi4JW*S3us*7X5^MLTqhtg5lr0qCRTw0ZLtgp#6uGrv_$-6QP0lJ`ThK%
zPF@(@dT#6N>}+UY(DSP{KVJj&+rC}!7}5)z>g$Rc-gFij-il9P*sQ8UK9TkGp}NoK
z6+uZvam(k_*2)P82uKa4#l&R&{Q1+k0jt8Sm<6!q)byO)fvfCJ^AQoN`s%?o0J}4y
zqG&hOfWu)JWt_m(7!vdhI?C`UG?40d)Y;kD>+vjJ=zZagnVFeM_5NJBgOzcG))=de
zjg6q0?D*bdYm7u>Yip~4j*ia6Tz<Y#c2?Hcsg<)QPrjx+3b=Cb(j};+#Kgp(cNJcV
z3eNwr(X>;fQ$Ca>3e(@ZeLFMr&70(+HEzYA^1=LXcsDp48d!QDx6CcQzOXuQb8>PP
z3=9m|4=eIS&4tNwRM%AXmo&g9&p_7E!XUl2SQ$3-iB<ZZ@8X!{%*;%CTplG1(-g&v
zY@+lRTNfvLG@5oQQ|r;Sv-vmT_ahKpXRB}TzJUVxiAhZ@61QzS!^fvmU|gi{<djEj
zPZn%WX{4<7=vMfX%+ybBw<k$yGAnPFKvv<ag9geRbORg4ekVh1u{fjmhvO{+QX_el
z(OV03x=_I!;<q1O^cX9=$FNre)zZ?Eq;Qnh!SORaxqicLRWp^t?k*!?J=)SU02)jI
z8?<_!_;WquRYu{K**EXQb8;t5EEs^2h4=qm`tiS|lmD(!1583(od!y5X8VhhkBM3w
zZ@dD*KlN3L&ht|Qqgl1*>tiBjS>xlkwNqsDAk~eWoSdFbxHvoaR(V*9n3Z3TxA0a^
zDA<ia{LTdsoUIw}yf#0WB;gQ=!{N$9HoPHm9Vc62MIw2j1@nacvY{QhRk3tiAN{Y>
zWb!NYiJu7!h}TDs&d}d~U0G>d7qDpT?(QB<f~NK8(WA@qI@;PVpkV*e%+yiz_rmG#
zUFwSLVVC@m4*cv{b~?2_S+s^<m7Sd~HQ4=bl(nS@<=fuwB&h<c!9kt7%sa?a6p0dl
zx0tlh%4Dms8Xd*pi1mSVq$q_bP2MCI3%>Or^lWwnd<@?1IE7PYcts@cj1Cx>*PKG|
z$op$S_XVcsT&j5Xtor2S<YoB|lz?XW-Q{un;VRd`%<Agu4DMWs<t@r9(UY03B<d5v
zYmRgM2P^#Msw#rWPIh;=`m@w|T|WPAJ9{i*WlwQq(7mxBm#nd%g5*l~P+h#m3cibX
z_0UI(#5WmSj<!m<Kgm-a*BQ2k&e4|t?dL>4UURWA`K|QWbv+xKQfKGe0$-e)QB)v3
zixJzuQWJ}i%#xnA)V{txxPfs%E6%H`s#1X@8J#jW?pN{gxhW|rO?V?CBMo)+SDjM^
zSFT*4MQ|5A-3BGh&fdOdV)<%qN=aD*^)O%7>Z)0hm1V)F$DQ4r9*4SUU*U9-?$xg$
zlw^(z{Jz-q3#zky1v;&9s&k1B5d{H@h9ai=ym3o|CyppHkjeV)?$Sk;oFeKi&3QeP
z_1rx3s`8iIE5*oITJ-yi=5la2F=`{%g)FOh%eBUZhJX^$>RXq-MiLSSch<v`w>)GX
zI7k51`TY5+s+yX#IY~Zf)v?UzL*(y&#W{wkkJvPx9oy|Yla17095Znmtv&8Ga3VJD
zno~2%QN;8}(f*@Z$?hoowwG7D*h#o-eecrMm2^$G{esL>X1YpeA8idEMyKB!GILsU
zbsCjh*;L+9sa@%CRPVm5Y;2dknoPK^t1J>q7Gf_psa=qpAtq<9uF6rqGw=2Luv?{Q
zoBNlLIhDupThq>Hzix2#`01AmqjkcHiVcGoDhP}CU&|AGhOAv=RaJF%_DCM2g^+M`
zf3b9d&uFbWm28cRLY_+vNQ;RTXjn@!G+`T=AOz;Fwn^ywmamTZ;8tgFc?KU8uQ_<m
z#>}{l%AGxZ&8)6}l?RfN80<?S6NA?#c$1g#Madg!jZ618c$9alV_w-bt|LZTS%t$<
zG11X)=I32suuEKnW^GpC9IK(JDdjPypiW#R<)(+_SGeFWE*R+RpHko4Ug<Q_(|e6V
z9q|LSXR{9R;x|LcRir6qCrY%Ze|oHaBACeTL{K_<2wyBM>_0eYZ+Bkgx`T*AJL<^M
zY+lNBp))TVsi8CLnE1`r{T*9sL+@;HMkeZY{_0*{xMyM-2Dy6{DeH^Ugj|PoDMHmA
zHfz^LUFk8$Mi0dNcoO37s2_d?!d1K2xMgT;oO77(n&S$wokPYmgMa1QRI&0=&(-Pf
z7u1H}b$3PT_6qF@rR}9po^edRvdzsn%Yek5vc-B^`lgl6<nUz=s{PJ$ZWsEdwnRt_
zZuzlT6OCew`+ZzTDlP!yVqCVzcUiMXY4duLf~lP6`?l^yq!{#}<#oz1q3!8dkFKT%
zhBCP}*3f<+L-tFn5brGs{H{ytNy^AZVrM#G9G@1pIb%P}C3qn!9x-xCNCOb0E&VYM
z$||();fMC*_NV=5OB{DVLQk^v=p7xYrt%e!RIiCF(y&0fZV)xpY5VR``p3-7?z-Q9
zF4Wm8DNzhH)WsWo@AOHsoL^jSlfbo#CA%CD@Y&pM5pLhsO+H=W$u5C7SToLX3Gs@J
zQ`j=NXg_ayn6vx%ajBMFuh^_((2ZuZ1nbSg&a8&u;ehAml0KtmW=XP5@Nv<q^UjqX
z@uzhp6)i&E&Ae{-?y+bkkxSW1mJM50m6(dRxZSuHF4icBx6=H@up~f#B0NJ=<AN{}
zxl<PScJ<>UeL^Gd72oL?3-2eZBoFA@=Zp-?Neu@2TH!5UU+Sd1L|0!4nvA{SdzhoR
z{*;}4(hxQ(zS2!Xu|ie#QDPmij@FET)whWgj}Eq{#K$BoUS{Eah<L9GQplBAoMyIN
zYnw)|rkFZ)iJ7P;%J|{Dtm)===4|ccMg_MG?V$CzonLA`+TYl$uj4KF6v>ABlJ}u<
z(=JqmEkDW|UG$)kUsY7tM~3}Uk#JzV+&Llcvx(ERkBk?Q@L@tY099u>#uHU@Y$*UE
zA^mK76HmPMuqPLDC;}2knOe4SrrP5On*)8fwsfLe%82)~nvE<8tD!j=90PL#$J}h2
z_VMJ4i9C(!eiez)KYoZk7@O(AC5YQIwYIdS=%n-PIPlX?*TP5nC@QXJR`P!2^7cX<
z(S3ZSBRY~deuqGfj$oAN{9I71ytQ>#!QP03u4|hcvG7qGktVU}0BY()#>6{V5kjlX
z$d~3egI3<A+I(tD;Zhb&MK&fD1Q4VG)<tiAOT&Ig>=T2d`JF?_VsjlCue39upke<W
zF%zxZwlZ_!LKe4tM<dcO#&jci`s7N`;DwcQAN$jEn6ih=3|fCq63Mu1dx5279*q7?
z$Dq!Ri$3C>8Hvd_eF+mwa)$#VbuS&urCgSywrP6nJ6kx+PJwTSEK&Tz1U8w(8|u48
z@$qi^6lr@~>t>RYB>=`!?-;66Svp`+iCUjGQ!~}3&F-+$twyYQa`be<Sf{L7Ej4WA
z{1i;3R6bwCw>b!qTr2=e@WLylU+*m>*jVj&oKup7M9lb86#|K&9`)TLcARVuX;0Q0
z7Aq*C6Sv;JJ&zqL{nGohMHt5@of5d+9Gz2T^C^pwQ>(MgL31y2k^vUld9gpj{{(`5
zI;KO}+N|u_P=opA7DhUtV+@J$?N%jn)JlXdEXAB98mMfpisPF@*Xs!NLC3Xt!yF?w
z)`oK;@Yd2&6<O$xRqt-qoT?;_p1Cwd+)>?xxl?o}exu!WMfV%+--pW8YnQI%)sGZL
zzF_BT*syQivhYs~G`^+r)Kr~Id2B@HurqSw8H}wT_7csSNW<D9B7rN~J)h<Wb8-)g
zNh&t($VGY4u>`@em}+Gh6Sy-Pyr|BsbSAKYco`dgI#KP-_Bv9UASl&0q9q<u9XQ|i
z`N`>*k6J3-h{qo@Yw}%7u+7c6$h(fmBstLOB;5&G3ewhHY;Xwru@Xr4kO2-a{xGKt
z&x-d_#;Pur-*3*|xE+Vfl3$ceM=u*evERCAbAQ9kK4;@*`K{m?ffe>Iv9ZLP=2aTV
z^#E;bij0kO{jb|I%G92aYm3_(Iq~U~=TS}!ELL#CmO20cPg#jRo`Sk@^mHr76JD!i
z#|pPUKyju7qifhFW(zRey)UI)?whni<cz+GHJ%UP*`g2=7<JMwRxoipJ0IHBCUq}(
zT_Z}#IfyRY6L|M@X#{ipfdg3pmqOp(*+OC9G}|rTGmejEz2I!O{6Z$<Qe<%`0q(6{
z7Oc3mdt0i@*h-vvo1k|HIdT+lC6V42JnWWJSuSZX$GdaOn4a#0J{(tVeV#$KGIcOS
z(rj^z7*u=Kplyn@K=5e#{5heoHbVDD^%T#_E#g`b6v=d4#Cp*96S4H3DoB>e*qn&a
zt+vtkQ3|)6Q{*Zr*j?ebfLMF(j>3jW4p~#3d8)aN+8PgEi;Qu{jE2ORL9tbsH(GEg
z3@BLCG?bRM1uJZq1bX@z#>h62s*-JYzBA`U$S<77Z;bkLruQApSb;?KjP>c15)Ou=
zLpK)`NECReL?nDOF&Cz+<`UnXZ*C?j>v)$2Mx}3muAeHkBOZsliQIU=o6v{J)=aiK
zx(|^`N5>K_Z0Y-|Mt)rviMsaULcsSiug8H`TZM~vP4!3xtw{#3Mjf|Ndb;p=zfU_M
zedo@?Dz{wu1Qro~gLVTIi=mw%rA@Pa`j{}H@BHcv@u-ZPb2~>FE?_AmCgx7$yWlF>
z(+FRCIC?Z2x7)(D;@ul9?Nay!%K%F(A%Ljm)OEtgw)w*Y;_>coZlpD0Uq!-oHU#23
z>g6pSYU$l~y$!)w!eWX~7Q^2AD^>^=ok9eCiOzWj&hN~ZOs@VP)lU9ezVO>N_h<m}
z=1ZRLR$;U7yvGwf|L(jI*b^+WFm7^QbnW{1=wO9o_bE^%T*f~?Ns{x+2fk#^g`Dm>
zX&E>fn<V3v2?C*Cr{emwpz_9(Fu;*V|0VeX%M+{x@>SQ*$<|m<Dna3xjx%@fe!#4B
zoY(?0*GJ*gxZS%{+}w)%7srZ5Mn+B#hbpBXIC#)}!~q-0{c|HkyAtF}Q<wfiy%Q`i
zu@g>}x_>bmsSQsXa%_5;TNl~=N!ug5P^^d*riT*48k(5oW@Tla9*#z#46Urj#+6{G
z5s6d=X)+RIj+IVj%%GBH+_~=YFVB+Q2&Hz<#>dCyx-L&mu5a;KUb*^59h`Hk7>$nf
z-G6xMDPrJ+kWhZJ3)U;iZ7XM4#LeU7UydHZtqU<Q#%+gTE=Wk2TQK^`|5<TibXamF
zK1C!XB)pxTp0+OtRg2<rt8Br4t6%AG@e0w>dcL_j(=)F0?%lhmFJ8D+j!rCBFT}sO
zmmKWE_{S^RQr+ir6)n7uUI(=)HZ5(;HF0Qh-DUgNvdBX8t5@Bw+n~Bi+P57KI}aK_
z0nB<D`Z_w1X?Mr>D{lOV9>O3o3)K@$a9GY`^?|6inPJm1d#T8DX^%9e&1pUmEgh*l
zl%Su@f))1d9HW6#sjr;j98a9H@H%i41xl41w{n=p`e;BpNEMQ|KksuJt2brX%M`;O
z8yhPXc>3G74-YB=SL{c<u-rkb-D<HZDTUUQS(DneAs0THRDS6u2x=NQ5F(|JJKM)F
zYoMD*5BqOS#=CT;DV{__+GhB8wV%BOT1Gk3-%*78qnP>}lGh<UGzW&6zy>RL*f}`N
zeSUJ(Vbo!w=|z%4VCfN%tyx*KQ7BYNFRi4VKwvX9H5EF0R`2rVdj}32Fx1!2oS&aB
zTkx6fJ*%B?{xY+|(xrra&~T@@ZlAKeq(&lOtatC;P4kcJ;w?|Bn>7SgT)1$-_uG3q
zFf0ncP&nwvAM=JCN9)5u%RLh1MFh+boGiu<AK&438madB8Pp(j?wo=x!ZqqId;^+D
z;5dzS3$2LzqaIj;lwRvKwp2pI#m5&|W5U`i*XQuMG@U`Q|6g<lo`SNaf9ec>Fqd9X
zOiWDFY9IbSS59|cSsC-Cglq(}OC>*8Vfll2F07MRu-cEw$+Lojxyyb=MzQ+(`rvED
zynM;hYpZGoa6(u#vA54r;L*)$-TxQ$*zd&f?f&kAl0L+?vPgDNa@H0`3;5I`CAZ9g
z+e5_fn;E#e7T&hCO?F$qtAO5`bo@pfGyC)>7XvfA=@g~INwXJX6B2&Cn1Ik4nwsWW
z*7-Zl54=NVrOF57mzL^-N5Jft2uW?J6OTNk1p`cuR$^w2&s6@~x7SrwRg>fcKp08x
z|FP#KI7@Cr<$23w!p@p>Y)p(Jp)nK%?S|=XRu=y>o)N0Z_acN=16S)eC+Yp;`bLNn
zJZQF3r_z>dwD?{ot{b3$P0Ynxhnc=ni|RKru@q2GlJ&{C7R6Hpn0>CWf+j7S=PK?V
zIR=AgzfY%PK)-n8gk)goO1tzNG1-I3#t3dUHCou1ru}!(xZU5#2>4?S{?qAzx=IgS
zODrubn<NLpBNmk2qk=jqULjzajs_5lz$p?^QnG4pZcdVNi33}ft1%NobLulI-FabQ
zO;FK=&YZacMG3MR$i$&d%b;Bil{rK$G^|^Z2FuhqrQLhM;K=}G_whStfakBjgUQQX
zfux49{`T8j$b)!G|3rQ#@@!wr*RNe52ur#Sl-O_GZLr#)?3^ABqs&>mKu&@2&=*d}
z@KIkL;FRBY15s^@Wde!0X-QBWP$L>YXJlhz^Ng-U05BPBG+b39xkw^1FE39|TU+}d
z%DXx-#`^?|qcuaSp^CEmTtUU3#rC>)xs|g&=}PMBEhw90m&yjHU?DLvkG|lL<u7b4
z?d?X!#&JiE9O;m8Z$=52)!+3hBbisa>RDJ=%spGU(%7i)%=zc8^-md3&JEGl+36wf
z8rnDPfe}%MyK`QX!g`sF?D&EHFX7N(2$r{T;Z3jpcY%R{EzQkNA(lo)*)cJ{ZG<$I
z?ZKkYu<ZM~xGpRqVZak5h?{(v`X|((Mo8Sf2zcT7;r?#OccLlw5QpUT;Pr(*IK8CM
zqRh-I;9xq8Dzof!1b;)$clxCjl0x=4<n>=`ym0?oT3Uv;Z;wkywtoEzRhI2f8p;O#
zp@H&W7|CmC(RjhDyz_iZ>LCVPrr8(LV>i;3Lhl>Xj>@5bl9wSGXwq4O9LI%`8bD-y
zJ-t`Nj?@CDo*bzfI({iBqvVTjDE?HXP&4Jt?x@uc`9;UkTEF~)0)xQ7nwHj9BXe`k
zJYy)ukD0A|FR7bVxu;%><T{()DVW}wtM}&G%%DTM+XW)ngZYJpG5n#>$b+D_lA8t7
zr*WEMeH&BhgkQNOCFhTTxrKPB{jUhHvz~GJK~nu5@Bp9uKVSTQ>D${O**Q71!k<bD
zMx(3850FM`BouZ7eqMbEeiRe})Vtx08yWZS(*sz8=et$7WdY-P(2rMYtMQm^)!`}+
zJj4`wlcAp8;hcOLXz&3q_U+xFY#G5TGc!+F`VZQnvib^4guz(?{O&5T`D8XCCM;ag
zJV)~%z+f1v3n*@+QWhWAy90JypZQ7WNu$s~4KKdyqQGksEXEgn{P<B~&->#1qN2Wh
zqk<pgAirjT_!KXTzu?q+Yo|m09j7*mXAXy6J&P~4E+YW~K=N&vu7{HQOr(5>T1F-&
zbD*lmLwkinV8@D?mR^0vCgxrl+8QSUbA(IqvSm17F$xIupXmBL2Fyk<4lQyS!@(02
zi6|-#+9Fj6sDu08-bj?uc{Yr3#$~Akx9Nt8Di0z%w<n0J$OkOAir>)C|4aX$(5oMR
zWZ$kI>s#Q$o{hpDo$>zI@y7-|g8am;knA^vsD{mp_7vHbLfbuz9H1nw(`^6tMq|*1
zVQwf};N|5lhGhfCuP;w>NV})dOjs$G{xf1?#b1{HSHMAGZf0N)sM1h>y=coiN&oqq
z>LAX_8v!X;p1@YkBtV<3X<`O<{YCAz?JcmBB<)&$YfqIo0UAJKX(Kg0Qa}JsONVFW
z<T&@_XrpL;>`Yql0tD(6ji%GO39y!K!Ss+!<Te)h+)x;**`|AvA%nljyh;a1Oibrg
zbmvshTfH}{^JSeE*?oZS0cvZYz&$1JY%R_WxfJ)QvRP&U^yC37!j4PN()Uk%-87Cq
z;5%gS-mSYXz(a9;#K(bqLPlo-xentcw6H^drtn^=sJ|OFh`$oe5cc6xLm3(wv3}RM
zFUGR(ZEdX?7)*cdM{n1SE-hBSfDJ28@Zi;HYf}$}p|ss-mOOOh>+y#V>qWLNS3RKF
z^uLyDpsj1t4p3C^ZGL`!3<{MQrMz<!c3A$^?3Ryw6D_DKBr9t=T00mh#U%`DTt(*6
z<VAuq&hDY%l`Hq>!j^|W{xQ#oFh#;h#`)(Ba5(?DiuL=kFHKFefI-<nV)-mJuKsbE
zg+!_2mc9pnelvpg5`6pXEsyIcrLB1h7)x5`$SreZAy{bvQY15{1Qo2It!)$%Qs1Gt
zZb5V2hiZHr?jB&eSJ`p-e?playj4_Ypi!Y8&C$S^f%cpM*nF7Xf6Me=x31=XRgDE{
zf<-^u{pPV~^*4IOwE<h3d`Zb69MaS@v9z=lDAIYqxqfZf&p2fL`ebjM>_D~G8yLd-
zz{`c5HJVdj+^}YLynwN>aupN0<xiWDLN{0SVBCyNO3JrujeGRYY}Zjb7F1SY;W@aM
zw;36iXaf_BSpNQNrt3A+na@_g@<mOHfeKpbK|o)Au%~%y1*+zhmeF1l^QjETli;=a
z?uPZT0>$7pM<8LF-ySea!n>R|=2jqWu*>+(#?V&6_VV04p=GqEGlG$`-1GYtn%Dg0
z%NLsdlB1KF4^VIn%L`jPu>FyJS|{Z#Sk$9H%{QBM!!ik!g#a70g3Qnm63~2ejZ~oe
z?AQ!SQ*^&i#p~{V*@0TW0{Gap;hVPp6@6M8*Jol~1S-dQ6CN%%k}|ZggU8Nw!NLlS
zaZ<+FFkNC{%Lj|hDumi%`Oa|N^nf`dl0O$t#A_Hd6E5|RtwjuBc0I4K(3f8pxjn(F
zqGx}7w+j<cp2_Rp07P$Y#IRrBCQ-XytH29kN${c39{j*N+tk1nqSRnqQqt@XBC%3t
z2Xq5(eFKAJ*E({)Ik-hlj|!V^IJ;1-{eYXuD^q54RSZpk`QBmTumFPD0TRsvOS7B1
z-cVi8HBB|kO`H!bIX;H7UUK~J4X<{-w3Vak1*f!*Rj~>9=|w?Nupa{-M8m4MN)yer
z%a(~h4`wc?w$1*kM%Md6DuOpD0jw1j6+;cdRUt~&G$(%XIb~ts-J8K*&C!B$sR3gk
zK<zUvdfk<_GkW^C&kfkAf*Q;ZSR8wOMm+0&H`2@N9SB#nJARyb_2F?k7GT6Cm@rC`
z+akU=9k^L%?P5W3W4=#^teRrbYFDXUE3GXhBplDzSIOb?>H79etnr4sI~uaKc4Tkv
z-ZxbA4+s{G+`;G?O`<W=`KtQ*`Y+45y6OGU0<RdmoGH&mR_$-%QdsR$k%KI<Z%a7*
z^oUA6A6SkAYiLXy686vC>qr;d6D0&=vQJ&z7cLr?o?Z;6ShfT5O#uHt+Ur^XSMC>*
zwUiT%pO)bzB{vLmw7Q?OOZbBM_;R&}=X(M}^9aiYXzzV^yvvuTtd|$xG<@tsnvlt3
zjd_H)rmFIW0!J6Bd+(U+O-xt=?E6*Ne2O?moyse4)TPqehx}sw8=4>p3r_e=@|=q4
zsVn?Ib58gkJ9jf9QNkh3uMKEWdBCC-x2$*8ARc;M5{2*`4-1ekw61&M&hJ2Da$l=F
zrqLY;=-ds`ha-`)`*zVMiQ7U$mdnChBO45gb{;|K&WMZab$55S8_Pw)nr2g2C%pYJ
zEcWx~x4P*{Ckv#08|uLfLrPtA9mp6gbC|2bP>O)QJ-rbJDLaGwAC~{{WV|(Rso7zU
zU6<H2wX|IOW(J-=KLica8IQK)koPa-cbe)*rS){Ma)*7juwJxfD|C>`$zGo>mK`P@
z1=wV``=)32z3bbDe0Pu51(*Z{)q)&|t|2Bhkzlb&`PT<#vk^fW9M%8Sh<1Z{sXIa3
z7S?dDn0b^`z$rT|a?#eMs=%kCZ6Ot)YlXP7Bd(T&?U5TMY&br+J{o$z!>%RfYzK7p
z6mEHc8F~MCPoV!ZTFDoSniJqSWC2)xxv*YJ)NLO@F|p3UceamLzCY%bgAEja*g+$G
zZfcSPBifzcY`WwhIPICEo4&BRNFba&d)5myJq~JbZ|~L2M@&vG)W``LSUn5?N|#EY
z?yN`hPd#F@@}&I;DeM_fuotUex<9deHwB22%+?yo2X<|yKVz}-ykWmyPsl_@CnwL{
zad421q^46hcm+j8J8C8(<x@hp)?`8O^-|!8S{Z*Pb}j!u-VC5n2dXSOEp6eBot-TF
z*a2Pw+moLUGK+kcfO5*idSl0%D5aqv>l9GQUOB&385*MUMJbHeZ~tuVfZ_UE$kK<s
z9nrc<GyMGgGS7~QJT*Kxji5ilg%zj1BzS&e)G4-VNP&H7S?Lka2P-fwNwM=a2hQf@
zLex^I6av)`PK-lPR(5b^tf9_lXJbmfCs#KBAdhut{6SWcPuJ3PblgrofRary=FQL$
zW<sXJKv%KvHWjlyDfY=MI9L&Yu>&TsRM?o12Mwnx6`EMVvWxVnZ?q4<hcr7C1Qt0E
zX8d7|bO9#RuckHLb_v2v=*l$$ZawJR`+MI-<h~AF*b4$q;4>dj*#3HiSvujQNyTjT
z6{mdv^jO}@%S#7%fCOxjI6$@PSXx@{5>iuBmF=KfemMl~T-cM1CISE7ypvC)GH6Nb
W(341+wEqo(P*>HtioSB|{(l4BCutf0

literal 0
HcmV?d00001

diff --git a/nav2_smac_planner/lattice_primitives/visualizations/all_trajectories.png b/nav2_smac_planner/lattice_primitives/visualizations/all_trajectories.png
new file mode 100644
index 0000000000000000000000000000000000000000..6c356ad7a524f74771753c1b3195ab88d3825a14
GIT binary patch
literal 42112
zcmeFYWmFtp6fM||Lm&hQ7Th&B1cwBIOK=Gq+}*8#1PSiJ6WraM;BLWP8+VsxitoL7
zGi%oSH-F~Gu#z;DuDVwDoU_k9`-Xl|ki<ZHhXw+HFr=l#l|djl2nYmTLqP)mhkIga
z3;5u3{;cV&VrS~?X6R@Fk~eg=x3+V(w)jTwYU1c*VQ0(9!o$MJOm6P%Z12R+%4+k!
zZeX!<G-DmRYP$j&LA96Casq)c4PX9(3q<lQK%nd+Y4MM$?rDe1Ztf~3L`bIs()e$+
zCFYC!6EOxQjL||G5S4<^KYs2Uv4>>-#K9-8691Tp)_|srJCCRpz^r8I4`)&%`e($w
zU4#^qPI`*RB9+$Sc-3XwG9~RSEp93;|AdP^_iYI9LvK#tLjuAAesGy-A>zQ##~@U4
z;8H2t5KaoXd~5s*gbiFH68`^R|9@s#K6#UX5NT6Kg5u~Dv)s=%*a*<uUI&MS2<F89
z&6Q31Qu*yOK?E}<8jl=^K|>)!FqBcP=$0Ryl5#M6vo9*!aeJ`sbs!o6j_6>*Mo$=g
zG;3r*0X>=IX3D&EZ;@K@c<I*<--Ckg`H7?0eOJFdw)Nb$vT}3dsuig=!h94uf1r|Y
z|5QIB!o}U5MKzen`iuqqR<GYNZ}@VnPuLkgG0!HdLdhi^B_$;qha4yr*l7XiIHJI1
z7I_y!^vlNUX9H9Jzxew5EkB5I@Ni$vp#KdHHco1`EPlrrG?s2qgOE`48Ah8Xyo{dC
ze|ArIjs$F6-+ohgJQgmv0`%(n!vp&eG*t<t3gb{LQQSG}7FcMDzgh=ykMU&Y*V4AI
zz<&J0a*v00M9M)CVC;`?a758euV=kc4hlnB0#M0g$iBfzbq;@<$RzSeqC7`~Szz5!
zWMi=ryan~8QYf*F!}i$U;i4n0_I^^;WCv59t7tQj%Yqo@=I9{eTO=<{z6F|8*DkD|
zFu$-N7ykjg2}+S3gy?_-J-FRP-ViiSm<QG!L+K6GB1pNy5SK0H=D&8B!8r!GPg6!5
zm1w{EzXFDN$3P2tTcQakwdM1_nze=pt{yKafo_RJfv>GK5#|2J*YSFA{W10o4HdxC
z5C1)_4pylECi8!L+7p)z7>pfY{9E}o2+^A_{R30}zxoG;`9B~2uj~J_?f+)P3`|y-
z7V@LJ#$xJAtEUU#KYn{$>}b~8>Lw1A>9>>GEY^MAo6J?pk;3{{`W2td`}8ANLiTuE
zM&#E^M=v9xXnPGgw>{hFArSK9%9BqGs<T<#s2&p*iy{@Oa68dcE!F0pI**BoA>ebQ
ze#dS3y}w_o1ZcV|7*o>Nn3~k*Qi+Th+?a)dFrq;IKPS%&W=5b@%m}9t3o_Up|5Kn*
zPAdF(DyCYfjF{<rUpTa#%472rnaumm)xoT!wKX%T@2%mI>!Qtk;ydL|BV0B_rbH07
z2bnAgUPnR#2^dfiyHPK1%L-s8)=Lc&r8b9imGDF#Y`YK2+cUN~_V)Iq6cyWHUY3@O
zpbl%EE^8hrrQ^xe#`)~6qjF8bqn|6R1=rMRZ5ajr?lDv7)skm1s)a>*g<&8eF4H|f
z<rw1morvEGh#X;keFhRvh7x{pGFJKW6431j1_mm$OX)Sr(z*_|2NT4Um6hub)XQ`T
z&Bii*^nC05j*7$PygSaEl+I;=O{<XhYwt>}SpARrL{{UgHb{40-vj@59rkxb@F6`X
z?PL-^(2%`J6v_}c?w&8k2#5ve@I)PkQ|n1Fh%zo*q#E%;pwF}V2XF;}RVGt!$THUs
z?9aP1$NaRlQ_9820YlJ~B0!J4=wZy|Xx*`1ZgOexzT?2g#%41eCiqvPg@Y-4`+LZw
zFOnq8EW=~x{-m(5us}H%t;yvesGy*rg$#oKYs3^7RJPS@843o*l?yNFVxx-TG$T26
z4z1^P$@+o4AYMPzZ<?{yWq2U&{n<vCFgk4#+}$Iq7UdDyV50o1{#*YJ-e+z3*NAkq
z5HJfZEfQ@UgNns!GN&0w0vdm@UMoN7w_#Thug^Uft=z9@6F34=(#X@(Q@QeBz{wpY
zrP99n=DZz5ltc4)wkh#F2+7uISybRTi530=deH87rG~QNGYjdLTJ4F)_jiSX*XG#o
zwCb$2MSJbWGB<B#8wv~%j&*;4K!%2f<|{28d6q%9x9)%m^d242=Mpd6#+;}X(~3lq
zj<iuHozG@{7PVyjk{$G^(J?6|#j+#ob6X^112G?^$WUVI{?v>0118kLIVKpWRc&fG
zn#MO*Z%3Bldv8H3;EGNq5e_1su7^iLZ)kh`SEzE<g(<SO))~m_eo8L<{OHtjv*P7`
zyB_T0g0l6g)^hTUiakcG^KH)F)HbxjkjMGN_uo4Ad0RO8S~sePULEsd0hY(BV#q6h
zVW0c8rA8<BquN>f{eKKx=Hnm-2Zw@deXPksm3(`-VimAK^*o#Fp(e4gHvt6&MS)T_
z(u(K7dvr44C5>QFmUvdo@82~ID}P1*Lh0_k=g<0Ucc5;t$V1MIPQcayEQyh3!>NSz
zvSr6iX@~rWJO00aPi`GV8OSL=QShhKVvDc*MW~2&e?03uyWNax_O?4-YJA-;a<>)d
zem+3=ikxxqRkl)5he8H(P_|?;2oIq&=LGiP|BHZ}pCA5aDZ9cv2At{Bi%>yMobX9Q
zwHRq@JSrmO9P;AYOXLU(;ieYEV{UX>P?&Y-$DXZ(z##WaB_fbPCmcCV?%F0A&q6IJ
z5s|x*J~kruOURJ3g6}~Xf7lsAo}7f-U-{SBT+J@lsTJt!uT-Cq$gTX*J<ul7q&$E9
z-^~Yz0|?{I@o+WCIAmVKmHQJ0<FW`MPpXMDClh5Z_*1`dl!RYAZJ#EbCfiIo^7S#|
zPdr2&m!OkX{guUP<hhDJYn!iV0T0SZ@Z!n|a5@71xWIO2F+(CpAAOy<en-TNQW&<#
z+QIJ*(TAXk{MV~2h!5}gO`6av?~4e*`jf|XIj@t0a{n#klRn(=VN)M;(HF;s{}Nu*
ztG2km2Qg|gcMQa}MdsP32xuUYwe8UY4!R_1YGtfH+J*j^h8B*8=nWQli!F`@Ya8q3
zIa98EmA{qmc%SF3JIpc%E)Hd$xfVDweP|DqVUvr`4(LF%Fajn9EmBV?VxxuJoy_X{
z`#mFOWkK3}ob=l4^tG-t{UEEp7rx$|y~04yKY3XPs3zR5kaln+7#`B!pO(a`F6W7`
zST1`POd=YB_WGsceC{AKxL@rGX`r?Tg^XsEr{al3-L|wNjte3j(X0RfM)Z$ap#J8O
zWZaILuLc+6ny>1aKHF;qB8fJ;RvjPCuY|D@Sj_SQZJqo#z?xJFS<ppP58QAX!<Z1m
z)0Fwa@UPBwk8>PZQ0V{cGXX>@K%1)&@8;mZu_YykZhz96QSNurD!MmW6@kliF4AUT
zT-kqz07Oq*gqK%P_6d=kOVu{IFY=B*<?qLXj<bF$QTGPufuwkU)AeFj3!kGntP}Os
zCILho(I1GUnViXxJo)XrCYQE|TsB^B0?kI{43T^D<NEB<=5L6_(2GD<n-5mBoBG%>
zZf9fR^dRSbY7xJu>3WB^NUPrP^oY*K=>JNRACCSN(PV}x->W*tdo9#1H&)wilB3}P
zdj}5Us`7ZHA<avFGgF?FDdf53^L2rz#zy&=J{Pm3^r>1*d<BT&^q?9WCYxzseP5Ga
zx^VdqnNG#kULRew3E2=G%sN>%w9K`W@w&(?sMlF+9+-y&zyr$;`jQjkYh=uAe|)rx
znI^&tRk6}Y#+OeWtQ$>-e@)!~a2=4~DqnBIBu$RxLH6=Mrz^xG--XM3TE<fys-{UW
zUX{C9H{hgw#k%;6g-Z;qA^1}>Uzy&>e(%!Y*uH**4>y~~sAYoHJA((2tblG{prT9O
zoL^cRR1INw`Q_QDa@w&oWx6mMl|?+9j-~lZ_y%{Zke_fVA8uT@#5)5~$(0}|s9@X1
z&l%P4=A1%9IrK6IsMR{|a$T$l>yJ+j_j1HCCW`#*TGh%NDF3rIxR57%wk^Wa?`6_B
z4ALX4Jz+yUM>ijjr^-y!pRxbTjKJcupxIJ*4i0Z3+20fV7F%WyA`x}+t_Wvxo=v`^
zbKvg<1^`AwBgc3LG8qJEl!3n1-SVe&Y(!l9y{m&CP9I72FV-+QUcJBaA;D0x2e8gt
z(Qt|1Mv}l}4Q_N>P!3n#kF_R)ju&ViC~xL8p5LLjMf{+j17I>5o3>i=jC+P-okO<V
z2le8_c%JS0^KBD7!(51b>bc*;4_sg^?*OaG0%g-cQhB7+*tUm>HlO)6-60u`-daTv
zaNRT2IpWJ=?*tJ+iqyc>$pb;nHIYaw1+Z!JMN`Q)Hzi+@HJL@P_Rq<)0$#R92LXBs
z(sg&Om1l5rfE1|SI%cIbUI<u^JG_^Uy3at%^)AKe$tq=OHB%Rf+O?1>6PmllrfIJS
z24>!8))>2&^$^uA9JoH>;3hU1@x8^jyXN91;3W=k26l_<aXj<|Fk-xns^AmiZ#|(s
zm-~m4w#eRgTyf+#7y<zeW>=Ptu0&j?3gy<6gM=^L_P9d$s^=jXL|cs?7N0F@e5~il
zA82Q5tP*qm#M=O;Krd>6x;B`goboC8Xr2n56Q2o*%l7c;iQ58BTmlZC1>|+(>F4L)
zZyINX2kbCP{TB=)m&$`NU92G~E{>4$!7kEeKKyf~^PaNv(E_f?VEji_)wkO9wh@_r
zutrdJ?&Emwt1m6Qrzh<tNTWZm&BmL!-(YV%J^lFJ-*$K-AE{oB^qLGQE9(VeW8-vu
zGWefhwl|6_ibTNmx6i}%?nE|nB#FR|<6W)QY!-lhLc+qj09-j<X(YMi&_^-{e1atv
zJd{cZPEWhez3-h@dpF9_s-7}~jzHD<y2VcqvK9<jcStYIpKlEia2TTmy(anTd9}X|
z00Qv@Mzzzu{LCgD7;x47inRrjYS%_sYdQTF_zcyqwOkJ-^QB+wXDAQCOv;o^g3DKY
z;KB3DR_#Ow9fiYxGCTUmU~uQ?QKs80zD0!ih2hgmd`yf#0N)LVf3d$VcRQHD0PwD}
z6%kJ=MiPter$Xi2DjixOA(B1U?8UsiJUGy>Dd(`M6Uzn?5++ZlF`OK2A%7fn@R-kl
z>+iZ-pGo>^oJ*~XOhVPh)_{~wo3%vcH`3r;rv;CZ<Hr?us_~)(R8&-n;WUe$FxGp;
zF?hF)n>>|#NO$*_Uvg+8DOzjN1o?{a>HNv|+cI<|`gRiAgU98IC4>eXKh|y?;3G*m
zvp*q@8I57K2|t$s&3y0q_9>Nz=U=JS$TjC#-|e!?(L4LyXhZ?G#Lh-19(oi15n9en
z%~B-IQpj}CQ^sB6b@Ry1V;jX(mD7$U7G7dAFZ|h(GRzQ;U@bN-&ahTnY1D%G2arlr
zFfa_1+O#~w9vU3C(Ma9b{F#%I|DV`90}*3wUkxXbd2jp7GXoVoju<8KI-c1B!(=G0
z(Ou_rGKU4qtBcI;0WEKD@4AzEYb``n)REHxx>@_pi1rR+#@`)F@hXoi=O^tGAluK*
zm)wuA2gazG<>vaguSu;jfAiw8>V<h;vAEaxs{XTSu&01q^=$@!Sc=}5Xu2EGfUYJ_
zqsb*t;)*KOm{-C(IE31kLRrriWfebA@-@<rrZEI?n*B8O)M<DFCVwkRRNm(}Cgczj
zQn6#XTvJoy9T^0N3hcGNYg~Uzl^Lh3%9w#WacJc_+*W;pt4xMW8<&7Rdp68Drt>WH
zKd6F_^|s4z0Aj$Qg^;kr>qLw5sqf&}uY>Qs9gi&P_<9$QPBUJ>D||}(^-l<|iS2a<
zwc1($#ELdu)VY^0$g14EDK^YmJs%EW=@&cv^XHEi%WNgp>2?yD*{C=r*QKlT$uXI3
zz;eXu*Nx5X`*KwVI3cg=OY`NC)T<eG28Po0_5A`>1_Z$7T@LERl33dA1igJ=={-G!
z_Se=ZG6~86*p&zgF^@k20P*v_+S;xX@CQDUlzXio3qbi@SF^FPjxgJZa8-KUdu-bV
z3hecDq<Tv(!BQ}LGDNS{cE|<ww|bmdXDF%xBrG)4f53Q9b-`_CI9V<?U=(;GFB#_=
zkQ7P)Hd3X_BPd9`zP_HUhlznPGM(gPPJen>O<KN#1<W$j_ovuUqL9OeIDI5>4C@t@
zBrzF~fk;cA=eBuNAJ`-?Ap?-MyLZs}+tH%z*Ybb@?a@8r;2^j_BrusbXgCEoK;Zb)
zS1zOfh&xt;;TgB3r<*`<p?w62S*PH+&D{ZI#d~UPf|-(?T^pU^)a7`)_~OKRhWYNU
zp9<+M>#?ukO8E1+$zDmdEy?MaNCS*yUBPSNThG=*2keiJWOpx|k`i8tX63t=B|g6!
z^NDE}(ZSymkOOjJO-+IquL-=UC7+w+S^cP}sJ?W!mE>*DSNJU5Kyp$EyyGR`ha*I+
zB1HmpN6xY@;D)`t;S(krmBL0D^xD2$Sw}HmpLcUrpY=a*y)~a#4yxMyhiX2J)(BG+
zeDP5Tr+sApn|=O_YTh`(BGL+pnc5sXWf4T%PON%56jWV5yMo0k9H_?YN*@--uM2zL
z97d8N`rb<e$g1FEqrV@7jcrW6ukV)t#S1$y3Bw;tTbmb=NgVdI{K!*I<=2N$@y2~8
z*=)Ss&u__jO5S|AiQ9Dem%mY8q+xdm7AWWbx;qr-_I!vHEOq|DpabN6IU#92T{L|)
z*+u#JCy;?GnDwo{LW2L&R3n(eJ@nY7%n@DT^QpnBen~WvG+{FE>cBq~$3N?HuJIRH
zlvJ*d{<VF@Dw0J38s&+xc=#jSfeN-b9Ud23&}BC!Vi8~LVhxR+77sbl#W&y`ecfLo
zEYNLUySAzwOzUcgG5S>>%z#HnR*KZ(+Gm3>NN`%PX<7u_xMIrL0jlHvaxc&cI=Wsv
ztJekOZsWf{!gu%d`~loP2%?g&z!#2sJ8zcR`K@v-lrEzKM^Vt>>B=7-`5TaMA(FJ|
za}sQcJjK%Z=gD8qCcwQWnUux$eZb)L!n?gQ^8i&T^hJ7p(#(%^v!kbykb>iNqCK6I
zJ@uH*F5jjFx42?xR*7fa9GG?OjD%{oHZdkLgN5%=>?hFv($GTqtF_8uPbYrL<w|J0
z_M*?JI<4BKCY2g_;g3SD$IMat)5*I^_Ae1x5O%xyk_+m#LDT*{japEh4;z3i|3&vz
z7*5(+kC8T>J>D}_m_{tQC$c0UgH%fj=mOIPpnlv(x^7^TAsJ{AnIFQTHUdKK5;a8R
ziJGP@fe}o?Pp{MHh=<F>m?mh^p(p~rS=wB2{RP@@dnT2Vfg4OhuChpowd#vH@N;W_
zLP`_dyY)TW=qRn<_jAK|$F@!d2gLO4mozl`9S?#;RHEzWul4&M@_>EV%Vn3L*P~gS
z=&Q0nH*rxUV`gaB5lzvNBh9h+se%py9UT~(%w5)(e6S|<lsYk)uP?`lqf2G8%XmQj
zU8ndy$RIC5Qp8va7B53bCp(aFU4Jzu5_u9FEbG#GUGFj_kI0EZuJn676EOz=`**4Q
zX!TOa*`|qRXavz!>I{%kcLtu{9&&2sYjGN^i6&DP8xJfUxy`YymDWSlij5=J-f^Fw
z{DO<frvZinc5pav@$-AS3MP$3IL*(@>H5O6nXBK2EqZpd>exyY;c@Wjunq^f+75hu
z&?u++yNHgF4+p6FsXT3jb)od*04QYyeg3&?6A2>BG9G!?y0Bl~4mn!fW;aD2uV-a*
zE&va9QEU}yAUC-otCxUGhe0d80?$!2U))nbEC6#eox&Ak`Kc7&tSd-#v$rGDZ;<1-
z#>fbc-gG!RQ^*TWt=O}!Ou1v#FR6PnmzEiLf|DEL-R=kjzcVPH<m8=F+nP>?xbPbi
zjLYNqB(Gdg5_yzW@@1+Vt{6}6prVU)6z<pauYYk5L57l+SL+gg4=2OVjjZ6S#-9s<
z{}uTi$pTrd)dB}!r6Nd3e&bb`)$A5Ok{~ecz1aAl_2yUs_<9Y9`o0zb{ync}cPV--
zJzT-?zWwuv!G1?INb-{U{np`IJL}7hx9CEW=Kb4+`<jH&zmE*_W6#fXSeY)LU9p+s
zZ$}ep;hNX<ojZfLLU%57;72H#!4z$wg7<0X@QKLn@0NOpY}Hbz_#(8{Dmj=06>rs>
zN^M82Sf<cf0rsd;b|#ZRZTNVh2>(f})cw|wh{yKd>X@Oo7o{W4OOE`FO^PUwwH+an
zxN<E<6832%1ucn1+UUMF;_9Yc&!sgY`i;da6e+W4gB+K(kdWvzLMINtw#UQC&qx?<
zc~@(7`aA+LG568zoeNmv&DW$pTnA@t<$4KLjq*f=p3j#m4B!&ASME>}Cetw(ApnFX
znz+4=v9~|G%)iLlVr^95xhF#i53oi1=cm{un+59h&x~qt86Fb7LQXu!AxqA)Wr)|u
z?{DXP<cneNb@`#)9~p!Soz#GAekD~z1Zt^k195oJ3>Y|GR*ogXhC?S!yL*C1-dZSC
zl`h+sPc@!@I7xD?M1Bp*mRx*;+q1_N(gM^UfHayQr}80|hGS=*$gcko5uc_1($3{z
z|HRC=pFe?7`hi~b#DrK*(Be5MntZw`CT1Qp-RWjoIpuatq~CGW+^cg5nDNeWlPs&!
zNPo0djS5|(smQHp*}CQQR*JB%j$dZ3kN8R-kWfGXW-FYC4s{4{7;Z}xHd6q=zrT#>
z%<YSSe@!++wBmR6`LtH-f*Smf-*w3;irQa`L0ygvc|3iTyfX!)*Ty{E1kni$#vJ0-
z%KfR6U92(0?bS!(i1GYT-lE^uVz){AQmF}riyoj(7ixRfv#nrlXQp{?Hrg*b&TT1$
zD%<GDu_aeAatgJD`xOIb)?*&4E(X%JA!hBIq&ia?O`MKKr(6>KA7}v18mQ&9h}B8O
zW$dr@ysgk_7V^Sd@}3vp3VF$ki?wmxkGNlO&Z3=vnQm^x!eBzj-c;4_?O7}e(yP+V
z&kCppI%$`xrC&Z4kvdHl#>K0qi?ABKH<es;V{?IPbXVH>gJdl8H-Twe`*1<6Y$RK9
zTl>mcPLy)c510a4==hhyES*{zQv#1;zGP(FXd<VfMCEgA^riQ}>iAtAK=q_mr9zmk
z+HKVXN)b*^4I6uF{J4JNCt01YeQcyXX{7VZ3!ut+>_mtzcvGZ><9?&tcxP!;LBdNO
zancuwsI1~clvzK212;PO<5dTyNWYU?kstR$mB=G`sEc|)F222G(XgIMex%$J5=h9i
zzxL+1+3`Jg%rlU1tA->n(M_k7ejPd?B;?SrKU(N0(Sl6xFW$EFq+@rKw<liwk-yq0
z>y0pBth#ibmrzIZLj66>IQykkn`8SokBi=7o#Ti@hbiTAFEG4lP69|W=hYu>Mzz>F
z|E?hLNUCi4@Za8a1h(eF)hZ*lh7a62h6*4c#9NuovF$A>HCDCQat+o>h{K|QxA6v`
z63OsrX(c?(BqWC}wbS_r2;80@Hx~&mR1{Fx*1J0HuVRNSZ0^$-60RU)Pxk{&K-8Cv
zY|zUCc(&(9s#%$Q1;?X!dWu=)f!K4{W)%F@{vDV2f4(7$oZTTS<1&Gf=EDVDM)WA=
zP!jxy6%@f|@3k8|bw;RO$Eq)rh=5Bhwvw4-6kJbO@EYvFYkjVA_(Y=&-T9Dyto^R{
zO8C$V4CQhPX>WE{;&)zh&(>k0jnff8#vJ6X^{S^1#G$?AQJ{2uA$rJ_^1{PAk@`ED
z*VjAZ89K^zJ8bHuiOOzP;v^oX^5OBq>^4W<S8I`Fmus@m4w<LpF4Vg{!5&WF2?>!j
znQ6TrD-SFfYFsc$om(+%R`>Qi9~wlBD*GQ>YApx&*i&;=CdzajkG}sHcq0Zn-7lVZ
zYoefh{tMV=h~QlgwY#+RBq2hya(V(&?z(TQr?Sc2oRMf8t*Cd4V%JDQ1zU0zMt@UM
znR*e_QjkAYK63yL)OPz9A@%)*<3dxJ49d|B2_XYJVv1Vv);sq{heq1{={PIn9<)YN
zkPig9P;0_RiAD@;@c=2!U-9Q)<t(Sm0+?PDsj|@H0G;yZ7BkH8(p0&5mo<pp#z<*e
zkO7~sj&G9;Xw~;?F_H%8t+Fz@`_1z3ki~)JqT|l!7cQ9jv+r*WRtDUaYRR%63@Td-
zyAxvi06%4LZtMYD@$ygJ!bSv20pcI?-0Upc6Ej}7+-7Zh{rS_;A2TUze`!v5d)r55
zG8Z?IZGsdo8m)_ro_nd>%&11Z+U$wz{^a7J*WUJNx+n?Me~7D5&d*X@>UCLgz~h@S
zEP04Inu#nC5o_oGU}WKk{LGofpZu5`j~iji(<`}Q)ZyZ9H36uDNErBW2bG7wm~;t)
zh=Wz7Du?-ER$m#;yb?s_Lv7Pp(;4@q<%+u26V|akbiTv~_P#SAe=nb*m}=T%6m^6^
zUfz0Nv{|*#l*HP6%!?z1x^}ZVWcF~@cdoVS5F8Ja&nUS$t*y6B5BE3c8stEl9ia~G
zk9whvK1C40iv{&x<YxvA@z_F!k58X&Ch;mh!ZXn9FB7@1BR(I`MeD*9B)}3F;NLQ2
zwfpXbQ;UMe3u4eSbjw!|xvc5wpk0_Igz^(v_r5x{@aW$W#;p^_&PN5dt8K<bg%3{J
zlU#ku=Byn@Kpd|Px`Gh#S+TasUY2|RYSyG}Mg#_GyJvhbX?6zJ*~D2Ii00ky&tXZ)
zU3&g%+)#kqU4or7LN9Y%2j4zjVT!~fe34B&G?~TfUd8F?7GInB>d@-5xjS)A=(ZR8
z<ax0(;Hade%&j2q9|~B0G^2(>P$1aYca2G-N?({+uINYmorEonhzBH}Dr2YQm+jIr
z-V0v?ErRcF^XYiJJ8w~<>RUHlsMna4ZESl~=9f0FIzY)fS|k(je|)U=b|v~SXm)p0
ze?GtSFJ|-Hq){ETmYW0+a;;=cSRTkP=E>v9^@iwG8ZS6WNLRP<M$h2ERhm3D&P1m{
zL+`BI<Hs@sxW?}JgJ;c3OxLu>xSom(D`M@c#P4{JcIo73r6gu(xrNFd^GRc0>(#0+
z5w8nc!#dJhVSWn{&;w9m$RfwX`a}2Oq-E1L!8YQZvM1NK@6?~4AIQdQi^ExWF$D|Z
zU`HQX-&TKw=X-6o+%(|q35n0JxS&H;#<f?DC0;xzA2Pe1Gdd@nDOg{*?~B|MLuq{|
zL#GDTEbL{?ns4+9&s#jz%2%8u#lH_@pG|9U%$@B|D`#CkoanupwmGJST$8*5SxoH|
zNfvjm&8^5E@Z!*;uI*2A6kP02pKHC>1j(h+X8vjJ4F{L$!t*;v-$6p@D_-`S<8{~H
z;Y>{HmBab_r+e_%dKpwLW$f`r&eNS#ts7<*6rk}7=unL*<kG$EVzi{*V^-I5Y)<4k
zzI%2RM>G}V7uP@ti!Nj;Kli)Sf%;k}ve4pao_xpOACa?YScp8pzWTm1;tz)3x*Pv<
zujzhV^c8=u3Xy>KOKjxXroX@m7X|n9hbb|0>Dfl7$Uv^IFJ+p04sALD(S=^Z>N^Rp
zcUO(^V21baRU|SpdDrKVlo?uaLdvl_wv5;Oo~?dp=qD5&B4NaCwb|;9)j4hd)WHFr
z_ERYj%8L5~!3GN)jRe~s99SEplJsozoyh7~YV1hkQ?9+e7?Tiux-6ZmDO?K+tI4Rx
zdlj8h)b6_<Pi^_<k1~em#`hLdqF0xDaTl%+*N8yE38hnzD%6w;vA+1n;L{lYR*2&A
zGwWIekw4!1PZdUze1J8wnY{*3_&xE!v9iq0?Q%0dy}sTKj3(<ZPyhi&vSqjaiDB>l
z(aB@0mnuq1TBTGQ4LXYZ5jsjMmvQLX#AO#65?yBtG2*UJSNr-Wz}a&moP;S<N8!^a
z;zjAUr#kB)-^J?(X7v&S0a<z)CqcJe5H;<)FNKpd02A0r6}4sSmFdSKwn`8IR9A_w
ze-y9*(O-Pl<q$<KjeacC{gVv5Q??QnIYno{^%0I?B>}+0OggOn{qY#aO;@%6wv<kp
zFHuC+_!FS2+iW#X&VT`LOoSJ<S$A}!6+PV!U`;;XW4_Qme`IU-lh)eCj6$PXmPRDd
z9^cX*F<6g`cpHElLx{tPke6IjFVWgggoTvORe?;BbIiJnE)$>=k-TrxYq*!1xcxRq
zWL!~y`?wrkjCrZc;FGc(nAiNV^G*iYE~9Z-(L#Bd<vh*CQq4ZcP$ufwuyU&jKUmEg
z4+DwWFccEn4+Y$G41Bgy-b0ZpDz80=Kv?i*x*iFh&AS0$XyK-d;e#>ZZ5H8F@;%#S
z=Gv%2dOdn0e-W%rND8f9pRRi@3Lo(v*6V&D5si;^ny-6rzZHT$-eM6KKK5%dqe*6f
zkI(%JQd7nU)`wAc@wdNj&E&<C;~C(Y_0WIGO=^AX4R5h2_i##f*eGj;grn~npAU1c
z$ivkljI<%xKe+apNJPynqIptsOgc!sf!bI-T$&=k$=xE3BxZnK9ygmru9%LT*DWiG
zB#a4Di~HVhs=A8Dy8&UgKex`tzkVeaswkAe=W(tYirq1pdp}G?=DW*__|?~`46aVw
zpG4sDCOhkr$F}jT8%KGhbiPuI>|uOStwRQ!Eiu4{u$AWfP@1;cGS{R=M(}V{?>|#|
zQ71%st}h3A6>2e+_Xpt10WFf~#m-2j?Q#=yl90>(+xx47V1AeV4s)(=HBL%M6Mh*!
zcnzDc84R1AeF;~nS<#X9a(*Y!`#x628+aIvn?n)oR|J8^JT^jGXyj;Q`Dvpy$_IrH
zZ}uV(#!cg%e9uGE#*#U=({^0!cV6LdagO#!gDa2Bq$slf2BR8KRJfmg3nvwN2Pm@u
zorJ)6n~&G!?gV_Fj=Um>9AMOe0z~KS+2%xp!zNs0B3x)E$ouR^xR2ZJ8#Z@)So|CY
z5z=tR`^8Ki`QDVF6Y6ncu$-(DaT^UKr5|=$ex_@hg3Y1kd{RBOXsZ0q>C>eOpIf!r
zK_D0OhcbuvSPa#q`o+Z<-y*V-;R_4)OVEQO2%RwCk63V)UaRNrX&0tat_-gC{a!vx
zQdC4lk0hDTcQ8B>KzTJmvU5dMs;B`W6?#UCb7A~Kqgdf#Tu*1<mH1}$SdP(;d}#yM
zp+!1Krcd6@45G3A0yC+HQeBA=U93vJvHmeNq>*oCy({<fpFy-!((<h@U%H<cnfLD-
za=j{XVJAw)r6q#<_(!e(jpl}Z^*SyiBl7a{@}NB*_MtUk-a2UBdbIv&i|##(Uj!>1
z?5*EaUWdR5!P(^$t>8`Cr;8o+8tY^iSBL$#Uy9XHtn2jV+C`r3(;7$bui&kD;DcUs
z8_qDpGvg8EDc0}I9XOxp{r0+flamM3j5Ku;)f4CqL2m-KjDNI0*MZrdZX*8{DB&5q
zX)9(T@jJ6oyf5YCvE>4!btOq8+-SGAn{Pna-K&0IW74b<3`9e*@qUdYkHsmA4#ZLb
zwMt*ue1PXwm55$3E&==B@2qWXf6Y~Y2`&Eg1r!5)_UF`Jnq5bsWB_Z4?tb<e2L!+3
zHTJVN!e%}rHDE(=*3TB<(<6+S2&k}jfRfVE(2J*k0+k597QHeZKI(aVkFALu={Pne
zB8s_DK+pH(-_%_$nx?mROSxD-j$fz<51#qd4z2Z)ChE;noc}<apXAv_nH_nx*=k65
zxHd?xK&u+Xw)KiP2>oto`#kr>n$-SEl5zJfHA)Z*rG&&#xpf#1w&;%RPbWmj|09L~
zd2vigAK7YxMKYCS7Rb4KAZ`!P+welPL4a_E)AdN(UpHB!@%Z*kikSZ&q9}Wb=GiMo
zwWipszaUB~ueCwk<eS^!WHDZsB1VY);e0ioMw!lF&0~#4L`))HSPj>YF_G2KuXaMo
z+jXVt27~c=FVteV#1EzHx<AGPI3Zy}Er+?Xe|`xygMFV{(Woi7*Z1-j4f>;7S?beM
z-Ua1nw&CK*qAb+P0=2P49oyNY$L;&E_kNKgBU94tg<sg|)uM5@lzas|&Zz;h<o5`o
z4_URkNv$7N+~EQ5&;@&XsMdX+`D*t$j#mEk&rCYot1=l-v1(z!)|RpO=g-@F1T8(*
zJXFHMq`J+npUuq+%Dc{fiS%a1!qwQ41o*+&f14>1C9=5aYMWJq6Bv{2kJDhL<^_NI
zV|V~qkC7{5qGxvY4IT-DqtlXm?N6{Zz+I)-U=xs#1QcsvfZnQ?RAf1s^_5p$K>g3R
zw_93I18`4TsEC;Kbk>&|50Rx{J4EpJS8>l#&@sh6cuZq`YONww6c+tNY#>zBY#h~_
z=u1oQ$o1V1u_9d_GSJw54rA15`Q^f_&ctRMg8B-Nz>tFh@GQ8Pg3)(O504&eNA(fE
zIC@a3wOC(A{A>!sp|eK89P@sfv+mG!ORjLt#j3Srpr+O&_T+v@5z%|jK>2>FqN0L@
zgA*7Z|F->!l<vgve^VCmzyj1)_V`mYFACHDA-)=p6zL(uf%ksm9xuXugKyR?Nk(|~
z_f=Wa(>6Y53J<qv@jLERjN3JMoIJ%3>&a%u=mHO0yX(Owi4n+T2yOR+D7oRc8(}5T
z-PUK{<a|7~--?-UtQV2~l@zw?ml1!dXQ}WL0Tw(%TCdeoR7%RcE}a+~`x2V^AsATc
zs!{QTSkjJnHu_ilV8DocZILnIeC~JKMNT0ULt``FVq9Zi;Saipzrue9kX(^;n~xqZ
zDcW4$slKuM(-j5^W_!=JFs(eCdRe>z^QyOzXrVI9A%gEB|3Ku!BcHEdL>L;9fImV*
zMFks|-HM<Jx0tuL;6jbXW_CdVo7JRv5INX*p`k>AxJaMOeBtu)YPA;pqg)Sh#a+x3
zul+KXnmy&=WEPo1uL88Ys-I~!JFn}uH>RTEztXy$-+Fzt4GqP)w*v5qux_PD-qk_%
zbBdswg3LZwXlUfaOioV!DPz*1<@b}8sVkoMc+w&GEb&g2#-V9MBX<{$#{i^rDx~qd
z4&+&NO$GABGU(aYYKa+S2!o38u_+}Q3lJ>*0rehIwdu&;G(M*ayH#NTF;4Ni9ui1|
z6M#Vk0<O$R=mcvOqoe5pl}=Cvoi^_mHDb@prCa%d>nHKipw|fqJ(h^~*NCHO4y{Ys
zQO7}@<Z6kZ!v-4yv=h#@hhkg0d--VtZdRCb!+{hx>+_^ix7yb$*CS%T)q2ImTE*TP
z6WG(N!#p!kb0C-A%JkcBf4<8%n%o)o_`1+`xLK|353tz;)dEPOkyo9_G+3`O5ZmsM
z8la>ein5R2jJB67F1UyyIPeT&D9@XLAbJ_YebGxnqH}+pz_<p6&xwx-3BUwuoWSN0
zehGVL3s&WSq~mBcn_b^;Y-|7=5I2b9wEE%~pt<1rJe4P3X|u?~tXTo1Y&@UGGK74v
zgb~RDKlWx7f7S3z6TXZ#+Q=>ZLV@9PF9qlR*g#FcSSMZnZ6sGV8K<-PI5zqHCovkr
z7amyJAeo~h;=_JBQ3Ozv*rWLh(L-~)t=Mdu|4+-|sM^}}X}(IspqjA4LS<%1z)p5C
z$p>}q_6$4K$7|sJiW3tjE{0-Y{=;g2G&8{J<bEFw&Gh-SP?DTBb!y*wS*b6Qj6at=
z&3HmXGtijI6S4V(g~;?G)7^{`iKLQ@d_ftDbv9k^>z5p#S{t398#k+dZkl=w340D(
z{V@R;DRA1A-(m~iGQd5ahYjzp7s<{Sm+zB&rj?Jqw*Feze`&HnK8Pf?BVJ+i)nJ}O
z2+MN~EFFF5virNdJfiq!=Bu&~dVd|G`mjz!nl4;}Z|kG=qWXF-(U?HX&d-ZFX1w3Z
zG=&+6kn*-zi)m1(l59iG95*4MW4LV6AUo?<T4)E~+0;x_NVHRfM>TcgLXD3E)#-v&
z@@CDvamUfBZ`|aZUE9F2V86I3FHoAu(kTX*wPBBY=d}SkPC9>XOAtKrpi{A0QB*C%
zI_&<{?mub$d?~Cxar-ISWWD!-#F}k73!U>W62aGgkv#U5>igWrC_u%riVRxk4vCjA
z{j3PGDbP4c>#{0qSWthst~wEcMQa`!O?|6<Aj8<(>+rc5Fc=1iugANN-iXN1O!d~{
zx8se~X#zHA4S&S0u;)GoawNeRXi)FN31i7e)5D1SNU5{_MgaNHwsfX2kc!2I$fYw*
zW%JPiWzK`?%&&F3=~6(wYqE>a$yjd%kPbCgnOuq6?|xZ3Zl+<shk03070~!=W4pjw
z;+aYLsgL%{bmJ_yRPuG|tA~mZ*r>!#z|1s|SP(6>F^kWz?bo9P>FHZtH|Y}K7i-$M
zl^Lb5rv+}lJTl*CqZTNs^t;;ba%(L<I*oFfb6&I8Z^&4oucMQ-V7Fo!L=lc<M+}I9
zSMkC=OG}NBBr+lNyg=HVB}l}D@{3zG!|yUCUT&Vw|2h`}p=QtygAe?e$F*3m2nh4#
z`4dzN$PaXzsM9~n(Rw^y#!-KtEt9?~<GGa0S2*XS{IpP?F32#n?CrGUUM&Nvw@W(e
z?FPB*gA$nZCH5xKG|J%wTpRTIrt56mw61;5bK+wy)t0rx0b1}(x0!b6=`!x}I$>RJ
z>zCC(&|p%MlVSBh<9xHmXo5OWu&FRgm4r@;<U*hsoZklDYMG4^?`g@41G2J^^PaU%
z<sOv&24|zbcO<c*56T@iT*<RDuqQxU)eyHmL{ll?BIEhbbq|iqoczFiI*mzP>QiC-
zN+Ju7I(C7+Zw!w;jjYT=1A|Jm1!S`XTh!(@$@PfZkb?liV-w%yHJVnBi%Ub323QDh
zFb=Jh9Bo`Tiqx>o0DFlJL%9~SV>rgUes-V=;5<{HsWdteOXC>H1*Ba6Xr1g?o>b6o
zA2052w{QrP>}Qyz--L#M02U!WDhMO4NCNaraQ^$<#euHk`JpU0Lc(C0(c)m|a8w{S
z=b&9F{B(#_)RE)#?<=6VcJC;g2s&xW*2q0xdEb=+>NuRA^qMn{^-(R^ZHS7a)5UzB
zU3*ns&SSf%&evkbN+pJ^<^_}wgym^N4X>>|eAYyDGJTH|zvfK!#?st-Mi33L)6xyG
z!*>`(4K&(w>#enTDC3sUOkd~v$w<S&o^F%{`Hhf*y(lOpfkcj8KZ1ZF*A{eoR9}0x
zU+gIjIIdU%3n5p@C|98%1Lqp*SgRROt(C0woWO}xYLlXH=M}?7Z+MKu5y;>`-NMhD
zLmJhv@*9MwYkk%4VDU9Z0>Xf=y}9VOK?d-@jhQrFip8qTFCQ0YCCQ}i7896g@+Pur
zBt#?4mUumEIR@C5U6P260D|dap_W5V*4cUI!f7X*xEQZ~Am_y*7c&I46ei;t)X|9&
zJ&45LE&&O(Qk&NXDiGrf%Y6>s11gv3S*w5JvU@<4;x$&bq*-hFSKAo>UzvV-zL~sA
zrYmz5A7hzBWkzJ;qZ$8A<zF2srqMf~6YXeDmqfHem%QAbeM13vL-<v3s;2N$3}w|S
zJTmd@L><7G!%~zxvZDs0nQ-U^t>BSmzM0j^lq_6&d=8VbTXlk7ngE?d&a0i6j1(wg
z7pvC_+5e7*F(9@)RtZxN15xyEcGN=E${Q@i_rTuR!lja2U~@mU1x?Z_5~V6IA+|V6
z7qJ52Ug3=vK&iyC+c3AH;^e5>tO_%1!$ZRGx6Qm;+MaNka!6L<8AQYI?yqZd1{#kt
zW_rujnEJ3=E7*RU$o83V0rFAank$hz@dYx=T))jeS?lXcJSN({if@Qzbo~Gh#Vnl)
zVG@BAxczK&9;m#u`-`wAgf0cQQVm^at``&Ud~tLh@+T|7r$8{kzTGd5O`A)CvkasW
zxz@a%Dg`TBnr0&S!WyZy3=Qu&RPv;n1p#xcU~6?k`>pZBPY2PyDld-xaKsIDg)HzK
z7RQ+lNBb-!!E*D5-teVVu!TNdjFH5Nl;&_UC9w=N!T}P;tD{x>K%-b8k1FaladE8|
z21xH}n6vIcEta*FdOA*<VR^la!>ade(=)Z<keAM1-|s!AjrruHF-oFD#HDh_NUDjq
z&`1K)IcG7#_;iE+f|V8x&^cK87yyrx?Lincw59bd<m`S=wPpikAb_HpYE>(uhAJ>U
zd2M1ySAdgA#I4Ar#N%;a-i-@uTY-jrBydx@MXs|p=5nD*u8@vGT3m;a7cou-lg8GK
zcJ0;;AQBl$;<W4Y)dz&DEPnvuYQv~@ed7LpS1^g}gHDgvU6tOwMsZ^Iw@h;3i}3D4
zi}QNJKxZ`Yr4x_+?@Q{LUO4ZA_k@8lA@p>B^%aPQ6Xk0twVei_Cw=1>V8(0pltvEv
zBSw<%IrP&BTeIZXN~72Tag|^D<G%$s{>9d>c`5Lhv8}Z798rWQ{Z{fRye!Rt>a=f1
z&gvn*$AE2@J%NGh;>0uMF*pApUCZ+-*2xrTm%^xTki)KpB9sR!M&QMQ{5|eWT8qv{
zKKSp9Y~M{6i#9q=GJHEV@acda&e8clKV+&tz$s6A9+U#o>ieoVIx{_ehU6bq>=O>4
z-4gSY(yw%CGeCCgF(?3XfA)Fu*qfY;(<+A9{FC~IxTFXxQRkz3xE|nZ@>J8aThTc=
zdRVYJR{Mj4W$|C{o>ywEgN{RZ3=|Jn8Vs#ys9-$i=zT-QIH2q`rT_foM#z{8(AiI`
ze!As;_ch_|^{%Cl8O!5L3tm>UgZvA(fupZUVlx3@GjWU6%<jq4eevTZ^&JtL=%}<*
zNLn_aBy!42uxz_8qMi{K&oBTs7vi8nG&WP|zB1G2hjru^rbzmC>+_Q$Qb3u$$c5K}
z=p>CS++y9HTH|p%2yn$tNJ-3l4e!0K4u;xlCxyKl<zaa5W1SFop@+Fim)L5Ql!`A$
z<jU<cxVths$!%t8z%gKhD@zN`dSmI8xQ;3llcIXxCkTKK{H|+kA_)PpEBvHkwoYTo
zjZ&N77Zp==v4L1mF9GN+I?_Be5OZ+t-}U0K&3V0Xs4BQzuauS3wS;<dw9k~ac?=Q~
z0I2FI2i;`AxiUWt#(yue`L%(eqb7R22GKm$*V1dUtkR*{ws9SLty_1=oRbV&-Kf&%
zHP`;5KXKzC=|lnN|IQ{E*jUJZb+ZkX)WLL$GU(cBXC!W0!grhm#Kj(c;V}pcYUggp
z#HT&@qSHkb?x&rLx^*hll)!la@w?+LO#d1#cox0zj<rsTkkA;*i5M!A$L1Vrtx<#M
z*ycv3tD=9+Cy9F1jnZ(n>lz?(J}uKr&DMp=>Y%4zkdxMYH_$-YQ)X`)$(q+m<4WEL
zt<9URztl(?l_ouE=lEFQC9m=pI}Y&04mgf>Z$Mu;midDtH6X8yb$<O0ftrReI)gMo
zQFtIPBswGjkT?U25r_=G+`I_dO`ct8<(OH8$LxQ@=5jEY)_4I0q^AR?jE07>qF1{V
zx$YUX{y@b_N$LK?wEA(WR@N7fDGw70*Wci~K>jbCf7@FpU0|rHFLI#4KZREQ68oc#
zJ|W@#!^DJE(kIKbxb~hfn#SfQS3WvJLyk+i;Mb0$q&{y8R3t}DGKC#&Z;q;0vj7Gw
zpUwPD7!Xols)F$+S=s(4?e!b!ZZp4~E&33;Z&&L_{oy-?lY#S(9HsMtm~gtyG6RXq
z9U)j8Cr2$F$8`R*aeDH_YRHQxH+N7Ifz4hpnIE(C?hvc2OA*g|T~{C#G|~6fnt8f4
znx6?ktg^|T`cQ-?*6dm)5M~aH(&Hkht8X)EsF96eYw2>23R>jn$LHqveB`dcohu8U
z%A<99KJ=9s`x_-C2C#tVDGRkK6-L25hOQW;MnK@>IFOjiizO!Eg9Biok;_pU-(;QL
z60cToI`wJd*DbR`<wz1SYo4`KyBw8XB0G?xFhi?HFy_r;gG6|=OQyGSomyF%s?DNX
z6Cg=x&9A_VW4ayy#Qo_48D0uSJ=`(_jT5F!7Bh0Ax-pdZl@@^GOqQr&#|sV7K|ON;
ziK{jw0OznbB*MqzQj~Lnf{w}P(ngO-Q;m8lb)j~%dJ%tZ&d#mF-jQ}q0eV1-0oZ;W
zuEM)fHH@E#_sas-Oi^xq+?40x@d}3EH36WGI&V%C^%yj;*KzAc>C^gHv(hFb-IrJ*
zv(CX`EZ2$4WSnYfm>e9*{?4DrhC0-GLDlr#-67}NB#no@K+;QjrXxR7CRM!jJ~gBt
z7dWXCGy5Z0)?x*jOzM*ptffXDNMo>*fdcwgkHO0!7|*JiLaGDH=~N2~&%L#uxRX0&
z8mw!}ZYE!h->)<1lNRWHRfHV|hoE96ZLzg4J4LNvb^8|J2Y&p<ps(lqA(h78>qa-U
zRg;|$Pc%|Kbz7{Mr3~0&J?Gn1rJaG>Isit8mMM_%B}rsEw<HTKR*IovDX9aeOI4*9
z5OZ>*6jh;pjZU7J)4vETIPq}e?yyMGpLm@z*a#pK*}3_anZlDu70NWyLBN?QK)h6>
zA_CJi9oMWu*`32`a94^=7xFR|=n0dBGv<-4-BlDBIFL^P0oZ=!<`pXlgGfeN+u0dL
z9MuX~`MMsDZTxFyEsF`bxkk>5g9Uc5XdseuiY7oe+y9~VCi5$4Uu~$Y0Y9$66Yk`R
zqwcXymVSki#<lB;h{v}BF#r*iOAYSdmq?qIUr7I>Qf0(p;^Q>7->cu^e*NLeb~)#5
ziJP4?oW93r8RV`mlY+Mo;uVsZd#EjDQRq$_UUg}~lexL?=1k+LIo@LcYSqIin&xh1
z5phXqOB7Ja%a-)-mr4$tGmjTPS>h|)Amvw$B#KrbBjM+z^#!m#!+JEud9|(X+QUh1
zlYqdFy?cuPL@oVM)U<>4^nj4V>-v}dt-WM80akz$)NwVh7?4GLSeVS+_R)|A^gKXC
zJr<P4M}_J3BWUniHUZ7Qa~z3D<K4ucz{~HYI$DW^x?f{Ekjdg1$AN$99D$PMiN<>@
zohtE#9us4!ht8fbfVBa`?j${j;FQs>*8-KV_D@!PG-9WJyrQ||e7CJLTZYl{+(wK{
z-q&IrDmaVX4)}H`pr$$y@{;kAdcYnrX!H%CKLHl=E@XsOCA0Kv{%TDWX|4WWl#a?9
z(TS}2qzvA6y`~y;>NKL+GLyDOGneYwjQm{bEP!RV-;W225<Xtxv-vbxbMmO$ZXGQQ
z{2pBOK@KNl4V^eBm!*sFxPS<p#NX72zlr|-ZTHLG7Y^IBnlSjB8uRr}cX!)K^O*G;
zvg*b0-z+N^tj_^1XYbbGpwuxOLfxXhG28sXejCkx)yGi*R5<DT<g&K*v5*mS<{;m8
zd01@iO^g>Xa@^=}bl?!;lWD7A;EI=u5toeOnClUQR-T#H)%sPFTRR~x<M%9Qi$;I^
z_z;vry;9YICzr{|Er~W8BKw_iM8C&MOo`2ixw?_>=>jGMaa58yBV^=4nv1n@JpQst
zG67S05vo=zKJ^(WZI`j=)IK}m=mn2kag!OR*-@8~6tD3575(Jkj)FbGmz7U|<RTNu
zpISO0H8I`*MtlsHSR1GCXfun7k6iXDN59o+YdJW`m&RG!N%hyRW#;vE|G@8T4oQ43
z2Jor`vr&m}GK6h9$3&h{i)&GVf`egSWJx*MdkX8BVV5y`%E95^Z_R#EkLBj&QS$S@
zV_;wiIiHF#9Wmtu7_9MXGev-O#k++8)S-Op<#)RaQwH41{D>d3ZLLe)+O@GfUtn+i
zY^|3Wjpj<V;_h0<zjzR!#~vCRlMi`VVj%sNBjm#e>cbf<=^xX^GMS$4w-vE}=|flf
z?uHb^zdP@vU?wph%O*jp&8kHd4GpVGl$AYqii!XND3?>3JOX3a8^)>|YZzG5p8p-8
zb+(3<5)ROMKYLj&^B+*Sqw)Lty+v8w+!6wg1r5^WE9`y9zdFd>pY(<3i55-QV28OL
z<#hj5Ji*|r(%kwLMH07C@3g~v^LM%m&tf7|&2i(W@^L*0z=)3?taqJ1tol7@(7^?M
z%&de5**1KP<u7##Znc`d^oU*l6Hnc@x5u*D3Ol@61+1HvL6=levkVZ=7{6<b1~tBV
z%gRDx)a075I$}!rVG;(MMdiDLiaj8Ao5xS80n0tB5SbhTY_IK~%8n}a?RSq4c01d)
z4V{%gixfznWfZEiFu;z7nmc`Go97qZp=q9>O>Vz|)TYZgw)=dNrd5WEuDqQEo?YaI
zn9U|7-uX_YnJruC+y0W)0^sS(y7@e7ybglDOP%9jQ$hu(#Qcm|FYZ<=#frwbY2#EU
z)Jqwo7}YMJ3tjrbj;I(3lWmFb{mn8HtX!Xc@4o(nv0#!V8m&}{o#^Uj2f=RU%F;J~
z#fSrE!^vZiV`Fo2tajuq!i*KIhH{s67V54_o>V?6%WQ~v{$nq!_A@8JcFeZbHMq5Z
z#iA=Cn$BMuokkqcE#61X!LC$}Hp-BUiW;*oq`}^Lo#LDWoa>dYu$&$BSz4L=6IioY
ztR@?|>`{j)U-?ZL%{=o_o0(%Pf{B7+l5<AbqgY340U3|Uw0Bnjq^HRx!hQ{MfZk_0
zEfQDqmMM{s;or4dX^QjkX)wZ|0SFLa0c!8_g-{&NPX!5s08b*=cKazYG$LU;7(IsJ
zDI@?9kev|I(+DZ%D<(yvEsXF93OadF*`af!WxghjN4mU#%0dIu;#tz~di?;qRN7#)
z*q!|Bxx3=YWKc=p-IFdhmN96;T|do;TI)2B^>nge(1j;EgSz+Ql_;R%T_TGUiQrM!
z&y~@la~|y@!1+I@dh56-*RK70NJT&dMWsaPPU%#U7zr6tTDp<$QbADZ5-F9EmIjGI
zQBpv1C_!3UnjznX_x(J-=ly5z&;IP~X1M0O&U2k>9mn^W`K+nxTjYl=920-r)ihY3
zz~{acG5qP)6{mWfqXWD7xqL0W8(y-z78zOe5>Wq@d6q~T)PhWA>cw14zt?kuA{6tY
z$v|$@QF<)Hs!Jv6uq$Pb{yhjlK)w;k1Vzp=hCWu0!3iqj_>irI?0Ay{w^m^TP%NrU
zpB%c%B-!6VswQs0L@JIZorO!;@#K-#u~y#nu$|kOaGvGy@!pTank`k66<wvUU)fqR
z+Hf5mNT|3C*9P|YV(!TN<I(D*l&xaJzQk_JxNcT%y&ANW@xcsPgM)RlKwrTlb|vHw
zOPfq#yF{j2*j_t~Ni>y~xJP`?X_rWLQsw*~^}NKjVfFm{6rujfIu~YFlyycSy%fi(
zV*T!0N!URiAuFEuP0A@i-Q0;6t;z&<>Md4QrZySj2Z@Rr5mU_;MvJIyKN`Up4BH0-
zMI|M5geDUCvgBSnN?txxuhFlzqpj_UFI|dCo=QqQHJ@x=*!k?)p+e14Q)I;T_XFK&
zMkK>Zj?)-}hWr7HG3Q`iHR{%_1Lx}zNcZ*P0Ucv@cWjyOn7D^>-V6Gn>c{A!bM%_F
znBt?T$&p%Dr2AUo`r+|j!f1_p9^6a!qGY<acm&#e_E-InO23WQ*U~m0t}nL^)|S)S
z1+?*;7}m{}3sMUP7L&<vWz<UW7;D>}_|@jDR8vwgU?woeot1AUH+-8T={7}-3f~0K
zl9MAzY1t-;gj1}>IREUsGG1zIZHHqBRgC}j+6z|Wro23)|3Mxqf<|J3ghoW$_Q1^>
z{m$D5uT(EVuUtOw{k?>jksgwfmR<1&zwxqF(_8{i+1Zg1RGFIAA7`rW-m{=TM>g+t
zB=&u?Uk&-oubhc`YRZSp+1Y!d{=r0j@acA3`kNOoI{7QDZ?(o6W8O+^oE}%B?uFKz
zyl#z5*jg*r$c=w1`6w?(g|3>CjvLeYR>&n{-s)%~`YlJ`R&gV-^zp!P&kLfPSP7P(
zc+TLz->15-{q0UelG3jopiiFT_na>GsI5)N?yhRsj*DjJYq{1x*2-fb8c&x>U&YvK
zFU6_Fa~O>~`%X<ErFP;Ev+sG=;-pLF4a72+p>tFWlQa`}iU!g=ITGId*_i(|-9-c1
z1`6NBzTyYkDME2(rw4XDu^ztZ-vU?Otf)OGtW?m9M``4=2~CBgPh#y#kd&>fS_L@+
z`y0sB$qe*{?=x*0z9%MYws?MYXbIdY-_-guQ3b#Ik|re$O~KdZW-XXo4UAUWl^dSG
zSVO1lt&j$ou<}9rvDB|7vtu0yzhundwI_OSQLYOdgZT+FU3sc>?981d95wYFYlJQb
z6)1ZDtN|4fu^aa`rjevK3I~=3(-W%Lm__QdA0%)XPdKN*Agk+Wd-3Siq&NC-k5kAl
z5%r9aT`N<5KI{C$d>{AhgFPMF7Lx4UwVk7VNv%sFPYSdm=gNC#hx2tj{a1u++iH`A
z9^~Pj8$WDi64}U*=FV{M{SbM`Yiv|Zs&;y-OfTVJRV4d{H`A8P6_H;|m?zqp1R3Us
zU@P&wmnI(B3WiC)q^xWUqt=Y+9^_5GsJJ~oumWA>*2}tk@7~~#WWm9S2H!z|HfWlg
zXNmi4f9#wiqZ7}1d6hd~K7txP*slW1?$o@q|5#+wZ)R5-Z|YSume}hwAX`34JKtQX
zHNRNepXu?Yg-r%$_eO3ud*ze&sOrh$`1aZ1iF_U2?5o@^{){fIp`^&c!bLCFSFbf~
zr{%JF4B9gG!ALY(c0ZqZy-`0;z(RmklJJ>kZ3HcQ>s%44as1o5Lu*tgzs$z-RJ>=m
zwy^9P^4-5?+g*4J6MoJypAcV*f6I5L80YYKD4!XFbj-?bcNQ~7v&7ZU`yC&c;aZfp
zd*08GjvSn4e!#J_-5<%Ea&jGAF1vp=fyUK6Ut!&QHv(i9Q<Z=n$p>;vjloICHKT{_
z%R}y74*P$nKq8^-xGs39QB*)c0CttYT$m{rihH=#$9!+$5VDCyDJeBZ3fNImQQ0o`
z&Djoely_3Zr#K~EB(Un9bvzVKLDhM34rYbVc)X(2Sc(uCUuTLnJnBs1)miAVf<b}$
z(x6Uu+%s3ydoA8cmsgq=t>4+*H%upX)98=p2m>}gq{cbV%+iYK^xL-kEOfg>24e5$
zJxJPQJ?TuE)BEJafTLJ>lrB|!n!C7dQ|sZ>DCSDE3(C2?sY<&O=acf-_^g2xCrd}4
z-qK9et81LF2L`s5Xqud@f+sw`5lP_P{c!))G&m7ket~22ct<@`{_#S0n(mA#YDnL)
zmH=j$XUZP8<4TLp#HrwOH)MhE(&^3F-`^kOC^D%DB%A#Ih#k<W_cm7^fxuNo)Z<6!
zwj0%Io3-Yj5sj%{1&2SQT65mTx=!N$)PDxrpU<=w)p)3!O6F^6c>TRYbFfqX-tpJC
z)9En(?-rVSCp1Bi2qj^afWn0{JwWf4q~!XB=O2@=wXRe(ZtS$eDKqbE;Tx@REc#iT
zc6Y1_Y`=5<8Q8A>m09c*)Ee~n2d|Ei>Y^vW5>7H)(H=y6jkc$x_v59@nI;0+kDCb@
zIdr=9k6=W1j{f@J-7yw}+7GJ_ZgG(e*I}N*?uA}GJ`5%xeyv;cGb~eKXNC>zgd9r(
zLq&pfVgXKNdcpf%c$Po-7<js$9P9C-<lbxMbq;^}^86*`vEa$TfabSv-{1+~{O$}s
zNEa7@LT%0tG1AAzXF41U{yb|GDz<Zu`zkS5hSk4(H%|uj$X9R@3N|#tD!S4Y^`&ET
z1bi6Ie@!G1@`wwR90p$+_D&)q>7tq~@jxYnA-8l|pFoLveUf4dc;~g@R^gNp=DRKx
z)|cJy{3qHMPt0oW74H%QV`g_CnmwM|m*6t1<K2)8HazEucLz3RVnp3O5(kAXmr$~s
z%2(RvSr4t;9<7uJzChV$!q@h$4O^<_6do0|=YF1Nu!L;fi}A_fY7DSQ7^<9jUt}s|
zKH0Hl194P$DnEiPl#(;xPBv0!vmPxU5d@R^mpaA{&%ic}%XgCz&uc2=-gQ2c!1StL
z`IMlr5(S;+)uEySnBjf}Qx#4U4`pjCvz>bNT8L!px<iyCu4aKYC8*<xm6T#?d<KIq
zv%WN&sNb<3{+n9T><UWPF3hcHvQm7$rZh=!3UIW4ZHb`i9d*3@>;H*zlF@I#JRhNO
z!y6y0nhhtTp`<iFiwz}G#oaH=btd&xsJ=@;yzLSQTOKO15p`iYI9S%oXCt{`-^>^_
zT-<YGDhh_CG#xNxX@Q}UiSUEGr@bFRupEy&;hlTe+<dLdsS?+H`EB$YYjA}p{c&5)
z`8)MO8}J*|`@Dv@p(Omjf4<+U@ev|CM`oFxTJ7f@BJ5CslBJ;*>hRGfHvS8ix>p3l
z4K%Q1!giKPhiiI+va)_?Xb%(@JuLk|N=2ol`?-P%nb4tsbb5sMn%d&7Up<aqcxijq
z!`ia1C&xnufsVkEnDn=5dP*P<(|04EtJL`*b0@w3GWDzHWWwEzY1jb_#k?u@k6an&
zfACN5J0F)@5C06MU{;-eL63ZIt$zACgE{C<ceT*ZcpYF<b(jm}0VPhn{$xNyEf3!Q
z?{ng-W7Rrpa9%gtJxJow0=kc5wGPqqDn>n;j}5+p16w^37i&Qu<i6g3{NCz|7O`=(
z*~pMgexIY-HEvg{%TM)5T-u2J6|?6dyzzOhH(Y7Cs2}EAsZn(>%1rmPwZTEfzSB&G
zY}o&Rau_wg;i+nOI#`r~DrsOLT`e(aA3NFVpdUpT)iNwhS7K(zMXT`$;x7<u<U8n#
zRm&W`+cWKn+V@=7&Q~C7Jp(NWf%>n`L5~Yrq{rNlo;LQZ$~JpoAP>pRdf1BXc{#|@
z`lLrHl1>kqq`d^K&ezI;k;a$*-7U}5EO07*Y`x+8W2zFW0E;oAEKoXLEP*rP!$S|;
z(o@_<A1zC6YfSe>jz)P7YQ3>wV5D67;&6BYP1|H3=S~MF^4F7RGyGsW9dPYo-&x40
z9m9kW3kW`Ujp`qBj5Er?QleS7RR{?K;EvDt;e!CH@cZ|%mq3Py>P&iodKm5Jj3?u(
z9l2Ax5N|so;A(oYt#+yRUiYuj%13)g-4BjnVr;cOE?8%b9^bogx}_#o{F0Kc+P9gs
z&-=T#iF2UxyY6(Yr8G|gGOdBeh9AXdGRxsIE$dwWEjO>?j`H!9TlHxB?yYHMDnw}m
zE^VnTh7rl0-SV0$^I&g1(BIqP`kUO00KL^;bvj+?QW9*T!KKx{)g=6;8D7sr*mr*3
zLl)^T(BJ!NehuBl((L^D`jluP={wC#`PiTHmVe@#Uu0*si!J^cR?nLZ+*_&la>1+6
zH8=UCCLME2I5iG`RlOt~6(yZNkYvo^{wLZ~hSh1Z$w62wlozdOJIp60tkY^x3I`t;
zUubLfN0)7mMlZ2;OU+jX;$NiS1^&@x=Et@JZO*>gNG0~^P|`<%p(nAjBleqVe}CTF
zq<fqZ7Db!vu2hTumXY>9N4=tQnz$Am<|-i}A$gWvZ}Wug;01E}-4vXE@5RN7oxo^V
z&j+b42N##}_m&pILD`?Xppg6GUMXf#sql=@rFm!W2@_Pe#fBR2z2rrfq3=+UAl(-~
z3|upA`*bs1Q89XiE%h?AbIqv*ok-;hokhg*HI)*#2%RRH$mn8+U6C$i^xgik%k8q`
zGRIk(%R|X#(2vgRN|73KvduSeHT*1^$o3+nUFfrYvhQGPqv|^zqxe9@C5)#eE7Wz9
zRAQuYv&;Lx$p5=o*>!NC4(sRr4T2MI^;N4oY<imW{J%aAc7FLBokG)A>nadsGgcKT
zQUu;|e6#Nt_HuX1M&b`((le8aBXDsXV{BzioiHiV<bSxG;@tHXB?ZlGloNV2%_DW<
zbFh99g=_Bz7ZOlY@DG-(Ph7EC>tGLfbw6j!^~O1h=$`SoWSJk9wN(c@oszE<>pWak
z(q+!d?#`j2O_B8_-``O(<C8$1*#yz{zt74DAjoSxv#T<vN3j^1HuNcS0f+m`&l_`{
zorMHMM9A8$4H(+S8!{u!Rj(`!#N>T42ziqs(gaqDFLgMJ=K*Mj63=N6b~H%$j0vxH
z{5{j6^rN)Yg#>m1&$Y0KE`JGCtC6m6o7jcqTp_$!!IM9Xd&oa1)GW2nflYQtg6p_y
zk)8`2>rN-F>!f-rL0bbaca|3#S%Uu0ttJM;`@3-TL{5QoXEqK+9X&Qtz1CJj18q)`
zCD!yOD?|=~lBCp%J(J{&<{r<mTiJ0fQ_Vguo+tIUr#%OsRqF{YHY6KI{1z057k7~p
z7kf>ey<Tpuo^6Xsd{gQF;(6P)4Yo^P<gZyPt%RqXLMOisv+Dcz0v1!Fl`a=u>U?!H
zh*Z;-KFf*R*7PZ{rIQp8m@PLhmoE+xnT)r~v{I|pHgF#e504RcCL+T23-^x^#eZiA
zY@V<s{(&ETM0f5fOUKhMhzdK_C^Bipd^dsgA<6!p@$WdbS@n9+pE(g+{f$yTKM(+|
zsf5wfoDadR-(j|CPKEC~AxY(jJ@UTEeE&w^al+04B`0{TJIsFG1g(Wh>qY-$Xl%2l
zBTn{YeveoMPc;XJoyUx6Aa{LN-kgXAjBo5|<RZW`_u0k<%-Ir+mgQi!pI;i-m#cM&
zY|Tjvj2IySm$W3J$r_(S^Jh=sV7_buM<hClgE=D(M)Nr3R(R`ZUcm}cyCD+m2?Oco
zbg8+c3`zD34OL_UuDB=R!QPo?{K&jUE~(gvK-h^8U}-}|c)|{`u9<o2c-Q&w7O%g#
zo)My?X#)MX+V{#-zp)F<wO9aN;m^zj4`?cVtVq6p_uic4Xk!CY2v{Go9~=sM_<|ju
z&a({<0q^^4rx;GT+7AH%s5xd!Z8Ubr)R3`T+Z~*;NjxlUTJ#T+sAFOo`nS@gxyNcf
zxEFLu38jcYKOveL_ci*`J9oR^<6^ZofqQh&drr?tdJ58(rZi7vp~F~5ZG|pv5<m5v
zU|)CCmz%Ar{aC0OW4B%AX7#wWF0g6;2<uAGH^CG~+xP74u5LcLdUalWWWcBZMZl)b
z<+<?TW~#(F`{8%2ejpZfap2{+044Rw<Hhku-7@N*V9Gp;ld0}_-LS61o;vWDS>)O(
zi|Zy+bo9+AC<WXd>(zOoHF@!)HurMh+o#%v*B_RoOB$4pFgbS?S9ELT^$iw)W%fFb
zxlDFg@1OmL$nJvg)4I3`TDWGVi0WS~BD>vbbAoS#NxfG1_af2nz%9j`MJJ9h4j^({
zW+@dfAEAxXk~nwkmcWCTiI+xY5Mq<3C!AzyM6&ZXbV73-wW=!>3l<&ts&}*E<4tS>
zUo2zreD^GkicRkyuYdCQ1rWvQTmR6HH$gkr!5l|qJ+6C$QK3WSPaF9$VMTz{C#Q_@
zFd*Xr1Yn0lXQ&`?tU5`|@OX#C{5JB#!(!SeTV2ITN@_{o;kn8&wk~Qp*DBRaU6#S=
z22PPaWyJm;Mw34ePxB&grvyjIBrezuwJj8VU*dVi>}vBqmw_?cXDggFh{h4|wHbUM
z#!%gZ_Tm&P?#!^|va=+1vPDU=EOY8dj-`giqMs=rVz8jotO?=0y7I_mM*dj7(t!rx
zhFp3~WQefHM2<VF@&IOdRGqP<Fg4|pnI?Yby8Im>lJ#=jQsX?Sih{DT=*z1Kb!&qK
zeQQfc*dK$=yPzSWX!ywXXswT#T}wrED2WKWKGtdKto$*QWFupiELt~|1joqhP5{QV
zGRpY!WxNVo=2m)Y4i%CBEc4-W{@a@Qey^@!#xioUp`>Qd%F-pLw*C5ditsf(Sv*(r
zX{*j9u(p9{Pi<G&(am0js10jBVQ?y8JC^8x4J$jN=Dq5OkMS_}oM1;A*W%ibd|-W^
zBHSLq#iM#7sG{LB*7BXVD<hA=#}JKN5;h%GN6XPnP4-iEp$c8NTQP|}P)($9N)vBN
z78JIh@WWl`)@mIpA~ZJ_E!HV~Q!!~bc6EPC4Md>c$Bl<sO#A|<QD^_Z!ERwSF83!&
z9^MhZ&1HB2XoWMvlimuWHxFpIS-6StXX~Z@1{u)wNU_ox$HoN@76kWZ!t^Sp2irjj
zI18J5=Mj73cwp*O`kA8=OPelTr1RYl)3aJV_GIeoq;-Fea7P017vn&JmYqx~xU86N
z%@e|i_;Quw{X|TZ_Ge5P^TUddPLaWEm#E@ha!anoWWwD|PC<c@nVCRcUA^&QD>FcN
zYHCEcS4J+sKhb0|Z#9jXZEakQEf99^CLw$$N7S12&^MlE^`J7A*|l*|zsN;U+LLFC
zR7`ASXZadWjc4Ip$D+tGj3|~Qg(&~@D<EqTzbi3W$h{+Pr6cK+;nLiB6%icQdBy39
zK(wa1omLw*?Z|uPz<bY7ifF*JM28H>Dfw2_4*f~oMzmf_>`M3#;-mWpp*6hs*$w9^
zZz4sXNn=0-O7m<=A`|>qN_AqUHBeeEBtzR_uwIPMHok%p_!70r>mz2My=HxwrD%~V
z<K$NJn1fk{*8nhoZcfI8&*G1UCx>xbQ#P<$Yow=B$oAOCF`h?#p#uYf&WEdmFkb_-
zXLIQ>^owNIC;FUk37|rAYUi&0o=c*;jQ+@`bfrKmuA8n%|3RMa#`J*zZ~ePH`&}y_
zBP?LkJejaCe;Ez0a^!Bl0Ont}!rn-1gvhLcb-2khLV?*1UbJF3W^eM``p!K$@3Yky
zb)7`ENN<xb#Ubp~Sa7-&C*Ty<Ba^if9%(ak0jlI0Z$|`nT+EwRG}@mQiUvc9+5CZz
z=IuYS`7O&Sot=Gq%dPBlguP`>&TlWX5*YyVRM2CPBX4OS_H#=FKy0$bMQ>}i=+g?w
zO%!M<Ic?KZG7}A!nt?U5R%(mD>??l5ERT>@WTol-MX8fEMi<q_6TJzV+lnoEz}*0e
z@b?QgW(PeovG_a&=f?x;>}7O|=@~kpJt~L8!X!6(HIaShMBK7GzFTy19A_l*IHFdQ
zeck=TLu71aso9+O@=)L6dmY}^Z=i__&jZ|Fk^Lj7T4EySm5t=L9IF#$%glDAifU7l
zblF|+MSy0vy4Ui${j+VKX^gIh#tud`JsuheKq*~hPJeU$M+WbhgzwciqP_g&wBnlL
z-|a`4vR^tiFb__y#YYot!c)aU_g4MVdD6ibt{VvjI?T?GKFJ0+jcT^24-kqBQJ`-~
z1p!ti>!)qP-pXZZ?%sk&8u|B}YVRYzmc0%BZW<GN-vM3H$Z^=N27g_>hLhKKy!imL
zI@gGm&xkSYqE^~G!M5^yt0)i5G#-1{(1Rq~jiQg@3z>BE(hNNAc#!u58`c3Dp+WF+
zgp9%2JRaBljT*xRb8wD+s4@wTN8OiGoOW4^-M03hAzeH^T+Bti9cZXCV^8j7dv@7w
z!cSwgxMB0cljWjFB~aFvWn5+zU=0lNMCPtk*O~-t_UaW;G(RPvh|5tV562Gb%w2~+
z5?ErZ)%qn#Lu1Y$o`+O9gJ<CnEWFe+Q3D^t-R?yl<96Bp;+AGc2A38J5U1pG>yy;%
z88vw3qXddmge$rx1My~Btr`M)(q%K{QC&o{<71w^42%4SKkt?C9?q61kmUso(TX!^
z6+eEQP8TOEWdyLq!y0E4LA^`%kt4c?fH=$SmG<2~_A9!1iT+arHm#zTubDhXx1R>N
z`8=vb-Uwpy2Z&AovJLFd;?Mm~8{#igkXwW>gFH~k*?I6uCpXIrru%le9tRciud{O>
z(4Y_3&)x8QXxwcc%Rb=PIC2aJM81}6zIG*rf(meRhKd9dCIPcxDrWh5p>+rB`xiXN
z9LkCu#>~fdTWd^$znRc$Xs3gQlr-ARVGwDlU0Nc)hEYQzIujG{4@%J6vppx*)iM&R
z5&)gcQ!W=T^iNs@XK=nY>6h13DkkM(EDT7cys>*^?XPbg?g5bu<)7!i9rik&qEgT*
z>le*$qfO7%w{DS$tY4|Z=ue4?4LgZU>gXeeY#W_oMVm)^Yki?StaTpk!033F)N#B?
z3{cgkF{e%embjIE>-alpoN)F6>}w$qnSBfIYd8HR?*C*agg96=ZPs9QjCEn{{fk)x
zak2)}avU{js;3G!7DJB@U8Q*#E=Zs0izgk%Si)cC7>H2~&2IH%kM0cfFCsSnWTGMt
zX5$1h1a1mSuKeO0%+s?X^*rZ2@##*6sp^9h#Jl^%{>d7-+Y3-30%XPAPBq&t$W0ZC
zwOS17%ZXTaus!UHm6gqZUD?euQmTf_svM2)x^5)Kio}VM;h8yx4nS%hc5BW6wD$`h
zKX0~1QeWed16O?_K83pFzYn&vPBxIA{j=Vi0^XR{Sxi!Q@{9~i%IZB-a_**xYXy)I
z$8n7I$m~gC%XQ5^nFFdI_O{f)#~IqMyHa36B7}M_y+l80OS-}WW})te4qM}5Cc}Iq
z)yD(onF`AdZ_8&=@w?MFnZxxYzN<;&tTpQAcjNe9wB`Pyio^u~nqys8=u1l8Ok)|<
zT{*dAsJa+RhRa)DCK*qx{Pe{b`Fz$)v!!E2<josYuz9vmHU;|#IV!*Y5#!shBH&=4
zU2eEC!Z@>pBM_nIQLf9z+=(dbo@gjeIy`iG=VZM2+8J9WZPrq3=(GMi6$G>o5=A|@
z!e7PZ+jA4SX;*T9iAGyFWmmsjg7L$<GsXP{1E0@Ke9q23LW@noO>4huL9TQ;Yu219
z+H3T-5+cpU0ryN}+({LIitzpOaOe=bXUuLWm~5V=Cha_)*Z?S?TMozs7cTz_dIlEO
zZl&MGf$h$?6zfqk9i<lLv-R8z3|9YCuNLNrGvCeSt1RvD?f9B`vDLFZ(&WGl%5{vo
zA@D3jkQ+xZg_+!81Qmcm?i=(@*o~l&Je5_U#jzmnvzX*<{4;zHW8R@pp<9`s9uyj)
zbS|;ce@8bgY#i9AVHaMHa1nKSPU1{W36KyY#@(;|o$%fC>tpJPLq}g)Oe<I%P-?+Q
zc&Y|euYY=6;%sJP1{#Z_bVCW*RWNJ?^EN)A-Tj_kRpPq96snrGz4zHONmoh*msKTp
zVZD)a^%OxB$^y2OD&^3g#}tbH6_|dtPb^(Y<c|9`S5%$V{GykwKf|z&&~1?++fvL~
z^+fuVc7GK;?-&yUC>ta@)y=WM&GsK-=ZRwEZVRB)5I}gapEMf6u1R2APo%L~Mnw+g
zN?b89n}<t-O{rq(tnjhUyRC68u<@%`p&e3~LgCI-J&8*ulX~peCMUNG_pKPrBd*wY
zP`@#8btWzf8df-Ro9$9^bC``$xm>UOCWh6o*A<U4Xy8~kuKmyxkE6IgJX$H}l<RKy
zPEW|YIAIhxKIL|z60Y*%p4UkZ$_De>gSxtx271Sqgsg9t8q_kNK|WOf>Z*4lJ~c_x
zYbt>VgS$^}H0Nu@9)*M=6=HDk-r!UGQ})E@z(;ulXt3}O_>f@4SU1j8U#~qxpSJIN
zh&gmhV-FAP7TamA<?=!okLdy2msaOmqRzd%)>F@YnAe9)=({YhlN~f_2N(U7;;Q%T
zMh3e!0!k!;R!HTy83D1usYzTLTtxL#gklOose+m6-H(-DGYvr}?>rgxtWh2VIl&nR
zYgXvL4$5oDr=WM1EipP+xuI2n3p(^C+~8h6t}<W_MB<dxf8f<<tk;aNm3n&aV*EEy
zGWjQ?vbX)m>@cbUmPrWPPU{MXVb%~Bh5hcsBWznIzN)BIeT?V=7*w_o2u#7!{#PI`
z@1T)Y72juXsCowg81lE`K&X1g%ig?DK>0=>n;l{<j7suXf9dD*RraA76_{+2g*x4R
z#Ul!4c^U5d{2ir{(#yCn$9W8eJc?f>SzxnY4ChBNW0?QBOTK5AXxy@rS@VV~Q1>LS
zIc{6GLeEzxzd=d~_uk#0k#dVVg~&hIK}1A-F_ygy-s_#b&?Hd7jNPFi7X!++92Rs1
z_~tuH`FzP&u5$PJmVjSiYHD*U<XYL9I<ex1{_<S2P0xdhddqU9_7B?M$KF3{JPyXQ
zuueW)bLGORe{v_lQVX}nziz^kEc#2T=gJcU`AmnI>HEi(X07J5O`z*XHHBYh7Dj}<
zjD6!EOvN$(gQ}GYq<doXg|{8K7D#^fy!ZIAH&H+qzBQL5ubeXUT<Iox?LWg$LKjPa
zi9a@*Tt1T&*lyn&_Bwp^7BjxwOcRF2&C)}iLhaumLk8IUzL%1RWl!QnH6RXmoD{=;
zyzm7uT+Sn|$m~G)9%#{{w{3%@`COs5aaf?IWmK-mtlBl-1@zd1!`+I>)pNcViQ~kc
zfyaf5phAziWk54rj7bh)lIN*<oObQSTK2tc?Dl&V9wiYr9tk`wNKC!=><%vhzWI|s
z*e$r@PbC@WKb}=&Jo|P*wL?BflX7+`SF!}pb8f~&B=|uWbo!C`73>(x?)ZEIt2)mI
zYM9)N#PsqaeLNVhyryNJ*|kaJ#l)2eI|PYLL@$85gm(r&3;22Rq^;NQ-OFxxC#ci>
z#eUf2iC2#ib~CnZgjE*?(bBmpapvG1@!L`nyC~ao-k?g^*lNu{tV-cVj770Q41Sq|
z{-WY^p(44MXHWo7r7Llp?khmUJ-*HH<E;f4nn%<C(DDVUQ1UtAY^LyJ{(GZ2UCCzm
z4|iFM(Ip8dHlbwaVCdjXv$%-*JXlEe$%6P{;Tz5HlYdgd_-z{&ReVRZA#xwFxbi;X
z1T-6-9Lb{UInL!ewPn)U8B#?rF^zQZ5)tA$r?6N%66lO2+!WPX()~~^!UIU0N~3PO
zAyJP|oY~<VwIs9E-g}Y$Tb7G9M;)BW3&*QJ?;!Prz^I1!S(5QJPR;Sfmv_KQrXYq#
z-FIdll+-{{kQ9A$STj(PT^V7OfDRdSOKT#4;K`yAgfULaH*i>Kq~v50%hy#m-VM3N
zq^@~Ew@%v#f-3?h0RI+v_0^APxJD%}bh<m9=Jq1703ZEr`|5j8S4ue#j$38Xwi|H2
z*+a^8nXVQcX9~}DjaE_|cJfMe`=Clc&@R8E-1J=?EsFs2KzCOtY2=~L0^Pg21jn9-
zt|SOL2^Q0NWlj=8lO;`dEps>=5b*YQFlJ}vdn{=xoy`BhIoR_wXsO+H8ohl}TrWL0
z2Ukt<77^Im1nlHNgZ+1NuCs6P@z0yTXO$iWtmr1+|7HSrLwwMMAw9t{F?hJ8mS^fP
zVUYhJEso{pGXCEd8VkiFPMsjbIucPoQED3-!gwyCfr14eI-Rl6Pj_~I)z^M2^R50?
z<z22xl_5@Wv=v3$Jm!RF*3#y~s@72|?8wy$+6xyR2FAH@=vgjs2x%ooMJBrDIr9+U
z2jSpdvFwUnpbsRGJmi9J;mi);ef#lKH(i9e=(WRJI=4$2^JUQM(m)%W!;;A;_GL5k
z!!GP+*ubImv&;e&n^Sq{i?y&>$nCoNA>o9QlSPJ8CowC}&)bGxH!E!Uv0)h!5g~8m
zc@D9Rq4@+fEkvwWZN5Oat^V?m(u=6MPKzTn-ns33)1ub0t94&)uHqT{C<SCl=pcEL
zxZ_-(?Bu7$ffh1w^!DQQt(mCJLqpFgA6A;C4Ffwuvc!CE1bEZou5Un3Oin;{RzTZv
zYX52fw!Ii@Ejm^|0szigZ_O(}a~630V6stw=l7YU%}RJVd=k*;W-Y^|n-kB8;Es_u
zU9lSobeUzKA8B-H6?3Ht8YCdZ_vR~eD|f~|eF^eEM2dlBw!pywsOQB7k4%Y(<AzBH
z{jXDVd&n9Et_n$c(+J8VkE)z7m+zE6G8X112Xu+jm7Y|b9OW2zQ@5XV!^-Egf0J&l
zmKwI4K}3F*nj#m^3CvTi1}JRP#0{uKR8QX~OFqhHUml1FelLU+V+rv5#hh(A)6x!>
zn;5C0D=K4zH<=AbX&ZcZ5$x<J3Txg13fU%poC;o?A7JQZU@$jmYVe)g8!FoA6FJP+
zIUL4OT47QCIkpi&N38soY2m9hdBtSQhx;x(fhSaF{+^v{#%^@W;3!<kx&i~9Sij3>
z9N6o>c?@cT0KUKoYS!M~-anIQ8J^K7<nq_P><7+c4%N3H?uGCfjzs0qmYkzN3Z$59
zS9!#-))_gTp2X@_t2SD`6;7byxZy1qD-hv2ZFL(-krhF<-$GN|dO^}h6FImdby!G(
zlL>hMs|(WvWoihg&q^&b623!00|QUw<k$e~y0h6m*0*b6yd(MK=HDNJ$ulxOx3_kU
zyzh&DMWDEkM(tcwQ!n4?`u+;FDjnD;EwV9MDSP)RPBHYs-PizCjM6PK2X8qsusSd$
z#xg78wU;?{iobOvaGsgvw>=$$*Jt4$tjDUjAy1_U_hsyblu|QL=w@C13Wm$qSGuK-
zHG4EY);+j(YzNp}r*q@uw^iQU?5y8OG`jHTN8<%b#Ai>>U**>kASUvYs^j?m_X%8Y
zj`vcWv>)A|j~2rj2=#GJ*VqRF&X}XCUsvswhk^n1@h3P3*oPdh48tE|8oNOV7AX`Z
zvv#m7#c<bBBm8CT^~$OjkpFgtps>Mrj|0JR>(<nP9V8{85CiayxEh@Pooc=iG+Ju*
zqV-Vqf0TT!%U9!Hzb2{l!@4BI#7r$ebop*PW87%KuA~sJ_xbknT;pN4pTF3l!mrOr
zZvCA2!KIq|t}{8;fy;Vhia)KC?^36BN%24kadI_m_w}d4W(Q@DAB)k?)Hok(nziV<
z7)amNG&UO30f<IY-IF_}gNdtyoQJE8Tak?`4|v=8N6Pi)42FtQu#%nPH0AXH(6=yN
zJ6v~_{D!+wf<f2e-a7d=FKo9waNqQV-{-EI+ikrnd+*~)?IOKfu+V^PFPA3nzUg3m
zmgH1T6E3`ic4j*R+6U;p$of84vN89Ken!Ua{B==2*uFC5AN3X_by-kO=V^cuTr8kL
zv$&AYyNt5Im-BjrKv%NxU^1%d0Wa0YS{)0bscv15-!^{P!LIZ4sQeo_#Rov!U6qp;
z*S1O*CtzX;{c4U1i>$(lc2JOGd()L@A62f!iH8VBTRaf_F~72a;zTjwTb66q$}8wU
z7NBxIs=;ZEOr~pDeDylsX7a5tDfDofUQ%A5qPqGbl6zM^iV(qQ<n`q_#`v6oK;i!6
zsr|)HK*N*Jf>@MLa_cowc=$m|982Xl#wY{VN7Ey9B781c<Qh3|_@2D`I^T8mTZ^Ud
z8-fn{l0dcp1D#2XkAHrKefMdwA1PBrB0DhtDsOKST~1R-CirwFZQ<6tZM>H-IXyGu
zPj@$c|G|-EIk{O&g#r>uG?3?dV|BDh!60=RXU?+gGr6TJH$v8{hM+lg^=__e>*L$v
zk>}1U3haHqCSr<pc@*&H=dX`i%a^97%Od9v*9)3$9q?rzw?)frtfLhKyLRDk|7GH@
zOlF*yt#T^Wy_EE;1&4`69`_F}Le$+>YI?fZysrLx-{qj!{bhltx8&|?eF-5Z-`^c0
zt(gjqSMDUEzVhl(IrSNPWutX(DpKO2Rqg6%W$>+gj^LLj-RPEJp_TMHk4L!D6@NQ4
zq+K)+eXLC>XeBJ?<8!`Hn-Suf@CnH#mR-i^#IOI|>RAOvj}joyE$XrJ;?F}5FgJrw
zT#gqKWm+>56X%yNwY_+QICxKVnWaX#YUOyNl^%h3_UZ#(c6=vh%M{YYyjqGW&*H<*
zL{~tjx`m_({Mz$>qOt}F)roJOTs{5MD}QuYLX?eUKibxC)+`!j?6j!!JhwDZdW(CN
z<075FP09LOL7~Zs8W%uj#hA#O)K?oNT{d0Xx`g{N#eoZ&AGxX{0DNy^%@{cMP^MpC
zV`Ed~VIB&<BMCiquxw-V{-bUw>)YGyK8G;9Lm!!bNfyj??vZkMn6Ejs5V)W^l9S_y
zrFobaOa8x{{p8E}=~BT$c2{hNG`$X2eZpRLn8w@G&Cv)~m~LBNPUJ2K86KtqC$Ml5
z^NR)_9PsWv%#8#_rBSnvvDRHLFZ;wX$MNf64Ve1<C04h5-a|P()!)YI&PMg}q(<;1
z^nZ~1@93!^0`6dUHDy(xw6t`%((amShIHCu11KLp0OuP^t>NV*`qi-FIq>#(#&|_k
zZhm`17Z?`UPT@~d>%B$tQJrzY>*9^uj67@Cp1;_0qe2Tm3lk38&dtDOlz;P1YtWLF
zh0uffd9j3qwN!+eDT(#i(DTLqoxKTvkt+`p%^QxvM8^E!2Q;o<qOuKIOS~88S|`UU
z1s|UVaED$puUwi8w4z6st7m4#uWz&llS%V^u@szoMiy<c<E*-1tWl7--Q75I(WK+b
zWnf56H6D&%$dlb~-pROjLW@SX+$VU}5On-dS(#)B_On$+1_s<B{c3q6@<NsUh>Jhu
z9EhCFFC0cIHm1lQKHl7$Z2I|r({DwWJ)(m%+dR^M8+*7SX!0$r-U(ymQg-w@JO(F@
zW|UGk;JETfcZ?2n^CtZ=PbWUctZ=fl@P{644x70YBg8y2<>z`T92<Pvk2(+l2U6Bh
zkPD%K<4e25NXl(}g04A~^xoaOxMw+U_QQPJOM{*7@f0EatrRwcWpsv2;0+1SwQm!r
z^7&Fvej_~A#*^1q3EBS|He48o(MR}%%Yzf;TbAYRepTS~UK}iZ1RnT#|GMs#%d&Aw
zIkM#olIF^eqE0i<$jm=pbz%+*N-Y3@+PMEF5$sK={q;V_?o@xp0fo0^huLgY`3H2v
z8`m-R4*~tx2mNZtKSM<z!Lof0;e%pauD_dbvW6q-LHIqH3F?oU<J~bF1f;N>L0tHM
zHAcQZk3v@4U3*x?_36p2OR=r;jF7wlitoV&;OJRJ#*2DC^T$1p00(DH$Xo9%1F~-`
z#hD5ge=StPDP(h1BM-ObanCD;udIx?T!hP$s}I&?qKb-|L-5tNZ;EyDi^0lrkkEr5
ztU+ZXA2%Kwwh_!}DnP*6O%m$cZLieTAA^+;C2qxsAij#A<eYc#g{o@r7wcdSOB_=m
z9^tH7VX-J{P!P%ALiY}s!ojFFwrw27TO*;zRYDW02%Kc~@8+@CBry7CtwQ|QLGod9
z4Ok6+7nEdO?y<_iSsm+)cb3!K{{*cofst~B(G(nk%l8+$y~iei?=CXQ)eas90lN^9
zI2qZTJiMU3>>W&MTxPOh@OXuvA~JS%IQ_V)(%+Wft1}3vX&wE*rO_#a>Ai74a<zlU
za*kSqh+mC;QrLwveCq6PPzd8CCPH5-wmR=?U%v{(tX&2&<?i!epK&BS+W2@T=JW9%
zrc|jEbac11jSK&zi(~DCtgNyOisG9{gb?sa7LBVNH<Lk1uk}r*WIiCJq@H1LM5nl|
z@(J_d<4fUeh%@-ea(wJ#`Q24K4$bH#S&i<@vKqJF2w+~U@_QDO10V){io>Z5VNA15
zKOVL7;8bL!nJsDlKJwnH_C2YWW{Nzn@=HMAx~R@fXT?SKvaRBcRT6&*zZ_FLn%>To
z;g8)!Q6KHkVEf13gdG7wmEp7FuKgnIV*#ZXh-;U-YAiNP-?=YeqLreKaGAR@J(JDU
zt1CMxinPc8Vk(M~RB<tpbrk{qOkQ%Zy*QnFqlK&cFgy+%{nfC?)~-%CG?n*Eykvgh
zBEsf}-R!ISX&rAn*CW$&hIPz^UK=@reUVBGm5$yBB{`hV1F7wFwv#bDUc4oYT1z5#
z;&Ou-F%*sz#J|OHB;7ll6;sHhpOjo%&3+K6)l$-sQ+>-V0bx|fGFV2Cr@@*>(KvUN
zrr419OEcLElUc=MKd)&pX3buW?r|?!rkV@;?^DGS%+1A<@<H^-tYd}21YhN^xAW%>
zzjpoZsQinip{F0tNb-o4+<YDldCUHX7r@+!$$eJmMgGL<EM{<rO8%`d0kBmaLS9H{
zJ1EF^*zJ~H$+|!pl)zb3vo4+NXuhP0wZCX3HTNVDK&*~+DzPI+)Lw7H{LVO~5KI&V
zK%Xp&CC#1!+a%s|Z-wxHzkB0T-;FN9xOS0|r`pH*pDSiGZR8mrX1|nFQ^ubNG!9%}
ztuCiO+8-5a4bGShG_JyFg8(lxzCTjI#QAjZ%A+rV1!r&SaGje?3n`tFqY}K{xMeRP
zpUJcXJx*RULgsij%`5yr5ne8kS5w~8u?6l)sHX!B%O4Fj96GX7+5OV<!S4Q<O4I!{
zmw%igUyv{5p`8DqGH*3DuH0!s*ecTKt>@t?>Z#Ytn;GF%N;ie*LbvtePp-Ls!tCq-
zO>4KZ6s|)@&^Z1)7rN_a$grsfz3Qjq19<{_c7S-tl+;Di#d$o1w_&|UnsesRhS|Px
z))96dI3Sa}R8#+m*Yc<A2tSk0EHp!t2EvPvRURtry?oprmwj-s@4Q#dA5ibcgFSR=
zHTns#PFO<(rL*QrK8VGA_4*p$<?r8>+)eL3RSvH5t7W?X{{6(u0l{>ppq-_~{$3Ef
zG-rsW$QV!HnZIuM>`qrbwN7;NW=5CqI!<R|d%PEj1gvUILwuJN?l02aWl`N$8I%7{
zLhjSyf)Zq<NgPX%L^+-wSJT-@%;!Y%Nxw?d?kXr_d?Zjk*6m7XnA1hfa>;U<a7zRr
z7muY1w2bh~skn*CCgtH@qM4C!&CGbTHBX`6laBgqB~xUm4QdE#AzU7VpQp|@KQuuX
zk4+I`y|x-~1uB$O@uWdHB=`(2l;|j0qC(^KM=4*P#u`E<&;6o=2^>@<_ABw`(EvNu
zscN}b>D0kPUWjdtjQHr#tW{Z!PC$&-O%a+j6H<*5AYgzfJr>7WZro$u_u@;ka76z%
zasmO^&+aUZSY^8{1FN*Yy3nAG5H4MszQvCi2fw|#))~~MmzRt#T_;%iQF}obcnX@X
zxUnQ0%u)PiOoEXwagF?XozqtGgZzIHi?YJ30I>_{pQWaoBXFL(I{i^i3j&1OsP|g!
z(^pAhoBt@v6{ZAGKlbsgq<I?fW?uQbgXxoZT+yY%uf0zU8m-^K=Pn2vsnu%WzKQ)1
zwRjFvI21n+D=H1gG7}aW>XR!e2{d-<JGBeEjlXhJXwW%j_ox>Cr%{HqfSmHHtD96&
zRCu8B{@W$;C<>cfx%c`No^~<Uuva)+AoRX22w0Yr?BTXGH452T8d;7BFHRjc;WfoD
z3OJ0`d-plN^H{|t711esfOtiThfIiF=x#81L53;t$_Siz-Xr65Ue-sEjDD@AQ2Rg!
zT){^xTn{N^Wu^Ne#-XG7C8b5vADG~ze;uo`FlyB11~}Iow!G&vCLlWOi1>^ky)|Oh
z1Z^tU?+<vz8a+3&+z4wIS!^bCvclZ1h&@3-uIba1PLUG$`8XK5I|#AHJdHVTR4ix&
z<PlG#L_Iv{WLNg+&cm{8#h}q7TboM-n<FLq=O8~2%=XD+=Nga7n9#m^xPq4ZES3x%
z4G63_>xZK!C-vjwI8+#(vu;_o()ti&+3_|;V=Bx|zT#t;4Nj1Gz`@5WmUwEtR!8<x
zTjjJtPbkE9y{q2f-%s>kGcZxfxJt#Ift?I|&{uMW&$Bq5qVYr=8b@3@AD&AKPji+i
z_L%$^Am3(JqC_$LF8H-o?v+m_!M9HW=O~r%^{WYmof4OtIuiWpN*8`uCjY{ZUNX|D
zK-DoS7@#L}qmEq*IxQk^xQ{0ap5gI>#atot47G~(E;qOggVsFbH1WO%dh9^<&2IQz
z>l#Z-q5c~bV^^3i(*7(EVO2})rga!akV!EOCPThR)K5~Htb4NWE)e5^L}}p~?`NYN
zbw(_9tEYG}a7zWIH8dUZwNJw@8(y2VNu#G0{zv7HiJ{PVXWM(XI6Q=Sj#?~$_76s_
zIIT#3C)q^$L>EaK&c=7Pj3_|J6R`u)Y8(wiMHVbkZHnv-BWPn}BMG6g!dp)<(l0LS
z4SlcNTQ5?P^(MFD(@I)@|B;NYp{3t|7#Z+c(-h~wSlwH-WqT`Y#s{TZhtTH1vP%~e
zQ=8ECT#^89*>_yY)A|xdW(H5F1TNriY&5|LFv9QHCvR8idY1F)muCW(pC4ah4d%(c
zcs=_;C&~37O;;bJ{=)89x$0k+r7()Hmseu!=GM`3qx{!r^+qd{(gRPW{w|j^M#hGX
zpQTSkceX{R2;E$*>g%@%ODfWb2Oe~R(vK!Xq^2Kl4;-BfI>utE$BCB<v@&>g$3i(n
z_;Cg_3)wK;Lq#s!hIKifZdA9~_{JnD2xwp*2^l`v=_}v+MudgMa~8ib83<?FmW{R1
zoGR{cLDKe9Rpt0ceB+gt5ve^C%;q$Y*nbec%XuVNn+P3@b+NQJY?$$b9cj{tFqfvY
zkJsLYNaTuPuN$q#yx^?eQe8X`k!>NFX~rJoN(e@VZ7LPXJi?(w|KGtYxnGGx@h?T&
zEl&7m`~L<Rq@6-kM<pgN&ft|EBr9&x!#Q*zy!l12^NhK{XKDYS!GayR5=)ijp)TWv
zwJs;UI!On~ro!!YUK!xjhc>k<>An3I;Zfm#EmZa98YIi~%~1=@dOo{T{6*S-_ARZ5
zYOy-QCK>#Hao>DrAYJ#}ji*`9QUcXjU$N7QuK&9Q&xx2D1z*1YdL%kpMPO=XadCVd
z-Q!&ccmRV&uH_1cl;g55kTk%1`-<10gSSyxmccWCk_m}HxJjmzZ%6I?wzFL0zZlar
z(Ob{NbxL@&>VVJ2cm#HuD_~TmTER}=XZK(HHHIO-#^rfgHdDRj4*qxnJQDBysnmkr
zXZ$$$vp$8IN`$md1B%7pozMw(8tk>&>-8GNzc5DqW4RRSU!WI#<)*#x8D6bfr|7Nn
zV&WS%t=By=yv?=N>yW<IpWErv9QtKW*CQmEb?dLKR;|n_Nvd?(j!UjuC$`d<RRR)V
zLcy<au1)AVH3w!kU&|#z`n2AlJUrZmRHq;Zy{_a0*_LN3Bw%gjQQp@gE#jJ0VGz(!
zz0C6WrHTa3jyKv;#Xk*wf;&_c_syy8(&YCwPB^v!kq4#wg4@P)VUm7@Vxmp&-TCib
zN}pJQf|Lb;nijPPl5dRNP`sTGzk2a6;Dn~~=(AuvKy7t0Qr`F~jZGI`*3ElJoaG^^
zzbA)X>J2{Ujr_<(-4sW+rOq-n0EHB9P%Q4@x0`%azokl5&OQ7OP=B!RHqwjSE`x_T
z;hMXx1}J((h$KniSd@BNY1eBc_uQZk{%j?15M(vu4Q6;7*|si`T{rofE|ooga1_?&
z3Jc???fWZ%T-)OV*@f;~y4A(ty(6i?TAJ+Iv#&xPF%-4}Qr8f#udCmDd`MRWI!@fI
zocVgc$wriWE#!_`P1{dO1KHtGj?-jnby6$?v;J-=KF#Xr;q<E&UtLTIFJ`+v&Lv{f
zubhOY+&YGQrnOojj^)<=Y+T`jJIjZ00D{e$!9~y68Gu75JrUb@CR$+Px~=-)6A@Y3
zk1(t&$j{d|+<(Qin5TYk{+Lvrocy5HC6~HJnd+Udr}tX^m507X3S@i8?F#>^oL439
zt&1*?YKi+8)U%o4%rgm~iZ{6RnaJq{zJ>mMLcF^w2x$qkPG)~iu==%rBg<2lDSgh~
zJ6)g86%jsjlvKzpbERaB-K`!7ip}0z8^~+w&AckP@h#&<&oD$mUzaxUsU_r88^ysr
zsBqj`knoWIG%}uY&}GC|ecFFL!i5y~KxnZT*d%H)m8RXP85<j|+5j#LqyZLwASb}5
zfWI;M^R3XP$bGr=e>|L!w80B2!(d%6kc6b*l;nn`!C)Rk^BaS(Ylp>*|H%F9ULg7P
ztGWIcE*AOFo6aIQE5Gzd-@^!s&U&;W27@UQgqTEcH9Jh=WtO1n7=@rD<Fiu?4YE5a
zeHnqir-$p+Fvh$r?0^d*^#JUB!gvti+<;UFP^L?7DK2!EWtc2uRcF~@{bQBc^S&Oh
ziYV;|oJzSKX1mO7r)cG>#v<m=6@5AO-&3lz<SSo4KOM<%f406WnK3&^4;Lg(S28C8
z(Off;CTj>&09YooIQBBs3QKVl4O}PL^tR4u2Np#2&Y08Y&(J@;p^NP5xHUW4*u%3(
zJ^kvxYJrOX`QZ?XBsF!oD)M`Py%nJHv`&g56K_sMvf{6R_Rjl8VVcZ}O&Tx67~yBV
zSWgubaG02Fm-YEfJ{e$FbCO<H;ZJA>1N5^*y!}BPX!1g&&Vqjm2#6mF-h0s@W8k}M
ze)7T&GF1@*G~vfG27~!?QiLC5&jO~db*B$Lpbt2v1POoZ9wS3mM65?YxJG=Uefp2E
zePd+-{v)2Bwkk+(BF0@_{YZ5=68$?9MF?V!8+mzznnfI1uh&c|Kb;{O@f}YCYUSyD
zxdN-5_!R3#2uXu-(__;o$EufAc!cev1KS!8kWQU>?$Pl3{~3<Sp1YFz209ZE)Cj9H
z{p;STiuq`rr;{H<uCu9sc-}b&><}ZLMP>*Pocj6xURY%0S(YXA?wsD6@?E^16k&(S
zk{a&Kgd*`^1`cn-I@%<mWb+`CqUZ13?UZndU!r_=M#IPp*Kl1x<Dk*puyghAppJa1
zv>`!I{k+l(YDfiyW1}4ezu?9`*`cCf2z<q@*JkhyQ;d@$yqLWOW+)l^(-Yz!Z*DE$
zwq+tRw=bN@v>p7t`duaD41j(%RAFLaF`es3Km>t_^~&+#UMrc*(Qg-QMa6CV$woR}
z^pOCyh$9f3c}#$<|L0eoNR{I+z3BIc<AVjUzv;wc#o8rYm|TU5%g@Mv&v#d|-(qxt
zK=iG`TD>QqgSZ?*1cOr1)e~prc0@|i$__^I3;xeu=!U%E2O$&8dybxIK4f6XiZ3x!
zz)A^e5xyHju6wTb(HCoej27d69g$!=Ydd&hmtN|_(KmcP$jYlGgDH?4w9#PpK!kWB
z=E`!GF^qTa;yYD{wH*qFCk@2*X1ff(O(MMt>$5D%_qK!i1Qfn^YJ4aamAOJQ0)=n#
z^SgdXijUtCW?-a4|1~qqKKSVGdzvrYhdc%0pe6D9aaNfAvuW*)Rns7@|13-<-C6P$
zP6x$^0`r3o#Hi~+N2Ojg(Yx}GY!F7|jW|1@1y3gf1p^g8jB|QEU7R^PtT>)G<KOQv
z^02S}nHMd~rZ41v4(F((T%x1%m%`!&{lCeHTQcHvY-}v<voFyWmHddT++4<Aj-QGK
zPIGklQo8sSOzUF{bb`qll|Sh@A3rW_dt<kYmOwjIQjhCRnBwplkq|^5E4F=qnRj{;
zrxO;Du%K0tf7;kaiA?JL{Fy_fxx(wNM2T41B;=kfm)t{Khpa&Y%ksb<X>*Q86Q~c*
zab(ZO#!>#Te5J6wyKMCh2CR{alZ}AiPak{)*wO`9)a%k@#u<TD_g+x*c{-lI%Bu~%
zG-B{z24J7QM4KyNnh&k}v2;?lu%??z%|cIO??EPP!a@=+O>{idsWJ<bsMD=0O>}JP
zcW*JteKC*CVYqdx=49O3?dX>|N)^3OA^CU2YRDkz?m=heoyITlZ8I}7zlYvvvJtO-
z6<6#y`vfk8j|sh|5+po6*tw@E?EdEeZw%(v8#sPfvW|HkLhS2ZpPu{j3V){kNX6+5
zoz?WtOkMu+PW<(48}EI*1qywu&LoVij}P9!M`!X}wURpq1~>%sIjPfoiH!e);tzXu
zIFjk!HIG7MVP1442G!IM-zW-P()2`34VPPS7aP>h{!dCU3KGESfJ;U6nx>k*!I$2}
zPAV`Atw{b(mCoBb9+$`P!uOH#`_A*Pk2BkL7IaENr>F0$op}GPcb+x${C-Pvd}+fc
z=r+-Z-xd#FYB(IG{GSnrZlx_{<Nh=;t>}}csRl?Rg&EGR>w=cbRb#L+YQUW5yfVxW
zxyI`wEJ6QE*Ppe=biVwKtDJ1=cEGD+c*={x${2eFV1_U^vXRL!IoW__FDGCNq#}DU
zFr$NsQ*Wi+(8icO0K;zWFVN#bo`cC>lT)m#2Wx<nc;F2PquKRciXX|m^$4sI5Kn!R
zot<dXU+2R2S9SEKwXU)u7s~ulv0|;UJWd-sSWgRxg}4zYi4VB@qn#w?vF@9I7hn|-
z_@Uc)<T>tTrf{9)mPaKO;WZ%|vD0FV$9397DzU!AWpYB)|KRZB4g8}tOw5S>JF=MP
zoAI-^=kiz`4e953nb(~z^$Ow}Eb1CZ>|5~2B!ik?kj+$226S#j$(%(7^L%a&2@RcJ
z>J)=w_{2L(XDe)kYp}XDqSx5`+Gz<I7qZ}1-`?<H!$0wQ(<pulMug_5qYZ_RW9425
z7k?gI6M2Ji;VZlo{h9j^<DT)4bB{g2+sIu$oq7>`%@`ksAP|A|bD2)CY({OvH(!@u
zkoxxd4VR<Z49KGB;hY0iLjW7h+W8$TZa6_WE((SEWYKvQ&<ePSPu^RWSy@@{B81Hw
ziS;%;A;_=LsDV0LOwXaI4>JGO$*yqDkLez%AMVB?ijCJ#ZaaEc1~fD}xoFncFa>7>
zaA1lhp!Mf{u8xi16F73;gKhP?u>EzIFGNR22R&{MeGjvti*&Lu26+X10vrU_RpPa}
zb!q!wpDB&~cPPPi4_i(&%p_s-Y)p4y?(1Uzf%3WL7l*g6ptDKC8XOJ3tZIy1J2&H@
zQm387lW?Qv|7-5rqoMlx{xPCl3d!B%8gdI2BA2A1goH$LiwKchC^M7fnxvF#xrGW1
za#sf7M@T5+5)z`2`+dy3-&4QmS?hV$yPoyFYrTKGvj#Kc%s%^^v(Mh&{n?-I=L5Fp
z(1rcyHG^l*5M(DO=NzmXh$$*^zyjxyJIq|Kw3M%YnTR@?c42+(vWw8CJid<w>BP)A
zBg?&Cy7N01zVvr?65RU*!3myUg%Rinu4K0L>+B42=5$+suK-XfYrXOgL+z5v%G^^c
zXH_Z7$&{(?wz6xhuX`-g?2iF2$0eDZe*5}LnPXJ3M!sppLYq^L>p4izb9uM^@o4qE
zP$|_yh#Qc-dsK|HokT^*tc-8P4PBGRPcKi?RIxVQ#jeX_n=wm?j9@grK(L3miA!MY
z?6j3PgSIjB@3=or`_fgKzgVp4$ErFy=m`Xnb`&fW&R;J(Es#iyFUsEI0G49qE`#=k
zO<-lC|LWNBpIx;sdwO+E3x02DPOBu^9p#gp>)c~YmY<ovOqK^^Ix(u^2>r}-PC)^Y
z@$a42fL{cdZ>2EVelU=DoAf1l8Yyuva}nEkHs$+MTnoQ(`jv~Qv;{g-Yy}%wABToW
zI+cOZNoUOMZjSojTYInatQ0$onAJ8n*Vy3K)c7;LLgt86jq@~3&4*xPxcNP%SUF&l
zd?Sba!X%JMxSv)bDa%?u#vSnCitV3MP*bp(I&N8fKV;<7-mkO6lp$*RQEih4QzPG#
z7w{W*Kq?&aDxZHv!31G|fBQ=2FH{AXd=M2~{ORRkO7GhLX3eRWr{cthq*EI|pLTLM
zGmM}a$oyYIHIHPp4OnyV3vnu2LrR5^q%Y)hMdO!oi%$nDKa)Gy8F(PHrPIxop8Uk5
zFi#|l4GOX+(QA+2>)3|6ES}$PU|QhMBEl@Z%p@FeT!8s|4c*tNUP}M3#Ql(A+49LF
z<xzvTAlv;W74*D^0}bnR^5`LTg8wViy+MTUlsv}n($8sh#zmK??kX0}<oU*EP^PMm
zz9W@@LNV+D%6doFUYgzFKX?;++O|k&hXl!UHU7;DGnVJ3`f94~JzpoC!-G+aIK)ni
z5>@-P9})sp7(>1z9|If|3iuP6c+eehE2Xul=f_>$@Oxp>p4$=+-b;dIr=NC#==ptb
z(RoeuAivfz=v#tFdBB;HmxuGrg+)YW`cLWIcQ?yr1P1oXG=t1^(qJW5xqnAaIy=Mm
z-(Oi7a;z*2Tp~nr1wr*%+WiP_w0JG)bBtHWjBd(&yOP4+z_P#@#CFMp+VMTrx>VL<
z0VBg8YxAr8^m~m`dhtI^htRDY`HnO?5VQi6Y_Jo?Se1_A)RR?Ol!1$s9H-K!rQ)_#
z&r|^pWvv1f0cp5^!O9X3hrO&P<54r$@vfY4oO)kn(0UW^ZP>izEndsEa8;Q*!xq4n
zrgp5B{O4#64p1?!zL)I@NvJq^OL45*VPERu0{_C1QYjB<Q>3o$+1b<f8N^HxY0B>%
z!6M;DQjf(wTD*;A2?lU%7;ma6oeN7Oib=OSy27nQ8(}$qJaaLdksx;BCO`8TV>HQg
z9M%C+3kx;3XBsQ=E<ULG(WNoTnh|$#Q{8^c!+FXx>yZ;N>jzjf(*D%#P)0mrLJgP@
z5r%BLlzi(+Y9L6egJo_*WV5mq&6%uv3W-2ZV{GbcA|{){$(1GP`P7H8<8jc02#AVM
zVq_Aa<x8ugv+@*kY<5XZ1|`#s9={>yx$g6!B~^2~to6@w`7xo=e{3aq3VEC3a`s2P
zoicqY{^`QJ_xGlvd`r61N&Xsd(%$XV&ItaU+;v`-5d_-X6Oq6$^9L)vmzB8Ao9CI^
zHnRTZD}{F<gZ7gk%l)Q)^lSkcDze6DzbC8qKCq8~guA`ImL<}YW9In}96?##SBhr#
z90>_MSp*gU=qW~p+SdwE`H}mjX9Ta-t4DG(Fz$#O@AYNufc&j(QYQ72b_{zq3Xzg*
zHr-JydT<Gkk)m~S6&T8=!u%kRI}$AAh44NX9>2Lg4O4MOSmo35`+j%V!NRQt*FWcO
z^_dWMM=rO$r%?0G&kyF8fffk2zyA?5V~623oKGPKCfnK$HNV!BA3b!nZjZ@R@gg%F
zm+4V#;WWBKUe20e?{*Ds5La8%&i%zeNX03^3^UE`JlHq{@z@l6_3`{iQ5Xx)x;lc$
z(b2V>)Stxm1QDBuV1cPLYTZ|Yo8m<;A$Ui|rWRQcyvIcmP<N7g7pm_uX_iaC(-;h&
zMAq*k@!Bag-53oD#7MoXWskkCK*#FIBYy$(fHyg?*yxD~=f#;JA)b8~Tm7CuV3r2V
z5-@zy1>=E^tW!L0gKsKqF%o5aBrwX!9K*wr4hZ*sNS1{ax~J_)j`i(Yr(!~W4cz+q
zIf&PSy`vFRY{9CE*Q_HCvvXO(V0wXs+bjS^Ti%6=ohZQ#>|Z)ac$ypeg>{v3wn1X>
zK-F~zh<C%IqmPwc9q?S9>cvDReMvtUOf)&xcp@`6-FGoTog8k1d+~FYKLB)n$%oTa
zZV6{lc)m>{Qjjm*ANEM*90c2Ra#p2Vkk*t#oWO@@1nYX^97AmU<kBR$eR!q$p`-&8
zjnYBXuK$`^DsR@X6kF}FU?5DVGrylMqX#2+Nkd*FB-6|B&Qpy5DvODmtmgzfTzk;n
z<E>G4?+cljn3x1f_hOi_0qd}*{F=jKrnI*ZT(l-A(!(dtU&^mNZpnhVjfTeN`pk<L
zFA(H)FYD{xl(5X%-G3a&!ge&bOPjeVB-0ZWO@&hdl8y%Ul>tIHa=X>|JwaRjuC^x4
zEH??^z4qOpgK$@9+uenk_QKc|MLJeneSLi?t5UaYa?4}6C<w0H$S6suqs+tRSyom$
z2>gV{#6Wtn!Q&Pqly)U)*t)n5GFFh}il@cz78077WcPCH%Fae!ii(YR)Hh%keN`*C
z!^gD_6dxe?A%xjkx$r3U@qWU*Fnc>bFwh-Y@?3U<S`0n?j}#>)`$s17Hf7{OhZ87|
zvAMY!ow)Ay|4Ij~r`J)334v*9!~XWe-kU3sVQc!rgaLRVM@>yl4Z}t`onG_J&CN~p
zm7wu@5cb|*nycFURNpBJdW{?1^VbeMWf@%<Dv>F0jc3R)k{oW9Oip+1sEHKXHnotN
z;szO;H8A(UDB*+^KKze3KsyGGi;0PydF^!iQ{H*be9ICY_))mA>~<Hc-aR%CGH~12
z*xDBZ!LDlvSK)#YIetz_$qgYqm*UhpO21-s5YCOW6s*zkUAxwAT<qLVh#I;m{FFA~
zyyl`TK(B<m@i$LL%yUNijs;wg($9rS-T781Ee9^&ktl9C1W8u$Ksh_8;Zuj^Ba<EO
zl%|zp#g7IOLybpFKNh<VIysCBh<Xfv-AM;=GpJL(RR<bNXh_HwadC0mo&vjx&$;nO
zx84W!GW*^lM}z=3mn}3vJv}`@2u~CSZTPWb)pkWhM4)r1AYZ9b45v1;IdnhJ=i3rC
ze&!NC7T6v2ne5(<UW?4*(F))~73R1@P?Z2qT?nE15-1IAT$_2jk^xf^*c$*+8c`sR
zmX6z8uU6giKKQe=ik1XnmzxeXH@V0%AWC$e`(L8j%(zPGVW200c+ircMI4(f_ZnXv
z$%I)ckSWb$L|D3Yb%rZ8)t*a%LB^bp6Kw0Cp9Snbm@Ah6mBd@?;!fsL0c)=fg`O2P
zU2iff+%C)@1LMu_&;l1}R<kS5g27-kh%zZUf7TM?eBk92kGS-u-=Cp9?XFYEUJDq-
z{@&gl-nozu!;U}$PH!{^RIF<w|5>;q-80)P>xDj_aNRf>I*tJT_;>keS#dB-!0rY9
zS=TIIj2#nv03pkt+PO@pf~sSI9hU-nzXFK9eAX5%)WHVxTy=+9?782_y5)uz0V=ZE
zL+f&&m1+M@&5af6?4!}i(mng?Kr>=P>xYi`VN)Ie2Jh|Zu~}HAv2vWD+Kugu6@tY%
z)c8;1#<GZP-aX<w5ou$HZq#Qi3h}gyA)(z%zN)%+cEZR&F}C2H{d89OOTakEjb87F
zr1%zIjlR&qw`?M^tq_EA8Ny+K(`7I#>`+uRLl_2Ucq`Qe)r9xVO8+PSHV7xur`0@*
z!ywPHv)5H0<R!K_L3br`@E})udOGCTJKP;A<0W}0;%2(4NhLGhsJGbObMn@eTaf1_
zS&=bfBBB{@%6)$9%*f0P!ZLCx+_wK|RAKwN*K>YUf7iZ?f{eFo$;0JwNlB4l(g#K|
zBjJ&e^S-LP9Vpcet3gq7H&RWQ0iM)ph~%<6P;|_q0Iu3tWQVgZV1q%}P^*lL49YS~
z5B8t+uy3}RwSRA<s|!SDXa^h|91MIC1XJfbVPPYXd4J5`zZ|qlxf*Az(64C4UN7X_
z1{<BZ4+a1E3&{%q`ek^ldnA-em`nP9JM{W}a{-zx-@fWqQGR8&@TMj`(>x0fdPY|8
z0+wWvcUG@BM1wo5!M$uCG$DbPmz5Fv;Mdyr3<O}c{hODmTfEI{r|n599{0k-w|((^
zw>?2KD3I>h&m_sYR1il)VJDzC+aBOo3|yQDW;wv}w4UVi!^CI0j|&V(kC>Y7Lea=n
zJhcMJY-@eE3LtAtse6Bh&yP+ilV{t|nK+x~Y8sNc#|uaq%#Wa__$??{;kC#7)rE*B
z>en;yG-~gwvsHfX28l`xRo7=4!C93<Kp+aDj$yEPWY|^mp=-6Pv$Fs?us4+XmY(V$
z^~dtokt7;b02t@;HP3zFfWZjz2$+-20piSM9A`iGt$|k~@IYgXPz+YxFAZ3H6CIgb
z!8i50*BZbknczF;JXEdWH`N1+nT^9JrVU7nRoXSz!_iv*?xw1|-)O`J?A<E;rXxZ7
z!L7e^I}}=vz5WHJH-5W3N9(#)VE|F|8r93QuHxQUo_=>~-`kM*i-Ao0om!>7Hbqb~
z!<i%gJ9_-UpdskmV$yJHV>})qiCL6LJPhT`1XOubrLNWF;)oWQQ(@AeqQebRzH*ku
z#$w0sdiv+X*|`EyhI~AJ_zsULblBL70-86=0r`DIwr{<cGuJ=)G5t1>8{^_dy^St@
zBY*K9iN5l!K4J}<WHm2Ftpbfr4a)4hH7tLu74&*{8vnqw(U0Y<F5>e+9Sbo0Mv&4Q
z=sOO^s!pv5w;<ZV?Ay~Evq~on;})b->^T`Xs}py=g}BP1^SAjbRO`^2K?t{G9R`YB
z&SsB!R81?R_>0r)Uf06P2g$3O`nY-zxK)32?unB&doLj3B%}3hsL)|6z$M{3uxkEd
zx*3AdN9u?H+y*F4+hQP$X0dT`anqGr)K#%aE}2khtKf*Y^-pkfw@k-(@4h+LSJqXb
zpxT*cL&7m=(goq_ZHz%q(XPbu45%!Iky{?g#5?9ez$71EW+AI*Xt->d_i9uEhkqb6
z2Tf_C+-r;(4NyEn67t{F+f;ZthQQ68oqV;9)9COm@VO1#Z<-+soj6Iwu5Jr2Yp9zL
zFi05?cx5mtlXao`DiLCuo(gYC^gw<MRE>Z<CcxyEF%md!wUEW>ejerI?LZ}MiSJ=i
z!r6G<lgY8LmYeyx6b$z>RibQe--U8^aFMF{)(~Y)?K)%Z#Yuy)yuf$T+#ppGD}-AW
z;g-KJ(Z(K8v<m{=a4#ijZ&0jCzoWf_L!m2KK1DM~t?LFf?Y70#%Gb~@py3Q?o9X77
z=Y!OuT<cQ}0jOi!*(vSXdJpB~xf*&lKsnw)!pG2j$s$qs+dgMSMMZ@~gT#}rA5soI
zbj$;F#OoC|_4Kx#jF+D-v9NAFO`(eKo7;Es4e!xV)>;rjjfQ^Ne}AhiD6swn8)*dX
zlDcw?9O^=tz`14XTW*;%<_XAyp{R(6Yq@Hr5H5h6b1k&8j~T~Ukecsp$*~U}1Vc3K
zUu=}MtFx=in~Mm>M#Me%t!%sNp~wXdzPX0AmC`@9n1T=)mT&y3B9b6{jZp`)$}u%G
zx<?msX8Rzv1v;6E$bkc&a_7`q(8~TfueZ0dHP+o>0p*f19^L2iW8}5=_8C6Wn6;NH
zOtC6JR<=HP<&z#zr1jCCgKj;>OF&n)O9Y4K=OBV8ofTJkbFGM9-7gQ~vxjP4qv3s}
z?#cX0m%^FZ*v{cMNl&WIxyZ59D5TIm)QNa2T0bZqNH=)P)|K7kh<a_^4faek14(Z4
z;F7*Rx2Vix%!Q}8ia9yfvJZ)gH4zl3t$E|O%%;t5hFHHj_5d0%g*rYyE-NSJ=;S1E
z?%X-P9bv?ANHY0r#^^zZbKJ|9UV(v0midn}Gc!%i9u0f0zOah9ch7`K?wVZk^7ekX
zw6p|4=IF0q^7=&;6^Wlee<p8~m6h=c3AHyi9>03^YUkt<Z0&vXCgp`XEq_zr#LVpF
z%lCD4I_Bo)@cI>zNOGWttqASj?F0byz}1zu8`Rm^+0-6VRaNrChYwj9Z^KhiQ1B9n
znOa#{4HXG(5vIvxc1S%jl!T=~c*2X7l@*U)pRSZFZfuG-kBm4xj*s83q4DH+W-QEv
z;ONHY=jSK?8Xwu9txv|SeCX}%jTuT6-^9@dHv9YgQ>)ARr?Wp_$?9;vgu`j_@bD1Z
zySuxK%F9>3Q`go4`lkK(qmMgXzWka*qWut{HJ?}^tsE+|%qL<uRfKv;u-z|eNl6J&
zh}wT*d1h+r($%Z-BO@b;DJce310MzlFT$hR?vOZMoJ4A1MYK66%)CgN0*3wn<1aiW
X<;qBy&53an1Rnaw40T`W*arU>sY6$C

literal 0
HcmV?d00001

diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp
index dbe04b1f2f3..02afd8ea575 100644
--- a/nav2_util/include/nav2_util/controller_utils.hpp
+++ b/nav2_util/include/nav2_util/controller_utils.hpp
@@ -49,6 +49,21 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint(
   double &, const nav_msgs::msg::Path &,
   const bool interpolate_after_goal = false);
 
+
+/**
+* @brief Find the linear interpolation between two points
+* at a given distance starting from first endpoint.
+* @param p1 first endpoint of line segment
+* @param p2 second endpoint of line segment
+* @param target_dist interpolation distance from first endpoint of line segment
+* @return point of intersection
+*/
+geometry_msgs::msg::Point linearInterpolation(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  const double target_dist);
+  
+
 }  // namespace nav2_util
 
 #endif  // NAV2_UTIL__CONTROLLER_UTILS_HPP_
diff --git a/nav2_util/src/controller_utils.cpp b/nav2_util/src/controller_utils.cpp
index 18f2ae42dd3..52b484d1d91 100644
--- a/nav2_util/src/controller_utils.cpp
+++ b/nav2_util/src/controller_utils.cpp
@@ -54,6 +54,25 @@ geometry_msgs::msg::Point circleSegmentIntersection(
   return p;
 }
 
+geometry_msgs::msg::Point linearInterpolation(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  const double target_dist)
+{
+  geometry_msgs::msg::Point result;
+
+  double dx = p2.x - p1.x;
+  double dy = p2.y - p1.y;
+  double d_dist = std::hypot(dx, dy);
+
+  double target_ratio = target_dist / d_dist;
+
+  result.x = p1.x + target_ratio * dx;
+  result.y = p1.y + target_ratio * dy;
+  return result;
+}
+
+
 geometry_msgs::msg::PoseStamped getLookAheadPoint(
   double & lookahead_dist,
   const nav_msgs::msg::Path & transformed_plan,
@@ -63,21 +82,25 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint(
   // Using distance along the path
   const auto & poses = transformed_plan.poses;
   auto goal_pose_it = poses.begin();
-  double d = 0.0;
+  double path_dist = 0.0;
+  double interpolation_dist = 0.0;
 
   bool pose_found = false;
   for (size_t i = 1; i < poses.size(); i++) {
     const auto & prev_pose = poses[i - 1].pose.position;
     const auto & curr_pose = poses[i].pose.position;
 
-    d += std::hypot(curr_pose.x - prev_pose.x, curr_pose.y - prev_pose.y);
-    if (d >= lookahead_dist) {
+    const double d = std::hypot(curr_pose.x - prev_pose.x, curr_pose.y - prev_pose.y);
+    if (path_dist + d >= lookahead_dist) {
       goal_pose_it = poses.begin() + i;
       pose_found = true;
       break;
     }
+
+    path_dist += d;
   }
 
+  interpolation_dist = lookahead_dist - path_dist;
   if (!pose_found) {
     goal_pose_it = poses.end();
   }
@@ -98,10 +121,10 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint(
       projected_position.x += cos(end_path_orientation) * lookahead_dist;
       projected_position.y += sin(end_path_orientation) * lookahead_dist;
 
-      // Use the circle intersection to find the position at the correct look
+      // Use the linear interpolation to find the position at the correct look
       // ahead distance
-      const auto interpolated_position = circleSegmentIntersection(
-        last_pose_it->pose.position, projected_position, lookahead_dist);
+      const auto interpolated_position = linearInterpolation(
+        last_pose_it->pose.position, projected_position, interpolation_dist);
 
       geometry_msgs::msg::PoseStamped interpolated_pose;
       interpolated_pose.header = last_pose_it->header;
@@ -109,19 +132,19 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint(
 
       return interpolated_pose;
     } else {
-      lookahead_dist = d;  // Updating lookahead distance since using the final point
+      lookahead_dist = path_dist;  // Updating lookahead distance since using the final point
       goal_pose_it = std::prev(transformed_plan.poses.end());
     }
   } else if (goal_pose_it != transformed_plan.poses.begin()) {
-    // Find the point on the line segment between the two poses
+    // Find the point on the robot path
     // that is exactly the lookahead distance away from the robot pose (the origin)
-    // This can be found with a closed form for the intersection of a segment and a circle
-    // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
-    // and goal_pose is guaranteed to be outside the circle.
+    // This can be found with a linear interpolation between the prev_pose and
+    // the goal_pose, moving interpolation_dist starting from prev_pose in the
+    // direction of goal_pose.
     auto prev_pose_it = std::prev(goal_pose_it);
-    auto point = circleSegmentIntersection(
+    auto point = linearInterpolation(
       prev_pose_it->pose.position,
-      goal_pose_it->pose.position, lookahead_dist);
+      goal_pose_it->pose.position, interpolation_dist);
 
     // Calculate orientation towards interpolated position
     // Convert yaw to quaternion