diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt index 2498146..9426654 100644 --- a/toid_behaviors/CMakeLists.txt +++ b/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) diff --git a/toid_behaviors/include/toid_behaviors/rotate_acorns.hpp b/toid_behaviors/include/toid_behaviors/rotate_acorns.hpp new file mode 100644 index 0000000..411fc06 --- /dev/null +++ b/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 +{ +public: + RotateAcorns(); + ~RotateAcorns(); + + void configureCB() override; + + void activateCB() override; + void deactivateCB() override; + + ResultStatus onStart( + const std::shared_ptr 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::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 diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index e1c15b3..270cb9f 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/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 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_); diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp new file mode 100644 index 0000000..ffbf905 --- /dev/null +++ b/toid_behaviors/src/rotate_acorns.cpp @@ -0,0 +1,229 @@ +#include "toid_behaviors/rotate_acorns.hpp" + +#include + +#include "angles/angles.h" +#include "tf2/convert.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace toid +{ + +RotateAcorns::RotateAcorns() : SimpleMove() {} +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( + "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 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); \ No newline at end of file diff --git a/toid_behaviors/toid_behaviors.xml b/toid_behaviors/toid_behaviors.xml index 2556c3f..d64afa7 100644 --- a/toid_behaviors/toid_behaviors.xml +++ b/toid_behaviors/toid_behaviors.xml @@ -12,5 +12,8 @@ + + + \ No newline at end of file diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 0101127..aa399fc 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/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