Skip to content

Commit ae50cb3

Browse files
williamckhaclaude
williamckha
and
claude
committed
Refactor Thunderloop loop and remaining services
- Rewrite the main loop with std::chrono and move status assembly (localization, error codes, chicker/dribbler status) into the services. - Refactor PrimitiveExecutor to take robot_id and write RobotStatus directly. - Refactor RobotLocalizer to take explicit time and expose getRobotState(). - Refactor NetworkService to use PrimitiveTracker (replaces ProtoTracker). - Refactor ImuService to write ImuStatus into the RobotStatus. - Add ImuStatus/AngularAcceleration protos and rename primitive tracking fields. - Remove ScopedTimespecTimer and the battery test. Co-Authored-By: Claude Code <noreply@anthropic.com>
1 parent bcb67f4 commit ae50cb3

29 files changed

Lines changed: 1114 additions & 1441 deletions

‎src/proto/geometry.proto‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,12 @@ message AngularVelocity
3737
required double radians_per_second = 1;
3838
}
3939

40+
message AngularAcceleration
41+
{
42+
// the angular acceleration in radians
43+
required double radians_per_second_squared = 1;
44+
}
45+
4046
message Polygon
4147
{
4248
// list of polygon vertices

‎src/proto/robot_status_msg.proto‎

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -8,17 +8,16 @@ import "proto/geometry.proto";
88

99
message RobotStatus
1010
{
11-
reserved 8;
12-
1311
uint32 robot_id = 1;
1412
repeated ErrorCode error_code = 2;
1513
ChipperKickerStatus chipper_kicker_status = 4;
1614
MotorStatus motor_status = 5;
1715
NetworkStatus network_status = 6;
1816
PowerStatus power_status = 7;
17+
ImuStatus imu_status = 16;
1918
ThunderloopStatus thunderloop_status = 9;
2019
Timestamp time_sent = 10;
21-
uint64 last_handled_primitive_set = 11;
20+
uint64 last_handled_primitive_seq_num = 11;
2221
PrimitiveExecutorStatus primitive_executor_status = 12;
2322
Timestamp adjusted_time_sent = 13;
2423
string thunderloop_version = 14;
@@ -32,12 +31,12 @@ message PrimitiveExecutorStatus
3231

3332
message ThunderloopStatus
3433
{
35-
double network_service_poll_time_ms = 1;
36-
double primitive_executor_start_time_ms = 2;
37-
double primitive_executor_step_time_ms = 3;
38-
double motor_service_poll_time_ms = 4;
39-
double power_service_poll_time_ms = 5;
40-
double iteration_time_ms = 6;
34+
double network_service_poll_time_ms = 1;
35+
double primitive_executor_update_time_ms = 2;
36+
double primitive_executor_step_time_ms = 3;
37+
double motor_service_poll_time_ms = 4;
38+
double power_service_poll_time_ms = 5;
39+
double iteration_time_ms = 6;
4140
}
4241

4342
/* Data about the status of the chipper and kicker */
@@ -146,3 +145,10 @@ message PowerStatus
146145
uint32 sequence_num = 5;
147146
bool breakbeam_tripped = 6;
148147
}
148+
149+
message ImuStatus
150+
{
151+
AngularVelocity angular_velocity = 1;
152+
AngularAcceleration angular_acceleration = 2;
153+
Vector linear_acceleration = 3;
154+
}

‎src/shared/constants.h‎

Lines changed: 19 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -137,25 +137,25 @@ constexpr double BALL_SLIDING_FRICTION_DECELERATION_METERS_PER_SECOND_SQUARED =
137137
-BALL_SLIDING_FRICTION_NEWTONS / BALL_MASS_KG;
138138

139139
/* Unit Conversion */
140-
static const double MILLIMETERS_PER_METER = 1000.0;
141-
static const double METERS_PER_MILLIMETER = 1.0 / 1000.0;
142-
static const double CENTIMETERS_PER_METER = 100.0;
143-
static const double METERS_PER_CENTIMETER = 1.0 / 100.0;
144-
145-
static const double CENTIRADIANS_PER_RADIAN = 100.0;
146-
static const double RADIANS_PER_CENTIRADIAN = 1.0 / 100.0;
147-
148-
static const double NANOSECONDS_PER_MILLISECOND = 1000000.0;
149-
static const double NANOSECONDS_PER_SECOND = 1000000000.0;
150-
static const double MICROSECONDS_PER_MILLISECOND = 1000.0;
151-
static const double MICROSECONDS_PER_SECOND = 1000000.0;
152-
static const double MILLISECONDS_PER_SECOND = 1000.0;
153-
static const double SECONDS_PER_MICROSECOND = 1.0 / 1000000.0;
154-
static const double SECONDS_PER_NANOSECOND = 1.0 / 1000000000.0;
155-
static const double SECONDS_PER_MILLISECOND = 1.0 / 1000.0;
156-
static const double MILLISECONDS_PER_MICROSECOND = 1.0 / 1000.0;
157-
static const double MILLISECONDS_PER_NANOSECOND = 1.0 / 1000000.0;
158-
static const double SECONDS_PER_MINUTE = 60.0;
140+
static constexpr double MILLIMETERS_PER_METER = 1000.0;
141+
static constexpr double METERS_PER_MILLIMETER = 1.0 / 1000.0;
142+
static constexpr double CENTIMETERS_PER_METER = 100.0;
143+
static constexpr double METERS_PER_CENTIMETER = 1.0 / 100.0;
144+
145+
static constexpr double CENTIRADIANS_PER_RADIAN = 100.0;
146+
static constexpr double RADIANS_PER_CENTIRADIAN = 1.0 / 100.0;
147+
148+
static constexpr double NANOSECONDS_PER_MILLISECOND = 1000000.0;
149+
static constexpr double NANOSECONDS_PER_SECOND = 1000000000.0;
150+
static constexpr double MICROSECONDS_PER_MILLISECOND = 1000.0;
151+
static constexpr double MICROSECONDS_PER_SECOND = 1000000.0;
152+
static constexpr double MILLISECONDS_PER_SECOND = 1000.0;
153+
static constexpr double SECONDS_PER_MICROSECOND = 1.0 / 1000000.0;
154+
static constexpr double SECONDS_PER_NANOSECOND = 1.0 / 1000000000.0;
155+
static constexpr double SECONDS_PER_MILLISECOND = 1.0 / 1000.0;
156+
static constexpr double MILLISECONDS_PER_MICROSECOND = 1.0 / 1000.0;
157+
static constexpr double MILLISECONDS_PER_NANOSECOND = 1.0 / 1000000.0;
158+
static constexpr double SECONDS_PER_MINUTE = 60.0;
159159

160160
static const double DEFAULT_SIMULATOR_TICK_RATE_SECONDS_PER_TICK =
161161
1.0 / 60.0; // corresponds to 60 Hz

‎src/software/embedded/BUILD‎

Lines changed: 4 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -35,13 +35,15 @@ cc_library(
3535
"//proto/primitive:primitive_msg_factory",
3636
"//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular",
3737
"//software/ai/navigator/trajectory:trajectory_path",
38+
"//software/embedded:robot_localizer",
3839
"//software/embedded/motion_control:orientation_controller",
3940
"//software/embedded/motion_control:position_controller",
4041
"//software/math:math_functions",
4142
"//software/physics:velocity_conversion_util",
4243
"//software/time:duration",
4344
"//software/world:robot_state",
4445
"//software/world:team_colour",
46+
"@tracy",
4547
],
4648
)
4749

@@ -67,7 +69,6 @@ cc_library(
6769
deps = [
6870
":primitive_executor",
6971
"//proto:tbots_cc_proto",
70-
"//software/embedded:robot_localizer",
7172
"//software/embedded/services:imu",
7273
"//software/embedded/services:motor",
7374
"//software/embedded/services:power",
@@ -76,7 +77,6 @@ cc_library(
7677
"//software/logger:network_logger",
7778
"//software/physics:velocity_conversion_util",
7879
"//software/tracy:tracy_constants",
79-
"//software/util/scoped_timespec_timer",
8080
"@tracy",
8181
],
8282
)
@@ -109,29 +109,21 @@ filegroup(
109109
srcs = ["hash_thunderloop_binary.sh"],
110110
)
111111

112-
cc_test(
113-
name = "test_battery",
114-
srcs = ["battery_test.cpp"],
115-
deps = [
116-
":thunderloop",
117-
"//shared/test_util:tbots_gtest_main",
118-
],
119-
)
120-
121112
cc_library(
122113
name = "robot_localizer",
123114
srcs = ["robot_localizer.cpp"],
124115
hdrs = ["robot_localizer.h"],
125116
deps = [
126117
"//proto:tbots_cc_proto",
118+
"//proto/primitive:primitive_msg_factory",
127119
"//software:constants",
128120
"//software/embedded/services:imu",
129121
"//software/geom:angle",
130122
"//software/geom:angular_velocity",
131123
"//software/geom:point",
132124
"//software/geom:vector",
133125
"//software/sensor_fusion/filter:kalman_filter",
134-
"//software/util/scoped_timespec_timer",
126+
"//software/world:robot_state",
135127
"@eigen",
136128
],
137129
)

‎src/software/embedded/battery_test.cpp‎

Lines changed: 0 additions & 36 deletions
This file was deleted.

0 commit comments

Comments
 (0)