From 753f594a2acd45fe60abe0bf2e0b5f8c6fff39ec Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 6 Aug 2024 08:13:19 +0900 Subject: [PATCH] =?UTF-8?q?Receive=E3=82=B9=E3=82=AD=E3=83=AB=E3=81=A7?= =?UTF-8?q?=E5=8F=97=E5=8F=96=E3=81=BE=E3=81=A7=E3=81=AB=E6=99=82=E9=96=93?= =?UTF-8?q?=E3=81=8C=E3=81=82=E3=82=8B=E5=A0=B4=E5=90=88=E3=80=81=E5=89=8D?= =?UTF-8?q?=E9=80=B2=E3=81=97=E3=81=A6=E5=8F=97=E3=81=91=E5=8F=96=E3=82=8A?= =?UTF-8?q?=E3=81=AB=E8=A1=8C=E3=81=8F=20(#517)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/receive.hpp | 16 +++++++++++----- crane_robot_skills/src/attacker.cpp | 11 +++++++++-- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/receive.hpp b/crane_robot_skills/include/crane_robot_skills/receive.hpp index 0f8941b91..c3e5eb271 100644 --- a/crane_robot_skills/include/crane_robot_skills/receive.hpp +++ b/crane_robot_skills/include/crane_robot_skills/receive.hpp @@ -23,11 +23,13 @@ class Receive : public SkillBase setParameter("software_bumper_start_time", 0.5); // min_slack, max_slack, closest setParameter("policy", std::string("closest")); + setParameter("enable_active_receive", true); } Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override { auto offset = [&]() -> Point { + Point offset(0, 0); if (getParameter("enable_software_bumper")) { // ボール到着まで残り秒になったら、ボール速度方向に少し加速して衝撃を和らげる double ball_speed = world_model()->ball.vel.norm(); @@ -37,13 +39,17 @@ class Receive : public SkillBase // ボールから逃げ切らないようにするため、速度の0.5倍に制限 command.setMaxVelocity(ball_speed * 0.5); // ボール速度方向に速度の0.5倍だけオフセット（1m/sで近づいていたら0.5m） - return world_model()->ball.vel.normalized() * (world_model()->ball.vel.norm() * 0.5); - } else { - return Point(0, 0); + offset += world_model()->ball.vel.normalized() * (world_model()->ball.vel.norm() * 0.5); } - } else { - return Point(0, 0); } + if (getParameter("enable_active_receive")) { + if (world_model()->ball.isMovingTowards(robot()->pose.pos, 2.0, 0.5)) { + offset += (world_model()->ball.pos - robot()->pose.pos); + double distance = (world_model()->ball.pos - robot()->pose.pos).norm(); + command.setMaxVelocity(distance); + } + } + return offset; }(); auto interception_point = getInterceptionPoint() + offset; command.lookAtBallFrom(interception_point) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 1ee1add3b..85db2a8a5 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -157,8 +157,11 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) robot()->getDistance(world_model()->ball.pos) > 1.0 && world_model()->ball.vel.norm() > 0.5) { auto [best_angle, goal_angle_width] = world_model()->getLargestGoalAngleRangeFromPoint(robot()->pose.pos); - if (goal_angle_width * 180.0 / M_PI > 10.) { - // ゴールが見えている + double angle_diff_deg = + std::abs(getAngleDiff(getAngle(world_model()->ball.pos - robot()->pose.pos), best_angle)) * + 180.0 / M_PI; + if (goal_angle_width * 180.0 / M_PI > 10. && angle_diff_deg < 90.) { + // ゴールが見えている && リダイレクト角度が90度以内 return true; } } @@ -239,6 +242,10 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) }); addTransition(AttackerState::ENTRY_POINT, AttackerState::STANDARD_PASS, [this]() -> bool { + if (robot()->getDistance(world_model()->ball.pos) > 1.0) { + return false; + } + auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); // TODO(HansRobo): しっかりパス先を選定する // int receiver_id = getParameter("receiver_id");