Added rotate acorn and misc changes to approach acorns

toid_behaviors/CMakeLists.txt

@@ -33,6 +33,7 @@ set(
   src/simple_rotate.cpp
   src/simple_translate_x.cpp
   src/approach_acorns.cpp
+  src/rotate_acorns.cpp
 )
 
 find_package(ament_cmake REQUIRED)

toid_behaviors/include/toid_behaviors/rotate_acorns.hpp

@@ -0,0 +1,80 @@
+#pragma once
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "toid_behaviors/scl.hpp"
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_behaviors/rolling_average.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+
+namespace toid
+{
+using RotateAction = toid_msgs::action::SimpleRotate;
+using PoseStamped = geometry_msgs::msg::PoseStamped;
+using namespace nav2_behaviors;
+
+class RotateAcorns : public SimpleMove<RotateAction>
+{
+public:
+  RotateAcorns();
+  ~RotateAcorns();
+
+  void configureCB() override;
+
+  void activateCB() override;
+  void deactivateCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+    const geometry_msgs::msg::Twist & vel) override;
+
+  bool collisionDetection(
+    const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose);
+
+  void acorn_position_cb(PoseStamped::ConstSharedPtr msg);
+
+  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::LOCAL;
+  }
+
+  void calc_err_and_sign(
+    double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign);
+
+  double check_space(const geometry_msgs::msg::Pose pose, const double e, const double sign);
+
+  double calc_speed(const double err, const double sign, const double angular_speed);
+
+protected:
+  SmoothControlLaw scl_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_;
+  double new_target_angle_;
+  std::shared_mutex mutex_;
+  RollingAverage avg_ = RollingAverage(10);
+
+
+  //Goal
+  double target_angle_;
+  double min_turn_angle_;
+  double initial_direction_;
+  unsigned char mode_;
+
+  //State
+  double angular_speed_;
+  double last_angle_;
+  double last_time_;
+  double distance_ = 1.0;
+
+  //Config
+  double max_angular_speed_;
+  double min_angular_speed_;
+  double max_angular_accel_;
+  double max_angular_decel_;
+  double lookahead_;
+};
+
+}  // namespace toid

toid_behaviors/src/approach_acorns.cpp

@@ -73,9 +73,10 @@ void ApproachAcorns::activateCB()
   distance_ = 1.0;
 }
 
-void ApproachAcorns::deactivateCB() { 
-    acorn_pose_sub_.reset(); 
-    target_pose_pub_->on_deactivate();
+void ApproachAcorns::deactivateCB()
+{
+  acorn_pose_sub_.reset();
+  target_pose_pub_->on_deactivate();
 }
 
 void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
@@ -96,30 +97,34 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
     return;
   }
 
+  // Smooth out approach
+  if (x * x + y * y < 0.42) {
+    return;
+  }
+
   double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
 
-  if(tf2::getYaw(pose_local.pose.orientation) > 0) {
+  if (tf2::getYaw(pose_local.pose.orientation) > 0) {
     yaw += M_PI;
   }
 
-  yaw += M_PI/2;
+  yaw += M_PI / 2;
 
-
-  pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * + 0.005;
-  pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * + 0.005;
-  tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
+  pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * -0.005;
+  pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * -0.005;
+  tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation);
 
   yaw = angles::normalize_angle(yaw);
 
   std::lock_guard _lock(mutex_);
-  auto[a,b,c] = avg_.push(pose_global.pose);
+  auto [a, b, c] = avg_.push(pose_global.pose);
   distance_ = x * x + y * y;
-  new_target_pose_.position.x = a; 
-  new_target_pose_.position.y = b; 
-  tf2::convert(tf2::Quaternion::createFromRPY(0,0, c), new_target_pose_.orientation);
+  new_target_pose_.position.x = a;
+  new_target_pose_.position.y = b;
+  tf2::convert(tf2::Quaternion::createFromRPY(0, 0, c), new_target_pose_.orientation);
   new_target_angle_ = c;
 
-  if(debug_marker_) {
+  if (debug_marker_) {
     visualization_msgs::msg::Marker marker;
     marker.lifetime.sec = 1.0;
     marker.header = pose_global.header;
@@ -134,14 +139,12 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
     marker.color.b = 0;
     target_pose_pub_->publish(marker);
   }
-  
 }
 
 ResultStatus ApproachAcorns::onStart(
   const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose &,
   const geometry_msgs::msg::Twist & vel)
 {
-
   std::lock_guard _lock(mutex_);
   distance_ = 1.0;
   new_target_pose_.position.x = command->x;
@@ -235,7 +238,6 @@ ResultStatus ApproachAcorns::updateVel(
   const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
   geometry_msgs::msg::Twist & out_vel)
 {
-
   {
     std::lock_guard _lock(mutex_);
     target_pose_ = new_target_pose_;
@@ -247,7 +249,7 @@ ResultStatus ApproachAcorns::updateVel(
   const double current_yaw = tf2::getYaw(pose.orientation);
   double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
 
-  if (dist_left <= 0.001 && std::fabs(dist_left) <= 0.005) {
+  if (dist_left <= 0.001 && std::fabs(angle_dist) <= 0.030) {
     out_vel.linear.x = 0;
     out_vel.angular.z = 0;
     return ResultStatus{Status::SUCCEEDED};
@@ -279,6 +281,13 @@ ResultStatus ApproachAcorns::updateVel(
     last_speed_ = out_vel.linear.x;
     return ResultStatus{Status::RUNNING};
   }
+  
+  if (dist_left <= 0.001) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    last_speed_ = out_vel.linear.x;
+    return ResultStatus{Status::RUNNING};
+  }
 
   scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
   scl_.calculate_vel(target_pose_, pose, out_vel, backwards_);

toid_behaviors/src/rotate_acorns.cpp

@@ -0,0 +1,229 @@
+#include "toid_behaviors/rotate_acorns.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "tf2/convert.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+RotateAcorns::RotateAcorns() : SimpleMove<RotateAction>() {}
+RotateAcorns::~RotateAcorns() {}
+
+void RotateAcorns::configureCB()
+{
+  auto node = node_.lock();
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.0));
+
+  node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.2));
+  node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_accel", rclcpp::ParameterValue(4.0));
+  node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(4.0));
+  node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5));
+  node->get_parameter(behavior_name_ + ".lookahead", lookahead_);
+}
+
+
+void RotateAcorns::activateCB()
+{
+  auto node = node_.lock();
+  using namespace std::placeholders;
+  acorn_pose_sub_ = node->create_subscription<PoseStamped>(
+    "closest_acorn", rclcpp::QoS(1), std::bind(&RotateAcorns::acorn_position_cb, this, _1));
+  distance_ = 1.0;
+}
+
+void RotateAcorns::deactivateCB() { 
+    acorn_pose_sub_.reset(); 
+}
+
+void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
+{
+  geometry_msgs::msg::PoseStamped pose_local;
+  geometry_msgs::msg::PoseStamped pose_global;
+  try {
+    pose_local = tf_->transform(*msg, robot_base_frame_);
+    pose_global = tf_->transform(*msg, global_frame_);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]...");
+    return;
+  }
+
+  double x = pose_local.pose.position.x;
+  double y = pose_local.pose.position.y;
+  if (x * x + y * y > distance_ + 0.005) {
+    return;
+  }
+
+  double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
+
+  if(tf2::getYaw(pose_local.pose.orientation) > 0) {
+    yaw += M_PI;
+  }
+
+  yaw += M_PI/2;
+
+
+  pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * + 0.005;
+  pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * + 0.005;
+  tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
+
+  yaw = angles::normalize_angle(yaw);
+
+  std::lock_guard _lock(mutex_);
+  auto[a,b,c] = avg_.push(pose_global.pose);
+  distance_ = x * x + y * y;
+  new_target_angle_ = c;
+  
+}
+
+ResultStatus RotateAcorns::onStart(
+  const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  std::lock_guard _lock(mutex_);
+  target_angle_ = new_target_angle_;
+  min_turn_angle_ = abs(command->min_angle);
+  initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
+  max_angular_speed_ = command->max_speed;
+  mode_ = command->mode;
+  avg_.reset();
+
+  if (command->max_speed == 0) {
+    auto node = node_.lock();
+    node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
+  }
+
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  pose2d.theta = initial_direction_;
+  local_collision_checker_->isCollisionFree(pose2d, true);
+
+  last_angle_ = tf2::getYaw(pose.orientation);
+
+  angular_speed_ = vel.angular.z;
+  last_time_ = clock_->now().seconds();
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+void RotateAcorns::calc_err_and_sign(
+  double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign)
+{
+  err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  sign = (err < 0) ? -1.0 : 1.0;
+  err = std::abs(err);
+
+  if (min_turn_angle > 0.0) {
+    const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw);
+    min_turn_angle = std::max(0.0, min_turn_angle - initial_direction_ * angle_change);
+    err = std::max(initial_direction_ * sign * err, 0.0);
+    err = std::max(min_turn_angle, err);
+    sign = initial_direction_;
+  }
+}
+
+double RotateAcorns::calc_speed(
+  const double err, const double sign, const double angular_speed)
+{
+  const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_;
+  const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_;
+
+  const double speed_until_overshoot = std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+
+  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
+  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
+  return speed;
+}
+
+ResultStatus RotateAcorns::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  double min_turn_angle = min_turn_angle_;
+  double angular_speed = angular_speed_;
+  double err, sign;
+
+  calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);
+
+  if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
+    err = check_space(pose, err, sign);
+  }
+
+  double speed = 0.0;
+
+  if (err != 0.0) {
+    speed = calc_speed(err, sign, angular_speed);
+  }
+
+  if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) {
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  min_turn_angle_ = min_turn_angle;
+  last_angle_ = current_yaw;
+  angular_speed_ = speed;
+  out_vel.angular.z = speed;
+  return ResultStatus{Status::RUNNING};
+}
+
+double RotateAcorns::check_space(
+  const geometry_msgs::msg::Pose pose, const double e, const double sign)
+{
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  double initial_theta = tf2::getYaw(pose.orientation);
+  pose2d.theta = initial_theta;
+  const double step_size = 0.1;
+  const double err = std::min(e, 1.0);
+  const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES);
+
+  for (int i = 1; i < err / step_size; i++) {
+    pose2d.theta += sign * step_size;
+
+    if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+
+    if (check_rival_collision(pose2d)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+  }
+
+  pose2d.theta = initial_theta + sign * err;
+
+  if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  if (check_rival_collision(pose2d)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  return e;
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::RotateAcorns, nav2_core::Behavior);

toid_behaviors/toid_behaviors.xml

@@ -12,5 +12,8 @@
         <class type="toid::ApproachAcorns" base_class_type="nav2_core::Behavior">
             <description></description>
         </class>
+        <class type="toid::RotateAcorns" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
     </library>
 </class_libraries>

toid_navigation/params/toid_general_params.yaml

@@ -37,6 +37,10 @@ behavior_server:
       plugin: "toid::MoveCoords"
     approachAcorns:
       plugin: "toid::ApproachAcorns"
+      kphi: 2.0
+      kdelta: 1.0
+      lambda: 2.0
+      debug_marker: true
     local_frame: map
     global_frame: map
     robot_base_frame: base_footprint