Remove orocos bfl

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:
         """指定座標がプレースメントエリアにあるかを判定する関数."""

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)

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)

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 <chrono>
+#include <functional>
 #include <map>
 
 #include "consai_msgs/msg/state2_d.hpp"

consai_vision_tracker/CMakeLists.txt

@@ -15,30 +15,31 @@ 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)
 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
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>"
+)
 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

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 <Eigen/Dense>
+#include <vector>
+
+#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<double, 2, 4>;
+using Matrix42d = Eigen::Matrix<double, 4, 2>;
+
+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<TrackedBall> 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_

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 <Eigen/Dense>
+#include <vector>
+
+#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<double, 6, 1>;
+using Vector3d = Eigen::Vector3d;
+using Matrix3d = Eigen::Matrix3d;
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using Matrix36d = Eigen::Matrix<double, 3, 6>;
+
+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<TrackedRobot> 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_