Merge pull request #148 from ibis-ssl/delete_single_bot_planner

プランナの再編

crane_local_planner/include/crane_local_planner/simple_avoid_planner.hpp

@@ -125,9 +125,8 @@ class SimpleAvoidPlanner
 
     {
       // 味方ロボットを回避
-      for (const auto & r : world_model->ours.getAvailableRobots()) {
-        if (
-          robot->id != r->id && bg::distance(r->geometry(), latest_path) <= r->geometry().radius) {
+      for (const auto & r : world_model->ours.getAvailableRobots(robot->id)) {
+        if (bg::distance(r->geometry(), latest_path) <= r->geometry().radius) {
           auto avoidance_points_tmp =
             getAvoidancePoints(robot->pose.pos, r->geometry(), r->geometry().radius);
           avoidance_points.insert(
@@ -245,10 +244,8 @@ class SimpleAvoidPlanner
     // ただし、ゴールからたどっていく。
     {
       // 味方ロボットを回避
-      for (const auto & r : world_model->ours.getAvailableRobots()) {
-        if (
-          from_robot->id != r->id &&
-          bg::distance(r->geometry(), latest_path) <= r->geometry().radius) {
+      for (const auto & r : world_model->ours.getAvailableRobots(from_robot->id)) {
+        if (bg::distance(r->geometry(), latest_path) <= r->geometry().radius) {
           auto avoidance_points_tmp =
             getAvoidancePoints(to, r->geometry(), r->geometry().radius + 0.2);
           avoidance_points.insert(
@@ -433,8 +430,8 @@ class SimpleAvoidPlanner
         [&](Point p) {
           Segment path{from, p};
           // 味方ロボットとの干渉をチェック
-          for (const auto & r : world_model->ours.getAvailableRobots()) {
-            if (r->id != robot->id && bg::distance(r->geometry(), path) <= 0.0) {
+          for (const auto & r : world_model->ours.getAvailableRobots(robot->id)) {
+            if (bg::distance(r->geometry(), path) <= 0.0) {
               return true;
             }
           }

crane_robot_skills/include/crane_robot_skills/goalie.hpp

@@ -21,15 +21,11 @@ class Goalie : public SkillBase<>
   {
     auto ball = world_model->ball.pos;
     // パスできるロボットのリストアップ
-    auto passable_robot_list = world_model->ours.getAvailableRobots();
+    auto passable_robot_list = world_model->ours.getAvailableRobots(target.robot->id);
     passable_robot_list.erase(
       std::remove_if(
         passable_robot_list.begin(), passable_robot_list.end(),
         [&](const RobotInfo::SharedPtr & r) {
-          // 自分以外
-          if (target.robot->id == r->id) {
-            return true;
-          }
           // 敵に塞がれていたら除外
           Segment ball_to_robot_line(ball, r->pose.pos);
           for (const auto & enemy : world_model->theirs.getAvailableRobots()) {

crane_robot_skills/src/simple_attacker.cpp

@@ -21,12 +21,7 @@ SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptr<WorldModelWrapp
 
       // シュートの隙がないときは仲間へパス
       if (goal_angle_width < 0.07) {
-        auto our_robots = world_model->ours.getAvailableRobots();
-        our_robots.erase(
-          std::remove_if(
-            our_robots.begin(), our_robots.end(),
-            [&](const auto & our_robot) { return our_robot->id == robot->id; }),
-          our_robots.end());
+        auto our_robots = world_model->ours.getAvailableRobots(robot->id);
         auto nearest_robot =
           world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots);
         best_target = nearest_robot.first->pose.pos;

crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp

@@ -89,6 +89,8 @@ class WorldModelPublisherComponent : public rclcpp::Node
 
   Color their_color;
 
+  uint8_t our_goalie_id, their_goalie_id;
+
   uint8_t max_id;
 
   static constexpr float DISAPPEARED_TIME_THRESH = 3.0f;

crane_world_model_publisher/src/world_model_publisher.cpp

@@ -40,9 +40,13 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
       if (msg.yellow.name == team_name) {
         our_color = Color::YELLOW;
         their_color = Color::BLUE;
+        our_goalie_id = msg.yellow.goalkeeper;
+        their_goalie_id = msg.blue.goalkeeper;
       } else if (msg.blue.name == team_name) {
         our_color = Color::BLUE;
         their_color = Color::YELLOW;
+        our_goalie_id = msg.blue.goalkeeper;
+        their_goalie_id = msg.yellow.goalkeeper;
       } else {
         std::stringstream what;
         what << "Cannot find our team name, " << team_name << " in referee message. ";
@@ -61,6 +65,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
   if (initial_team_color == "BLUE") {
     our_color = Color::BLUE;
     their_color = Color::YELLOW;
+
   } else {
     our_color = Color::YELLOW;
     their_color = Color::BLUE;
@@ -189,6 +194,9 @@ void WorldModelPublisherComponent::publishWorldModel()
   wm.ball_placement_target.x = ball_placement_target_x;
   wm.ball_placement_target.y = ball_placement_target_y;
 
+  wm.our_goalie_id = our_goalie_id;
+  wm.their_goalie_id = their_goalie_id;
+
   pub_world_model->publish(wm);
 }
 

session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp

@@ -43,12 +43,7 @@ class AttackerPlanner : public PlannerBase
 
       // シュートの隙がないときは仲間へパス
       if (goal_angle_width < 0.07) {
-        auto our_robots = world_model->ours.getAvailableRobots();
-        our_robots.erase(
-          std::remove_if(
-            our_robots.begin(), our_robots.end(),
-            [&](const auto & robot) { return robot->id == robot_id.robot_id; }),
-          our_robots.end());
+        auto our_robots = world_model->ours.getAvailableRobots(robot_id.robot_id);
         auto nearest_robot =
           world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots);
         best_target = nearest_robot.first->pose.pos;

session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp

@@ -11,6 +11,7 @@
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/srv/robot_select.hpp>
 #include <crane_planner_base/planner_base.hpp>
+#include <crane_robot_skills/marker.hpp>
 #include <functional>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
@@ -33,21 +34,11 @@ class MarkerPlanner : public PlannerBase
     const std::vector<RobotIdentifier> & robots) override
   {
     std::vector<crane_msgs::msg::RobotCommand> robot_commands;
-    for (auto robot_id : robots) {
-      auto robot = world_model->getRobot(robot_id);
 
-      auto marked_robot = world_model->getTheirRobot(marking_target_map.at(robot_id.robot_id));
-      auto enemy_pos = marked_robot->pose.pos;
-      constexpr double INTERVAL = 0.5;
-
-      Point marking_point =
-        enemy_pos + (world_model->getOurGoalCenter() - enemy_pos).normalized() * INTERVAL;
-      double target_theta = getAngle(enemy_pos - world_model->getOurGoalCenter());
-
-      crane::RobotCommandWrapper target(robot_id.robot_id, world_model);
-      target.setTargetPosition(marking_point, target_theta);
-
-      robot_commands.emplace_back(target.getMsg());
+    for (auto & [id, value] : skill_map) {
+      auto & [command, skill] = value;
+      skill->run(*command, visualizer);
+      robot_commands.emplace_back(command->getMsg());
     }
     return {PlannerBase::Status::RUNNING, robot_commands};
   }
@@ -61,6 +52,7 @@ class MarkerPlanner : public PlannerBase
     }
 
     marking_target_map.clear();
+    skill_map.clear();
     std::vector<uint8_t> selected_robots;
     // 味方ゴールに近い敵ロボットをselectable_robots_num台選択
     std::vector<std::pair<std::shared_ptr<RobotInfo>, double>> robots_and_distances;
@@ -79,7 +71,7 @@ class MarkerPlanner : public PlannerBase
       uint8_t min_index = 0;
       for (int j = 0; j < selectable_robots.size(); j++) {
         double distance =
-          (enemy_robot->pose.pos - world_model->getOurRobot(selectable_robots[j])->pose.pos).norm();
+          world_model->getOurRobot(selectable_robots[j])->getDistance(enemy_robot->pose.pos);
         if (
           distance < min_distance &&
           std::count(selected_robots.begin(), selected_robots.end(), j) == 0) {
@@ -89,6 +81,14 @@ class MarkerPlanner : public PlannerBase
       }
       marking_target_map[selectable_robots[min_index]] = enemy_robot->id;
       selected_robots.push_back(selectable_robots[min_index]);
+      skill_map.emplace(
+        selectable_robots[min_index],
+        std::make_pair(
+          std::make_shared<RobotCommandWrapper>(selectable_robots[min_index], world_model),
+          std::make_shared<skills::Marker>(selectable_robots[min_index], world_model)));
+      skill_map[selectable_robots[min_index]].second->setParameter(
+        "marking_robot_id", enemy_robot->id);
+      skill_map[selectable_robots[min_index]].second->setParameter("mark_distance", 0.5);
     }
 
     return selected_robots;
@@ -97,6 +97,10 @@ class MarkerPlanner : public PlannerBase
 private:
   // key: ID of our robot in charge, value: ID of the enemy marked robot
   std::unordered_map<uint8_t, uint8_t> marking_target_map;
+
+  std::unordered_map<
+    uint8_t, std::pair<std::shared_ptr<RobotCommandWrapper>, std::shared_ptr<skills::Marker>>>
+    skill_map;
 };
 
 }  // namespace crane

session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp

@@ -25,12 +25,12 @@ class OurKickOffPlanner : public PlannerBase
 {
 private:
   std::shared_ptr<skills::KickoffAttack> kickoff_attack;
+
   std::shared_ptr<RobotCommandWrapper> attacker_command;
 
   std::shared_ptr<skills::KickoffSupport> kickoff_support;
-  std::shared_ptr<RobotCommandWrapper> supporter_command;
 
-  //  std::shared_ptr<ConsaiVisualizerWrapper> visualizer;
+  std::shared_ptr<RobotCommandWrapper> supporter_command;
 
 public:
   COMPOSITION_PUBLIC explicit OurKickOffPlanner(