diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 8961861eff..2859bc6c87 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/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; diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index f0fb3d2253..df081b321d 100644 --- a/src/software/embedded/primitive_executor.h +++ b/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 trajectory_path_; std::optional angular_trajectory_; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index bcd527699c..15916a04fe 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/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 diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index ba038290f0..ba150e3cb2 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -297,19 +297,19 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr 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 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>& 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,23 +366,14 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { SSLSimulationProto::RobotControl robot_control; - auto sim_state = getSimulatorState(); - std::map> 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 direct_control; TbotsProto::PrimitiveExecutorStatus status; // Added for compilation @@ -391,10 +381,10 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { 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> -ErForceSimulator::getRobotIdToLocalVelocityMap( +std::map ErForceSimulator::getRobotIdToRobotStateMap( const google::protobuf::RepeatedPtrField& sim_robots) { - std::map> robot_to_local_velocity; + std::map 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; } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 429b16d503..19a6f16fd6 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/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>& 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> - getRobotIdToLocalVelocityMap( + static std::map getRobotIdToRobotStateMap( const google::protobuf::RepeatedPtrField& sim_robots); /** diff --git a/src/software/world/BUILD b/src/software/world/BUILD index 96693b306f..b06e1a0d92 100644 --- a/src/software/world/BUILD +++ b/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", ], ) diff --git a/src/software/world/robot_state.cpp b/src/software/world/robot_state.cpp index 2e9f4c0b79..0e8fa01cad 100644 --- a/src/software/world/robot_state.cpp +++ b/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() && diff --git a/src/software/world/robot_state.h b/src/software/world/robot_state.h index 9f62084931..d4832badab 100644 --- a/src/software/world/robot_state.h +++ b/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