From 5a3fb6d2f3270c4618f2834f465c082a9b35b55d Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 14:38:34 -0700 Subject: [PATCH 1/5] Remove unused member functions and variables --- src/software/embedded/primitive_executor.cpp | 19 ++----------------- src/software/embedded/primitive_executor.h | 19 +------------------ src/software/embedded/thunderloop.cpp | 4 +--- .../simulation/er_force_simulator.cpp | 6 ++---- 4 files changed, 6 insertions(+), 42 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index d1f56161ee..86dd75f7d8 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,13 +11,8 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, const RobotId robot_id) - : current_primitive_(), - friendly_team_colour_(friendly_team_colour), - robot_constants_(robot_constants), - time_step_(time_step), - robot_id_(robot_id) + const Duration time_step, const robot_constants::RobotConstants& robot_constants) + : current_primitive_(), robot_constants_(robot_constants), time_step_(time_step) { } @@ -38,11 +33,6 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m } } -void PrimitiveExecutor::setStopPrimitive() -{ - current_primitive_ = *createStopPrimitiveProto(); -} - void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, const AngularVelocity& angular_velocity) { @@ -145,8 +135,3 @@ std::unique_ptr PrimitiveExecutor::stepPrimi } return std::make_unique(); } - -void PrimitiveExecutor::setRobotId(const RobotId robot_id) -{ - robot_id_ = robot_id; -} diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 145874f3c2..52d9de711e 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -17,13 +17,9 @@ class PrimitiveExecutor * @param time_step Time step which this primitive executor operates in * @param robot_constants The robot constants for the robot which uses this primitive * executor - * @param friendly_team_colour The colour of the friendly team - * @param robot_id The id of the robot which uses this primitive executor */ explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, - const RobotId robot_id); + const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive @@ -31,11 +27,6 @@ class PrimitiveExecutor */ void updatePrimitive(const TbotsProto::Primitive& primitive_msg); - /** - * Set the current primitive to the stop primitive - */ - void setStopPrimitive(); - /** * Update primitive executor with the current velocity of the robot * @@ -45,12 +36,6 @@ class PrimitiveExecutor void updateVelocity(const Vector& local_velocity, const AngularVelocity& angular_velocity); - /** - * Set the robot id - * @param robot_id The id of the robot which uses this primitive executor - */ - void setRobotId(RobotId robot_id); - /** * Steps the current primitive and returns a direct control primitive with the * target wheel velocities @@ -81,7 +66,6 @@ class PrimitiveExecutor Vector velocity_; AngularVelocity angular_velocity_; Angle orientation_; - TeamColour friendly_team_colour_; robot_constants::RobotConstants robot_constants_; std::optional trajectory_path_; std::optional angular_trajectory_; @@ -89,7 +73,6 @@ class PrimitiveExecutor // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant // time step to be used, in Seconds Duration time_step_; - RobotId robot_id_; // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index afc30bdd03..11bfc8489f 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -71,7 +71,6 @@ extern "C" Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz) - // TODO (#2495): Set the friendly team colour : toml_config_client_(std::make_unique(TOML_CONFIG_FILE_PATH)), motor_status_(std::nullopt), robot_constants_(robot_constants), @@ -84,8 +83,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants, - TeamColour::YELLOW, robot_id_) + primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants) { waitForNetworkUp(); diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7a645b85e1..c609bff582 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -281,15 +281,13 @@ void ErForceSimulator::setRobots( if (side == gameController::Team::BLUE) { auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::BLUE, id); + Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::YELLOW, id); + Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } From 868cf6b35c214a94249f18eaba4487f6ef636625 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 15:06:32 -0700 Subject: [PATCH 2/5] Fix call to setStopPrimitive this is removed even thought is technically used now cause future changes will get rid of it --- src/software/embedded/thunderloop.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 11bfc8489f..c2d9c08a70 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -4,6 +4,7 @@ #include #include "proto/message_translation/tbots_protobuf.h" +#include "proto/primitive/primitive_msg_factory.h" #include "proto/robot_crash_msg.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" @@ -282,7 +283,7 @@ void Thunderloop::runLoop() if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) { - primitive_executor_.setStopPrimitive(); + primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } direct_control_ = From 67b3833f3e3fc8069e43d4c6edc06dec7568134c Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 14:49:33 -0700 Subject: [PATCH 3/5] Pass in delta_time dynamically for prim exec --- src/software/embedded/primitive_executor.cpp | 8 ++++---- src/software/embedded/primitive_executor.h | 12 ++++-------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 86dd75f7d8..8961861eff 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,8 +11,8 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants) - : current_primitive_(), robot_constants_(robot_constants), time_step_(time_step) + const robot_constants::RobotConstants& robot_constants) + : current_primitive_(), robot_constants_(robot_constants) { } @@ -78,9 +78,9 @@ AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() std::unique_ptr PrimitiveExecutor::stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status) + TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time) { - time_since_trajectory_creation_ += time_step_; + time_since_trajectory_creation_ += delta_time; status.set_running_primitive(true); switch (current_primitive_.primitive_case()) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 52d9de711e..f0fb3d2253 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -14,12 +14,10 @@ class PrimitiveExecutor public: /** * Constructor - * @param time_step Time step which this primitive executor operates in * @param robot_constants The robot constants for the robot which uses this primitive * executor */ - explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants); + explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive @@ -39,13 +37,15 @@ class PrimitiveExecutor /** * Steps the current primitive and returns a direct control primitive with the * target wheel velocities + * * @param status The status of the primitive executor, set to false if current * primitive is a Stop primitive + * @param delta_time The elapsed time since the last primitive step * * @returns DirectControlPrimitive The direct control primitive msg */ std::unique_ptr stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status); + TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time); private: /* @@ -70,10 +70,6 @@ class PrimitiveExecutor std::optional trajectory_path_; std::optional angular_trajectory_; - // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant - // time step to be used, in Seconds - Duration time_step_; - // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; From dd126dbd05c29bd40162e1215af6a287cca970c5 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 15:03:43 -0700 Subject: [PATCH 4/5] Fix calls to prim exec constructor and stepPrimitive --- src/software/embedded/thunderloop.cpp | 4 +-- .../simulation/er_force_simulator.cpp | 31 ++++++++++--------- src/software/simulation/er_force_simulator.h | 16 +++++----- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index c2d9c08a70..01445023b0 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -84,7 +84,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants) + primitive_executor_(robot_constants) { waitForNetworkUp(); @@ -287,7 +287,7 @@ void Thunderloop::runLoop() } direct_control_ = - *primitive_executor_.stepPrimitive(primitive_executor_status_); + *primitive_executor_.stepPrimitive(primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_)); } thunderloop_status_.set_primitive_executor_step_time_ms( diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index c609bff582..ba038290f0 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -20,10 +20,10 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping, - double primitive_executor_time_step) + Duration primitive_executor_time_step) : yellow_team_world_msg(std::make_unique()), blue_team_world_msg(std::make_unique()), - primitive_executor_time_step_s(primitive_executor_time_step), + primitive_executor_time_step(primitive_executor_time_step), frame_number(0), euclidean_to_four_wheel(robot_constants), robot_constants(robot_constants), @@ -280,14 +280,14 @@ void ErForceSimulator::setRobots( { if (side == gameController::Team::BLUE) { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); + auto robot_primitive_executor = + std::make_shared(robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); + auto robot_primitive_executor = + std::make_shared(robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } @@ -389,15 +389,17 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( TbotsProto::PrimitiveExecutorStatus status; // Added for compilation if (ramping) { - auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); - direct_control = getRampedVelocityPrimitive( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *direct_control_no_ramp, - primitive_executor_time_step_s); + auto direct_control_no_ramp = + primitive_executor->stepPrimitive(status, primitive_executor_time_step); + direct_control = getRampedVelocityPrimitive( + current_velocity_map.at(robot_id).first, + current_velocity_map.at(robot_id).second, *direct_control_no_ramp, + primitive_executor_time_step); } else { - direct_control = primitive_executor->stepPrimitive(status); + direct_control = + primitive_executor->stepPrimitive(status, primitive_executor_time_step); } auto command = *getRobotCommandFromDirectControl( @@ -411,8 +413,7 @@ std::unique_ptr ErForceSimulator::getRampedVelocityPrimitive( const Vector current_local_velocity, const AngularVelocity current_local_angular_velocity, - TbotsProto::DirectControlPrimitive& target_velocity_primitive, - const double& time_to_ramp) + TbotsProto::DirectControlPrimitive& target_velocity_primitive, Duration time_to_ramp) { TbotsProto::MotorControl_DirectVelocityControl direct_velocity = target_velocity_primitive.motor_control().direct_velocity_control(); @@ -435,7 +436,7 @@ ErForceSimulator::getRampedVelocityPrimitive( euclidean_to_four_wheel.getWheelVelocity(current_euclidean_velocity); WheelSpace_t ramped_four_wheel = euclidean_to_four_wheel.rampWheelVelocity( - current_wheel_velocity, target_wheel_velocity, time_to_ramp); + current_wheel_velocity, target_wheel_velocity, time_to_ramp.toSeconds()); EuclideanSpace_t ramped_euclidean = euclidean_to_four_wheel.getEuclideanVelocity(ramped_four_wheel); diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 80459c4df4..429b16d503 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -27,12 +27,12 @@ class ErForceSimulator * @param robot_constants The robot constants * @param realism_config realism configuration */ - explicit ErForceSimulator(const TbotsProto::FieldType& field_type, - const robot_constants::RobotConstants& robot_constants, - std::unique_ptr& realism_config, - const bool ramping = false, - double primitive_executor_time_step_s = - DEFAULT_SIMULATOR_TICK_RATE_SECONDS_PER_TICK); + explicit ErForceSimulator( + const TbotsProto::FieldType& field_type, + const robot_constants::RobotConstants& robot_constants, + std::unique_ptr& realism_config, const bool ramping = false, + Duration primitive_executor_time_step_s = + Duration::fromSeconds(DEFAULT_SIMULATOR_TICK_RATE_SECONDS_PER_TICK)); ErForceSimulator() = delete; ~ErForceSimulator() = default; @@ -202,7 +202,7 @@ class ErForceSimulator const Vector current_local_velocity, const AngularVelocity current_local_angular_velocity, TbotsProto::DirectControlPrimitive& target_velocity_primitive, - const double& time_to_ramp); + Duration time_to_ramp); // Map of Robot id to Primitive Executor std::unordered_map> @@ -212,7 +212,7 @@ class ErForceSimulator std::unique_ptr yellow_team_world_msg; std::unique_ptr blue_team_world_msg; - double primitive_executor_time_step_s; + Duration primitive_executor_time_step; unsigned int frame_number; // The current time. From 5f6563a7747faee352a0168e2b107d48c7fe16b8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 5 Jun 2026 00:36:54 +0000 Subject: [PATCH 5/5] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/thunderloop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 01445023b0..bcd527699c 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -286,8 +286,8 @@ void Thunderloop::runLoop() primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } - direct_control_ = - *primitive_executor_.stepPrimitive(primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_)); + direct_control_ = *primitive_executor_.stepPrimitive( + primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_)); } thunderloop_status_.set_primitive_executor_step_time_ms(