Receiver復活！

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/receive_planner.hpp

@@ -13,7 +13,9 @@
 
 #include "crane_game_analyzer/evaluations/evaluations.hpp"
 #include "crane_geometry/eigen_adapter.hpp"
+#include "crane_geometry/interval.hpp"
 #include "crane_msg_wrappers/world_model_wrapper.hpp"
+#include "crane_msg_wrappers/robot_command_wrapper.hpp"
 #include "crane_msgs/msg/pass_info.hpp"
 #include "crane_msgs/msg/receiver_plan.hpp"
 #include "crane_msgs/msg/world_model.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,58 @@ 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.5, 8);
+
+    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;
+      }
+
+      const double score = getLargestGoalAngleWidthFromPosition(dpps_point);
+      if (score > best_score) {
+        best_score = score;
+        best_position = dpps_point;
+      }
+    }
+
+    std::vector<crane_msgs::msg::RobotCommand> commands;
+    for(const auto & robot: robots){
+      crane::RobotCommandWrapper target(robot.robot_id, world_model);
+      target.setTargetPosition(best_position);
+      target.setTargetTheta(getAngle(world_model->ball.pos - best_position));
+      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 +205,29 @@ 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;
+  }
+
   std::vector<std::pair<double, Point>> getPositionsWithScore(Segment ball_line, Point next_target)
   {
     auto points = getPoints(ball_line, 0.05);
@@ -181,6 +263,21 @@ 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);
+    }), 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);
   }