Pass in delta_time dynamically for prim exec

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<TbotsProto::DirectControlPrimitive> 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())

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<TbotsProto::DirectControlPrimitive> stepPrimitive(
-        TbotsProto::PrimitiveExecutorStatus& status);
+        TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time);
 
    private:
     /*
@@ -70,10 +70,6 @@ class PrimitiveExecutor
     std::optional<TrajectoryPath> trajectory_path_;
     std::optional<BangBangTrajectory1DAngular> 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;
 

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();
 
@@ -286,8 +286,8 @@ void Thunderloop::runLoop()
                     primitive_executor_.updatePrimitive(*createStopPrimitiveProto());
                 }
 
-                direct_control_ =
-                    *primitive_executor_.stepPrimitive(primitive_executor_status_);
+                direct_control_ = *primitive_executor_.stepPrimitive(
+                    primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_));
             }
 
             thunderloop_status_.set_primitive_executor_step_time_ms(

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<RealismConfigErForce>& realism_config,
                                    const bool ramping,
-                                   double primitive_executor_time_step)
+                                   Duration primitive_executor_time_step)
     : yellow_team_world_msg(std::make_unique<TbotsProto::World>()),
       blue_team_world_msg(std::make_unique<TbotsProto::World>()),
-      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<PrimitiveExecutor>(
-                Duration::fromSeconds(primitive_executor_time_step_s), robot_constants);
+            auto robot_primitive_executor =
+                std::make_shared<PrimitiveExecutor>(robot_constants);
             blue_primitive_executor_map.insert({id, robot_primitive_executor});
         }
         else
         {
-            auto robot_primitive_executor = std::make_shared<PrimitiveExecutor>(
-                Duration::fromSeconds(primitive_executor_time_step_s), robot_constants);
+            auto robot_primitive_executor =
+                std::make_shared<PrimitiveExecutor>(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<TbotsProto::DirectControlPrimitive>
 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);

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<RealismConfigErForce>& 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<RealismConfigErForce>& 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<unsigned int, std::shared_ptr<PrimitiveExecutor>>
@@ -212,7 +212,7 @@ class ErForceSimulator
     std::unique_ptr<TbotsProto::World> yellow_team_world_msg;
     std::unique_ptr<TbotsProto::World> blue_team_world_msg;
 
-    double primitive_executor_time_step_s;
+    Duration primitive_executor_time_step;
     unsigned int frame_number;
 
     // The current time.