Merge branch 'develop' into 0111

ibis-ssl · Jan 13, 2025 · e4fb752 · e4fb752
2 parents f8f4537 + 9cc51a7
commit e4fb752
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 2 deletions.
diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp
@@ -138,6 +138,8 @@ class WorldModelPublisherComponent : public rclcpp::Node
 
   std::deque<crane_msgs::msg::BallInfo> ball_info_history;
 
+  rclcpp::Time last_ball_detect_time;
+
   std::vector<crane_msgs::msg::RobotInfo> robot_info[2];
 
   std::unique_ptr<multicast::MulticastReceiver> geometry_receiver;

diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp
@@ -205,6 +205,12 @@ void WorldModelPublisherComponent::on_udp_timer()
       if (packet.has_geometry()) {
         visionGeometryCallback(packet.geometry());
       }
+      if (packet.has_detection()) {
+        int balls_size = packet.detection().balls().size();
+        if (0 > balls_size) {
+          last_ball_detect_time = now();
+        }
+      }
     }
   }
 }
@@ -271,6 +277,8 @@ void WorldModelPublisherComponent::visionDetectionsCallback(const TrackedFrame &
       auto last_ball_info_history = ball_info_history[ball_info_history_size - 1];
       double elapsed_time_since_last_detected =
         tracked_frame.timestamp() - last_ball_info_history.detection_time;
+      // double elapsed_time_since_last_detected = tracked_frame.timestamp() - last_ball_info_history.detection_time;
+      double elapsed_time_since_last_detected = (now() - last_ball_detect_time).seconds();
 
       // 0.5secビジョンから見えていなければ見失った
       if (0.5 < elapsed_time_since_last_detected) {
@@ -545,8 +553,13 @@ void WorldModelPublisherComponent::updateBallContact()
       ball_sensor_detected[i] && not robot_info[static_cast<uint8_t>(our_color)][i].disappeared &&
       ball_info.disappeared) {
       // ビジョンはボール見失っているけどロボットが保持しているので、ロボットの座標にボールがあることにする
-      ball_info.pose.x = robot_info[static_cast<uint8_t>(our_color)][i].pose.x;
-      ball_info.pose.y = robot_info[static_cast<uint8_t>(our_color)][i].pose.y;
+
+      Point center_to_kicker =
+        getNormVec(robot_info[static_cast<uint8_t>(our_color)][i].pose.theta) * 0.09;
+      ball_info.pose.x =
+        robot_info[static_cast<uint8_t>(our_color)][i].pose.x + center_to_kicker.x();
+      ball_info.pose.y =
+        robot_info[static_cast<uint8_t>(our_color)][i].pose.y + center_to_kicker.y();
 
       robot_info[static_cast<uint8_t>(our_color)][i].ball_contact.is_vision_source = false;
       robot_info[static_cast<uint8_t>(our_color)][i].ball_contact.current_time = now;