total defenseでgoalieを有効化

ibis-ssl · Jan 4, 2025 · bf567e5 · bf567e5
1 parent 602b118
commit bf567e5
Showing 1 changed file with 19 additions and 9 deletions.
diff --git a/session/crane_planner_plugins/src/total_defense_planner.cpp b/session/crane_planner_plugins/src/total_defense_planner.cpp
@@ -16,6 +16,18 @@ TotalDefensePlanner::calculateRobotCommand(
     return {PlannerBase::Status::RUNNING, {}};
   }
 
+  std::vector<crane_msgs::msg::RobotCommand> robot_commands;
+
+  if (goalie) {
+    goalie->run();
+    robot_commands.emplace_back(goalie->getRobotCommand());
+  }
+
+  auto defender_robots = robots | ranges::views::filter([&](const auto & id) {
+                           return id.robot_id != world_model->getOurGoalieId();
+                         }) |
+                         ranges::to<std::vector>();
+
   auto ball = world_model->ball.pos;
   [[maybe_unused]] const double OFFSET_X = 0.2;
   [[maybe_unused]] const double OFFSET_Y = 0.2;
@@ -41,23 +53,22 @@ TotalDefensePlanner::calculateRobotCommand(
     if (
       world_model->getDistanceFromBall(world_model->getOurGoalCenter()) <
       world_model->field_size.y() * 0.5) {
-      return getDefenseLinePoints(robots.size(), ball_line);
+      return getDefenseLinePoints(defender_robots.size(), ball_line);
     } else {
-      return getDefenseArcPoints(robots.size(), ball_line);
+      return getDefenseArcPoints(defender_robots.size(), ball_line);
     }
   }();
 
   if (not defense_points.empty()) {
     std::vector<Point> robot_points;
-    for (auto robot_id : robots) {
+    for (auto robot_id : defender_robots) {
       robot_points.emplace_back(world_model->getRobot(robot_id)->pose.pos);
     }
 
     auto solution = getOptimalAssignments(robot_points, defense_points);
 
-    std::vector<crane_msgs::msg::RobotCommand> robot_commands;
-    for (auto robot_id = robots.begin(); robot_id != robots.end(); ++robot_id) {
-      int index = std::distance(robots.begin(), robot_id);
+    for (auto robot_id = defender_robots.begin(); robot_id != defender_robots.end(); ++robot_id) {
+      int index = std::distance(defender_robots.begin(), robot_id);
       Point target_point = defense_points[solution[index]];
 
       auto command = std::make_shared<crane::RobotCommandWrapperPosition>(
@@ -73,9 +84,8 @@ TotalDefensePlanner::calculateRobotCommand(
     }
     return {PlannerBase::Status::RUNNING, robot_commands};
   } else {
-    std::vector<crane_msgs::msg::RobotCommand> robot_commands;
-    for (auto robot_id = robots.begin(); robot_id != robots.end(); ++robot_id) {
-      int index = std::distance(robots.begin(), robot_id);
+    for (auto robot_id = defender_robots.begin(); robot_id != defender_robots.end(); ++robot_id) {
+      int index = std::distance(defender_robots.begin(), robot_id);
       [[maybe_unused]] Point target_point = [&]() {
         if (not defense_points.empty()) {
           return defense_points.at(index);