diff --git a/ateam_bringup/launch/state_tracking.launch.xml b/ateam_bringup/launch/state_tracking.launch.xml
index 1ae047ce..7f8496d0 100644
--- a/ateam_bringup/launch/state_tracking.launch.xml
+++ b/ateam_bringup/launch/state_tracking.launch.xml
@@ -3,7 +3,7 @@
-
+
diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt
new file mode 100644
index 00000000..bc1aa078
--- /dev/null
+++ b/new_super_vision_filter/CMakeLists.txt
@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.8)
+project(new_super_vision_filter)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(ateam_msgs REQUIRED)
+find_package(ateam_common REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+ src/vision_filter_node.cpp
+ src/camera.cpp
+ src/filtered_robot.cpp
+ src/filtered_ball.cpp
+ src/filter/kalman_filter.cpp
+)
+
+target_include_directories(${PROJECT_NAME}
+ PUBLIC
+ $
+ $
+ PRIVATE
+ ${CMAKE_CURRENT_SOURCE_DIR}/src
+)
+set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 20)
+ament_target_dependencies(${PROJECT_NAME}
+ rclcpp
+ rclcpp_components
+ ateam_msgs
+ ateam_common
+ tf2
+ tf2_geometry_msgs
+ ssl_league_msgs
+ Eigen3
+)
+target_link_libraries(${PROJECT_NAME}
+ Boost::boost
+)
+
+rclcpp_components_register_node(
+ ${PROJECT_NAME}
+ PLUGIN "new_super_vision::VisionFilterNode"
+ EXECUTABLE ateam_new_super_vision
+)
+install(TARGETS ${PROJECT_NAME}
+ EXPORT ${PROJECT_NAME}
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib
+ RUNTIME DESTINATION bin
+ INCLUDES DESTINATION include
+)
+
+if(BUILD_TESTING)
+ find_package(ament_cmake_lint_cmake REQUIRED)
+ find_package(ament_cmake_gtest REQUIRED)
+ file(GLOB_RECURSE AMENT_LINT_AUTO_FILE_EXCLUDE
+ "kalman/*"
+ "kalman/cmake/*"
+ )
+ file(GLOB_RECURSE ALL_CMAKE_FILES
+ "${CMAKE_CURRENT_SOURCE_DIR}/*.cmake"
+ "${CMAKE_CURRENT_SOURCE_DIR}/CMakeLists.txt"
+ )
+
+ list(FILTER ALL_CMAKE_FILES EXCLUDE REGEX
+ ".*/kalman/.*"
+ )
+
+ ament_lint_cmake(${ALL_CMAKE_FILES})
+ add_subdirectory(test)
+endif()
+
+ament_package()
diff --git a/new_super_vision_filter/package.xml b/new_super_vision_filter/package.xml
new file mode 100644
index 00000000..cad4180c
--- /dev/null
+++ b/new_super_vision_filter/package.xml
@@ -0,0 +1,27 @@
+
+
+
+ new_super_vision_filter
+ 1.0.0
+ wahoo! we can filter vision data!
+ christian
+ Apache-2.0
+
+ ament_cmake
+
+ rclcpp
+ rclcpp_components
+ tf2
+ tf2_geometry_msgs
+ eigen
+ ateam_msgs
+ ateam_common
+ boost
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp
new file mode 100644
index 00000000..1d7f8c50
--- /dev/null
+++ b/new_super_vision_filter/src/camera.cpp
@@ -0,0 +1,42 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include
+
+#include "camera.hpp"
+#include "filtered_ball.hpp"
+#include "filtered_robot.hpp"
+
+namespace ateam_super_vision
+{
+Camera::Camera(int camera_id)
+: camera_id(camera_id) {}
+
+void Camera::process_detection_frame(
+ const ssl_league_msgs::msg::VisionDetectionFrame & detection_frame_msg)
+{
+ (void)detection_frame_msg;
+}
+
+// void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData & geometry) {}
+
+void Camera::clear_old_messages() {}
+
+} // namespace ateam_super_vision
\ No newline at end of file
diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp
new file mode 100644
index 00000000..dc8a933c
--- /dev/null
+++ b/new_super_vision_filter/src/camera.hpp
@@ -0,0 +1,53 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef CAMERA_HPP_
+#define CAMERA_HPP_
+
+#include
+#include
+#include
+#include
+#include
+#include "filtered_ball.hpp"
+#include "filtered_robot.hpp"
+
+namespace ateam_super_vision {
+class Camera {
+public:
+ // Need to process an individual frame
+ // Set geometry from VisionGeometryCameraCalibration.msg
+ // Have a queue/buffer that we can remove old frames/have a set capacity
+ Camera(int camera_id);
+
+ void process_detection_frame(
+ const ssl_league_msgs::msg::VisionDetectionFrame & detection_frame_msg);
+
+ void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData & geometry);
+
+ void clear_old_messages();
+
+private:
+ int camera_id;
+ std::chrono::time_point last_updated;
+};
+} // namespace ateam_super_vision
+
+#endif // CAMERA_HPP_
diff --git a/new_super_vision_filter/src/filter/kalman_filter.cpp b/new_super_vision_filter/src/filter/kalman_filter.cpp
new file mode 100644
index 00000000..e9467f08
--- /dev/null
+++ b/new_super_vision_filter/src/filter/kalman_filter.cpp
@@ -0,0 +1,63 @@
+#include "filter/kalman_filter.hpp"
+
+#include
+
+namespace ateam_super_vision {
+ KalmanFilter::KalmanFilter() {}
+
+ void KalmanFilter::init(Eigen::VectorXd & initial_state, Eigen::MatrixXd & initial_error_covar) {
+ state_estimate = initial_state;
+ error_covar = initial_error_covar;
+ }
+
+ void KalmanFilter::predict(Eigen::VectorXd & control_input) {
+ state_estimate = state_transition_model * state_estimate + control_model * control_input;
+ error_covar = state_transition_model * error_covar * state_transition_model.transpose() + process_noise_covar;
+ }
+
+ void KalmanFilter::update(Eigen::VectorXd &measurement) {
+ // NOTE: This function expects you will normalize angles first outside of this function
+ // unlike in Joe's implementation.
+ Eigen::VectorXd predicted_measurement = measurement_model * state_estimate;
+ Eigen::VectorXd measure_residual = measurement - predicted_measurement;
+
+ // S matrix
+ Eigen::MatrixXd innovation_covar = measurement_model * error_covar
+ * measurement_model.transpose() + measurement_noise_covar;
+ // K matrix
+ Eigen::MatrixXd kalman_gain = error_covar * measurement_model.transpose() * innovation_covar.inverse();
+
+ state_estimate = state_estimate + kalman_gain * measure_residual;
+
+ Eigen::MatrixXd I = Eigen::MatrixXd::Identity(error_covar.rows(), error_covar.cols());
+ // Also have to update the error covariance here
+ error_covar = (I - kalman_gain * measurement_model) * error_covar *
+ (I - kalman_gain * measurement_model).transpose() +
+ (kalman_gain * measurement_noise_covar * kalman_gain.transpose());
+ }
+
+ void KalmanFilter::set_process_noise_covar(Eigen::MatrixXd &covar_mat) {
+ this->process_noise_covar = covar_mat;
+ }
+
+ void KalmanFilter::set_measurement_noise_covar(Eigen::MatrixXd &covar_mat) {
+ this->measurement_noise_covar = covar_mat;
+ }
+
+ void KalmanFilter::set_state_transition_model(Eigen::MatrixXd &model_mat) {
+ this->state_transition_model = model_mat;
+ }
+
+ void KalmanFilter::set_control_model(Eigen::MatrixXd &model_mat) {
+ this->control_model = model_mat;
+ }
+
+ void KalmanFilter::set_measurement_model(Eigen::MatrixXd &model_mat) {
+ this->measurement_model = model_mat;
+ }
+
+ const Eigen::VectorXd & KalmanFilter::get_state_estimate() const {
+ return state_estimate;
+ }
+}
+
diff --git a/new_super_vision_filter/src/filter/kalman_filter.hpp b/new_super_vision_filter/src/filter/kalman_filter.hpp
new file mode 100644
index 00000000..06d8f0ca
--- /dev/null
+++ b/new_super_vision_filter/src/filter/kalman_filter.hpp
@@ -0,0 +1,50 @@
+#ifndef FILTER__KALMAN_FILTER_HPP_
+#define FILTER__KALMAN_FILTER_HPP_
+
+#include
+
+namespace ateam_super_vision {
+
+class KalmanFilter {
+ public:
+ KalmanFilter();
+
+ void init(Eigen::VectorXd &initial_state, Eigen::MatrixXd &initial_error_covar);
+
+ void predict(Eigen::VectorXd &control_input);
+
+ void update(Eigen::VectorXd &measurement);
+
+ void set_process_noise_covar(Eigen::MatrixXd &covar_mat);
+
+ void set_measurement_noise_covar(Eigen::MatrixXd &covar_mat);
+
+ void set_state_transition_model(Eigen::MatrixXd &model_mat);
+
+ void set_control_model(Eigen::MatrixXd &model_mat);
+
+ void set_measurement_model(Eigen::MatrixXd &model_mat);
+
+ const Eigen::VectorXd & get_state_estimate() const;
+
+ private:
+ // P matrix
+ Eigen::MatrixXd error_covar;
+ // Q matrix
+ Eigen::MatrixXd process_noise_covar;
+ // R matrix
+ Eigen::MatrixXd measurement_noise_covar;
+ // F matrix
+ Eigen::MatrixXd state_transition_model;
+ // B matrix
+ Eigen::MatrixXd control_model;
+ // H matrix
+ Eigen::MatrixXd measurement_model;
+
+ // x_hat
+ Eigen::VectorXd state_estimate;
+};
+
+} // namespace ateam_super_vision
+
+#endif // FILTER__KALMAN_FILTER_HPP_
\ No newline at end of file
diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp
new file mode 100644
index 00000000..afd42c3b
--- /dev/null
+++ b/new_super_vision_filter/src/filter_types.hpp
@@ -0,0 +1,282 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef FILTER_TYPES_HPP_
+#define FILTER_TYPES_HPP_
+
+#include
+#include
+#include
+
+namespace ateam_super_vision
+{
+/*
+ Position measurement for robot or balls
+
+ Structure is {
+ x_pos
+ y_pos
+ }
+
+ Measurements are in m
+*/
+class PosMeasurement : public Eigen::Vector2d
+{
+public:
+ using Base = Eigen::Vector2d;
+ using Base::Base;
+ using Base::operator=;
+
+ static constexpr size_t X = 0;
+ static constexpr size_t Y = 1;
+
+ double x() const
+ {
+ return (*this)[X];
+ }
+
+ double y() const
+ {
+ return (*this)[Y];
+ }
+};
+
+/*
+ 4D state vector for robot or ball position
+
+ State vector structure is
+ { x_pos,
+ y_pos,
+ x_vel,
+ y_vel }
+
+ Measurements are in m
+*/
+class PosState : public Eigen::Vector4d
+{
+public:
+ using Base = Eigen::Vector4d;
+ using Base::Base;
+ using Base::operator=;
+
+ static constexpr size_t PX = 0;
+ static constexpr size_t PY = 1;
+ static constexpr size_t VX = 2;
+ static constexpr size_t VY = 3;
+
+ double px() const {return (*this)[PX];}
+ double py() const {return (*this)[PY];}
+ double vx() const {return (*this)[VX];}
+ double vy() const {return (*this)[VY];}
+};
+
+/*
+ Measurement model for robots/balls' X and Y pos from position
+ measurements.
+
+ Curently assumes no measurement noise.
+*/
+class PosMeasurementModel
+{
+public:
+ // H matrix: maps a PosState to the position-only PosMeasurement it predicts.
+ static Eigen::MatrixXd measurementMatrix()
+ {
+ Eigen::MatrixXd H = Eigen::MatrixXd::Zero(2, 4);
+ H(PosMeasurement::X, PosState::PX) = 1; // dz_px / d_px
+ H(PosMeasurement::Y, PosState::PY) = 1; // dz_py / d_py
+ return H;
+ }
+
+ // R matrix. Currently assumes no measurement noise.
+ static Eigen::MatrixXd measurementNoiseCovar()
+ {
+ return Eigen::MatrixXd::Zero(2, 2);
+ }
+};
+
+/*
+ System (measurement to state) transition model for X/Y position
+ that assumes a constant velocity and no control inputs.
+*/
+class PosSystemModel
+{
+public:
+ PosSystemModel()
+ : last_update(std::chrono::steady_clock::now()) {}
+
+ // Seconds elapsed since the previous call, resetting the internal clock.
+ double stepDt()
+ {
+ const auto now = std::chrono::steady_clock::now();
+ const std::chrono::duration dt = now - last_update;
+ last_update = now;
+ return dt.count();
+ }
+
+ /*
+ F matrix for the given elapsed time.
+
+ We assume the velocity stays constant and add v * dt to the
+ current position.
+
+ pos_t = pos_{t-1} + vel_{t-1} * dt
+ vel_t = vel_{t-1}
+ */
+ static Eigen::MatrixXd stateTransition(double dt_s)
+ {
+ Eigen::MatrixXd F = Eigen::MatrixXd::Identity(4, 4);
+ F(PosState::PX, PosState::VX) = dt_s;
+ F(PosState::PY, PosState::VY) = dt_s;
+ return F;
+ }
+
+ // Q matrix. Values borrowed from the previous vision filter.
+ static Eigen::MatrixXd processNoiseCovar()
+ {
+ return Eigen::MatrixXd::Identity(4, 4) * 1e-3;
+ }
+
+private:
+ std::chrono::time_point last_update;
+};
+
+/*
+ Angular position measurement (in rad)
+*/
+class AngleMeasurement : public Eigen::Matrix
+{
+public:
+ using Base = Eigen::Matrix;
+ using Base::Base;
+ using Base::operator=;
+
+ static constexpr size_t W = 0;
+
+ double w() const
+ {
+ return (*this)[W];
+ }
+};
+
+/*
+ Angular state (position and velocity)
+
+ State vector format is
+ {
+ w_pos
+ w_vel
+ }
+*/
+class AngleState : public Eigen::Vector2d
+{
+public:
+ using Base = Eigen::Vector2d;
+ using Base::Base;
+ using Base::operator=;
+
+ static constexpr size_t PW = 0;
+ static constexpr size_t VW = 1;
+
+ double pw() const {return (*this)[PW];}
+ double vw() const {return (*this)[VW];}
+};
+
+/*
+ Angle measurement model.
+
+ Currently assumes no measurement noise.
+*/
+class AngleMeasurementModel
+{
+public:
+ // H matrix: maps an AngleState to the angle-only AngleMeasurement it predicts.
+ static Eigen::MatrixXd measurementMatrix()
+ {
+ Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 2);
+ H(AngleMeasurement::W, AngleState::PW) = 1; // dz_pw / d_pw
+ return H;
+ }
+
+ // R matrix. Measurement error borrowed from previous vision filter.
+ static Eigen::MatrixXd measurementNoiseCovar()
+ {
+ const double sigma_theta_squared = 0.01; // Position measurement error
+ Eigen::MatrixXd R = Eigen::MatrixXd::Zero(1, 1);
+ R(0, 0) = sigma_theta_squared;
+ return R;
+ }
+};
+
+/*
+ System (measurement to state) transition model for angular position
+ that assumes a constant angular velocity and no control inputs.
+*/
+class AngleSystemModel
+{
+public:
+ AngleSystemModel()
+ : last_update(std::chrono::steady_clock::now()) {}
+
+ // Seconds elapsed since the previous call, resetting the internal clock.
+ double stepDt()
+ {
+ const auto now = std::chrono::steady_clock::now();
+ const std::chrono::duration dt = now - last_update;
+ last_update = now;
+ return dt.count();
+ }
+
+ /*
+ F matrix for the given elapsed time.
+
+ We assume the angular velocity stays constant and add w * dt to the
+ current angle. Callers are responsible for wrapping the resulting
+ angle to (-pi, pi], since that's a nonlinear operation the F matrix
+ can't express.
+
+ pos_t = pos_{t-1} + vel_{t-1} * dt
+ vel_t = vel_{t-1}
+ */
+ static Eigen::MatrixXd stateTransition(double dt_s)
+ {
+ Eigen::MatrixXd F = Eigen::MatrixXd::Identity(2, 2);
+ F(AngleState::PW, AngleState::VW) = dt_s;
+ return F;
+ }
+
+ static double wrapAngle(double angle)
+ {
+ return std::fmod(angle + M_PI, 2 * M_PI) - M_PI;
+ }
+
+ // Q matrix. No equivalent value was defined in the previous vision
+ // filter; this magnitude matches the position filter's process noise.
+ static Eigen::MatrixXd processNoiseCovar()
+ {
+ return Eigen::MatrixXd::Identity(2, 2) * 1e-3;
+ }
+
+private:
+ std::chrono::time_point last_update;
+};
+} // namespace ateam_super_vision
+
+#endif // FILTER_TYPES_HPP
diff --git a/new_super_vision_filter/src/filtered_ball.cpp b/new_super_vision_filter/src/filtered_ball.cpp
new file mode 100644
index 00000000..902dd3dd
--- /dev/null
+++ b/new_super_vision_filter/src/filtered_ball.cpp
@@ -0,0 +1,94 @@
+#include "filtered_ball.hpp"
+
+namespace ateam_super_vision {
+FilteredBall::FilteredBall(const BallMeasurement & measurement)
+{
+ Eigen::VectorXd initial_state_xy(4);
+ // We don't get a velocity input in the measurement itself,
+ // so that starts at 0.
+ initial_state_xy <<
+ measurement.pos.x(),
+ measurement.pos.y(),
+ 0,
+ 0;
+ // This is in m, so initial covariance is 100 mm.
+ Eigen::MatrixXd initial_error_covar = Eigen::MatrixXd::Identity(4, 4) * 1e-3;
+
+ // We don't add any control inputs to the vision system, so the
+ // control model maps a zero-length control vector onto the state.
+ Eigen::MatrixXd control_model = Eigen::MatrixXd::Zero(4, 0);
+ Eigen::MatrixXd measurement_model = PosMeasurementModel::measurementMatrix();
+ Eigen::MatrixXd measurement_noise_covar = PosMeasurementModel::measurementNoiseCovar();
+ Eigen::MatrixXd process_noise_covar = PosSystemModel::processNoiseCovar();
+
+ posFilterXY.set_control_model(control_model);
+ posFilterXY.set_measurement_model(measurement_model);
+ posFilterXY.set_measurement_noise_covar(measurement_noise_covar);
+ posFilterXY.set_process_noise_covar(process_noise_covar);
+
+ posFilterXY.init(initial_state_xy, initial_error_covar);
+ posXYEstimate = initial_state_xy;
+}
+
+void FilteredBall::update(const BallMeasurement & measurement)
+{
+ // Make sure this detection isn't crazy off from our previous ones
+ // (unless our filter is still new/only has a few measurements)
+ if (age < oldEnough) {
+ ++age;
+ }
+ if (health < maxHealth) {
+ health += 2;
+ }
+ bool is_new = age < oldEnough;
+ // As long as its reasonable, update the Kalman Filter
+ const std::chrono::time_point now =
+ std::chrono::steady_clock::now();
+ // If it's been too long, don't use this message
+ if (now - measurement.timestamp > update_threshold || is_new) {
+ return;
+ }
+ // The state transition matrix (F) depends on the elapsed time, so it's
+ // rebuilt every update with the latest dt.
+ Eigen::MatrixXd state_transition_model = PosSystemModel::stateTransition(systemModelXY.stepDt());
+ posFilterXY.set_state_transition_model(state_transition_model);
+
+ // Predict state forward
+ // Predict covariance forward
+ // (All encompassed by the .predict() function)
+ Eigen::VectorXd control_input(0);
+ posFilterXY.predict(control_input);
+
+ // Compute Kalman gain (contained in filter)
+ // Update state estimate (contained in filter)
+ // Update covariance estimate (contained in filter)
+ // All encompassed by the .update() function
+ Eigen::VectorXd z(2);
+ z << measurement.pos.x(), measurement.pos.y();
+ posFilterXY.update(z);
+
+ posXYEstimate = posFilterXY.get_state_estimate();
+}
+
+ateam_msgs::msg::VisionStateBall FilteredBall::toMsg()
+{
+ ateam_msgs::msg::VisionStateBall ball_state_msg{};
+ bool is_new = age < oldEnough;
+
+ if (health > 0 && !is_new) {
+ // NOTE: Does not contain acceleration info
+ ball_state_msg.visible = true;
+ ball_state_msg.pose.position.x = posXYEstimate.px();
+ ball_state_msg.pose.position.y = posXYEstimate.py();
+ ball_state_msg.twist.linear.x = posXYEstimate.vx();
+ ball_state_msg.twist.linear.y = posXYEstimate.vy();
+ --health;
+ }
+ return ball_state_msg;
+}
+
+bool FilteredBall::isHealthy() const
+{
+ return health > 0;
+}
+} // namespace ateam_super_vision
\ No newline at end of file
diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp
new file mode 100644
index 00000000..358f1604
--- /dev/null
+++ b/new_super_vision_filter/src/filtered_ball.hpp
@@ -0,0 +1,66 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef FILTERED_BALL_HPP_
+#define FILTERED_BALL_HPP_
+
+#include
+#include
+#include
+#include
+#include "measurements/ball_measurement.hpp"
+#include "filter/kalman_filter.hpp"
+
+namespace ateam_super_vision {
+enum KickState
+{
+ ROLLING,
+ KICKED,
+ CHIPPED
+};
+
+class FilteredBall {
+public:
+ FilteredBall(const BallMeasurement & measurement);
+
+ void update(const BallMeasurement & measurement);
+
+ ateam_msgs::msg::VisionStateBall toMsg();
+
+ bool isHealthy() const;
+
+private:
+ int age = 0;
+ int oldEnough = 3;
+ int maxHealth = 20;
+ int health = 2;
+ double maxDistance = -1.0;
+ KickState currentKickState = ROLLING;
+ std::chrono::milliseconds update_threshold{50};
+ std::chrono::steady_clock::time_point timestamp;
+ std::chrono::steady_clock::time_point last_visible_timestamp;
+ // Filter
+ ateam_super_vision::KalmanFilter posFilterXY;
+ PosState posXYEstimate;
+ PosSystemModel systemModelXY;
+};
+} // namespace ateam_super_vision
+
+#endif // FILTERED_BALL_HPP_
diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp
new file mode 100644
index 00000000..ceb62cd1
--- /dev/null
+++ b/new_super_vision_filter/src/filtered_robot.cpp
@@ -0,0 +1,192 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include "filtered_robot.hpp"
+#include "filter_types.hpp"
+#include "measurements/robot_measurement.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+// See https://thekalmanfilter.com/extended-kalman-filter-python-example/
+// or https://thekalmanfilter.com/kalman-filter-explained-simply/
+// OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp
+
+namespace ateam_super_vision {
+FilteredRobot::FilteredRobot(
+ const RobotMeasurement & measurement,
+ ateam_common::TeamColor & team_color)
+: posFilterXY(), posFilterW(), bot_id(measurement.getId()), team(team_color)
+{
+ // Initialize XY KF
+ Eigen::VectorXd initial_state_xy(4);
+ initial_state_xy <<
+ measurement.pos.x(),
+ measurement.pos.y(),
+ 0,
+ 0;
+ // This is in m, so initial covariance is 100 mm.
+ // We don't get a velocity input in the measurement itself,
+ // so that has a large initial uncertainty.
+ Eigen::MatrixXd xy_covariance(4, 4);
+ xy_covariance << 1e-4, 0, 0, 0,
+ 0, 1e-4, 0, 0,
+ 0, 0, 1e-2, 0,
+ 0, 0, 0, 1e-2;
+
+ Eigen::MatrixXd control_model_xy = Eigen::MatrixXd::Zero(4, 0);
+ Eigen::MatrixXd measurement_model_xy = PosMeasurementModel::measurementMatrix();
+ Eigen::MatrixXd measurement_noise_covar_xy = PosMeasurementModel::measurementNoiseCovar();
+ Eigen::MatrixXd process_noise_covar_xy = PosSystemModel::processNoiseCovar();
+
+ posFilterXY.set_control_model(control_model_xy);
+ posFilterXY.set_measurement_model(measurement_model_xy);
+ posFilterXY.set_measurement_noise_covar(measurement_noise_covar_xy);
+ posFilterXY.set_process_noise_covar(process_noise_covar_xy);
+
+ posFilterXY.init(initial_state_xy, xy_covariance);
+ posXYEstimate = initial_state_xy;
+
+ // Initialize angular KF
+ /*
+ State vector is simply
+ w_pos,
+ w_vel
+ */
+ Eigen::VectorXd initial_state_w(2);
+ initial_state_w <<
+ measurement.angle.w(),
+ 0;
+ /*
+ Initial covariance is approx 2 deg. for pos,
+ 10 deg. for vel
+ */
+ Eigen::MatrixXd w_covariance(2, 2);
+ w_covariance << M_PI / 180.0, 0,
+ 0, M_PI / 180.0;
+
+ Eigen::MatrixXd control_model_w = Eigen::MatrixXd::Zero(2, 0);
+ Eigen::MatrixXd measurement_model_w = AngleMeasurementModel::measurementMatrix();
+ Eigen::MatrixXd measurement_noise_covar_w = AngleMeasurementModel::measurementNoiseCovar();
+ Eigen::MatrixXd process_noise_covar_w = AngleSystemModel::processNoiseCovar();
+
+ posFilterW.set_control_model(control_model_w);
+ posFilterW.set_measurement_model(measurement_model_w);
+ posFilterW.set_measurement_noise_covar(measurement_noise_covar_w);
+ posFilterW.set_process_noise_covar(process_noise_covar_w);
+
+ posFilterW.init(initial_state_w, w_covariance);
+ posWEstimate = initial_state_w;
+}
+
+void FilteredRobot::update(const RobotMeasurement & measurement)
+{
+ // Make sure this detection isn't crazy off from our previous ones
+ // (unless our filter is still new/only has a few measurements)
+ if (age < oldEnough) {
+ ++age;
+ }
+ if (health < maxHealth) {
+ health += 2;
+ }
+ bool is_new = age < oldEnough;
+ // As long as its reasonable, update the Kalman Filter
+ const std::chrono::time_point now =
+ std::chrono::steady_clock::now();
+ // If it's been too long, don't use this message
+ if (now - measurement.getTimestamp() > update_threshold) {
+ return;
+ }
+ if (is_new) {
+ return;
+ }
+ // The state transition matrices (F) depend on elapsed time, so they're
+ // rebuilt every update with the latest dt.
+ Eigen::MatrixXd state_transition_xy = PosSystemModel::stateTransition(systemModelXY.stepDt());
+ posFilterXY.set_state_transition_model(state_transition_xy);
+ Eigen::MatrixXd state_transition_w = AngleSystemModel::stateTransition(systemModelW.stepDt());
+ posFilterW.set_state_transition_model(state_transition_w);
+
+ // Predict state forward
+ // Predict covariance forward
+ // (All encompassed by the .predict() function)
+ Eigen::VectorXd control_input_xy(0);
+ posFilterXY.predict(control_input_xy);
+ Eigen::VectorXd control_input_w(0);
+ posFilterW.predict(control_input_w);
+
+ // Compute Kalman gain (contained in filter)
+ // Update state estimate (contained in filter)
+ // Update covariance estimate (contained in filter)
+ // All encompassed by the .update() function
+ Eigen::VectorXd z_xy(2);
+ z_xy << measurement.pos.x(), measurement.pos.y();
+ posFilterXY.update(z_xy);
+ posXYEstimate = posFilterXY.get_state_estimate();
+
+ Eigen::VectorXd z_w(1);
+ z_w << measurement.angle.w();
+ posFilterW.update(z_w);
+ posWEstimate = posFilterW.get_state_estimate();
+ posWEstimate[AngleState::PW] = AngleSystemModel::wrapAngle(posWEstimate[AngleState::PW]);
+}
+
+ateam_msgs::msg::VisionStateRobot FilteredRobot::toMsg()
+{
+ ateam_msgs::msg::VisionStateRobot robot_state_msg{};
+ bool is_new = age < oldEnough;
+
+ if (health > 0 && !is_new) {
+ robot_state_msg.visible = true;
+ robot_state_msg.pose.position.x = posXYEstimate.px();
+ robot_state_msg.pose.position.y = posXYEstimate.py();
+ robot_state_msg.twist.linear.x = posXYEstimate.vx();
+ robot_state_msg.twist.linear.y = posXYEstimate.vy();
+
+ robot_state_msg.pose.orientation =
+ tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), posWEstimate.pw()));
+ robot_state_msg.twist.angular.z = posWEstimate.vw();
+
+ // Convert to body velocities for plotting/debugging
+ ateam_geometry::Vector velocity(robot_state_msg.twist.linear.x, robot_state_msg.twist.linear.y);
+ CGAL::Aff_transformation_2 transformation(CGAL::ROTATION,
+ std::sin(-posWEstimate.pw()), std::cos(-posWEstimate.pw()));
+ const auto velocity_trans = velocity.transform(transformation);
+ robot_state_msg.twist_body.linear.x = velocity_trans.x();
+ robot_state_msg.twist_body.linear.y = velocity_trans.y();
+ robot_state_msg.twist_body.angular.z = robot_state_msg.twist.angular.z;
+ --health;
+ }
+ return robot_state_msg;
+}
+
+int FilteredRobot::getId() const
+{
+ return bot_id;
+}
+
+bool FilteredRobot::isHealthy() const
+{
+ return health > 0;
+}
+} // namespace ateam_super_vision
diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp
new file mode 100644
index 00000000..9eb7f694
--- /dev/null
+++ b/new_super_vision_filter/src/filtered_robot.hpp
@@ -0,0 +1,70 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+#ifndef FILTERED_ROBOT_HPP_
+#define FILTERED_ROBOT_HPP_
+
+#include "filter/kalman_filter.hpp"
+#include "filter_types.hpp"
+#include "measurements/robot_measurement.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+namespace ateam_super_vision {
+class FilteredRobot {
+public:
+ FilteredRobot(const RobotMeasurement & measurement, ateam_common::TeamColor & team_color);
+
+ void update(const RobotMeasurement & measurement);
+
+ ateam_msgs::msg::VisionStateRobot toMsg();
+
+ int getId() const;
+
+ bool isHealthy() const;
+
+private:
+ ateam_super_vision::KalmanFilter posFilterXY;
+ ateam_super_vision::KalmanFilter posFilterW;
+ int age = 0;
+ int oldEnough = 3;
+ int health = 2;
+ int maxHealth = 20;
+ double maxDistance = -1.0;
+ std::chrono::milliseconds update_threshold{2000};
+ std::chrono::time_point timestamp;
+ std::chrono::time_point last_visible_timestamp;
+ // double height; // in m
+ // Use the below filtered values when getting X/Y/w (theta) and
+ // velocities
+ // X, Y
+ PosSystemModel systemModelXY;
+ PosState posXYEstimate{};
+ // Theta
+ AngleSystemModel systemModelW;
+ AngleState posWEstimate{};
+ int bot_id;
+ ateam_common::TeamColor team;
+};
+} // namespace ateam_super_vision
+#endif // FILTERED_ROBOT_HPP_
diff --git a/new_super_vision_filter/src/measurements/ball_measurement.hpp b/new_super_vision_filter/src/measurements/ball_measurement.hpp
new file mode 100644
index 00000000..ca4dcfa1
--- /dev/null
+++ b/new_super_vision_filter/src/measurements/ball_measurement.hpp
@@ -0,0 +1,50 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef BALL_MEASUREMENT_HPP_
+#define BALL_MEASUREMENT_HPP_
+
+#include "filter_types.hpp"
+
+#include
+
+namespace ateam_super_vision {
+class BallMeasurement {
+public:
+ BallMeasurement(const ssl_league_msgs::msg::VisionDetectionBall & ball_detection, int & camera_id)
+ : camera_id(camera_id)
+ {
+ pos << ball_detection.pos.x,
+ ball_detection.pos.y;
+ timestamp = std::chrono::steady_clock::now();
+ }
+
+ void invert()
+ {
+ pos *= -1.0;
+ }
+
+ PosMeasurement pos;
+ std::chrono::time_point timestamp;
+ int camera_id;
+};
+} // namespace ateam_super_vision
+
+#endif // BALL_MEASUREMENT_HPP_
diff --git a/new_super_vision_filter/src/measurements/robot_measurement.hpp b/new_super_vision_filter/src/measurements/robot_measurement.hpp
new file mode 100644
index 00000000..0a242688
--- /dev/null
+++ b/new_super_vision_filter/src/measurements/robot_measurement.hpp
@@ -0,0 +1,91 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef ROBOT_MEASUREMENT_HPP_
+#define ROBOT_MEASUREMENT_HPP_
+
+#include
+#include
+#include
+// TODO (christian): Which of these do we actually need?
+#include
+#include
+#include
+
+#include "filter_types.hpp"
+
+namespace ateam_super_vision {
+/**
+ * @brief Robot position and angle measurement from a single camera detection,
+ * used to provide updates to the Kalman filter.
+ */
+class RobotMeasurement {
+public:
+ RobotMeasurement(
+ const ssl_league_msgs::msg::VisionDetectionRobot & bot_detection,
+ int & camera_id,
+ ateam_common::TeamColor & team
+ )
+ : camera_id(camera_id), team(team)
+ {
+ pos << bot_detection.pose.position.x,
+ bot_detection.pose.position.y;
+ tf2::Quaternion tf2_quat;
+ tf2::fromMsg(bot_detection.pose.orientation, tf2_quat);
+ angle << tf2::getYaw(tf2_quat);
+ robot_id = bot_detection.robot_id;
+ timestamp = std::chrono::steady_clock::now();
+ }
+
+ int getId() const
+ {
+ return robot_id;
+ }
+
+ void invert()
+ {
+ pos *= -1.0;
+ // A bit kludgy... since we use templating of Eigen types for measurements,
+ // this seemed like the easiest way to modify the data and ensure it's
+ // clamped properly without writing a bunch of extra functions/code
+ double new_angle = angles::normalize_angle_positive(angle.w() + M_PI);
+ AngleMeasurement new_angle_measurement;
+ new_angle_measurement << new_angle;
+ angle = new_angle_measurement;
+ }
+
+ std::chrono::time_point getTimestamp() const
+ {
+ return timestamp;
+ }
+
+ PosMeasurement pos;
+ AngleMeasurement angle;
+
+private:
+ std::chrono::time_point timestamp;
+ int camera_id;
+ ateam_common::TeamColor team;
+ int robot_id;
+
+};
+} // namespace ateam_super_vision
+
+#endif // ROBOT_MEASUREMENT_HPP_
diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp
new file mode 100644
index 00000000..9cfea30c
--- /dev/null
+++ b/new_super_vision_filter/src/vision_filter_node.cpp
@@ -0,0 +1,281 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+#include "camera.hpp"
+#include "filtered_robot.hpp"
+#include "filtered_ball.hpp"
+#include "measurements/ball_measurement.hpp"
+#include "measurements/robot_measurement.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using namespace std::chrono_literals;
+
+namespace ateam_super_vision
+{
+
+class VisionFilterNode : public rclcpp::Node
+{
+
+public:
+ explicit VisionFilterNode(const rclcpp::NodeOptions & options)
+ : rclcpp::Node("ateam_new_super_vision", options),
+ game_controller_listener_(*this)
+ {
+ declare_parameters("offsets", {
+ {"robots.x", 0.0},
+ {"robots.y", 0.0}
+ });
+
+ timer_ = create_wall_timer(10ms, std::bind(&VisionFilterNode::timer_callback, this));
+
+ 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
+ );
+
+ ssl_vision_subscription_ =
+ create_subscription(
+ std::string(Topics::kVisionMessages),
+ 10,
+ std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1));
+
+ field_subscription_ =
+ create_subscription(
+ std::string(Topics::kField),
+ 10,
+ std::bind(&VisionFilterNode::field_callback, this, std::placeholders::_1));
+ }
+
+ // Will also need to add publishers here and wall clock timer
+
+private:
+ std::map cameras;
+ rclcpp::TimerBase::SharedPtr timer_;
+
+ std::vector blue_robots;
+ std::vector yellow_robots;
+ std::optional ball;
+
+ boost::circular_buffer ball_measurements{10};
+ boost::circular_buffer blue_measurements{50};
+ boost::circular_buffer yellow_measurements{50};
+
+ // All this stuff interacts with other nodes (pub/sub related)
+ std::array::SharedPtr,
+ 16> blue_robots_publisher_;
+ std::array::SharedPtr,
+ 16> yellow_robots_publisher_;
+ rclcpp::Publisher::SharedPtr ball_publisher_;
+
+ ateam_common::GameControllerListener game_controller_listener_;
+ rclcpp::Subscription::SharedPtr ssl_vision_subscription_;
+ // The two below are in case we are sharing a field during testing at event
+ rclcpp::Subscription::SharedPtr field_subscription_;
+ int ignore_side_;
+
+ void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg)
+ {
+ const auto team_side = game_controller_listener_.GetTeamSide();
+ // Add detections to the queues
+ if (!vision_wrapper_msg->detection.empty()) {
+ for (const auto & detection : vision_wrapper_msg->detection) {
+ int detect_camera = detection.camera_id;
+ // Create a new camera if we haven't seen this one before
+ if (!(cameras.contains(detect_camera))) {
+ cameras.try_emplace(detect_camera, detect_camera);
+ }
+
+ // Create a measurement from each robot in this message
+ for (const auto & bot : detection.robots_yellow) {
+ ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow;
+ auto measurement = RobotMeasurement(bot, detect_camera, team_color);
+ yellow_measurements.push_back(
+ measurement
+ );
+ }
+
+ for (const auto & bot : detection.robots_blue) {
+ ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue;
+
+ auto measurement = RobotMeasurement(bot, detect_camera, team_color);
+ blue_measurements.push_back(
+ measurement
+ );
+ }
+
+ // Create a measurement from each ball in this message
+ for (const auto & ball: detection.balls) {
+ auto measurement = BallMeasurement(ball, detect_camera);
+ ball_measurements.push_back(
+ measurement
+ );
+ }
+ }
+
+ if (team_side == ateam_common::TeamSide::PositiveHalf) {
+ for (auto & measurement : yellow_measurements) {
+ measurement.invert();
+ }
+
+ for (auto & measurement : blue_measurements) {
+ measurement.invert();
+ }
+
+ for (auto & measurement : ball_measurements) {
+ measurement.invert();
+ }
+ }
+
+ // Sort our measurements to make sure they are in order of the time received
+ // in case created/processed them out of order
+
+ // Process all the new updates from the measurements
+ for (const auto & bot_measurement : blue_measurements) {
+ ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue;
+ auto it = std::find_if(
+ blue_robots.begin(),
+ blue_robots.end(),
+ [bot_measurement](const FilteredRobot & bot){
+ return bot_measurement.getId() == bot.getId();
+ }
+ );
+ if (it == blue_robots.end()) {
+ blue_robots.push_back(
+ FilteredRobot(bot_measurement, team_color)
+ );
+ } else {
+ it->update(bot_measurement);
+ }
+ }
+ for (const auto & bot_measurement : yellow_measurements) {
+ ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow;
+ auto it = std::find_if(
+ yellow_robots.begin(),
+ yellow_robots.end(),
+ [bot_measurement](const FilteredRobot & bot){
+ return bot_measurement.getId() == bot.getId();
+ }
+ );
+ if (it == yellow_robots.end()) {
+ yellow_robots.push_back(
+ FilteredRobot(bot_measurement, team_color)
+ );
+ } else {
+ it->update(bot_measurement);
+ }
+ }
+ for (const auto & ball_measurement : ball_measurements) {
+ if (!ball.has_value()) {
+ ball = FilteredBall(ball_measurement);
+ } else {
+ ball->update(ball_measurement);
+ }
+ }
+ }
+ return;
+ }
+
+ void timer_callback()
+ {
+ // Remove the ball if it's bad quality
+ if (ball.has_value()) {
+ if (!ball.value().isHealthy()) {
+ ball.reset();
+ }
+ }
+ // Erase any robots that are bad quality
+ std::erase_if(blue_robots, [](FilteredRobot & bot) {return !bot.isHealthy();});
+ std::erase_if(yellow_robots, [](FilteredRobot & bot) {return !bot.isHealthy();});
+ // Publish the most recent ball
+ ateam_msgs::msg::VisionStateBall ball_msg{};
+ if (ball.has_value()) {
+ ball_msg = ball.value().toMsg();
+ }
+ ball_publisher_->publish(ball_msg);
+
+ // Publish the robots
+ // - Blue -
+ for (int id = 0; id < 16; id++) {
+ auto robot_msg = ateam_msgs::msg::VisionStateRobot{};
+ auto it = std::find_if(
+ blue_robots.begin(),
+ blue_robots.end(),
+ [id](const FilteredRobot & bot){return id == bot.getId();}
+ );
+ if (it != blue_robots.end()) {
+ robot_msg = it->toMsg();
+ }
+ blue_robots_publisher_.at(id)->publish(robot_msg);
+ }
+ // - Yellow -
+ for (int id = 0; id < 16; id++) {
+ auto robot_msg = ateam_msgs::msg::VisionStateRobot{};
+ auto it = std::find_if(
+ yellow_robots.begin(),
+ yellow_robots.end(),
+ [id](const FilteredRobot & bot){return id == bot.getId();}
+ );
+ if (it != yellow_robots.end()) {
+ robot_msg = it->toMsg();
+ }
+ yellow_robots_publisher_.at(id)->publish(robot_msg);
+ }
+ return;
+ }
+
+ void field_callback(
+ const ateam_msgs::msg::FieldInfo::SharedPtr field_info_msg)
+ {
+ const auto team_side = game_controller_listener_.GetTeamSide();
+ if (team_side == ateam_common::TeamSide::PositiveHalf) {
+ ignore_side_ = -field_info_msg->ignore_side;
+ } else {
+ ignore_side_ = field_info_msg->ignore_side;
+ }
+ }
+};
+
+} // namespace ateam_super_vision
+
+RCLCPP_COMPONENTS_REGISTER_NODE(ateam_super_vision::VisionFilterNode)
diff --git a/new_super_vision_filter/test/CMakeLists.txt b/new_super_vision_filter/test/CMakeLists.txt
new file mode 100644
index 00000000..4ee4f9b3
--- /dev/null
+++ b/new_super_vision_filter/test/CMakeLists.txt
@@ -0,0 +1,8 @@
+find_package(ament_cmake_gtest REQUIRED)
+ament_add_gtest(filter_test filter_test.cpp)
+
+target_include_directories(filter_test PUBLIC
+ $
+)
+
+target_link_libraries(filter_test ${PROJECT_NAME})
diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp
new file mode 100644
index 00000000..085a4ca1
--- /dev/null
+++ b/new_super_vision_filter/test/filter_test.cpp
@@ -0,0 +1,91 @@
+// Copyright 2026 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include "filtered_robot.hpp"
+#include "measurements/robot_measurement.hpp"
+
+namespace ateam_msgs
+{
+namespace msg
+{
+void PrintTo(const VisionStateRobot & state, std::ostream * os)
+{
+ *os << "\n" << "VisionStateRobot: \n" <<
+ "x pos: " << state.pose.position.x << " y pos: " << state.pose.position.y <<
+ " z pos: " << state.pose.position.z << "\n" << "visible? " << state.visible <<
+ "\n" << "orientation: " << state.pose.orientation.w;
+}
+}
+}
+
+class FilteredRobotTest : public ::testing::Test
+{
+protected:
+ ateam_common::TeamColor team = ateam_common::TeamColor::Blue;
+ int oldEnoughAge = 2;
+ ssl_league_msgs::msg::VisionDetectionRobot robot_msg{};
+ std::unique_ptr bot;
+
+ void SetUp() override
+ {
+ int camera = 0;
+ auto measurement = ateam_super_vision::RobotMeasurement(robot_msg, camera, team);
+ bot = std::make_unique(measurement, team);
+ }
+};
+
+TEST_F(FilteredRobotTest, WaitUntilOldEnough)
+{
+ ateam_msgs::msg::VisionStateRobot default_msg{};
+ int camera = 0;
+ for (int i = 0; i < oldEnoughAge; ++i) {
+ // Set it up so we can print our messages more effectively
+ // https://google.github.io/googletest/advanced.html#teaching-googletest-how-to-print-your-values
+ ssl_league_msgs::msg::VisionDetectionRobot fake_vision_data{};
+ fake_vision_data.pose.position.x = 1;
+ fake_vision_data.pose.position.y = 1;
+ if (i < oldEnoughAge) {
+ auto fake_measurement = ateam_super_vision::RobotMeasurement(fake_vision_data, camera, team);
+ bot->update(fake_measurement);
+ auto msg = bot->toMsg();
+ // We shouldn't update if our filter is too new
+ EXPECT_EQ(default_msg, msg);
+ } else {
+ // Sorry, adding a short sleep was easier than mocking the timestamp...
+ std::this_thread::sleep_for(std::chrono::milliseconds(60));
+ auto fake_measurement = ateam_super_vision::RobotMeasurement(fake_vision_data, camera, team);
+ bot->update(fake_measurement);
+ auto msg = bot->toMsg();
+ // We should update if our filter is old enough
+ EXPECT_NE(default_msg, msg);
+ }
+ }
+}