/game_analysisをworld_model_publisherに内包する (#740)

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

crane_game_analyzer/include/crane_game_analyzer/game_analyzer.hpp

@@ -10,13 +10,11 @@
 #include <algorithm>
 #include <crane_msg_wrappers/crane_visualizer_wrapper.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
-#include <crane_msgs/msg/game_analysis.hpp>
 #include <crane_msgs/msg/world_model.hpp>
 #include <deque>
 #include <rclcpp/rclcpp.hpp>
 #include <vector>
 
-#include "kick_event_detector.hpp"
 #include "visibility_control.h"
 
 namespace crane
@@ -120,10 +118,6 @@ class GameAnalyzerComponent : public rclcpp::Node
 
   WorldModelWrapper::UniquePtr world_model;
 
-  KickEventDetector kick_event_detector;
-
-  rclcpp::Publisher<crane_msgs::msg::GameAnalysis>::SharedPtr game_analysis_pub;
-
   GameAnalyzerConfig config;
 
   CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer;

crane_game_analyzer/src/crane_game_analyzer.cpp

@@ -15,7 +15,6 @@ namespace crane
 {
 GameAnalyzerComponent::GameAnalyzerComponent(const rclcpp::NodeOptions & options)
 : Node("crane_game_analyzer", options),
-  game_analysis_pub(create_publisher<crane_msgs::msg::GameAnalysis>("game_analysis", 10)),
   visualizer(std::make_unique<CraneVisualizerBuffer::MessageBuilder>("game_analyzer"))
 {
   RCLCPP_INFO(get_logger(), "GameAnalyzer is constructed.");
@@ -25,14 +24,6 @@ GameAnalyzerComponent::GameAnalyzerComponent(const rclcpp::NodeOptions & options
   world_model = std::make_unique<WorldModelWrapper>(*this);
 
   world_model->addCallback([&]() {
-    kick_event_detector.update(*world_model, visualizer);
-    crane_msgs::msg::GameAnalysis game_analysis_msg;
-    updateBallPossession(game_analysis_msg.ball);
-    if (auto kick = kick_event_detector.getOnGoingKick(); kick.has_value()) {
-      game_analysis_msg.ongoing_kick.push_back(*kick);
-    }
-
-    game_analysis_pub->publish(game_analysis_msg);
     auto robot_collision_info = getRobotCollisionInfo();
 
     if (robot_collision_info) {

crane_msgs/msg/world_model/BallInfo.msg

@@ -2,7 +2,7 @@
 bool disappeared
 
 # カーブボールかどうか
-bool curved
+# bool curved
 
 # 位置・速度
 geometry_msgs/Pose2D pose
@@ -12,8 +12,3 @@ float32 velocity_norm
 
 bool detected
 float64 detection_time
-
-bool is_our_ball
-bool is_their_ball
-bool event_detected
-bool state_changed

crane_msgs/msg/world_model/RobotInfo.msg

@@ -5,6 +5,9 @@ geometry_msgs/Pose2D pose
 geometry_msgs/Pose2D velocity
 float32 velocity_norm
 
+geometry_msgs/Pose2D acceleration
+float32 acceleration_norm
+
 # If robot is detected by vision in this frame, this field is set to true
 bool detected
 

crane_world_model_publisher/CMakeLists.txt

@@ -17,7 +17,10 @@ find_package(rclcpp_components REQUIRED)
 ament_auto_find_build_dependencies()
 
 ament_auto_add_library(${PROJECT_NAME}_component SHARED
-  src/world_model_publisher.cpp include/crane_world_model_publisher/world_model_publisher.hpp src/visualization_data_handler.cpp)
+  src/world_model_publisher.cpp
+  src/visualization_data_handler.cpp
+  src/world_model_data_provider.cpp
+)
 
 target_link_libraries(${PROJECT_NAME}_component
         ${robocup_ssl_msgs_LIBRARIES} ${Boost_LIBRARIES}

crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp → crane_world_model_publisher/include/crane_world_model_publisher/kick_event_detector.hpp

@@ -4,8 +4,8 @@
 // license that can be found in the LICENSE file or at
 // https://opensource.org/licenses/MIT.
 
-#ifndef CRANE_GAME_ANALYZER__KICK_EVENT_DETECTOR_HPP_
-#define CRANE_GAME_ANALYZER__KICK_EVENT_DETECTOR_HPP_
+#ifndef CRANE_WORLD_MODEL_PUBLISHER__KICK_EVENT_DETECTOR_HPP_
+#define CRANE_WORLD_MODEL_PUBLISHER__KICK_EVENT_DETECTOR_HPP_
 
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/kick.hpp>
@@ -29,7 +29,11 @@ struct KickOrigin
 class KickEventDetector
 {
 public:
-  KickEventDetector() : ros_clock(RCL_ROS_TIME) {}
+  KickEventDetector()
+  : ros_clock(RCL_ROS_TIME),
+    visualizer(std::make_unique<CraneVisualizerBuffer::MessageBuilder>("kick_event"))
+  {
+  }
 
   void update(
     const WorldModelWrapper & world_model,
@@ -261,7 +265,9 @@ class KickEventDetector
   double distance_threshold = 0.15;
 
   rclcpp::Clock ros_clock;
+
+  CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
 };
 }  // namespace crane
 
-#endif  // CRANE_GAME_ANALYZER__KICK_EVENT_DETECTOR_HPP_
+#endif  // CRANE_WORLD_MODEL_PUBLISHER__KICK_EVENT_DETECTOR_HPP_

crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp

@@ -34,7 +34,7 @@ class VisualizationDataHandler
 
   void publish_vis_geometry(const SSL_GeometryData & geometry_data);
   void publish_vis_tracked(const TrackedFrame & tracked_frame);
-  void publish_vis_referee(const Referee::SharedPtr msg);
+  void publish_vis_referee(const Referee & msg);
 
 private:
   rclcpp::Subscription<Referee>::SharedPtr sub_referee_;

crane_world_model_publisher/include/crane_world_model_publisher/world_model_data_provider.hpp

@@ -0,0 +1,141 @@
+// Copyright (c) 2025 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_WORLD_MODEL_PUBLISHER__WORLD_MODEL_DATA_PROVIDER_HPP_
+#define CRANE_WORLD_MODEL_PUBLISHER__WORLD_MODEL_DATA_PROVIDER_HPP_
+
+#include <robocup_ssl_msgs/ssl_vision_detection_tracked.pb.h>
+#include <robocup_ssl_msgs/ssl_vision_geometry.pb.h>
+#include <robocup_ssl_msgs/ssl_vision_wrapper.pb.h>
+
+#include <crane_basics/multicast.hpp>
+#include <crane_msgs/msg/play_situation.hpp>
+#include <crane_msgs/msg/robot_feedback_array.hpp>
+#include <crane_msgs/msg/world_model.hpp>
+#include <crane_world_model_publisher/visualization_data_handler.hpp>
+#include <deque>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <robocup_ssl_msgs/msg/robots_status.hpp>
+#include <string>
+#include <vector>
+
+namespace crane
+{
+class WorldModelDataProvider
+{
+public:
+  explicit WorldModelDataProvider(rclcpp::Node & node);
+
+  ~WorldModelDataProvider() = default;
+
+  void on_udp_timer();
+
+  crane_msgs::msg::WorldModel getMsg();
+
+  [[nodiscard]] auto available() const -> bool { return has_tracker_updated && has_vision_updated; }
+
+private:
+  rclcpp::Node & node;
+
+  std::unique_ptr<multicast::MulticastReceiver> tracker_receiver;
+
+  std::unique_ptr<multicast::MulticastReceiver> vision_receiver;
+
+  rclcpp::TimerBase::SharedPtr udp_timer;
+
+  VisualizationDataHandler vis_data_handler;
+
+  enum class Color { BLUE, YELLOW };
+
+  struct GameData
+  {
+    std::string team_name;
+
+    Color our_color;
+
+    Color their_color;
+
+    int our_goalie_id;
+
+    int their_goalie_id;
+
+    int our_max_allowed_bots;
+
+    int their_max_allowed_bots;
+
+    double field_w;
+
+    double field_h;
+
+    double goal_w;
+
+    double goal_h;
+
+    double penalty_area_w;
+
+    double penalty_area_h;
+  } game_data;
+
+  struct Data
+  {
+    double ball_placement_target_x;
+
+    double ball_placement_target_y;
+
+    std::vector<crane_msgs::msg::RobotInfo> robot_info[2];
+
+    crane_msgs::msg::BallInfo ball_info;
+
+    std::vector<bool> ball_sensor_detected;
+  } data;
+
+  bool on_positive_half;
+
+  bool is_emplace_positive_side;
+
+  bool has_tracker_updated = false;
+
+  bool has_vision_updated = false;
+
+  rclcpp::Time last_ball_detect_time;
+
+  struct BallAnalysis
+  {
+    bool is_our_ball;
+
+    bool is_their_ball;
+
+    bool ball_event_detected;
+
+    enum class BallEvent { NONE, OUR_BALL, THEIR_BALL };
+
+    BallEvent last_ball_event;
+  };
+
+  rclcpp::Subscription<crane_msgs::msg::PlaySituation>::SharedPtr sub_play_situation;
+
+  crane_msgs::msg::PlaySituation latest_play_situation;
+
+  rclcpp::Subscription<crane_msgs::msg::RobotFeedbackArray>::SharedPtr sub_robot_feedback;
+
+  crane_msgs::msg::RobotFeedbackArray robot_feedback;
+
+  rclcpp::Subscription<robocup_ssl_msgs::msg::RobotsStatus>::SharedPtr sub_robots_status_blue;
+
+  rclcpp::Subscription<robocup_ssl_msgs::msg::RobotsStatus>::SharedPtr sub_robots_status_yellow;
+
+  rclcpp::Subscription<robocup_ssl_msgs::msg::Referee>::SharedPtr sub_referee;
+
+  void trackerCallback(const TrackedFrame & tracked_frame);
+
+  void visionGeometryCallback(const SSL_GeometryData & geometry_data);
+
+  void visionDetectionCallback(const SSL_DetectionFrame & detection_frame);
+};
+}  // namespace crane
+
+#endif  // CRANE_WORLD_MODEL_PUBLISHER__WORLD_MODEL_DATA_PROVIDER_HPP_