add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -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)