Attackerスキルに CUT_THEIR_PASS / STEAL_BALL を追加 (#506)

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

crane_bringup/launch/crane.launch.xml

@@ -56,12 +56,14 @@
     <node pkg="crane_sender" exec="sim_sender_node" output="screen">
       <param name="no_movement" value="false"/>
       <param name="latency_ms" value="0.0"/>
-      <param name="k_gain" value="2.0"/>
+      <param name="k_gain" value="1.5"/>
       <param name="i_gain" value="0.0"/>
-      <param name="d_gain" value="0.1"/>
+      <param name="d_gain" value="1.5"/>
       <param name="theta_k_gain" value="2.0"/>
       <param name="theta_i_gain" value="0.0"/>
       <param name="theta_d_gain" value="0.1"/>
+      <param name="kick_power_limit_straight" value="0.6"/>
+      <param name="kick_power_limit_chip" value="1.0"/>
       <param name="sim_mode" value="true"/>
     </node>
   </group>
@@ -74,6 +76,8 @@
       <param name="theta_ki" value="0.0"/>
       <param name="theta_kd" value="0.5"/>
       <param name="sim_mode" value="$(var sim)"/>
+      <param name="kick_power_limit_straight" value="1.0"/>
+      <param name="kick_power_limit_chip" value="1.0"/>
     </node>
   </group>
 

crane_robot_skills/include/crane_robot_skills/attacker.hpp

@@ -14,6 +14,7 @@
 #include <crane_robot_skills/receive.hpp>
 #include <crane_robot_skills/redirect.hpp>
 #include <crane_robot_skills/skill_base.hpp>
+#include <crane_robot_skills/steal_ball.hpp>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -23,6 +24,8 @@ namespace crane::skills
 enum class AttackerState {
   ENTRY_POINT,
   FORCED_PASS,
+  CUT_THEIR_PASS,
+  STEAL_BALL,
   REDIRECT_GOAL_KICK,
   GOAL_KICK,
   CLEARING_KICK,
@@ -55,6 +58,8 @@ class Attacker : public SkillBaseWithState<AttackerState, RobotCommandWrapperPos
   Receive receive_skill;
 
   Redirect redirect_skill;
+
+  StealBall steal_ball_skill;
 };
 }  // namespace crane::skills
 #endif  // CRANE_ROBOT_SKILLS__ATTACKER_HPP_

crane_robot_skills/include/crane_robot_skills/ball_nearby_positioner.hpp

@@ -42,6 +42,8 @@ class BallNearByPositioner : public SkillBase<RobotCommandWrapperPosition>
           return 0.5;
         case crane_msgs::msg::PlaySituation::STOP:
           return 0.5;
+        case crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT:
+          return 0.5;
         default:
           return 0.0;
       }

crane_robot_skills/include/crane_robot_skills/steal_ball.hpp

@@ -19,8 +19,6 @@ namespace crane::skills
 enum class StealBallState {
   MOVE_TO_FRONT,
   STEAL,
-  PASS,
-  INTERCEPT,
 };
 class StealBall : public SkillBaseWithState<StealBallState, RobotCommandWrapperPosition>
 {

crane_robot_skills/src/attacker.cpp

@@ -17,7 +17,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   kick_skill(base),
   goal_kick_skill(base),
   receive_skill(base),
-  redirect_skill(base)
+  redirect_skill(base),
+  steal_ball_skill(base)
 {
   setParameter("receiver_id", 0);
   addStateFunction(
@@ -83,7 +84,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
           Point target =
             world_model()->ball.pos +
             getVerticalVec(receiver->pose.pos - world_model()->ball.pos).normalized() * 0.3;
-          command.setTargetPosition(target).lookAtBallFrom(target);
+          command.setTargetPosition(target).lookAtBallFrom(target).enableBallAvoidance();
           if (robot()->getDistance(target) < 0.1) {
             forced_pass_phase = 1;
           }
@@ -111,6 +112,40 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
       return Status::RUNNING;
     });
 
+  addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool {
+    return not world_model()->isOurBallByBallOwnerCalculator() && world_model()->ball.isMoving(0.2);
+  });
+
+  addTransition(AttackerState::CUT_THEIR_PASS, AttackerState::ENTRY_POINT, [this]() -> bool {
+    return world_model()->isOurBallByBallOwnerCalculator();
+  });
+
+  addStateFunction(
+    AttackerState::CUT_THEIR_PASS,
+    [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+      return receive_skill.run(visualizer);
+    });
+
+  addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool {
+    // 止まっているボールを相手が持っているとき
+    return not world_model()->isOurBallByBallOwnerCalculator() &&
+           world_model()->ball.isStopped(0.1) &&
+           world_model()
+               ->getNearestRobotWithDistanceFromPoint(
+                 world_model()->ball.pos, world_model()->theirs.getAvailableRobots())
+               .second < 0.5;
+  });
+
+  addTransition(AttackerState::STEAL_BALL, AttackerState::ENTRY_POINT, [this]() -> bool {
+    return world_model()->isOurBallByBallOwnerCalculator();
+  });
+
+  addStateFunction(
+    AttackerState::STEAL_BALL,
+    [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+      return steal_ball_skill.run(visualizer);
+    });
+
   addTransition(AttackerState::ENTRY_POINT, AttackerState::REDIRECT_GOAL_KICK, [this]() -> bool {
     // ボールが遠くにいる
     if (
@@ -129,6 +164,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     // ボールが止まっている
     if (world_model()->ball.vel.norm() < 0.5) {
       return true;
+    } else if (not world_model()->isOurBallByBallOwnerCalculator()) {
+      // 敵にボールを奪われた
+      return true;
     } else {
       return false;
     }
@@ -163,25 +201,33 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   addTransition(AttackerState::ENTRY_POINT, AttackerState::GOAL_KICK, [this]() -> bool {
     auto [best_angle, goal_angle_width] =
       world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos);
+    // ボールが近い条件はいらないかも？
     return robot()->getDistance(world_model()->ball.pos) < 2.0 &&
            goal_angle_width * 180.0 / M_PI > 5.;
   });
 
+  addTransition(AttackerState::GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool {
+    // 敵にボールを奪われた
+    return not world_model()->isOurBallByBallOwnerCalculator();
+  });
+
   addStateFunction(
     AttackerState::GOAL_KICK,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       return goal_kick_skill.run(visualizer);
     });
 
-  addTransition(
-    AttackerState::ENTRY_POINT, AttackerState::CLEARING_KICK, [this]() -> bool { return false; });
+  addTransition(AttackerState::ENTRY_POINT, AttackerState::CLEARING_KICK, [this]() -> bool {
+    // 未実装：やばいときに蹴る
+    return false;
+  });
 
   addStateFunction(
     AttackerState::CLEARING_KICK,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       kick_skill.setParameter("target", world_model()->getTheirGoalCenter());
       kick_skill.setParameter("kick_power", 1.0);
-      kick_skill.setParameter("dot_threshold", 0.95);
+      kick_skill.setParameter("dot_threshold", 0.9);
       kick_skill.setParameter("kick_with_chip", true);
       return kick_skill.run(visualizer);
     });
@@ -227,6 +273,11 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     return best_score > 0.5;
   });
 
+  addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool {
+    // 敵にボールを奪われた
+    return not world_model()->isOurBallByBallOwnerCalculator();
+  });
+
   addStateFunction(
     AttackerState::STANDARD_PASS,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
@@ -290,6 +341,11 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
            goal_angle_width * 180.0 / M_PI > 1.;
   });
 
+  addTransition(AttackerState::LOW_CHANCE_GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool {
+    // 敵にボールを奪われた
+    return not world_model()->isOurBallByBallOwnerCalculator();
+  });
+
   addStateFunction(
     AttackerState::LOW_CHANCE_GOAL_KICK,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
@@ -305,6 +361,12 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
              x_diff_with_their_goal >= world_model()->field_size.x() * 0.5;
     });
 
+  addTransition(
+    AttackerState::MOVE_BALL_TO_OPPONENT_HALF, AttackerState::ENTRY_POINT, [this]() -> bool {
+      // 敵にボールを奪われた
+      return not world_model()->isOurBallByBallOwnerCalculator();
+    });
+
   addStateFunction(
     AttackerState::MOVE_BALL_TO_OPPONENT_HALF,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
@@ -325,6 +387,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     // ボールが止まっている
     if (world_model()->ball.vel.norm() < 0.5) {
       return true;
+    } else if (not world_model()->isOurBallByBallOwnerCalculator()) {
+      // 敵にボールを奪われた
+      return true;
     } else {
       return false;
     }

crane_robot_skills/src/steal_ball.cpp

@@ -44,19 +44,6 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base)
     return skill_state == Status::SUCCESS;
   });
 
-  // 敵よりもボールに近い場合はパス
-  addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::PASS, [this]() {
-    auto theirs = world_model()->theirs.getAvailableRobots();
-    if (not theirs.empty()) {
-      auto [their_attacker, their_distance] =
-        world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs);
-      double our_distance = robot()->getDistance(world_model()->ball.pos);
-      return our_distance < their_distance - 0.2;
-    } else {
-      return true;
-    }
-  });
-
   addStateFunction(
     StealBallState::STEAL,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
@@ -68,7 +55,7 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base)
           world_model()->ball.pos, getAngle(world_model()->ball.pos - robot()->pose.pos));
         command.dribble(0.5);
       } else if (method == "side") {
-        if (robot()->getDistance(world_model()->ball.pos) < (0.085 + 0.000)) {
+        if (robot()->getDistance(world_model()->ball.pos) < (0.085 - 0.030)) {
           // ロボット半径より近くに来れば急回転して刈り取れる
           command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2);
         } else {
@@ -79,111 +66,5 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base)
       }
       return Status::RUNNING;
     });
-
-  // 敵よりもボールに近い場合はパス
-  addTransition(StealBallState::STEAL, StealBallState::PASS, [this]() {
-    auto theirs = world_model()->theirs.getAvailableRobots();
-    if (not theirs.empty()) {
-      auto [their_attacker, their_distance] =
-        world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs);
-      double our_distance = robot()->getDistance(world_model()->ball.pos);
-      return our_distance < their_distance - 0.2;
-    } else {
-      return true;
-    }
-  });
-
-  addStateFunction(
-    StealBallState::PASS,
-    [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
-      if (attacker_skill == nullptr) {
-        attacker_skill = std::make_shared<skills::SimpleAttacker>(command_base);
-      }
-      auto ours = world_model()->ours.getAvailableRobots(robot()->id);
-      ours.erase(
-        std::remove_if(
-          ours.begin(), ours.end(),
-          [this](auto e) {
-            return e->getDistance(world_model()->getTheirGoalCenter()) >
-                   robot()->getDistance(world_model()->getTheirGoalCenter());
-          }),
-        ours.end());
-      if (not ours.empty()) {
-        auto [target_bot, distance] = world_model()->getNearestRobotWithDistanceFromPoint(
-          world_model()->getTheirGoalCenter(), ours);
-        attacker_skill->setParameter("receiver_id", target_bot->id);
-      }
-      return attacker_skill->run(visualizer);
-    });
-
-  addTransition(StealBallState::PASS, StealBallState::MOVE_TO_FRONT, [this]() {
-    auto theirs = world_model()->theirs.getAvailableRobots();
-    if (theirs.empty()) {
-      return false;
-    } else {
-      auto [their_attacker, their_distance] =
-        world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs);
-      double our_distance = robot()->getDistance(world_model()->ball.pos);
-      return our_distance > their_distance;
-    }
-  });
-
-  addTransition(StealBallState::PASS, StealBallState::INTERCEPT, [this]() {
-    return world_model()->ball.vel.norm() > 0.5;
-  });
-
-  addTransition(StealBallState::STEAL, StealBallState::INTERCEPT, [this]() {
-    return world_model()->ball.vel.norm() > 0.5;
-  });
-
-  addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::INTERCEPT, [this]() {
-    return world_model()->ball.vel.norm() > 0.5;
-  });
-
-  addStateFunction(StealBallState::INTERCEPT, [this](const ConsaiVisualizerWrapper::SharedPtr &) {
-    Point vel_seg = world_model()->ball.vel * 5.0;
-    if (vel_seg.norm() < 1.0) {
-      vel_seg = vel_seg.normalized() * 1.0;
-    }
-    Segment ball_line{world_model()->ball.pos, world_model()->ball.pos + vel_seg};
-
-    Point across_point = [&]() {
-      const double OFFSET = 0.3;
-      const double X = world_model()->field_size.x() / 2.0 - OFFSET;
-      const double Y = world_model()->field_size.y() / 2.0 - OFFSET;
-
-      Segment seg1{Point(X, Y), Point(X, -Y)};
-      Segment seg2{Point(-X, Y), Point(-X, -Y)};
-      Segment seg3{Point(Y, X), Point(-Y, X)};
-      Segment seg4{Point(Y, -X), Point(-Y, -X)};
-      std::vector<Point> intersections;
-      if (bg::intersection(ball_line, seg1, intersections); not intersections.empty()) {
-        return intersections.front();
-      } else if (bg::intersection(ball_line, seg2, intersections); not intersections.empty()) {
-        return intersections.front();
-      } else if (bg::intersection(ball_line, seg3, intersections); not intersections.empty()) {
-        return intersections.front();
-      } else if (bg::intersection(ball_line, seg4, intersections); not intersections.empty()) {
-        return intersections.front();
-      } else {
-        return ball_line.second;
-      }
-    }();
-
-    // ゴールとボールの中間方向を向く
-    auto [goal_angle, width] = world_model()->getLargestGoalAngleRangeFromPoint(across_point);
-    auto to_goal = getNormVec(goal_angle);
-    auto to_ball = (world_model()->ball.pos - across_point).normalized();
-    double intermediate_angle = getAngle(2 * to_goal + to_ball);
-    command.liftUpDribbler();
-    command.kickStraight(getParameter<double>("kicker_power"));
-    command.setDribblerTargetPosition(across_point, intermediate_angle);
-
-    return Status::RUNNING;
-  });
-
-  addTransition(StealBallState::INTERCEPT, StealBallState::MOVE_TO_FRONT, [this]() {
-    return world_model()->ball.vel.norm() < 0.3;
-  });
 }
 }  // namespace crane::skills

crane_sender/include/crane_sender/sender_base.hpp

@@ -29,6 +29,12 @@ class SenderBase : public rclcpp::Node
     declare_parameter<double>("delay_s", 0.0);
     get_parameter("delay_s", delay_s);
 
+    declare_parameter<double>("kick_power_limit_straight", 1.0);
+    get_parameter("kick_power_limit_straight", kick_power_limit_straight);
+
+    declare_parameter<double>("kick_power_limit_chip", 1.0);
+    get_parameter("kick_power_limit_chip", kick_power_limit_chip);
+
     declare_parameter<double>("latency_ms", 0.0);
     get_parameter("latency_ms", current_latency_ms);
 
@@ -57,6 +63,10 @@ class SenderBase : public rclcpp::Node
 
   double current_latency_ms = 0.0;
 
+  double kick_power_limit_straight;
+
+  double kick_power_limit_chip;
+
   virtual void sendCommands(const crane_msgs::msg::RobotCommands & msg) = 0;
 
   float normalizeAngle(float angle_rad) const