Created path segmenter controller

toid_spinner_controller/include/toid_spinner_controller/path_handler.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <memory>
+#include <rclcpp/logger.hpp>
+#include <tf2_ros/buffer.hpp>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "tf2/time.hpp"
+
+namespace toid {
+
+class PathHandler {
+public:
+  PathHandler(tf2::Duration tranform_tolerance,
+              std::shared_ptr<tf2_ros::Buffer> tf,
+              std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
+
+  ~PathHandler() = default;
+
+  nav_msgs::msg::Path
+  transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose,
+                      double max_robot_pose_search_dist,
+                      double angle_tolerance);
+
+  bool tranformPose(const std::string frame,
+                    const geometry_msgs::msg::PoseStamped &in_pose,
+                    geometry_msgs::msg::PoseStamped &out_pose);
+
+  void setPlan(const nav_msgs::msg::Path &path) { global_plan_ = path; }
+
+  nav_msgs::msg::Path getPlan() { return global_plan_; }
+
+protected:
+
+  double dist2(const geometry_msgs::msg::PoseStamped &p1, const geometry_msgs::msg::PoseStamped &p2);
+
+  rclcpp::Logger logger_{rclcpp::get_logger("MultiPlexPathHandler")};
+  tf2::Duration transform_tolerance_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav_msgs::msg::Path global_plan_;
+};
+
+} // namespace toid

toid_spinner_controller/include/toid_spinner_controller/toid_spinner_controller.hpp

@@ -0,0 +1,82 @@
+#pragma once
+#include <memory>
+
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav2_controller/plugins/position_goal_checker.hpp"
+#include "nav2_core/controller.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "toid_spinner_controller/path_handler.hpp"
+
+namespace toid {
+class SpinnerController : public nav2_core::Controller {
+public:
+  SpinnerController();
+  ~SpinnerController() override = default;
+
+  void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
+                 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+                 std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) override;
+
+  void cleanup() override;
+
+  void activate() override;
+
+  void deactivate() override;
+
+  void setPlan(const nav_msgs::msg::Path &path) override;
+
+  bool cancel() override;
+
+  geometry_msgs::msg::TwistStamped
+  computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
+                          const geometry_msgs::msg::Twist &velocity,
+                          nav2_core::GoalChecker *goal_checker) override;
+
+  geometry_msgs::msg::TwistStamped
+  computeRotateToHeadingCommand(const double &angular_distance_to_heading,
+                                const geometry_msgs::msg::PoseStamped &pose);
+
+  void setSpeedLimit(const double &speed_limit,
+                     const bool &percentage) override;
+
+protected:
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+  rclcpp::Logger logger_{rclcpp::get_logger("SpinnerController")};
+  rclcpp::Clock::SharedPtr clock_;
+  std::unique_ptr<PathHandler> path_handler_;
+  std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
+  std::unique_ptr<
+      nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
+      collision_checker_;
+  rclcpp_lifecycle::LifecyclePublisher<
+      geometry_msgs::msg::PointStamped>::SharedPtr closest_pt_pub_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::string plugin_name_;
+  bool new_path_recieved = false;
+  double last_angular_vel_;
+
+  pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
+  nav2_core::Controller::Ptr primary_controller_;
+
+  // Params
+  double angular_dist_threshold_;
+  double angular_disengage_threshold_;
+  double forward_sampling_distance_;
+  double max_angular_accel_;
+  double rotate_to_heading_;
+
+  double control_duration_;
+};
+} // namespace toid