From f469d477f4afb5b3ded783a4cff8d1869432c63e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 6 Mar 2025 02:36:58 +0900 Subject: [PATCH 1/7] =?UTF-8?q?=E3=83=91=E3=82=B9=E3=82=B9=E3=82=B3?= =?UTF-8?q?=E3=82=A2=E3=81=AE=E6=8F=8F=E7=94=BB?= 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 | 58 +++++++++++++++++++ .../include/crane_basics/ddps.hpp | 15 +++++ 3 files changed, 75 insertions(+) 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 af9f4e7d..ba065aec 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 @@ -96,6 +96,8 @@ class WorldModelPublisherComponent : public rclcpp::Node CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + CraneVisualizerBuffer::MessageBuilder::UniquePtr pass_score_visualizer; + std::array, 20> friend_history; std::array, 20> enemy_history; diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 1d20563e..efa1c0aa 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -4,6 +4,7 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. +#include #include #include #include @@ -21,6 +22,9 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt visualizer = std::make_unique("world_model/trajectory"); + pass_score_visualizer = + std::make_unique("world_model/pass_score"); + declare_parameter("position_history_size", 200); get_parameter("position_history_size", history_size); @@ -233,6 +237,60 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar } game_analysis_msg.their_slack.push_back(slack_msg); } + + auto our_robots = world_model->ours.getAvailableRobots(true); + const auto enemy_robots = world_model->theirs.getAvailableRobots(); + + auto calc_score = [&](Point p) { + Segment ball_to_target{world_model->ball.pos, p}; + double score = 1.0; + // 0~4mで遠くなるほどスコアが高い + score += std::clamp((p - world_model->ball.pos).norm() * 0.5, 0.0, 2.0); + // パス先のゴールチャンスが大きい場合はスコアを上げる(30度以上で最大0.5上昇) + auto [best_angle, goal_angle_width] = world_model->getLargestGoalAngleRangeFromPoint(p); + score += std::clamp(goal_angle_width / (M_PI / 12.), 0.0, 0.5); + // 敵ゴールに近いときはスコアを上げる + double normed_distance_to_their_goal = + ((p - world_model->getTheirGoalCenter()).norm() - (world_model->field_size.x() * 0.5)) / + (world_model->field_size.x() * 0.5); + // マイナスのときはゴールに近い + score *= (1.0 - normed_distance_to_their_goal * 0.5); + if (auto nearest_enemy = + world_model->getNearestRobotWithDistanceFromSegment(ball_to_target, enemy_robots); + nearest_enemy) { + // ボールから遠い敵がパスコースを塞いでいる場合は諦める + if ( + nearest_enemy->robot->getDistance(world_model->ball.pos) > 2.0 && + nearest_enemy->distance < 0.4) { + score = 0.0; + } + // パスラインに敵がいるときはスコアを下げる + score *= 1.0 / (1.0 + nearest_enemy->distance); + } + return score; + }; + + constexpr double UNIT = 0.2; + auto grid_points = getPoints( + Point(0, 0), UNIT, UNIT, world_model->field_size.x() / UNIT, + world_model->field_size.y() / UNIT); + auto score_grid = + grid_points | + ranges::views::transform([&](const auto & p) { return std::make_pair(p, calc_score(p)); }) | + ranges::to(); + + ranges::for_each(score_grid, [&](const auto & pair) { + SvgCircleBuilder circle; + circle.center(pair.first).radius(pair.second * 0.05).stroke("red").strokeWidth(2.); + pass_score_visualizer->add(circle.getSvgString()); + }); + pass_score_visualizer->flush(); + + auto score_with_bots = our_robots | ranges::views::transform([&](const auto & robot) { + return std::make_pair(robot, calc_score(robot->pose.pos)); + }) | + ranges::to(); + world_model->update(game_analysis_msg); } diff --git a/utility/crane_basics/include/crane_basics/ddps.hpp b/utility/crane_basics/include/crane_basics/ddps.hpp index f9e7c21d..b4704f65 100644 --- a/utility/crane_basics/include/crane_basics/ddps.hpp +++ b/utility/crane_basics/include/crane_basics/ddps.hpp @@ -51,5 +51,20 @@ inline auto getPoints(const Point & center, float unit, int unit_num) -> std::ve return points; } +inline auto getPoints( + const Point & center, float unit_x, float unit_y, int unit_num_x, int unit_num_y) + -> std::vector +{ + std::vector points; + for (float x = center.x() - unit_x * (unit_num_x / 2.f); + x <= center.x() + unit_x * (unit_num_x / 2.f); x += unit_x) { + for (float y = center.y() - unit_y * (unit_num_y / 2.f); + y <= center.y() + unit_y * (unit_num_y / 2.f); y += unit_y) { + points.emplace_back(Point(x, y)); + } + } + return points; +} + } // namespace crane #endif // CRANE_BASICS__DDPS_HPP_ From f2264434278150c4b8c8b62f18a18ad4f9a62942 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 02:58:44 +0900 Subject: [PATCH 2/7] =?UTF-8?q?=E3=83=91=E3=82=B9=E3=82=B9=E3=82=B3?= =?UTF-8?q?=E3=82=A2=E3=82=92GameAnalysis=E3=81=AB=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_msgs/CMakeLists.txt | 1 + crane_msgs/msg/analysis/FloatWithID.msg | 2 ++ crane_msgs/msg/analysis/GameAnalysis.msg | 3 +++ crane_world_model_publisher/src/world_model_publisher.cpp | 8 ++++++++ 4 files changed, 14 insertions(+) create mode 100755 crane_msgs/msg/analysis/FloatWithID.msg diff --git a/crane_msgs/CMakeLists.txt b/crane_msgs/CMakeLists.txt index f9888cb2..57a6e3a7 100755 --- a/crane_msgs/CMakeLists.txt +++ b/crane_msgs/CMakeLists.txt @@ -37,6 +37,7 @@ set(msg_files "msg/analysis/Kick.msg" "msg/analysis/Slack.msg" "msg/analysis/SlackWithPoint.msg" + "msg/analysis/FloatWithID.msg" "msg/planner/PassPlan.msg" "msg/planner/PassInfo.msg" diff --git a/crane_msgs/msg/analysis/FloatWithID.msg b/crane_msgs/msg/analysis/FloatWithID.msg new file mode 100755 index 00000000..e59b395d --- /dev/null +++ b/crane_msgs/msg/analysis/FloatWithID.msg @@ -0,0 +1,2 @@ +uint8 id +float32 value diff --git a/crane_msgs/msg/analysis/GameAnalysis.msg b/crane_msgs/msg/analysis/GameAnalysis.msg index 70ba9aa5..d4d67a36 100755 --- a/crane_msgs/msg/analysis/GameAnalysis.msg +++ b/crane_msgs/msg/analysis/GameAnalysis.msg @@ -5,3 +5,6 @@ Slack[] our_slack Slack[] their_slack float32 ball_horizon + +# sorted by score +FloatWithID[] pass_scores diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index efa1c0aa..05af6398 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -290,6 +290,14 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar return std::make_pair(robot, calc_score(robot->pose.pos)); }) | ranges::to(); + // larger score first + ranges::sort(score_with_bots, [](const auto & a, const auto & b) { return a.second > b.second; }); + + game_analysis_msg.pass_scores = score_with_bots | ranges::views::transform([](const auto & pair) { + crane_msgs::msg::FloatWithID msg; + return msg.set__id(pair.first->id).set__value(pair.second); + }) | + ranges::to(); world_model->update(game_analysis_msg); } From fc3cd74730d9a94febf2cd1bb746dc62ae33b0bf Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 02:59:05 +0900 Subject: [PATCH 3/7] =?UTF-8?q?=E3=83=9A=E3=83=8A=E3=83=AB=E3=83=86?= =?UTF-8?q?=E3=82=A3=E3=82=A8=E3=83=AA=E3=82=A2=E5=86=85=E3=81=AE=E3=83=91?= =?UTF-8?q?=E3=82=B9=E3=82=B9=E3=82=B3=E3=82=A2=E3=82=920=E3=81=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 05af6398..1ab8b0fd 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -267,6 +267,10 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar // パスラインに敵がいるときはスコアを下げる score *= 1.0 / (1.0 + nearest_enemy->distance); } + + if (world_model->point_checker.isPenaltyArea(p)) { + score = 0.0; + } return score; }; From 9a7b8a7014ad827a06e7dbbd8f1df6d13c6a0fc9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 02:59:21 +0900 Subject: [PATCH 4/7] =?UTF-8?q?=E3=83=81=E3=83=83=E3=83=97=E3=82=AD?= =?UTF-8?q?=E3=83=83=E3=82=AF=E3=83=9C=E3=83=BC=E3=83=8A=E3=82=B9=E3=81=AF?= =?UTF-8?q?1m=E3=81=84=E3=81=AA=E3=81=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 1ab8b0fd..88d4c506 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -260,7 +260,7 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar nearest_enemy) { // ボールから遠い敵がパスコースを塞いでいる場合は諦める if ( - nearest_enemy->robot->getDistance(world_model->ball.pos) > 2.0 && + nearest_enemy->robot->getDistance(world_model->ball.pos) > 1.0 && nearest_enemy->distance < 0.4) { score = 0.0; } From 389162421fe6a429af7d715bf9f4be386062e2e7 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 03:27:07 +0900 Subject: [PATCH 5/7] =?UTF-8?q?attacker=E3=81=A7=E8=A8=88=E7=AE=97?= =?UTF-8?q?=E3=81=97=E3=81=9Fpass=20score=E3=82=92=E4=BD=BF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 56 +++++------------------------ 1 file changed, 9 insertions(+), 47 deletions(-) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 24592d3b..e863fb91 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -217,58 +217,20 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) auto our_robots = world_model()->ours.getAvailableRobots(robot()->id, true); const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - // TODO(HansRobo): しっかりパス先を選定する - // int receiver_id = getParameter("receiver_id"); double best_score = 0.0; Point best_target; - int best_id = -1; - for (auto & our_robot : our_robots) { - // ゴールに近い味方は対象外 - if (our_robot->getDistance(world_model()->getOurGoalCenter()) < 3.0) { + for (const auto score_with_id : world_model()->getMsg().game_analysis.pass_scores) { + if (score_with_id.id != robot()->id) { continue; - } - Segment ball_to_target{world_model()->ball.pos, our_robot->pose.pos}; - auto target = our_robot->pose.pos; - double score = 1.0; - // パス先のゴールチャンスが大きい場合はスコアを上げる(30度以上で最大0.5上昇) - auto [best_angle, goal_angle_width] = - world_model()->getLargestGoalAngleRangeFromPoint(target); - score += std::clamp(goal_angle_width / (M_PI / 12.), 0.0, 0.5); - - // 敵ゴールに近いときはスコアを上げる - double normed_distance_to_their_goal = - ((target - world_model()->getTheirGoalCenter()).norm() - - (world_model()->field_size.x() * 0.5)) / - (world_model()->field_size.x() * 0.5); - // マイナスのときはゴールに近い - score *= (1.0 - normed_distance_to_their_goal); - - auto nearest_enemy = world_model()->getNearestRobotWithDistanceFromSegment( - ball_to_target, world_model()->theirs.getAvailableRobots()); - // ボールから遠い敵がパスコースを塞いでいる場合は諦める - if (nearest_enemy.has_value()) { - if ( - nearest_enemy->robot->getDistance(world_model()->ball.pos) > 1.0 && - nearest_enemy->distance < 0.4) { - score = 0.0; - } - // パスラインに敵がいるときはスコアを下げる - score *= 1.0 / (1.0 + nearest_enemy->distance); - } - - if (score > best_score) { - best_score = score; - best_target = target; - best_id = our_robot->id; + } else if (score_with_id.id != world_model()->getOurGoalieId()) { + continue; + } else { + kick_target = world_model()->getOurRobot(score_with_id.id)->pose.pos; + pass_receiver_id = score_with_id.id; + return true; } } - - auto ret = best_score > 0.5; - if (ret) { - kick_target = best_target; - pass_receiver_id = best_id; - } - return ret; + return false; }); addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { From 4ff9a7aef56d9e568ff6fef2fc58292c34199337 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:10:46 +0900 Subject: [PATCH 6/7] =?UTF-8?q?=E5=8F=AF=E8=A6=96=E5=8C=96=E3=81=8C?= =?UTF-8?q?=E9=87=8D=E3=81=9F=E3=81=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 88d4c506..e2dcc9af 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -288,7 +288,7 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar circle.center(pair.first).radius(pair.second * 0.05).stroke("red").strokeWidth(2.); pass_score_visualizer->add(circle.getSvgString()); }); - pass_score_visualizer->flush(); + // pass_score_visualizer->flush(); auto score_with_bots = our_robots | ranges::views::transform([&](const auto & robot) { return std::make_pair(robot, calc_score(robot->pose.pos)); From 7f9668563008410bdd30c91927cddad3f9910988 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 00:22:31 +0900 Subject: [PATCH 7/7] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E3=82=A2=E3=82=A6=E3=83=88=E3=81=AE=E4=BB=95=E6=96=B9=E3=81=8C?= =?UTF-8?q?=E6=82=AA=E3=81=8F=E3=80=81=E5=A4=A7=E9=87=8F=E3=81=AE=E3=83=A1?= =?UTF-8?q?=E3=83=A2=E3=83=AA=E3=82=92=E6=B6=88=E8=B2=BB=E3=81=97=E3=81=A6?= =?UTF-8?q?=E3=81=97=E3=81=BE=E3=81=A3=E3=81=A6=E3=81=84=E3=81=9F=E3=81=AE?= =?UTF-8?q?=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index e2dcc9af..02808575 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -286,9 +286,9 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar ranges::for_each(score_grid, [&](const auto & pair) { SvgCircleBuilder circle; circle.center(pair.first).radius(pair.second * 0.05).stroke("red").strokeWidth(2.); - pass_score_visualizer->add(circle.getSvgString()); + // pass_score_visualizer->add(circle.getSvgString()); }); - // pass_score_visualizer->flush(); + pass_score_visualizer->flush(); auto score_with_bots = our_robots | ranges::views::transform([&](const auto & robot) { return std::make_pair(robot, calc_score(robot->pose.pos));