add humble-navigation2

navigation2/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp

@@ -0,0 +1,165 @@
+// Copyright (c) 2021 RoboTech Vision
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
+#define NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <unordered_map>
+#include <vector>
+
+#include "nav2_core/smoother.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
+#include "nav2_costmap_2d/footprint_subscriber.hpp"
+#include "nav2_msgs/action/smooth_path.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "pluginlib/class_loader.hpp"
+
+namespace nav2_smoother
+{
+
+/**
+ * @class nav2_smoother::SmootherServer
+ * @brief This class hosts variety of plugins of different algorithms to
+ * smooth or refine a path from the exposed SmoothPath action server.
+ */
+class SmootherServer : public nav2_util::LifecycleNode
+{
+public:
+  using SmootherMap = std::unordered_map<std::string, nav2_core::Smoother::Ptr>;
+
+  /**
+   * @brief A constructor for nav2_smoother::SmootherServer
+   * @param options Additional options to control creation of the node.
+   */
+  explicit SmootherServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /**
+   * @brief Destructor for nav2_smoother::SmootherServer
+   */
+  ~SmootherServer();
+
+protected:
+  /**
+   * @brief Configures smoother parameters and member variables
+   *
+   * Configures smoother plugin and costmap; Initialize odom subscriber,
+   * velocity publisher and smooth path action server.
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   * @throw pluginlib::PluginlibException When failed to initialize smoother
+   * plugin
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Loads smoother plugins from parameter file
+   * @return bool if successfully loaded the plugins
+   */
+  bool loadSmootherPlugins();
+
+  /**
+   * @brief Activates member variables
+   *
+   * Activates smoother, costmap, velocity publisher and smooth path action
+   * server
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Deactivates member variables
+   *
+   * Deactivates smooth path action server, smoother, costmap and velocity
+   * publisher. Before calling deactivate state, velocity is being set to zero.
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Calls clean up states and resets member variables.
+   *
+   * Smoother and costmap clean up state is called, and resets rest of the
+   * variables
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Called when in Shutdown state
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  using Action = nav2_msgs::action::SmoothPath;
+  using ActionServer = nav2_util::SimpleActionServer<Action>;
+
+  /**
+   * @brief SmoothPath action server callback. Handles action server updates and
+   * spins server until goal is reached
+   *
+   * Provides global path to smoother received from action client. Local
+   * section of the path is optimized using smoother.
+   * @throw nav2_core::PlannerException
+   */
+  void smoothPlan();
+
+  /**
+   * @brief Find the valid smoother ID name for the given request
+   *
+   * @param c_name The requested smoother name
+   * @param name Reference to the name to use for control if any valid available
+   * @return bool Whether it found a valid smoother to use
+   */
+  bool findSmootherId(const std::string & c_name, std::string & name);
+
+  // Our action server implements the SmoothPath action
+  std::unique_ptr<ActionServer> action_server_;
+
+  // Transforms
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
+
+  // Publishers and subscribers
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
+
+  // Smoother Plugins
+  pluginlib::ClassLoader<nav2_core::Smoother> lp_loader_;
+  SmootherMap smoothers_;
+  std::vector<std::string> default_ids_;
+  std::vector<std::string> default_types_;
+  std::vector<std::string> smoother_ids_;
+  std::vector<std::string> smoother_types_;
+  std::string smoother_ids_concat_, current_smoother_;
+
+  // Utilities
+  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
+};
+
+}  // namespace nav2_smoother
+
+#endif  // NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_

navigation2/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_

navigation2/nav2_smoother/include/nav2_smoother/simple_smoother.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__SIMPLE_SMOOTHER_HPP_
+#define NAV2_SMOOTHER__SIMPLE_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::SimpleSmoother
+ * @brief A path smoother implementation
+ */
+class SimpleSmoother : public nav2_core::Smoother
+{
+public:
+  /**
+   * @brief A constructor for nav2_smoother::SimpleSmoother
+   */
+  SimpleSmoother() = default;
+
+  /**
+   * @brief A destructor for nav2_smoother::SimpleSmoother
+   */
+  ~SimpleSmoother() 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 {costmap_sub_.reset();}
+
+  /**
+   * @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,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const double & max_time);
+
+  /**
+   * @brief Get the field value for a given dimension
+   * @param msg Current pose to sample
+   * @param dim Dimension ID of interest
+   * @return dim value
+   */
+  inline double getFieldByDim(
+    const geometry_msgs::msg::PoseStamped & msg,
+    const unsigned int & dim);
+
+  /**
+   * @brief Set the field value for a given dimension
+   * @param msg Current pose to sample
+   * @param dim Dimension ID of interest
+   * @param value to set the dimention to for the pose
+   */
+  inline void setFieldByDim(
+    geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
+    const double & value);
+
+  double tolerance_, data_w_, smooth_w_;
+  int max_its_, refinement_ctr_;
+  bool do_refinement_;
+  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")};
+};
+
+}  // namespace nav2_smoother
+
+#endif  // NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_

navigation2/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_