diff --git a/.gitmodules b/.gitmodules index 08ea60349..015d9ec6a 100644 --- a/.gitmodules +++ b/.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 diff --git a/ateam_bringup/launch/autonomy.launch.xml b/ateam_bringup/launch/autonomy.launch.xml index 6f0a5f00f..6423da7e6 100644 --- a/ateam_bringup/launch/autonomy.launch.xml +++ b/ateam_bringup/launch/autonomy.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt new file mode 100644 index 000000000..661accf51 --- /dev/null +++ b/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 + $ + $ + 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() diff --git a/new_super_vision_filter/kalman b/new_super_vision_filter/kalman new file mode 160000 index 000000000..9f40c2f7b --- /dev/null +++ b/new_super_vision_filter/kalman @@ -0,0 +1 @@ +Subproject commit 9f40c2f7bedb08e964297a00a2b9360036590896 diff --git a/new_super_vision_filter/package.xml b/new_super_vision_filter/package.xml new file mode 100644 index 000000000..51d14b445 --- /dev/null +++ b/new_super_vision_filter/package.xml @@ -0,0 +1,24 @@ + + + + new_super_vision_filter + 1.0.0 + wahoo! we can filter vision data! + christian + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_components + eigen + ateam_msgs + ateam_common + + 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 000000000..a2e0c5f86 --- /dev/null +++ b/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 + +#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() {} diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp new file mode 100644 index 000000000..bc2bf3708 --- /dev/null +++ b/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 +#include +#include +#include +#include +#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 last_updated; +}; + +#endif // CAMERA_HPP_ 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 000000000..7d58a40b4 --- /dev/null +++ b/new_super_vision_filter/src/filter_types.hpp @@ -0,0 +1,260 @@ +// 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 "kalman/Types.hpp" +#include "kalman/LinearizedMeasurementModel.hpp" +#include "kalman/LinearizedSystemModel.hpp" + +/* + Position measurement for robot or balls + + Structure is { + x_pos + y_pos + } + + Measurements are in m +*/ +class PosMeasurement : public Kalman::Vector +{ +public: + KALMAN_VECTOR(PosMeasurement, double, 2) + + 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 Kalman::Vector +{ +public: + KALMAN_VECTOR(PosState, double, 4) + + static constexpr size_t PX = 0; + static constexpr size_t PY = 1; + static constexpr size_t VX = 2; + static constexpr size_t VY = 3; + + const double px() {return (*this)[PX];} + const double py() {return (*this)[PY];} + const double vx() {return (*this)[VX];} + const double vy() {return (*this)[VY];} + +}; + +/* + Measurement model for robots/balls' X and Y pos + velocity from + position measurements + + Curently assumes no measurement noise +*/ +class PosMeasurementModel + : public Kalman::LinearizedMeasurementModel +{ +public: + // h(x) = predicted measurement + PosMeasurement h(const PosState & x) const + { + PosMeasurement z; + z[0] = x(x.PX); // px + z[1] = x(x.PY); // py + return z; + } + + // Jacobian H = ∂h/∂x + +protected: + void updateJacobians(const PosState & x) + { + this->H.setZero(); + + this->H(0, 0) = 1; // dz_px / d_px + this->H(1, 1) = 1; // dz_py / d_py + } +}; + +/* + System (measurement to state) transition model for X/Y position + that assumes a constant velocity and no control inputs. + +*/ +class PosSystemModel : public Kalman::LinearizedSystemModel +{ +private: + mutable std::chrono::time_point last_update; + +public: + // For now, we don't add any control inputs to the vision system + // TODO (Christian): We might need this for the robots, even if we + // don't use it for the ball + using Control = Kalman::Vector; + using Seconds = std::chrono::duration; + + /* + The f() function applies what would be the A (state transition) + matrix but lazily skips doing a matmul. + + 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} + */ + PosState f(const PosState & x, const Control & /*u*/) const override + { + PosState x_updated{}; + auto now = std::chrono::steady_clock::now(); + Seconds dt = now - last_update; + double dt_s = dt.count(); + + // B/c dt is in ms, we need to convert to s, since + // our velocities are all in m/s + x_updated(x.PX) = x(x.PX) + x(x.VX) * dt_s; + // We assume constant velocity in the system model + x_updated(x.VX) = x(x.VX); + x_updated(x.PY) = x(x.PY) + x(x.VY) * dt_s; + x_updated(x.VY) = x(x.VY); + + last_update = now; + return x_updated; + } + +}; + +/* + Angular position measurement (in rad) +*/ +class AngleMeasurement : public Kalman::Vector +{ +public: + KALMAN_VECTOR(AngleMeasurement, double, 1) + + 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 Kalman::Vector +{ +public: + KALMAN_VECTOR(AngleState, double, 2) + + static constexpr size_t PW = 0; + static constexpr size_t VW = 1; + + const double pw() {return (*this)[PW];} + const double vw() {return (*this)[VW];} +}; + +/* + Linearized angle measurement model + + Currently assumes no measurement noise +*/ +class AngleMeasurementModel + : public Kalman::LinearizedMeasurementModel +{ +public: + // h(x) = predicted measurement + AngleMeasurement h(const AngleState & x) const override + { + AngleMeasurement z{}; + z[0] = x[0]; // pw + return z; + } + + // Jacobian H = ∂h/∂x + void updateJacobians(const AngleState & x) override + { + this->H.setZero(); + this->H(0, 0) = 1; // dz_px / d_px + } +}; + +class AngleSystemModel : public Kalman::LinearizedSystemModel +{ +private: + mutable std::chrono::time_point last_update; + +public: + using Control = Kalman::Vector; + using Seconds = std::chrono::duration; + /* + This is the system's state transition function (applies + the A matrix) + + 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} + */ + AngleState f(const AngleState & x, const Control &) const + { + AngleState x_updated{}; + + const auto now = std::chrono::steady_clock::now(); + Seconds dt = now - last_update; + double dt_s = dt.count(); + + x_updated(x.PW) = x(x.PW) + x(x.VW) * dt_s; + + return x_updated; + } +}; + +#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 000000000..1c60efa1f --- /dev/null +++ b/new_super_vision_filter/src/filtered_ball.cpp @@ -0,0 +1,73 @@ +#include "filtered_ball.hpp" + +FilteredBall::FilteredBall(const BallMeasurement & measurement) +{ + PosState initial_state_xy; + initial_state_xy << + measurement.pos.x(), + measurement.pos.y(), + 0, + 0; + posFilterXY.init(initial_state_xy); + Kalman::Matrix xy_covariance; + // 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. + xy_covariance << 1e-2, 0, 0, 0, + 0, 1e-2, 0, 0, + 0, 0, 1e3, 0, + 0, 0, 0, 1e3; + posFilterXY.setCovariance(xy_covariance); +} + +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; + } + // Predict state forward + // Predict covariance forward + // (All encompassed by the .predict() function) + auto xy_pred = posFilterXY.predict(systemModelXY); + // Update the Jacobian matrix (contained in filter) + // Compute Kalman gain (contained in filter) + // Update state estimate (returned) + // Update covariance estimate (contained in filter) + // All encompassed by the .update() function + posXYEstimate = posFilterXY.update(measurementModelXY, measurement.pos); +} + +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; +} 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 000000000..11d03120d --- /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 "kalman/ExtendedKalmanFilter.hpp" +#include "filter_types.hpp" +#include "measurements/ball_measurement.hpp" + +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 + Kalman::ExtendedKalmanFilter posFilterXY; + PosState posXYEstimate; + PosSystemModel systemModelXY; + PosMeasurementModel measurementModelXY; +}; + +#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 000000000..294bc71aa --- /dev/null +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -0,0 +1,147 @@ +// 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 + +// 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 + +FilteredRobot::FilteredRobot(const RobotMeasurement & measurement, ateam_common::TeamColor & team_color) +: posFilterXY(), posFilterW(), bot_id(measurement.getId()), team(team_color) +{ + // Initialize XY KF + PosState initial_state_xy; + initial_state_xy << + measurement.pos.x(), + measurement.pos.y(), + 0, + 0; + posFilterXY.init(initial_state_xy); + Kalman::Matrix xy_covariance; + // 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. + xy_covariance << 1e-2, 0, 0, 0, + 0, 1e-2, 0, 0, + 0, 0, 1e3, 0, + 0, 0, 0, 1e3; + posFilterXY.setCovariance(xy_covariance); + + // Initialize angular KF + /* + State vector is simply + w_pos, + w_vel + */ + AngleState initial_state_w; + initial_state_w << + measurement.angle.w(), + 0; + posFilterW.init(initial_state_w); + /* + Initial covariance is approx 2 deg. for pos, + 10 deg. for vel + */ + Kalman::Matrix w_covariance; + w_covariance << std::pow(2 * M_PI / 180.0, 2), 0, + 0, std::pow(10 * M_PI / 180.0, 2); + posFilterW.setCovariance(w_covariance); +} + +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 || is_new) { + return; + } + // Predict state forward + // Predict covariance forward + // (All encompassed by the .predict() function) + auto xy_pred = posFilterXY.predict(systemModelXY); + auto w_pred = posFilterW.predict(systemModelW); + // Update the Jacobian matrix (contained in filter) + // Compute Kalman gain (contained in filter) + // Update state estimate (returned) + // Update covariance estimate (contained in filter) + // All encompassed by the .update() function + posXYEstimate = posFilterXY.update(measurementModelXY, measurement.pos); + posWEstimate = posFilterW.update(measurementModelW, measurement.angle); +} + +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.x = 0; + robot_state_msg.pose.orientation.y = 0; + robot_state_msg.pose.orientation.z = 1; + robot_state_msg.pose.orientation.w = 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; +} 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 000000000..b1e15bc91 --- /dev/null +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -0,0 +1,71 @@ +// 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 "kalman/ExtendedKalmanFilter.hpp" +#include "filter_types.hpp" +#include "measurements/robot_measurement.hpp" + +#include +#include +#include +#include +#include + +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: + int bot_id; + ateam_common::TeamColor team; + int age = 0; + int oldEnough = 3; + int health = 2; + int maxHealth = 20; + double maxDistance = -1.0; + std::chrono::milliseconds update_threshold{50}; + 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 + Kalman::ExtendedKalmanFilter posFilterXY; + PosSystemModel systemModelXY; + PosState posXYEstimate{}; + PosMeasurementModel measurementModelXY; + // Theta + Kalman::ExtendedKalmanFilter posFilterW; + AngleSystemModel systemModelW; + AngleState posWEstimate{}; + AngleMeasurementModel measurementModelW; +}; + +#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 000000000..552a30748 --- /dev/null +++ b/new_super_vision_filter/src/measurements/ball_measurement.hpp @@ -0,0 +1,44 @@ +// 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 + +class BallMeasurement { +public: + BallMeasurement(const ssl_league_msgs::msg::VisionDetectionBall & ball_detection, int & camera_id) + : camera_id(camera_id) + { + PosMeasurement pos; + pos << ball_detection.pos.x, + ball_detection.pos.y; + timestamp = std::chrono::steady_clock::now(); + } + + PosMeasurement pos; + std::chrono::time_point timestamp; + int camera_id; +}; + +#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 000000000..a3b2cc95b --- /dev/null +++ b/new_super_vision_filter/src/measurements/robot_measurement.hpp @@ -0,0 +1,71 @@ +// 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 "filter_types.hpp" + +/** + * @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) + { + PosMeasurement pos; + pos << bot_detection.pose.position.x, + bot_detection.pose.position.y; + angle << bot_detection.pose.orientation.w; + robot_id = bot_detection.robot_id; + timestamp = std::chrono::steady_clock::now(); + } + + int getId() const + { + return robot_id; + } + + 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; + +}; + +#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 000000000..47ecde238 --- /dev/null +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -0,0 +1,264 @@ +// 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 + +using namespace std::chrono_literals; + +namespace new_super_vision +{ + +class VisionFilterNode : public rclcpp::Node +{ + +public: + explicit VisionFilterNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("new_ateam_vision_filter", options), + game_controller_listener_(*this) + { + declare_parameters("offsets", { + {"robots.x", 0.0}, + {"robots.y", 0.0} + }); + ssl_vision_subscription_ = + create_subscription( + std::string(Topics::kVisionMessages), + 10, + std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1) + ); + + 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; + + std::vector ball_measurements; + std::vector blue_measurements; + std::vector yellow_measurements; + + // 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) + { + // 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; + yellow_measurements.push_back( + RobotMeasurement(bot, detect_camera, team_color) + ); + } + + for (const auto & bot : detection.robots_blue) { + ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; + blue_measurements.push_back( + RobotMeasurement(bot, detect_camera, team_color) + ); + } + + // Create a measurement from each ball in this message + for (const auto & ball: detection.balls) { + ball_measurements.push_back( + BallMeasurement(ball, detect_camera) + ); + } + } + + // 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 (std::size_t 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 (std::size_t 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 new_super_vision + +RCLCPP_COMPONENTS_REGISTER_NODE(new_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 000000000..4ee4f9b39 --- /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 000000000..d3b639470 --- /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 = 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 (size_t 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 = 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 = 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); + } + } +}