Receiveスキルで受取までに時間がある場合、前進して受け取りに行く (#517)

ibis-ssl · Aug 5, 2024 · 753f594 · 753f594
1 parent b4104fd
commit 753f594
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 7 deletions.
diff --git 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<RobotCommandWrapperPosition>
     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<bool>("enable_software_bumper")) {
         // ボール到着まで残り<software_bumper_start_time>秒になったら、ボール速度方向に少し加速して衝撃を和らげる
         double ball_speed = world_model()->ball.vel.norm();
@@ -37,13 +39,17 @@ class Receive : public SkillBase<RobotCommandWrapperPosition>
           // ボールから逃げ切らないようにするため、速度の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<bool>("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
@@ -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<int>("receiver_id");