From 07b9178ee8fef679420326bfc78c7113d5bd7769 Mon Sep 17 00:00:00 2001 From: ssaattww Date: Mon, 13 Jan 2025 15:14:35 +0900 Subject: [PATCH] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=82=92=E8=A6=8B?= =?UTF-8?q?=E5=A4=B1=E3=81=A3=E3=81=9F=E5=88=A4=E5=AE=9A=E3=82=92=E3=80=81?= =?UTF-8?q?=E3=83=88=E3=83=A9=E3=83=83=E3=82=AB=E3=83=BC=E3=81=AE=E8=A6=8B?= =?UTF-8?q?=E5=A4=B1=E3=81=A3=E3=81=9F=E6=99=82=E9=96=93=E3=81=8B=E3=82=89?= =?UTF-8?q?=E3=83=93=E3=82=B8=E3=83=A7=E3=83=B3=E3=81=8C=E8=A6=8B=E5=A4=B1?= =?UTF-8?q?=E3=81=A3=E3=81=9F=E6=99=82=E9=96=93=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model_publisher.hpp | 2 ++ .../src/world_model_publisher.cpp | 14 +++++++++++--- 2 files changed, 13 insertions(+), 3 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 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;