er_force_simulator.h

#pragma once
 
#include "extlibs/er_force_sim/src/amun/simulator/simulator.h"
#include "proto/robot_status_msg.pb.h"
#include "proto/ssl_vision_wrapper.pb.h"
#include "proto/tbots_software_msgs.pb.h"
#include "software/embedded/primitive_executor.h"
#include "software/physics/euclidean_to_wheel.h"
#include "software/world/field.h"
#include "software/world/team_types.h"
#include "software/world/world.h"
 
 
class ErForceSimulator
{
   public:
    explicit ErForceSimulator(const TbotsProto::FieldType& field_type,
                              const RobotConstants_t& 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);
    ErForceSimulator()  = delete;
    ~ErForceSimulator() = default;
 
    void setBallState(const BallState& ball_state);
 
    void setYellowRobots(const std::vector<RobotStateWithId>& robots);
    void setBlueRobots(const std::vector<RobotStateWithId>& robots);
    void setRobots(const std::vector<RobotStateWithId>& robots,
                   gameController::Team team);
    void setRobots(const google::protobuf::Map<uint32_t, TbotsProto::RobotState>& robots,
                   gameController::Team side);
 
    void setWorldState(const TbotsProto::WorldState& world_state);
 
    void setYellowRobotPrimitiveSet(const TbotsProto::PrimitiveSet& primitive_set_msg,
                                    std::unique_ptr<TbotsProto::World> world_msg);
    void setBlueRobotPrimitiveSet(const TbotsProto::PrimitiveSet& primitive_set_msg,
                                  std::unique_ptr<TbotsProto::World> world_msg);
 
    void stepSimulation(const Duration& time_step);
 
    std::vector<TbotsProto::RobotStatus> getBlueRobotStatuses() const;
    std::vector<TbotsProto::RobotStatus> getYellowRobotStatuses() const;
 
    std::vector<SSLProto::SSL_WrapperPacket> getSSLWrapperPackets() const;
 
    world::SimulatorState getSimulatorState() const;
 
    Field getField() const;
 
    Timestamp getTimestamp() const;
 
    void resetCurrentTime();
 
    static std::unique_ptr<RealismConfigErForce> createDefaultRealismConfig();
 
    static std::unique_ptr<RealismConfigErForce> createRealisticRealismConfig();
 
   private:
    static void 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);
 
    static std::map<RobotId, std::pair<Vector, AngularVelocity>>
    getRobotIdToLocalVelocityMap(
        const google::protobuf::RepeatedPtrField<world::SimRobot>& sim_robots);
 
    SSLSimulationProto::RobotControl updateSimulatorRobots(
        std::unordered_map<unsigned int, std::shared_ptr<PrimitiveExecutor>>&
            robot_primitive_executor_map,
        const TbotsProto::World& world_msg, gameController::Team side);
 
    std::unique_ptr<TbotsProto::DirectControlPrimitive> getRampedVelocityPrimitive(
        const Vector current_local_velocity,
        const AngularVelocity current_local_angular_velocity,
        TbotsProto::DirectControlPrimitive& target_velocity_primitive,
        const double& time_to_ramp);
 
    // Map of Robot id to Primitive Executor
    std::unordered_map<unsigned int, std::shared_ptr<PrimitiveExecutor>>
        yellow_primitive_executor_map;
    std::unordered_map<unsigned int, std::shared_ptr<PrimitiveExecutor>>
        blue_primitive_executor_map;
    std::unique_ptr<TbotsProto::World> yellow_team_world_msg;
    std::unique_ptr<TbotsProto::World> blue_team_world_msg;
 
    double primitive_executor_time_step_s;
    unsigned int frame_number;
 
    // The current time.
    Timestamp current_time;
 
    amun::SimulatorSetup er_force_sim_setup;
    std::unique_ptr<camun::simulator::Simulator> er_force_sim;
    EuclideanToWheel euclidean_to_four_wheel;
 
    RobotConstants_t robot_constants;
    Field field;
 
    std::optional<RobotId> blue_robot_with_ball;
    std::optional<RobotId> yellow_robot_with_ball;
 
    bool ramping;
 
    const std::string CONFIG_FILE      = "simulator/2020";
    const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/";
};