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 93222ecc1..93c3e8990 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/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(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; robot_info[static_cast(our_color)][i].ball_contact.last_contacted_time = now;