diff --git a/consai_game/consai_game/world_model/referee_model.py b/consai_game/consai_game/world_model/referee_model.py index 9b61668c..aa851567 100644 --- a/consai_game/consai_game/world_model/referee_model.py +++ b/consai_game/consai_game/world_model/referee_model.py @@ -17,7 +17,7 @@ """Refereeメッセージを解析し, ゲームの状態を抽象化したモデルに変換するモジュール.""" -from dataclasses import dataclass +from dataclasses import dataclass, field from robocup_ssl_msgs.msg import Referee from consai_game.utils.geometry import Point @@ -49,9 +49,9 @@ class RefereeModel: our_ball_placement: bool = False their_ball_placement: bool = False running: bool = False - placement_pos: Point = Point(0.0, 0.0) + placement_pos: Point = field(default_factory=lambda: Point(0.0, 0.0)) - stop_game_ball_pos: State2D = State2D(x=0.0, y=0.0) + stop_game_ball_pos: State2D = field(default_factory=lambda: State2D(x=0.0, y=0.0)) def is_position_on_placement_area(self, pos: State2D) -> bool: """指定座標がプレースメントエリアにあるかを判定する関数.""" diff --git a/consai_game/consai_game/world_model/world_model.py b/consai_game/consai_game/world_model/world_model.py index d98519f7..289be1be 100644 --- a/consai_game/consai_game/world_model/world_model.py +++ b/consai_game/consai_game/world_model/world_model.py @@ -18,6 +18,7 @@ """試合状況を統合的に表現するWorldModelの定義モジュール.""" from dataclasses import dataclass +from dataclasses import field as dataclass_field from consai_game.world_model.ball_activity_model import BallActivityModel from consai_game.world_model.ball_model import BallModel @@ -36,15 +37,22 @@ class WorldModel: """試合全体の状態を統合的に保持するデータクラス.""" - referee: RefereeModel = RefereeModel() - robots: RobotsModel = RobotsModel() - ball: BallModel = BallModel() - field: Field = Field() - field_points: FieldPoints = FieldPoints.create_field_points(field) - ball_position: BallPositionModel = BallPositionModel(field, field_points) - robot_activity: RobotActivityModel = RobotActivityModel() - ball_activity: BallActivityModel = BallActivityModel() - kick_target: KickTargetModel = KickTargetModel() - game_config: GameConfigModel = GameConfigModel() - threats: ThreatsModel = ThreatsModel(field, field_points) - meta: WorldMetaModel = WorldMetaModel() + referee: RefereeModel = dataclass_field(default_factory=RefereeModel) + robots: RobotsModel = dataclass_field(default_factory=RobotsModel) + ball: BallModel = dataclass_field(default_factory=BallModel) + field: Field = dataclass_field(default_factory=Field) + robot_activity: RobotActivityModel = dataclass_field(default_factory=RobotActivityModel) + ball_activity: BallActivityModel = dataclass_field(default_factory=BallActivityModel) + kick_target: KickTargetModel = dataclass_field(default_factory=KickTargetModel) + game_config: GameConfigModel = dataclass_field(default_factory=GameConfigModel) + meta: WorldMetaModel = dataclass_field(default_factory=WorldMetaModel) + + field_points: FieldPoints = dataclass_field(init=False) + ball_position: BallPositionModel = dataclass_field(init=False) + threats: ThreatsModel = dataclass_field(init=False) + + def __post_init__(self): + """クラスメンバを使って初期化を行う""" + self.field_points = FieldPoints.create_field_points(self.field) + self.ball_position = BallPositionModel(self.field, self.field_points) + self.threats = ThreatsModel(self.field, self.field_points) diff --git a/consai_robot_controller/CMakeLists.txt b/consai_robot_controller/CMakeLists.txt index f03b4d75..0dd292aa 100644 --- a/consai_robot_controller/CMakeLists.txt +++ b/consai_robot_controller/CMakeLists.txt @@ -15,6 +15,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -march=native -mtune=native") + # find dependencies find_package(ament_cmake REQUIRED) find_package(consai_frootspi_msgs REQUIRED) diff --git a/consai_robot_controller/include/consai_robot_controller/tactic/ball_boy_tactics.hpp b/consai_robot_controller/include/consai_robot_controller/tactic/ball_boy_tactics.hpp index e9ba424f..416ac4b6 100644 --- a/consai_robot_controller/include/consai_robot_controller/tactic/ball_boy_tactics.hpp +++ b/consai_robot_controller/include/consai_robot_controller/tactic/ball_boy_tactics.hpp @@ -16,6 +16,7 @@ #define CONSAI_ROBOT_CONTROLLER__TACTIC__BALL_BOY_TACTICS_HPP_ #include +#include #include #include "consai_msgs/msg/state2_d.hpp" diff --git a/consai_vision_tracker/CMakeLists.txt b/consai_vision_tracker/CMakeLists.txt index 672dd625..a406c6c8 100644 --- a/consai_vision_tracker/CMakeLists.txt +++ b/consai_vision_tracker/CMakeLists.txt @@ -15,15 +15,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -march=native -mtune=native") + # find dependencies find_package(ament_cmake REQUIRED) find_package(consai_msgs REQUIRED) find_package(consai_tools REQUIRED) find_package(consai_visualizer_msgs REQUIRED) +find_package(Eigen3 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(robocup_ssl_msgs REQUIRED) -pkg_check_modules(BFL REQUIRED orocos-bfl) include(FetchContent) fetchcontent_declare(json URL https://github.com/nlohmann/json/releases/download/v3.11.3/json.tar.xz) @@ -31,14 +33,13 @@ fetchcontent_makeavailable(json) include_directories( include - ${BFL_INCLUDE_DIRS} ) # Tracker Component add_library(tracker_component SHARED src/tracker_component.cpp - src/ball_tracker.cpp - src/robot_tracker.cpp + src/ball_kalman_filter.cpp + src/robot_kalman_filter.cpp src/visualization_data_handler.cpp ) target_compile_definitions(tracker_component @@ -47,12 +48,17 @@ ament_target_dependencies(tracker_component consai_msgs consai_tools consai_visualizer_msgs + Eigen3 rclcpp rclcpp_components robocup_ssl_msgs ) +target_include_directories(tracker_component PUBLIC + "$" + "$" + "$" +) target_link_libraries(tracker_component - ${BFL_LIBRARIES} nlohmann_json::nlohmann_json ) rclcpp_components_register_nodes(tracker_component "consai_vision_tracker::Tracker") @@ -66,7 +72,10 @@ ament_export_dependencies(rclcpp) ament_export_dependencies(rclcpp_components) ament_export_dependencies(robocup_ssl_msgs) -ament_export_include_directories(include) +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIR} +) ament_export_libraries(tracker_component) # Install diff --git a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp new file mode 100644 index 00000000..268fa9e2 --- /dev/null +++ b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 Roots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONSAI_VISION_TRACKER__BALL_KALMAN_FILTER_HPP_ +#define CONSAI_VISION_TRACKER__BALL_KALMAN_FILTER_HPP_ + +#include +#include + +#include "robocup_ssl_msgs/msg/detection_ball.hpp" +#include "robocup_ssl_msgs/msg/tracked_ball.hpp" + + +namespace consai_vision_tracker +{ + +using DetectionBall = robocup_ssl_msgs::msg::DetectionBall; +using TrackedBall = robocup_ssl_msgs::msg::TrackedBall; +using Vector4d = Eigen::Vector4d; +using Vector2d = Eigen::Vector2d; +using Matrix4d = Eigen::Matrix4d; +using Matrix2d = Eigen::Matrix2d; +using Matrix24d = Eigen::Matrix; +using Matrix42d = Eigen::Matrix; + +class BallKalmanFilter +{ +public: + explicit BallKalmanFilter( + const double dt = 0.01, const double lifetime = 2.0, + const double visibility_increase_rate = 5.0, + const double outlier_time_limit = 0.1); + + void push_back_observation(const DetectionBall & ball); + TrackedBall update(const bool use_uncertain_sys_model); + TrackedBall get_estimation(void) const; + void update_noise_covariance_matrix( + const double q_max_acc, const double q_uncertain_max_acc, const double r_pos_stddev); + +private: + Vector2d make_observation(void) const; + bool is_outlier(const double chi_squared_value) const; + void reset_x_and_p(const Vector2d & observation); + + const double DT_; + const double VISIBILITY_CONTROL_VALUE_; + const double VISIBILITY_INCREASE_RATE_; + const int OUTLIER_COUNT_THRESHOLD_; + + double visibility_ = 1.0; + int outlier_count_ = 0; + + std::vector ball_observations_; + + Vector4d x_; + Matrix4d F_; + Matrix24d H_; + Matrix4d Q_; + Matrix4d Q_uncertain_; + Matrix2d R_; + Matrix4d P_; +}; + +} // namespace consai_vision_tracker + +#endif // CONSAI_VISION_TRACKER__BALL_KALMAN_FILTER_HPP_ diff --git a/consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp b/consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp deleted file mode 100644 index b34ca922..00000000 --- a/consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2021 Roots -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONSAI_VISION_TRACKER__BALL_TRACKER_HPP_ -#define CONSAI_VISION_TRACKER__BALL_TRACKER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "robocup_ssl_msgs/msg/detection_ball.hpp" -#include "robocup_ssl_msgs/msg/tracked_ball.hpp" - -namespace consai_vision_tracker -{ - -using DetectionBall = robocup_ssl_msgs::msg::DetectionBall; -using TrackedBall = robocup_ssl_msgs::msg::TrackedBall; -using ConditionalGaussian = BFL::LinearAnalyticConditionalGaussian; -using SystemModelGaussianUncertainty = BFL::LinearAnalyticSystemModelGaussianUncertainty; -using MeasurementModelGaussianUncertainty = BFL::LinearAnalyticMeasurementModelGaussianUncertainty; -using Gaussian = BFL::Gaussian; -using ExtendedKalmanFilter = BFL::ExtendedKalmanFilter; - -class BallTracker -{ -public: - explicit BallTracker(const double dt = 0.01); - - void push_back_observation(const DetectionBall & ball); - TrackedBall update(const bool use_uncertain_sys_model); - TrackedBall prev_estimation(void) const {return prev_tracked_ball_;} - -private: - void reset_prior(); - bool is_outlier(const TrackedBall & observation) const; - - std::vector ball_observations_; - TrackedBall prev_tracked_ball_; - - std::shared_ptr sys_uncertain_pdf_; - std::shared_ptr sys_uncertain_model_; - std::shared_ptr sys_certain_pdf_; - std::shared_ptr sys_certain_model_; - std::shared_ptr meas_pdf_; - std::shared_ptr meas_model_; - std::shared_ptr prior_; - std::shared_ptr filter_; - - int outlier_count_ = 0; -}; - -} // namespace consai_vision_tracker - -#endif // CONSAI_VISION_TRACKER__BALL_TRACKER_HPP_ diff --git a/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp new file mode 100644 index 00000000..a25ba6c2 --- /dev/null +++ b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp @@ -0,0 +1,85 @@ +// Copyright 2024 Roots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONSAI_VISION_TRACKER__ROBOT_KALMAN_FILTER_HPP_ +#define CONSAI_VISION_TRACKER__ROBOT_KALMAN_FILTER_HPP_ + +#include +#include + +#include "consai_msgs/msg/robot_local_velocity.hpp" +#include "robocup_ssl_msgs/msg/detection_robot.hpp" +#include "robocup_ssl_msgs/msg/robot_id.hpp" +#include "robocup_ssl_msgs/msg/tracked_robot.hpp" + + +namespace consai_vision_tracker +{ + +using DetectionRobot = robocup_ssl_msgs::msg::DetectionRobot; +using RobotId = robocup_ssl_msgs::msg::RobotId; +using TrackedRobot = robocup_ssl_msgs::msg::TrackedRobot; +using RobotLocalVelocity = consai_msgs::msg::RobotLocalVelocity; +using Vector6d = Eigen::Matrix; +using Vector3d = Eigen::Vector3d; +using Matrix3d = Eigen::Matrix3d; +using Matrix6d = Eigen::Matrix; +using Matrix36d = Eigen::Matrix; + +class RobotKalmanFilter +{ +public: + explicit RobotKalmanFilter( + const int team_color, const int id, + const double dt = 0.01, const double lifetime = 2.0, + const double visibility_increase_rate = 5.0, + const double outlier_time_limit = 0.1); + + void push_back_observation(const DetectionRobot & robot); + TrackedRobot update(void); + TrackedRobot get_estimation(void) const; + void update_noise_covariance_matrix( + const double q_max_acc_xy, const double q_max_acc_theta, + const double r_pos_stddev_xy, const double r_pos_stddev_theta); + RobotLocalVelocity calc_local_velocity(void) const; + +private: + Vector3d make_observation(void) const; + bool is_outlier(const double chi_squared_value) const; + void reset_x_and_p(const Vector3d & observation); + double normalize_angle(const double angle) const; + + const double DT_; + const double VISIBILITY_CONTROL_VALUE_; + const double VISIBILITY_INCREASE_RATE_; + const int OUTLIER_COUNT_THRESHOLD_; + const double OMEGA_ZERO_ = 1.0e-6; + + double visibility_ = 1.0; + int outlier_count_ = 0; + + RobotId robot_id_; + std::vector robot_observations_; + + Vector6d x_; + Matrix6d F_; + Matrix36d H_; + Matrix6d Q_; + Matrix3d R_; + Matrix6d P_; +}; + +} // namespace consai_vision_tracker + +#endif // CONSAI_VISION_TRACKER__ROBOT_KALMAN_FILTER_HPP_ diff --git a/consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp b/consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp deleted file mode 100644 index 30f75f2d..00000000 --- a/consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2021 Roots -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONSAI_VISION_TRACKER__ROBOT_TRACKER_HPP_ -#define CONSAI_VISION_TRACKER__ROBOT_TRACKER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "consai_msgs/msg/robot_local_velocity.hpp" -#include "robocup_ssl_msgs/msg/detection_robot.hpp" -#include "robocup_ssl_msgs/msg/tracked_robot.hpp" - -namespace consai_vision_tracker -{ - -using RobotLocalVelocity = consai_msgs::msg::RobotLocalVelocity; -using DetectionRobot = robocup_ssl_msgs::msg::DetectionRobot; -using TrackedRobot = robocup_ssl_msgs::msg::TrackedRobot; -using ConditionalGaussian = BFL::LinearAnalyticConditionalGaussian; -using SystemModelGaussianUncertainty = BFL::LinearAnalyticSystemModelGaussianUncertainty; -using MeasurementModelGaussianUncertainty = BFL::LinearAnalyticMeasurementModelGaussianUncertainty; -using Gaussian = BFL::Gaussian; -using ExtendedKalmanFilter = BFL::ExtendedKalmanFilter; - -class RobotTracker -{ -public: - RobotTracker(const int team_color, const int id, const double dt = 0.01); - - void push_back_observation(const DetectionRobot & robot); - TrackedRobot update(); - RobotLocalVelocity calc_local_velocity(); - TrackedRobot prev_estimation(void) const {return prev_tracked_robot_;} - -private: - void reset_prior(); - bool is_outlier(const TrackedRobot & observation) const; - void correct_orientation_overflow_of_prior(); - double normalize_orientation(double orientation) const; - double normalize_orientation(const double from, const double to) const; - - std::vector robot_observations_; - TrackedRobot prev_tracked_robot_; - - std::shared_ptr sys_pdf_; - std::shared_ptr sys_model_; - std::shared_ptr meas_pdf_; - std::shared_ptr meas_model_; - std::shared_ptr prior_; - std::shared_ptr filter_; - - int outlier_count_ = 0; -}; - -} // namespace consai_vision_tracker - -#endif // CONSAI_VISION_TRACKER__ROBOT_TRACKER_HPP_ diff --git a/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp b/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp index b73c4aef..02cdca6c 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp @@ -21,8 +21,8 @@ #include "consai_msgs/msg/robot_local_velocities.hpp" #include "consai_msgs/msg/robot_local_velocity.hpp" #include "consai_vision_tracker/visibility_control.h" -#include "consai_vision_tracker/ball_tracker.hpp" -#include "consai_vision_tracker/robot_tracker.hpp" +#include "consai_vision_tracker/ball_kalman_filter.hpp" +#include "consai_vision_tracker/robot_kalman_filter.hpp" #include "consai_vision_tracker/visualization_data_handler.hpp" #include "robocup_ssl_msgs/msg/detection_ball.hpp" #include "robocup_ssl_msgs/msg/detection_frame.hpp" @@ -54,7 +54,6 @@ class Tracker : public rclcpp::Node void on_timer(); private: - void gen_robot_trackers(const unsigned int num); void callback_detection(const DetectionFrame::SharedPtr msg); void callback_detection_invert(const DetectionFrame::SharedPtr msg); void callback_geometry(const GeometryData::SharedPtr msg); @@ -69,9 +68,9 @@ class Tracker : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_consai_param_rule_; rclcpp::Publisher::SharedPtr pub_tracked_; rclcpp::Publisher::SharedPtr pub_robot_velocities_; - std::shared_ptr ball_tracker_; - std::vector> blue_robot_tracker_; - std::vector> yellow_robot_tracker_; + std::unique_ptr ball_kalman_filter_; + std::vector> blue_robot_kalman_filter_; + std::vector> yellow_robot_kalman_filter_; std::shared_ptr vis_data_handler_; }; diff --git a/consai_vision_tracker/package.xml b/consai_vision_tracker/package.xml index cf6f8df5..7dfb31f0 100644 --- a/consai_vision_tracker/package.xml +++ b/consai_vision_tracker/package.xml @@ -13,10 +13,10 @@ consai_msgs consai_tools consai_visualizer_msgs + eigen rclcpp rclcpp_components robocup_ssl_msgs - liborocos-bfl-dev ament_lint_auto ament_lint_common diff --git a/consai_vision_tracker/src/ball_kalman_filter.cpp b/consai_vision_tracker/src/ball_kalman_filter.cpp new file mode 100644 index 00000000..bb4e6621 --- /dev/null +++ b/consai_vision_tracker/src/ball_kalman_filter.cpp @@ -0,0 +1,211 @@ +// Copyright 2024 Roots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "consai_vision_tracker/ball_kalman_filter.hpp" +#include "robocup_ssl_msgs/msg/vector3.hpp" + +#include + +namespace consai_vision_tracker +{ + +using SSLVector3 = robocup_ssl_msgs::msg::Vector3; + +BallKalmanFilter::BallKalmanFilter( + const double dt, const double lifetime, + const double visibility_increase_rate, + const double outlier_time_limit) +:DT_(dt), + VISIBILITY_CONTROL_VALUE_(dt / lifetime), + VISIBILITY_INCREASE_RATE_(visibility_increase_rate), + OUTLIER_COUNT_THRESHOLD_(outlier_time_limit / dt) +{ + + // State transition matrix + F_ = Matrix4d::Identity(); + F_(0, 2) = DT_; + F_(1, 3) = DT_; + + // Observation matrix + H_ = Matrix24d::Zero(); + H_(0, 0) = 1.0; + H_(1, 1) = 1.0; + + // Process noise covariance matrix + // Observation noise covariance matrix + update_noise_covariance_matrix(0.1, 0.5, 0.03); + + // State vector + // x_ = (x, y, vx, vy) + // Error covariance matrix + reset_x_and_p(Vector2d::Zero()); +} + +void BallKalmanFilter::push_back_observation(const DetectionBall & ball) +{ + TrackedBall observation; + observation.pos.x = ball.x * 0.001; // mm to meters + observation.pos.y = ball.y * 0.001; // mm to meters + ball_observations_.push_back(observation); +} + +TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) +{ + // Select process noise covariance matrix + Matrix4d Q = Q_; + if (use_uncertain_sys_model) { + Q = Q_uncertain_; + } + + Vector4d x_pred; + Matrix4d P_pred; + + auto prediction = [&](void) { + x_pred = F_ * x_; + P_pred = F_ * P_ * F_.transpose() + Q; + }; + + auto calc_chi_square = [&](const Vector2d & z) { + Matrix2d S = H_ * P_pred * H_.transpose() + R_; + Vector2d innovation = z - H_ * x_pred; + double chi_square = innovation.transpose() * S.inverse() * innovation; + return chi_square; + }; + + // Prediction step + prediction(); + + // Remove outliers + for (auto it = ball_observations_.begin(); it != ball_observations_.end(); ) { + const Vector2d z(it->pos.x, it->pos.y); + const double chi_square = calc_chi_square(z); + + if (!is_outlier(chi_square)) { + outlier_count_ = 0; + it++; + continue; + } + + outlier_count_++; + if (outlier_count_ < OUTLIER_COUNT_THRESHOLD_) { + it = ball_observations_.erase(it); + } else { + reset_x_and_p(z); + prediction(); // Use new state vector + it++; + } + } + + // Make observation + Vector2d z(x_[0], x_[1]); + if (ball_observations_.empty()) { + visibility_ = std::max(0.0, visibility_ - VISIBILITY_CONTROL_VALUE_); + } else { + z = make_observation(); + visibility_ = std::min(1.0, + visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); + ball_observations_.clear(); + } + + // Calculate innovation + Vector2d innovation = z - H_ * x_pred; + // Calculate innovation covariance matrix + Matrix2d S = H_ * P_pred * H_.transpose() + R_; + + // Update step + Matrix42d K = P_pred * H_.transpose() * S.inverse(); + x_ = x_pred + K * innovation; + P_ = (Matrix4d::Identity() - K * H_) * P_pred; + + return get_estimation(); +} + +TrackedBall BallKalmanFilter::get_estimation(void) const +{ + TrackedBall estimation; + estimation.pos.x = x_[0]; + estimation.pos.y = x_[1]; + + SSLVector3 vel; + vel.x = x_[2]; + vel.y = x_[3]; + estimation.vel.push_back(vel); + estimation.visibility.push_back(visibility_); + + return estimation; +} + +void BallKalmanFilter::update_noise_covariance_matrix( + const double q_max_acc, const double q_uncertain_max_acc, const double r_pos_stddev) +{ + // Process noise covariance matrix + auto gen_Q = [this](const double max_acc) { + // Process noise depends on the maximum acceleration + const double sigma_vel = max_acc * DT_; + const double sigma_pos = 0.5 * max_acc * DT_ * DT_; + const double var_vel = sigma_vel * sigma_vel; + const double var_pos = sigma_pos * sigma_pos; + const double var_vel_pos = sigma_vel * sigma_pos; + + Matrix4d Q; + Q << var_pos, 0.0, var_vel_pos, 0.0, + 0.0, var_pos, 0.0, var_vel_pos, + var_vel_pos, 0.0, var_vel, 0.0, + 0.0, var_vel_pos, 0.0, var_vel; + return Q; + }; + Q_ = gen_Q(q_max_acc); // m/s^2 + Q_uncertain_ = gen_Q(q_uncertain_max_acc); // m/s^2 + + // Observation noise covariance matrix + auto gen_R = [&](const double std_dev) { + const double var = std_dev * std_dev; + return Eigen::DiagonalMatrix(var, var); + }; + R_ = gen_R(r_pos_stddev); // meters +} + +Vector2d BallKalmanFilter::make_observation(void) const +{ + if (ball_observations_.empty()) { + return Vector2d(x_[0], x_[1]); + } + + Vector2d mean_observation(0.0, 0.0); + for (const auto & observation : ball_observations_) { + mean_observation[0] += observation.pos.x; + mean_observation[1] += observation.pos.y; + } + mean_observation /= ball_observations_.size(); + return mean_observation; +} + +bool BallKalmanFilter::is_outlier(const double chi_squared_value) const +{ + constexpr double THRESHOLD = 5.991; // 2 degrees of freedom, 0.05 significance level + + if (chi_squared_value > THRESHOLD) { + return true; + } + return false; +} + +void BallKalmanFilter::reset_x_and_p(const Vector2d & observation) +{ + x_ << observation[0], observation[1], 0.0, 0.0; + P_ = Matrix4d::Identity() * 10.0; +} + + +} // namespace consai_vision_tracker diff --git a/consai_vision_tracker/src/ball_tracker.cpp b/consai_vision_tracker/src/ball_tracker.cpp deleted file mode 100644 index 5529e32f..00000000 --- a/consai_vision_tracker/src/ball_tracker.cpp +++ /dev/null @@ -1,271 +0,0 @@ -// Copyright 2021 Roots -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "consai_vision_tracker/ball_tracker.hpp" -#include "robocup_ssl_msgs/msg/vector3.hpp" - -namespace consai_vision_tracker -{ - -using ColumnVector = BFL::ColumnVector; -using Matrix = BFL::Matrix; -using MatrixWrapper = BFL::Matrix_Wrapper; -using SymmetricMatrix = BFL::SymmetricMatrix; -using Vector3 = robocup_ssl_msgs::msg::Vector3; - -// visibilityの操作量 -// 追跡対象の情報が来ないとき、この操作量だけvisibilityを小さくする -// 操作量 = 1.0 / (trackerの更新周波数 * 消失判定までに掛ける時間) -// 例：trackerが100Hzで更新されるとき、操作量が0.002であれば、 -// 追跡対象が消失してから5秒かけてvisibilityが1.0から0.0になる -static const double VISIBILITY_CONTROL_VALUE = 0.005; - -static constexpr auto FIELD_MAX_X = 6.5; -static constexpr auto FIELD_MAX_Y = 5.0; - -BallTracker::BallTracker(const double dt) -{ - // visibilityはoptionalなので、ここでデフォルト値を設定しておく - prev_tracked_ball_.visibility.push_back(1.0); - // velocityはoptionalなので、ここでデフォルト値を設定しておく - prev_tracked_ball_.vel.push_back(Vector3()); - - // システムモデル - // pos(t+1) = pos(t) + vel(t)*dt + undefined_input(t) + noise - // pos = (x, y) - Matrix A(4, 4); - A(1, 1) = 1.0; A(1, 2) = 0.0; A(1, 3) = dt; A(1, 4) = 0.0; - A(2, 1) = 0.0; A(2, 2) = 1.0; A(2, 3) = 0.0; A(2, 4) = dt; - A(3, 1) = 0.0; A(3, 2) = 0.0; A(3, 3) = 1.0; A(3, 4) = 0.0; - A(4, 1) = 0.0; A(4, 2) = 0.0; A(4, 3) = 0.0; A(4, 4) = 1.0; - - // 入力は未実装 - Matrix B(4, 2); - B(1, 1) = 0.0; B(1, 2) = 0.0; - B(2, 1) = 0.0; B(2, 2) = 0.0; - B(3, 1) = 0.0; B(3, 2) = 0.0; - B(4, 1) = 0.0; B(4, 2) = 0.0; - - std::vector AB(2); - AB[0] = A; - AB[1] = B; - - auto gen_uncertainty = [&](const double max_linear_acc_mps) { - const auto max_linear_movement_in_dt = max_linear_acc_mps / 2 * std::pow(dt, 2); // [m] - const auto max_linear_acc_in_dt = max_linear_acc_mps * dt; // [m/s] - // 位置、速度の変化をシステムノイズで表現する - // システムノイズの平均値 - ColumnVector sys_noise_mu(4); - sys_noise_mu = 0.0; - - // システムノイズの分散 - SymmetricMatrix sys_noise_cov(4); - sys_noise_cov = 0.0; - sys_noise_cov(1, 1) = std::pow(max_linear_movement_in_dt, 2); - sys_noise_cov(2, 2) = std::pow(max_linear_movement_in_dt, 2); - sys_noise_cov(3, 3) = std::pow(max_linear_acc_in_dt, 2); - sys_noise_cov(4, 4) = std::pow(max_linear_acc_in_dt, 2); - return Gaussian(sys_noise_mu, sys_noise_cov); - }; - - // システムノイズを大きくして不確かさを増やす - sys_uncertain_pdf_ = std::make_shared(AB, gen_uncertainty(0.5 / dt)); - sys_uncertain_model_ = std::make_shared(sys_uncertain_pdf_.get()); - - // システムノイズを小さくして不確かさを減らす - sys_certain_pdf_ = std::make_shared(AB, gen_uncertainty(0.1 / dt)); - sys_certain_model_ = std::make_shared(sys_certain_pdf_.get()); - - // 観測モデル - // ~pos(t) = pos(t) + noise - // pos = (x, y) - Matrix H(2, 4); - H = 0.0; - H(1, 1) = 1.0; - H(2, 2) = 1.0; - - // 観測ノイズの平均値 - ColumnVector meas_noise_mu(2); - meas_noise_mu = 0.0; - - // 観測ノイズの分散 - SymmetricMatrix meas_noise_cov(2); - meas_noise_cov(1, 1) = std::pow(0.05, 2); - meas_noise_cov(2, 2) = std::pow(0.05, 2); - Gaussian measurement_uncertainty(meas_noise_mu, meas_noise_cov); - - meas_pdf_ = std::make_shared(H, measurement_uncertainty); - meas_model_ = - std::make_shared(meas_pdf_.get()); - - // 事前分布 - ColumnVector prior_mu(4); - prior_mu = 0.0; - - SymmetricMatrix prior_cov(4); - prior_cov = 0.0; - prior_cov(1, 1) = 100.0; - prior_cov(2, 2) = 100.0; - prior_cov(3, 3) = 100.0; - prior_cov(4, 4) = 100.0; - - prior_ = std::make_shared(prior_mu, prior_cov); - - // カルマンフィルタの生成 - filter_ = std::make_shared(prior_.get()); -} - -void BallTracker::push_back_observation(const DetectionBall & ball) -{ - TrackedBall observation; - observation.pos.x = ball.x * 0.001; // mm to meters - observation.pos.y = ball.y * 0.001; // mm to meters - ball_observations_.push_back(observation); -} - -TrackedBall BallTracker::update(const bool use_uncertain_sys_model) -{ - constexpr auto OUTLIER_COUNT_THRESHOLD = 10; - - // 観測値から外れ値を取り除く - for (auto it = ball_observations_.begin(); it != ball_observations_.end(); ) { - if (is_outlier(*it)) { - // 外れ値が連続できたら、観測値をそのまま使用する（誘拐対応） - outlier_count_++; - if (outlier_count_ > OUTLIER_COUNT_THRESHOLD) { - break; - } - it = ball_observations_.erase(it); - } else { - outlier_count_ = 0; - ++it; - } - } - - auto size = ball_observations_.size(); - if (size == 0) { - // 観測値が無い場合の処理 - // visibilityを下げる - prev_tracked_ball_.visibility[0] -= VISIBILITY_CONTROL_VALUE; - if (prev_tracked_ball_.visibility[0] <= 0) { - // visibilityが0になったらカルマンフィルタの演算を実行しない - prev_tracked_ball_.visibility[0] = 0.0; - reset_prior(); - return prev_tracked_ball_; - } - - } else { - // 観測値があればvisibilityをn倍のレートで上げる - prev_tracked_ball_.visibility[0] += VISIBILITY_CONTROL_VALUE * 5.0; - if (prev_tracked_ball_.visibility[0] > 1.0) { - prev_tracked_ball_.visibility[0] = 1.0; - } - - // 観測値が複数ある場合は、その平均値をもとめる - // この処理で観測値を全て削除する - ColumnVector mean_observation(2); - mean_observation(1) = 0.0; - mean_observation(2) = 0.0; - - for (auto it = ball_observations_.begin(); it != ball_observations_.end(); ) { - mean_observation(1) += it->pos.x; - mean_observation(2) += it->pos.y; - it = ball_observations_.erase(it); - } - mean_observation(1) /= size; - mean_observation(2) /= size; - - filter_->Update(meas_model_.get(), mean_observation); - } - - // 事後分布から予測値を取得 - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - // prev_tracked_ball_.pos.x = expected_value(1); - // prev_tracked_ball_.pos.y = expected_value(2); - prev_tracked_ball_.pos.x = std::clamp(expected_value(1), -FIELD_MAX_X, FIELD_MAX_X); - prev_tracked_ball_.pos.y = std::clamp(expected_value(2), -FIELD_MAX_Y, FIELD_MAX_Y); - prev_tracked_ball_.vel[0].x = expected_value(3); - prev_tracked_ball_.vel[0].y = expected_value(4); - - // 次の状態を予測する - // 例えば、ボールの加速度が入力値になる - // 入力値は未実装なので0とする - ColumnVector input(2); - input(1) = 0; - input(2) = 0; - - if (use_uncertain_sys_model) { - filter_->Update(sys_uncertain_model_.get(), input); - } else { - filter_->Update(sys_certain_model_.get(), input); - } - - return prev_tracked_ball_; -} - -void BallTracker::reset_prior() -{ - // 事前分布を初期化する - ColumnVector prior_mu(4); - prior_mu = 0.0; - - SymmetricMatrix prior_cov(4); - prior_cov = 0.0; - prior_cov(1, 1) = 100.0; - prior_cov(2, 2) = 100.0; - prior_cov(3, 3) = 100.0; - prior_cov(4, 4) = 100.0; - - prior_->ExpectedValueSet(prior_mu); - prior_->CovarianceSet(prior_cov); - filter_->Reset(prior_.get()); -} - -bool BallTracker::is_outlier(const TrackedBall & observation) const -{ - // 観測が外れ値かどうか判定する - // Reference: https://myenigma.hatenablog.com/entry/20140825/1408975706 - const double THRESHOLD = 5.99; // 自由度2、棄却率5%のしきい値 - - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - auto covariance = filter_->PostGet()->CovarianceGet(); - - // マハラノビス距離を求める - double diff_x = observation.pos.x - expected_value(1); - double diff_y = observation.pos.y - expected_value(2); - double covariance_x = covariance(1, 1); - double covariance_y = covariance(2, 2); - - // 0 除算を避ける - if (std::fabs(covariance_x) < 1E-15 || std::fabs(covariance_y) < 1E-15) { - return false; - } - - double mahalanobis = std::sqrt( - std::pow(diff_x, 2) / covariance_x + std::pow( - diff_y, - 2) / covariance_y); - if (mahalanobis > THRESHOLD) { - return true; - } - - return false; -} - -} // namespace consai_vision_tracker diff --git a/consai_vision_tracker/src/robot_kalman_filter.cpp b/consai_vision_tracker/src/robot_kalman_filter.cpp new file mode 100644 index 00000000..800d4960 --- /dev/null +++ b/consai_vision_tracker/src/robot_kalman_filter.cpp @@ -0,0 +1,294 @@ +// Copyright 2024 Roots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "consai_vision_tracker/robot_kalman_filter.hpp" +#include "robocup_ssl_msgs/msg/vector2.hpp" + +#include + +namespace consai_vision_tracker +{ + +using Vector2d = Eigen::Vector2d; +using Matrix2d = Eigen::Matrix2d; +using Matrix63d = Eigen::Matrix; +using SSLVector2 = robocup_ssl_msgs::msg::Vector2; + +RobotKalmanFilter::RobotKalmanFilter( + const int team_color, const int id, + const double dt, const double lifetime, + const double visibility_increase_rate, + const double outlier_time_limit) +:DT_(dt), + VISIBILITY_CONTROL_VALUE_(dt / lifetime), + VISIBILITY_INCREASE_RATE_(visibility_increase_rate), + OUTLIER_COUNT_THRESHOLD_(outlier_time_limit / dt) +{ + robot_id_.team_color = team_color; + robot_id_.id = id; + + // State transition matrix + F_ = Matrix6d::Identity(); + F_(0, 3) = DT_; + F_(1, 4) = DT_; + F_(2, 5) = DT_; + + // Observation matrix + H_ = Matrix36d::Zero(); + H_(0, 0) = 1.0; + H_(1, 1) = 1.0; + H_(2, 2) = 1.0; + + // Process noise covariance matrix + // Observation noise covariance matrix + update_noise_covariance_matrix( + 50.0, // xy m/s^2 + 30.0, // theta rad/s^2 + 0.05, // xy m + 0.05 // theta rad + ); + + // State vector + // x_ = (x, y, theta, vx, vy, vtheta) + // Error covariance matrix + reset_x_and_p(Vector3d::Zero()); +} + +void RobotKalmanFilter::push_back_observation(const DetectionRobot & robot) +{ + // Empty orientation data means that the data is not available + if (robot.orientation.size() == 0) { + return; + } + + TrackedRobot observation; + observation.pos.x = robot.x * 0.001; // mm to meters + observation.pos.y = robot.y * 0.001; // mm to meters + observation.orientation = robot.orientation[0]; + robot_observations_.push_back(observation); +} + +TrackedRobot RobotKalmanFilter::update(void) +{ + Vector6d x_pred; + Matrix6d P_pred; + + auto prediction = [&, this](void) { + x_pred = F_ * x_; + x_pred(2) = normalize_angle(x_pred(2)); + P_pred = F_ * P_ * F_.transpose() + Q_; + }; + + auto calc_innovation = [&](const Vector3d & z) { + Vector3d innovation = z - H_ * x_pred; + innovation(2) = normalize_angle(z(2) - (H_ * x_pred)(2)); + return innovation; + }; + + auto calc_chi_square = [&](const Vector3d & z) { + Matrix3d S = H_ * P_pred * H_.transpose() + R_; + Vector3d innovation = calc_innovation(z); + + // Use x and y only + Matrix2d S_xy = S.block<2, 2>(0, 0); + Vector2d innovation_xy = innovation.block<2, 1>(0, 0); + + double chi_square = innovation_xy.transpose() * S_xy.inverse() * innovation_xy; + return chi_square; + }; + + // Prediction step + prediction(); + + // Remove outliers + for (auto it = robot_observations_.begin(); it != robot_observations_.end(); ) { + const Vector3d z(it->pos.x, it->pos.y, it->orientation); + const double chi_square = calc_chi_square(z); + + if (!is_outlier(chi_square)) { + outlier_count_ = 0; + it++; + continue; + } + + outlier_count_++; + if (outlier_count_ < OUTLIER_COUNT_THRESHOLD_) { + it = robot_observations_.erase(it); + } else { + reset_x_and_p(z); + prediction(); // Use new state vector + it++; + } + } + + // Make observation + Vector3d z(x_[0], x_[1], x_[2]); + if (robot_observations_.empty()) { + visibility_ = std::max(0.0, visibility_ - VISIBILITY_CONTROL_VALUE_); + } else { + z = make_observation(); + visibility_ = std::min(1.0, + visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); + robot_observations_.clear(); + } + + // Calculate innovation + Vector3d innovation = calc_innovation(z); + // Calculate innovation covariance matrix + Matrix3d S = H_ * P_pred * H_.transpose() + R_; + + // Update step + Matrix63d K = P_pred * H_.transpose() * S.inverse(); + x_ = x_pred + K * innovation; + x_(2) = normalize_angle(x_(2)); + P_ = (Matrix6d::Identity() - K * H_) * P_pred; + + return get_estimation(); +} + +TrackedRobot RobotKalmanFilter::get_estimation(void) const +{ + TrackedRobot estimation; + estimation.robot_id = robot_id_; + estimation.pos.x = x_(0); + estimation.pos.y = x_(1); + estimation.orientation = x_(2); + + SSLVector2 vel; + vel.x = x_(3); + vel.y = x_(4); + estimation.vel.push_back(vel); + estimation.vel_angular.push_back(x_(5)); + estimation.visibility.push_back(visibility_); + + return estimation; +} + +void RobotKalmanFilter::update_noise_covariance_matrix( + const double q_max_acc_xy, const double q_max_acc_theta, + const double r_pos_stddev_xy, const double r_pos_stddev_theta) +{ + // Process noise covariance matrix + auto gen_Q = [this](const double q_max_acc_xy, const double q_max_acc_theta) { + // Process noise depends on the maximum acceleration + const double sigma_vel_xy = q_max_acc_xy * DT_; + const double sigma_vel_theta = q_max_acc_theta * DT_; + const double sigma_pos_xy = 0.5 * q_max_acc_xy * DT_ * DT_; + const double sigma_pos_theta = 0.5 * q_max_acc_theta * DT_ * DT_; + + const double var_vel_xy = std::pow(sigma_vel_xy, 2); + const double var_vel_theta = std::pow(sigma_vel_theta, 2); + const double var_pos_xy = std::pow(sigma_pos_xy, 2); + const double var_pos_theta = std::pow(sigma_pos_theta, 2); + + const double sigma_vel_pos_xy = sigma_vel_xy * sigma_pos_xy; + const double sigma_vel_pos_theta = sigma_vel_theta * sigma_pos_theta; + + Matrix6d Q = Matrix6d::Zero(); + Q(0, 0) = var_pos_xy; + Q(1, 1) = var_pos_xy; + Q(2, 2) = var_pos_theta; + Q(3, 3) = var_vel_xy; + Q(4, 4) = var_vel_xy; + Q(5, 5) = var_vel_theta; + + Q(0, 3) = sigma_vel_pos_xy; + Q(1, 4) = sigma_vel_pos_xy; + Q(2, 5) = sigma_vel_pos_theta; + Q(3, 0) = sigma_vel_pos_xy; + Q(4, 1) = sigma_vel_pos_xy; + Q(5, 2) = sigma_vel_pos_theta; + return Q; + }; + Q_ = gen_Q(q_max_acc_xy, q_max_acc_theta); + + // Observation noise covariance matrix + auto gen_R = [&](const double std_dev_xy, const double std_dev_theta) { + const double var_xy = std_dev_xy * std_dev_xy; + const double var_theta = std_dev_theta * std_dev_theta; + return Eigen::DiagonalMatrix(var_xy, var_xy, var_theta); + }; + R_ = gen_R(r_pos_stddev_xy, r_pos_stddev_theta); +} + +RobotLocalVelocity RobotKalmanFilter::calc_local_velocity(void) const +{ + auto local_velocity = RobotLocalVelocity(); + local_velocity.robot_id = robot_id_; + + const auto estimation = get_estimation(); + const auto theta = estimation.orientation; + const auto world_vel = estimation.vel[0]; + + auto local_vel = world_vel; + local_vel.x = world_vel.x * std::cos(theta) + world_vel.y * std::sin(theta); + local_vel.y = -(world_vel.x * std::sin(theta) - world_vel.y * std::cos(theta)); + local_velocity.velocity.x = local_vel.x; + local_velocity.velocity.y = local_vel.y; + local_velocity.norm = std::sqrt(std::pow(local_vel.x, 2) + std::pow(local_vel.y, 2)); + local_velocity.velocity.theta = estimation.vel_angular[0]; + + return local_velocity; +} + +Vector3d RobotKalmanFilter::make_observation(void) const +{ + if (robot_observations_.empty()) { + return Vector3d(x_[0], x_[1], x_[2]); + } + + Vector3d mean_observation(0.0, 0.0, 0.0); + double sum_cos = 0.0; + double sum_sin = 0.0; + for (const auto & observation : robot_observations_) { + mean_observation(0) += observation.pos.x; + mean_observation(1) += observation.pos.y; + sum_cos += std::cos(observation.orientation); + sum_sin += std::sin(observation.orientation); + } + mean_observation(0) /= robot_observations_.size(); + mean_observation(1) /= robot_observations_.size(); + sum_cos /= robot_observations_.size(); + sum_sin /= robot_observations_.size(); + mean_observation(2) = std::atan2(sum_sin, sum_cos); + + return mean_observation; +} + +bool RobotKalmanFilter::is_outlier(const double chi_squared_value) const +{ + constexpr double THRESHOLD = 5.991; // 2 degrees of freedom, 0.05 significance level + + if (chi_squared_value > THRESHOLD) { + return true; + } + return false; +} + +void RobotKalmanFilter::reset_x_and_p(const Vector3d & observation) +{ + x_ << observation[0], observation[1], observation[2], 0.0, 0.0, 0.0; + P_ = Matrix6d::Identity() * 10.0; +} + +double RobotKalmanFilter::normalize_angle(const double angle) const +{ + // Normalize angle to -pi ~ pi + double result = angle; + while (result > M_PI) {result -= 2.0 * M_PI;} + while (result < -M_PI) {result += 2.0 * M_PI;} + return result; +} + +} // namespace consai_vision_tracker diff --git a/consai_vision_tracker/src/robot_tracker.cpp b/consai_vision_tracker/src/robot_tracker.cpp deleted file mode 100644 index da076900..00000000 --- a/consai_vision_tracker/src/robot_tracker.cpp +++ /dev/null @@ -1,353 +0,0 @@ -// Copyright 2021 Roots -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "consai_vision_tracker/robot_tracker.hpp" -#include "robocup_ssl_msgs/msg/vector2.hpp" - -namespace consai_vision_tracker -{ - -using ColumnVector = BFL::ColumnVector; -using Matrix = BFL::Matrix; -using MatrixWrapper = BFL::Matrix_Wrapper; -using SymmetricMatrix = BFL::SymmetricMatrix; -using Vector2 = robocup_ssl_msgs::msg::Vector2; - -// visibilityの操作量 -// 追跡対象の情報が来ないとき、この操作量だけvisibilityを小さくする -// 操作量 = 1.0 / (trackerの更新周波数 * 消失判定までに掛ける時間) -// 例：trackerが100Hzで更新されるとき、操作量が0.002であれば、 -// 追跡対象が消失してから5秒かけてvisibilityが1.0から0.0になる -static const double VISIBILITY_CONTROL_VALUE = 0.005; - -RobotTracker::RobotTracker(const int team_color, const int id, const double dt) -{ - prev_tracked_robot_.robot_id.team_color = team_color; - prev_tracked_robot_.robot_id.id = id; - // visibilityはoptionalなので、ここでデフォルト値を設定しておく - prev_tracked_robot_.visibility.push_back(0.0); - // velocityはoptionalなので、ここでデフォルト値を設定しておく - prev_tracked_robot_.vel.push_back(Vector2()); - prev_tracked_robot_.vel_angular.push_back(0.0); - - // システムモデル - // pos(t+1) = pos(t) + vel(t)*dt + undefined_input(t) + noise - // pos = (x, y, theta) - Matrix A(6, 6); - A(1, 1) = 1.0; A(1, 2) = 0.0; A(1, 3) = 0.0; A(1, 4) = dt; A(1, 5) = 0.0; A(1, 6) = 0.0; - A(2, 1) = 0.0; A(2, 2) = 1.0; A(2, 3) = 0.0; A(2, 4) = 0.0; A(2, 5) = dt; A(2, 6) = 0.0; - A(3, 1) = 0.0; A(3, 2) = 0.0; A(3, 3) = 1.0; A(3, 4) = 0.0; A(3, 5) = 0.0; A(3, 6) = dt; - A(4, 1) = 0.0; A(4, 2) = 0.0; A(4, 3) = 0.0; A(4, 4) = 1.0; A(4, 5) = 0.0; A(4, 6) = 0.0; - A(5, 1) = 0.0; A(5, 2) = 0.0; A(5, 3) = 0.0; A(5, 4) = 0.0; A(5, 5) = 1.0; A(5, 6) = 0.0; - A(6, 1) = 0.0; A(6, 2) = 0.0; A(6, 3) = 0.0; A(6, 4) = 0.0; A(6, 5) = 0.0; A(6, 6) = 1.0; - - // 入力は未実装 - Matrix B(6, 3); - B(1, 1) = 0.0; B(1, 2) = 0.0; B(1, 3) = 0.0; - B(2, 1) = 0.0; B(2, 2) = 0.0; B(2, 3) = 0.0; - B(3, 1) = 0.0; B(3, 2) = 0.0; B(3, 3) = 0.0; - B(4, 1) = 0.0; B(4, 2) = 0.0; B(4, 3) = 0.0; - B(5, 1) = 0.0; B(5, 2) = 0.0; B(5, 3) = 0.0; - B(6, 1) = 0.0; B(6, 2) = 0.0; B(6, 3) = 0.0; - - std::vector AB(2); - AB[0] = A; - AB[1] = B; - - // システムノイズの平均値 - ColumnVector sys_noise_mu(6); - sys_noise_mu = 0.0; - - // 位置、速度の変化をのシステムノイズで表現する（つまりめちゃくちゃノイズがでかい） - // 0 m/s から、いきなり1.0 m/sに変化しうる、ということ - // 例：1.0[m/s] / 0.001[s] = 100 [m/ss] - const double MAX_LINEAR_ACC_MPS = 0.1 * 1.0 / dt; - // 例：1.0[rad/s] / 0.001[s] = 100 [rad/ss] - const double MAX_ANGULAR_ACC_RADPS = 0.05 * M_PI / dt; - const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt; // [m/s] - const double MAX_ANGULAR_ACCEL_IN_DT = MAX_ANGULAR_ACC_RADPS * dt; // [rad/s] - const double MAX_LINEAR_MOVEMENT_IN_DT = MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2); // [m] - const double MAX_ANGULAR_MOVEMENT_IN_DT = MAX_ANGULAR_ACC_RADPS / 2 * std::pow(dt, 2); // [rad] - - // システムノイズの分散 - SymmetricMatrix sys_noise_cov(6); - sys_noise_cov = 0.0; - sys_noise_cov(1, 1) = std::pow(MAX_LINEAR_MOVEMENT_IN_DT, 2); - sys_noise_cov(2, 2) = std::pow(MAX_LINEAR_MOVEMENT_IN_DT, 2); - sys_noise_cov(3, 3) = std::pow(MAX_ANGULAR_MOVEMENT_IN_DT, 2); - sys_noise_cov(4, 4) = std::pow(MAX_LINEAR_ACCEL_IN_DT, 2); - sys_noise_cov(5, 5) = std::pow(MAX_LINEAR_ACCEL_IN_DT, 2); - sys_noise_cov(6, 6) = std::pow(MAX_ANGULAR_ACCEL_IN_DT, 2); - - Gaussian system_uncertainty(sys_noise_mu, sys_noise_cov); - sys_pdf_ = std::make_shared(AB, system_uncertainty); - sys_model_ = std::make_shared(sys_pdf_.get()); - - // 観測モデル - // ~pos(t) = pos(t) + noise - // pos = (x, y, theta) - Matrix H(3, 6); - H = 0.0; - H(1, 1) = 1.0; - H(2, 2) = 1.0; - H(3, 3) = 1.0; - - // 観測ノイズの平均値 - ColumnVector meas_noise_mu(3); - meas_noise_mu = 0.0; - - // 観測ノイズの分散 - SymmetricMatrix meas_noise_cov(3); - meas_noise_cov(1, 1) = std::pow(0.02, 2); - meas_noise_cov(2, 2) = std::pow(0.02, 2); - meas_noise_cov(3, 3) = std::pow(0.02 * M_PI, 2); - Gaussian measurement_uncertainty(meas_noise_mu, meas_noise_cov); - - meas_pdf_ = std::make_shared(H, measurement_uncertainty); - meas_model_ = - std::make_shared(meas_pdf_.get()); - - // 事前分布 - ColumnVector prior_mu(6); - prior_mu = 0.0; - - SymmetricMatrix prior_cov(6); - prior_cov = 0.0; - prior_cov(1, 1) = 100.0; - prior_cov(2, 2) = 100.0; - prior_cov(3, 3) = 100.0; - prior_cov(4, 4) = 100.0; - prior_cov(5, 5) = 100.0; - prior_cov(6, 6) = 100.0; - - prior_ = std::make_shared(prior_mu, prior_cov); - - // カルマンフィルタの生成 - filter_ = std::make_shared(prior_.get()); -} - -void RobotTracker::push_back_observation(const DetectionRobot & robot) -{ - // 角度データを含まない場合は、そのデータを参照しない - if (robot.orientation.size() == 0) { - return; - } - - TrackedRobot observation; - observation.pos.x = robot.x * 0.001; // mm to meters - observation.pos.y = robot.y * 0.001; // mm to meters - observation.orientation = robot.orientation[0]; - robot_observations_.push_back(observation); -} - -TrackedRobot RobotTracker::update() -{ - constexpr auto OUTLIER_COUNT_THRESHOLD = 10; - - // 観測値から外れ値を取り除く - for (auto it = robot_observations_.begin(); it != robot_observations_.end(); ) { - if (is_outlier(*it)) { - // 外れ値が連続できたら、観測値をそのまま使用する（誘拐対応） - outlier_count_++; - if (outlier_count_ > OUTLIER_COUNT_THRESHOLD) { - break; - } - it = robot_observations_.erase(it); - } else { - outlier_count_ = 0; - ++it; - } - } - - auto size = robot_observations_.size(); - if (size == 0) { - // 観測値が無い場合の処理 - // visibilityを下げる - prev_tracked_robot_.visibility[0] -= VISIBILITY_CONTROL_VALUE; - if (prev_tracked_robot_.visibility[0] <= 0) { - // visibilityが0になったらカルマンフィルタの演算を実行しない - prev_tracked_robot_.visibility[0] = 0.0; - reset_prior(); - return prev_tracked_robot_; - } - - } else { - // 観測値があればvisibilityをn倍のレートで上げる - prev_tracked_robot_.visibility[0] += VISIBILITY_CONTROL_VALUE * 5.0; - if (prev_tracked_robot_.visibility[0] > 1.0) { - prev_tracked_robot_.visibility[0] = 1.0; - } - - // 観測値が複数ある場合は、その平均値をもとめる - // この処理で観測値を全て削除する - ColumnVector mean_observation(3); - mean_observation(1) = 0.0; - mean_observation(2) = 0.0; - double sum_x = 0.0; - double sum_y = 0.0; - - for (auto it = robot_observations_.begin(); it != robot_observations_.end(); ) { - mean_observation(1) += it->pos.x; - mean_observation(2) += it->pos.y; - // 角度は-pi ~ piの範囲なので、2次元ベクトルに変換してから平均値を求める - sum_x += std::cos(it->orientation); - sum_y += std::sin(it->orientation); - it = robot_observations_.erase(it); - } - mean_observation(1) /= size; - mean_observation(2) /= size; - sum_x /= size; - sum_y /= size; - mean_observation(3) = std::fmod(std::atan2(sum_y, sum_x), M_PI); - - // 観測値と前回の予測値がpi, -pi付近にあるとき、 - // ２つの角度の差分が大きくならないように、観測値の符号と値を調節する - mean_observation(3) = normalize_orientation( - filter_->PostGet()->ExpectedValueGet()(3), mean_observation(3)); - - filter_->Update(meas_model_.get(), mean_observation); - correct_orientation_overflow_of_prior(); - } - // 事後分布から予測値を取得 - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - prev_tracked_robot_.pos.x = expected_value(1); - prev_tracked_robot_.pos.y = expected_value(2); - prev_tracked_robot_.orientation = expected_value(3); - prev_tracked_robot_.vel[0].x = expected_value(4); - prev_tracked_robot_.vel[0].y = expected_value(5); - prev_tracked_robot_.vel_angular[0] = expected_value(6); - - // 次の状態を予測する - // 例えば、ロボットの加速度が入力値になる - // 入力値は未実装なので0とする - ColumnVector input(3); - input(1) = 0; - input(2) = 0; - input(3) = 0; - filter_->Update(sys_model_.get(), input); - correct_orientation_overflow_of_prior(); - - return prev_tracked_robot_; -} - -RobotLocalVelocity RobotTracker::calc_local_velocity() -{ - // ロボット座標系でのロボット速度を計算する - auto local_velocity = RobotLocalVelocity(); - local_velocity.robot_id = prev_tracked_robot_.robot_id; - - if (prev_tracked_robot_.vel.size() > 0) { - auto theta = prev_tracked_robot_.orientation; - auto world_vel = prev_tracked_robot_.vel[0]; - auto local_vel = world_vel; - local_vel.x = world_vel.x * std::cos(theta) + world_vel.y * std::sin(theta); - local_vel.y = -(world_vel.x * std::sin(theta) - world_vel.y * std::cos(theta)); - local_velocity.velocity.x = local_vel.x; - local_velocity.velocity.y = local_vel.y; - local_velocity.norm = std::sqrt(std::pow(local_vel.x, 2) + std::pow(local_vel.y, 2)); - } - if (prev_tracked_robot_.vel_angular.size() > 0) { - local_velocity.velocity.theta = prev_tracked_robot_.vel_angular[0]; - } - return local_velocity; -} - -void RobotTracker::reset_prior() -{ - // 事前分布を初期化する - ColumnVector prior_mu(6); - prior_mu = 0.0; - - SymmetricMatrix prior_cov(6); - prior_cov = 0.0; - prior_cov(1, 1) = 100.0; - prior_cov(2, 2) = 100.0; - prior_cov(3, 3) = 100.0; - prior_cov(4, 4) = 100.0; - prior_cov(5, 5) = 100.0; - prior_cov(6, 6) = 100.0; - - prior_->ExpectedValueSet(prior_mu); - prior_->CovarianceSet(prior_cov); - filter_->Reset(prior_.get()); -} - -bool RobotTracker::is_outlier(const TrackedRobot & observation) const -{ - // 観測が外れ値かどうか判定する - // Reference: https://myenigma.hatenablog.com/entry/20140825/1408975706 - const double THRESHOLD = 5.99; // 自由度2、棄却率5%のしきい値 - - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - auto covariance = filter_->PostGet()->CovarianceGet(); - - // マハラノビス距離を求める - double diff_x = observation.pos.x - expected_value(1); - double diff_y = observation.pos.y - expected_value(2); - double covariance_x = covariance(1, 1); - double covariance_y = covariance(2, 2); - - // 0 除算を避ける - if (std::fabs(covariance_x) < 1E-15 || std::fabs(covariance_y) < 1E-15) { - return false; - } - - double mahalanobis = std::sqrt( - std::pow(diff_x, 2) / covariance_x + std::pow( - diff_y, - 2) / covariance_y); - if (mahalanobis > THRESHOLD) { - return true; - } - - return false; -} - -void RobotTracker::correct_orientation_overflow_of_prior() -{ - // 事後分布の角度を取得し、-pi ~ piの範囲に収め、事前分布にセットする - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - auto covariance = filter_->PostGet()->CovarianceGet(); - - if (expected_value(3) < -M_PI || expected_value(3) > M_PI) { - expected_value(3) = normalize_orientation(expected_value(3)); - - prior_->ExpectedValueSet(expected_value); - prior_->CovarianceSet(covariance); - - filter_->Reset(prior_.get()); - } -} - -double RobotTracker::normalize_orientation(double orientation) const -{ - // 角度を-pi ~ piの範囲に収める - while (orientation >= M_PI) {orientation -= 2.0 * M_PI;} - while (orientation <= -M_PI) {orientation += 2.0 * M_PI;} - return orientation; -} - -double RobotTracker::normalize_orientation(const double from, const double to) const -{ - // fromからtoへ連続に角度が変化するようにtoの符号と大きさを変更する - // from(150 deg) -> to(-150 deg) => from(150 deg) -> to(210 deg) - return from + normalize_orientation(to - from); -} - -} // namespace consai_vision_tracker diff --git a/consai_vision_tracker/src/tracker_component.cpp b/consai_vision_tracker/src/tracker_component.cpp index be2d7c69..c89d572d 100644 --- a/consai_vision_tracker/src/tracker_component.cpp +++ b/consai_vision_tracker/src/tracker_component.cpp @@ -40,7 +40,29 @@ using std::placeholders::_1; Tracker::Tracker(const rclcpp::NodeOptions & options) : Node("tracker", options), update_rate_(0.01s) { - ball_tracker_ = std::make_shared(update_rate_.count()); + const auto UPDATE_RATE = 0.01s; + + ball_kalman_filter_ = std::make_unique(UPDATE_RATE.count()); + declare_parameter("ball_kalman_filter/q_acc", 5.0); + declare_parameter("ball_kalman_filter/q_uncertain_acc", 30.0); + declare_parameter("ball_kalman_filter/r_pos_stddev", 0.05); + declare_parameter("robot_kalman_filter/q_max_acc_xy", 10.0); + declare_parameter("robot_kalman_filter/q_max_acc_theta", 6.0); + declare_parameter("robot_kalman_filter/r_pos_stddev_xy", 0.05); + declare_parameter("robot_kalman_filter/r_pos_stddev_theta", 0.06); + + for (int i = 0; i < 16; i++) { + blue_robot_kalman_filter_.push_back( + std::make_unique( + RobotId::TEAM_COLOR_BLUE, i, + UPDATE_RATE.count())); + yellow_robot_kalman_filter_.push_back( + std::make_unique( + RobotId::TEAM_COLOR_YELLOW, i, + UPDATE_RATE.count())); + } + + timer_ = create_wall_timer(UPDATE_RATE, std::bind(&Tracker::on_timer, this)); pub_tracked_ = create_publisher("detection_tracked", 10); pub_robot_velocities_ = create_publisher("robot_local_velocities", 10); @@ -61,23 +83,29 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) sub_geometry_ = create_subscription( "geometry", 10, std::bind(&Tracker::callback_geometry, this, _1)); - auto qos = rclcpp::QoS(rclcpp::KeepLast(10)) - .reliable(); - - auto callback_param = [this](const std_msgs::msg::String::SharedPtr msg) - { - try { - nlohmann::json json_data = nlohmann::json::parse(msg->data); - gen_robot_trackers(json_data["robots"]["num_of_ids"]); - } catch (const std::exception & e) { - RCLCPP_ERROR(get_logger(), "Error: %s", e.what()); - } - }; - - sub_consai_param_rule_ = create_subscription( - "consai_param/rule", qos, callback_param); + // TODO(ShotaAk): use parameter callback + ball_kalman_filter_->update_noise_covariance_matrix( + get_parameter("ball_kalman_filter/q_acc").get_value(), + get_parameter("ball_kalman_filter/q_uncertain_acc").get_value(), + get_parameter("ball_kalman_filter/r_pos_stddev").get_value()); + + // TODO(ShotaAk): use parameter callback + for (auto && filter : blue_robot_kalman_filter_) { + filter->update_noise_covariance_matrix( + get_parameter("robot_kalman_filter/q_max_acc_xy").get_value(), + get_parameter("robot_kalman_filter/q_max_acc_theta").get_value(), + get_parameter("robot_kalman_filter/r_pos_stddev_xy").get_value(), + get_parameter("robot_kalman_filter/r_pos_stddev_theta").get_value()); + } - timer_ = create_wall_timer(update_rate_, std::bind(&Tracker::on_timer, this)); + // TODO(ShotaAk): use parameter callback + for (auto && filter : yellow_robot_kalman_filter_) { + filter->update_noise_covariance_matrix( + get_parameter("robot_kalman_filter/q_max_acc_xy").get_value(), + get_parameter("robot_kalman_filter/q_max_acc_theta").get_value(), + get_parameter("robot_kalman_filter/r_pos_stddev_xy").get_value(), + get_parameter("robot_kalman_filter/r_pos_stddev_theta").get_value()); + } } void Tracker::on_timer() @@ -85,22 +113,22 @@ void Tracker::on_timer() auto tracked_msg = std::make_unique(); auto robot_vel_msg = std::make_unique(); - for (auto && tracker : blue_robot_tracker_) { - tracked_msg->robots.push_back(tracker->update()); - robot_vel_msg->velocities.push_back(tracker->calc_local_velocity()); + for (auto && filter : blue_robot_kalman_filter_) { + tracked_msg->robots.push_back(filter->update()); + robot_vel_msg->velocities.push_back(filter->calc_local_velocity()); } - for (auto && tracker : yellow_robot_tracker_) { - tracked_msg->robots.push_back(tracker->update()); - robot_vel_msg->velocities.push_back(tracker->calc_local_velocity()); + for (auto && filter : yellow_robot_kalman_filter_) { + tracked_msg->robots.push_back(filter->update()); + robot_vel_msg->velocities.push_back(filter->calc_local_velocity()); } auto ball_is_near_a_robot = [this]( - const decltype(blue_robot_tracker_) & trackers, + const decltype(blue_robot_kalman_filter_) & filters, const State & ball_pose) { const auto NEAR_DISTANCE = 0.5; - for (auto && tracker : trackers) { - const auto robot = tracker->prev_estimation(); + for (auto && filter : filters) { + const auto robot = filter->get_estimation(); if (!tools::is_visible(robot)) { continue; } @@ -112,17 +140,17 @@ void Tracker::on_timer() return false; }; - const auto ball = ball_tracker_->prev_estimation(); + const auto ball = ball_kalman_filter_->get_estimation(); bool use_uncertain_sys_model = false; if (tools::is_visible(ball)) { const auto ball_pose = tools::pose_state(ball); use_uncertain_sys_model = - ball_is_near_a_robot(blue_robot_tracker_, ball_pose) || - ball_is_near_a_robot(yellow_robot_tracker_, ball_pose); + ball_is_near_a_robot(blue_robot_kalman_filter_, ball_pose) || + ball_is_near_a_robot(yellow_robot_kalman_filter_, ball_pose); } - tracked_msg->balls.push_back(ball_tracker_->update(use_uncertain_sys_model)); + tracked_msg->balls.push_back(ball_kalman_filter_->update(use_uncertain_sys_model)); tracked_msg = vis_data_handler_->publish_vis_tracked(std::move(tracked_msg)); @@ -130,41 +158,21 @@ void Tracker::on_timer() pub_robot_velocities_->publish(std::move(robot_vel_msg)); } -void Tracker::gen_robot_trackers(const unsigned int num) -{ - if (blue_robot_tracker_.size() >= num) { - return; - } - - for (auto i = blue_robot_tracker_.size(); i < num; i++) { - blue_robot_tracker_.push_back( - std::make_shared( - RobotId::TEAM_COLOR_BLUE, i, - update_rate_.count())); - yellow_robot_tracker_.push_back( - std::make_shared( - RobotId::TEAM_COLOR_YELLOW, i, - update_rate_.count())); - } -} - void Tracker::callback_detection(const DetectionFrame::SharedPtr msg) { for (const auto & ball : msg->balls) { - ball_tracker_->push_back_observation(ball); + ball_kalman_filter_->push_back_observation(ball); } for (const auto & blue_robot : msg->robots_blue) { - if (blue_robot.robot_id.size() > 0 && blue_robot.robot_id[0] < blue_robot_tracker_.size()) { - blue_robot_tracker_[blue_robot.robot_id[0]]->push_back_observation(blue_robot); + if (blue_robot.robot_id.size() > 0) { + blue_robot_kalman_filter_[blue_robot.robot_id[0]]->push_back_observation(blue_robot); } } for (const auto & yellow_robot : msg->robots_yellow) { - if (yellow_robot.robot_id.size() > 0 && - yellow_robot.robot_id[0] < yellow_robot_tracker_.size()) - { - yellow_robot_tracker_[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot); + if (yellow_robot.robot_id.size() > 0) { + yellow_robot_kalman_filter_[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot); } } @@ -177,22 +185,20 @@ void Tracker::callback_detection_invert(const DetectionFrame::SharedPtr msg) for (auto && ball : msg->balls) { invert_ball(ball); - ball_tracker_->push_back_observation(ball); + ball_kalman_filter_->push_back_observation(ball); } for (auto && blue_robot : msg->robots_blue) { - if (blue_robot.robot_id.size() > 0 && blue_robot.robot_id[0] < blue_robot_tracker_.size()) { + if (blue_robot.robot_id.size() > 0) { invert_robot(blue_robot); - blue_robot_tracker_[blue_robot.robot_id[0]]->push_back_observation(blue_robot); + blue_robot_kalman_filter_[blue_robot.robot_id[0]]->push_back_observation(blue_robot); } } for (auto && yellow_robot : msg->robots_yellow) { - if (yellow_robot.robot_id.size() > 0 && - yellow_robot.robot_id[0] < yellow_robot_tracker_.size()) - { + if (yellow_robot.robot_id.size() > 0) { invert_robot(yellow_robot); - yellow_robot_tracker_[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot); + yellow_robot_kalman_filter_[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot); } } diff --git a/hooks/run_ament_flake8.sh b/hooks/run_ament_flake8.sh index cc571e07..9163666d 100755 --- a/hooks/run_ament_flake8.sh +++ b/hooks/run_ament_flake8.sh @@ -4,6 +4,10 @@ set -e # プロジェクトルートへ移動（どこから実行されても .cfg が見えるように） cd "$(git rev-parse --show-toplevel)" +# 現在の設定を表示 +echo "Current configuration:" +cat setup.cfg + # ステージ済みの Python ファイルのみ取得 FILES=$(git diff --cached --name-only --diff-filter=ACM | grep '\.py$' || true) @@ -15,5 +19,4 @@ fi echo "[ament_flake8] Running on files:" echo "$FILES" -# 設定ファイル (setup.cfg or tox.ini) を自動検出し、flake8 を実行 ament_flake8 --config setup.cfg $FILES diff --git a/setup.cfg b/setup.cfg index cf3096d5..5974d70a 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,3 +1,3 @@ [flake8] max-line-length = 120 -ignore = D100, D400, D104, D202, D403, I100, Q000, W503 \ No newline at end of file +extend-ignore = D100, D400, D104, D202, D403, I100, Q000, W503, I201 \ No newline at end of file