-
Notifications
You must be signed in to change notification settings - Fork 4.5k
/
Environment.hpp
128 lines (108 loc) · 3.58 KB
/
Environment.hpp
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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.
#ifndef airsim_core_Environment_hpp
#define airsim_core_Environment_hpp
#include "common/Common.hpp"
#include "common/UpdatableObject.hpp"
#include "common/CommonStructs.hpp"
#include "common/EarthUtils.hpp"
namespace msr
{
namespace airlib
{
class Environment : public UpdatableObject
{
public:
struct State
{
//these fields must be set at initialization time
Vector3r position;
GeoPoint geo_point;
//these fields are computed
Vector3r gravity;
real_T air_pressure;
real_T temperature;
real_T air_density;
State()
{
}
State(const Vector3r& position_val, const GeoPoint& geo_point_val)
: position(position_val), geo_point(geo_point_val)
{
}
};
public:
Environment()
{
//allow default constructor with later call for initialize
}
Environment(const State& initial)
{
initialize(initial);
}
void initialize(const State& initial)
{
initial_ = initial;
setHomeGeoPoint(initial_.geo_point);
updateState(initial_, home_geo_point_);
}
void setHomeGeoPoint(const GeoPoint& home_geo_point)
{
home_geo_point_ = HomeGeoPoint(home_geo_point);
}
GeoPoint getHomeGeoPoint() const
{
return home_geo_point_.home_geo_point;
}
//in local NED coordinates
void setPosition(const Vector3r& position)
{
current_.position = position;
}
const State& getInitialState() const
{
return initial_;
}
const State& getState() const
{
return current_;
}
State& getState()
{
return current_;
}
virtual void update() override
{
updateState(current_, home_geo_point_);
}
protected:
virtual void resetImplementation() override
{
current_ = initial_;
}
virtual void failResetUpdateOrdering(std::string err) override
{
unused(err);
//Do nothing.
//The environment gets reset() twice without an update() inbetween,
//via MultirotorPawnSimApi::reset() and CarSimApi::reset(), because
//those functions directly reset an environment, and also call other reset()s that reset the same environment.
}
private:
static void updateState(State& state, const HomeGeoPoint& home_geo_point)
{
state.geo_point = EarthUtils::nedToGeodetic(state.position, home_geo_point);
real_T geo_pot = EarthUtils::getGeopotential(state.geo_point.altitude / 1000.0f);
state.temperature = EarthUtils::getStandardTemperature(geo_pot);
state.air_pressure = EarthUtils::getStandardPressure(geo_pot, state.temperature);
state.air_density = EarthUtils::getAirDensity(state.air_pressure, state.temperature);
//TODO: avoid recalculating square roots
state.gravity = Vector3r(0, 0, EarthUtils::getGravity(state.geo_point.altitude));
}
private:
State initial_, current_;
HomeGeoPoint home_geo_point_;
};
}
} //namespace
#endif