Software/src/software/world/robot_state.cpp at 467fd3b3c5a25814d82d7939c16fe4ac032b9539 · UBC-Thunderbots/Software · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
#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)
    : position_(position),
      velocity_(velocity),
      orientation_(orientation),
      angular_velocity_(angular_velocity),
      breakbeam_tripped_(breakbeam_tripped)
{
}

RobotState::RobotState(const TbotsProto::RobotState& robot_state_proto)
    : position_(Point(robot_state_proto.global_position().x_meters(),
                      robot_state_proto.global_position().y_meters())),
      velocity_(Vector(robot_state_proto.global_velocity().x_component_meters(),
                       robot_state_proto.global_velocity().y_component_meters())),
      orientation_(Angle::fromRadians(robot_state_proto.global_orientation().radians())),
      angular_velocity_(AngularVelocity::fromRadians(
          robot_state_proto.global_angular_velocity().radians_per_second())),
      breakbeam_tripped_(false)
{
}

Point RobotState::position() const
{
    return position_;
}

Vector RobotState::velocity() const
{
    return velocity_;
}

Vector RobotState::localVelocity() const
{
    return globalToLocalVelocity(velocity_, orientation_);
}

Angle RobotState::orientation() const
{
    return orientation_;
}

AngularVelocity RobotState::angularVelocity() const
{
    return angular_velocity_;
}

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() &&
           this->orientation() == other.orientation() &&
           this->angularVelocity() == other.angularVelocity();
}

bool RobotState::operator!=(const RobotState& other) const
{
    return !(*this == other);
}