add humble-navigation2

navigation2/nav2_constrained_smoother/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_constrained_smoother)
+
+set(CMAKE_BUILD_TYPE Release) # significant Ceres optimization speedup
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(angles REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
+
+set(CMAKE_CXX_STANDARD 17)
+
+if(${CERES_VERSION} VERSION_LESS_EQUAL 2.0.0)
+  add_definitions(-DUSE_OLD_CERES_API)
+endif()
+
+nav2_package()
+
+set(library_name nav2_constrained_smoother)
+
+include_directories(
+  include
+  ${CERES_INCLUDES}
+)
+
+set(dependencies
+  angles
+  rclcpp
+  rclcpp_action
+  nav2_msgs
+  nav2_costmap_2d
+  nav2_util
+  nav2_core
+  pluginlib
+)
+
+add_library(${library_name} SHARED src/constrained_smoother.cpp)
+target_link_libraries(${library_name} ${CERES_LIBRARIES})
+# prevent pluginlib from using boost
+target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+ament_target_dependencies(${library_name} ${dependencies})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
+endif()
+
+install(
+  TARGETS
+    ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+
+pluginlib_export_plugin_description_file(nav2_core nav2_constrained_smoother.xml)
+
+ament_package()

navigation2/nav2_constrained_smoother/README.md

@@ -0,0 +1,49 @@
+# Constrained Smoother
+
+A smoother plugin for `nav2_smoother` based on the original deprecated smoother in `nav2_smac_planner` by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) and put into operational state by [**RoboTech Vision**](https://robotechvision.com/). Suitable for applications which need planned global path to be pushed away from obstacles and/or for Reeds-Shepp motion models.
+
+See documentation on navigation.ros.org: https://navigation.ros.org/configuration/packages/configuring-constrained-smoother.html
+
+
+Example of configuration (see indoor_navigation package of this repo for a full launch configuration):
+
+```
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["SmoothPath"]
+
+    SmoothPath:
+      plugin: "nav2_constrained_smoother/ConstrainedSmoother"
+      reversing_enabled: true       # whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned
+      path_downsampling_factor: 3   # every n-th node of the path is taken. Useful for speed-up
+      path_upsampling_factor: 1     # 0 - path remains downsampled, 1 - path is upsampled back to original granularity using cubic bezier, 2... - more upsampling
+      keep_start_orientation: true  # whether to prevent the start orientation from being smoothed
+      keep_goal_orientation: true   # whether to prevent the gpal orientation from being smoothed
+      minimum_turning_radius: 0.40  # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots
+      w_curve: 30.0                 # weight to enforce minimum_turning_radius
+      w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
+      w_smooth: 2000000.0           # weight to maximize smoothness of path
+      w_cost: 0.015                 # weight to steer robot away from collision and cost
+
+      # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes)
+      # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification
+      w_cost_cusp_multiplier: 3.0   # option to have higher weight during forward/reverse direction change which is often accompanied with dangerous rotations
+      cusp_zone_length: 2.5         # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight equals w_cost*w_cost_cusp_multiplier)
+
+      # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...]
+      # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots)
+      # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification
+      # cost_check_points: [-0.185, 0.0, 1.0]
+
+      optimizer:
+        max_iterations: 70            # max iterations of smoother
+        debug_optimizer: false        # print debug info
+        gradient_tol: 5e3
+        fn_tol: 1.0e-15
+        param_tol: 1.0e-20
+```
+
+Note: Smoothing paths which contain multiple subsequent poses at one point (e.g. in-place rotations from Smac lattice planners) is currently not supported
+
+Note: Constrained Smoother is recommended to be used on a path with a bounded length. TruncatePathLocal BT Node can be used for extracting a relevant path section around robot (in combination with DistanceController to achieve periodicity)

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp

@@ -0,0 +1,104 @@
+// Copyright (c) 2021 RoboTech Vision
+// 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_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_
+#define NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+#include "nav2_core/smoother.hpp"
+#include "nav2_constrained_smoother/smoother.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/odometry_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+
+namespace nav2_constrained_smoother
+{
+
+/**
+ * @class nav2_constrained_smoother::ConstrainedSmoother
+ * @brief Regulated pure pursuit controller plugin
+ */
+class ConstrainedSmoother : public nav2_core::Smoother
+{
+public:
+  /**
+   * @brief Constructor for nav2_constrained_smoother::ConstrainedSmoother
+   */
+  ConstrainedSmoother() = default;
+
+  /**
+   * @brief Destrructor for nav2_constrained_smoother::ConstrainedSmoother
+   */
+  ~ConstrainedSmoother() override = default;
+
+  /**
+   * @brief Configure smoother parameters and member variables
+   * @param parent WeakPtr to node
+   * @param name Name of plugin
+   * @param tf TF buffer
+   * @param costmap_ros Costmap2DROS object of environment
+   */
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
+    std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub) override;
+
+  /**
+   * @brief Cleanup controller state machine
+   */
+  void cleanup() override;
+
+  /**
+   * @brief Activate controller state machine
+   */
+  void activate() override;
+
+  /**
+   * @brief Deactivate controller state machine
+   */
+  void deactivate() override;
+
+  /**
+   * @brief Method to smooth given path
+   *
+   * @param path In-out path to be optimized
+   * @param max_time Maximum duration smoothing should take
+   * @return Smoothed path
+   */
+  bool smooth(
+    nav_msgs::msg::Path & path,
+    const rclcpp::Duration & max_time) override;
+
+protected:
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::string plugin_name_;
+  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  rclcpp::Logger logger_ {rclcpp::get_logger("ConstrainedSmoother")};
+
+  std::unique_ptr<nav2_constrained_smoother::Smoother> smoother_;
+  SmootherParams smoother_params_;
+  OptimizerParams optimizer_params_;
+};
+
+}  // namespace nav2_constrained_smoother
+
+#endif  // NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp

@@ -0,0 +1,202 @@
+// Copyright (c) 2021 RoboTech Vision
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_
+#define NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_
+
+#include <map>
+#include <string>
+#include <vector>
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "ceres/ceres.h"
+
+namespace nav2_constrained_smoother
+{
+
+/**
+ * @struct nav2_smac_planner::SmootherParams
+ * @brief Parameters for the smoother cost function
+ */
+struct SmootherParams
+{
+  /**
+   * @brief A constructor for nav2_smac_planner::SmootherParams
+   */
+  SmootherParams()
+  {
+  }
+
+  /**
+   * @brief Get params from ROS parameter
+   * @param node_ Ptr to node
+   * @param name Name of plugin
+   */
+  void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
+  {
+    std::string local_name = name + std::string(".");
+
+    // Smoother params
+    double minimum_turning_radius;
+    nav2_util::declare_parameter_if_not_declared(
+      node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4));
+    node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius);
+    max_curvature = 1.0f / minimum_turning_radius;
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_curve", rclcpp::ParameterValue(30.0));
+    node->get_parameter(local_name + "w_curve", curvature_weight);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_cost", rclcpp::ParameterValue(0.015));
+    node->get_parameter(local_name + "w_cost", costmap_weight);
+    double cost_cusp_multiplier;
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_cost_cusp_multiplier", rclcpp::ParameterValue(3.0));
+    node->get_parameter(local_name + "w_cost_cusp_multiplier", cost_cusp_multiplier);
+    cusp_costmap_weight = costmap_weight * cost_cusp_multiplier;
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "cusp_zone_length", rclcpp::ParameterValue(2.5));
+    node->get_parameter(local_name + "cusp_zone_length", cusp_zone_length);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_dist", rclcpp::ParameterValue(0.0));
+    node->get_parameter(local_name + "w_dist", distance_weight);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_smooth", rclcpp::ParameterValue(2000000.0));
+    node->get_parameter(local_name + "w_smooth", smooth_weight);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "cost_check_points", rclcpp::ParameterValue(std::vector<double>()));
+    node->get_parameter(local_name + "cost_check_points", cost_check_points);
+    if (cost_check_points.size() % 3 != 0) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger(
+          "constrained_smoother"),
+        "cost_check_points parameter must contain values as follows: "
+        "[x1, y1, weight1, x2, y2, weight2, ...]");
+      throw std::runtime_error("Invalid parameter: cost_check_points");
+    }
+    // normalize check point weights so that their sum == 1.0
+    double check_point_weights_sum = 0.0;
+    for (size_t i = 2u; i < cost_check_points.size(); i += 3) {
+      check_point_weights_sum += cost_check_points[i];
+    }
+    for (size_t i = 2u; i < cost_check_points.size(); i += 3) {
+      cost_check_points[i] /= check_point_weights_sum;
+    }
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "path_downsampling_factor", rclcpp::ParameterValue(1));
+    node->get_parameter(local_name + "path_downsampling_factor", path_downsampling_factor);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "path_upsampling_factor", rclcpp::ParameterValue(1));
+    node->get_parameter(local_name + "path_upsampling_factor", path_upsampling_factor);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "reversing_enabled", rclcpp::ParameterValue(true));
+    node->get_parameter(local_name + "reversing_enabled", reversing_enabled);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "keep_goal_orientation", rclcpp::ParameterValue(true));
+    node->get_parameter(local_name + "keep_goal_orientation", keep_goal_orientation);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "keep_start_orientation", rclcpp::ParameterValue(true));
+    node->get_parameter(local_name + "keep_start_orientation", keep_start_orientation);
+  }
+
+  double smooth_weight{0.0};
+  double costmap_weight{0.0};
+  double cusp_costmap_weight{0.0};
+  double cusp_zone_length{0.0};
+  double distance_weight{0.0};
+  double curvature_weight{0.0};
+  double max_curvature{0.0};
+  double max_time{10.0};  // adjusted by action goal, not by parameters
+  int path_downsampling_factor{1};
+  int path_upsampling_factor{1};
+  bool reversing_enabled{true};
+  bool keep_goal_orientation{true};
+  bool keep_start_orientation{true};
+  std::vector<double> cost_check_points{};
+};
+
+/**
+ * @struct nav2_smac_planner::OptimizerParams
+ * @brief Parameters for the ceres optimizer
+ */
+struct OptimizerParams
+{
+  OptimizerParams()
+  : debug(false),
+    max_iterations(50),
+    param_tol(1e-8),
+    fn_tol(1e-6),
+    gradient_tol(1e-10)
+  {
+  }
+
+  /**
+   * @brief Get params from ROS parameter
+   * @param node_ Ptr to node
+   * @param name Name of plugin
+   */
+  void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
+  {
+    std::string local_name = name + std::string(".optimizer.");
+
+    // Optimizer params
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "linear_solver_type", rclcpp::ParameterValue("SPARSE_NORMAL_CHOLESKY"));
+    node->get_parameter(local_name + "linear_solver_type", linear_solver_type);
+    if (solver_types.find(linear_solver_type) == solver_types.end()) {
+      std::stringstream valid_types_str;
+      for (auto type = solver_types.begin(); type != solver_types.end(); type++) {
+        if (type != solver_types.begin()) {
+          valid_types_str << ", ";
+        }
+        valid_types_str << type->first;
+      }
+      RCLCPP_ERROR(
+        rclcpp::get_logger("constrained_smoother"),
+        "Invalid linear_solver_type. Valid values are %s", valid_types_str.str().c_str());
+      throw std::runtime_error("Invalid parameter: linear_solver_type");
+    }
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "param_tol", rclcpp::ParameterValue(1e-15));
+    node->get_parameter(local_name + "param_tol", param_tol);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7));
+    node->get_parameter(local_name + "fn_tol", fn_tol);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10));
+    node->get_parameter(local_name + "gradient_tol", gradient_tol);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "max_iterations", rclcpp::ParameterValue(100));
+    node->get_parameter(local_name + "max_iterations", max_iterations);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "debug_optimizer", rclcpp::ParameterValue(false));
+    node->get_parameter(local_name + "debug_optimizer", debug);
+  }
+
+  const std::map<std::string, ceres::LinearSolverType> solver_types = {
+    {"DENSE_QR", ceres::DENSE_QR},
+    {"SPARSE_NORMAL_CHOLESKY", ceres::SPARSE_NORMAL_CHOLESKY}};
+
+  bool debug;
+  std::string linear_solver_type;
+  int max_iterations;  // Ceres default: 50
+
+  double param_tol;  // Ceres default: 1e-8
+  double fn_tol;  // Ceres default: 1e-6
+  double gradient_tol;  // Ceres default: 1e-10
+};
+
+}  // namespace nav2_constrained_smoother
+
+#endif  // NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp

@@ -0,0 +1,401 @@
+// Copyright (c) 2021 RoboTech Vision
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_
+#define NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <utility>
+#include <deque>
+#include <limits>
+#include <algorithm>
+
+#include "nav2_constrained_smoother/smoother_cost_function.hpp"
+#include "nav2_constrained_smoother/utils.hpp"
+
+#include "ceres/ceres.h"
+#include "Eigen/Core"
+
+namespace nav2_constrained_smoother
+{
+
+/**
+ * @class nav2_smac_planner::Smoother
+ * @brief A Conjugate Gradient 2D path smoother implementation
+ */
+class Smoother
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::Smoother
+   */
+  Smoother() {}
+
+  /**
+   * @brief A destructor for nav2_smac_planner::Smoother
+   */
+  ~Smoother() {}
+
+  /**
+   * @brief Initialization of the smoother
+   * @param params OptimizerParam struct
+   */
+  void initialize(const OptimizerParams params)
+  {
+    debug_ = params.debug;
+
+    options_.linear_solver_type = params.solver_types.at(params.linear_solver_type);
+
+    options_.max_num_iterations = params.max_iterations;
+
+    options_.function_tolerance = params.fn_tol;
+    options_.gradient_tolerance = params.gradient_tol;
+    options_.parameter_tolerance = params.param_tol;
+
+    if (debug_) {
+      options_.minimizer_progress_to_stdout = true;
+      options_.logging_type = ceres::LoggingType::PER_MINIMIZER_ITERATION;
+    } else {
+      options_.logging_type = ceres::SILENT;
+    }
+  }
+
+  /**
+   * @brief Smoother method
+   * @param path Reference to path
+   * @param start_dir Orientation of the first pose
+   * @param end_dir Orientation of the last pose
+   * @param costmap Pointer to minimal costmap
+   * @param params parameters weights
+   * @return If smoothing was successful
+   */
+  bool smooth(
+    std::vector<Eigen::Vector3d> & path,
+    const Eigen::Vector2d & start_dir,
+    const Eigen::Vector2d & end_dir,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const SmootherParams & params)
+  {
+    // Path has always at least 2 points
+    if (path.size() < 2) {
+      throw std::runtime_error("Constrained smoother: Path must have at least 2 points");
+    }
+
+    options_.max_solver_time_in_seconds = params.max_time;
+
+    ceres::Problem problem;
+    std::vector<Eigen::Vector3d> path_optim;
+    std::vector<bool> optimized;
+    if (buildProblem(path, costmap, params, problem, path_optim, optimized)) {
+      // solve the problem
+      ceres::Solver::Summary summary;
+      ceres::Solve(options_, &problem, &summary);
+      if (debug_) {
+        RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "%s", summary.FullReport().c_str());
+      }
+      if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost < 0.0) {
+        return false;
+      }
+    } else {
+      RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "Path too short to optimize");
+    }
+
+    upsampleAndPopulate(path_optim, optimized, start_dir, end_dir, params, path);
+
+    return true;
+  }
+
+private:
+  /**
+   * @brief Build problem method
+   * @param path Reference to path
+   * @param costmap Pointer to costmap
+   * @param params Smoother parameters
+   * @param problem Output problem to solve
+   * @param path_optim Output path on which the problem will be solved
+   * @param optimized False for points skipped by downsampling
+   * @return If there is a problem to solve
+   */
+  bool buildProblem(
+    const std::vector<Eigen::Vector3d> & path,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const SmootherParams & params,
+    ceres::Problem & problem,
+    std::vector<Eigen::Vector3d> & path_optim,
+    std::vector<bool> & optimized)
+  {
+    // Create costmap grid
+    costmap_grid_ = std::make_shared<ceres::Grid2D<u_char>>(
+      costmap->getCharMap(), 0, costmap->getSizeInCellsY(), 0, costmap->getSizeInCellsX());
+    auto costmap_interpolator = std::make_shared<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(
+      *costmap_grid_);
+
+    // Create residual blocks
+    const double cusp_half_length = params.cusp_zone_length / 2;
+    ceres::LossFunction * loss_function = NULL;
+    path_optim = path;
+    optimized = std::vector<bool>(path.size());
+    optimized[0] = true;
+    int prelast_i = -1;
+    int last_i = 0;
+    double last_direction = path_optim[0][2];
+    bool last_was_cusp = false;
+    bool last_is_reversing = false;
+    std::deque<std::pair<double, SmootherCostFunction *>> potential_cusp_funcs;
+    double last_segment_len = EPSILON;
+    double potential_cusp_funcs_len = 0;
+    double len_since_cusp = std::numeric_limits<double>::infinity();
+
+    for (size_t i = 1; i < path_optim.size(); i++) {
+      auto & pt = path_optim[i];
+      bool is_cusp = false;
+      if (i != path_optim.size() - 1) {
+        is_cusp = pt[2] * last_direction < 0;
+        last_direction = pt[2];
+
+        // skip to downsample if can be skipped (no forward/reverse direction change)
+        if (!is_cusp &&
+          i > (params.keep_start_orientation ? 1 : 0) &&
+          i < path_optim.size() - (params.keep_goal_orientation ? 2 : 1) &&
+          static_cast<int>(i - last_i) < params.path_downsampling_factor)
+        {
+          continue;
+        }
+      }
+
+      // keep distance inequalities between poses
+      // (some might have been downsampled while others might not)
+      double current_segment_len = (path_optim[i] - path_optim[last_i]).block<2, 1>(0, 0).norm();
+
+      // forget cost functions which don't have chance to be part of a cusp zone
+      potential_cusp_funcs_len += current_segment_len;
+      while (!potential_cusp_funcs.empty() && potential_cusp_funcs_len > cusp_half_length) {
+        potential_cusp_funcs_len -= potential_cusp_funcs.front().first;
+        potential_cusp_funcs.pop_front();
+      }
+
+      // update cusp zone costmap weights
+      if (is_cusp) {
+        double len_to_cusp = current_segment_len;
+        for (int i = potential_cusp_funcs.size() - 1; i >= 0; i--) {
+          auto & f = potential_cusp_funcs[i];
+          double new_weight =
+            params.cusp_costmap_weight * (1.0 - len_to_cusp / cusp_half_length) +
+            params.costmap_weight * len_to_cusp / cusp_half_length;
+          if (std::abs(new_weight - params.cusp_costmap_weight) <
+            std::abs(f.second->getCostmapWeight() - params.cusp_costmap_weight))
+          {
+            f.second->setCostmapWeight(new_weight);
+          }
+          len_to_cusp += f.first;
+        }
+        potential_cusp_funcs_len = 0;
+        potential_cusp_funcs.clear();
+        len_since_cusp = 0;
+      }
+
+      // add cost function
+      optimized[i] = true;
+      if (prelast_i != -1) {
+        double costmap_weight = params.costmap_weight;
+        if (len_since_cusp <= cusp_half_length) {
+          costmap_weight =
+            params.cusp_costmap_weight * (1.0 - len_since_cusp / cusp_half_length) +
+            params.costmap_weight * len_since_cusp / cusp_half_length;
+        }
+        SmootherCostFunction * cost_function = new SmootherCostFunction(
+          path[last_i].template block<2, 1>(
+            0,
+            0),
+          (last_was_cusp ? -1 : 1) * last_segment_len / current_segment_len,
+          last_is_reversing,
+          costmap,
+          costmap_interpolator,
+          params,
+          costmap_weight
+        );
+        problem.AddResidualBlock(
+          cost_function->AutoDiff(), loss_function,
+          path_optim[last_i].data(), pt.data(), path_optim[prelast_i].data());
+
+        potential_cusp_funcs.emplace_back(current_segment_len, cost_function);
+      }
+
+      // shift current to last and last to pre-last
+      last_was_cusp = is_cusp;
+      last_is_reversing = last_direction < 0;
+      prelast_i = last_i;
+      last_i = i;
+      len_since_cusp += current_segment_len;
+      last_segment_len = std::max(EPSILON, current_segment_len);
+    }
+
+    int posesToOptimize = problem.NumParameterBlocks() - 2;  // minus start and goal
+    if (params.keep_goal_orientation) {
+      posesToOptimize -= 1;  // minus goal orientation holder
+    }
+    if (params.keep_start_orientation) {
+      posesToOptimize -= 1;  // minus start orientation holder
+    }
+    if (posesToOptimize <= 0) {
+      return false;  // nothing to optimize
+    }
+    // first two and last two points are constant (to keep start and end direction)
+    problem.SetParameterBlockConstant(path_optim.front().data());
+    if (params.keep_start_orientation) {
+      problem.SetParameterBlockConstant(path_optim[1].data());
+    }
+    if (params.keep_goal_orientation) {
+      problem.SetParameterBlockConstant(path_optim[path_optim.size() - 2].data());
+    }
+    problem.SetParameterBlockConstant(path_optim.back().data());
+    return true;
+  }
+
+  /**
+   * @brief Populate optimized points to path, assigning orientations and upsampling poses using cubic bezier
+   * @param path_optim Path with optimized points
+   * @param optimized False for points skipped by downsampling
+   * @param start_dir Orientation of the first pose
+   * @param end_dir Orientation of the last pose
+   * @param params Smoother parameters
+   * @param path Output path with upsampled optimized points
+   */
+  void upsampleAndPopulate(
+    const std::vector<Eigen::Vector3d> & path_optim,
+    const std::vector<bool> & optimized,
+    const Eigen::Vector2d & start_dir,
+    const Eigen::Vector2d & end_dir,
+    const SmootherParams & params,
+    std::vector<Eigen::Vector3d> & path)
+  {
+    // Populate path, assign orientations, interpolate skipped/upsampled poses
+    path.clear();
+    if (params.path_upsampling_factor > 1) {
+      path.reserve(params.path_upsampling_factor * (path_optim.size() - 1) + 1);
+    }
+    int last_i = 0;
+    int prelast_i = -1;
+    Eigen::Vector2d prelast_dir;
+    for (int i = 1; i <= static_cast<int>(path_optim.size()); i++) {
+      if (i == static_cast<int>(path_optim.size()) || optimized[i]) {
+        if (prelast_i != -1) {
+          Eigen::Vector2d last_dir;
+          auto & prelast = path_optim[prelast_i];
+          auto & last = path_optim[last_i];
+
+          // Compute orientation of last
+          if (i < static_cast<int>(path_optim.size())) {
+            auto & current = path_optim[i];
+            Eigen::Vector2d tangent_dir = tangentDir<double>(
+              prelast.block<2, 1>(0, 0),
+              last.block<2, 1>(0, 0),
+              current.block<2, 1>(0, 0),
+              prelast[2] * last[2] < 0);
+
+            last_dir =
+              tangent_dir.dot((current - last).block<2, 1>(0, 0) * last[2]) >= 0 ?
+              tangent_dir :
+              -tangent_dir;
+            last_dir.normalize();
+          } else if (params.keep_goal_orientation) {
+            last_dir = end_dir;
+          } else {
+            last_dir = (last - prelast).block<2, 1>(0, 0) * last[2];
+            last_dir.normalize();
+          }
+          double last_angle = atan2(last_dir[1], last_dir[0]);
+
+          // Interpolate poses between prelast and last
+          int interp_cnt = (last_i - prelast_i) * params.path_upsampling_factor - 1;
+          if (interp_cnt > 0) {
+            Eigen::Vector2d last_pt = last.block<2, 1>(0, 0);
+            Eigen::Vector2d prelast_pt = prelast.block<2, 1>(0, 0);
+            double dist = (last_pt - prelast_pt).norm();
+            Eigen::Vector2d pt1 = prelast_pt + prelast_dir * dist * 0.4 * prelast[2];
+            Eigen::Vector2d pt2 = last_pt - last_dir * dist * 0.4 * prelast[2];
+            for (int j = 1; j <= interp_cnt; j++) {
+              double interp = j / static_cast<double>(interp_cnt + 1);
+              Eigen::Vector2d pt = cubicBezier(prelast_pt, pt1, pt2, last_pt, interp);
+              path.emplace_back(pt[0], pt[1], 0.0);
+            }
+          }
+          path.emplace_back(last[0], last[1], last_angle);
+
+          // Assign orientations to interpolated points
+          for (size_t j = path.size() - 1 - interp_cnt; j < path.size() - 1; j++) {
+            Eigen::Vector2d tangent_dir = tangentDir<double>(
+              path[j - 1].block<2, 1>(0, 0),
+              path[j].block<2, 1>(0, 0),
+              path[j + 1].block<2, 1>(0, 0),
+              false);
+            tangent_dir =
+              tangent_dir.dot((path[j + 1] - path[j]).block<2, 1>(0, 0) * prelast[2]) >= 0 ?
+              tangent_dir :
+              -tangent_dir;
+            path[j][2] = atan2(tangent_dir[1], tangent_dir[0]);
+          }
+
+          prelast_dir = last_dir;
+        } else {  // start pose
+          auto & start = path_optim[0];
+          Eigen::Vector2d dir = params.keep_start_orientation ?
+            start_dir :
+            ((path_optim[i] - start).block<2, 1>(0, 0) * start[2]).normalized();
+          path.emplace_back(start[0], start[1], atan2(dir[1], dir[0]));
+          prelast_dir = dir;
+        }
+        prelast_i = last_i;
+        last_i = i;
+      }
+    }
+  }
+
+  /*
+    Piecewise cubic bezier curve as defined by Adobe in Postscript
+    The two end points are pt0 and pt3
+    Their associated control points are pt1 and pt2
+  */
+  static Eigen::Vector2d cubicBezier(
+    Eigen::Vector2d & pt0, Eigen::Vector2d & pt1,
+    Eigen::Vector2d & pt2, Eigen::Vector2d & pt3, double mu)
+  {
+    Eigen::Vector2d a, b, c, pt;
+
+    c[0] = 3 * (pt1[0] - pt0[0]);
+    c[1] = 3 * (pt1[1] - pt0[1]);
+    b[0] = 3 * (pt2[0] - pt1[0]) - c[0];
+    b[1] = 3 * (pt2[1] - pt1[1]) - c[1];
+    a[0] = pt3[0] - pt0[0] - c[0] - b[0];
+    a[1] = pt3[1] - pt0[1] - c[1] - b[1];
+
+    pt[0] = a[0] * mu * mu * mu + b[0] * mu * mu + c[0] * mu + pt0[0];
+    pt[1] = a[1] * mu * mu * mu + b[1] * mu * mu + c[1] * mu + pt0[1];
+
+    return pt;
+  }
+
+  bool debug_;
+  ceres::Solver::Options options_;
+  std::shared_ptr<ceres::Grid2D<u_char>> costmap_grid_;
+};
+
+}  // namespace nav2_constrained_smoother
+
+#endif  // NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp

@@ -0,0 +1,252 @@
+// Copyright (c) 2021 RoboTech Vision
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_
+#define NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <unordered_map>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "ceres/ceres.h"
+#include "ceres/cubic_interpolation.h"
+#include "Eigen/Core"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_constrained_smoother/options.hpp"
+#include "nav2_constrained_smoother/utils.hpp"
+
+namespace nav2_constrained_smoother
+{
+
+/**
+ * @struct nav2_constrained_smoother::SmootherCostFunction
+ * @brief Cost function for path smoothing with multiple terms
+ * including curvature, smoothness, distance from original and obstacle avoidance.
+ */
+class SmootherCostFunction
+{
+public:
+  /**
+   * @brief A constructor for nav2_constrained_smoother::SmootherCostFunction
+   * @param original_path Original position of the path node
+   * @param next_to_last_length_ratio Ratio of next path segment compared to previous.
+   *  Negative if one of them represents reversing motion.
+   * @param reversing Whether the path segment after this node represents reversing motion.
+   * @param costmap A costmap to get values for collision and obstacle avoidance
+   * @param params Optimization weights and parameters
+   * @param costmap_weight Costmap cost weight. Can be params.costmap_weight or params.cusp_costmap_weight
+   */
+  SmootherCostFunction(
+    const Eigen::Vector2d & original_pos,
+    double next_to_last_length_ratio,
+    bool reversing,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> & costmap_interpolator,
+    const SmootherParams & params,
+    double costmap_weight)
+  : original_pos_(original_pos),
+    next_to_last_length_ratio_(next_to_last_length_ratio),
+    reversing_(reversing),
+    params_(params),
+    costmap_weight_(costmap_weight),
+    costmap_origin_(costmap->getOriginX(), costmap->getOriginY()),
+    costmap_resolution_(costmap->getResolution()),
+    costmap_interpolator_(costmap_interpolator)
+  {
+  }
+
+  ceres::CostFunction * AutoDiff()
+  {
+    return new ceres::AutoDiffCostFunction<SmootherCostFunction, 4, 2, 2, 2>(this);
+  }
+
+  void setCostmapWeight(double costmap_weight)
+  {
+    costmap_weight_ = costmap_weight;
+  }
+
+  double getCostmapWeight()
+  {
+    return costmap_weight_;
+  }
+
+  /**
+   * @brief Smoother cost function evaluation
+   * @param pt X, Y coords of current point
+   * @param pt_next X, Y coords of next point
+   * @param pt_prev X, Y coords of previous point
+   * @param pt_residual array of output residuals (smoothing, curvature, distance, cost)
+   * @return if successful in computing values
+   */
+  template<typename T>
+  bool operator()(
+    const T * const pt, const T * const pt_next, const T * const pt_prev,
+    T * pt_residual) const
+  {
+    Eigen::Map<const Eigen::Matrix<T, 2, 1>> xi(pt);
+    Eigen::Map<const Eigen::Matrix<T, 2, 1>> xi_next(pt_next);
+    Eigen::Map<const Eigen::Matrix<T, 2, 1>> xi_prev(pt_prev);
+    Eigen::Map<Eigen::Matrix<T, 4, 1>> residual(pt_residual);
+    residual.setZero();
+
+    // compute cost
+    addSmoothingResidual<T>(params_.smooth_weight, xi, xi_next, xi_prev, residual[0]);
+    addCurvatureResidual<T>(params_.curvature_weight, xi, xi_next, xi_prev, residual[1]);
+    addDistanceResidual<T>(
+      params_.distance_weight, xi,
+      original_pos_.template cast<T>(), residual[2]);
+    addCostResidual<T>(costmap_weight_, xi, xi_next, xi_prev, residual[3]);
+
+    return true;
+  }
+
+protected:
+  /**
+   * @brief Cost function term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt_next Point Xi+1 for calculating Xi's cost
+   * @param pt_prev Point Xi-1 for calculating Xi's cost
+   * @param r Residual (cost) of term
+   */
+  template<typename T>
+  inline void addSmoothingResidual(
+    const double & weight,
+    const Eigen::Matrix<T, 2, 1> & pt,
+    const Eigen::Matrix<T, 2, 1> & pt_next,
+    const Eigen::Matrix<T, 2, 1> & pt_prev,
+    T & r) const
+  {
+    Eigen::Matrix<T, 2, 1> d_next = pt_next - pt;
+    Eigen::Matrix<T, 2, 1> d_prev = pt - pt_prev;
+    Eigen::Matrix<T, 2, 1> d_diff = next_to_last_length_ratio_ * d_next - d_prev;
+    r += (T)weight * d_diff.dot(d_diff);    // objective function value
+  }
+
+  /**
+   * @brief Cost function term for maximum curved paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt_next Point Xi+1 for calculating Xi's cost
+   * @param pt_prev Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct to cache computations for the jacobian to use
+   * @param r Residual (cost) of term
+   */
+  template<typename T>
+  inline void addCurvatureResidual(
+    const double & weight,
+    const Eigen::Matrix<T, 2, 1> & pt,
+    const Eigen::Matrix<T, 2, 1> & pt_next,
+    const Eigen::Matrix<T, 2, 1> & pt_prev,
+    T & r) const
+  {
+    Eigen::Matrix<T, 2, 1> center = arcCenter(
+      pt_prev, pt, pt_next,
+      next_to_last_length_ratio_ < 0);
+    if (CERES_ISINF(center[0])) {
+      return;
+    }
+    T turning_rad = (pt - center).norm();
+    T ki_minus_kmax = (T)1.0 / turning_rad - params_.max_curvature;
+
+    if (ki_minus_kmax <= (T)EPSILON) {
+      return;
+    }
+
+    r += (T)weight * ki_minus_kmax * ki_minus_kmax;  // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for steering away changes in pose
+   * @param weight Weight to apply to function
+   * @param xi Point Xi for evaluation
+   * @param xi_original original point Xi for evaluation
+   * @param r Residual (cost) of term
+   */
+  template<typename T>
+  inline void addDistanceResidual(
+    const double & weight,
+    const Eigen::Matrix<T, 2, 1> & xi,
+    const Eigen::Matrix<T, 2, 1> & xi_original,
+    T & r) const
+  {
+    r += (T)weight * (xi - xi_original).squaredNorm();  // objective function value
+  }
+
+  /**
+   * @brief Cost function term for steering away from costs
+   * @param weight Weight to apply to function
+   * @param value Point Xi's cost'
+   * @param params computed values to reduce overhead
+   * @param r Residual (cost) of term
+   */
+  template<typename T>
+  inline void addCostResidual(
+    const double & weight,
+    const Eigen::Matrix<T, 2, 1> & pt,
+    const Eigen::Matrix<T, 2, 1> & pt_next,
+    const Eigen::Matrix<T, 2, 1> & pt_prev,
+    T & r) const
+  {
+    if (params_.cost_check_points.empty()) {
+      Eigen::Matrix<T, 2, 1> interp_pos =
+        (pt - costmap_origin_.template cast<T>()) / (T)costmap_resolution_;
+      T value;
+      costmap_interpolator_->Evaluate(interp_pos[1] - (T)0.5, interp_pos[0] - (T)0.5, &value);
+      r += (T)weight * value * value;  // objective function value
+    } else {
+      Eigen::Matrix<T, 2, 1> dir = tangentDir(
+        pt_prev, pt, pt_next,
+        next_to_last_length_ratio_ < 0);
+      dir.normalize();
+      if (((pt_next - pt).dot(dir) < (T)0) != reversing_) {
+        dir = -dir;
+      }
+      Eigen::Matrix<T, 3, 3> transform;
+      transform << dir[0], -dir[1], pt[0],
+        dir[1], dir[0], pt[1],
+        (T)0, (T)0, (T)1;
+      for (size_t i = 0; i < params_.cost_check_points.size(); i += 3) {
+        Eigen::Matrix<T, 3, 1> ccpt((T)params_.cost_check_points[i],
+          (T)params_.cost_check_points[i + 1], (T)1);
+        auto ccpt_world = (transform * ccpt).template block<2, 1>(0, 0);
+        Eigen::Matrix<T, 2,
+          1> interp_pos = (ccpt_world - costmap_origin_.template cast<T>()) /
+          (T)costmap_resolution_;
+        T value;
+        costmap_interpolator_->Evaluate(interp_pos[1] - (T)0.5, interp_pos[0] - (T)0.5, &value);
+
+        r += (T)weight * (T)params_.cost_check_points[i + 2] * value * value;
+      }
+    }
+  }
+
+  const Eigen::Vector2d original_pos_;
+  double next_to_last_length_ratio_;
+  bool reversing_;
+  SmootherParams params_;
+  double costmap_weight_;
+  Eigen::Vector2d costmap_origin_;
+  double costmap_resolution_;
+  std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> costmap_interpolator_;
+};
+
+}  // namespace nav2_constrained_smoother
+
+#endif  // NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_

navigation2/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp

@@ -0,0 +1,122 @@
+// Copyright (c) 2021 RoboTech Vision
+//
+// 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_CONSTRAINED_SMOOTHER__UTILS_HPP_
+#define NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_
+
+#include <limits>
+#include "Eigen/Core"
+
+#define EPSILON 0.0001
+
+/**
+ * Compatibility with different ceres::isinf() and ceres::IsInfinite() API
+ * used in Ceres Solver 2.1.0+ and 2.0.0- versions respectively
+ */
+#if defined(USE_OLD_CERES_API)
+  #define CERES_ISINF(x) ceres::IsInfinite(x)
+#else
+  #define CERES_ISINF(x) ceres::isinf(x)
+#endif
+
+namespace nav2_constrained_smoother
+{
+
+/**
+ * @brief Center of an arc between three points
+ * @param pt_prev Starting point of the arc
+ * @param pt Mid point of the arc
+ * @param pt_next Last point of the arc
+ * @param is_cusp True if pt is a cusp point
+ * @result position of the center or Vector2(inf, inf) for straight lines and 180 deg turns
+ */
+template<typename T>
+inline Eigen::Matrix<T, 2, 1> arcCenter(
+  Eigen::Matrix<T, 2, 1> pt_prev,
+  Eigen::Matrix<T, 2, 1> pt,
+  Eigen::Matrix<T, 2, 1> pt_next,
+  bool is_cusp)
+{
+  Eigen::Matrix<T, 2, 1> d1 = pt - pt_prev;
+  Eigen::Matrix<T, 2, 1> d2 = pt_next - pt;
+
+  if (is_cusp) {
+    d2 = -d2;
+    pt_next = pt + d2;
+  }
+
+  T det = d1[0] * d2[1] - d1[1] * d2[0];
+  if (ceres::abs(det) < (T)1e-4) {  // straight line
+    return Eigen::Matrix<T, 2, 1>(
+      (T)std::numeric_limits<double>::infinity(), (T)std::numeric_limits<double>::infinity());
+  }
+
+  // circle center is at the intersection of mirror axes of the segments:
+  // http://paulbourke.net/geometry/circlesphere/
+  // line intersection:
+  // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines
+  Eigen::Matrix<T, 2, 1> mid1 = (pt_prev + pt) / (T)2;
+  Eigen::Matrix<T, 2, 1> mid2 = (pt + pt_next) / (T)2;
+  Eigen::Matrix<T, 2, 1> n1(-d1[1], d1[0]);
+  Eigen::Matrix<T, 2, 1> n2(-d2[1], d2[0]);
+  T det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0];
+  T det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0];
+  Eigen::Matrix<T, 2, 1> center((det1 * n2[0] - det2 * n1[0]) / det,
+    (det1 * n2[1] - det2 * n1[1]) / det);
+  return center;
+}
+
+/**
+ * @brief Direction of a line which contains pt and is tangential to arc
+ * between pt_prev, pt, pt_next
+ * @param pt_prev Starting point of the arc
+ * @param pt Mid point of the arc, lying on the tangential line
+ * @param pt_next Last point of the arc
+ * @param is_cusp True if pt is a cusp point
+ * @result Tangential line direction.
+ * Note: the sign of tangentDir is undefined here, should be assigned in post-process
+ * depending on movement direction. Also, for speed reasons, direction vector is not normalized.
+ */
+template<typename T>
+inline Eigen::Matrix<T, 2, 1> tangentDir(
+  Eigen::Matrix<T, 2, 1> pt_prev,
+  Eigen::Matrix<T, 2, 1> pt,
+  Eigen::Matrix<T, 2, 1> pt_next,
+  bool is_cusp)
+{
+  Eigen::Matrix<T, 2, 1> center = arcCenter(pt_prev, pt, pt_next, is_cusp);
+  if (CERES_ISINF(center[0])) {  // straight line
+    Eigen::Matrix<T, 2, 1> d1 = pt - pt_prev;
+    Eigen::Matrix<T, 2, 1> d2 = pt_next - pt;
+
+    if (is_cusp) {
+      d2 = -d2;
+      pt_next = pt + d2;
+    }
+
+    Eigen::Matrix<T, 2, 1> result(pt_next[0] - pt_prev[0], pt_next[1] - pt_prev[1]);
+    if (result[0] == 0.0 && result[1] == 0.0) {  // a very rare edge situation
+      return Eigen::Matrix<T, 2, 1>(d1[1], -d1[0]);
+    }
+    return result;
+  }
+
+  // tangent is prependicular to (pt - center)
+  // Note: not determining + or - direction here, this should be handled at the caller side
+  return Eigen::Matrix<T, 2, 1>(center[1] - pt[1], pt[0] - center[0]);
+}
+
+}  // namespace nav2_constrained_smoother
+
+#endif  // NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_

navigation2/nav2_constrained_smoother/nav2_constrained_smoother.xml

@@ -0,0 +1,7 @@
+<class_libraries>
+  <library path="nav2_constrained_smoother">
+    <class name="nav2_constrained_smoother/ConstrainedSmoother" type="nav2_constrained_smoother::ConstrainedSmoother" base_class_type="nav2_core::Smoother">
+      <description>Increases smoothness and distance from obstacles of a path using Ceres solver optimization</description>
+    </class>
+  </library>
+</class_libraries>

navigation2/nav2_constrained_smoother/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_constrained_smoother</name>
+  <version>1.1.18</version>
+  <description>Ceres constrained smoother</description>
+  <maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>nav2_common</depend>
+  <depend>angles</depend>
+  <depend>rclcpp</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>nav2_core</depend>
+  <depend>pluginlib</depend>
+  <depend>libceres-dev</depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/nav2_constrained_smoother.xml" />
+  </export>
+</package>

navigation2/nav2_constrained_smoother/src/constrained_smoother.cpp

@@ -0,0 +1,167 @@
+// Copyright (c) 2021 RoboTech Vision
+// 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 <algorithm>
+#include <string>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "nav2_constrained_smoother/constrained_smoother.hpp"
+#include "nav2_core/exceptions.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+#include "tf2/utils.h"
+
+using nav2_util::declare_parameter_if_not_declared;
+using nav2_util::geometry_utils::euclidean_distance;
+using namespace nav2_costmap_2d;  // NOLINT
+
+namespace nav2_constrained_smoother
+{
+
+void ConstrainedSmoother::configure(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
+  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>)
+{
+  auto node = parent.lock();
+  if (!node) {
+    throw std::runtime_error("Unable to lock node!");
+  }
+
+  costmap_sub_ = costmap_sub;
+  tf_ = tf;
+  plugin_name_ = name;
+  logger_ = node->get_logger();
+
+  smoother_ = std::make_unique<nav2_constrained_smoother::Smoother>();
+  optimizer_params_.get(node.get(), name);
+  smoother_params_.get(node.get(), name);
+  smoother_->initialize(optimizer_params_);
+}
+
+void ConstrainedSmoother::cleanup()
+{
+  RCLCPP_INFO(
+    logger_,
+    "Cleaning up smoother: %s of type"
+    " nav2_constrained_smoother::ConstrainedSmoother",
+    plugin_name_.c_str());
+}
+
+void ConstrainedSmoother::activate()
+{
+  RCLCPP_INFO(
+    logger_,
+    "Activating smoother: %s of type "
+    "nav2_constrained_smoother::ConstrainedSmoother",
+    plugin_name_.c_str());
+}
+
+void ConstrainedSmoother::deactivate()
+{
+  RCLCPP_INFO(
+    logger_,
+    "Deactivating smoother: %s of type "
+    "nav2_constrained_smoother::ConstrainedSmoother",
+    plugin_name_.c_str());
+}
+
+bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Duration & max_time)
+{
+  if (path.poses.size() < 2) {
+    return true;
+  }
+
+  // populate smoother input with (x, y, forward/reverse dir)
+  std::vector<Eigen::Vector3d> path_world;
+  path_world.reserve(path.poses.size());
+  // smoother keeps record of start/end orientations so that it
+  // can use them in the final path, preventing degradation of these (often important) values
+  Eigen::Vector2d start_dir;
+  Eigen::Vector2d end_dir;
+  for (size_t i = 0; i < path.poses.size(); i++) {
+    auto & pose = path.poses[i].pose;
+    double angle = tf2::getYaw(pose.orientation);
+    Eigen::Vector2d orientation(cos(angle), sin(angle));
+    if (i == path.poses.size() - 1) {
+      // Note: `reversing` indicates the direction of the segment after the point and
+      // there is no segment after the last point. Most probably the value is irrelevant, but
+      // copying it from the last but one point, just to make it defined...
+      path_world.emplace_back(pose.position.x, pose.position.y, path_world.back()[2]);
+      end_dir = orientation;
+    } else {
+      auto & pos_next = path.poses[i + 1].pose.position;
+      Eigen::Vector2d mvmt(pos_next.x - pose.position.x, pos_next.y - pose.position.y);
+      // robot is considered reversing when angle between its orientation and movement direction
+      // is more than 90 degrees (i.e. dot product is less than 0)
+      bool reversing = smoother_params_.reversing_enabled && orientation.dot(mvmt) < 0;
+      // we transform boolean value of "reversing" into sign of movement direction (+1 or -1)
+      // to simplify further computations
+      path_world.emplace_back(pose.position.x, pose.position.y, reversing ? -1 : 1);
+      if (i == 0) {
+        start_dir = orientation;
+      } else if (i == 1 && !smoother_params_.keep_start_orientation) {
+        // overwrite start forward/reverse when orientation was set to be ignored
+        // note: start_dir is overwritten inside Smoother::upsampleAndPopulate() method
+        path_world[0][2] = path_world.back()[2];
+      }
+    }
+  }
+
+  smoother_params_.max_time = max_time.seconds();
+
+  // Smooth plan
+  auto costmap = costmap_sub_->getCostmap();
+  if (!smoother_->smooth(path_world, start_dir, end_dir, costmap.get(), smoother_params_)) {
+    RCLCPP_WARN(
+      logger_,
+      "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.",
+      plugin_name_.c_str());
+    throw new nav2_core::PlannerException(
+            "Failed to smooth plan, Ceres could not find a usable solution.");
+  }
+
+  // populate final path
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header = path.poses.front().header;
+  path.poses.clear();
+  path.poses.reserve(path_world.size());
+  for (auto & pw : path_world) {
+    pose.pose.position.x = pw[0];
+    pose.pose.position.y = pw[1];
+    pose.pose.orientation.z = sin(pw[2] / 2);
+    pose.pose.orientation.w = cos(pw[2] / 2);
+
+    path.poses.push_back(pose);
+  }
+
+  return true;
+}
+
+}  // namespace nav2_constrained_smoother
+
+// Register this smoother as a nav2_core plugin
+PLUGINLIB_EXPORT_CLASS(
+  nav2_constrained_smoother::ConstrainedSmoother,
+  nav2_core::Smoother)

navigation2/nav2_constrained_smoother/test/CMakeLists.txt

@@ -0,0 +1,20 @@
+ament_add_gtest(test_constrained_smoother
+  test_constrained_smoother.cpp
+)
+
+target_link_libraries(test_constrained_smoother
+  ${library_name}
+)
+ament_target_dependencies(test_constrained_smoother
+  ${dependencies}
+)
+
+ament_add_gtest(test_smoother_cost_function
+  test_smoother_cost_function.cpp
+)
+target_link_libraries(test_smoother_cost_function
+  ${library_name}
+)
+ament_target_dependencies(test_smoother_cost_function
+  ${dependencies}
+)

navigation2/nav2_constrained_smoother/test/test_smoother_cost_function.cpp

@@ -0,0 +1,141 @@
+// Copyright (c) 2021 RoboTech Vision
+//
+// 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 <string>
+#include <memory>
+#include <chrono>
+#include <iostream>
+#include <future>
+#include <thread>
+#include <algorithm>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_constrained_smoother/smoother_cost_function.hpp"
+
+class TestableSmootherCostFunction : nav2_constrained_smoother::SmootherCostFunction
+{
+public:
+  TestableSmootherCostFunction(
+    const Eigen::Vector2d & original_pos,
+    double next_to_last_length_ratio,
+    bool reversing,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> & costmap_interpolator,
+    const nav2_constrained_smoother::SmootherParams & params,
+    double costmap_weight)
+  : SmootherCostFunction(
+      original_pos, next_to_last_length_ratio, reversing,
+      costmap, costmap_interpolator,
+      params, costmap_weight)
+  {
+  }
+
+  inline double getCurvatureResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_next,
+    const Eigen::Vector2d & pt_prev) const
+  {
+    double r = 0.0;
+    addCurvatureResidual<double>(weight, pt, pt_next, pt_prev, r);
+    return r;
+  }
+};
+
+class Test : public ::testing::Test
+{
+protected:
+  void SetUp()
+  {
+  }
+};
+
+TEST_F(Test, testingCurvatureResidual)
+{
+  nav2_costmap_2d::Costmap2D costmap;
+  TestableSmootherCostFunction fn(
+    Eigen::Vector2d(1.0, 0.0), 1.0, false,
+    &costmap, std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(),
+    nav2_constrained_smoother::SmootherParams(), 0.0
+  );
+
+  // test for edge values
+  Eigen::Vector2d pt(1.0, 0.0);
+  Eigen::Vector2d pt_other(0.0, 0.0);
+  EXPECT_EQ(fn.getCurvatureResidual(0.0, pt, pt_other, pt_other), 0.0);
+
+  nav2_constrained_smoother::SmootherParams params_no_min_turning_radius;
+  params_no_min_turning_radius.max_curvature = 1.0f / 0.0;
+  TestableSmootherCostFunction fn_no_min_turning_radius(
+    Eigen::Vector2d(1.0, 0.0), 1.0, false,
+    &costmap, std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(),
+    params_no_min_turning_radius, 0.0
+  );
+  EXPECT_EQ(fn_no_min_turning_radius.getCurvatureResidual(1.0, pt, pt_other, pt_other), 0.0);
+}
+
+TEST_F(Test, testingUtils)
+{
+  Eigen::Vector2d pt(1.0, 0.0);
+  Eigen::Vector2d pt_prev(0.0, 0.0);
+  Eigen::Vector2d pt_next(0.0, 0.0);
+
+  // test for intermediate values
+  auto center = nav2_constrained_smoother::arcCenter(pt_prev, pt, pt_next, false);
+  // although in this situation the center would be at (0.5, 0.0),
+  // cases where pt_prev == pt_next are very rare and thus unhandled
+  // during the smoothing points will be separated (and thus made valid) by smoothness cost anyways
+  EXPECT_EQ(center[0], std::numeric_limits<double>::infinity());
+  EXPECT_EQ(center[1], std::numeric_limits<double>::infinity());
+
+  auto tangent =
+    nav2_constrained_smoother::tangentDir(pt_prev, pt, pt_next, false).normalized();
+  EXPECT_NEAR(tangent[0], 0, 1e-10);
+  EXPECT_NEAR(std::abs(tangent[1]), 1, 1e-10);
+
+  // no rotation when mid point is a cusp
+  tangent = nav2_constrained_smoother::tangentDir(pt_prev, pt, pt_next, true).normalized();
+  EXPECT_NEAR(std::abs(tangent[0]), 1, 1e-10);
+  EXPECT_NEAR(tangent[1], 0, 1e-10);
+
+  pt_prev[0] = -1.0;
+  // rotation is mathematically invalid, picking direction of a shorter segment
+  tangent = nav2_constrained_smoother::tangentDir(pt_prev, pt, pt_next, true).normalized();
+  EXPECT_NEAR(std::abs(tangent[0]), 1, 1e-10);
+  EXPECT_NEAR(tangent[1], 0, 1e-10);
+
+  pt_prev[0] = 0.0;
+  pt_next[0] = -1.0;
+  // rotation is mathematically invalid, picking direction of a shorter segment
+  tangent = nav2_constrained_smoother::tangentDir(pt_prev, pt, pt_next, true).normalized();
+  EXPECT_NEAR(std::abs(tangent[0]), 1, 1e-10);
+  EXPECT_NEAR(tangent[1], 0, 1e-10);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}