Visionから消えたときにボールセンサの値を使って上書き

ibis-ssl · Mar 4, 2025 · 4bf54c5 · 4bf54c5
1 parent a6e1716
commit 4bf54c5
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 33 deletions.
diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp
@@ -71,9 +71,9 @@ void WorldModelPublisherComponent::publishWorldModel()
 {
   auto msg = data_provider.getMsg();
   updateHistory(msg);
-  updateBallContact();
 
   wrapper->update(msg);
+  updateBallContact();
   postProcessWorldModel(wrapper);
 
   pub_world_model->publish(wrapper->getMsg());
@@ -160,38 +160,16 @@ void WorldModelPublisherComponent::updateBallContact()
 
   // ローカルセンサーの情報でボール情報を更新
   auto friend_robots = wrapper->ours.getAvailableRobots();
-  // for (std::size_t i = 0; i < friend_robots.size(); i++) {
-  //   auto robot = friend_robots[i];
-  //   double ball_distance = robot->getDistance(wrapper->ball.pos);
-  //   // ビジョンがボールを見失っているときに
-  //   // ボールセンサが反応している間は、接触しているものとみなす。
-  //   if (ball_sensor_detected[i] && friend_robots[i]->available && ball_info.disappeared) {
-  //     // ビジョンはボール見失っているけどロボットが保持しているので、
-  //     // ロボットの座標にボールがあることにする
-  //
-  //     Point center_to_kicker = getNormVec(robot->pose.theta) * 0.09;
-  //     ball_info.pose.x = robot->pose.pos.x() + center_to_kicker.x();
-  //     ball_info.pose.y = robot->pose.pos.y() + center_to_kicker.y();
-  //
-  //     // robot->ball_contact.is_vision_source = false;
-  //     // robot->ball_contact.current_time = now;
-  //     // robot->ball_contact.last_contacted_time = now;
-  //     if (not is_our_ball) {
-  //       std::cout << "敵ボール接触" << std::endl;
-  //       is_our_ball = true;
-  //       ball_event_detected = true;
-  //     }
-  //   } else if (ball_sensor_detected[i] && robot->available && ball_distance < 0.3) {
-  //     // robot->ball_contact.is_vision_source = false;
-  //     // robot->ball_contact.current_time = now;
-  //     // robot->ball_contact.last_contacted_time = now;
-  //     if (not is_our_ball) {
-  //       std::cout << "敵ボール接触" << std::endl;
-  //       is_our_ball = true;
-  //       ball_event_detected = true;
-  //     }
-  //   }
-  // }
+  for (std::size_t i = 0; i < friend_robots.size(); i++) {
+    auto robot = friend_robots[i];
+    // ビジョンがボールを見失っているときに
+    // ボールセンサが反応している間は、接触しているものとみなす。
+    if (robot->getBallSensorAvailable(now) && not wrapper->ball.detected) {
+      // ビジョンはボール見失っているけどロボットが保持しているので、
+      // ロボットの座標にボールがあることにする
+      wrapper->overwriteBallPos(robot->kicker_center());
+    }
+  }
 }
 }  // namespace crane
 

diff --git a/utility/crane_basics/include/crane_basics/robot_info.hpp b/utility/crane_basics/include/crane_basics/robot_info.hpp
@@ -46,6 +46,13 @@ struct RobotInfo
 
   bool ball_sensor = false;
 
+  bool getBallSensorAvailable(
+    rclcpp::Time now, rclcpp::Duration interval = rclcpp::Duration::from_seconds(0.001)) const
+  {
+    return now.get_clock_type() == ball_sensor_stamp.get_clock_type() &&
+           (now - ball_sensor_stamp).seconds() < interval.seconds();
+  }
+
   using SharedPtr = std::shared_ptr<RobotInfo>;
 
   [[nodiscard]] double getDribblerDistance() const { return 0.090; }

diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp
@@ -81,6 +81,13 @@ struct WorldModelWrapper
 
   void update(const crane_msgs::msg::GameAnalysis & msg) { latest_msg.game_analysis = msg; }
 
+  void overwriteBallPos(Point pos)
+  {
+    ball.pos = pos;
+    latest_msg.ball_info.pose.x = pos.x();
+    latest_msg.ball_info.pose.y = pos.y();
+  }
+
   [[nodiscard]] const auto & getMsg() const { return latest_msg; }
 
   [[nodiscard]] auto onPositiveHalf() const { return (latest_msg.on_positive_half); }