Use RobotState instead of individual variables for prim exec

src/software/embedded/primitive_executor.cpp

@@ -22,30 +22,31 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m
 
     if (current_primitive_.has_move())
     {
-        trajectory_path_ = createTrajectoryPathFromParams(
-            current_primitive_.move().xy_traj_params(), velocity_, robot_constants_);
+        trajectory_path_ =
+            createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(),
+                                           state_.velocity(), robot_constants_);
 
         angular_trajectory_ =
             createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(),
-                                              angular_velocity_, robot_constants_);
+                                              state_.angularVelocity(), robot_constants_);
 
         time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S);
     }
 }
 
-void PrimitiveExecutor::updateVelocity(const Vector& local_velocity,
-                                       const AngularVelocity& angular_velocity)
+void PrimitiveExecutor::updateState(const RobotState& state)
 {
-    Vector actual_global_velocity = localToGlobalVelocity(local_velocity, orientation_);
-    velocity_                     = actual_global_velocity;
-    angular_velocity_             = angular_velocity;
+    Vector actual_global_velocity =
+        localToGlobalVelocity(state.localVelocity(), state_.orientation());
+    state_.setVelocity(actual_global_velocity);
+    state_.setAngularVelocity(state.angularVelocity());
 }
 
 Vector PrimitiveExecutor::getTargetLinearVelocity()
 {
     Vector local_velocity = globalToLocalVelocity(
         trajectory_path_->getVelocity(time_since_trajectory_creation_.toSeconds()),
-        orientation_);
+        state_.orientation());
     Point position =
         trajectory_path_->getPosition(time_since_trajectory_creation_.toSeconds());
     double distance_to_destination =
@@ -61,13 +62,13 @@ Vector PrimitiveExecutor::getTargetLinearVelocity()
 
 AngularVelocity PrimitiveExecutor::getTargetAngularVelocity()
 {
-    orientation_ =
-        angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds());
+    state_.setOrientation(
+        angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds()));
 
     AngularVelocity angular_velocity =
         angular_trajectory_->getVelocity(time_since_trajectory_creation_.toSeconds());
     Angle orientation_to_destination =
-        orientation_.minDiff(angular_trajectory_->getDestination());
+        state_.orientation().minDiff(angular_trajectory_->getDestination());
     if (orientation_to_destination.toDegrees() < 5)
     {
         angular_velocity *= orientation_to_destination.toDegrees() / 5;

src/software/embedded/primitive_executor.h

@@ -26,13 +26,11 @@ class PrimitiveExecutor
     void updatePrimitive(const TbotsProto::Primitive& primitive_msg);
 
     /**
-     * Update primitive executor with the current velocity of the robot
+     * Update primitive executor with the state of the robot
      *
-     * @param local_velocity The current _local_ velocity
-     * @param angular_velocity The current angular velocity
+     * @param state The current robot state
      */
-    void updateVelocity(const Vector& local_velocity,
-                        const AngularVelocity& angular_velocity);
+    void updateState(const RobotState& state);
 
     /**
      * Steps the current primitive and returns a direct control primitive with the
@@ -61,11 +59,9 @@ class PrimitiveExecutor
      */
     AngularVelocity getTargetAngularVelocity();
 
+    RobotState state_;
     TbotsProto::Primitive current_primitive_;
     Duration time_since_trajectory_creation_;
-    Vector velocity_;
-    AngularVelocity angular_velocity_;
-    Angle orientation_;
     robot_constants::RobotConstants robot_constants_;
     std::optional<TrajectoryPath> trajectory_path_;
     std::optional<BangBangTrajectory1DAngular> angular_trajectory_;

src/software/embedded/thunderloop.cpp

@@ -18,6 +18,7 @@
 #include "software/networking/tbots_network_exception.h"
 #include "software/tracy/tracy_constants.h"
 #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h"
+#include "software/world/robot_state.h"
 
 /**
  * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example
@@ -262,9 +263,9 @@ void Thunderloop::runLoop()
             if (motor_status_.has_value())
             {
                 auto status = motor_status_.value();
-                primitive_executor_.updateVelocity(
-                    createVector(status.local_velocity()),
-                    createAngularVelocity(status.angular_velocity()));
+                primitive_executor_.updateState(
+                    RobotState(Point(), createVector(status.local_velocity()), Angle(),
+                               createAngularVelocity(status.angular_velocity())));
             }
 
             // Timeout Overrides for Primitives

src/software/simulation/er_force_simulator.cpp

@@ -297,19 +297,19 @@ void ErForceSimulator::setYellowRobotPrimitiveSet(
     const TbotsProto::PrimitiveSet& primitive_set_msg,
     std::unique_ptr<TbotsProto::World> world_msg)
 {
-    auto sim_state                   = getSimulatorState();
-    const auto& sim_robots           = sim_state.yellow_robots();
-    const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots);
+    auto sim_state         = getSimulatorState();
+    const auto& sim_robots = sim_state.yellow_robots();
+    const auto robot_map   = getRobotIdToRobotStateMap(sim_robots);
 
     yellow_team_world_msg               = std::move(world_msg);
     const TbotsProto::World world_proto = *yellow_team_world_msg;
     for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives())
     {
-        if (robot_to_vel_pair_map.contains(robot_id))
+        if (robot_map.contains(robot_id))
         {
-            auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id);
+            const auto& robot_state = robot_map.at(robot_id);
             setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map,
-                              world_proto, local_vel, angular_vel);
+                              world_proto, robot_state);
         }
     }
 }
@@ -318,20 +318,20 @@ void ErForceSimulator::setBlueRobotPrimitiveSet(
     const TbotsProto::PrimitiveSet& primitive_set_msg,
     std::unique_ptr<TbotsProto::World> world_msg)
 {
-    auto sim_state                   = getSimulatorState();
-    const auto& sim_robots           = sim_state.blue_robots();
-    const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots);
+    auto sim_state         = getSimulatorState();
+    const auto& sim_robots = sim_state.blue_robots();
+    const auto robot_map   = getRobotIdToRobotStateMap(sim_robots);
 
     blue_team_world_msg                 = std::move(world_msg);
     const TbotsProto::World world_proto = *blue_team_world_msg;
 
     for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives())
     {
-        if (robot_to_vel_pair_map.contains(robot_id))
+        if (robot_map.contains(robot_id))
         {
-            auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id);
+            const auto& robot_state = robot_map.at(robot_id);
             setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map,
-                              world_proto, local_vel, angular_vel);
+                              world_proto, robot_state);
         }
     }
 }
@@ -340,8 +340,7 @@ void ErForceSimulator::setRobotPrimitive(
     RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg,
     std::unordered_map<unsigned int, std::shared_ptr<PrimitiveExecutor>>&
         robot_primitive_executor_map,
-    const TbotsProto::World& world_msg, const Vector& local_velocity,
-    const AngularVelocity angular_velocity)
+    const TbotsProto::World& world_msg, const RobotState& robot_state)
 {
     // Set to NEG_X because the world msg in this simulator is normalized
     // correctly
@@ -351,7 +350,7 @@ void ErForceSimulator::setRobotPrimitive(
     {
         auto primitive_executor = robot_primitive_executor_iter->second;
         primitive_executor->updatePrimitive(primitive_set_msg.robot_primitives().at(id));
-        primitive_executor->updateVelocity(local_velocity, angular_velocity);
+        primitive_executor->updateState(robot_state);
     }
     else
     {
@@ -367,34 +366,25 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots(
 {
     SSLSimulationProto::RobotControl robot_control;
 
-    auto sim_state = getSimulatorState();
-    std::map<RobotId, std::pair<Vector, Angle>> current_velocity_map;
-    if (side == gameController::Team::BLUE)
-    {
-        const auto& sim_robots = sim_state.blue_robots();
-        current_velocity_map   = getRobotIdToLocalVelocityMap(sim_robots);
-    }
-    else
-    {
-        const auto& sim_robots = sim_state.yellow_robots();
-        current_velocity_map   = getRobotIdToLocalVelocityMap(sim_robots);
-    }
+    auto sim_state         = getSimulatorState();
+    const auto& sim_robots = (side == gameController::Team::BLUE)
+                                 ? sim_state.blue_robots()
+                                 : sim_state.yellow_robots();
+    const auto robot_map   = getRobotIdToRobotStateMap(sim_robots);
 
-    for (auto& primitive_executor_with_id : robot_primitive_executor_map)
+    for (auto& [robot_id, primitive_executor] : robot_primitive_executor_map)
     {
-        unsigned int robot_id    = primitive_executor_with_id.first;
-        auto& primitive_executor = primitive_executor_with_id.second;
         std::unique_ptr<TbotsProto::DirectControlPrimitive> direct_control;
 
         TbotsProto::PrimitiveExecutorStatus status;  // Added for compilation
         if (ramping)
         {
             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);
+            const auto& robot_state = robot_map.at(robot_id);
+            direct_control          = getRampedVelocityPrimitive(
+                         robot_state.localVelocity(), robot_state.angularVelocity(),
+                         *direct_control_no_ramp, primitive_executor_time_step);
         }
         else
         {
@@ -563,18 +553,18 @@ void ErForceSimulator::resetCurrentTime()
     current_time = Timestamp::fromSeconds(0);
 }
 
-std::map<RobotId, std::pair<Vector, AngularVelocity>>
-ErForceSimulator::getRobotIdToLocalVelocityMap(
+std::map<RobotId, RobotState> ErForceSimulator::getRobotIdToRobotStateMap(
     const google::protobuf::RepeatedPtrField<world::SimRobot>& sim_robots)
 {
-    std::map<RobotId, std::pair<Vector, AngularVelocity>> robot_to_local_velocity;
+    std::map<RobotId, RobotState> robot_map;
     for (const auto& sim_robot : sim_robots)
     {
-        const Vector local_vel =
-            globalToLocalVelocity(Vector(sim_robot.v_x(), sim_robot.v_y()),
-                                  Angle::fromRadians(sim_robot.angle()));
-        const AngularVelocity angular_vel       = Angle::fromRadians(sim_robot.r_z());
-        robot_to_local_velocity[sim_robot.id()] = {local_vel, angular_vel};
+        const auto position         = Point(sim_robot.p_x(), sim_robot.p_y());
+        const auto velocity         = Vector(sim_robot.v_x(), sim_robot.v_y());
+        const auto orientation      = Angle::fromRadians(sim_robot.angle());
+        const auto angular_velocity = AngularVelocity::fromRadians(sim_robot.r_z());
+        robot_map[sim_robot.id()] =
+            RobotState(position, velocity, orientation, angular_velocity);
     }
-    return robot_to_local_velocity;
+    return robot_map;
 }

src/software/simulation/er_force_simulator.h

@@ -7,6 +7,7 @@
 #include "software/embedded/primitive_executor.h"
 #include "software/physics/euclidean_to_wheel.h"
 #include "software/world/field.h"
+#include "software/world/robot_state.h"
 #include "software/world/team_types.h"
 #include "software/world/world.h"
 
@@ -158,8 +159,7 @@ class ErForceSimulator
         RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg,
         std::unordered_map<unsigned int, std::shared_ptr<PrimitiveExecutor>>&
             robot_primitive_executor_map,
-        const TbotsProto::World& world_msg, const Vector& local_velocity,
-        const AngularVelocity angular_velocity);
+        const TbotsProto::World& world_msg, const RobotState& robot_state);
 
     /**
      * Gets a map from robot id to local and angular velocity from repeated sim robots
@@ -168,8 +168,7 @@ class ErForceSimulator
      *
      * @return a map from robot id to local velocity and angular velocity
      */
-    static std::map<RobotId, std::pair<Vector, AngularVelocity>>
-    getRobotIdToLocalVelocityMap(
+    static std::map<RobotId, RobotState> getRobotIdToRobotStateMap(
         const google::protobuf::RepeatedPtrField<world::SimRobot>& sim_robots);
 
     /**

src/software/world/BUILD

@@ -139,6 +139,7 @@ cc_library(
         "//software/geom:angular_velocity",
         "//software/geom:point",
         "//software/geom:vector",
+        "//software/physics:velocity_conversion_util",
         "//software/time:duration",
     ],
 )

src/software/world/robot_state.cpp

@@ -1,5 +1,7 @@
 #include "software/world/robot_state.h"
 
+#include "software/physics/velocity_conversion_util.h"
+
 RobotState::RobotState(const Point& position, const Vector& velocity,
                        const Angle& orientation, const AngularVelocity& angular_velocity,
                        const bool breakbeam_tripped)
@@ -33,6 +35,11 @@ Vector RobotState::velocity() const
     return velocity_;
 }
 
+Vector RobotState::localVelocity() const
+{
+    return globalToLocalVelocity(velocity_, orientation_);
+}
+
 Angle RobotState::orientation() const
 {
     return orientation_;
@@ -48,6 +55,26 @@ bool RobotState::breakbeamTripped() const
     return breakbeam_tripped_;
 }
 
+void RobotState::setPosition(const Point& position)
+{
+    position_ = position;
+}
+
+void RobotState::setVelocity(const Vector& velocity)
+{
+    velocity_ = velocity;
+}
+
+void RobotState::setOrientation(const Angle& orientation)
+{
+    orientation_ = orientation;
+}
+
+void RobotState::setAngularVelocity(const AngularVelocity& angular_velocity)
+{
+    angular_velocity_ = angular_velocity;
+}
+
 bool RobotState::operator==(const RobotState& other) const
 {
     return this->position() == other.position() && this->velocity() == other.velocity() &&

src/software/world/robot_state.h

@@ -17,6 +17,11 @@ using RobotId = unsigned int;
 class RobotState
 {
    public:
+    /**
+     * Creates a robot state with default values
+     */
+    RobotState() = default;
+
     /**
      * Creates a new robot state with the given position, velocity, orientation, angular
      * velocity, and timestamp
@@ -54,6 +59,13 @@ class RobotState
      */
     Vector velocity() const;
 
+    /**
+     * Returns the local velocity of the robot represented by this state
+     *
+     * @return the local velocity of the robot represented by this state
+     */
+    Vector localVelocity() const;
+
     /**
      * Returns the orientation of the robot represented by this state
      *
@@ -75,6 +87,34 @@ class RobotState
      */
     bool breakbeamTripped() const;
 
+    /**
+     * Sets the position of the robot, with coordinates in metres
+     *
+     * @param position The new position of the robot
+     */
+    void setPosition(const Point& position);
+
+    /**
+     * Sets the velocity of the robot, in metres per second
+     *
+     * @param velocity The new velocity of the robot
+     */
+    void setVelocity(const Vector& velocity);
+
+    /**
+     * Sets the orientation of the robot
+     *
+     * @param orientation The new orientation of the robot
+     */
+    void setOrientation(const Angle& orientation);
+
+    /**
+     * Sets the angular velocity of the robot
+     *
+     * @param angular_velocity The new angular velocity of the robot
+     */
+    void setAngularVelocity(const AngularVelocity& angular_velocity);
+
     /**
      * Defines the equality operator for a RobotState. RobotStates are equal if
      * all their members are equal