From 88ebe9fb6829aa7361560ef796a966958ef9e4ea Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 1 Apr 2026 17:46:49 +0200 Subject: [PATCH] added collision detection --- .../include/toid_behaviors/move_coords.hpp | 11 +- toid_behaviors/include/toid_behaviors/scl.hpp | 10 +- .../include/toid_behaviors/simple_move.hpp | 59 +++++++++ .../include/toid_behaviors/simple_rotate.hpp | 18 ++- toid_behaviors/src/move_coords.cpp | 120 ++++++++++++++---- toid_behaviors/src/scl.cpp | 31 ++++- toid_behaviors/src/simple_rotate.cpp | 113 ++++++++++++++--- 7 files changed, 301 insertions(+), 61 deletions(-) diff --git a/toid_behaviors/include/toid_behaviors/move_coords.hpp b/toid_behaviors/include/toid_behaviors/move_coords.hpp index 0bf0170..1296af2 100644 --- a/toid_behaviors/include/toid_behaviors/move_coords.hpp +++ b/toid_behaviors/include/toid_behaviors/move_coords.hpp @@ -23,17 +23,24 @@ public: const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) override; + double distanceToTarget( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, const double target_theta, bool backwards); + + double velocityTarget(const double dist_left); + + bool collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose); + 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; + return nav2_core::CostmapInfoType::LOCAL; } protected: - SmoothControlLaw scl; + SmoothControlLaw scl_; //Goal geometry_msgs::msg::Pose target_pose_; diff --git a/toid_behaviors/include/toid_behaviors/scl.hpp b/toid_behaviors/include/toid_behaviors/scl.hpp index af8194f..d80528f 100644 --- a/toid_behaviors/include/toid_behaviors/scl.hpp +++ b/toid_behaviors/include/toid_behaviors/scl.hpp @@ -19,13 +19,17 @@ public: 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); + 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, + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, geometry_msgs::msg::Twist & out_speed, bool backwards = false); + + void step( + const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt, + bool backwards = false); }; } // namespace toid \ No newline at end of file diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp index 0c26e5d..fefd9f5 100644 --- a/toid_behaviors/include/toid_behaviors/simple_move.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp @@ -6,6 +6,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "toid_msgs/action/simple_rotate.hpp" +#include "toid_msgs/msg/rival.hpp" namespace toid { @@ -14,6 +15,8 @@ template class SimpleMove : public nav2_behaviors::TimedBehavior { public: + using Rival = toid_msgs::msg::Rival; + virtual void configureCB() {} virtual void cleanupCB() {} @@ -34,6 +37,18 @@ public: 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(); + + nav2_util::declare_parameter_if_not_declared(node, "robot_width", rclcpp::ParameterValue(0.30)); + node->get_parameter("robot_width", robot_width_); + + nav2_util::declare_parameter_if_not_declared( + node, "robot_length", rclcpp::ParameterValue(0.30)); + node->get_parameter("robot_length", robot_length_); + + nav2_util::declare_parameter_if_not_declared( + node, "rival_radius", rclcpp::ParameterValue(0.30)); + node->get_parameter("rival_radius", rival_radius_); + odom_sub_ = node->create_subscription( odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) { std::lock_guard lock(mut_); @@ -52,12 +67,17 @@ public: void activate() override { nav2_behaviors::TimedBehavior::activate(); + rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock(); + using namespace std::placeholders; + rivals_sub_ = node->create_subscription( + "/dynamicObstacle", 1, std::bind(&SimpleMove::rival_cb, this, _1)); activateCB(); } void deactivate() override { nav2_behaviors::TimedBehavior::deactivate(); + rivals_sub_.reset(); deactivateCB(); } @@ -105,12 +125,51 @@ public: return r; } + bool check_rival_collision(geometry_msgs::msg::Pose2D & pose) + { + if (!rivals_) { + return false; + } + const double cosp = std::cos(pose.theta); + const double sinp = std::sin(pose.theta); + for (const auto & rival : rivals_->point) { + geometry_msgs::msg::Point local_rival; + const double dx = rival.x - pose.x; + const double dy = rival.y - pose.y; + local_rival.x = dx * cosp + dy * sinp; + local_rival.y = -dx * sinp + dy * cosp; + + const double qx = std::abs(local_rival.x) - robot_length_ / 2.0; + const double qy = std::abs(local_rival.y) - robot_width_ / 2.0; + + const double mqx = std::max(qx, 0.0); + const double mqy = std::max(qy, 0.0); + + double length = std::sqrt(mqx * mqx + mqy * mqy); + + double sdf = length + std::min(std::max(qx, qy), 0.0); + if (sdf < rival_radius_) { + return true; + } + } + return false; + } + + void rival_cb(Rival::SharedPtr msg) { rivals_ = msg; } + protected: rclcpp::Subscription::SharedPtr odom_sub_; geometry_msgs::msg::Pose current_pose_; geometry_msgs::msg::Twist current_vel_; std::recursive_mutex mut_; double control_duration_; + + double robot_width_ = 0.30; + double robot_length_ = 0.30; + double rival_radius_ = 0.30; + + Rival::SharedPtr rivals_; + rclcpp::Subscription::SharedPtr rivals_sub_; }; } // namespace toid \ No newline at end of file diff --git a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp index cec55e2..758eb68 100644 --- a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp +++ b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp @@ -2,6 +2,7 @@ #include "toid_behaviors/simple_move.hpp" #include "toid_msgs/action/simple_rotate.h" +#include "toid_msgs/msg/rival.hpp" namespace toid { @@ -23,17 +24,25 @@ public: 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; + + virtual nav2_core::CostmapInfoType getResourceInfo() override + { + return nav2_core::CostmapInfoType::LOCAL; } -protected: + 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: //Goal double target_angle_; double min_turn_angle_; double initial_direction_; + unsigned char mode_; //State double angular_speed_; @@ -45,6 +54,7 @@ protected: double min_angular_speed_; double max_angular_accel_; double max_angular_decel_; + double lookahead_; }; } // namespace toid diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp index d4a5320..b80ed1d 100644 --- a/toid_behaviors/src/move_coords.cpp +++ b/toid_behaviors/src/move_coords.cpp @@ -69,39 +69,83 @@ ResultStatus MoveCoords::onStart( target_sign_ = backwards_ ? -1.0 : 1.0; max_vel_speed_ = command->max_speed; - if(command->max_speed == 0) { + if (command->max_speed == 0) { auto node = node_.lock(); node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_); } - scl.k_phi = k_phi_; - scl.k_delta = k_delta_; - scl.bbeta = beta_; - scl.lam = lambda_; - scl.slowdown_radius = slowdown_radius_; - scl.v_angular_max = max_angular_speed_; - scl.v_linear_min = min_vel_speed_; - scl.v_linear_max = max_vel_speed_; + scl_.k_phi = k_phi_; + scl_.k_delta = k_delta_; + scl_.bbeta = beta_; + scl_.lam = lambda_; + scl_.slowdown_radius = slowdown_radius_; + scl_.v_angular_max = max_angular_speed_; + scl_.v_linear_min = min_vel_speed_; + scl_.v_linear_max = max_vel_speed_; last_speed_ = vel.angular.x; return ResultStatus{Status::SUCCEEDED}; } +double MoveCoords::distanceToTarget( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, + const double target_theta, bool backwards) +{ + const double dx = target_point.x - pose.position.x; + const double dy = target_point.y - pose.position.y; + const double target_sign = backwards? -1.0 : 1.0; + + return target_sign * (dx * cos(target_theta) + dy * sin(target_theta)); +} + + +double MoveCoords::velocityTarget(const double dist_left) { + const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_; + const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_; + + + double vel = max_vel_speed_; + double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left); + vel = std::min(vel, max_vel_to_stop); + return std::clamp(target_sign_ * vel, lower_bound, upper_bound); +} + +bool MoveCoords::collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose) { + const int samples = static_cast(0.5/control_duration_); + geometry_msgs::msg::Pose current_pose = pose; + last_ok_pose = pose; + for(int i = 0; i < samples; i++) { + scl_.step(target_pose_, current_pose, control_duration_, backwards_); + + geometry_msgs::msg::Pose2D p; + p.x = current_pose.position.x; + p.y = current_pose.position.y; + p.theta = tf2::getYaw(current_pose.orientation); + + if(!local_collision_checker_->isCollisionFree(p, i==0)) { + return true; + } + + if(check_rival_collision(p)) { + return true; + } + + last_ok_pose = current_pose; + const double dist_left = distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_); + if(dist_left < 0.005) { + return false; + } + } + return false; +} + ResultStatus MoveCoords::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 angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_); - const double dx = target_pose_.position.x - pose.position.x; - const double dy = target_pose_.position.y - pose.position.y; - - const double dist_left = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_)); - - const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_; - const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_; + double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_); if (dist_left <= 0.001) { out_vel.linear.x = 0; @@ -109,30 +153,52 @@ ResultStatus MoveCoords::updateVel( return ResultStatus{Status::SUCCEEDED}; } - double vel = max_vel_speed_; - double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left); - vel = std::min(vel, max_vel_to_stop); + const double current_yaw = tf2::getYaw(pose.orientation); + double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_); + geometry_msgs::msg::Pose last_ok_pose; + if(collisionDetection(pose, last_ok_pose)) { + + dist_left = distanceToTarget(pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_); + if(dist_left <= 0.02) { + out_vel.linear.x = 0; + out_vel.angular.z = 0; + } else { + scl_.v_linear_max = target_sign_ * velocityTarget(dist_left); + scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_); + } + + + + last_speed_ = out_vel.linear.x; + + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max); + + return ResultStatus{Status::RUNNING}; + } if (dist_left <= 0.02) { - out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound); + out_vel.linear.x = velocityTarget(dist_left); 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_ * std::clamp(target_sign_ * vel, lower_bound, upper_bound); - scl.calculate_vel(target_pose_, pose, out_vel, backwards_); + + scl_.v_linear_max = target_sign_ * velocityTarget(dist_left); + scl_.calculate_vel(target_pose_, pose, out_vel, backwards_); + + last_speed_ = out_vel.linear.x; - RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); - RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max); return ResultStatus{Status::RUNNING}; } } // namespace toid - #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(toid::MoveCoords, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_behaviors/src/scl.cpp b/toid_behaviors/src/scl.cpp index 12ad53f..fe22bdc 100644 --- a/toid_behaviors/src/scl.cpp +++ b/toid_behaviors/src/scl.cpp @@ -3,20 +3,21 @@ #include #include "angles/angles.h" +#include "nav2_util/geometry_utils.hpp" #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -namespace toid { +namespace toid +{ void SmoothControlLaw::calculate_vel( const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, - geometry_msgs::msg::Twist &out_speed, bool backwards) + geometry_msgs::msg::Twist & out_speed, bool backwards) { double r, phi, delta; egocentric_polar(target, current, backwards, r, phi, delta); double curvature = this->curvature(r, phi, delta); - curvature = backwards? -curvature : curvature; + curvature = backwards ? -curvature : curvature; double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam)); @@ -44,8 +45,8 @@ double SmoothControlLaw::curvature(double r, double phi, double delta) } void SmoothControlLaw::egocentric_polar( - const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, double & r, - double & phi, double & delta) + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, + double & r, double & phi, double & delta) { const double dx = target.position.x - current.position.x; const double dy = target.position.y - current.position.y; @@ -59,4 +60,20 @@ void SmoothControlLaw::egocentric_polar( delta = angles::normalize_angle(ctheta + los); } -} \ No newline at end of file +void SmoothControlLaw::step( + const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt, + bool backwards) +{ + geometry_msgs::msg::Twist twist; + calculate_vel(target, current, twist, backwards); + + double theta = tf2::getYaw(current.orientation); + double dx = twist.linear.x * dt * cos(theta); + double dy = twist.linear.x * dt * sin(theta); + current.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(theta + twist.angular.z * dt); + current.position.x += dx; + current.position.y += dy; +} + +} // namespace toid \ No newline at end of file diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp index 4c75f60..9fdde0f 100644 --- a/toid_behaviors/src/simple_rotate.cpp +++ b/toid_behaviors/src/simple_rotate.cpp @@ -4,6 +4,7 @@ #include "angles/angles.h" #include "tf2/convert.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace toid { @@ -30,6 +31,10 @@ void SimpleRotateBehavior::configureCB() 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_); } ResultStatus SimpleRotateBehavior::onStart( @@ -40,12 +45,19 @@ ResultStatus SimpleRotateBehavior::onStart( 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; - if(command->max_speed == 0) { + 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; @@ -53,43 +65,108 @@ ResultStatus SimpleRotateBehavior::onStart( return ResultStatus{Status::SUCCEEDED}; } +void SimpleRotateBehavior::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 SimpleRotateBehavior::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 SimpleRotateBehavior::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); - const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw); - last_angle_ = current_yaw; + double min_turn_angle = min_turn_angle_; + double angular_speed = angular_speed_; + double err, sign; - double err = angles::shortest_angular_distance(current_yaw, target_angle_); - double sign = (err < 0)? -1.0 : 1.0; - err = std::abs(err); + calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign); - if (min_turn_angle_ > 0.0) { - 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_; + if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) { + err = check_space(pose, err, sign); } - const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_; - const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_; + double speed = 0.0; - 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_); + 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 SimpleRotateBehavior::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"