robot_state.h

#pragma once
 
#include "proto/vision.pb.h"
#include "shared/2021_robot_constants.h"
#include "shared/constants.h"
#include "shared/robot_constants.h"
#include "software/geom/angle.h"
#include "software/geom/angular_velocity.h"
#include "software/geom/point.h"
#include "software/geom/vector.h"
#include "software/world/team_types.h"
 
using RobotId = unsigned int;
 
class RobotState
{
   public:
    explicit RobotState(const Point &position, const Vector &velocity,
                        const Angle &orientation,
                        const AngularVelocity &angular_velocity);
 
    explicit RobotState(const TbotsProto::RobotState &robot_state_proto);
 
    Point position() const;
 
    Vector velocity() const;
 
    Angle orientation() const;
 
    AngularVelocity angularVelocity() const;
 
    bool operator==(const RobotState &other) const;
 
    bool operator!=(const RobotState &other) const;
 
   private:
    Point position_;
    Vector velocity_;
    Angle orientation_;
    AngularVelocity angular_velocity_;
};
 
struct RobotStateWithId
{
    unsigned int id;
    RobotState robot_state;
 
    bool operator==(const RobotStateWithId &other) const
    {
        return id == other.id && robot_state == other.robot_state;
    }
};