Resurgence (PY2022)
Codebase for the Husky Robotics 2021-2022 rover Resurgence
Loading...
Searching...
No Matches
data.h
1#pragma once
2
3#include "../navtypes.h"
4
5#include <bitset>
6#include <chrono>
7#include <optional>
8#include <stdexcept>
9#include <type_traits>
10#include <vector>
11
12#include <frozen/string.h>
13#include <frozen/unordered_map.h>
14#include <frozen/unordered_set.h>
15
16// forward declare cv::Mat instead of importing OpenCV
17// we do this to avoid unnecessarily including OpenCV in all build targets
18namespace cv {
19class Mat;
20}
21
26namespace robot::types {
27
29using dataclock = std::chrono::steady_clock;
32
33template <typename T>
34class DataPoint;
35
42
47
49enum class indication_t {
50 off,
51 autonomous,
52 teleop,
53 arrivedAtDest
54};
55
57enum class motorid_t {
58 leftTread,
59 rightTread,
60 armBase,
61 shoulder,
62 elbow,
63 forearm,
64 wristDiffRight,
65 wristDiffLeft,
66 hand,
67 drillActuator,
68 drillMotor,
69 fourbar1,
70 fourbar2,
71 // Hack, see real_world_interface.cpp for details
72 scienceServoBoard,
73 scienceStepperBoard,
74};
75
78 none,
79 arm,
80 armServo,
81 scienceStation,
82};
83
84enum class jointid_t {
85 armBase,
86 shoulder,
87 elbow,
88 forearm,
89 wristPitch,
90 wristRoll,
91 hand,
92 ikForward,
93 ikUp,
94 fourBarLinkage,
95 drillActuator,
96 drillMotor
97};
98
99constexpr auto all_jointid_t = frozen::make_unordered_set<jointid_t>(
100 {jointid_t::armBase, jointid_t::shoulder, jointid_t::elbow, jointid_t::forearm,
101 jointid_t::wristRoll, jointid_t::wristPitch, jointid_t::hand, jointid_t::ikForward,
102 jointid_t::ikUp, jointid_t::fourBarLinkage, jointid_t::drillActuator, jointid_t::drillMotor});
103
104constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, jointid_t>(
105 {{"armBase", jointid_t::armBase},
106 {"shoulder", jointid_t::shoulder},
107 {"elbow", jointid_t::elbow},
108 {"forearm", jointid_t::forearm},
109 {"wristPitch", jointid_t::wristPitch},
110 {"wristRoll", jointid_t::wristRoll},
111 {"hand", jointid_t::hand},
112 {"ikForward", jointid_t::ikForward},
113 {"ikUp", jointid_t::ikUp},
114 {"fourBarLinkage", jointid_t::fourBarLinkage},
115 {"drillActuator", jointid_t::drillActuator},
116 {"drillMotor", jointid_t::drillMotor}});
117
118enum class servoid_t {
119 microscope,
120 syringe,
121 cuvette,
122 filter,
123 soilBox,
124};
125
126constexpr auto all_servoid_t = frozen::make_unordered_set<servoid_t>(
127 {servoid_t::microscope, servoid_t::syringe, servoid_t::soilBox,
128 servoid_t::cuvette, servoid_t::filter});
129
130constexpr auto name_to_servoid = frozen::make_unordered_map<frozen::string, servoid_t>(
131 {{"microscope", servoid_t::microscope},
132 {"syringe", servoid_t::syringe},
133 {"cuvette", servoid_t::cuvette},
134 {"filter", servoid_t::filter},
135 {"soilBox", servoid_t::soilBox}});
136
137constexpr auto servoid_to_servo_num = frozen::make_unordered_map<servoid_t, int>(
138 {{servoid_t::microscope, 7},
139 {servoid_t::syringe, 9},
140 {servoid_t::cuvette, 5},
141 {servoid_t::filter, 8},
142 {servoid_t::soilBox, 6}});
143
144enum class stepperid_t {
145 plunger,
146 judges,
147 mast,
148 lock,
149 lazySusan,
150};
151
152constexpr auto all_stepperid_t = frozen::make_unordered_set<stepperid_t>(
153 {stepperid_t::plunger, stepperid_t::judges, stepperid_t::mast,
154 stepperid_t::lock, stepperid_t::lazySusan});
155
156constexpr auto name_to_stepperid = frozen::make_unordered_map<frozen::string, stepperid_t>(
157 {{"plunger", stepperid_t::plunger},
158 {"judges", stepperid_t::judges},
159 {"mast", stepperid_t::mast},
160 {"lock", stepperid_t::lock},
161 {"lazySusan", stepperid_t::lazySusan}});
162
163constexpr auto stepperid_to_stepper_num = frozen::make_unordered_map<stepperid_t, uint8_t>(
164 {{stepperid_t::plunger, 4},
165 {stepperid_t::judges, 5},
166 {stepperid_t::mast, 2},
167 {stepperid_t::lock, 1},
168 {stepperid_t::lazySusan, 6}});
169
170class bad_datapoint_access : public std::runtime_error {
171public:
172 bad_datapoint_access() : std::runtime_error("bad_datapoint_access") {}
173};
174
180template <typename T>
182public:
186 DataPoint() : datapoint() {}
187
193 DataPoint(T data) : DataPoint(dataclock::now(), data) {}
194
201 DataPoint(datatime_t time, T data) : datapoint({time, data}) {}
202
208 operator bool() const {
209 return isValid();
210 }
211
219 bool isFresh(std::chrono::milliseconds duration) const {
220 return isValid() && dataclock::now() - duration <= getTime();
221 }
222
228 bool isValid() const {
229 return datapoint.has_value();
230 }
231
239 return datapoint.value().first;
240 }
241
248 T getData() const {
249 if (isValid()) {
250 return datapoint.value().second;
251 } else {
252 throw bad_datapoint_access();
253 }
254 }
255
263 T getDataOrElse(T defaultData) {
264 return isValid() ? getData() : defaultData;
265 }
266
276 template <typename F>
278 if (isValid()) {
280 } else {
281 return {};
282 }
283 }
284
285private:
289 std::optional<std::pair<datatime_t, T>> datapoint;
290};
291
295constexpr size_t N_LIMIT_SWITCH = 8;
296
297constexpr int LIMIT_SWITCH_LIM_MIN_IDX = 0;
298constexpr int LIMIT_SWITCH_LIM_MAX_IDX = 1;
299
304public:
313 LimitSwitchData(unsigned long long data);
314
323 bool isOpen(size_t idx);
324
333 bool isClosed(size_t idx);
334
340 bool isAnyOpen();
341
347 bool isAnyClosed();
348
359
360private:
362};
363
364} // namespace robot::types
365
366namespace util {
367std::string to_string(robot::types::jointid_t joint);
368std::string to_string(const robot::types::CameraID& id);
370std::string to_string(robot::types::servoid_t servo);
371} // namespace util
_GLIBCXX_END_NAMESPACE_CXX11 typedef basic_string< char > string
duration< int64_t, milli > milliseconds
DataPoint()
Construct an invalid DataPoint, holding no data.
Definition data.h:186
LimitSwitchData(unsigned long long data)
Construct a new LimitSwitchData object, from the given bits.
Definition data.cpp:5
Represents data measured using a sensor at a given time.
Definition data.h:181
bool isFresh(std::chrono::milliseconds duration) const
Check if this measurement was taken recently.
Definition data.h:219
DataPoint(T data)
Construct a new DataPoint object, measured now.
Definition data.h:193
T getData() const
Get the value of this data point.
Definition data.h:248
DataPoint(datatime_t time, T data)
Construct a new DataPoint object, measured at the given time.
Definition data.h:201
datatime_t getTime() const
Get the time at which the measurement was taken.
Definition data.h:238
bool isValid() const
Check if this measurement is valid.
Definition data.h:228
DataPoint()
Construct an invalid DataPoint, holding no data.
Definition data.h:186
DataPoint< std::invoke_result_t< F, T > > transform(const F &f)
Transforms the data in this datapoint by the given function.
Definition data.h:277
T getDataOrElse(T defaultData)
Get the value of this data point, defaulting to a given value if this data point is invalid.
Definition data.h:263
std::bitset< N_LIMIT_SWITCH > diff(const LimitSwitchData &other)
Check which indices differ between this data and other.
Definition data.cpp:23
LimitSwitchData(unsigned long long data)
Construct a new LimitSwitchData object, from the given bits.
Definition data.cpp:5
bool isAnyClosed()
Check if any index is closed.
Definition data.cpp:15
bool isAnyOpen()
Check if any index is open.
Definition data.cpp:19
bool isClosed(size_t idx)
Check if the given index is closed.
Definition data.cpp:11
bool isOpen(size_t idx)
Check if the given index is open.
Definition data.cpp:7
::uint32_t uint32_t
Types for use in the world interface.
Definition data.cpp:3
indication_t
An indication enum, used to command the LED to flash different signals.
Definition data.h:49
std::chrono::time_point< dataclock > datatime_t
A point in time as measured by dataclock.
Definition data.h:31
motorid_t
The motors on the robot.
Definition data.h:57
std::vector< DataPoint< navtypes::point_t > > landmarks_t
A data structure that represents when each landmark was seen.
Definition data.h:41
mountedperipheral_t
the mounted peripheral on the robot.
Definition data.h:77
std::pair< cv::Mat, uint32_t > CameraFrame
A pair of a camera frame and its corresponding frame number.
Definition data.h:44
uint32_t CameraID
The type of a camera id.
Definition data.h:46
std::chrono::steady_clock dataclock
The clock used for time measurements for data.
Definition data.h:29
constexpr size_t N_LIMIT_SWITCH
The maximum number of limit switches associated with any motor.
Definition data.h:295
A collection of utility functions and classes with common use-cases.
Definition SwerveController.cpp:145