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 index 8213c0197..75242ef47 100644 --- 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 ball_info_history; + rclcpp::Time last_ball_detect_time; + std::vector robot_info[2]; std::unique_ptr geometry_receiver; diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 1a6bb49a7..95ef73406 100644 --- 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(our_color)][i].disappeared && ball_info.disappeared) { // ビジョンはボール見失っているけどロボットが保持しているので、ロボットの座標にボールがあることにする - ball_info.pose.x = robot_info[static_cast(our_color)][i].pose.x; - ball_info.pose.y = robot_info[static_cast(our_color)][i].pose.y; + + Point center_to_kicker = + getNormVec(robot_info[static_cast(our_color)][i].pose.theta) * 0.09; + ball_info.pose.x = + robot_info[static_cast(our_color)][i].pose.x + center_to_kicker.x(); + ball_info.pose.y = + robot_info[static_cast(our_color)][i].pose.y + center_to_kicker.y(); robot_info[static_cast(our_color)][i].ball_contact.is_vision_source = false; robot_info[static_cast(our_color)][i].ball_contact.current_time = now;