敵ロボットの数が少ない場合に例外を吐くバグを修正 (#528)

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

crane_local_planner/src/gridmap_planner.cpp

@@ -551,17 +551,20 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand(
 
         // ロボットに衝突しそうなときに速度を抑える
         {
-          auto [nearest_robot, nearest_robot_distance] =
-            world_model->getNearestRobotWithDistanceFromPoint(
-              robot->pose.pos, world_model->theirs.getAvailableRobots());
-
-          if (nearest_robot) {
-            Velocity relative_velocity = (robot->vel.linear - nearest_robot->vel.linear);
-            // 2m以内のロボットに対してx,y ともに近づいていて、速度が1.0m以上の場合、速度を1.0にする
-            if (
-              nearest_robot_distance < 2.0 && relative_velocity.x() > 0.0 &&
-              relative_velocity.y() > 0.0 && relative_velocity.norm() > 1.0) {
-              max_vel = std::max(1.0, max_vel * 0.5);
+          const auto & enemy_robots = world_model->theirs.getAvailableRobots();
+          if (not enemy_robots.empty()) {
+            auto [nearest_robot, nearest_robot_distance] =
+              world_model->getNearestRobotWithDistanceFromPoint(
+                robot->pose.pos, world_model->theirs.getAvailableRobots());
+
+            if (nearest_robot) {
+              Velocity relative_velocity = (robot->vel.linear - nearest_robot->vel.linear);
+              // 2m以内のロボットに対してx,y ともに近づいていて、速度が1.0m以上の場合、速度を1.0にする
+              if (
+                nearest_robot_distance < 2.0 && relative_velocity.x() > 0.0 &&
+                relative_velocity.y() > 0.0 && relative_velocity.norm() > 1.0) {
+                max_vel = std::max(1.0, max_vel * 0.5);
+              }
             }
           }
         }

crane_robot_skills/include/crane_robot_skills/goal_kick.hpp

@@ -57,11 +57,16 @@ class GoalKick : public SkillBase<RobotCommandWrapperPosition>
       double best_angle1, best_angle2;
       best_angle1 = best_angle - goal_angle_width / 2.0 + minimum_angle_accuracy;
       best_angle2 = best_angle + goal_angle_width / 2.0 - minimum_angle_accuracy;
-      Point their_goalie_pos =
-        world_model
-          ->getNearestRobotWithDistanceFromPoint(
-            world_model->getTheirGoalCenter(), world_model->theirs.getAvailableRobots())
-          .first->pose.pos;
+      Point their_goalie_pos = [&]() -> Point {
+        const auto & enemy_robots = world_model->theirs.getAvailableRobots();
+        if (not enemy_robots.empty()) {
+          return world_model
+            ->getNearestRobotWithDistanceFromPoint(world_model->getTheirGoalCenter(), enemy_robots)
+            .first->pose.pos;
+        } else {
+          return world_model->getTheirGoalCenter();
+        }
+      }();
       double their_goalie_angle = getAngle(their_goalie_pos - from_point);
       // 敵ゴールキーパーから角度差が大きい方を選択
       if (

crane_robot_skills/src/attacker.cpp

@@ -68,11 +68,13 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
           kick_skill.setParameter("kick_power", 0.8);
           Segment kick_line{world_model()->ball.pos, receiver->pose.pos};
           // 近くに敵ロボットがいればチップキック
-          const auto & [nearest_enemy, enemy_distance] =
-            world_model()->getNearestRobotWithDistanceFromSegment(
-              kick_line, world_model()->theirs.getAvailableRobots());
-          if (enemy_distance < 0.4 && nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) {
-            kick_skill.setParameter("kick_with_chip", true);
+          if (const auto enemy_robots = world_model()->theirs.getAvailableRobots();
+              not enemy_robots.empty()) {
+            const auto & [nearest_enemy, enemy_distance] =
+              world_model()->getNearestRobotWithDistanceFromSegment(kick_line, enemy_robots);
+            if (enemy_distance < 0.4 && nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) {
+              kick_skill.setParameter("kick_with_chip", true);
+            }
           }
           kick_skill.run(visualizer);
           break;
@@ -102,11 +104,11 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
 
   addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool {
     // 止まっているボールを相手が持っているとき
-    return not world_model()->isOurBallByBallOwnerCalculator() &&
+    const auto enemy_robots = world_model()->theirs.getAvailableRobots();
+    return not enemy_robots.empty() && not world_model()->isOurBallByBallOwnerCalculator() &&
            world_model()->ball.isStopped(0.1) &&
            world_model()
-               ->getNearestRobotWithDistanceFromPoint(
-                 world_model()->ball.pos, world_model()->theirs.getAvailableRobots())
+               ->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, enemy_robots)
                .second < 0.5;
   });
 
@@ -218,24 +220,19 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     }
 
     auto our_robots = world_model()->ours.getAvailableRobots(robot()->id);
+    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;
     for (auto & our_robot : our_robots) {
       Segment ball_to_target{world_model()->ball.pos, our_robot->pose.pos};
-      auto [nearest_enemy, enemy_distance] = world_model()->getNearestRobotWithDistanceFromSegment(
-        ball_to_target, world_model()->theirs.getAvailableRobots());
       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);
-      // ボールから遠い敵がパスコースを塞いでいる場合は諦める
-      if (nearest_enemy->getDistance(world_model()->ball.pos) > 1.0 && enemy_distance < 0.4) {
-        score = 0.0;
-      }
 
       // 敵ゴールに近いときはスコアを上げる
       double normed_distance_to_their_goal =
@@ -245,8 +242,17 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
       // マイナスのときはゴールに近い
       score *= (1.0 - normed_distance_to_their_goal);
 
-      // パスラインに敵がいるときはスコアを下げる
-      score *= 1.0 / (1.0 + enemy_distance);
+      if (not enemy_robots.empty()) {
+        auto [nearest_enemy, enemy_distance] =
+          world_model()->getNearestRobotWithDistanceFromSegment(
+            ball_to_target, world_model()->theirs.getAvailableRobots());
+        // ボールから遠い敵がパスコースを塞いでいる場合は諦める
+        if (nearest_enemy->getDistance(world_model()->ball.pos) > 1.0 && enemy_distance < 0.4) {
+          score = 0.0;
+        }
+        // パスラインに敵がいるときはスコアを下げる
+        score *= 1.0 / (1.0 + enemy_distance);
+      }
 
       if (score > best_score) {
         best_score = score;
@@ -266,25 +272,19 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     AttackerState::STANDARD_PASS,
     [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       auto our_robots = world_model()->ours.getAvailableRobots(robot()->id);
+      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;
       for (auto & our_robot : our_robots) {
         Segment ball_to_target{world_model()->ball.pos, our_robot->pose.pos};
-        auto [nearest_enemy, enemy_distance] =
-          world_model()->getNearestRobotWithDistanceFromSegment(
-            ball_to_target, world_model()->theirs.getAvailableRobots());
         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);
-        // ボールから遠い敵がパスコースを塞いでいる場合は諦める
-        if (nearest_enemy->getDistance(world_model()->ball.pos) > 1.0 && enemy_distance < 0.4) {
-          score = 0.0;
-        }
 
         // 敵ゴールに近いときはスコアを上げる
         double normed_distance_to_their_goal =
@@ -294,8 +294,17 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
         // マイナスのときはゴールに近い
         score *= (1.0 - normed_distance_to_their_goal);
 
-        // パスラインに敵がいるときはスコアを下げる
-        score *= 1.0 / (1.0 + enemy_distance);
+        if (not enemy_robots.empty()) {
+          auto [nearest_enemy, enemy_distance] =
+            world_model()->getNearestRobotWithDistanceFromSegment(
+              ball_to_target, world_model()->theirs.getAvailableRobots());
+          // ボールから遠い敵がパスコースを塞いでいる場合は諦める
+          if (nearest_enemy->getDistance(world_model()->ball.pos) > 1.0 && enemy_distance < 0.4) {
+            score = 0.0;
+          }
+          // パスラインに敵がいるときはスコアを下げる
+          score *= 1.0 / (1.0 + enemy_distance);
+        }
 
         if (score > best_score) {
           best_score = score;
@@ -307,10 +316,13 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
 
       kick_skill.setParameter("target", best_target);
       Segment ball_to_target{world_model()->ball.pos, best_target};
-      auto [nearest_enemy, enemy_distance] = world_model()->getNearestRobotWithDistanceFromSegment(
-        ball_to_target, world_model()->theirs.getAvailableRobots());
-      if (nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) {
-        kick_skill.setParameter("kick_with_chip", true);
+      if (not enemy_robots.empty()) {
+        auto [nearest_enemy, enemy_distance] =
+          world_model()->getNearestRobotWithDistanceFromSegment(
+            ball_to_target, world_model()->theirs.getAvailableRobots());
+        if (nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) {
+          kick_skill.setParameter("kick_with_chip", true);
+        }
       }
       kick_skill.setParameter("kick_power", 0.5);
       kick_skill.setParameter("dot_threshold", 0.97);
@@ -400,21 +412,16 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
 std::shared_ptr<RobotInfo> Attacker::selectPassReceiver()
 {
   auto our_robots = world_model()->ours.getAvailableRobots(robot()->id);
+  const auto enemy_robots = world_model()->theirs.getAvailableRobots();
   double best_score = 0.0;
   std::shared_ptr<RobotInfo> best_bot = nullptr;
   for (auto & our_robot : our_robots) {
     Segment ball_to_target{world_model()->ball.pos, our_robot->pose.pos};
-    auto [nearest_enemy, enemy_distance] = world_model()->getNearestRobotWithDistanceFromSegment(
-      ball_to_target, world_model()->theirs.getAvailableRobots());
     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);
-    // ボールから遠い敵がパスコースを塞いでいる場合は諦める
-    if (nearest_enemy->getDistance(world_model()->ball.pos) > 1.0 && enemy_distance < 0.4) {
-      score = 0.0;
-    }
 
     // 敵ゴールに近いときはスコアを上げる
     double normed_distance_to_their_goal = ((target - world_model()->getTheirGoalCenter()).norm() -
@@ -423,8 +430,16 @@ std::shared_ptr<RobotInfo> Attacker::selectPassReceiver()
     // マイナスのときはゴールに近い
     score *= (1.0 - normed_distance_to_their_goal);
 
-    // パスラインに敵がいるときはスコアを下げる
-    score *= 1.0 / (1.0 + enemy_distance);
+    if (not enemy_robots.empty()) {
+      auto [nearest_enemy, enemy_distance] =
+        world_model()->getNearestRobotWithDistanceFromSegment(ball_to_target, enemy_robots);
+      // ボールから遠い敵がパスコースを塞いでいる場合は諦める
+      if (nearest_enemy->getDistance(world_model()->ball.pos) > 1.0 && enemy_distance < 0.4) {
+        score = 0.0;
+      }
+      // パスラインに敵がいるときはスコアを下げる
+      score *= 1.0 / (1.0 + enemy_distance);
+    }
 
     if (score > best_score) {
       best_score = score;

session/crane_planner_plugins/src/marker_planner.cpp

@@ -43,12 +43,14 @@ auto MarkerPlanner::getSelectedRobots(
     return a.second > b.second;
   });
 
-  for (int i = 0; i < selectable_robots_num; i++) {
-    auto enemy_robot = robots_and_goal_angles.at(i).first;
+  for (const auto & [enemy_robot, goal_angle_from_enemy] : robots_and_goal_angles) {
+    if (selected_robots.size() >= selectable_robots_num) {
+      break;
+    }
     if (not world_model->point_checker.isInOurHalf(enemy_robot->pose.pos)) {
       // 相手コートにいる敵ロボットはマークしない
       continue;
-    } else if (robots_and_goal_angles.at(i).second < 3.0 * M_PI / 180.) {
+    } else if (goal_angle_from_enemy < 3.0 * M_PI / 180.) {
       // シュートコースが狭い場合はマークしない
       continue;
     } else if ((enemy_robot->pose.pos - world_model->ball.pos).norm() < 1.0) {

utility/crane_msg_wrappers/src/world_model_wrapper.cpp

@@ -168,6 +168,9 @@ auto WorldModelWrapper::getNearestRobotWithDistanceFromPoint(
   const Point & point, const std::vector<std::shared_ptr<RobotInfo>> robots) const
   -> std::pair<std::shared_ptr<RobotInfo>, double>
 {
+  if (robots.empty()) {
+    throw std::runtime_error("getNearestRobotWithDistanceFromPoint: robots is empty");
+  }
   std::shared_ptr<RobotInfo> nearest_robot = nullptr;
   double min_sq_distance = std::numeric_limits<double>::max();
   for (const auto & robot : robots) {
@@ -187,6 +190,9 @@ auto WorldModelWrapper::getNearestRobotWithDistanceFromSegment(
   const Segment & segment, const std::vector<std::shared_ptr<RobotInfo>> robots) const
   -> std::pair<std::shared_ptr<RobotInfo>, double>
 {
+  if (robots.empty()) {
+    throw std::runtime_error("getNearestRobotWithDistanceFromSegment: robots is empty");
+  }
   std::shared_ptr<RobotInfo> nearest_robot = nullptr;
   double min_distance = std::numeric_limits<double>::max();
   for (const auto & robot : robots) {
@@ -316,6 +322,9 @@ auto WorldModelWrapper::getLargestGoalAngleRangeFromPoint(Point from) -> std::pa
 auto WorldModelWrapper::getLargestOurGoalAngleRangeFromPoint(
   Point from, std::vector<std::shared_ptr<RobotInfo>> robots) -> std::pair<double, double>
 {
+  if (robots.empty()) {
+    throw std::runtime_error("getLargestOurGoalAngleRangeFromPoint: robots is empty");
+  }
   Interval goal_range;
 
   auto goal_posts = getOurGoalPosts();