New super vision filter by fourpenny · Pull Request #379 · SSL-A-Team/software

fourpenny · 2026-03-04T03:37:36Z

A simpler vision filter that just uses basic Kalman filtering and that's about it. It keeps the same interface as our previous filter but has a slimmed down design.

You can test it:
colcon test --packages-select new_vision_filter

Or run the sim (or real) launch file:
ros2 launch ateam_bringup bringup_simulation.launch.py

…ucture

…on_filter/!

barulicm

Some interesting stuff in here. Excited to see a fresh start to this.
We should add more tests to fully exercise as much of the code and math as possible.
We may want to have a deeper conversation about architecture and object management in this at some point. There might be some simplifications we can make, especially on the robot tracking side where we know exactly how many robots we could be tracking.

barulicm · 2026-03-15T18:01:20Z

new_super_vision_filter/package.xml

+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>new_vision_filter</name>


Package name should match the folder name

Do you have a preference for which we should pick? I'll change them all to match.

I made it "new_super_vision_filter" for now, since that's more fun, but if the length is a problem I can change it to "new_vision_filter".

new_super_vision_filter/CMakeLists.txt

barulicm · 2026-03-15T18:15:37Z

new_super_vision_filter/src/measurements/ball_measurement.hpp

+    PosMeasurement pos;
+    pos << ball_detection.pos.x,
+      ball_detection.pos.y;
+    timestamp = std::chrono::steady_clock::now();


Should we think about using one of the timestamps from the vision detection frame message? Not sure if you've already considered this, and not sure if the time difference would actually matter in practice.

Hm, I think it depends on whether we think degradation in quality/lag might be more likely from our vision filter internally or from SSL vision. Do you think it would be worth checking both? I think that switching to the SSL time stamps or adding them as an additional quality check would be straightforward.

barulicm · 2026-03-15T18:16:46Z

new_super_vision_filter/src/measurements/robot_measurement.hpp

+      bot_detection.pose.position.y;
+    angle << bot_detection.pose.orientation.w;
+    robot_id = bot_detection.robot_id;
+  }


timestamp appears to not be initialized or set anywhere. Am I missing something?

This should be initialized now in the constructor for the RobotMeasurement!

barulicm · 2026-03-15T18:20:40Z

new_super_vision_filter/src/measurements/ball_track.hpp

+
+#include <ssl_league_msgs/msg/vision_detection_ball.hpp>
+
+class BallTrack {


Consider calling these types 'measurement' or 'detection'. At least in my mind, 'track' usually represents the internal filter state used to store detection history or correlate new detections to existing estimate instances.

Suggested change

class BallTrack {

class BallDetection {

Changed both this and the RobotTrack class to use the suffix "Measurement" instead.

barulicm · 2026-03-15T18:23:44Z

new_super_vision_filter/src/filtered_ball.cpp

+    ++age;
+  }
+  if (health < maxHealth) {
+    health += 2;


Does health just track the staleness of the ball detections? Seems like it might be useful to include some measure of filter confidence in that metric as well.

For now it just checks the staleness. We could make this number a combination of multiple checks, or another idea I have is to rename this something related to its function ("staleness", maybe) and then add other explicit defined checks related to the filter quality. My thought is that's a good option, since we can continue adding more as we go for now, until we have a better idea of what the best quality checks are.

barulicm · 2026-03-15T18:29:20Z

new_super_vision_filter/src/vision_filter_node.cpp

+                        blue_robots.end(),
+          [bot_track](const FilteredRobot & bot){return bot_track.getId() == bot.getId();}
+        );
+        if (it != blue_robots.end()) {


Is this condition inverted? It looks like you're trying to create FilteredRobot instances on demand as new IDs pop up. iterator != end means that an existing element was found.

This should also segfault as written when it takes the else branch as it does not point to a valid object.

Whoops... this is fixed. I'm impressed (I think?) it never segfaulted in testing...

fourpenny · 2026-03-16T02:16:50Z

Some interesting stuff in here. Excited to see a fresh start to this. We should add more tests to fully exercise as much of the code and math as possible. We may want to have a deeper conversation about architecture and object management in this at some point. There might be some simplifications we can make, especially on the robot tracking side where we know exactly how many robots we could be tracking.

I think at least as an initial test before the PTO this weekend, I'll plan to see what generally happens to the filter state in a few conditions we're likely to see:

If we have multiple balls and/or a very noisy ball
If we have robots dropping in and out of visibility
If we have two of the same robot on the field (like if we practice on a field with another team with Yellow Robot 1)... I think this is more of a stretch goal/we can rush to address it if needed.

fourpenny added 30 commits September 2, 2025 22:12

Added kalman library stuff and start to outline new vision filter str…

8654751

…ucture

Add some basic things to vision filter node

d075464

Added filtered ball and robot classes

45bb904

Make a camera when there isn't one yet

6ea51ba

Start adding ball and robot processing, need to configure KF

09b924d

Start doing some kalman filter stuff for the robot tracker

a1185bf

Small changes to filtered robot

452e65b

Add filter types and initialize state/covariance of robot kf

31cf6a2

Merge branch 'main' into dev/christian/visionV2

7b554a8

Update cmake and packaging, fix vision filter types

d96edad

Add details to update step of filtered robot

342bc7d

Update to filtered robot and create system models

e299604

Add filtered robot test outline, update filtered robot

6456255

Building but so many compiler errors

0991880

Fixed many compiler errors

b3696b0

Fixing more errors

574f120

Fix camera map creation

cc58f2d

Now building!

4612d1e

Remove unused extra queue

316c351

Change to using tracks on ros node rather than per camera

f1125a8

Minor updates and notes on next TODOs

44cf585

Add init function and update to filtered ball

1197465

Add publishers, subscribers, other ROS stuff to new filter node

4aa1afe

Ready to work on compilation then test

dd2331d

It compilesgit add new_super_vision_filter/ YAYgit add new_super_visi…

ae8378a

…on_filter/!

uncrustify

491b22c

WIP test

2103c9e

Tests compile now

f00936b

Fix test iterator, uncrustify kalman package

0cbd70f

Bring up file for new vision with sim

11c3b7c

fourpenny added 2 commits March 3, 2026 22:33

Move kalman to git submodule

d1840c5

Add new vision to autonomy stack

7cf7909

fourpenny requested review from barulicm and chachmu March 4, 2026 03:37

fourpenny self-assigned this Mar 4, 2026

fourpenny added 2 commits March 3, 2026 22:40

Remove old comment

fea56b7

Update package description

3b32cdd

barulicm reviewed Mar 15, 2026

View reviewed changes

fourpenny added 2 commits March 15, 2026 22:03

Change track to measurement, reenable linting

4f2831c

Ensure package name is fixed in bringup, fix iterator bug

4eecb12


		#include <ssl_league_msgs/msg/vision_detection_ball.hpp>

		class BallTrack {

Conversation

fourpenny commented Mar 4, 2026

Uh oh!

barulicm left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

fourpenny Mar 16, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

fourpenny commented Mar 16, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

fourpenny Mar 16, 2026 •

edited

Loading