From fac624dc5ef4c38bd3c5256d467e45308146299c Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 29 Sep 2025 02:11:25 -0400 Subject: [PATCH 1/7] Moves ateam_field_manager to state_tracking folder. --- .../ateam_field_manager}/CMakeLists.txt | 0 .../ateam_field_manager}/package.xml | 0 .../ateam_field_manager}/src/ateam_field_manager_node.cpp | 0 .../ateam_field_manager}/src/message_conversions.cpp | 0 .../ateam_field_manager}/src/message_conversions.hpp | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename {ateam_field_manager => state_tracking/ateam_field_manager}/CMakeLists.txt (100%) rename {ateam_field_manager => state_tracking/ateam_field_manager}/package.xml (100%) rename {ateam_field_manager => state_tracking/ateam_field_manager}/src/ateam_field_manager_node.cpp (100%) rename {ateam_field_manager => state_tracking/ateam_field_manager}/src/message_conversions.cpp (100%) rename {ateam_field_manager => state_tracking/ateam_field_manager}/src/message_conversions.hpp (100%) diff --git a/ateam_field_manager/CMakeLists.txt b/state_tracking/ateam_field_manager/CMakeLists.txt similarity index 100% rename from ateam_field_manager/CMakeLists.txt rename to state_tracking/ateam_field_manager/CMakeLists.txt diff --git a/ateam_field_manager/package.xml b/state_tracking/ateam_field_manager/package.xml similarity index 100% rename from ateam_field_manager/package.xml rename to state_tracking/ateam_field_manager/package.xml diff --git a/ateam_field_manager/src/ateam_field_manager_node.cpp b/state_tracking/ateam_field_manager/src/ateam_field_manager_node.cpp similarity index 100% rename from ateam_field_manager/src/ateam_field_manager_node.cpp rename to state_tracking/ateam_field_manager/src/ateam_field_manager_node.cpp diff --git a/ateam_field_manager/src/message_conversions.cpp b/state_tracking/ateam_field_manager/src/message_conversions.cpp similarity index 100% rename from ateam_field_manager/src/message_conversions.cpp rename to state_tracking/ateam_field_manager/src/message_conversions.cpp diff --git a/ateam_field_manager/src/message_conversions.hpp b/state_tracking/ateam_field_manager/src/message_conversions.hpp similarity index 100% rename from ateam_field_manager/src/message_conversions.hpp rename to state_tracking/ateam_field_manager/src/message_conversions.hpp From d7ea5eefc5d6841e6db415fba68d962451215ff7 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Mon, 29 Sep 2025 02:12:48 -0400 Subject: [PATCH 2/7] Moves ateam_vision_filter to state_tracking folder. --- .../ateam_vision_filter}/CMakeLists.txt | 0 .../ateam_vision_filter}/package.xml | 0 .../ateam_vision_filter}/scripts/calibrate_field.py | 0 .../ateam_vision_filter}/src/ateam_vision_filter_node.cpp | 0 .../ateam_vision_filter}/src/camera.cpp | 0 .../ateam_vision_filter}/src/camera.hpp | 0 .../src/filters/interacting_multiple_model_filter.cpp | 0 .../src/filters/interacting_multiple_model_filter.hpp | 0 .../ateam_vision_filter}/src/filters/kalman_filter.cpp | 0 .../ateam_vision_filter}/src/filters/kalman_filter.hpp | 0 .../src/filters/multiple_hypothesis_tracker.cpp | 0 .../src/filters/multiple_hypothesis_tracker.hpp | 0 .../ateam_vision_filter}/src/generators/generator_util.cpp | 0 .../ateam_vision_filter}/src/generators/generator_util.hpp | 0 .../ateam_vision_filter}/src/generators/model_input_generator.cpp | 0 .../ateam_vision_filter}/src/generators/model_input_generator.hpp | 0 .../src/generators/transmission_probability_generator.cpp | 0 .../src/generators/transmission_probability_generator.hpp | 0 .../ateam_vision_filter}/src/message_conversions.cpp | 0 .../ateam_vision_filter}/src/message_conversions.hpp | 0 .../ateam_vision_filter}/src/types/ball.hpp | 0 .../ateam_vision_filter}/src/types/ball_measurement.hpp | 0 .../ateam_vision_filter}/src/types/camera_measurement.hpp | 0 .../ateam_vision_filter}/src/types/models.hpp | 0 .../ateam_vision_filter}/src/types/robot.hpp | 0 .../ateam_vision_filter}/src/types/robot_measurement.hpp | 0 .../ateam_vision_filter}/src/world.cpp | 0 .../ateam_vision_filter}/src/world.hpp | 0 .../ateam_vision_filter}/test/camera_test.cpp | 0 .../test/filters/interacting_multiple_model_filter_test.cpp | 0 .../ateam_vision_filter}/test/filters/kalman_filter_test.cpp | 0 .../test/filters/multiple_hypothesis_tracker_test.cpp | 0 .../test/generators/model_input_generator_test.cpp | 0 .../test/generators/transmission_probability_generator_test.cpp | 0 .../ateam_vision_filter}/test/world_test.cpp | 0 35 files changed, 0 insertions(+), 0 deletions(-) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/CMakeLists.txt (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/package.xml (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/scripts/calibrate_field.py (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/ateam_vision_filter_node.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/camera.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/camera.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/interacting_multiple_model_filter.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/interacting_multiple_model_filter.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/kalman_filter.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/kalman_filter.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/multiple_hypothesis_tracker.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/filters/multiple_hypothesis_tracker.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/generator_util.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/generator_util.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/model_input_generator.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/model_input_generator.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/transmission_probability_generator.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/generators/transmission_probability_generator.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/message_conversions.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/message_conversions.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/ball.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/ball_measurement.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/camera_measurement.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/models.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/robot.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/types/robot_measurement.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/world.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/src/world.hpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/camera_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/filters/interacting_multiple_model_filter_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/filters/kalman_filter_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/filters/multiple_hypothesis_tracker_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/generators/model_input_generator_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/generators/transmission_probability_generator_test.cpp (100%) rename {ateam_vision_filter => state_tracking/ateam_vision_filter}/test/world_test.cpp (100%) diff --git a/ateam_vision_filter/CMakeLists.txt b/state_tracking/ateam_vision_filter/CMakeLists.txt similarity index 100% rename from ateam_vision_filter/CMakeLists.txt rename to state_tracking/ateam_vision_filter/CMakeLists.txt diff --git a/ateam_vision_filter/package.xml b/state_tracking/ateam_vision_filter/package.xml similarity index 100% rename from ateam_vision_filter/package.xml rename to state_tracking/ateam_vision_filter/package.xml diff --git a/ateam_vision_filter/scripts/calibrate_field.py b/state_tracking/ateam_vision_filter/scripts/calibrate_field.py similarity index 100% rename from ateam_vision_filter/scripts/calibrate_field.py rename to state_tracking/ateam_vision_filter/scripts/calibrate_field.py diff --git a/ateam_vision_filter/src/ateam_vision_filter_node.cpp b/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp similarity index 100% rename from ateam_vision_filter/src/ateam_vision_filter_node.cpp rename to state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp diff --git a/ateam_vision_filter/src/camera.cpp b/state_tracking/ateam_vision_filter/src/camera.cpp similarity index 100% rename from ateam_vision_filter/src/camera.cpp rename to state_tracking/ateam_vision_filter/src/camera.cpp diff --git a/ateam_vision_filter/src/camera.hpp b/state_tracking/ateam_vision_filter/src/camera.hpp similarity index 100% rename from ateam_vision_filter/src/camera.hpp rename to state_tracking/ateam_vision_filter/src/camera.hpp diff --git a/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp similarity index 100% rename from ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp rename to state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp diff --git a/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp similarity index 100% rename from ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp rename to state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp diff --git a/ateam_vision_filter/src/filters/kalman_filter.cpp b/state_tracking/ateam_vision_filter/src/filters/kalman_filter.cpp similarity index 100% rename from ateam_vision_filter/src/filters/kalman_filter.cpp rename to state_tracking/ateam_vision_filter/src/filters/kalman_filter.cpp diff --git a/ateam_vision_filter/src/filters/kalman_filter.hpp b/state_tracking/ateam_vision_filter/src/filters/kalman_filter.hpp similarity index 100% rename from ateam_vision_filter/src/filters/kalman_filter.hpp rename to state_tracking/ateam_vision_filter/src/filters/kalman_filter.hpp diff --git a/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp similarity index 100% rename from ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp rename to state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp diff --git a/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp similarity index 100% rename from ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp rename to state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp diff --git a/ateam_vision_filter/src/generators/generator_util.cpp b/state_tracking/ateam_vision_filter/src/generators/generator_util.cpp similarity index 100% rename from ateam_vision_filter/src/generators/generator_util.cpp rename to state_tracking/ateam_vision_filter/src/generators/generator_util.cpp diff --git a/ateam_vision_filter/src/generators/generator_util.hpp b/state_tracking/ateam_vision_filter/src/generators/generator_util.hpp similarity index 100% rename from ateam_vision_filter/src/generators/generator_util.hpp rename to state_tracking/ateam_vision_filter/src/generators/generator_util.hpp diff --git a/ateam_vision_filter/src/generators/model_input_generator.cpp b/state_tracking/ateam_vision_filter/src/generators/model_input_generator.cpp similarity index 100% rename from ateam_vision_filter/src/generators/model_input_generator.cpp rename to state_tracking/ateam_vision_filter/src/generators/model_input_generator.cpp diff --git a/ateam_vision_filter/src/generators/model_input_generator.hpp b/state_tracking/ateam_vision_filter/src/generators/model_input_generator.hpp similarity index 100% rename from ateam_vision_filter/src/generators/model_input_generator.hpp rename to state_tracking/ateam_vision_filter/src/generators/model_input_generator.hpp diff --git a/ateam_vision_filter/src/generators/transmission_probability_generator.cpp b/state_tracking/ateam_vision_filter/src/generators/transmission_probability_generator.cpp similarity index 100% rename from ateam_vision_filter/src/generators/transmission_probability_generator.cpp rename to state_tracking/ateam_vision_filter/src/generators/transmission_probability_generator.cpp diff --git a/ateam_vision_filter/src/generators/transmission_probability_generator.hpp b/state_tracking/ateam_vision_filter/src/generators/transmission_probability_generator.hpp similarity index 100% rename from ateam_vision_filter/src/generators/transmission_probability_generator.hpp rename to state_tracking/ateam_vision_filter/src/generators/transmission_probability_generator.hpp diff --git a/ateam_vision_filter/src/message_conversions.cpp b/state_tracking/ateam_vision_filter/src/message_conversions.cpp similarity index 100% rename from ateam_vision_filter/src/message_conversions.cpp rename to state_tracking/ateam_vision_filter/src/message_conversions.cpp diff --git a/ateam_vision_filter/src/message_conversions.hpp b/state_tracking/ateam_vision_filter/src/message_conversions.hpp similarity index 100% rename from ateam_vision_filter/src/message_conversions.hpp rename to state_tracking/ateam_vision_filter/src/message_conversions.hpp diff --git a/ateam_vision_filter/src/types/ball.hpp b/state_tracking/ateam_vision_filter/src/types/ball.hpp similarity index 100% rename from ateam_vision_filter/src/types/ball.hpp rename to state_tracking/ateam_vision_filter/src/types/ball.hpp diff --git a/ateam_vision_filter/src/types/ball_measurement.hpp b/state_tracking/ateam_vision_filter/src/types/ball_measurement.hpp similarity index 100% rename from ateam_vision_filter/src/types/ball_measurement.hpp rename to state_tracking/ateam_vision_filter/src/types/ball_measurement.hpp diff --git a/ateam_vision_filter/src/types/camera_measurement.hpp b/state_tracking/ateam_vision_filter/src/types/camera_measurement.hpp similarity index 100% rename from ateam_vision_filter/src/types/camera_measurement.hpp rename to state_tracking/ateam_vision_filter/src/types/camera_measurement.hpp diff --git a/ateam_vision_filter/src/types/models.hpp b/state_tracking/ateam_vision_filter/src/types/models.hpp similarity index 100% rename from ateam_vision_filter/src/types/models.hpp rename to state_tracking/ateam_vision_filter/src/types/models.hpp diff --git a/ateam_vision_filter/src/types/robot.hpp b/state_tracking/ateam_vision_filter/src/types/robot.hpp similarity index 100% rename from ateam_vision_filter/src/types/robot.hpp rename to state_tracking/ateam_vision_filter/src/types/robot.hpp diff --git a/ateam_vision_filter/src/types/robot_measurement.hpp b/state_tracking/ateam_vision_filter/src/types/robot_measurement.hpp similarity index 100% rename from ateam_vision_filter/src/types/robot_measurement.hpp rename to state_tracking/ateam_vision_filter/src/types/robot_measurement.hpp diff --git a/ateam_vision_filter/src/world.cpp b/state_tracking/ateam_vision_filter/src/world.cpp similarity index 100% rename from ateam_vision_filter/src/world.cpp rename to state_tracking/ateam_vision_filter/src/world.cpp diff --git a/ateam_vision_filter/src/world.hpp b/state_tracking/ateam_vision_filter/src/world.hpp similarity index 100% rename from ateam_vision_filter/src/world.hpp rename to state_tracking/ateam_vision_filter/src/world.hpp diff --git a/ateam_vision_filter/test/camera_test.cpp b/state_tracking/ateam_vision_filter/test/camera_test.cpp similarity index 100% rename from ateam_vision_filter/test/camera_test.cpp rename to state_tracking/ateam_vision_filter/test/camera_test.cpp diff --git a/ateam_vision_filter/test/filters/interacting_multiple_model_filter_test.cpp b/state_tracking/ateam_vision_filter/test/filters/interacting_multiple_model_filter_test.cpp similarity index 100% rename from ateam_vision_filter/test/filters/interacting_multiple_model_filter_test.cpp rename to state_tracking/ateam_vision_filter/test/filters/interacting_multiple_model_filter_test.cpp diff --git a/ateam_vision_filter/test/filters/kalman_filter_test.cpp b/state_tracking/ateam_vision_filter/test/filters/kalman_filter_test.cpp similarity index 100% rename from ateam_vision_filter/test/filters/kalman_filter_test.cpp rename to state_tracking/ateam_vision_filter/test/filters/kalman_filter_test.cpp diff --git a/ateam_vision_filter/test/filters/multiple_hypothesis_tracker_test.cpp b/state_tracking/ateam_vision_filter/test/filters/multiple_hypothesis_tracker_test.cpp similarity index 100% rename from ateam_vision_filter/test/filters/multiple_hypothesis_tracker_test.cpp rename to state_tracking/ateam_vision_filter/test/filters/multiple_hypothesis_tracker_test.cpp diff --git a/ateam_vision_filter/test/generators/model_input_generator_test.cpp b/state_tracking/ateam_vision_filter/test/generators/model_input_generator_test.cpp similarity index 100% rename from ateam_vision_filter/test/generators/model_input_generator_test.cpp rename to state_tracking/ateam_vision_filter/test/generators/model_input_generator_test.cpp diff --git a/ateam_vision_filter/test/generators/transmission_probability_generator_test.cpp b/state_tracking/ateam_vision_filter/test/generators/transmission_probability_generator_test.cpp similarity index 100% rename from ateam_vision_filter/test/generators/transmission_probability_generator_test.cpp rename to state_tracking/ateam_vision_filter/test/generators/transmission_probability_generator_test.cpp diff --git a/ateam_vision_filter/test/world_test.cpp b/state_tracking/ateam_vision_filter/test/world_test.cpp similarity index 100% rename from ateam_vision_filter/test/world_test.cpp rename to state_tracking/ateam_vision_filter/test/world_test.cpp From 6f893ccd4db266994582ecef07f9d326b0d1ef12 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 00:17:15 -0400 Subject: [PATCH 3/7] Splits vision/game state messages and removes unused message types. --- ateam_msgs/CMakeLists.txt | 24 ++++++++++--------- ateam_msgs/msg/BallState.msg | 8 ------- ateam_msgs/msg/BehaviorExecutorState.msg | 1 - ateam_msgs/msg/GameStateBall.msg | 4 ++++ ateam_msgs/msg/GameStateRobot.msg | 6 +++++ ateam_msgs/msg/GameStateWorld.msg | 12 ++++++++++ ateam_msgs/msg/KenobiStatus.msg | 1 + ateam_msgs/msg/RobotState.msg | 10 -------- ateam_msgs/msg/Sample3d.msg | 7 ------ ateam_msgs/msg/Trajectory.msg | 1 - ateam_msgs/msg/VisionCameraState.msg | 4 ---- ateam_msgs/msg/VisionIMMState.msg | 2 -- ateam_msgs/msg/VisionMHTState.msg | 1 - ateam_msgs/msg/VisionStateBall.msg | 4 ++++ ateam_msgs/msg/VisionStateCamera.msg | 4 ++++ ateam_msgs/msg/VisionStateCameraArray.msg | 1 + ateam_msgs/msg/VisionStateIMM.msg | 2 ++ ateam_msgs/msg/VisionStateMHT.msg | 1 + ...ionModelState.msg => VisionStateModel.msg} | 2 +- ateam_msgs/msg/VisionStateRobot.msg | 5 ++++ ateam_msgs/msg/VisionWorldState.msg | 1 - ateam_msgs/msg/World.msg | 18 -------------- 22 files changed, 54 insertions(+), 65 deletions(-) delete mode 100644 ateam_msgs/msg/BallState.msg delete mode 100644 ateam_msgs/msg/BehaviorExecutorState.msg create mode 100644 ateam_msgs/msg/GameStateBall.msg create mode 100644 ateam_msgs/msg/GameStateRobot.msg create mode 100644 ateam_msgs/msg/GameStateWorld.msg create mode 100644 ateam_msgs/msg/KenobiStatus.msg delete mode 100644 ateam_msgs/msg/RobotState.msg delete mode 100644 ateam_msgs/msg/Sample3d.msg delete mode 100644 ateam_msgs/msg/Trajectory.msg delete mode 100644 ateam_msgs/msg/VisionCameraState.msg delete mode 100644 ateam_msgs/msg/VisionIMMState.msg delete mode 100644 ateam_msgs/msg/VisionMHTState.msg create mode 100644 ateam_msgs/msg/VisionStateBall.msg create mode 100644 ateam_msgs/msg/VisionStateCamera.msg create mode 100644 ateam_msgs/msg/VisionStateCameraArray.msg create mode 100644 ateam_msgs/msg/VisionStateIMM.msg create mode 100644 ateam_msgs/msg/VisionStateMHT.msg rename ateam_msgs/msg/{VisionModelState.msg => VisionStateModel.msg} (60%) create mode 100644 ateam_msgs/msg/VisionStateRobot.msg delete mode 100644 ateam_msgs/msg/VisionWorldState.msg delete mode 100644 ateam_msgs/msg/World.msg diff --git a/ateam_msgs/CMakeLists.txt b/ateam_msgs/CMakeLists.txt index b9da1e8d9..139cc0d92 100755 --- a/ateam_msgs/CMakeLists.txt +++ b/ateam_msgs/CMakeLists.txt @@ -10,23 +10,25 @@ find_package(sensor_msgs REQUIRED) find_package(ssl_league_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - msg/BallState.msg - msg/BehaviorExecutorState.msg msg/JoystickControlStatus.msg + msg/KenobiStatus.msg msg/Overlay.msg msg/OverlayArray.msg msg/RefereeInfo.msg msg/RobotMotionCommand.msg - msg/RobotState.msg - msg/Sample3d.msg msg/TeamClientConnectionStatus.msg - msg/Trajectory.msg - msg/VisionCameraState.msg - msg/VisionIMMState.msg - msg/VisionMHTState.msg - msg/VisionModelState.msg - msg/VisionWorldState.msg - msg/World.msg + + msg/GameStateBall.msg + msg/GameStateRobot.msg + msg/GameStateWorld.msg + + msg/VisionStateBall.msg + msg/VisionStateCamera.msg + msg/VisionStateCameraArray.msg + msg/VisionStateIMM.msg + msg/VisionStateMHT.msg + msg/VisionStateModel.msg + msg/VisionStateRobot.msg msg/FieldInfo.msg msg/FieldSidedInfo.msg diff --git a/ateam_msgs/msg/BallState.msg b/ateam_msgs/msg/BallState.msg deleted file mode 100644 index 12bc9cd82..000000000 --- a/ateam_msgs/msg/BallState.msg +++ /dev/null @@ -1,8 +0,0 @@ -# XY in linear -# Velocity/Accel in field frame -# All units in meters and seconds -geometry_msgs/Pose pose -geometry_msgs/Twist twist -geometry_msgs/Accel accel - -bool visible \ No newline at end of file diff --git a/ateam_msgs/msg/BehaviorExecutorState.msg b/ateam_msgs/msg/BehaviorExecutorState.msg deleted file mode 100644 index 4635ed68e..000000000 --- a/ateam_msgs/msg/BehaviorExecutorState.msg +++ /dev/null @@ -1 +0,0 @@ -ateam_msgs/Trajectory[] previous_trajectories \ No newline at end of file diff --git a/ateam_msgs/msg/GameStateBall.msg b/ateam_msgs/msg/GameStateBall.msg new file mode 100644 index 000000000..8fb831044 --- /dev/null +++ b/ateam_msgs/msg/GameStateBall.msg @@ -0,0 +1,4 @@ +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Accel accel +bool visible diff --git a/ateam_msgs/msg/GameStateRobot.msg b/ateam_msgs/msg/GameStateRobot.msg new file mode 100644 index 000000000..839f1ed76 --- /dev/null +++ b/ateam_msgs/msg/GameStateRobot.msg @@ -0,0 +1,6 @@ +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Twist twist_body +geometry_msgs/Accel accel +bool visible +bool connected diff --git a/ateam_msgs/msg/GameStateWorld.msg b/ateam_msgs/msg/GameStateWorld.msg new file mode 100644 index 000000000..88a3ed4ff --- /dev/null +++ b/ateam_msgs/msg/GameStateWorld.msg @@ -0,0 +1,12 @@ +builtin_interfaces/Time current_time + +FieldInfo field +RefereeInfo referee_info + +GameStateBall ball +GameStateRobot[] our_robots +GameStateRobot[] their_robots + +bool ball_in_play +bool double_touch_enforced +int32 double_touch_id diff --git a/ateam_msgs/msg/KenobiStatus.msg b/ateam_msgs/msg/KenobiStatus.msg new file mode 100644 index 000000000..0ffd5d67f --- /dev/null +++ b/ateam_msgs/msg/KenobiStatus.msg @@ -0,0 +1 @@ +float64 fps diff --git a/ateam_msgs/msg/RobotState.msg b/ateam_msgs/msg/RobotState.msg deleted file mode 100644 index cc1b00090..000000000 --- a/ateam_msgs/msg/RobotState.msg +++ /dev/null @@ -1,10 +0,0 @@ -# XY in linear, Z in angular -# Velocity/Accel in field frame -# All units in meters and seconds -geometry_msgs/Pose pose -geometry_msgs/Twist twist -geometry_msgs/Accel accel - -geometry_msgs/Twist twist_body - -bool visible \ No newline at end of file diff --git a/ateam_msgs/msg/Sample3d.msg b/ateam_msgs/msg/Sample3d.msg deleted file mode 100644 index 7680226e9..000000000 --- a/ateam_msgs/msg/Sample3d.msg +++ /dev/null @@ -1,7 +0,0 @@ -# XY in linear, Z in angular -# Velocity/Accel in field frame -# All units in meters and seconds -builtin_interfaces/Time time -geometry_msgs/Pose pose -geometry_msgs/Twist twist -geometry_msgs/Accel accel \ No newline at end of file diff --git a/ateam_msgs/msg/Trajectory.msg b/ateam_msgs/msg/Trajectory.msg deleted file mode 100644 index 15edeb3a9..000000000 --- a/ateam_msgs/msg/Trajectory.msg +++ /dev/null @@ -1 +0,0 @@ -ateam_msgs/Sample3d[] samples \ No newline at end of file diff --git a/ateam_msgs/msg/VisionCameraState.msg b/ateam_msgs/msg/VisionCameraState.msg deleted file mode 100644 index 0e1dcab25..000000000 --- a/ateam_msgs/msg/VisionCameraState.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint8 camera_id -ateam_msgs/VisionMHTState[] yellow_robots -ateam_msgs/VisionMHTState[] blue_robots -ateam_msgs/VisionMHTState ball \ No newline at end of file diff --git a/ateam_msgs/msg/VisionIMMState.msg b/ateam_msgs/msg/VisionIMMState.msg deleted file mode 100644 index a16992b39..000000000 --- a/ateam_msgs/msg/VisionIMMState.msg +++ /dev/null @@ -1,2 +0,0 @@ -ateam_msgs/VisionModelState[] models -uint32 updates_until_valid_track \ No newline at end of file diff --git a/ateam_msgs/msg/VisionMHTState.msg b/ateam_msgs/msg/VisionMHTState.msg deleted file mode 100644 index 325e719cb..000000000 --- a/ateam_msgs/msg/VisionMHTState.msg +++ /dev/null @@ -1 +0,0 @@ -ateam_msgs/VisionIMMState[] tracks \ No newline at end of file diff --git a/ateam_msgs/msg/VisionStateBall.msg b/ateam_msgs/msg/VisionStateBall.msg new file mode 100644 index 000000000..8fb831044 --- /dev/null +++ b/ateam_msgs/msg/VisionStateBall.msg @@ -0,0 +1,4 @@ +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Accel accel +bool visible diff --git a/ateam_msgs/msg/VisionStateCamera.msg b/ateam_msgs/msg/VisionStateCamera.msg new file mode 100644 index 000000000..1e62903d4 --- /dev/null +++ b/ateam_msgs/msg/VisionStateCamera.msg @@ -0,0 +1,4 @@ +uint8 camera_id +VisionStateMHT[] yellow_robots +VisionStateMHT[] blue_robots +VisionStateMHT ball diff --git a/ateam_msgs/msg/VisionStateCameraArray.msg b/ateam_msgs/msg/VisionStateCameraArray.msg new file mode 100644 index 000000000..2d95c5662 --- /dev/null +++ b/ateam_msgs/msg/VisionStateCameraArray.msg @@ -0,0 +1 @@ +VisionStateCamera[] camera_states diff --git a/ateam_msgs/msg/VisionStateIMM.msg b/ateam_msgs/msg/VisionStateIMM.msg new file mode 100644 index 000000000..b9a300ffe --- /dev/null +++ b/ateam_msgs/msg/VisionStateIMM.msg @@ -0,0 +1,2 @@ +VisionStateModel[] models +uint32 updates_until_valid_track diff --git a/ateam_msgs/msg/VisionStateMHT.msg b/ateam_msgs/msg/VisionStateMHT.msg new file mode 100644 index 000000000..c3c289119 --- /dev/null +++ b/ateam_msgs/msg/VisionStateMHT.msg @@ -0,0 +1 @@ +VisionStateIMM[] tracks diff --git a/ateam_msgs/msg/VisionModelState.msg b/ateam_msgs/msg/VisionStateModel.msg similarity index 60% rename from ateam_msgs/msg/VisionModelState.msg rename to ateam_msgs/msg/VisionStateModel.msg index 7c0e05647..4d2d4d415 100644 --- a/ateam_msgs/msg/VisionModelState.msg +++ b/ateam_msgs/msg/VisionStateModel.msg @@ -1,2 +1,2 @@ int32 model_type -float32 mu \ No newline at end of file +float32 mu diff --git a/ateam_msgs/msg/VisionStateRobot.msg b/ateam_msgs/msg/VisionStateRobot.msg new file mode 100644 index 000000000..a462624d4 --- /dev/null +++ b/ateam_msgs/msg/VisionStateRobot.msg @@ -0,0 +1,5 @@ +geometry_msgs/Pose pose +geometry_msgs/Twist twist +geometry_msgs/Twist twist_body +geometry_msgs/Accel accel +bool visible diff --git a/ateam_msgs/msg/VisionWorldState.msg b/ateam_msgs/msg/VisionWorldState.msg deleted file mode 100644 index 81cc37884..000000000 --- a/ateam_msgs/msg/VisionWorldState.msg +++ /dev/null @@ -1 +0,0 @@ -ateam_msgs/VisionCameraState[] camera_states \ No newline at end of file diff --git a/ateam_msgs/msg/World.msg b/ateam_msgs/msg/World.msg deleted file mode 100644 index 9de809e2d..000000000 --- a/ateam_msgs/msg/World.msg +++ /dev/null @@ -1,18 +0,0 @@ -builtin_interfaces/Time current_time - -FieldInfo field -RefereeInfo referee_info - -BallState[] balls -RobotState[] our_robots -RobotState[] their_robots - -RobotState[] plan_from_our_robots - -ateam_msgs/BehaviorExecutorState behavior_executor_state - -bool ball_in_play -bool double_touch_enforced -int32 double_touch_id - -float64 fps From 58a592f4b574d12cefc5fe5ef21131427735d9c5 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 00:34:48 -0400 Subject: [PATCH 4/7] Switches vision filter to new vision state message types --- .../src/ateam_vision_filter_node.cpp | 17 +++++++++-------- .../ateam_vision_filter/src/camera.cpp | 10 +++++----- .../ateam_vision_filter/src/camera.hpp | 4 ++-- .../interacting_multiple_model_filter.cpp | 6 +++--- .../interacting_multiple_model_filter.hpp | 4 ++-- .../src/filters/multiple_hypothesis_tracker.cpp | 4 ++-- .../src/filters/multiple_hypothesis_tracker.hpp | 4 ++-- .../src/message_conversions.cpp | 8 ++++---- .../src/message_conversions.hpp | 8 ++++---- .../ateam_vision_filter/src/world.cpp | 6 +++--- .../ateam_vision_filter/src/world.hpp | 4 ++-- 11 files changed, 38 insertions(+), 37 deletions(-) diff --git a/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp b/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp index 26266641d..3db6dba90 100644 --- a/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp +++ b/state_tracking/ateam_vision_filter/src/ateam_vision_filter_node.cpp @@ -53,26 +53,26 @@ class VisionFilterNode : public rclcpp::Node }); timer_ = create_wall_timer(10ms, std::bind(&VisionFilterNode::timer_callback, this)); - ball_publisher_ = create_publisher( + ball_publisher_ = create_publisher( std::string(Topics::kBall), rclcpp::SystemDefaultsQoS()); ateam_common::indexed_topic_helpers::create_indexed_publishers - ( + ( blue_robots_publisher_, Topics::kBlueTeamRobotPrefix, rclcpp::SystemDefaultsQoS(), this ); ateam_common::indexed_topic_helpers::create_indexed_publishers - ( + ( yellow_robots_publisher_, Topics::kYellowTeamRobotPrefix, rclcpp::SystemDefaultsQoS(), this ); - vision_state_publisher_ = create_publisher( + vision_state_publisher_ = create_publisher( std::string(Topics::kVisionState), rclcpp::SystemDefaultsQoS()); @@ -140,11 +140,12 @@ class VisionFilterNode : public rclcpp::Node private: rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr ball_publisher_; - std::array::SharedPtr, 16> blue_robots_publisher_; - std::array::SharedPtr, + rclcpp::Publisher::SharedPtr ball_publisher_; + std::array::SharedPtr, + 16> blue_robots_publisher_; + std::array::SharedPtr, 16> yellow_robots_publisher_; - rclcpp::Publisher::SharedPtr vision_state_publisher_; + rclcpp::Publisher::SharedPtr vision_state_publisher_; rclcpp::Subscription::SharedPtr ssl_vision_subscription_; rclcpp::Subscription::SharedPtr ssl_vision_subs_; rclcpp::Subscription::SharedPtr field_subscription_; diff --git a/state_tracking/ateam_vision_filter/src/camera.cpp b/state_tracking/ateam_vision_filter/src/camera.cpp index af45d17c8..51594773f 100644 --- a/state_tracking/ateam_vision_filter/src/camera.cpp +++ b/state_tracking/ateam_vision_filter/src/camera.cpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include "filters/kalman_filter.hpp" #include "filters/interacting_multiple_model_filter.hpp" @@ -116,14 +116,14 @@ std::array, 16> Camera::get_blue_robot_est return get_robot_estimates_with_score(blue_team); } -ateam_msgs::msg::VisionCameraState Camera::get_vision_camera_state() const +ateam_msgs::msg::VisionStateCamera Camera::get_vision_camera_state() const { - ateam_msgs::msg::VisionCameraState camera_state; + ateam_msgs::msg::VisionStateCamera camera_state; // Prefill up to team size for (std::size_t i = 0; i < yellow_team.size(); i++) { - camera_state.yellow_robots.push_back(ateam_msgs::msg::VisionMHTState()); - camera_state.blue_robots.push_back(ateam_msgs::msg::VisionMHTState()); + camera_state.yellow_robots.push_back(ateam_msgs::msg::VisionStateMHT()); + camera_state.blue_robots.push_back(ateam_msgs::msg::VisionStateMHT()); } for (std::size_t i = 0; i < yellow_team.size(); i++) { diff --git a/state_tracking/ateam_vision_filter/src/camera.hpp b/state_tracking/ateam_vision_filter/src/camera.hpp index 7902df26d..8282af9d8 100644 --- a/state_tracking/ateam_vision_filter/src/camera.hpp +++ b/state_tracking/ateam_vision_filter/src/camera.hpp @@ -30,7 +30,7 @@ #include -#include +#include #include "filters/multiple_hypothesis_tracker.hpp" #include "generators/model_input_generator.hpp" @@ -81,7 +81,7 @@ class Camera /** * @return ROS2 msg containing the current internal state */ - ateam_msgs::msg::VisionCameraState get_vision_camera_state() const; + ateam_msgs::msg::VisionStateCamera get_vision_camera_state() const; void set_ignored_half(int ignored_half) { diff --git a/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp index b85ea668e..b5f7326f5 100644 --- a/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp +++ b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.cpp @@ -164,11 +164,11 @@ double InteractingMultipleModelFilter::get_potential_measurement_error( return potential_measurement_error.norm(); } -ateam_msgs::msg::VisionIMMState InteractingMultipleModelFilter::get_vision_imm_state() const +ateam_msgs::msg::VisionStateIMM InteractingMultipleModelFilter::get_vision_imm_state() const { - ateam_msgs::msg::VisionIMMState imm_state; + ateam_msgs::msg::VisionStateIMM imm_state; for (const auto & model_type : model_types) { - ateam_msgs::msg::VisionModelState model_state; + ateam_msgs::msg::VisionStateModel model_state; model_state.model_type = model_type; model_state.mu = mu.at(model_type); imm_state.models.push_back(model_state); diff --git a/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp index 44da44c46..97f637165 100644 --- a/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp +++ b/state_tracking/ateam_vision_filter/src/filters/interacting_multiple_model_filter.hpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include "filters/kalman_filter.hpp" #include "types/models.hpp" @@ -101,7 +101,7 @@ class InteractingMultipleModelFilter /** * @return ROS2 msg containing the current internal state */ - ateam_msgs::msg::VisionIMMState get_vision_imm_state() const; + ateam_msgs::msg::VisionStateIMM get_vision_imm_state() const; private: void update_mu(const Eigen::VectorXd & zt); diff --git a/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp index 6fec90ffc..3f38451f9 100644 --- a/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp +++ b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.cpp @@ -124,9 +124,9 @@ get_state_estimate() const return std::make_pair(best_track->get_state_estimate(), best_track->get_validity_score()); } -ateam_msgs::msg::VisionMHTState MultipleHypothesisTracker::get_vision_mht_state() const +ateam_msgs::msg::VisionStateMHT MultipleHypothesisTracker::get_vision_mht_state() const { - ateam_msgs::msg::VisionMHTState mht_state; + ateam_msgs::msg::VisionStateMHT mht_state; for (const auto & track : tracks) { mht_state.tracks.push_back(track.get_vision_imm_state()); } diff --git a/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp index 69aca2655..8203d0beb 100644 --- a/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp +++ b/state_tracking/ateam_vision_filter/src/filters/multiple_hypothesis_tracker.hpp @@ -32,7 +32,7 @@ #include #include -#include +#include #include "filters/interacting_multiple_model_filter.hpp" @@ -54,7 +54,7 @@ class MultipleHypothesisTracker /** * @return ROS2 msg containing the current internal state */ - ateam_msgs::msg::VisionMHTState get_vision_mht_state() const; + ateam_msgs::msg::VisionStateMHT get_vision_mht_state() const; private: void remove_expired_tracks(); diff --git a/state_tracking/ateam_vision_filter/src/message_conversions.cpp b/state_tracking/ateam_vision_filter/src/message_conversions.cpp index b1886ff1a..d071ff580 100644 --- a/state_tracking/ateam_vision_filter/src/message_conversions.cpp +++ b/state_tracking/ateam_vision_filter/src/message_conversions.cpp @@ -33,9 +33,9 @@ namespace ateam_vision_filter::message_conversions { -ateam_msgs::msg::BallState toMsg(const std::optional & maybe_ball) +ateam_msgs::msg::VisionStateBall toMsg(const std::optional & maybe_ball) { - ateam_msgs::msg::BallState ball_state_msg; + ateam_msgs::msg::VisionStateBall ball_state_msg; ball_state_msg.visible = maybe_ball.has_value(); if (maybe_ball.has_value()) { auto obj = maybe_ball.value(); @@ -51,9 +51,9 @@ ateam_msgs::msg::BallState toMsg(const std::optional & maybe_ball) return ball_state_msg; } -ateam_msgs::msg::RobotState toMsg(const std::optional & maybe_robot) +ateam_msgs::msg::VisionStateRobot toMsg(const std::optional & maybe_robot) { - ateam_msgs::msg::RobotState robot_state_msg; + ateam_msgs::msg::VisionStateRobot robot_state_msg; robot_state_msg.visible = maybe_robot.has_value(); if (maybe_robot.has_value()) { diff --git a/state_tracking/ateam_vision_filter/src/message_conversions.hpp b/state_tracking/ateam_vision_filter/src/message_conversions.hpp index 15dd705a3..0ad9e5c14 100644 --- a/state_tracking/ateam_vision_filter/src/message_conversions.hpp +++ b/state_tracking/ateam_vision_filter/src/message_conversions.hpp @@ -24,8 +24,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -43,8 +43,8 @@ namespace ateam_vision_filter::message_conversions { -ateam_msgs::msg::BallState toMsg(const std::optional & maybe_ball); -ateam_msgs::msg::RobotState toMsg(const std::optional & maybe_robot); +ateam_msgs::msg::VisionStateBall toMsg(const std::optional & maybe_ball); +ateam_msgs::msg::VisionStateRobot toMsg(const std::optional & maybe_robot); CameraMeasurement fromMsg( const ssl_league_msgs::msg::VisionDetectionFrame & ros_msg, diff --git a/state_tracking/ateam_vision_filter/src/world.cpp b/state_tracking/ateam_vision_filter/src/world.cpp index 689cf105d..2e6109a15 100644 --- a/state_tracking/ateam_vision_filter/src/world.cpp +++ b/state_tracking/ateam_vision_filter/src/world.cpp @@ -227,11 +227,11 @@ std::array, 16> World::get_blue_robots_estimate() return blue_robots_estimates; } -ateam_msgs::msg::VisionWorldState World::get_vision_world_state() const +ateam_msgs::msg::VisionStateCameraArray World::get_vision_world_state() const { - ateam_msgs::msg::VisionWorldState world_state; + ateam_msgs::msg::VisionStateCameraArray world_state; for (const auto & camera_pair : cameras) { - ateam_msgs::msg::VisionCameraState camera_state = camera_pair.second.get_vision_camera_state(); + ateam_msgs::msg::VisionStateCamera camera_state = camera_pair.second.get_vision_camera_state(); camera_state.camera_id = camera_pair.first; world_state.camera_states.push_back(camera_state); } diff --git a/state_tracking/ateam_vision_filter/src/world.hpp b/state_tracking/ateam_vision_filter/src/world.hpp index 2b55fea6c..5f7464148 100644 --- a/state_tracking/ateam_vision_filter/src/world.hpp +++ b/state_tracking/ateam_vision_filter/src/world.hpp @@ -29,7 +29,7 @@ #include -#include +#include #include "camera.hpp" #include "generators/model_input_generator.hpp" @@ -80,7 +80,7 @@ class World /** * @return ROS2 msg containing the current internal state */ - ateam_msgs::msg::VisionWorldState get_vision_world_state() const; + ateam_msgs::msg::VisionStateCameraArray get_vision_world_state() const; private: rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; From 50590a5b4eab41e6a354168940fc8dadd0cfc768 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 00:36:37 -0400 Subject: [PATCH 5/7] Switches UI to new vision state messages --- ateam_ui/src/src/rosManager.ts | 4 ++-- ateam_ui/test/launch_tests/ateam_ui_test.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ateam_ui/src/src/rosManager.ts b/ateam_ui/src/src/rosManager.ts index d62289f8f..ad958ed24 100644 --- a/ateam_ui/src/src/rosManager.ts +++ b/ateam_ui/src/src/rosManager.ts @@ -204,7 +204,7 @@ export class RosManager { ballTopic = new ROSLIB.Topic({ ros: this.ros, name: '/ball', - messageType: 'ateam_msgs/msg/BallState' + messageType: 'ateam_msgs/msg/VisionStateBall' }); this.subscriptions.set("ball", ballTopic); } @@ -217,7 +217,7 @@ export class RosManager { robotTopic = new ROSLIB.Topic({ ros: this.ros, name: '/' + team + '_team/robot' + i, - messageType: 'ateam_msgs/msg/RobotState' + messageType: 'ateam_msgs/msg/VisionStateRobot' }); this.subscriptions.set("/" + team + "_team/robot" + i, robotTopic); } diff --git a/ateam_ui/test/launch_tests/ateam_ui_test.py b/ateam_ui/test/launch_tests/ateam_ui_test.py index f2c10f7b0..6d96ab634 100755 --- a/ateam_ui/test/launch_tests/ateam_ui_test.py +++ b/ateam_ui/test/launch_tests/ateam_ui_test.py @@ -103,12 +103,12 @@ def test_0_subscribers_created(self): self.assertIsNotNone(subscriptions) # Check subscription to /ball - self.assertIn(("/ball", ['ateam_msgs/msg/BallState']), subscriptions) + self.assertIn(("/ball", ['ateam_msgs/msg/VisionStateBall']), subscriptions) # Check robot subscriptions for team in ["yellow", "blue"]: for id in range(0, 16): - topic = (f"/{team}_team/robot{id}", ['ateam_msgs/msg/RobotState']) + topic = (f"/{team}_team/robot{id}", ['ateam_msgs/msg/VisionStateRobot']) self.assertIn(topic, subscriptions) # Check subscription to /overlay From b82fa02a37c87db8794d39b26ad8d228d506d947 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 00:42:10 -0400 Subject: [PATCH 6/7] Switches motion benchmarks to new vision state messages --- .../src/angular_velocity_benchmark.cpp | 16 ++++++++++------ .../src/velocity_benchmark.cpp | 16 ++++++++++------ 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp index b229a8bad..37bedaf34 100644 --- a/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/angular_velocity_benchmark.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -125,7 +125,7 @@ struct Options struct Robot { bool connected = false; - std::optional state; + std::optional state; std::optional feedback; std::optional motion_feedback; }; @@ -156,8 +156,9 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node const auto state_topic_prefix = options_.team_color == ateam_common::TeamColor::Blue ? Topics::kBlueTeamRobotPrefix : Topics::kYellowTeamRobotPrefix; - create_indexed_subscribers( - robot_state_subs_, state_topic_prefix, 1, &AngularVelocityBenchmarkNode::RobotStateCallback, + create_indexed_subscribers( + robot_state_subs_, state_topic_prefix, 1, + &AngularVelocityBenchmarkNode::VisionStateRobotCallback, this); create_indexed_subscribers(robot_feedback_subs_, @@ -196,7 +197,8 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node rclcpp::Publisher::SharedPtr command_pub_; rclcpp::Subscription::SharedPtr field_sub_; - std::array::SharedPtr, 16> robot_state_subs_; + std::array::SharedPtr, + 16> robot_state_subs_; std::array::SharedPtr, 16> robot_feedback_subs_; std::array::SharedPtr, @@ -210,7 +212,9 @@ class AngularVelocityBenchmarkNode : public rclcpp::Node field_ = *msg; } - void RobotStateCallback(const ateam_msgs::msg::RobotState::SharedPtr msg, int robot_id) + void VisionStateRobotCallback( + const ateam_msgs::msg::VisionStateRobot::SharedPtr msg, + int robot_id) { robots_[robot_id].state = *msg; } diff --git a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp index 7b1e18f30..4d5f7db2b 100644 --- a/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp +++ b/motion/ateam_motion_benchmark/src/velocity_benchmark.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include @@ -120,7 +120,7 @@ struct Options struct Robot { bool connected = false; - std::optional state; + std::optional state; std::optional feedback; std::optional motion_feedback; }; @@ -151,8 +151,9 @@ class VelocityBenchmarkNode : public rclcpp::Node const auto state_topic_prefix = options_.team_color == ateam_common::TeamColor::Blue ? Topics::kBlueTeamRobotPrefix : Topics::kYellowTeamRobotPrefix; - create_indexed_subscribers( - robot_state_subs_, state_topic_prefix, 1, &VelocityBenchmarkNode::RobotStateCallback, this); + create_indexed_subscribers( + robot_state_subs_, state_topic_prefix, 1, &VelocityBenchmarkNode::VisionStateRobotCallback, + this); create_indexed_subscribers(robot_feedback_subs_, Topics::kRobotFeedbackPrefix, 1, &VelocityBenchmarkNode::RobotFeedbackCallback, this); @@ -187,7 +188,8 @@ class VelocityBenchmarkNode : public rclcpp::Node rclcpp::Publisher::SharedPtr command_pub_; rclcpp::Subscription::SharedPtr field_sub_; - std::array::SharedPtr, 16> robot_state_subs_; + std::array::SharedPtr, + 16> robot_state_subs_; std::array::SharedPtr, 16> robot_feedback_subs_; std::array::SharedPtr, @@ -201,7 +203,9 @@ class VelocityBenchmarkNode : public rclcpp::Node field_ = *msg; } - void RobotStateCallback(const ateam_msgs::msg::RobotState::SharedPtr msg, int robot_id) + void VisionStateRobotCallback( + const ateam_msgs::msg::VisionStateRobot::SharedPtr msg, + int robot_id) { robots_[robot_id].state = *msg; } From bfbb75aba9236ccd2349c5dd71c90f894f2effcc Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 30 Sep 2025 01:03:42 -0400 Subject: [PATCH 7/7] Switches Kenobi to new vision/game state types. --- ateam_kenobi/src/core/fps_tracker.hpp | 4 +- .../src/core/motion/motion_controller.cpp | 1 - .../src/core/types/message_conversions.cpp | 24 +++++------ .../src/core/types/message_conversions.hpp | 23 ++++------- ateam_kenobi/src/kenobi_node.cpp | 41 +++++++++++-------- 5 files changed, 47 insertions(+), 46 deletions(-) diff --git a/ateam_kenobi/src/core/fps_tracker.hpp b/ateam_kenobi/src/core/fps_tracker.hpp index bc0c5af1a..ecbafa25c 100644 --- a/ateam_kenobi/src/core/fps_tracker.hpp +++ b/ateam_kenobi/src/core/fps_tracker.hpp @@ -36,7 +36,7 @@ class FpsTracker std::fill(buffer_.begin(), buffer_.end(), average_fps_ / kWindowWidth); } - void update(World & world) + double Update(const World & world) { const auto dt = ateam_common::TimeDiffSeconds(world.current_time, prev_time_); prev_time_ = world.current_time; @@ -45,7 +45,7 @@ class FpsTracker buffer_[buffer_index_] = current_fps / kWindowWidth; average_fps_ += buffer_[buffer_index_]; buffer_index_ = (buffer_index_ + 1) % kWindowWidth; - world.fps = average_fps_; + return average_fps_; } private: diff --git a/ateam_kenobi/src/core/motion/motion_controller.cpp b/ateam_kenobi/src/core/motion/motion_controller.cpp index 7f062a378..2a70aedbe 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.cpp +++ b/ateam_kenobi/src/core/motion/motion_controller.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include "ateam_geometry/ateam_geometry.hpp" #include "ateam_common/robot_constants.hpp" #include "pid.hpp" diff --git a/ateam_kenobi/src/core/types/message_conversions.cpp b/ateam_kenobi/src/core/types/message_conversions.cpp index 8e20f28b3..90d57f676 100644 --- a/ateam_kenobi/src/core/types/message_conversions.cpp +++ b/ateam_kenobi/src/core/types/message_conversions.cpp @@ -21,9 +21,11 @@ #include "core/types/message_conversions.hpp" #include - #include +#include "core/types/field.hpp" +#include "core/types/referee_info.hpp" + namespace ateam_kenobi::message_conversions { ateam_msgs::msg::FieldInfo toMsg(const Field & obj) @@ -81,9 +83,9 @@ ateam_msgs::msg::RefereeInfo toMsg(const RefereeInfo & obj) } -ateam_msgs::msg::BallState toMsg(const Ball & obj) +ateam_msgs::msg::GameStateBall toMsg(const Ball & obj) { - ateam_msgs::msg::BallState ball_state_msg; + ateam_msgs::msg::GameStateBall ball_state_msg; ball_state_msg.pose.position.x = obj.pos.x(); ball_state_msg.pose.position.y = obj.pos.y(); ball_state_msg.twist.linear.x = obj.vel.x(); @@ -93,9 +95,9 @@ ateam_msgs::msg::BallState toMsg(const Ball & obj) return ball_state_msg; } -ateam_msgs::msg::RobotState toMsg(const Robot & obj) +ateam_msgs::msg::GameStateRobot toMsg(const Robot & obj) { - ateam_msgs::msg::RobotState robot_state_msg; + ateam_msgs::msg::GameStateRobot robot_state_msg; robot_state_msg.pose.position.x = obj.pos.x(); robot_state_msg.pose.position.y = obj.pos.y(); robot_state_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), obj.theta)); @@ -107,9 +109,9 @@ ateam_msgs::msg::RobotState toMsg(const Robot & obj) return robot_state_msg; } -ateam_msgs::msg::World toMsg(const World & obj) +ateam_msgs::msg::GameStateWorld toMsg(const World & obj) { - ateam_msgs::msg::World world_msg; + ateam_msgs::msg::GameStateWorld world_msg; world_msg.current_time = rclcpp::Time( @@ -119,13 +121,13 @@ ateam_msgs::msg::World toMsg(const World & obj) world_msg.field = toMsg(obj.field); world_msg.referee_info = toMsg(obj.referee_info); - world_msg.balls.push_back(toMsg(obj.ball)); + world_msg.ball = toMsg(obj.ball); for (const Robot & robot : obj.our_robots) { if (robot.visible || robot.radio_connected) { world_msg.our_robots.push_back(toMsg(robot)); } else { - world_msg.our_robots.push_back(ateam_msgs::msg::RobotState()); + world_msg.our_robots.push_back(ateam_msgs::msg::GameStateRobot()); } } @@ -133,7 +135,7 @@ ateam_msgs::msg::World toMsg(const World & obj) if (robot.visible) { world_msg.their_robots.push_back(toMsg(robot)); } else { - world_msg.their_robots.push_back(ateam_msgs::msg::RobotState()); + world_msg.their_robots.push_back(ateam_msgs::msg::GameStateRobot()); } } @@ -141,8 +143,6 @@ ateam_msgs::msg::World toMsg(const World & obj) world_msg.double_touch_enforced = obj.double_touch_forbidden_id_.has_value(); world_msg.double_touch_id = obj.double_touch_forbidden_id_.value_or(-1); - world_msg.fps = obj.fps; - return world_msg; } diff --git a/ateam_kenobi/src/core/types/message_conversions.hpp b/ateam_kenobi/src/core/types/message_conversions.hpp index a5cd7a696..5413438de 100644 --- a/ateam_kenobi/src/core/types/message_conversions.hpp +++ b/ateam_kenobi/src/core/types/message_conversions.hpp @@ -22,25 +22,20 @@ #ifndef CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ #define CORE__TYPES__MESSAGE_CONVERSIONS_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include "core/types/ball.hpp" +#include "core/types/robot.hpp" #include "core/types/world.hpp" -#include "core/types/field.hpp" -#include "core/types/referee_info.hpp" + namespace ateam_kenobi::message_conversions { -ateam_msgs::msg::BallState toMsg(const Ball & obj); -ateam_msgs::msg::RobotState toMsg(const Robot & obj); - -ateam_msgs::msg::World toMsg(const World & obj); +ateam_msgs::msg::GameStateBall toMsg(const Ball & obj); +ateam_msgs::msg::GameStateRobot toMsg(const Robot & obj); +ateam_msgs::msg::GameStateWorld toMsg(const World & obj); } // namespace ateam_kenobi::message_conversions diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 44035069a..85db050b1 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -25,18 +25,19 @@ #include #include #include -#include #include #include -#include +#include +#include #include #include #include #include -#include +#include #include #include #include +#include #include #include #include @@ -86,14 +87,14 @@ class KenobiNode : public rclcpp::Node "/play_info", rclcpp::SystemDefaultsQoS()); - create_indexed_subscribers( + create_indexed_subscribers( blue_robots_subscriptions_, Topics::kBlueTeamRobotPrefix, 10, &KenobiNode::blue_robot_state_callback, this); - create_indexed_subscribers( + create_indexed_subscribers( yellow_robots_subscriptions_, Topics::kYellowTeamRobotPrefix, 10, @@ -118,12 +119,12 @@ class KenobiNode : public rclcpp::Node robot_commands_publishers_, Topics::kRobotMotionCommandPrefix, rclcpp::SystemDefaultsQoS(), this); - ball_subscription_ = create_subscription( + ball_subscription_ = create_subscription( std::string(Topics::kBall), 10, std::bind(&KenobiNode::ball_state_callback, this, std::placeholders::_1)); - world_publisher_ = create_publisher( + world_publisher_ = create_publisher( "~/world", rclcpp::SystemDefaultsQoS()); @@ -146,6 +147,9 @@ class KenobiNode : public rclcpp::Node "~/playbook_state", rclcpp::SystemDefaultsQoS()); + status_publisher_ = create_publisher("~/status", + rclcpp::SystemDefaultsQoS()); + timer_ = create_wall_timer(10ms, std::bind(&KenobiNode::timer_callback, this)); const auto playbook_path = declare_parameter("playbook", ""); @@ -182,10 +186,10 @@ class KenobiNode : public rclcpp::Node FpsTracker fps_tracker_; rclcpp::Publisher::SharedPtr overlay_publisher_; rclcpp::Publisher::SharedPtr play_info_publisher_; - rclcpp::Subscription::SharedPtr ball_subscription_; - std::array::SharedPtr, + rclcpp::Subscription::SharedPtr ball_subscription_; + std::array::SharedPtr, 16> blue_robots_subscriptions_; - std::array::SharedPtr, + std::array::SharedPtr, 16> yellow_robots_subscriptions_; std::array::SharedPtr, 16> robot_feedback_subscriptions_; @@ -198,10 +202,11 @@ class KenobiNode : public rclcpp::Node rclcpp::Service::SharedPtr override_service_; rclcpp::Service::SharedPtr play_enabled_service_; rclcpp::Publisher::SharedPtr playbook_state_publisher_; + rclcpp::Publisher::SharedPtr status_publisher_; ateam_common::GameControllerListener game_controller_listener_; - rclcpp::Publisher::SharedPtr world_publisher_; + rclcpp::Publisher::SharedPtr world_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -216,7 +221,7 @@ class KenobiNode : public rclcpp::Node } void blue_robot_state_callback( - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg, + const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg, int id) { const auto our_color = game_controller_listener_.GetTeamColor(); @@ -229,7 +234,7 @@ class KenobiNode : public rclcpp::Node } void yellow_robot_state_callback( - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg, + const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg, int id) { const auto our_color = game_controller_listener_.GetTeamColor(); @@ -244,7 +249,7 @@ class KenobiNode : public rclcpp::Node void robot_state_callback( std::array & robot_states, std::size_t id, - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg) + const ateam_msgs::msg::VisionStateRobot::SharedPtr robot_state_msg) { robot_states.at(id).visible = robot_state_msg->visible; if (robot_state_msg->visible) { @@ -278,7 +283,7 @@ class KenobiNode : public rclcpp::Node world_.our_robots.at(id).radio_connected = robot_connection_msg->radio_connected; } - void ball_state_callback(const ateam_msgs::msg::BallState::SharedPtr ball_state_msg) + void ball_state_callback(const ateam_msgs::msg::VisionStateBall::SharedPtr ball_state_msg) { world_.ball.visible = ball_state_msg->visible; const auto ball_visibility_timed_out = ateam_common::TimeDiffSeconds(world_.current_time, @@ -427,10 +432,12 @@ class KenobiNode : public rclcpp::Node "DETECTED TEAM SIDE WAS UNKNOWN"); } - fps_tracker_.update(world_); - world_publisher_->publish(ateam_kenobi::message_conversions::toMsg(world_)); + ateam_msgs::msg::KenobiStatus status_msg; + status_msg.fps = fps_tracker_.Update(world_); + status_publisher_->publish(status_msg); + auto motion_commands = runPlayFrame(world_); defense_area_enforcement::EnforceDefenseAreaKeepout(world_, motion_commands);