ボールを見失った判定を、トラッカーの見失った時間からビジョンが見失った時間に変更

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;

crane_world_model_publisher/src/world_model_publisher.cpp

@@ -204,6 +204,11 @@ void WorldModelPublisherComponent::on_udp_timer()
       packet.ParseFromString(std::string(buf.begin(), buf.end()));
       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();
+        }
       }
     }
   }
@@ -269,7 +274,8 @@ void WorldModelPublisherComponent::visionDetectionsCallback(const TrackedFrame &
     int ball_info_history_size = ball_info_history.size();
     if(1 > ball_info_history_size){
       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,9 +551,11 @@ void WorldModelPublisherComponent::updateBallContact()
       && 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;
       robot_info[static_cast<uint8_t>(our_color)][i].ball_contact.last_contacted_time = now;