Created path segmenter controller

toid_spinner_controller/src/toid_spinner_controller.cpp

@@ -0,0 +1,206 @@
+#include "toid_spinner_controller/toid_spinner_controller.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "pluginlib/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+#include "toid_spinner_controller/path_handler.hpp"
+
+#include <algorithm>
+#include <angles/angles.h>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <rclcpp/duration.hpp>
+#include <tf2/time.hpp>
+#include <tf2/utils.hpp>
+
+namespace toid {
+SpinnerController::SpinnerController()
+    : lp_loader_("nav2_core", "nav2_core::Controller") {}
+
+void SpinnerController::configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name,
+    std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
+
+  position_goal_checker_ =
+      std::make_unique<nav2_controller::PositionGoalChecker>();
+  position_goal_checker_->initialize(parent, ".position_checker", costmap_ros);
+  plugin_name_ = name;
+  node_ = parent;
+  auto node = node_.lock();
+
+  tf_ = tf;
+  logger_ = node->get_logger();
+  clock_ = node->get_clock();
+
+  std::string primary_controller;
+  double control_frequency;
+
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".angular_dist_threshold",
+      rclcpp::ParameterValue(0.785));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".angular_disengage_threshold",
+      rclcpp::ParameterValue(0.785 / 2.0));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".forward_sampling_distance",
+      rclcpp::ParameterValue(0.5));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".rotate_to_heading", rclcpp::ParameterValue(1.0));
+  nav2_util::declare_parameter_if_not_declared(
+      node, plugin_name_ + ".primary_controller.plugin",
+      rclcpp::PARAMETER_STRING);
+
+  node->get_parameter(plugin_name_ + ".angular_dist_threshold",
+                      angular_dist_threshold_);
+  node->get_parameter(plugin_name_ + ".angular_disengage_threshold",
+                      angular_disengage_threshold_);
+  node->get_parameter(plugin_name_ + ".forward_sampling_distance",
+                      forward_sampling_distance_);
+  node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
+  node->get_parameter(plugin_name_ + ".rotate_to_heading", rotate_to_heading_);
+
+  primary_controller =
+      node->get_parameter(plugin_name_ + ".primary_controller.plugin")
+          .as_string();
+
+  node->get_parameter("controller_frequency", control_frequency);
+  control_duration_ = 1.0 / control_frequency;
+
+  try {
+    primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
+    RCLCPP_INFO(logger_,
+                "Created internal controller for multiplexer: %s of type %s",
+                plugin_name_.c_str(), primary_controller.c_str());
+  } catch (const pluginlib::PluginlibException &ex) {
+    RCLCPP_FATAL(
+        logger_,
+        "Failed to create primary controller for multiplexer. Exception: %s",
+        ex.what());
+    return;
+  }
+  primary_controller_->configure(parent, name + ".primary_controller", tf,
+                                 costmap_ros);
+
+  path_handler_ = std::make_unique<PathHandler>(tf2::durationFromSec(0.1), tf_,
+                                                costmap_ros);
+
+  closest_pt_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
+      "start_point", 1);
+}
+
+void SpinnerController::cleanup() {
+  RCLCPP_INFO(logger_, "Cleaning SpinnerController");
+  primary_controller_->cleanup();
+  primary_controller_->reset();
+  position_goal_checker_.reset();
+  path_handler_.reset();
+  closest_pt_pub_.reset();
+}
+
+void SpinnerController::activate() {
+  RCLCPP_INFO(logger_, "Activating SpinnerController");
+  last_angular_vel_ = std::numeric_limits<double>::max();
+  new_path_recieved = false;
+  auto node = node_.lock();
+  closest_pt_pub_->on_activate();
+  primary_controller_->activate();
+  position_goal_checker_->reset();
+}
+
+void SpinnerController::deactivate() {
+  RCLCPP_INFO(logger_, "Deactivating SpinnerController");
+  closest_pt_pub_.reset();
+  primary_controller_->deactivate();
+  closest_pt_pub_->on_deactivate();
+}
+
+void SpinnerController::setPlan(const nav_msgs::msg::Path &path) {
+  path_handler_->setPlan(path);
+  new_path_recieved = true;
+  primary_controller_->setPlan(path);
+}
+
+geometry_msgs::msg::TwistStamped SpinnerController::computeVelocityCommands(
+    const geometry_msgs::msg::PoseStamped &pose,
+    const geometry_msgs::msg::Twist &velocity,
+    nav2_core::GoalChecker *goal_checker) {
+
+  auto path = path_handler_->transformGlobalPlan(pose, 1.0, M_PI / 8);
+
+  if (path.poses.size() > 0) {
+    geometry_msgs::msg::PointStamped pt;
+    const auto &pose = path.poses.back();
+    pt.point = pose.pose.position;
+    pt.header.frame_id = path.header.frame_id;
+    pt.header.stamp = path.header.stamp;
+    closest_pt_pub_->publish(pt);
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  path_handler_->tranformPose(path.header.frame_id, pose, robot_pose);
+  const double robot_angle = tf2::getYaw(robot_pose.pose.orientation);
+
+  if (path.poses.front().pose.position == path.poses.back().pose.position) {
+    const double path_angle = tf2::getYaw(path.poses.back().pose.orientation);
+    const double angle =
+        angles::shortest_angular_distance(robot_angle, path_angle);
+    return computeRotateToHeadingCommand(angle, pose);
+  }
+
+  if (new_path_recieved) {
+    new_path_recieved = false;
+    primary_controller_->setPlan(path);
+  }
+
+  auto twist = primary_controller_->computeVelocityCommands(pose, velocity,
+                                                            goal_checker);
+  last_angular_vel_ = twist.twist.angular.z;
+  return twist;
+}
+
+geometry_msgs::msg::TwistStamped
+SpinnerController::computeRotateToHeadingCommand(
+    const double &angular_distance_to_heading,
+    const geometry_msgs::msg::PoseStamped &pose) {
+  geometry_msgs::msg::TwistStamped cmd;
+
+  const double last_angular_vel =
+      (last_angular_vel_ == std::numeric_limits<double>::max())
+          ? 0
+          : last_angular_vel_;
+  const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
+  const double speed_upper_bound =
+      last_angular_vel + control_duration_ * max_angular_accel_;
+  const double speed_lower_bound =
+      last_angular_vel - control_duration_ * max_angular_accel_;
+
+  const double speed_until_overshoot =
+      0.9 * std::sqrt(2.0 * max_angular_accel_ *
+                    std::fabs(angular_distance_to_heading));
+  const double requested_speed = sign * std::fmin(speed_until_overshoot,rotate_to_heading_);
+  const double speed =
+      std::clamp(requested_speed, speed_lower_bound, speed_upper_bound);
+
+  cmd.header.frame_id = pose.header.frame_id;
+  cmd.header.stamp = pose.header.stamp;
+  cmd.twist.angular.z = speed;
+  last_angular_vel_ = speed;
+  return cmd;
+}
+
+void SpinnerController::setSpeedLimit(const double &speed_limit,
+                                      const bool &percentage) {
+  primary_controller_->setSpeedLimit(speed_limit, percentage);
+}
+
+bool SpinnerController::cancel() { return primary_controller_->cancel(); }
+
+} // namespace toid
+
+PLUGINLIB_EXPORT_CLASS(toid::SpinnerController, nav2_core::Controller)