Feature/receive planner

crane_bringup/launch/play_switcher.launch.py

@@ -176,6 +176,10 @@ def generate_launch_description():
                    # output="screen"
                    )
 
+    receive = Node(package="crane_planner_plugins", executable="receive_node",
+                  # output="screen"
+                  )
+
     play_switcher = Node(
         package="crane_play_switcher",
         executable="play_switcher_node",
@@ -226,6 +230,7 @@ def generate_launch_description():
             kickoff,
             marker,
             attacker,
+            receive,
             world_model_publisher,
             play_switcher,
             visualizer,

crane_bringup/launch/play_switcher_y.launch.py

@@ -178,6 +178,10 @@ def generate_launch_description():
                   # output="screen"
                   )
 
+    receive = Node(package="crane_planner_plugins", executable="receive_node",
+                   # output="screen"
+                   )
+
     play_switcher = Node(
         package="crane_play_switcher", executable="play_switcher_node", output="screen"
     )
@@ -226,6 +230,7 @@ def generate_launch_description():
             goalie,
             kickoff,
             attacker,
+            receive,
             world_model_publisher,
             play_switcher,
             visualizer,

session/crane_planner_plugins/CMakeLists.txt

@@ -44,10 +44,11 @@ add_planner_plugin(
   CLASS_NAME "crane::BallPlacementPlanner"
 )
 
-#add_planner_plugin(
-#  NAME "receive"
-#  CLASS_NAME "crane::ReceivePlanner"
-#)
+add_planner_plugin(
+  NAME "receive"
+  CLASS_NAME "crane::ReceivePlanner"
+)
+
 ament_auto_add_executable(receive_test_node src/receive/test.cpp)
 ament_target_dependencies(receive_test_node "Python3")
 

session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp

@@ -40,7 +40,20 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase
       crane::RobotCommandWrapper target(robot_id.robot_id, world_model);
       auto robot = world_model->getRobot(robot_id);
 
-      auto best_target = getBestShootTarget();
+      auto [best_target, goal_angle_width] = getBestShootTargetWithWidth();
+
+      // シュートの隙がないときは仲間へパス
+      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 nearest_robot =
+          world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots);
+        best_target = nearest_robot.first->pose.pos;
+      }
 
       // 経由ポイント
 
@@ -57,7 +70,7 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase
         target.enableCollisionAvoidance();
       } else {
         target.setTargetPosition(world_model->ball.pos);
-        target.kickStraight(1.0).disableCollisionAvoidance();
+        target.kickStraight(0.7).disableCollisionAvoidance();
         target.enableCollisionAvoidance();
       }
 
@@ -88,7 +101,7 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase
       });
   }
 
-  auto getBestShootTarget() -> Point
+  auto getBestShootTargetWithWidth() -> std::pair<Point, double>
   {
     const auto & ball = world_model->ball.pos;
 
@@ -108,12 +121,14 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase
       goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
     }
 
-    auto largetst_interval = goal_range.getLargestInterval();
-    std::cout << "interval width: " << largetst_interval.second - largetst_interval.first
+    auto largest_interval = goal_range.getLargestInterval();
+    std::cout << "interval width: " << largest_interval.second - largest_interval.first
               << std::endl;
-    double target_angle = (largetst_interval.first + largetst_interval.second) / 2.0;
+    double target_angle = (largest_interval.first + largest_interval.second) / 2.0;
 
-    return ball + Point(cos(target_angle), sin(target_angle)) * 0.5;
+    return {
+      ball + Point(cos(target_angle), sin(target_angle)) * 0.5,
+      largest_interval.second - largest_interval.first};
   }
 };
 

session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp

@@ -13,6 +13,8 @@
 
 #include "crane_game_analyzer/evaluations/evaluations.hpp"
 #include "crane_geometry/eigen_adapter.hpp"
+#include "crane_geometry/interval.hpp"
+#include "crane_msg_wrappers/robot_command_wrapper.hpp"
 #include "crane_msg_wrappers/world_model_wrapper.hpp"
 #include "crane_msgs/msg/pass_info.hpp"
 #include "crane_msgs/msg/receiver_plan.hpp"
@@ -27,8 +29,12 @@ namespace crane
  * ReceivePlannerは現在進行形でパスされているボールに対して，
  * ボールを受けるロボット(passer),その次にボールを受けるロボット(receiver)を指定するだけで
  * 最適なパス地点を計算し，その2台に対する指令を生成するプランナーです
+ *
+ * 何もないとき：パスの通る隙の多い地点に陣取る
+ * ボールが向かってきているとき：最適なパス地点を計算し，その地点に向かう
+ *
  */
-class ReceivePlanner : public rclcpp::Node
+class ReceivePlanner : public rclcpp::Node, public PlannerBase
 {
 public:
   enum class ReceivePhase {
@@ -53,8 +59,9 @@ class ReceivePlanner : public rclcpp::Node
 
   COMPOSITION_PUBLIC
   explicit ReceivePlanner(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
-  : rclcpp::Node("receive_planner", options)
+  : rclcpp::Node("receive_planner", options), PlannerBase("receive", *this)
   {
+    RCLCPP_INFO(get_logger(), "initializing");
     pass_info_pub = create_publisher<crane_msgs::msg::PassInfo>("path_info", 1);
     using namespace std::placeholders;
     pass_req_service = create_service<crane_msgs::srv::PassRequest>(
@@ -64,6 +71,96 @@ class ReceivePlanner : public rclcpp::Node
       [this](void) -> void { pass_info.world_model = world_model->getMsg(); });
   }
 
+  std::vector<crane_msgs::msg::RobotCommand> calculateControlTarget(
+    const std::vector<RobotIdentifier> & robots) override
+  {
+    auto dpps_points = getDPPSPoints(world_model->ball.pos, 0.25, 16);
+
+    std::vector<crane_msgs::msg::RobotCommand> commands;
+    for (const auto & robot : robots) {
+      auto robot_info = world_model->getRobot(robot);
+      crane::RobotCommandWrapper target(robot.robot_id, world_model);
+      // モード判断
+      //  こちらへ向かう速度成分
+      float ball_vel =
+        world_model->ball.vel.dot((robot_info->pose.pos - world_model->ball.pos).normalized());
+      target.kickStraight(0.7);
+      if (ball_vel > 0.5) {
+        Segment ball_line(
+          world_model->ball.pos,
+          (world_model->ball.pos + world_model->ball.vel.normalized() *
+                                     (world_model->ball.pos - robot_info->pose.pos).norm()));
+        //  ボールの進路上に移動
+        ClosestPoint result;
+        bg::closest_point(robot_info->pose.pos, ball_line, result);
+
+        // ゴールとボールの中間方向を向く
+        auto goal_angle = getLargestGoalAngleFromPosition(result.closest_point);
+        Vector2 to_goal{cos(goal_angle), sin(goal_angle)};
+        auto to_ball = (world_model->ball.pos - result.closest_point).normalized();
+        double intermediate_angle = getAngle(2 * to_goal + to_ball);
+        target.setTargetTheta(intermediate_angle);
+
+        // キッカーの中心のためのオフセット
+        target.setTargetPosition(
+          result.closest_point - (2 * to_goal + to_ball).normalized() * 0.055);
+
+      } else {
+        Point best_position;
+        double best_score = 0.0;
+        for (const auto & dpps_point : dpps_points) {
+          Segment line{world_model->ball.pos, dpps_point};
+          double closest_distance = [&]() -> double {
+            double closest_distance = std::numeric_limits<double>::max();
+            for (const auto & robot : world_model->theirs.getAvailableRobots()) {
+              ClosestPoint result;
+              bg::closest_point(robot->pose.pos, line, result);
+              if (result.distance < closest_distance) {
+                closest_distance = result.distance;
+              }
+            }
+            return closest_distance;
+          }();
+
+          if (closest_distance < 0.4) {
+            continue;
+          }
+
+          double score = getLargestGoalAngleWidthFromPosition(dpps_point);
+          const double dist = world_model->getDistanceFromRobot(robot, dpps_point);
+          score = score * (1.0 - dist / 10.0);
+
+          if (score > best_score) {
+            best_score = score;
+            best_position = dpps_point;
+          }
+        }
+        target.setTargetPosition(best_position);
+        //        target.setTargetTheta(getAngle(world_model->ball.pos - best_position));
+      }
+
+      // ゴールとボールの中間方向を向く
+      Point target_pos{target.latest_msg.target_x.front(), target.latest_msg.target_y.front()};
+      auto goal_angle = getLargestGoalAngleFromPosition(target_pos);
+      Vector2 to_goal{cos(goal_angle), sin(goal_angle)};
+      auto to_ball = (world_model->ball.pos - target_pos).normalized();
+      target.setTargetTheta(getAngle(to_goal + to_ball));
+
+      commands.push_back(target.getMsg());
+    }
+
+    return commands;
+  }
+  auto getSelectedRobots(
+    uint8_t selectable_robots_num, const std::vector<uint8_t> & selectable_robots)
+    -> std::vector<uint8_t> override
+  {
+    return this->getSelectedRobotsByScore(
+      selectable_robots_num, selectable_robots, [this](const std::shared_ptr<RobotInfo> & robot) {
+        return 100. / world_model->getSquareDistanceFromRobotToBall({true, robot->id});
+      });
+  }
+
   void passRequestHandle(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<crane_msgs::srv::PassRequest::Request> request,
@@ -146,6 +243,51 @@ class ReceivePlanner : public rclcpp::Node
 
     return ret;
   }
+
+  auto getLargestGoalAngleWidthFromPosition(const Point point) -> double
+  {
+    Interval goal_range;
+
+    auto goal_posts = world_model->getTheirGoalPosts();
+    goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point));
+
+    for (auto & enemy : world_model->theirs.robots) {
+      double distance = (point - enemy->pose.pos).norm();
+      constexpr double MACHINE_RADIUS = 0.1;
+
+      double center_angle = getAngle(enemy->pose.pos - point);
+      double diff_angle =
+        atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));
+
+      goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
+    }
+
+    auto largest_interval = goal_range.getLargestInterval();
+    return largest_interval.second - largest_interval.first;
+  }
+
+  auto getLargestGoalAngleFromPosition(const Point point) -> double
+  {
+    Interval goal_range;
+
+    auto goal_posts = world_model->getTheirGoalPosts();
+    goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point));
+
+    for (auto & enemy : world_model->theirs.robots) {
+      double distance = (point - enemy->pose.pos).norm();
+      constexpr double MACHINE_RADIUS = 0.1;
+
+      double center_angle = getAngle(enemy->pose.pos - point);
+      double diff_angle =
+        atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));
+
+      goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
+    }
+
+    auto largest_interval = goal_range.getLargestInterval();
+    return (largest_interval.second + largest_interval.first) * 0.5;
+  }
+
   std::vector<std::pair<double, Point>> getPositionsWithScore(Segment ball_line, Point next_target)
   {
     auto points = getPoints(ball_line, 0.05);
@@ -181,6 +323,26 @@ class ReceivePlanner : public rclcpp::Node
     return points;
   }
 
+  std::vector<Point> getDPPSPoints(Point center, double r_resolution, int theta_div_num)
+  {
+    std::vector<Point> points;
+    for (int theta_index = 0; theta_index < theta_div_num; theta_index++) {
+      double theta = 2.0 * M_PI * theta_index / theta_div_num;
+      for (double r = r_resolution; r <= 10.0; r += r_resolution) {
+        points.emplace_back(Point(center.x() + r * cos(theta), center.y() + r * sin(theta)));
+      }
+    }
+    points.erase(
+      std::remove_if(
+        points.begin(), points.end(),
+        [&](const auto & point) {
+          return (not world_model->isFieldInside(point)) or world_model->isDefenseArea(point);
+        }),
+      points.end());
+
+    return points;
+  }
+
   double getPointScore(Point p, Point next_target)
   {
     double nearest_dist;

session/crane_planner_plugins/src/receive/receive_node.cpp

@@ -6,7 +6,7 @@
 
 #include <memory>
 
-#include "crane_planner{plugins/receive_planner.hpp"
+#include "crane_planner_plugins/receive_planner.hpp"
 
 int main(int argc, char * argv[])
 {

session/crane_planner_plugins/src/receive/test.cpp

@@ -14,8 +14,7 @@
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::NodeOptions options;
-  crane::ReceivePlanner receive_planner(options);
+  crane::ReceivePlanner receive_planner;
   receive_planner.session_info.receiver_id = 2;
   auto world_model = std::make_shared<crane_msgs::msg::WorldModel>();
   // ball

session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml

@@ -6,6 +6,8 @@ sessions:
     capacity: 1
   - name: "attacker"
     capacity: 1
+  - name: "receive"
+    capacity: 1
   - name: "defender"
     capacity: 4
   - name: "waiter"

session/crane_session_controller/src/crane_session_controller.cpp

@@ -25,6 +25,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
   session_planners["kickoff"] = std::make_shared<SessionModule>("kickoff");
   session_planners["attacker"] = std::make_shared<SessionModule>("attacker");
   session_planners["marker"] = std::make_shared<SessionModule>("marker");
+  session_planners["receive"] = std::make_shared<SessionModule>("receive");
   for (auto & planner : session_planners) {
     planner.second->construct(*this);
   }