Added move coords behavior

toid_behaviors/include/toid_behaviors/move_coords.hpp

@@ -0,0 +1,64 @@
+#pragma once
+
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_behaviors/scl.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
+
+#include "geometry_msgs/msg/pose.hpp"
+
+namespace toid
+{
+using MoveAction = toid_msgs::action::SimpleMoveCoords;
+using namespace nav2_behaviors;
+
+class MoveCoords : public SimpleMove<MoveAction>
+{
+public:
+  MoveCoords();
+  ~MoveCoords();
+
+  void configureCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+    const geometry_msgs::msg::Twist & vel) override;
+
+  ResultStatus updateVel(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
+    geometry_msgs::msg::Twist & out_vel) override;
+  
+  virtual nav2_core::CostmapInfoType getResourceInfo() override {
+    return nav2_core::CostmapInfoType::NONE;
+  }
+
+protected:
+
+  SmoothControlLaw scl;
+
+  //Goal
+  geometry_msgs::msg::Pose target_pose_;
+  double target_angle_;
+  double target_sign_;
+  bool backwards_;
+
+  //State
+  double last_speed_;
+
+  //Config
+  double max_vel_accel_;
+  double max_vel_decel_;
+  double max_vel_speed_;
+  double min_vel_speed_;
+
+  double max_angular_speed_;
+
+  double kp_;
+  double k_phi_;
+  double k_delta_;
+  double beta_;
+  double lambda_;
+  double slowdown_radius_;
+};
+
+}  // namespace toid
+

toid_behaviors/include/toid_behaviors/scl.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace toid
+{
+class SmoothControlLaw
+{
+public:
+  double k_phi = 5.0;
+  double k_delta = 2.0;
+  double bbeta = 0.4;
+  double lam = 2.0;
+
+  double slowdown_radius = 0.20;
+  double v_linear_min = 0.05;
+  double v_linear_max = 0.1;
+  double v_angular_max = 2.0;
+
+  void egocentric_polar(
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
+    double & r, double & phi, double & delta);
+
+  double curvature(double r, double phi, double delta);
+
+  void calculate_vel(
+    const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current,
+    geometry_msgs::msg::Twist & out_speed, bool backwards = false);
+};
+}  // namespace toid