Added simple rotate behavior

toid_behaviors/include/toid_behaviors/simple_move.hpp

@@ -0,0 +1,104 @@
+#pragma once
+#include "nav2_behaviors/timed_behavior.hpp"
+#include "nav2_core/behavior.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/twist_publisher.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "toid_msgs/action/simple_rotate.hpp"
+
+namespace toid
+{
+
+template <typename ActionT>
+class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
+{
+public:
+  virtual void configureCB() {}
+  virtual void cleanupCB() {}
+
+  virtual void activateCB() {}
+  virtual void deactivateCB() {}
+
+  virtual nav2_behaviors::ResultStatus onStart(
+    const std::shared_ptr<const typename ActionT::Goal> command,
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) = 0;
+
+  virtual nav2_behaviors::ResultStatus updateVel(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
+    geometry_msgs::msg::Twist & out_vel) = 0;
+
+  void onConfigure() override
+  {
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
+    nav2_util::declare_parameter_if_not_declared(
+      node, "odom_topic", rclcpp::ParameterValue("/odom"));
+    std::string odom_topic_name = node->get_parameter("odom_topic").as_string();
+    odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
+      odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
+        std::lock_guard lock(mut_);
+        current_pose_ = msg.pose.pose;
+        current_vel_ = msg.twist.twist;
+      });
+    control_duration_ = 1.0 / this->cycle_frequency_;
+  }
+
+  void onCleanup() override
+  {
+    odom_sub_.reset();
+    cleanupCB();
+  }
+
+  void activate() override
+  {
+    nav2_behaviors::TimedBehavior<ActionT>::activate();
+    activateCB();
+  }
+
+  void deactivate() override
+  {
+    nav2_behaviors::TimedBehavior<ActionT>::deactivate();
+    deactivateCB();
+  }
+
+  nav2_behaviors::ResultStatus onRun(
+    const std::shared_ptr<const typename ActionT::Goal> command) override
+  {
+    geometry_msgs::msg::Pose pose;
+    geometry_msgs::msg::Twist vel;
+    {
+      std::lock_guard lock(mut_);
+      pose = current_pose_;
+      vel = current_vel_;
+    }
+    return onStart(command, pose, vel);
+  }
+
+  nav2_behaviors::ResultStatus onCycleUpdate() override
+  {
+    geometry_msgs::msg::Pose pose;
+    geometry_msgs::msg::Twist vel;
+    geometry_msgs::msg::Twist out_vel;
+    auto vel_p = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    {
+      std::lock_guard lock(mut_);
+      pose = current_pose_;
+      vel = current_vel_;
+    }
+    nav2_behaviors::ResultStatus r = updateVel(pose, vel, out_vel);
+    vel_p->twist = out_vel;
+    vel_p->header.stamp = this->clock_->now();
+    vel_p->header.frame_id = this->robot_base_frame_;
+    this->vel_pub_->publish(std::move(vel_p));
+    return r;
+  }
+
+protected:
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  geometry_msgs::msg::Pose current_pose_;
+  geometry_msgs::msg::Twist current_vel_;
+  std::recursive_mutex mut_;
+  double control_duration_;
+};
+
+}  // namespace toid

toid_behaviors/include/toid_behaviors/simple_rotate.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_msgs/action/simple_rotate.h"
+
+namespace toid
+{
+using RotateAction = toid_msgs::action::SimpleRotate;
+using namespace nav2_behaviors;
+
+class SimpleRotateBehavior : public SimpleMove<RotateAction>
+{
+public:
+  SimpleRotateBehavior();
+
+  void configureCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const RotateAction::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;
+
+protected:
+
+  //Goal
+  double target_angle_;
+  double min_turn_angle_;
+  double initial_direction_;
+
+  //State
+  double angular_speed_;
+  double last_angle_;
+
+  //Config
+  double max_angular_speed_;
+  double min_angular_speed_;
+  double max_angular_accel_;
+};
+
+}  // namespace toid