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");