PTO Gameplay and Game Tracking changes

ateam_kenobi/CMakeLists.txt

@@ -39,6 +39,7 @@ target_sources(kenobi_node_component PRIVATE
   src/core/motion/motion_controller.cpp
   src/core/motion/motion_executor.cpp
   src/core/visualization/overlays.cpp
+  src/core/play_helpers/intercept_calculation.cpp
   src/core/play_helpers/lanes.cpp
   src/core/play_helpers/possession.cpp
   src/core/play_helpers/available_robots.cpp

ateam_kenobi/src/core/motion/motion_executor.cpp

@@ -116,7 +116,7 @@ std::array<std::optional<BodyVelocity>,
         }
     }, intent.angular);
 
-    if(!path.empty()) {
+    if(use_controller_linvel || use_controller_omega) {
       auto controller_vel = controller.get_command(robot, current_time, intent.motion_options);
       if (use_controller_linvel) {
         body_velocity.linear = controller_vel.linear;

ateam_kenobi/src/core/motion/motion_options.hpp

@@ -32,11 +32,11 @@ struct MotionOptions
   /// @brief angle around the end point that will be considered completed
   double angular_completion_threshold = 0.035;  // radians
 
-  double max_velocity = 1.0;  // m/s
+  double max_velocity = 2.0;  // m/s
   double max_acceleration = 2.0;  // m/s^2
   double max_deceleration = 2.0;  // m/s^2
 
-  double max_angular_velocity = 3.0;  // rad/s
+  double max_angular_velocity = 3.5;  // rad/s
   double max_angular_acceleration = 4.0;  // rad/s^2
 
   double max_allowed_turn_angle = M_PI / 4.0;  // radians

ateam_kenobi/src/core/play_helpers/intercept_calculation.cpp

@@ -0,0 +1,112 @@
+// Copyright 2026 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include "intercept_calculation.hpp"
+#include <angles/angles.h>
+#include <ateam_geometry/angles.hpp>
+#include <ateam_common/robot_constants.hpp>
+
+namespace ateam_kenobi::play_helpers
+{
+
+InterceptResults calculateIntercept(
+  const World & world, const Robot & robot,
+  double offset_distance)
+{
+  InterceptResults result;
+
+  // ball velocity
+  const double vb = ateam_geometry::norm(world.ball.vel);
+
+  // TODO(chachmu): pass this as an argument or something?
+  // max robot velocity (slightly decreased to give robot time to prepare)
+  // const double vr = max_speed_ * 0.85;
+  const double vr = 1.5;
+
+  // Need to get there a bit in front of the ball
+  const auto offset_ball_pos = world.ball.pos +
+    (offset_distance * ateam_geometry::normalize(world.ball.vel));
+  const auto ball_to_robot = robot.pos - offset_ball_pos;
+
+  // robot distance to ball projected onto the balls trajectory
+  const double d = (world.ball.vel * ball_to_robot) / vb;
+
+  const auto robot_proj_ball = world.ball.vel * d / vb;
+
+  const auto robot_perp_ball = ball_to_robot - robot_proj_ball;
+
+  // robot distance tangent to the balls trajectory
+  const double h = ateam_geometry::norm(robot_perp_ball);
+
+  result.robot_proj_ball_trajectory = robot_proj_ball;
+  result.robot_perp_ball_trajectory = robot_perp_ball;
+  result.d = d;
+  result.h = h;
+
+  /*
+    Initial Equation:
+      d - vb*t = sin(acos(h/(vr*t))) * vr*t
+    Solve for t:
+      t = (d * vb - sqrt((d^2*vr^2) + (h^2 * vr^2) - (h^2 * vb^2)) / (vb^2 - vr^2)
+  */
+
+  double discriminant = (d * d * vr * vr) + (h * h * vr * vr) - (h * h * vb * vb);
+  double denominator = vb * vb - vr * vr;
+
+  // Imaginary result means we will never catch the ball
+  if (discriminant < 0) {
+    return result;
+  }
+
+  // If the robot and ball speeds are very similar then the denominator can go to 0
+  if (abs(denominator) < 0.01) {
+    // Shift the denominator to represent a slightly faster robot since:
+    //  a. we already assume a slower than actual robot speed
+    //  b. the ball will decelerate as it rolls
+    denominator = -0.1;
+  }
+
+  // Time to intercept moving towards the ball
+  double t = (d * vb - sqrt(discriminant)) / denominator;
+
+  // TODO(chachmu): add better checks to see if these times are possible to
+  // respond to
+  if (t > 0.0) {
+    result.t = t;
+    result.equation_sign = -1.0;
+    result.intercept_pos = std::make_optional(offset_ball_pos + (t * world.ball.vel));
+
+    return result;
+  }
+
+  // Time to intercept moving away from the ball
+  t = (d * vb + sqrt(discriminant)) / denominator;
+
+  if (t > 0.0) {
+    result.t = t;
+    result.equation_sign = 1.0;
+    result.intercept_pos = std::make_optional(offset_ball_pos + (t * world.ball.vel));
+
+    return result;
+  }
+
+  return result;
+}
+}  // namespace ateam_kenobi::play_helpers

ateam_kenobi/src/core/play_helpers/intercept_calculation.hpp

@@ -0,0 +1,52 @@
+// Copyright 2026 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+
+#ifndef CORE__PLAY_HELPERS__INTERCEPT_CALCULATION_HPP_
+#define CORE__PLAY_HELPERS__INTERCEPT_CALCULATION_HPP_
+
+#include <ateam_geometry/types.hpp>
+#include "core/types/state_types.hpp"
+
+namespace ateam_kenobi::play_helpers
+{
+
+struct InterceptResults
+{
+  ateam_geometry::Vector robot_proj_ball_trajectory;
+  ateam_geometry::Vector robot_perp_ball_trajectory;
+
+  double d;   // robot distance to ball projected onto the balls trajectory
+  double h;   // robot distance tangent to the balls trajectory
+
+  double t;   // time for robot to intercept ball
+  double equation_sign;   // -1 or +1 to represent which version of the equation was used
+  std::optional<ateam_geometry::Point> intercept_pos = std::nullopt;
+};
+
+/// Calculate the earliest time and location the robot could intercept the ball
+/// @Param offset_distance How far in front of the ball the robot should be at interception time
+InterceptResults calculateIntercept(
+  const World & world, const Robot & robot,
+  double offset_distance);
+
+}  // namespace ateam_kenobi::play_helpers
+
+#endif  // CORE__PLAY_HELPERS__INTERCEPT_CALCULATION_HPP_

ateam_kenobi/src/kenobi_node.cpp

@@ -45,7 +45,6 @@
 #include "core/in_play_eval.hpp"
 #include "core/double_touch_eval.hpp"
 #include "core/ballsense_emulator.hpp"
-#include "core/ballsense_filter.hpp"
 #include "core/motion/frame_conversions.hpp"
 #include "core/motion/motion_executor.hpp"
 #include "plays/halt_play.hpp"
@@ -132,7 +131,6 @@ class KenobiNode : public rclcpp::Node
   InPlayEval in_play_eval_;
   DoubleTouchEval double_touch_eval_;
   BallSenseEmulator ballsense_emulator_;
-  BallSenseFilter ballsense_filter_;
   std::vector<uint8_t> heatmap_render_buffer_;
   JoystickEnforcer joystick_enforcer_;
   visualization::Overlays overlays_;

ateam_kenobi/src/plays/defenders_only_play.cpp

@@ -33,17 +33,8 @@ DefendersOnlyPlay::DefendersOnlyPlay(stp::Options stp_options)
   setEnabled(false);
 }
 
-stp::PlayScore DefendersOnlyPlay::getScore(const World & world)
+stp::PlayScore DefendersOnlyPlay::getScore([[maybe_unused]] const World & world)
 {
-  if (world.referee_info.running_command == ateam_common::GameCommand::ForceStart ||
-    world.referee_info.running_command == ateam_common::GameCommand::NormalStart ||
-    world.referee_info.running_command == ateam_common::GameCommand::DirectFreeOurs ||
-    world.referee_info.running_command == ateam_common::GameCommand::DirectFreeTheirs ||
-    world.referee_info.running_command == ateam_common::GameCommand::PrepareKickoffOurs ||
-    world.referee_info.running_command == ateam_common::GameCommand::PrepareKickoffTheirs)
-  {
-    return stp::PlayScore::Max();
-  }
   return stp::PlayScore::NaN();
 }
 

ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp

@@ -68,6 +68,11 @@ void FreeKickOnGoalPlay::enter()
   striker_.Reset();
 }
 
+void FreeKickOnGoalPlay::exit()
+{
+  striker_.Reset();
+}
+
 std::array<std::optional<RobotCommand>,
   16> FreeKickOnGoalPlay::runFrame(const World & world)
 {

ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.hpp

@@ -40,6 +40,7 @@ class FreeKickOnGoalPlay : public stp::Play
   stp::PlayScore getScore(const World & world) override;
 
   void enter() override;
+  void exit() override;
 
   std::array<std::optional<RobotCommand>,
     16> runFrame(const World & world) override;

ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.cpp

@@ -19,6 +19,7 @@
 // THE SOFTWARE.
 
 #include "kickoff_on_goal.hpp"
+#include <vector>
 #include "core/play_helpers/available_robots.hpp"
 #include "core/play_helpers/robot_assignment.hpp"
 #include "core/play_helpers/window_evaluation.hpp"
@@ -84,7 +85,13 @@ std::array<std::optional<RobotCommand>, 16> KickoffOnGoalPlay::runFrame(
   multi_move_to_.SetFacePoint(world.ball.pos);
 
   play_helpers::GroupAssignmentSet groups;
-  groups.AddPosition("kicker", kick_.GetAssignmentPoint(world));
+
+  std::vector<int> disallowed_strikers;
+  if (world.double_touch_forbidden_id_) {
+    disallowed_strikers.push_back(*world.double_touch_forbidden_id_);
+  }
+
+  groups.AddPosition("kicker", kick_.GetAssignmentPoint(world), disallowed_strikers);
 
   const auto enough_bots_for_defense = available_robots.size() >= 3;
   if (enough_bots_for_defense) {

ateam_kenobi/src/plays/test_plays/controls_test_play.cpp

@@ -32,8 +32,8 @@ ControlsTestPlay::ControlsTestPlay(stp::Options stp_options)
 {
   // Turn 180 deg in place
   // waypoints = {
-  //   {ateam_geometry::Point(-1.0,0.5), AngleMode::face_absolute,  -M_PI/2, 3.0},
-  //   {ateam_geometry::Point(-1.0,0.5), AngleMode::face_absolute, M_PI/2, 3.0}
+  //   {ateam_geometry::Point(1.0, 1.0), motion::AngleMode::face_absolute, 0.0, 3.0},
+  //   {ateam_geometry::Point(1.0, 1.0), motion::AngleMode::face_absolute, M_PI, 3.0},
   // };
 
   // Drive in square

ateam_kenobi/src/plays/test_plays/controls_test_play.hpp

@@ -57,7 +57,7 @@ class ControlsTestPlay : public stp::Play
   std::vector<Waypoint> waypoints;
   bool goal_hit;
   std::chrono::steady_clock::time_point goal_hit_time;
-  double position_threshold = 0.01;
+  double position_threshold = 0.025;
   double angle_threshold = 8.0;
 
   bool isGoalHit(const Robot & robot);

ateam_kenobi/src/skills/capture.hpp

@@ -65,8 +65,8 @@ class Capture : public stp::Skill
 private:
   bool done_ = false;
   int ball_detected_filter_ = 0;
-  double approach_radius_ = 0.3;  // m
-  double capture_speed_ = 0.15;  // m/s
+  double approach_radius_ = 0.25;  // m
+  double capture_speed_ = 0.3;  // m/s
   double max_speed_ = 2.0;  // m/s
   double decel_limit_ = 3.0;  // m/s/s
 

ateam_kenobi/src/skills/line_kick.cpp

@@ -177,8 +177,8 @@ RobotCommand LineKick::RunMoveBehindBall(
 
   command.motion_intent.angular = motion::intents::angular::FacingIntent{target_point_};
 
-  auto vel = ateam_geometry::Vector(0.002, 0);
-  if (ateam_geometry::norm(world.ball.vel) > 0) {
+  auto vel = 0.1 * ateam_geometry::normalize(target_point_ - world.ball.pos);
+  if (ateam_geometry::norm(world.ball.vel) > 0.05) {
     vel = world.ball.vel;
   }
   command.motion_intent.linear =

ateam_kenobi/src/skills/pivot_kick.cpp

@@ -63,7 +63,7 @@ RobotCommand PivotKick::RunFrame(const World & world, const Robot & robot)
 
   const auto robot_to_target = target_point_ - robot.pos;
   const auto robot_to_target_angle = std::atan2(robot_to_target.y(), robot_to_target.x());
-  if (abs(angles::shortest_angular_distance(robot.theta, robot_to_target_angle)) > 0.05) {
+  if (abs(angles::shortest_angular_distance(robot.theta, robot_to_target_angle)) > 0.1) {
     if (prev_state_ != State::Pivot) {
       prev_state_ = State::Pivot;
     }
@@ -89,7 +89,9 @@ RobotCommand PivotKick::Capture(
   const World & world,
   const Robot & robot)
 {
-  return capture_.runFrame(world, robot);
+  RobotCommand capture_result = capture_.runFrame(world, robot);
+  ForwardPlayInfo(capture_);
+  return capture_result;
 }
 
 RobotCommand PivotKick::Pivot(const Robot & robot)
@@ -120,7 +122,7 @@ RobotCommand PivotKick::Pivot(const Robot & robot)
     trapezoidal_vel = vel + (error_direction * pivot_accel_ * dt);
   }
 
-  const auto min_angular_vel = 1.0;
+  const auto min_angular_vel = 0.5;
   if (abs(trapezoidal_vel) < min_angular_vel) {
     trapezoidal_vel = std::copysign(min_angular_vel, angle_error);
   }

ateam_kenobi/src/skills/pivot_kick.hpp

@@ -85,8 +85,8 @@ class PivotKick : public KickSkill
   ateam_geometry::Point target_point_;
   skills::Capture capture_;
   bool done_ = false;
-  double pivot_speed_ = 2.0;  // rad/s
-  double pivot_accel_ = 1.5;  // rad/s^2
+  double pivot_speed_ = 2.8;  // rad/s
+  double pivot_accel_ = 3.5;  // rad/s^2
 
   enum class State
   {

ateam_kenobi/src/core/ballsense_filter.hpp → state_tracking/ateam_game_state/src/game_state_tracker/ballsense_filter.hpp

@@ -19,14 +19,14 @@
 // THE SOFTWARE.
 
 
-#ifndef CORE__BALLSENSE_FILTER_HPP_
-#define CORE__BALLSENSE_FILTER_HPP_
+#ifndef GAME_STATE_TRACKER__BALLSENSE_FILTER_HPP_
+#define GAME_STATE_TRACKER__BALLSENSE_FILTER_HPP_
 
 #include <ranges>
 #include <array>
-#include "core/types/state_types.hpp"
+#include "ateam_game_state/world.hpp"
 
-namespace ateam_kenobi
+namespace ateam_game_state
 {
 
 class BallSenseFilter
@@ -79,6 +79,6 @@ class BallSenseFilter
   }
 };
 
-}  // namespace ateam_kenobi
+}  // namespace ateam_game_state
 
-#endif  // CORE__BALLSENSE_FILTER_HPP_
+#endif  // GAME_STATE_TRACKER__BALLSENSE_FILTER_HPP_