From 06ef11193822a11f7e30c605acaedb374f42c2ee Mon Sep 17 00:00:00 2001 From: "T.W" <36392290+ssaattww@users.noreply.github.com> Date: Mon, 13 Jan 2025 19:20:18 +0900 Subject: [PATCH 1/2] =?UTF-8?q?=E3=83=93=E3=82=B8=E3=83=A7=E3=83=B3?= =?UTF-8?q?=E3=81=8C=E3=83=9C=E3=83=BC=E3=83=AB=E3=82=92=E8=A6=8B=E5=A4=B1?= =?UTF-8?q?=E3=81=A3=E3=81=9F=E3=81=A8=E3=81=8D=E3=81=AE=E5=8B=95=E4=BD=9C?= =?UTF-8?q?=E3=82=92=E6=94=B9=E5=96=84=20(#695)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../world_model_publisher.hpp | 6 +- .../src/world_model_publisher.cpp | 63 +++++++++++++++---- 2 files changed, 56 insertions(+), 13 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 83167a997..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 @@ -132,10 +132,14 @@ class WorldModelPublisherComponent : public rclcpp::Node double ball_placement_target_y; - bool ball_detected[20] = {}; + bool ball_sensor_detected[20] = {}; crane_msgs::msg::BallInfo ball_info; + 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 324dadcbd..92061b146 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -68,7 +68,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt if (feedback->ball_sensor) { contact.last_contacted_time = now; } - ball_detected[robot.robot_id] = feedback->ball_sensor; + ball_sensor_detected[robot.robot_id] = feedback->ball_sensor; } } }); @@ -78,7 +78,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt if (our_color == Color::BLUE) { auto now = rclcpp::Clock().now(); for (auto status : msg->robots_status) { - ball_detected[status.robot_id] = status.infrared; + ball_sensor_detected[status.robot_id] = status.infrared; auto & contact = robot_info[static_cast(our_color)][status.robot_id].ball_contact; contact.current_time = now; @@ -95,7 +95,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt if (our_color == Color::YELLOW) { auto now = rclcpp::Clock().now(); for (auto status : msg->robots_status) { - ball_detected[status.robot_id] = status.infrared; + ball_sensor_detected[status.robot_id] = status.infrared; auto & contact = robot_info[static_cast(our_color)][status.robot_id].ball_contact; contact.current_time = now; @@ -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(); + } + } } } } @@ -264,6 +270,24 @@ void WorldModelPublisherComponent::visionDetectionsCallback(const TrackedFrame & ball_info.disappeared = false; } else { ball_info.detected = false; + + // ball disappeared 判定 + 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 = (now() - last_ball_detect_time).seconds(); + + // 0.5secビジョンから見えていなければ見失った + if (0.5 < elapsed_time_since_last_detected) { + ball_info.disappeared = true; + } + } + } + + ball_info_history.emplace_back(ball_info); + if (ball_info_history.size() > 10) { + ball_info_history.pop_front(); } has_vision_updated = true; @@ -303,11 +327,12 @@ void WorldModelPublisherComponent::publishWorldModel() wm.is_yellow = (our_color == Color::YELLOW); wm.on_positive_half = on_positive_half; wm.is_emplace_positive_side = is_emplace_positive_side; - wm.ball_info = ball_info; wm.our_max_allowed_bots = our_max_allowed_bots; wm.their_max_allowed_bots = their_max_allowed_bots; updateBallContact(); + wm.ball_info = ball_info; + ball_history.push_back(Point(wm.ball_info.pose.x, wm.ball_info.pose.y)); if (ball_history.size() > history_size) { ball_history.pop_front(); @@ -455,12 +480,6 @@ void WorldModelPublisherComponent::publishWorldModel() void WorldModelPublisherComponent::updateBallContact() { auto now = rclcpp::Clock().now(); - static std::deque ball_info_history; - - ball_info_history.emplace_back(ball_info); - if (ball_info_history.size() > 10) { - ball_info_history.pop_front(); - } // bool pre_is_our_ball = std::exchange(is_our_ball, false); is_their_ball = false; @@ -524,12 +543,32 @@ void WorldModelPublisherComponent::updateBallContact() // ローカルセンサーの情報でボール情報を更新 for (std::size_t i = 0; i < robot_info[static_cast(our_color)].size(); i++) { - // ボールがロボットに近いときのみ接触とみなす（誤作動防止） double ball_distance = std::hypot( 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); + // ビジョンがボールを見失っているときにボールセンサが反応している間は、接触しているものとみなす。 if ( - ball_detected[i] && not robot_info[static_cast(our_color)][i].disappeared && + ball_sensor_detected[i] && not robot_info[static_cast(our_color)][i].disappeared && + ball_info.disappeared) { + // ビジョンはボール見失っているけどロボットが保持しているので、ロボットの座標にボールがあることにする + + 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; + if (not is_our_ball) { + std::cout << "敵ボール接触" << std::endl; + is_our_ball = true; + ball_event_detected = true; + } + } else if ( + ball_sensor_detected[i] && not robot_info[static_cast(our_color)][i].disappeared && ball_distance < 0.3) { 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; From 9cc51a7d1c28d99c38649d6c37369bcf700de706 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 13 Jan 2025 19:42:54 +0900 Subject: [PATCH 2/2] =?UTF-8?q?omega=5Flimit=E3=82=92=E4=BD=BF=E3=81=88?= =?UTF-8?q?=E3=82=8B=E7=8A=B6=E6=85=8B=E3=81=AB=20(#697)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_msgs/msg/control/RobotCommand.msg | 2 +- crane_sender/src/ibis_sender_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_msgs/msg/control/RobotCommand.msg b/crane_msgs/msg/control/RobotCommand.msg index 0e3fcdab7..bd1cef3fc 100755 --- a/crane_msgs/msg/control/RobotCommand.msg +++ b/crane_msgs/msg/control/RobotCommand.msg @@ -29,7 +29,7 @@ geometry_msgs/Pose2D current_pose bool enable_local_feedback float32 target_theta -float32 omega_limit 100.0 +float32 omega_limit 10.0 float32 latency_ms 0.0 diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp index 187406a98..f3a85d753 100644 --- a/crane_sender/src/ibis_sender_node.cpp +++ b/crane_sender/src/ibis_sender_node.cpp @@ -158,7 +158,7 @@ class IbisSenderNode : public SenderBase packet.stop_emergency = command.stop_flag; packet.acceleration_limit = command.local_planner_config.max_acceleration + 1.0; packet.linear_velocity_limit = command.local_planner_config.max_velocity; - packet.angular_velocity_limit = 10.; + packet.angular_velocity_limit = command.omega_limit; packet.prioritize_move = true; packet.prioritize_accurate_acceleration = true;