Comp 2025: ateam_kenobi tactics

ateam_kenobi/src/tactics/defenders.cpp

@@ -56,7 +56,8 @@ void Defenders::runFrame(
     const auto & defender_point = defender_points[i];
     auto & emt = easy_move_tos_[robot.id];
     emt.setTargetPosition(defender_point);
-    emt.face_point(world.ball.pos);
+    // emt.face_point(world.ball.pos);
+    emt.face_absolute(0.0);
     path_planning::PlannerOptions planner_options;
     planner_options.avoid_ball = false;
     planner_options.footprint_inflation = 0.03;
@@ -69,20 +70,30 @@ void Defenders::runFrame(
 
 std::vector<ateam_geometry::Point> Defenders::getDefenderPoints(const World & world)
 {
-  const auto first_position = getBallBlockPoint(world);
-  const auto visible_oponent_robots = play_helpers::getVisibleRobots(world.their_robots);
-  ateam_geometry::Point second_position;
-  const auto min_opponent_bots = 2;
-  if (visible_oponent_robots.size() >= min_opponent_bots) {
-    second_position = getPassBlockPoint(world);
-  }
+  if(isBallInDefenseArea(world)) {
+    const auto x = -((world.field.field_length / 2.0) -
+      world.field.defense_area_depth) + kDefenseSegmentOffset;
+    const auto y = (world.field.defense_area_width / 2.0) + kDefenseSegmentOffset;
+    return {
+      ateam_geometry::Point{x, -y},
+      ateam_geometry::Point{x, y}
+    };
+  } else {
+    const auto first_position = getBallBlockPoint(world);
+    const auto visible_oponent_robots = play_helpers::getVisibleRobots(world.their_robots);
+    ateam_geometry::Point second_position;
+    const auto min_opponent_bots = 2;
+    if (visible_oponent_robots.size() >= min_opponent_bots) {
+      second_position = getPassBlockPoint(world);
+    }
 
-  if (visible_oponent_robots.size() < min_opponent_bots ||
-    CGAL::squared_distance(second_position, first_position) < kRobotDiameter * kRobotDiameter)
-  {
-    second_position = getAdjacentBlockPoint(world, first_position);
+    if (visible_oponent_robots.size() < min_opponent_bots ||
+      CGAL::squared_distance(second_position, first_position) < kRobotDiameter * kRobotDiameter)
+    {
+      second_position = getAdjacentBlockPoint(world, first_position);
+    }
+    return {first_position, second_position};
   }
-  return {first_position, second_position};
 }
 
 ateam_geometry::Point Defenders::getBallBlockPoint(const World & world)
@@ -168,13 +179,10 @@ std::vector<ateam_geometry::Segment> Defenders::getDefenseSegments(const World &
 {
   std::vector<ateam_geometry::Segment> segments;
 
-  const auto margin = 0.05;
-
-  const auto pos_y_extent = (world.field.defense_area_width / 2.0) + kRobotRadius + margin;
+  const auto pos_y_extent = (world.field.defense_area_width / 2.0) + kDefenseSegmentOffset;
   const auto neg_y_extent = -pos_y_extent;
   const auto x_front = (-world.field.field_length / 2.0) + world.field.defense_area_depth +
-    kRobotRadius +
-    margin;
+    kDefenseSegmentOffset;
   const auto x_back = -world.field.field_length / 2.0;
 
   segments.push_back(
@@ -210,4 +218,16 @@ void Defenders::drawDefenseSegments(const World & world)
   }
 }
 
+bool Defenders::isBallInDefenseArea(const World & world)
+{
+  const auto def_area_front_x = -((world.field.field_length / 2.0) -
+    world.field.defense_area_depth);
+  const auto def_area_half_width = world.field.defense_area_width / 2.0;
+  ateam_geometry::Rectangle def_area{
+    ateam_geometry::Point{def_area_front_x, -def_area_half_width},
+    ateam_geometry::Point{-world.field.field_length / 2.0, def_area_half_width}
+  };
+  return ateam_geometry::doIntersect(world.ball.pos, def_area);
+}
+
 }  // namespace ateam_kenobi::tactics

ateam_kenobi/src/tactics/defenders.hpp

@@ -48,6 +48,8 @@ class Defenders : public stp::Tactic
     std::array<std::optional<ateam_msgs::msg::RobotMotionCommand>, 16> & motion_commands);
 
 private:
+  static constexpr double kMargin = 0.05;
+  static constexpr double kDefenseSegmentOffset = kRobotRadius + kMargin;
   std::array<play_helpers::EasyMoveTo, 16> easy_move_tos_;
 
   std::vector<ateam_geometry::Point> getDefenderPoints(const World & world);
@@ -63,6 +65,8 @@ class Defenders : public stp::Tactic
   std::vector<ateam_geometry::Segment> getDefenseSegments(const World & world);
 
   void drawDefenseSegments(const World & world);
+
+  bool isBallInDefenseArea(const World & world);
 };
 
 }  // namespace ateam_kenobi::tactics

ateam_kenobi/src/tactics/pass.cpp

@@ -21,23 +21,24 @@
 
 #include "pass.hpp"
 #include <algorithm>
+#include <ateam_common/time.hpp>
 
 namespace ateam_kenobi::tactics
 {
 
 Pass::Pass(stp::Options stp_options)
 : stp::Tactic(stp_options),
   receiver_(createChild<skills::PassReceiver>("receiver")),
-  kick_(createChild<skills::PivotKick>("kicker", skills::KickSkill::WaitType::WaitToKick))
+  kick_(createChild<skills::UniversalKick>("kicker", skills::KickSkill::WaitType::WaitToKick))
 {
-  kick_.SetPivotSpeed(1.75);
 }
 
 void Pass::reset()
 {
   receiver_.reset();
   kick_.Reset();
   kicker_id_ = -1;
+  missed_ = false;
 }
 
 ateam_geometry::Point Pass::getKickerAssignmentPoint(const World & world)
@@ -60,56 +61,101 @@ void Pass::runFrame(
 {
   kicker_id_ = kicker_bot.id;
 
+  getPlayInfo()["Kicker"]["Robot"] = kicker_id_;
+
   if (ateam_geometry::norm(world.ball.vel) < 0.01) {
     receiver_.setTarget(target_);
     kick_.SetTargetPoint(target_);
   }
 
+  if (kick_.IsDone()) {
+    const ateam_geometry::Segment pass_segment{kicker_bot.pos, receiver_bot.pos};
+    const auto missed_dist_threshold = 0.5;
+    const auto ball_to_segment_dist = CGAL::approximate_sqrt(CGAL::squared_distance(pass_segment,
+        world.ball.pos));
+    if(ball_to_segment_dist > missed_dist_threshold) {
+      // Missed or intercepted
+      missed_ = true;
+    }
+  }
+
+  const auto kicker_ready = kick_.IsReady();
+  if(kicker_ready && !prev_kicker_ready_) {
+    kicker_ready_start_time_ = world.current_time;
+  }
+  prev_kicker_ready_ = kicker_ready;
+
+  getPlayInfo()["Kicker Ready"] = kicker_ready;
+
+  const auto ball_speed = ateam_geometry::norm(world.ball.vel);
+  getPlayInfo()["Ball Speed"] = ball_speed;
+
   receiver_command = receiver_.runFrame(world, receiver_bot);
+  ForwardPlayInfo(receiver_);
+
+  getPlayInfo()["Kicker Done"] = kick_.IsDone();
+  getPlayInfo()["Receiver Done"] = receiver_.isDone();
 
-  const bool is_stalled = kick_.IsDone() && !receiver_.isDone() && ateam_geometry::norm(
-    world.ball.vel) < 0.02;
+  const bool is_stalled = kick_.IsDone() && !receiver_.isDone() && ball_speed < 0.02;
+
+  getPlayInfo()["is_stalled"] = is_stalled;
 
   const bool is_in_receiver_territory =
-    std::sqrt(CGAL::squared_distance(world.ball.pos, receiver_bot.pos)) < 0.25;
+    std::sqrt(CGAL::squared_distance(world.ball.pos, target_)) < 0.25;
+
+  getPlayInfo()["is_in_receiver_territory"] = is_in_receiver_territory;
 
   if (is_stalled && !is_in_receiver_territory) {
     kick_.Reset();
   }
 
+  const auto kicker_ready_time = ateam_common::TimeDiffSeconds(world.current_time,
+      kicker_ready_start_time_);
+
+  getPlayInfo()["Ready Time"] = kicker_ready_time;
+
   auto receiver_threshold = kReceiverPositionThreshold;
   if (std::sqrt(CGAL::squared_distance(world.ball.pos, target_)) > 3.0) {
-    receiver_threshold = 5.0;
+    receiver_threshold = 0.5;
   }
+
+  const auto kick_speed = speed_.value_or(calculateDefaultKickSpeed(world));
   if (ateam_geometry::norm(receiver_bot.pos, target_) <= receiver_threshold) {
     kick_.AllowKicking();
+    kick_.SetKickSpeed(kick_speed);
+    receiver_.setExpectedKickSpeed(kick_speed);
+    getPlayInfo()["kicking_allowed"] = "yes";
+  } else if (kicker_ready && kicker_ready_time > 3.0) {
+    kick_.AllowKicking();
+    kick_.SetKickSpeed(kick_speed / 2.0);
+    receiver_.setExpectedKickSpeed(kick_speed);
+    getPlayInfo()["kicking_allowed"] = "yes (time)";
   } else {
     kick_.DisallowKicking();
+    getPlayInfo()["kicking_allowed"] = "no";
   }
 
-  if (speed_) {
-    kick_.SetKickSpeed(*speed_);
-  } else {
-    kick_.SetKickSpeed(calculateDefaultKickSpeed(world));
-  }
+  /* TODO(barulicm): The kicker should make sure it's out of the way if the ball is in receiver
+   * territory
+   */
   if (!is_in_receiver_territory) {
     kicker_command = kick_.RunFrame(world, kicker_bot);
   }
 }
 
 bool Pass::isDone()
 {
-  return receiver_.isDone();
+  return missed_ || receiver_.isDone();
 }
 
 double Pass::calculateDefaultKickSpeed(const World & world)
 {
   const auto distance = CGAL::approximate_sqrt(CGAL::squared_distance(world.ball.pos, target_));
-  const auto ball_friction_acceleration = 1.0;
-  const auto max_kick_speed = 5.0;
-  const auto velocity_at_receiver = 1.0;
+  const auto ball_friction_acceleration = 0.4;
+  const auto max_kick_speed = 3.0;
+  const auto velocity_at_receiver = 0.1;
   const auto stop_at_receiver_velocity = std::sqrt(2.0 * ball_friction_acceleration * distance);
-  return std::min((velocity_at_receiver + stop_at_receiver_velocity) / 3.0, max_kick_speed);
+  return std::min(velocity_at_receiver + stop_at_receiver_velocity, max_kick_speed);
 }
 
 }  // namespace ateam_kenobi::tactics

ateam_kenobi/src/tactics/pass.hpp

@@ -25,7 +25,7 @@
 #include <ateam_geometry/types.hpp>
 #include "core/stp/tactic.hpp"
 #include "core/types/world.hpp"
-#include "skills/pivot_kick.hpp"
+#include "skills/universal_kick.hpp"
 #include "skills/pass_receiver.hpp"
 
 namespace ateam_kenobi::tactics
@@ -61,16 +61,25 @@ class Pass : public stp::Tactic
 
   void setCaptureSpeed(double speed)
   {
-    kick_.SetCaptureSpeed(speed);
+    (void)speed;
+    // kick_.SetCaptureSpeed(speed);
+  }
+
+  void SetPreferredKickType(skills::UniversalKick::KickType type)
+  {
+    kick_.SetPreferredKickType(type);
   }
 
 private:
-  const double kReceiverPositionThreshold = 0.1;
+  const double kReceiverPositionThreshold = 0.15;
   std::optional<double> speed_;
   ateam_geometry::Point target_;
   skills::PassReceiver receiver_;
-  skills::PivotKick kick_;
+  skills::UniversalKick kick_;
   int kicker_id_ = -1;
+  std::chrono::steady_clock::time_point kicker_ready_start_time_;
+  bool prev_kicker_ready_ = false;
+  bool missed_ = false;
 
   double calculateDefaultKickSpeed(const World & world);
 };

ateam_kenobi/src/tactics/standard_defense.cpp

@@ -47,7 +47,9 @@ void StandardDefense::runFrame(
   std::array<std::optional<ateam_msgs::msg::RobotMotionCommand>, 16> & motion_commands)
 {
   goalie_.runFrame(world, motion_commands);
+  ForwardPlayInfo(goalie_);
   defenders_.runFrame(world, defender_bots, motion_commands);
+  ForwardPlayInfo(defenders_);
 }
 
 }  // namespace ateam_kenobi::tactics