New super vision filter

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "radio/ateam_radio_msgs/software-communication"]
 	path = radio/ateam_radio_msgs/software-communication
 	url = https://github.com/SSL-A-Team/software-communication.git
+[submodule "new_super_vision_filter/kalman"]
+	path = new_super_vision_filter/kalman
+	url = https://github.com/mherb/kalman.git

ateam_bringup/launch/autonomy.launch.xml

@@ -7,7 +7,7 @@
   <arg name="vision_offset_robot_x" default="0.0"/>
   <arg name="vision_offset_robot_y" default="0.0"/>
 
-  <node name="vision_filter" pkg="ateam_vision_filter" exec="ateam_vision_filter_node" respawn="True">
+  <node name="vision_filter" pkg="new_vision_filter" exec="new_ateam_vision_filter" respawn="True">
     <param name="gc_team_name" value="$(var team_name)"/>
     <param name="offsets.robots.x" value="$(var vision_offset_robot_x)"/>
     <param name="offsets.robots.y" value="$(var vision_offset_robot_y)"/>

new_super_vision_filter/CMakeLists.txt

@@ -0,0 +1,73 @@
+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)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(ateam_msgs REQUIRED)
+find_package(ateam_common REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+  src/vision_filter_node.cpp
+  src/camera.cpp
+  src/filtered_robot.cpp
+  src/filtered_ball.cpp
+)
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/kalman/include>
+    $<INSTALL_INTERFACE:include>
+  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
+  ssl_league_msgs
+  Eigen3
+)
+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()

new_super_vision_filter/package.xml

@@ -0,0 +1,24 @@
+<?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_super_vision_filter</name>
+  <version>1.0.0</version>
+  <description>wahoo! we can filter vision data!</description>
+  <maintainer email="christianclark687@gmail.com">christian</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>eigen</depend>
+  <depend>ateam_msgs</depend>
+  <depend>ateam_common</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

new_super_vision_filter/src/camera.cpp

@@ -0,0 +1,39 @@
+// 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 <algorithm>
+
+#include "camera.hpp"
+#include "filtered_ball.hpp"
+#include "filtered_robot.hpp"
+
+Camera::Camera(int camera_id)
+: camera_id(camera_id) {}
+
+void Camera::process_detection_frame(
+  const ssl_league_msgs::msg::VisionDetectionFrame & detection_frame_msg)
+{
+    // process_balls(detection_frame_msg);
+    // process_robots(detection_frame_msg);
+}
+
+void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData & geometry) {}
+
+void Camera::clear_old_messages() {}

new_super_vision_filter/src/camera.hpp

@@ -0,0 +1,51 @@
+// 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 <vector>
+#include <ssl_league_msgs/msg/vision_geometry_data.hpp>
+#include <ssl_league_msgs/msg/vision_detection_ball.hpp>
+#include <ssl_league_msgs/msg/vision_detection_robot.hpp>
+#include <ssl_league_msgs/msg/vision_detection_frame.hpp>
+#include "filtered_ball.hpp"
+#include "filtered_robot.hpp"
+
+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<std::chrono::steady_clock> last_updated;
+};
+
+#endif // CAMERA_HPP_