#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
#include <cstdint>
#include <initializer_list>
#include <limits>
#include <memory>
#include <queue>
#include <stdexcept>
#include <sstream>
#include <string>
#include <utility>
#include <vector>

#include "builtin_interfaces/msg/time.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/string.hpp"
#include "tf2/exceptions.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

namespace
{
constexpr double kPi = 3.14159265358979323846;

double yawFromQuaternion(const geometry_msgs::msg::Quaternion & q)
{
  return std::atan2(
    2.0 * (q.w * q.z + q.x * q.y),
    1.0 - 2.0 * (q.y * q.y + q.z * q.z));
}

double normalizeAngle(double angle)
{
  return std::atan2(std::sin(angle), std::cos(angle));
}

double positiveAngle(double angle)
{
  const double wrapped = normalizeAngle(angle);
  return wrapped < 0.0 ? wrapped + 2.0 * kPi : wrapped;
}

builtin_interfaces::msg::Time toMsgTime(const rclcpp::Time & time)
{
  builtin_interfaces::msg::Time stamp;
  const int64_t nanoseconds = time.nanoseconds();
  stamp.sec = static_cast<int32_t>(nanoseconds / 1000000000LL);
  stamp.nanosec = static_cast<uint32_t>(nanoseconds % 1000000000LL);
  return stamp;
}

geometry_msgs::msg::PoseStamped makePose(
  const std::string & frame_id,
  const builtin_interfaces::msg::Time & stamp,
  double x,
  double y,
  double yaw)
{
  geometry_msgs::msg::PoseStamped pose;
  pose.header.frame_id = frame_id;
  pose.header.stamp = stamp;
  pose.pose.position.x = x;
  pose.pose.position.y = y;
  pose.pose.position.z = 0.0;
  pose.pose.orientation.z = std::sin(0.5 * yaw);
  pose.pose.orientation.w = std::cos(0.5 * yaw);
  return pose;
}
}  // namespace

class GridAstarThetaPlanner : public rclcpp::Node
{
public:
  GridAstarThetaPlanner()
  : Node("grid_astar_theta_planner"),
    tf_buffer_(this->get_clock()),
    tf_listener_(tf_buffer_)
  {
    declareParameters();
    loadParameters();
    validateMap();

    plan_pub_ = create_publisher<nav_msgs::msg::Path>(plan_topic_, 10);
    status_pub_ = create_publisher<std_msgs::msg::String>(status_topic_, 10);

    odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
      odom_topic_, 10,
      std::bind(&GridAstarThetaPlanner::odomCallback, this, std::placeholders::_1));
    goal_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
      goal_pose_topic_, 10,
      std::bind(&GridAstarThetaPlanner::goalCallback, this, std::placeholders::_1));
    obstacles_sub_ = create_subscription<std_msgs::msg::Float32MultiArray>(
      obstacles_topic_, 10,
      std::bind(&GridAstarThetaPlanner::obstaclesCallback, this, std::placeholders::_1));
    clear_obstacles_sub_ = create_subscription<std_msgs::msg::Bool>(
      clear_obstacles_topic_, 10,
      std::bind(&GridAstarThetaPlanner::clearObstaclesCallback, this, std::placeholders::_1));
    cancel_sub_ = create_subscription<std_msgs::msg::Bool>(
      cancel_topic_, 10,
      std::bind(&GridAstarThetaPlanner::cancelCallback, this, std::placeholders::_1));
    const auto static_map_qos = rclcpp::QoS(rclcpp::KeepLast(1))
      .reliable().transient_local();
    costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
      costmap_topic_, static_map_qos,
      std::bind(&GridAstarThetaPlanner::costmapCallback, this, std::placeholders::_1));

    const auto period = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::duration<double>(1.0 / std::max(0.1, replan_rate_)));
    replan_timer_ = create_wall_timer(period, std::bind(&GridAstarThetaPlanner::onReplanTimer, this));
    if (state_log_period_ > 0.0) {
      const auto log_period = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::duration<double>(std::max(0.001, state_log_period_)));
      state_log_timer_ =
        create_wall_timer(log_period, std::bind(&GridAstarThetaPlanner::logRuntimeState, this));
    }

    publishStatus(
      "ready hybrid_astar size=" + std::to_string(width_) + "x" + std::to_string(height_) +
      " resolution=" + std::to_string(resolution_) +
      " min_turning_radius=" + std::to_string(min_turning_radius_));
  }

private:
  struct Pose2D
  {
    double x{0.0};
    double y{0.0};
    double yaw{0.0};
  };

  struct Obstacle
  {
    double x{0.0};
    double y{0.0};
    double radius{0.0};
  };

  struct Cell
  {
    int x{0};
    int y{0};
  };

  struct SearchState
  {
    int x{0};
    int y{0};
    int yaw_bin{0};
    int gear{1};
  };

  struct SearchNode
  {
    SearchState state;
    double x{0.0};
    double y{0.0};
    double yaw{0.0};
    double g{0.0};
    int parent{-1};
    int gear{1};
    int steering{0};
    std::vector<Pose2D> segment;
  };

  struct QueueItem
  {
    double priority{0.0};
    int node_index{0};

    bool operator<(const QueueItem & other) const
    {
      return priority > other.priority;
    }
  };

  struct GridQueueItem
  {
    double cost{0.0};
    int cell_index{0};

    bool operator<(const GridQueueItem & other) const
    {
      return cost > other.cost;
    }
  };

  struct CostmapView
  {
    nav_msgs::msg::OccupancyGrid::SharedPtr msg;
    bool enabled{false};
    bool same_frame{true};
    double frame_tx{0.0};
    double frame_ty{0.0};
    double frame_yaw{0.0};
    double origin_yaw{0.0};
  };

  struct SearchResult
  {
    std::vector<Pose2D> path;
    int iterations{0};
    bool timed_out{false};
    bool reached{false};
  };

  void declareParameters()
  {
    declare_parameter("map_frame", "map");
    declare_parameter("odom_topic", "/odom");
    declare_parameter("goal_pose_topic", "/goal_pose");
    declare_parameter("obstacles_topic", "/random_obstacles");
    declare_parameter("clear_obstacles_topic", "/clear_random_obstacles");
    declare_parameter("cancel_topic", "/navigation_cancel");
    declare_parameter("costmap_topic", "/global_costmap/costmap");
    declare_parameter("plan_topic", "/plan");
    declare_parameter("status_topic", "/ackermann_lattice_status");
    declare_parameter("map_min_x", -5.0);
    declare_parameter("map_min_y", -5.0);
    declare_parameter("map_max_x", 5.0);
    declare_parameter("map_max_y", 5.0);
    declare_parameter("resolution", 0.10);
    declare_parameter("fit_map_to_costmap", true);
    declare_parameter("obstacle_format", "xyr");
    declare_parameter("default_obstacle_radius", 0.15);
    declare_parameter("robot_radius", 0.14);
    declare_parameter("localization_error", 0.10);
    declare_parameter("latency_margin", 0.05);
    declare_parameter("safety_margin", 0.05);
    declare_parameter("unknown_obstacle_radius", 0.15);
    declare_parameter("accumulate_obstacles", true);
    declare_parameter("obstacle_merge_distance", 0.20);
    declare_parameter("costmap_occupied_threshold", 50);
    declare_parameter("unknown_is_obstacle", true);
    declare_parameter("outside_costmap_is_obstacle", true);
    declare_parameter("inflate_static_map", false);
    declare_parameter("plan_on_obstacle_update", true);
    declare_parameter("max_iterations", 50000);
    declare_parameter("planning_timeout_ms", 100.0);
    declare_parameter("replan_rate", 2.0);
    declare_parameter("min_turning_radius", 0.40);
    declare_parameter("yaw_bins", 72);
    declare_parameter("primitive_length", 0.12);
    declare_parameter("collision_sample_step", 0.03);
    declare_parameter("goal_xy_tolerance", 0.20);
    declare_parameter("goal_yaw_tolerance", 3.0 * kPi / 180.0);
    declare_parameter("require_goal_yaw", true);
    declare_parameter("use_grid_heuristic", true);
    declare_parameter("heuristic_weight", 1.0);
    declare_parameter("goal_yaw_heuristic_weight", 1.0);
    declare_parameter("reverse_penalty", 1.3);
    declare_parameter("turn_penalty", 0.05);
    declare_parameter("gear_change_penalty", 0.20);
    declare_parameter("steering_change_penalty", 0.02);
    declare_parameter("max_odom_age", 0.80);
    declare_parameter("tf_timeout", 0.05);
    declare_parameter("state_log_period", 1.0);
    declare_parameter("search_progress_log_period_ms", 500.0);
  }

  void loadParameters()
  {
    map_frame_ = get_parameter("map_frame").as_string();
    odom_topic_ = get_parameter("odom_topic").as_string();
    goal_pose_topic_ = get_parameter("goal_pose_topic").as_string();
    obstacles_topic_ = get_parameter("obstacles_topic").as_string();
    clear_obstacles_topic_ = get_parameter("clear_obstacles_topic").as_string();
    cancel_topic_ = get_parameter("cancel_topic").as_string();
    costmap_topic_ = get_parameter("costmap_topic").as_string();
    plan_topic_ = get_parameter("plan_topic").as_string();
    status_topic_ = get_parameter("status_topic").as_string();
    map_min_x_ = get_parameter("map_min_x").as_double();
    map_min_y_ = get_parameter("map_min_y").as_double();
    map_max_x_ = get_parameter("map_max_x").as_double();
    map_max_y_ = get_parameter("map_max_y").as_double();
    resolution_ = get_parameter("resolution").as_double();
    fit_map_to_costmap_ = get_parameter("fit_map_to_costmap").as_bool();
    obstacle_format_ = get_parameter("obstacle_format").as_string();
    default_obstacle_radius_ = get_parameter("default_obstacle_radius").as_double();
    robot_radius_ = get_parameter("robot_radius").as_double();
    localization_error_ = get_parameter("localization_error").as_double();
    latency_margin_ = get_parameter("latency_margin").as_double();
    safety_margin_ = get_parameter("safety_margin").as_double();
    unknown_obstacle_radius_ = get_parameter("unknown_obstacle_radius").as_double();
    accumulate_obstacles_ = get_parameter("accumulate_obstacles").as_bool();
    obstacle_merge_distance_ = get_parameter("obstacle_merge_distance").as_double();
    costmap_occupied_threshold_ =
      static_cast<int>(get_parameter("costmap_occupied_threshold").as_int());
    unknown_is_obstacle_ = get_parameter("unknown_is_obstacle").as_bool();
    outside_costmap_is_obstacle_ = get_parameter("outside_costmap_is_obstacle").as_bool();
    inflate_static_map_ = get_parameter("inflate_static_map").as_bool();
    plan_on_obstacle_update_ = get_parameter("plan_on_obstacle_update").as_bool();
    max_iterations_ = static_cast<int>(get_parameter("max_iterations").as_int());
    planning_timeout_ms_ = get_parameter("planning_timeout_ms").as_double();
    replan_rate_ = get_parameter("replan_rate").as_double();
    min_turning_radius_ = get_parameter("min_turning_radius").as_double();
    yaw_bins_ = static_cast<int>(get_parameter("yaw_bins").as_int());
    primitive_length_ = get_parameter("primitive_length").as_double();
    collision_sample_step_ = get_parameter("collision_sample_step").as_double();
    goal_xy_tolerance_ = get_parameter("goal_xy_tolerance").as_double();
    goal_yaw_tolerance_ = get_parameter("goal_yaw_tolerance").as_double();
    require_goal_yaw_ = get_parameter("require_goal_yaw").as_bool();
    use_grid_heuristic_ = get_parameter("use_grid_heuristic").as_bool();
    heuristic_weight_ = get_parameter("heuristic_weight").as_double();
    goal_yaw_heuristic_weight_ = get_parameter("goal_yaw_heuristic_weight").as_double();
    reverse_penalty_ = get_parameter("reverse_penalty").as_double();
    turn_penalty_ = get_parameter("turn_penalty").as_double();
    gear_change_penalty_ = get_parameter("gear_change_penalty").as_double();
    steering_change_penalty_ = get_parameter("steering_change_penalty").as_double();
    max_odom_age_ = get_parameter("max_odom_age").as_double();
    tf_timeout_ = get_parameter("tf_timeout").as_double();
    state_log_period_ = get_parameter("state_log_period").as_double();
    search_progress_log_period_ms_ = get_parameter("search_progress_log_period_ms").as_double();
  }

  void validateMap()
  {
    if (resolution_ <= 0.0) {
      throw std::runtime_error("resolution must be positive");
    }
    if (map_max_x_ <= map_min_x_ || map_max_y_ <= map_min_y_) {
      throw std::runtime_error("invalid map bounds");
    }
    if (min_turning_radius_ <= 0.0) {
      throw std::runtime_error("min_turning_radius must be positive");
    }
    if (yaw_bins_ < 8) {
      throw std::runtime_error("yaw_bins must be >= 8");
    }
    if (primitive_length_ <= 0.0 || collision_sample_step_ <= 0.0) {
      throw std::runtime_error("primitive_length and collision_sample_step must be positive");
    }
    if (heuristic_weight_ <= 0.0) {
      throw std::runtime_error("heuristic_weight must be positive");
    }
    if (goal_yaw_heuristic_weight_ < 0.0) {
      throw std::runtime_error("goal_yaw_heuristic_weight must be non-negative");
    }
    if (goal_yaw_tolerance_ < 0.0) {
      throw std::runtime_error("goal_yaw_tolerance must be non-negative");
    }

    width_ = static_cast<int>(std::ceil((map_max_x_ - map_min_x_) / resolution_));
    height_ = static_cast<int>(std::ceil((map_max_y_ - map_min_y_) / resolution_));
    if (width_ <= 1 || height_ <= 1) {
      throw std::runtime_error("map bounds are too small for the selected resolution");
    }
  }

  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
  {
    latest_odom_ = msg;
  }

  void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
  {
    Pose2D transformed;
    if (!transformPoseMsg(*msg, transformed)) {
      publishStatus("goal_rejected transform_failed");
      return;
    }
    goal_pose_ = transformed;
    has_goal_ = true;
    pending_replan_ = false;
    publishStatus(
      "goal_received map=(" + formatDouble(goal_pose_.x) + "," +
      formatDouble(goal_pose_.y) + "," + formatDouble(goal_pose_.yaw) + ")");
    plan();
  }

  void obstaclesCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
  {
    const auto incoming = parseObstacles(msg->data);
    if (accumulate_obstacles_) {
      mergeObstacles(incoming);
    } else {
      obstacles_ = incoming;
    }
    publishStatus(
      "obstacles_updated incoming=" + std::to_string(incoming.size()) +
      " stored=" + std::to_string(obstacles_.size()));
    requestReplan();
  }

  void clearObstaclesCallback(const std_msgs::msg::Bool::SharedPtr msg)
  {
    if (!msg->data) {
      return;
    }
    obstacles_.clear();
    publishStatus("obstacles_cleared");
    requestReplan();
  }

  void cancelCallback(const std_msgs::msg::Bool::SharedPtr msg)
  {
    if (!msg->data) {
      return;
    }
    has_goal_ = false;
    pending_replan_ = false;
    last_path_.clear();
    publishEmptyPath();
    publishStatus("navigation_cancelled");
  }

  void costmapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
  {
    latest_costmap_ = msg;
    maybeFitMapToCostmap(*msg);
    requestReplan();
  }

  void onReplanTimer()
  {
    if (!pending_replan_ || !has_goal_) {
      return;
    }
    pending_replan_ = false;
    plan();
  }

  void requestReplan()
  {
    if (plan_on_obstacle_update_ && has_goal_) {
      pending_replan_ = true;
    }
  }

  void maybeFitMapToCostmap(const nav_msgs::msg::OccupancyGrid & msg)
  {
    if (!fit_map_to_costmap_ || msg.info.width == 0 || msg.info.height == 0 ||
      msg.info.resolution <= 0.0)
    {
      return;
    }

    const std::string frame = msg.header.frame_id.empty() ? map_frame_ : msg.header.frame_id;
    const double origin_yaw = yawFromQuaternion(msg.info.origin.orientation);
    if (frame != map_frame_ || std::abs(normalizeAngle(origin_yaw)) > 1e-3) {
      return;
    }

    const double new_min_x = msg.info.origin.position.x;
    const double new_min_y = msg.info.origin.position.y;
    const double new_max_x = new_min_x + static_cast<double>(msg.info.width) * msg.info.resolution;
    const double new_max_y = new_min_y + static_cast<double>(msg.info.height) * msg.info.resolution;
    if (std::abs(new_min_x - map_min_x_) < 1e-6 &&
      std::abs(new_min_y - map_min_y_) < 1e-6 &&
      std::abs(new_max_x - map_max_x_) < 1e-6 &&
      std::abs(new_max_y - map_max_y_) < 1e-6)
    {
      return;
    }

    map_min_x_ = new_min_x;
    map_min_y_ = new_min_y;
    map_max_x_ = new_max_x;
    map_max_y_ = new_max_y;
    validateMap();
    publishStatus(
      "costmap_bounds_applied size=" + std::to_string(width_) + "x" + std::to_string(height_));
  }

  std::vector<Obstacle> parseObstacles(const std::vector<float> & values) const
  {
    const int stride = obstacle_format_ == "xyr" ? 3 : 2;
    std::vector<Obstacle> obstacles;
    const int count = static_cast<int>(values.size()) / stride;
    obstacles.reserve(count);
    for (int i = 0; i < count; ++i) {
      const int base = i * stride;
      Obstacle obs;
      obs.x = values[base];
      obs.y = values[base + 1];
      obs.radius = default_obstacle_radius_;
      if (stride == 3) {
        obs.radius = std::max(0.0, static_cast<double>(values[base + 2]));
      }
      obstacles.push_back(obs);
    }
    return obstacles;
  }

  void mergeObstacles(const std::vector<Obstacle> & incoming)
  {
    const double merge_distance = std::max(0.0, obstacle_merge_distance_);
    const double merge_distance_sq = merge_distance * merge_distance;

    for (const auto & obs : incoming) {
      auto matched = std::find_if(
        obstacles_.begin(), obstacles_.end(),
        [&obs, merge_distance_sq](const Obstacle & existing) {
          const double dx = obs.x - existing.x;
          const double dy = obs.y - existing.y;
          return dx * dx + dy * dy <= merge_distance_sq;
        });

      if (matched == obstacles_.end()) {
        obstacles_.push_back(obs);
        continue;
      }

      matched->x = obs.x;
      matched->y = obs.y;
      matched->radius = std::max(matched->radius, obs.radius);
    }
  }

  bool plan()
  {
    const auto search_start = std::chrono::steady_clock::now();
    Pose2D start_pose;
    if (!currentPose(start_pose)) {
      publishStatus("plan_rejected no_recent_odom");
      return false;
    }
    if (!has_goal_) {
      publishStatus("plan_rejected no_goal");
      return false;
    }

    publishStatus(
      "plan_start start=(" + formatDouble(start_pose.x) + "," + formatDouble(start_pose.y) +
      "," + formatDouble(start_pose.yaw) + ") goal=(" + formatDouble(goal_pose_.x) + "," +
      formatDouble(goal_pose_.y) + "," + formatDouble(goal_pose_.yaw) + ") obstacles=" +
      std::to_string(obstacles_.size()) + " resolution=" + formatDouble(resolution_) +
      " yaw_bins=" + std::to_string(yaw_bins_) + " timeout_ms=" +
      formatDouble(planning_timeout_ms_) + " grid_heuristic=" +
      std::string(use_grid_heuristic_ ? "true" : "false") + " heuristic_weight=" +
      formatDouble(heuristic_weight_) + " yaw_heuristic_weight=" +
      formatDouble(goal_yaw_heuristic_weight_) + " goal_yaw_tolerance=" +
      formatDouble(goal_yaw_tolerance_));

    Cell start_cell;
    Cell goal_cell;
    if (!worldToGrid(start_pose.x, start_pose.y, start_cell) ||
      !worldToGrid(goal_pose_.x, goal_pose_.y, goal_cell))
    {
      publishEmptyPath();
      publishStatus("plan_rejected out_of_bounds clear_path");
      return false;
    }

    auto occupied = buildOccupancy(start_pose, goal_pose_);
    if (!poseCollisionFree(start_pose, occupied) || !poseCollisionFree(goal_pose_, occupied)) {
      return handlePlanningFailure("start_or_goal_occupied", occupied);
    }

    const SearchResult result = search(start_pose, goal_pose_, occupied, search_start);
    if (!result.reached || result.path.empty()) {
      return handlePlanningFailure(result.timed_out ? "timeout" : "no_path", occupied);
    }

    last_path_ = result.path;
    publishPath(last_path_);

    const Pose2D & published_end = result.path.back();
    const double goal_error =
      std::hypot(published_end.x - goal_pose_.x, published_end.y - goal_pose_.y);
    const double yaw_error = std::abs(normalizeAngle(published_end.yaw - goal_pose_.yaw));
    const double planning_ms = std::chrono::duration<double, std::milli>(
      std::chrono::steady_clock::now() - search_start).count();
    publishStatus(
      "plan_ok hybrid_astar points=" + std::to_string(last_path_.size()) +
      " iterations=" + std::to_string(result.iterations) +
      " obstacles=" + std::to_string(obstacles_.size()) +
      " planning_ms=" + formatDouble(planning_ms) +
      " goal_error=" + formatDouble(goal_error) +
      " yaw_error=" + formatDouble(yaw_error));
    return true;
  }

  bool handlePlanningFailure(const std::string & reason, const std::vector<bool> & occupied)
  {
    if (!last_path_.empty() && pathCollisionFree(last_path_, occupied)) {
      publishStatus("plan_failed " + reason + " keeping_last_path");
      return false;
    }

    last_path_.clear();
    publishEmptyPath();
    publishStatus("plan_failed " + reason + " clear_path");
    return false;
  }

  std::vector<bool> buildOccupancy(const Pose2D &, const Pose2D &)
  {
    std::vector<bool> occupied(static_cast<size_t>(width_ * height_), false);
    const CostmapView costmap = makeCostmapView();

    if (costmap.enabled) {
      for (int gy = 0; gy < height_; ++gy) {
        for (int gx = 0; gx < width_; ++gx) {
          const auto [wx, wy] = gridToWorld(Cell{gx, gy});
          if (costmapOccupiedAt(costmap, wx, wy)) {
            occupied[index(Cell{gx, gy})] = true;
          }
        }
      }
      if (inflate_static_map_) {
        const double inflation_radius =
          robot_radius_ + localization_error_ + latency_margin_ + safety_margin_;
        const int cell_radius = static_cast<int>(std::ceil(inflation_radius / resolution_));
        const std::vector<bool> source = occupied;
        for (int gy = 0; gy < height_; ++gy) {
          for (int gx = 0; gx < width_; ++gx) {
            if (!source[index(Cell{gx, gy})]) {
              continue;
            }
            for (int oy = -cell_radius; oy <= cell_radius; ++oy) {
              for (int ox = -cell_radius; ox <= cell_radius; ++ox) {
                if (ox * ox + oy * oy > cell_radius * cell_radius) {
                  continue;
                }
                const Cell inflated{gx + ox, gy + oy};
                if (inBounds(inflated)) {
                  occupied[index(inflated)] = true;
                }
              }
            }
          }
        }
      }
    }

    const double base_padding =
      robot_radius_ + localization_error_ + latency_margin_ + safety_margin_;
    for (const auto & obs : obstacles_) {
      const double radius = std::max(obs.radius, unknown_obstacle_radius_) + base_padding;
      Cell min_cell;
      Cell max_cell;
      worldToGrid(obs.x - radius, obs.y - radius, min_cell, true);
      worldToGrid(obs.x + radius, obs.y + radius, max_cell, true);
      const double radius_sq = radius * radius;
      for (int gy = min_cell.y; gy <= max_cell.y; ++gy) {
        for (int gx = min_cell.x; gx <= max_cell.x; ++gx) {
          const auto [wx, wy] = gridToWorld(Cell{gx, gy});
          const double dx = wx - obs.x;
          const double dy = wy - obs.y;
          if ((dx * dx + dy * dy) <= radius_sq) {
            occupied[index(Cell{gx, gy})] = true;
          }
        }
      }
    }

    return occupied;
  }

  CostmapView makeCostmapView()
  {
    CostmapView view;
    view.msg = latest_costmap_;
    if (!view.msg || view.msg->info.width == 0 || view.msg->info.height == 0 ||
      view.msg->info.resolution <= 0.0 || view.msg->data.empty())
    {
      return view;
    }

    const std::string frame =
      view.msg->header.frame_id.empty() ? map_frame_ : view.msg->header.frame_id;
    view.origin_yaw = yawFromQuaternion(view.msg->info.origin.orientation);
    view.same_frame = frame == map_frame_;
    if (!view.same_frame) {
      try {
        const auto transform = tf_buffer_.lookupTransform(
          frame, map_frame_, tf2::TimePointZero,
          tf2::durationFromSec(tf_timeout_));
        const auto & t = transform.transform.translation;
        view.frame_tx = t.x;
        view.frame_ty = t.y;
        view.frame_yaw = yawFromQuaternion(transform.transform.rotation);
      } catch (const tf2::TransformException & ex) {
        const auto now_ns = now().nanoseconds();
        if (now_ns - last_tf_warn_ns_ > 2000000000LL) {
          last_tf_warn_ns_ = now_ns;
          RCLCPP_WARN(
            get_logger(), "TF unavailable %s->%s for costmap query: %s",
            map_frame_.c_str(), frame.c_str(), ex.what());
        }
        return view;
      }
    }

    view.enabled = true;
    return view;
  }

  bool costmapOccupiedAt(const CostmapView & view, double wx, double wy) const
  {
    if (!view.enabled || !view.msg) {
      return false;
    }

    double cx = wx;
    double cy = wy;
    if (!view.same_frame) {
      const double cos_yaw = std::cos(view.frame_yaw);
      const double sin_yaw = std::sin(view.frame_yaw);
      cx = view.frame_tx + cos_yaw * wx - sin_yaw * wy;
      cy = view.frame_ty + sin_yaw * wx + cos_yaw * wy;
    }

    const double dx = cx - view.msg->info.origin.position.x;
    const double dy = cy - view.msg->info.origin.position.y;
    const double cos_origin = std::cos(view.origin_yaw);
    const double sin_origin = std::sin(view.origin_yaw);
    const double mx = cos_origin * dx + sin_origin * dy;
    const double my = -sin_origin * dx + cos_origin * dy;
    const int gx = static_cast<int>(std::floor(mx / view.msg->info.resolution));
    const int gy = static_cast<int>(std::floor(my / view.msg->info.resolution));
    if (gx < 0 || gy < 0 ||
      gx >= static_cast<int>(view.msg->info.width) ||
      gy >= static_cast<int>(view.msg->info.height))
    {
      return outside_costmap_is_obstacle_;
    }

    const auto idx = static_cast<size_t>(gy * static_cast<int>(view.msg->info.width) + gx);
    if (idx >= view.msg->data.size()) {
      return outside_costmap_is_obstacle_;
    }

    const int value = view.msg->data[idx];
    if (value < 0) {
      return unknown_is_obstacle_;
    }
    return value >= costmap_occupied_threshold_;
  }

  SearchResult search(
    const Pose2D & start,
    const Pose2D & goal,
    const std::vector<bool> & occupied,
    const std::chrono::steady_clock::time_point & search_start) const
  {
    SearchResult result;
    std::priority_queue<QueueItem> open_heap;
    const int state_count = width_ * height_ * yaw_bins_ * 2;
    std::vector<double> best_cost(
      static_cast<size_t>(state_count), std::numeric_limits<double>::infinity());
    std::vector<SearchNode> nodes;
    nodes.reserve(4096);
    auto last_progress_log = search_start;

    Cell start_cell;
    worldToGrid(start.x, start.y, start_cell, true);
    Cell goal_cell;
    worldToGrid(goal.x, goal.y, goal_cell, true);
    const std::vector<double> grid_heuristic =
      use_grid_heuristic_ ? buildGridHeuristic(goal_cell, occupied) : std::vector<double>{};
    const int start_yaw_bin = yawToBin(start.yaw);
    for (const int gear : {1, -1}) {
      SearchNode node;
      node.state = SearchState{start_cell.x, start_cell.y, start_yaw_bin, gear};
      node.x = start.x;
      node.y = start.y;
      node.yaw = normalizeAngle(start.yaw);
      node.gear = gear;
      node.g = 0.0;
      const int node_index = static_cast<int>(nodes.size());
      nodes.push_back(node);
      const int state_idx = stateIndex(node.state);
      best_cost[state_idx] = 0.0;
      open_heap.push(QueueItem{
        heuristic_weight_ * heuristic(node, goal, grid_heuristic), node_index});
    }

    while (!open_heap.empty() && result.iterations < max_iterations_) {
      if (timedOut(search_start)) {
        result.timed_out = true;
        return result;
      }

      const int current_index = open_heap.top().node_index;
      open_heap.pop();
      const SearchNode current = nodes[static_cast<size_t>(current_index)];
      const int current_state_idx = stateIndex(current.state);
      if (current.g > best_cost[current_state_idx] + 1e-9) {
        continue;
      }

      ++result.iterations;
      if (search_progress_log_period_ms_ > 0.0) {
        const auto now_time = std::chrono::steady_clock::now();
        const double progress_elapsed_ms =
          std::chrono::duration<double, std::milli>(now_time - last_progress_log).count();
        if (progress_elapsed_ms >= search_progress_log_period_ms_) {
          last_progress_log = now_time;
          const double total_elapsed_ms =
            std::chrono::duration<double, std::milli>(now_time - search_start).count();
          RCLCPP_INFO(
            get_logger(),
            "search_progress iterations=%d open=%zu explored=%zu current=(%.3f,%.3f,%.3f) "
            "goal_distance=%.3f elapsed_ms=%.1f",
            result.iterations, open_heap.size(), nodes.size(), current.x, current.y, current.yaw,
            std::hypot(goal.x - current.x, goal.y - current.y), total_elapsed_ms);
        }
      }
      if (goalReached(current, goal)) {
        result.reached = true;
        result.path = reconstructPath(nodes, current_index, start);
        return result;
      }

      for (const int gear : {1, -1}) {
        for (const int steering : {0, -1, 1}) {
          SearchNode next;
          if (!propagate(current, gear, steering, occupied, next)) {
            continue;
          }

          next.parent = current_index;
          next.g = current.g + transitionCost(current, gear, steering);
          const int next_state_idx = stateIndex(next.state);
          if (next.g + 1e-9 >= best_cost[next_state_idx]) {
            continue;
          }

          best_cost[next_state_idx] = next.g;
          const int next_index = static_cast<int>(nodes.size());
          nodes.push_back(std::move(next));
          open_heap.push(QueueItem{
            nodes[static_cast<size_t>(next_index)].g +
              heuristic_weight_ *
                heuristic(nodes[static_cast<size_t>(next_index)], goal, grid_heuristic),
            next_index});
        }
      }
    }

    return result;
  }

  std::vector<double> buildGridHeuristic(
    const Cell & goal_cell,
    const std::vector<bool> & occupied) const
  {
    std::vector<double> cost(
      static_cast<size_t>(width_ * height_), std::numeric_limits<double>::infinity());
    if (!inBounds(goal_cell) || occupied[static_cast<size_t>(index(goal_cell))]) {
      return cost;
    }

    std::priority_queue<GridQueueItem> open;
    const int goal_index = index(goal_cell);
    cost[static_cast<size_t>(goal_index)] = 0.0;
    open.push(GridQueueItem{0.0, goal_index});

    constexpr int kNeighborCount = 8;
    const int dx[kNeighborCount] = {1, -1, 0, 0, 1, 1, -1, -1};
    const int dy[kNeighborCount] = {0, 0, 1, -1, 1, -1, 1, -1};

    while (!open.empty()) {
      const GridQueueItem current = open.top();
      open.pop();
      if (current.cost > cost[static_cast<size_t>(current.cell_index)] + 1e-9) {
        continue;
      }

      const Cell cell{current.cell_index % width_, current.cell_index / width_};
      for (int i = 0; i < kNeighborCount; ++i) {
        const Cell next{cell.x + dx[i], cell.y + dy[i]};
        if (!inBounds(next) || occupied[static_cast<size_t>(index(next))]) {
          continue;
        }
        const double step_cost =
          resolution_ * ((dx[i] == 0 || dy[i] == 0) ? 1.0 : std::sqrt(2.0));
        const int next_index = index(next);
        const double new_cost = current.cost + step_cost;
        if (new_cost + 1e-9 >= cost[static_cast<size_t>(next_index)]) {
          continue;
        }
        cost[static_cast<size_t>(next_index)] = new_cost;
        open.push(GridQueueItem{new_cost, next_index});
      }
    }

    return cost;
  }

  bool propagate(
    const SearchNode & current,
    int gear,
    int steering,
    const std::vector<bool> & occupied,
    SearchNode & next) const
  {
    const int sample_count =
      std::max(1, static_cast<int>(std::ceil(primitive_length_ / collision_sample_step_)));
    next.segment.clear();
    next.segment.reserve(static_cast<size_t>(sample_count));

    for (int sample = 1; sample <= sample_count; ++sample) {
      const double distance =
        primitive_length_ * static_cast<double>(sample) / static_cast<double>(sample_count);
      const Pose2D pose = integrate(current.x, current.y, current.yaw, gear, steering, distance);
      if (!poseCollisionFree(pose, occupied)) {
        return false;
      }
      next.segment.push_back(pose);
    }

    const Pose2D & final_pose = next.segment.back();
    Cell cell;
    if (!worldToGrid(final_pose.x, final_pose.y, cell)) {
      return false;
    }

    next.x = final_pose.x;
    next.y = final_pose.y;
    next.yaw = normalizeAngle(final_pose.yaw);
    next.gear = gear;
    next.steering = steering;
    next.state = SearchState{cell.x, cell.y, yawToBin(final_pose.yaw), gear};
    return true;
  }

  Pose2D integrate(
    double x,
    double y,
    double yaw,
    int gear,
    int steering,
    double distance) const
  {
    Pose2D pose;
    if (steering == 0) {
      pose.x = x + static_cast<double>(gear) * distance * std::cos(yaw);
      pose.y = y + static_cast<double>(gear) * distance * std::sin(yaw);
      pose.yaw = normalizeAngle(yaw);
      return pose;
    }

    const double curvature = static_cast<double>(steering) / min_turning_radius_;
    const double new_yaw = normalizeAngle(yaw + static_cast<double>(gear) * curvature * distance);
    pose.x = x + (std::sin(new_yaw) - std::sin(yaw)) / curvature;
    pose.y = y + (std::cos(yaw) - std::cos(new_yaw)) / curvature;
    pose.yaw = new_yaw;
    return pose;
  }

  double transitionCost(
    const SearchNode & current,
    int gear,
    int steering) const
  {
    double cost = primitive_length_;
    if (gear < 0) {
      cost *= reverse_penalty_;
    }
    if (steering != 0) {
      cost += turn_penalty_;
    }
    if (gear != current.gear) {
      cost += gear_change_penalty_;
    }
    cost += steering_change_penalty_ * std::abs(steering - current.steering);
    return cost;
  }

  double heuristic(
    const SearchNode & node,
    const Pose2D & goal,
    const std::vector<double> & grid_heuristic) const
  {
    const double distance = std::hypot(goal.x - node.x, goal.y - node.y);
    double cost_to_go = distance;
    if (!grid_heuristic.empty()) {
      const Cell cell{node.state.x, node.state.y};
      if (inBounds(cell)) {
        const double grid_cost = grid_heuristic[static_cast<size_t>(index(cell))];
        if (std::isfinite(grid_cost)) {
          cost_to_go = std::max(cost_to_go, grid_cost);
        }
      }
    }
    if (!require_goal_yaw_) {
      return cost_to_go;
    }
    const double yaw_error = std::abs(normalizeAngle(goal.yaw - node.yaw));
    return cost_to_go + goal_yaw_heuristic_weight_ * min_turning_radius_ * yaw_error;
  }

  bool goalReached(const SearchNode & node, const Pose2D & goal) const
  {
    if (std::hypot(node.x - goal.x, node.y - goal.y) > goal_xy_tolerance_) {
      return false;
    }
    return !require_goal_yaw_ ||
          std::abs(normalizeAngle(node.yaw - goal.yaw)) <= goal_yaw_tolerance_;
  }

  std::vector<Pose2D> reconstructPath(
    const std::vector<SearchNode> & nodes,
    int goal_index,
    const Pose2D & start) const
  {
    std::vector<int> chain;
    for (int index = goal_index; index >= 0; index = nodes[static_cast<size_t>(index)].parent) {
      chain.push_back(index);
      if (nodes[static_cast<size_t>(index)].parent < 0) {
        break;
      }
    }
    std::reverse(chain.begin(), chain.end());

    std::vector<Pose2D> path;
    path.push_back(Pose2D{start.x, start.y, normalizeAngle(start.yaw)});
    for (size_t i = 1; i < chain.size(); ++i) {
      const auto & segment = nodes[static_cast<size_t>(chain[i])].segment;
      path.insert(path.end(), segment.begin(), segment.end());
    }
    return path;
  }

  bool timedOut(const std::chrono::steady_clock::time_point & start_time) const
  {
    const auto elapsed_ms =
      std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - start_time)
        .count();
    return elapsed_ms >= planning_timeout_ms_;
  }

  bool poseCollisionFree(const Pose2D & pose, const std::vector<bool> & occupied) const
  {
    Cell cell;
    if (!worldToGrid(pose.x, pose.y, cell)) {
      return false;
    }
    return !occupied[static_cast<size_t>(index(cell))];
  }

  bool pathCollisionFree(const std::vector<Pose2D> & path, const std::vector<bool> & occupied) const
  {
    return std::all_of(path.begin(), path.end(), [this, &occupied](const Pose2D & pose) {
      return poseCollisionFree(pose, occupied);
    });
  }

  void publishPath(const std::vector<Pose2D> & points)
  {
    nav_msgs::msg::Path path;
    path.header.frame_id = map_frame_;
    path.header.stamp = toMsgTime(now());

    for (const auto & point : points) {
      path.poses.push_back(makePose(map_frame_, path.header.stamp, point.x, point.y, point.yaw));
    }
    plan_pub_->publish(path);
  }

  void publishEmptyPath()
  {
    nav_msgs::msg::Path path;
    path.header.frame_id = map_frame_;
    path.header.stamp = toMsgTime(now());
    plan_pub_->publish(path);
  }

  bool currentPose(Pose2D & pose)
  {
    if (!latest_odom_) {
      return false;
    }

    if (latest_odom_->header.stamp.sec != 0 || latest_odom_->header.stamp.nanosec != 0) {
      const double age = (now() - rclcpp::Time(latest_odom_->header.stamp)).seconds();
      if (age > max_odom_age_) {
        return false;
      }
    }

    pose.x = latest_odom_->pose.pose.position.x;
    pose.y = latest_odom_->pose.pose.position.y;
    pose.yaw = yawFromQuaternion(latest_odom_->pose.pose.orientation);
    const std::string source_frame =
      latest_odom_->header.frame_id.empty() ? map_frame_ : latest_odom_->header.frame_id;
    if (source_frame != map_frame_) {
      return transformPose2D(source_frame, pose);
    }
    return true;
  }

  bool transformPoseMsg(const geometry_msgs::msg::PoseStamped & msg, Pose2D & pose)
  {
    pose.x = msg.pose.position.x;
    pose.y = msg.pose.position.y;
    pose.yaw = yawFromQuaternion(msg.pose.orientation);
    const std::string source_frame = msg.header.frame_id.empty() ? map_frame_ : msg.header.frame_id;
    if (source_frame != map_frame_) {
      return transformPose2D(source_frame, pose);
    }
    return true;
  }

  bool transformPose2D(const std::string & source_frame, Pose2D & pose)
  {
    try {
      const auto transform = tf_buffer_.lookupTransform(
        map_frame_, source_frame, tf2::TimePointZero,
        tf2::durationFromSec(tf_timeout_));
      const auto & t = transform.transform.translation;
      const double transform_yaw = yawFromQuaternion(transform.transform.rotation);
      const double cos_yaw = std::cos(transform_yaw);
      const double sin_yaw = std::sin(transform_yaw);
      const double x = pose.x;
      const double y = pose.y;
      pose.x = t.x + cos_yaw * x - sin_yaw * y;
      pose.y = t.y + sin_yaw * x + cos_yaw * y;
      pose.yaw = normalizeAngle(transform_yaw + pose.yaw);
      return true;
    } catch (const tf2::TransformException & ex) {
      const auto now_ns = now().nanoseconds();
      if (now_ns - last_tf_warn_ns_ > 2000000000LL) {
        last_tf_warn_ns_ = now_ns;
        RCLCPP_WARN(
          get_logger(), "TF unavailable %s->%s: %s",
          source_frame.c_str(), map_frame_.c_str(), ex.what());
      }
      return false;
    }
  }

  bool worldToGrid(double x, double y, Cell & cell, bool clamp = false) const
  {
    int gx = static_cast<int>(std::floor((x - map_min_x_) / resolution_));
    int gy = static_cast<int>(std::floor((y - map_min_y_) / resolution_));
    if (clamp) {
      gx = std::min(width_ - 1, std::max(0, gx));
      gy = std::min(height_ - 1, std::max(0, gy));
      cell = Cell{gx, gy};
      return true;
    }
    cell = Cell{gx, gy};
    return inBounds(cell);
  }

  std::pair<double, double> gridToWorld(const Cell & cell) const
  {
    return {
      map_min_x_ + (static_cast<double>(cell.x) + 0.5) * resolution_,
      map_min_y_ + (static_cast<double>(cell.y) + 0.5) * resolution_};
  }

  bool inBounds(const Cell & cell) const
  {
    return cell.x >= 0 && cell.x < width_ && cell.y >= 0 && cell.y < height_;
  }

  int index(const Cell & cell) const
  {
    return cell.y * width_ + cell.x;
  }

  int yawToBin(double yaw) const
  {
    const double bin_size = 2.0 * kPi / static_cast<double>(yaw_bins_);
    return static_cast<int>(std::floor((positiveAngle(yaw) + 0.5 * bin_size) / bin_size)) %
          yaw_bins_;
  }

  int stateIndex(const SearchState & state) const
  {
    const int gear_index = state.gear > 0 ? 1 : 0;
    return ((((state.y * width_) + state.x) * yaw_bins_ + state.yaw_bin) * 2) + gear_index;
  }

  void logRuntimeState()
  {
    std::ostringstream stream;
    stream.precision(3);
    stream << std::fixed;
    stream << "planner_state ";

    Pose2D pose;
    const bool have_pose = currentPose(pose);
    if (have_pose) {
      stream << "pose=(" << pose.x << "," << pose.y << "," << pose.yaw << ")";
    } else {
      stream << "pose=unavailable";
      if (!latest_odom_) {
        stream << " odom=none";
      } else if (latest_odom_->header.stamp.sec != 0 || latest_odom_->header.stamp.nanosec != 0) {
        const double age = std::max(0.0, (now() - rclcpp::Time(latest_odom_->header.stamp)).seconds());
        stream << " odom_age=" << age;
      }
    }

    if (has_goal_) {
      stream << " goal=(" << goal_pose_.x << "," << goal_pose_.y << "," << goal_pose_.yaw << ")";
      if (have_pose) {
        stream << " goal_distance=" << std::hypot(goal_pose_.x - pose.x, goal_pose_.y - pose.y);
        stream << " yaw_error=" << std::abs(normalizeAngle(goal_pose_.yaw - pose.yaw));
      }
    } else {
      stream << " goal=none";
    }

    stream << " path_points=" << last_path_.size();
    stream << " obstacles=" << obstacles_.size();
    stream << " pending_replan=" << (pending_replan_ ? "true" : "false");
    stream << " map_bounds=(" << map_min_x_ << "," << map_min_y_ << ")-(" <<
      map_max_x_ << "," << map_max_y_ << ")";
    stream << " grid=" << width_ << "x" << height_ << "@" << resolution_;
    if (latest_costmap_) {
      stream << " costmap=" << latest_costmap_->info.width << "x" <<
        latest_costmap_->info.height << "@" << latest_costmap_->info.resolution;
    } else {
      stream << " costmap=none";
    }

    RCLCPP_INFO(get_logger(), "%s", stream.str().c_str());
  }

  void publishStatus(const std::string & text)
  {
    std_msgs::msg::String msg;
    msg.data = text;
    status_pub_->publish(msg);
    RCLCPP_INFO(get_logger(), "%s", text.c_str());
  }

  static std::string formatDouble(double value)
  {
    std::ostringstream stream;
    stream.precision(3);
    stream << std::fixed << value;
    return stream.str();
  }

  std::string map_frame_;
  std::string odom_topic_;
  std::string goal_pose_topic_;
  std::string obstacles_topic_;
  std::string clear_obstacles_topic_;
  std::string cancel_topic_;
  std::string costmap_topic_;
  std::string plan_topic_;
  std::string status_topic_;
  std::string obstacle_format_;
  double map_min_x_{-5.0};
  double map_min_y_{-5.0};
  double map_max_x_{5.0};
  double map_max_y_{5.0};
  double resolution_{0.1};
  bool fit_map_to_costmap_{true};
  double default_obstacle_radius_{0.15};
  double robot_radius_{0.14};
  double localization_error_{0.10};
  double latency_margin_{0.05};
  double safety_margin_{0.05};
  double unknown_obstacle_radius_{0.15};
  bool accumulate_obstacles_{true};
  double obstacle_merge_distance_{0.20};
  int costmap_occupied_threshold_{50};
  bool unknown_is_obstacle_{true};
  bool outside_costmap_is_obstacle_{true};
  bool inflate_static_map_{false};
  bool plan_on_obstacle_update_{true};
  int max_iterations_{50000};
  double planning_timeout_ms_{100.0};
  double replan_rate_{2.0};
  double min_turning_radius_{0.40};
  int yaw_bins_{72};
  double primitive_length_{0.12};
  double collision_sample_step_{0.03};
  double goal_xy_tolerance_{0.20};
  double goal_yaw_tolerance_{3.0 * kPi / 180.0};
  bool require_goal_yaw_{true};
  bool use_grid_heuristic_{true};
  double heuristic_weight_{1.0};
  double goal_yaw_heuristic_weight_{1.0};
  double reverse_penalty_{1.3};
  double turn_penalty_{0.05};
  double gear_change_penalty_{0.20};
  double steering_change_penalty_{0.02};
  double max_odom_age_{0.80};
  double tf_timeout_{0.05};
  double state_log_period_{1.0};
  double search_progress_log_period_ms_{500.0};
  int width_{0};
  int height_{0};
  int64_t last_tf_warn_ns_{0};
  bool has_goal_{false};
  bool pending_replan_{false};
  Pose2D goal_pose_;
  std::vector<Obstacle> obstacles_;
  std::vector<Pose2D> last_path_;
  nav_msgs::msg::Odometry::SharedPtr latest_odom_;
  nav_msgs::msg::OccupancyGrid::SharedPtr latest_costmap_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr plan_pub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr status_pub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
  rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr obstacles_sub_;
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr clear_obstacles_sub_;
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr cancel_sub_;
  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_sub_;
  rclcpp::TimerBase::SharedPtr replan_timer_;
  rclcpp::TimerBase::SharedPtr state_log_timer_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<GridAstarThetaPlanner>());
  rclcpp::shutdown();
  return 0;
}
