smac_planner_hybrid_impl.hpp

// Copyright (c) 2020, Samsung Research America
// 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. Reserved.
 
#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_IMPL_HPP_
#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_IMPL_HPP_
 
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
 
#include "nav2_smac_planner/smac_planner_hybrid.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
 
// #define BENCHMARK_TESTING
 
namespace nav2_smac_planner
{
 
using namespace std::chrono;  // NOLINT
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
 
template<typename NodeT>
SmacPlannerHybridT<NodeT>::SmacPlannerHybridT()
: _a_star(nullptr),
  _collision_checker(nullptr, 1, nullptr),
  _smoother(nullptr),
  _costmap(nullptr),
  _costmap_ros(nullptr),
  _costmap_downsampler(nullptr)
{
}
 
template<typename NodeT>
SmacPlannerHybridT<NodeT>::~SmacPlannerHybridT()
{
  RCLCPP_INFO(
    _logger, "Destroying plugin %s of type SmacPlannerHybrid",
    _name.c_str());
}
 
template<typename NodeT>
void SmacPlannerHybridT<NodeT>::configure(
  const nav2::LifecycleNode::WeakPtr & parent,
  std::string name, nav2::TransformBuffer::SharedPtr/*tf*/,
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
  _node = parent;
  auto node = parent.lock();
  _logger = node->get_logger();
  _clock = node->get_clock();
  _costmap = costmap_ros->getCostmap();
  _costmap_ros = costmap_ros;
  _name = name;
  _global_frame = costmap_ros->getGlobalFrameID();
 
  RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", name.c_str());
 
  int angle_quantizations;
  bool smooth_path;
 
  // General planner params
  _downsample_costmap = node->declare_or_get_parameter(name + ".downsample_costmap", false);
  _downsampling_factor = node->declare_or_get_parameter(name + ".downsampling_factor", 1);
 
  angle_quantizations = node->declare_or_get_parameter(name + ".angle_quantization_bins", 72);
  _angle_bin_size = 2.0 * M_PI / angle_quantizations;
  _angle_quantizations = static_cast<unsigned int>(angle_quantizations);
 
  _tolerance = static_cast<float>(node->declare_or_get_parameter(name + ".tolerance", 0.25));
  _allow_unknown = node->declare_or_get_parameter(name + ".allow_unknown", true);
  _max_iterations = node->declare_or_get_parameter(name + ".max_iterations", 1000000);
  _max_on_approach_iterations =
    node->declare_or_get_parameter(name + ".max_on_approach_iterations", 1000);
  _terminal_checking_interval =
    node->declare_or_get_parameter(name + ".terminal_checking_interval", 5000);
  smooth_path = node->declare_or_get_parameter(name + ".smooth_path", true);
 
  _minimum_turning_radius_global_coords =
    node->declare_or_get_parameter(name + ".minimum_turning_radius", 0.4);
  _search_info.allow_primitive_interpolation =
    node->declare_or_get_parameter(name + ".allow_primitive_interpolation", true);
  _search_info.cache_obstacle_heuristic =
    node->declare_or_get_parameter(name + ".cache_obstacle_heuristic", false);
  _search_info.reverse_penalty =
    node->declare_or_get_parameter(name + ".reverse_penalty", 2.0);
  _search_info.change_penalty =
    node->declare_or_get_parameter(name + ".change_penalty", 0.0);
  _search_info.non_straight_penalty =
    node->declare_or_get_parameter(name + ".non_straight_penalty", 1.2);
  _search_info.cost_penalty =
    node->declare_or_get_parameter(name + ".cost_penalty", 2.0);
  _search_info.retrospective_penalty =
    node->declare_or_get_parameter(name + ".retrospective_penalty", 0.015);
  _search_info.analytic_expansion_ratio =
    node->declare_or_get_parameter(name + ".analytic_expansion_ratio", 3.5);
  _search_info.analytic_expansion_max_cost =
    node->declare_or_get_parameter(name + ".analytic_expansion_max_cost", 200.0);
  _search_info.analytic_expansion_max_cost_override =
    node->declare_or_get_parameter(name + ".analytic_expansion_max_cost_override", false);
  _search_info.use_quadratic_cost_penalty =
    node->declare_or_get_parameter(name + ".use_quadratic_cost_penalty", false);
  _search_info.downsample_obstacle_heuristic =
    node->declare_or_get_parameter(name + ".downsample_obstacle_heuristic", true);
 
  _analytic_expansion_max_length_m =
    node->declare_or_get_parameter(name + ".analytic_expansion_max_length", 3.0);
 
  _max_planning_time = node->declare_or_get_parameter(name + ".max_planning_time", 5.0);
  _lookup_table_size = node->declare_or_get_parameter(name + ".lookup_table_size", 20.0);
 
  _debug_visualizations = node->declare_or_get_parameter(name + ".debug_visualizations", false);
 
  _motion_model_for_search =
    node->declare_or_get_parameter(name + ".motion_model_for_search", std::string("DUBIN"));
 
  std::string goal_heading_type =
    node->declare_or_get_parameter(name + ".goal_heading_mode", std::string("DEFAULT"));
  _goal_heading_mode = fromStringToGH(goal_heading_type);
 
  _coarse_search_resolution =
    node->declare_or_get_parameter(name + ".coarse_search_resolution", 1);
 
  if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) {
    std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' "
      "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ";
    throw nav2_core::PlannerException(error_msg);
  }
 
  _motion_model = fromString(_motion_model_for_search);
 
  if (_motion_model == MotionModel::UNKNOWN) {
    RCLCPP_WARN(
      _logger,
      "Unable to get MotionModel search type. Given '%s', "
      "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.",
      _motion_model_for_search.c_str());
  }
 
  if (_max_on_approach_iterations <= 0) {
    RCLCPP_WARN(
      _logger, "On approach iteration selected as <= 0, "
      "disabling tolerance and on approach iterations.");
    _max_on_approach_iterations = std::numeric_limits<int>::max();
  }
 
  if (_max_iterations <= 0) {
    RCLCPP_WARN(
      _logger, "maximum iteration selected as <= 0, "
      "disabling maximum iterations.");
    _max_iterations = std::numeric_limits<int>::max();
  }
 
  if (_coarse_search_resolution <= 0) {
    RCLCPP_WARN(
      _logger, "coarse iteration resolution selected as <= 0, "
      "disabling coarse iteration resolution search for goal heading"
    );
 
    _coarse_search_resolution = 1;
  }
 
  if (angle_quantizations <= 0 || angle_quantizations > 1024) {
    std::string error_msg = "angle_quantization_bins must be in [1, 1024]";
    throw nav2_core::PlannerException(error_msg);
  }
 
  if (_angle_quantizations % _coarse_search_resolution != 0) {
    std::string error_msg = "coarse iteration should be an increment"
      " of the number of angular bins configured";
    throw nav2_core::PlannerException(error_msg);
  }
 
  if (_analytic_expansion_max_length_m < 0.0 ||
    _analytic_expansion_max_length_m > 1000.0)
  {
    std::string error_msg = "analytic_expansion_max_length must be in [0.0, 1000.0]";
    throw nav2_core::PlannerException(error_msg);
  }
 
  if (_lookup_table_size < 0.0 || _lookup_table_size > 20.0) {
    std::string error_msg = "lookup_table_size must be in [0.0, 20.0]";
    throw nav2_core::PlannerException(error_msg);
  }
 
  if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) {
    RCLCPP_WARN(
      _logger, "Min turning radius cannot be less than the search grid cell resolution!");
    _minimum_turning_radius_global_coords = _costmap->getResolution() * _downsampling_factor;
  }
 
  // convert to grid coordinates
  if (!_downsample_costmap) {
    _downsampling_factor = 1;
  }
  updateSearchResolution(_costmap->getResolution() * _downsampling_factor);
 
  // Initialize collision checker
  _collision_checker = GridCollisionChecker(_costmap_ros, _angle_quantizations, node);
  _collision_checker.setFootprint(
    _costmap_ros->getRobotFootprint(),
    _costmap_ros->getUseRadius(),
    findCircumscribedCost(_costmap_ros));
 
  // Initialize A* template
  _a_star = std::make_unique<AStarAlgorithm<NodeT>>(_motion_model, _search_info);
  _a_star->initialize(
    _allow_unknown,
    _max_iterations,
    _max_on_approach_iterations,
    _terminal_checking_interval,
    _max_planning_time,
    _lookup_table_dim,
    _angle_quantizations);
 
  // Initialize path smoother
  SmootherParams params;
  params.get(node, name);
  if (smooth_path) {
    _smoother = std::make_unique<Smoother>(params);
    _smoother->initialize(_minimum_turning_radius_global_coords);
  }
 
  // Initialize costmap downsampler
  _costmap_downsampler = std::make_unique<CostmapDownsampler>();
  std::string topic_name = "downsampled_costmap";
  _costmap_downsampler->on_configure(
    node, _global_frame, topic_name, _costmap, _downsampling_factor);
 
  _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan");
 
  if (_debug_visualizations) {
    _expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions");
    _planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
      "planned_footprints");
    _smoothed_footprints_publisher =
      node->create_publisher<visualization_msgs::msg::MarkerArray>(
      "smoothed_footprints");
  }
 
  RCLCPP_INFO(
    _logger, "Configured plugin %s of type SmacPlannerHybrid with "
    "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f."
    "Using motion model: %s.",
    _name.c_str(), _max_iterations, _max_on_approach_iterations,
    _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
    _tolerance, toString(_motion_model).c_str());
}
 
template<typename NodeT>
void SmacPlannerHybridT<NodeT>::activate()
{
  RCLCPP_INFO(
    _logger, "Activating plugin %s of type SmacPlannerHybrid",
    _name.c_str());
  _raw_plan_publisher->on_activate();
  if (_debug_visualizations) {
    _expansions_publisher->on_activate();
    _planned_footprints_publisher->on_activate();
    _smoothed_footprints_publisher->on_activate();
  }
  if (_costmap_downsampler) {
    _costmap_downsampler->on_activate();
  }
  auto node = _node.lock();
  // Add callback for dynamic parameters
  _post_set_params_handler = node->add_post_set_parameters_callback(
    std::bind(
      &SmacPlannerHybridT<NodeT>::updateParametersCallback,
      this, std::placeholders::_1));
  _on_set_params_handler = node->add_on_set_parameters_callback(
    std::bind(
      &SmacPlannerHybridT<NodeT>::validateParameterUpdatesCallback,
      this, std::placeholders::_1));
}
 
template<typename NodeT>
void SmacPlannerHybridT<NodeT>::deactivate()
{
  RCLCPP_INFO(
    _logger, "Deactivating plugin %s of type SmacPlannerHybrid",
    _name.c_str());
  _raw_plan_publisher->on_deactivate();
  if (_debug_visualizations) {
    _expansions_publisher->on_deactivate();
    _planned_footprints_publisher->on_deactivate();
    _smoothed_footprints_publisher->on_deactivate();
  }
  if (_costmap_downsampler) {
    _costmap_downsampler->on_deactivate();
  }
  // shutdown dyn_param_handler
  auto node = _node.lock();
  if (_post_set_params_handler && node) {
    node->remove_post_set_parameters_callback(_post_set_params_handler.get());
  }
  _post_set_params_handler.reset();
  if (_on_set_params_handler && node) {
    node->remove_on_set_parameters_callback(_on_set_params_handler.get());
  }
  _on_set_params_handler.reset();
}
 
template<typename NodeT>
void SmacPlannerHybridT<NodeT>::cleanup()
{
  RCLCPP_INFO(
    _logger, "Cleaning up plugin %s of type SmacPlannerHybrid",
    _name.c_str());
  _a_star.reset();
  _smoother.reset();
  if (_costmap_downsampler) {
    _costmap_downsampler->on_cleanup();
    _costmap_downsampler.reset();
  }
  _raw_plan_publisher.reset();
  if (_debug_visualizations) {
    _expansions_publisher.reset();
    _planned_footprints_publisher.reset();
    _smoothed_footprints_publisher.reset();
  }
}
 
template<typename NodeT>
void SmacPlannerHybridT<NodeT>::updateSearchResolution(double resolution)
{
  _search_resolution = resolution;
  _search_info.minimum_turning_radius =
    std::max(_minimum_turning_radius_global_coords / resolution, 1.0);
  _search_info.analytic_expansion_max_length = _analytic_expansion_max_length_m / resolution;
  _lookup_table_dim =
    static_cast<float>(_lookup_table_size) /
    static_cast<float>(resolution);
 
  // Make sure its a whole number
  _lookup_table_dim = static_cast<float>(static_cast<int>(_lookup_table_dim));
 
  // Make sure its an odd number
  if (static_cast<int>(_lookup_table_dim) % 2 == 0) {
    RCLCPP_INFO(
      _logger,
      "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
      _lookup_table_dim);
    _lookup_table_dim += 1.0;
  }
}
 
template<typename NodeT>
nav_msgs::msg::Path SmacPlannerHybridT<NodeT>::createPlan(
  const geometry_msgs::msg::PoseStamped & start,
  const geometry_msgs::msg::PoseStamped & goal,
  const std::vector<geometry_msgs::msg::PoseStamped> & viapoints,
  std::function<bool()> cancel_checker)
{
  if (!viapoints.empty()) {
    RCLCPP_WARN(_logger, "Received %zu viapoints, but this planner ignores them",
      viapoints.size());
  }
 
  std::lock_guard<std::mutex> lock_reinit(_mutex);
  steady_clock::time_point a = steady_clock::now();
 
  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
 
  // Downsample costmap, if required
  nav2_costmap_2d::Costmap2D * costmap = _costmap;
  if (_downsample_costmap && _downsampling_factor > 1) {
    costmap = _costmap_downsampler->downsample(_downsampling_factor);
  }
 
  // The resolution can change, even if the parameter is not updated
  // (e.g. if the map is provided with a different resolution). Therefore, we
  // need to check the costmap resolution and update the search resolution if it has changed.
  if (_search_resolution != costmap->getResolution()) {
    updateSearchResolution(costmap->getResolution());
    _a_star = std::make_unique<AStarAlgorithm<NodeT>>(_motion_model, _search_info);
    _a_star->initialize(
      _allow_unknown, _max_iterations, _max_on_approach_iterations,
      _terminal_checking_interval, _max_planning_time, _lookup_table_dim,
      _angle_quantizations);
  }
  _collision_checker.setCostmap(costmap);
 
  // Set collision checker and costmap information
  _collision_checker.setFootprint(
    _costmap_ros->getRobotFootprint(),
    _costmap_ros->getUseRadius(),
    findCircumscribedCost(_costmap_ros));
  _a_star->setCollisionChecker(&_collision_checker);
 
  // Set starting point, in A* bin search coordinates
  float mx_start, my_start, mx_goal, my_goal;
  if (!costmap->worldToMapContinuous(
      start.pose.position.x,
      start.pose.position.y,
      mx_start,
      my_start))
  {
    throw nav2_core::StartOutsideMapBounds(
            "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
            std::to_string(start.pose.position.y) + ") was outside bounds");
  }
 
  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 (start_orientation_bin >= static_cast<float>(_angle_quantizations)) {
    start_orientation_bin -= static_cast<float>(_angle_quantizations);
  }
  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(
      goal.pose.position.x,
      goal.pose.position.y,
      mx_goal,
      my_goal))
  {
    throw nav2_core::GoalOutsideMapBounds(
            "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
            std::to_string(goal.pose.position.y) + ") was outside bounds");
  }
  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 (goal_orientation_bin >= static_cast<float>(_angle_quantizations)) {
    goal_orientation_bin -= static_cast<float>(_angle_quantizations);
  }
  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
  nav_msgs::msg::Path plan;
  plan.header.stamp = _clock->now();
  plan.header.frame_id = _global_frame;
  geometry_msgs::msg::PoseStamped pose;
  pose.header = plan.header;
  pose.pose.position.z = 0.0;
  pose.pose.orientation.x = 0.0;
  pose.pose.orientation.y = 0.0;
  pose.pose.orientation.z = 0.0;
  pose.pose.orientation.w = 1.0;
 
  // 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) &&
    start_orientation_bin_int == goal_orientation_bin_int)
  {
    pose.pose = start.pose;
    pose.pose.orientation = goal.pose.orientation;
    plan.poses.push_back(pose);
 
    // Publish raw path for debug
    if (_raw_plan_publisher->get_subscription_count() > 0) {
      auto msg = std::make_unique<nav_msgs::msg::Path>(plan);
      _raw_plan_publisher->publish(std::move(msg));
    }
 
    return plan;
  }
 
  // Compute plan
  typename NodeT::CoordinateVector path;
  int num_iterations = 0;
  std::string error;
  std::unique_ptr<std::vector<std::tuple<float, float, float>>> expansions = nullptr;
  if (_debug_visualizations) {
    expansions = std::make_unique<std::vector<std::tuple<float, float, float>>>();
  }
  // Note: All exceptions thrown are handled by the planner server and returned to the action
  if (!_a_star->createPath(
      path, num_iterations,
      _tolerance / static_cast<float>(costmap->getResolution()), cancel_checker, expansions.get()))
  {
    if (_debug_visualizations) {
      auto msg = std::make_unique<geometry_msgs::msg::PoseArray>();
      geometry_msgs::msg::Pose msg_pose;
      msg->header.stamp = _clock->now();
      msg->header.frame_id = _global_frame;
      for (auto & e : *expansions) {
        msg_pose.position.x = std::get<0>(e);
        msg_pose.position.y = std::get<1>(e);
        msg_pose.orientation = getWorldOrientation(std::get<2>(e));
        msg->poses.push_back(msg_pose);
      }
      _expansions_publisher->publish(std::move(msg));
    }
 
    // Note: If the start is blocked only one iteration will occur before failure
    if (num_iterations == 1) {
      throw nav2_core::StartOccupied("Start occupied");
    }
 
    if (num_iterations < _a_star->getMaxIterations()) {
      throw nav2_core::NoValidPathCouldBeFound("no valid path found");
    } else {
      throw nav2_core::PlannerTimedOut("exceeded maximum iterations");
    }
  }
 
  // Convert to world coordinates
  plan.poses.reserve(path.size());
  for (int i = path.size() - 1; i >= 0; --i) {
    pose.pose = getWorldCoords(path[i].x, path[i].y, costmap);
    pose.pose.orientation = getWorldOrientation(path[i].theta);
    plan.poses.push_back(pose);
  }
 
  // Publish raw path for debug
  if (_raw_plan_publisher->get_subscription_count() > 0) {
    auto msg = std::make_unique<nav_msgs::msg::Path>(plan);
    _raw_plan_publisher->publish(std::move(msg));
  }
 
  if (_debug_visualizations) {
    // Publish expansions for debug
    auto now = _clock->now();
    auto msg = std::make_unique<geometry_msgs::msg::PoseArray>();
    geometry_msgs::msg::Pose msg_pose;
    msg->header.stamp = now;
    msg->header.frame_id = _global_frame;
    for (auto & e : *expansions) {
      msg_pose.position.x = std::get<0>(e);
      msg_pose.position.y = std::get<1>(e);
      msg_pose.orientation = getWorldOrientation(std::get<2>(e));
      msg->poses.push_back(msg_pose);
    }
    _expansions_publisher->publish(std::move(msg));
 
    if (_planned_footprints_publisher->get_subscription_count() > 0) {
      // Clear all markers first
      auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
      visualization_msgs::msg::Marker clear_all_marker;
      clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
      marker_array->markers.push_back(clear_all_marker);
      _planned_footprints_publisher->publish(std::move(marker_array));
 
      // Publish smoothed footprints for debug
      marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
      for (size_t i = 0; i < plan.poses.size(); i++) {
        const std::vector<geometry_msgs::msg::Point> edge =
          transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
        marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
      }
      _planned_footprints_publisher->publish(std::move(marker_array));
    }
  }
 
  // Find how much time we have left to do smoothing
  steady_clock::time_point b = steady_clock::now();
  duration<double> time_span = duration_cast<duration<double>>(b - a);
  double time_remaining = _max_planning_time - static_cast<double>(time_span.count());
 
#ifdef BENCHMARK_TESTING
  std::cout << "It took " << time_span.count() * 1000 <<
    " milliseconds with " << num_iterations << " iterations." << std::endl;
#endif
 
  // Smooth plan
  if (_smoother && num_iterations > 1) {
    _smoother->smooth(
      plan,
      costmap,
      time_remaining,
      _costmap_ros->getUseRadius() ? std::vector<geometry_msgs::msg::Point>() :
      _costmap_ros->getRobotFootprint());
  }
 
#ifdef BENCHMARK_TESTING
  steady_clock::time_point c = steady_clock::now();
  duration<double> time_span2 = duration_cast<duration<double>>(c - b);
  std::cout << "It took " << time_span2.count() * 1000 <<
    " milliseconds to smooth path." << std::endl;
#endif
 
  if (_debug_visualizations) {
    if (_smoothed_footprints_publisher->get_subscription_count() > 0) {
      // Clear all markers first
      auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
      visualization_msgs::msg::Marker clear_all_marker;
      clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
      marker_array->markers.push_back(clear_all_marker);
      _smoothed_footprints_publisher->publish(std::move(marker_array));
 
      // Publish smoothed footprints for debug
      marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
      auto now = _clock->now();
      for (size_t i = 0; i < plan.poses.size(); i++) {
        const std::vector<geometry_msgs::msg::Point> edge =
          transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
        marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
      }
      _smoothed_footprints_publisher->publish(std::move(marker_array));
    }
  }
 
  return plan;
}
 
template<typename NodeT>
rcl_interfaces::msg::SetParametersResult
SmacPlannerHybridT<NodeT>::validateParameterUpdatesCallback(
  const std::vector<rclcpp::Parameter> & parameters)
{
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  for (const auto & parameter : parameters) {
    const auto & param_type = parameter.get_type();
    const auto & param_name = parameter.get_name();
    if (param_name.find(_name + ".") != 0) {
      continue;
    }
    if (param_type == ParameterType::PARAMETER_DOUBLE) {
      if (parameter.as_double() < 0.0) {
        RCLCPP_WARN(
        _logger, "The value of parameter '%s' is incorrectly set to %f, "
        "it should be >=0. Ignoring parameter update.",
        param_name.c_str(), parameter.as_double());
        result.successful = false;
      } else if (param_name == _name + ".lookup_table_size" && // NOLINT
        parameter.as_double() > 20.0)
      {
        RCLCPP_WARN(
          _logger, "The value of parameter lookup_table_size is incorrectly set to %f, "
          "it should be in [0.0, 20.0]. Ignoring parameter update.",
          parameter.as_double());
        result.successful = false;
      } else if (param_name == _name + ".analytic_expansion_max_length" && // NOLINT
        parameter.as_double() > 1000.0)
      {
        RCLCPP_WARN(
          _logger, "The value of parameter analytic_expansion_max_length is incorrectly set "
          "to %f, it should be <= 1000.0. Ignoring parameter update.",
          parameter.as_double());
        result.successful = false;
      } else if (param_name == _name + ".minimum_turning_radius" && // NOLINT
        parameter.as_double() < _costmap->getResolution() * _downsampling_factor)
      {
        RCLCPP_WARN(
          _logger, "The value of parameter minimum_turning_radius is incorrectly set to %f, "
          "it should be >= costmap resolution * downsampling factor (%f). "
          "Ignoring parameter update.",
          parameter.as_double(),
          _costmap->getResolution() * _downsampling_factor);
        result.successful = false;
      }
    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
      if (parameter.as_int() <= 0 && (param_name != _name + ".max_on_approach_iterations" && // NOLINT
        param_name != _name + ".max_iterations"))
      {
        RCLCPP_WARN(
        _logger, "The value of parameter '%s' is incorrectly set to %ld, "
        "it should be >0. Ignoring parameter update.",
        param_name.c_str(), parameter.as_int());
        result.successful = false;
      } else if (param_name == _name + ".angle_quantization_bins") {
        unsigned int angle_quantizations = static_cast<unsigned int>(parameter.as_int());
        if (angle_quantizations > 1024) {
          RCLCPP_WARN(
            _logger,
            "The value of parameter angle_quantization_bins is incorrectly set to %u, "
            "it should be <= 1024. Ignoring parameter update.",
            angle_quantizations);
          result.successful = false;
        } else if (angle_quantizations % _coarse_search_resolution != 0) {
          RCLCPP_WARN(
            _logger,
            "The value of parameter angle_quantization_bins is incorrectly set to %u, "
            "it should be an increment of the coarse_search_resolution (%u). "
            "Ignoring parameter update.",
            angle_quantizations,
            _coarse_search_resolution);
          result.successful = false;
        }
      } else if (param_name == _name + ".coarse_search_resolution") {
        if (_angle_quantizations % static_cast<unsigned int>(parameter.as_int()) != 0) {
          RCLCPP_WARN(
            _logger,
            "The value of parameter coarse_search_resolution is incorrectly set to %ld, "
            "it should be an increment of the angle_quantization_bins (%u). "
            "Ignoring parameter update.",
            parameter.as_int(),
            _angle_quantizations);
          result.successful = false;
        }
      }
    } else if (param_type == ParameterType::PARAMETER_STRING) {
      if (param_name == _name + ".motion_model_for_search") {
        MotionModel motion_model = fromString(parameter.as_string());
        if (motion_model == MotionModel::UNKNOWN) {
          RCLCPP_WARN(
            _logger,
            "Unable to get MotionModel search type. Given '%s', "
            "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE. "
            "Ignoring parameter update.",
            parameter.as_string().c_str());
          result.successful = false;
        }
      } else if (param_name == _name + ".goal_heading_mode") {
        GoalHeadingMode goal_heading_mode = fromStringToGH(parameter.as_string());
        if (goal_heading_mode == GoalHeadingMode::UNKNOWN) {
          RCLCPP_WARN(
            _logger,
            "Unable to get GoalHeader type. Given '%s' Valid options are DEFAULT, "
            "BIDIRECTIONAL, ALL_DIRECTION. Ignoring parameter update.",
            parameter.as_string().c_str());
          result.successful = false;
        }
      }
    }
  }
  return result;
}
 
template<typename NodeT>
void
SmacPlannerHybridT<NodeT>::updateParametersCallback(
  const std::vector<rclcpp::Parameter> & parameters)
{
  std::lock_guard<std::mutex> lock_reinit(_mutex);
 
  bool reinit_collision_checker = false;
  bool reinit_a_star = false;
  bool reinit_lookup_table = false;
  bool reinit_downsampler = false;
  bool reinit_smoother = false;
 
  for (auto parameter : parameters) {
    const auto & param_type = parameter.get_type();
    const auto & param_name = parameter.get_name();
    if (param_name.find(_name + ".") != 0) {
      continue;
    }
    if (param_type == ParameterType::PARAMETER_DOUBLE) {
      if (param_name == _name + ".max_planning_time") {
        reinit_a_star = true;
        _max_planning_time = parameter.as_double();
      } else if (param_name == _name + ".tolerance") {
        _tolerance = static_cast<float>(parameter.as_double());
      } else if (param_name == _name + ".lookup_table_size") {
        reinit_a_star = true;
        reinit_lookup_table = true;
        _lookup_table_size = parameter.as_double();
      } else if (param_name == _name + ".minimum_turning_radius") {
        reinit_a_star = true;
        reinit_lookup_table = true;
        if (_smoother) {
          reinit_smoother = true;
        }
        _minimum_turning_radius_global_coords = static_cast<float>(parameter.as_double());
      } else if (param_name == _name + ".reverse_penalty") {
        reinit_a_star = true;
        _search_info.reverse_penalty = static_cast<float>(parameter.as_double());
      } else if (param_name == _name + ".change_penalty") {
        reinit_a_star = true;
        _search_info.change_penalty = static_cast<float>(parameter.as_double());
      } else if (param_name == _name + ".non_straight_penalty") {
        reinit_a_star = true;
        _search_info.non_straight_penalty = static_cast<float>(parameter.as_double());
      } else if (param_name == _name + ".cost_penalty") {
        reinit_a_star = true;
        _search_info.cost_penalty = static_cast<float>(parameter.as_double());
      } else if (param_name == _name + ".analytic_expansion_ratio") {
        reinit_a_star = true;
        _search_info.analytic_expansion_ratio = static_cast<float>(parameter.as_double());
      } else if (param_name == _name + ".analytic_expansion_max_length") {
        reinit_a_star = true;
        _analytic_expansion_max_length_m = parameter.as_double();
      } else if (param_name == _name + ".analytic_expansion_max_cost") {
        reinit_a_star = true;
        _search_info.analytic_expansion_max_cost = static_cast<float>(parameter.as_double());
      }
    } else if (param_type == ParameterType::PARAMETER_BOOL) {
      if (param_name == _name + ".downsample_costmap") {
        reinit_downsampler = true;
        _downsample_costmap = parameter.as_bool();
      } else if (param_name == _name + ".allow_unknown") {
        reinit_a_star = true;
        _allow_unknown = parameter.as_bool();
      } else if (param_name == _name + ".cache_obstacle_heuristic") {
        reinit_a_star = true;
        _search_info.cache_obstacle_heuristic = parameter.as_bool();
      } else if (param_name == _name + ".allow_primitive_interpolation") {
        _search_info.allow_primitive_interpolation = parameter.as_bool();
        reinit_a_star = true;
      } else if (param_name == _name + ".smooth_path") {
        if (parameter.as_bool()) {
          reinit_smoother = true;
        } else {
          _smoother.reset();
        }
      } else if (param_name == _name + ".analytic_expansion_max_cost_override") {
        _search_info.analytic_expansion_max_cost_override = parameter.as_bool();
        reinit_a_star = true;
      }
    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
      if (param_name == _name + ".downsampling_factor") {
        reinit_a_star = true;
        reinit_lookup_table = true;
        reinit_downsampler = true;
        _downsampling_factor = parameter.as_int();
      } else if (param_name == _name + ".max_iterations") {
        reinit_a_star = true;
        _max_iterations = parameter.as_int();
        if (_max_iterations <= 0) {
          RCLCPP_INFO(
            _logger, "maximum iteration selected as <= 0, "
            "disabling maximum iterations.");
          _max_iterations = std::numeric_limits<int>::max();
        }
      } else if (param_name == _name + ".max_on_approach_iterations") {
        reinit_a_star = true;
        _max_on_approach_iterations = parameter.as_int();
        if (_max_on_approach_iterations <= 0) {
          RCLCPP_INFO(
            _logger, "On approach iteration selected as <= 0, "
            "disabling tolerance and on approach iterations.");
          _max_on_approach_iterations = std::numeric_limits<int>::max();
        }
      } else if (param_name == _name + ".terminal_checking_interval") {
        reinit_a_star = true;
        _terminal_checking_interval = parameter.as_int();
      } else if (param_name == _name + ".angle_quantization_bins") {
        reinit_collision_checker = true;
        reinit_a_star = true;
        reinit_lookup_table = true;
        int angle_quantizations = parameter.as_int();
        _angle_bin_size = 2.0 * M_PI / angle_quantizations;
        _angle_quantizations = static_cast<unsigned int>(angle_quantizations);
      } else if (param_name == _name + ".coarse_search_resolution") {
        _coarse_search_resolution = parameter.as_int();
      }
    } else if (param_type == ParameterType::PARAMETER_STRING) {
      if (param_name == _name + ".motion_model_for_search") {
        reinit_a_star = true;
        reinit_lookup_table = true;
        _motion_model = fromString(parameter.as_string());
      } else if (param_name == _name + ".goal_heading_mode") {
        std::string goal_heading_type = parameter.as_string();
        GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type);
        RCLCPP_INFO(
          _logger,
          "GoalHeadingMode type set to '%s'.",
          goal_heading_type.c_str());
        _goal_heading_mode = goal_heading_mode;
      }
    }
  }
 
  if (reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother) {
    std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
    if (_search_resolution != _costmap->getResolution() *
      (_downsample_costmap ? _downsampling_factor : 1))
    {
      reinit_a_star = true;
      reinit_lookup_table = true;
    }
    // convert to grid coordinates
    if (!_downsample_costmap) {
      _downsampling_factor = 1;
    }
    updateSearchResolution(_costmap->getResolution() * _downsampling_factor);
    auto node = _node.lock();
 
    // Re-Initialize A* template
    if (reinit_a_star) {
      if (reinit_lookup_table) {
        _a_star = std::make_unique<AStarAlgorithm<NodeT>>(_motion_model, _search_info);
      } else {
        _a_star->setSearchInfo(_search_info);
      }
      _a_star->initialize(
        _allow_unknown,
        _max_iterations,
        _max_on_approach_iterations,
        _terminal_checking_interval,
        _max_planning_time,
        _lookup_table_dim,
        _angle_quantizations);
    }
 
    // Re-Initialize costmap downsampler
    if (reinit_downsampler) {
      if (_downsample_costmap && _downsampling_factor > 1) {
        std::string topic_name = "downsampled_costmap";
        _costmap_downsampler = std::make_unique<CostmapDownsampler>();
        _costmap_downsampler->on_configure(
          node, _global_frame, topic_name, _costmap, _downsampling_factor);
        _costmap_downsampler->on_activate();
      }
    }
 
    // Re-Initialize collision checker
    if (reinit_collision_checker) {
      _collision_checker = GridCollisionChecker(_costmap_ros, _angle_quantizations, node);
      _collision_checker.setFootprint(
        _costmap_ros->getRobotFootprint(),
        _costmap_ros->getUseRadius(),
        findCircumscribedCost(_costmap_ros));
    }
 
    // Re-Initialize smoother
    if (reinit_smoother) {
      SmootherParams params;
      params.get(node, _name);
      _smoother = std::make_unique<Smoother>(params);
      _smoother->initialize(_minimum_turning_radius_global_coords);
    }
  }
}
 
}  // namespace nav2_smac_planner
 
#endif  // NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_IMPL_HPP_