Adding new Nav2 Smoother: Savitzky-Golay Smoother

nav2_smoother/CMakeLists.txt

@@ -68,8 +68,18 @@ add_library(simple_smoother SHARED
 ament_target_dependencies(simple_smoother
   ${dependencies}
 )
+
+# Savitzky Golay Smoother plugin
+add_library(savitzky_golay_smoother SHARED
+  src/savitzky_golay_smoother.cpp
+)
+ament_target_dependencies(savitzky_golay_smoother
+  ${dependencies}
+)
+
 pluginlib_export_plugin_description_file(nav2_core plugins.xml)
 
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -82,7 +92,7 @@ endif()
 rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer")
 
 install(
-  TARGETS ${library_name} simple_smoother
+  TARGETS ${library_name} simple_smoother savitzky_golay_smoother
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -104,6 +114,6 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name} simple_smoother)
+ament_export_libraries(${library_name} simple_smoother savitzky_golay_smoother)
 ament_export_dependencies(${dependencies})
 ament_package()

nav2_smoother/README.md

@@ -7,3 +7,5 @@ A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface i
 See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins. 
 
 See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions.
+
+This package contains the Simple Smoother and Savitzky-Golay Smoother plugins.

nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp

@@ -0,0 +1,108 @@
+// Copyright (c) 2022, 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_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_
+#define NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_
+
+#include <cmath>
+#include <vector>
+#include <string>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "nav2_core/smoother.hpp"
+#include "nav2_smoother/smoother_utils.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "angles/angles.h"
+#include "tf2/utils.h"
+
+namespace nav2_smoother
+{
+
+/**
+ * @class nav2_smoother::SavitzkyGolaySmoother
+ * @brief A path smoother implementation using Savitzky Golay filters
+ */
+class SavitzkyGolaySmoother : public nav2_core::Smoother
+{
+public:
+  /**
+   * @brief A constructor for nav2_smoother::SavitzkyGolaySmoother
+   */
+  SavitzkyGolaySmoother() = default;
+
+  /**
+   * @brief A destructor for nav2_smoother::SavitzkyGolaySmoother
+   */
+  ~SavitzkyGolaySmoother() override = default;
+
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
+    std::string name, std::shared_ptr<tf2_ros::Buffer>,
+    std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
+    std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>) override;
+
+  /**
+   * @brief Method to cleanup resources.
+   */
+  void cleanup() override {}
+
+  /**
+   * @brief Method to activate smoother and any threads involved in execution.
+   */
+  void activate() override {}
+
+  /**
+   * @brief Method to deactivate smoother and any threads involved in execution.
+   */
+  void deactivate() override {}
+
+  /**
+   * @brief Method to smooth given path
+   *
+   * @param path In-out path to be smoothed
+   * @param max_time Maximum duration smoothing should take
+   * @return If smoothing was completed (true) or interrupted by time limit (false)
+   */
+  bool smooth(
+    nav_msgs::msg::Path & path,
+    const rclcpp::Duration & max_time) override;
+
+protected:
+  /**
+   * @brief Smoother method - does the smoothing on a segment
+   * @param path Reference to path
+   * @param reversing_segment Return if this is a reversing segment
+   * @param costmap Pointer to minimal costmap
+   * @param max_time Maximum time to compute, stop early if over limit
+   * @return If smoothing was successful
+   */
+  bool smoothImpl(
+    nav_msgs::msg::Path & path,
+    bool & reversing_segment);
+
+  bool do_refinement_;
+  int refinement_num_;
+  rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")};
+};
+
+}  // namespace nav2_smoother
+
+#endif  // NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_

nav2_smoother/include/nav2_smoother/simple_smoother.hpp

@@ -24,6 +24,7 @@
 #include <utility>
 
 #include "nav2_core/smoother.hpp"
+#include "nav2_smoother/smoother_utils.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -35,19 +36,6 @@
 namespace nav2_smoother
 {
 
-/**
- * @class nav2_smoother::PathSegment
- * @brief A segment of a path in start/end indices
- */
-struct PathSegment
-{
-  unsigned int start;
-  unsigned int end;
-};
-
-typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
-typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
-
 /**
  * @class nav2_smoother::SimpleSmoother
  * @brief A path smoother implementation
@@ -132,23 +120,6 @@ class SimpleSmoother : public nav2_core::Smoother
     geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
     const double & value);
 
-  /**
-   * @brief Finds the starting and end indices of path segments where
-   * the robot is traveling in the same direction (e.g. forward vs reverse)
-   * @param path Path in which to look for cusps
-   * @return Set of index pairs for each segment of the path in a given direction
-   */
-  std::vector<PathSegment> findDirectionalPathSegments(const nav_msgs::msg::Path & path);
-
-  /**
-   * @brief For a given path, update the path point orientations based on smoothing
-   * @param path Path to approximate the path orientation in
-   * @param reversing_segment Return if this is a reversing segment
-   */
-  inline void updateApproximatePathOrientations(
-    nav_msgs::msg::Path & path,
-    bool & reversing_segment);
-
   double tolerance_, data_w_, smooth_w_;
   int max_its_, refinement_ctr_;
   bool do_refinement_;

nav2_smoother/include/nav2_smoother/smoother_utils.hpp

@@ -0,0 +1,132 @@
+// Copyright (c) 2022, 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_SMOOTHER__SMOOTHER_UTILS_HPP_
+#define NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_
+
+#include <cmath>
+#include <vector>
+#include <string>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "nav2_core/smoother.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "angles/angles.h"
+#include "tf2/utils.h"
+
+namespace smoother_utils
+{
+
+/**
+ * @class nav2_smoother::PathSegment
+ * @brief A segment of a path in start/end indices
+ */
+struct PathSegment
+{
+  unsigned int start;
+  unsigned int end;
+};
+
+typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
+typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
+
+inline std::vector<PathSegment> findDirectionalPathSegments(
+  const nav_msgs::msg::Path & path)
+{
+  std::vector<PathSegment> segments;
+  PathSegment curr_segment;
+  curr_segment.start = 0;
+
+  // Iterating through the path to determine the position of the cusp
+  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+    // We have two vectors for the dot product OA and AB. Determining the vectors.
+    double oa_x = path.poses[idx].pose.position.x -
+      path.poses[idx - 1].pose.position.x;
+    double oa_y = path.poses[idx].pose.position.y -
+      path.poses[idx - 1].pose.position.y;
+    double ab_x = path.poses[idx + 1].pose.position.x -
+      path.poses[idx].pose.position.x;
+    double ab_y = path.poses[idx + 1].pose.position.y -
+      path.poses[idx].pose.position.y;
+
+    // Checking for the existance of cusp, in the path, using the dot product.
+    double dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+    if (dot_product < 0.0) {
+      curr_segment.end = idx;
+      segments.push_back(curr_segment);
+      curr_segment.start = idx;
+    }
+
+    // Checking for the existance of a differential rotation in place.
+    double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation);
+    double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation);
+    double dtheta = angles::shortest_angular_distance(cur_theta, next_theta);
+    if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) {
+      curr_segment.end = idx;
+      segments.push_back(curr_segment);
+      curr_segment.start = idx;
+    }
+  }
+
+  curr_segment.end = path.poses.size() - 1;
+  segments.push_back(curr_segment);
+  return segments;
+}
+
+inline void updateApproximatePathOrientations(
+  nav_msgs::msg::Path & path,
+  bool & reversing_segment)
+{
+  double dx, dy, theta, pt_yaw;
+  reversing_segment = false;
+
+  // Find if this path segment is in reverse
+  dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
+  dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
+  theta = atan2(dy, dx);
+  pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);
+  if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
+    reversing_segment = true;
+  }
+
+  // Find the angle relative the path position vectors
+  for (unsigned int i = 0; i != path.poses.size() - 1; i++) {
+    dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
+    dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
+    theta = atan2(dy, dx);
+
+    // If points are overlapping, pass
+    if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) {
+      continue;
+    }
+
+    // Flip the angle if this path segment is in reverse
+    if (reversing_segment) {
+      theta += M_PI;  // orientationAroundZAxis will normalize
+    }
+
+    path.poses[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
+  }
+}
+
+}  // namespace smoother_utils
+
+#endif  // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_

nav2_smoother/plugins.xml

@@ -4,4 +4,10 @@
       <description>Does a simple smoothing process with collision checking</description>
     </class>
   </library>
+
+  <library path="savitzky_golay_smoother">
+    <class type="nav2_smoother::SavitzkyGolaySmoother" base_class_type="nav2_core::Smoother">
+      <description>Does Savitzky-Golay smoothing</description>
+    </class>
+  </library>
 </class_libraries>

nav2_smoother/src/nav2_smoother.cpp

@@ -1,5 +1,6 @@
 // Copyright (c) 2019 RoboTech Vision
 // Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2022 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.