パススコア計算を共通化 (#745)

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"

crane_msgs/msg/analysis/FloatWithID.msg

@@ -0,0 +1,2 @@
+uint8 id
+float32 value

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

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<int>("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 {

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<std::deque<crane_msgs::msg::RobotInfo>, 20> friend_history;
 
   std::array<std::deque<crane_msgs::msg::RobotInfo>, 20> enemy_history;

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 <crane_basics/ddps.hpp>
 #include <crane_basics/geometry_operations.hpp>
 #include <crane_basics/time.hpp>
 #include <crane_world_model_publisher/world_model_publisher.hpp>
@@ -21,6 +22,9 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
   visualizer =
     std::make_unique<crane::CraneVisualizerBuffer::MessageBuilder>("world_model/trajectory");
 
+  pass_score_visualizer =
+    std::make_unique<crane::CraneVisualizerBuffer::MessageBuilder>("world_model/pass_score");
+
   declare_parameter("position_history_size", 200);
   get_parameter<int>("position_history_size", history_size);
 
@@ -233,6 +237,72 @@ 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) > 1.0 &&
+        nearest_enemy->distance < 0.4) {
+        score = 0.0;
+      }
+      // パスラインに敵がいるときはスコアを下げる
+      score *= 1.0 / (1.0 + nearest_enemy->distance);
+    }
+
+    if (world_model->point_checker.isPenaltyArea(p)) {
+      score = 0.0;
+    }
+    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<std::vector>();
+
+  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<std::vector>();
+  // 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<std::vector>();
+
   world_model->update(game_analysis_msg);
 }
 

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<Point>
+{
+  std::vector<Point> 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_