新しいボール所持判定を使って場面を切り替える (#518)

crane_msgs/msg/analysis/PlaySituation.msg

@@ -48,9 +48,10 @@ uint8 THEIR_INDIRECT_FREE = 26
 # uint8 THEIR_GOAL = 28 use halt
 uint8 THEIR_BALL_PLACEMENT = 29
 
-uint8 OUR_INPLAY = 50
-uint8 THEIR_INPLAY = 51
-uint8 AMBIGUOUS_INPLAY = 52
+uint8 INPLAY = 50
+uint8 OUR_INPLAY = 51
+uint8 THEIR_INPLAY = 52
+uint8 AMBIGUOUS_INPLAY = 53
 
 uint32 stage
 

crane_play_switcher/src/play_switcher.cpp

@@ -20,25 +20,6 @@ PlaySwitcher::PlaySwitcher(const rclcpp::NodeOptions & options)
 {
   world_model = std::make_shared<WorldModelWrapper>(*this);
 
-  world_model->addCallback([&]() {
-    using crane_msgs::msg::PlaySituation;
-    // INPLAYの詳細判定
-    if (
-      play_situation_msg.command >= PlaySituation::OUR_INPLAY &&
-      world_model->isBallPossessionStateChanged()) {
-      //      auto pre_command = play_situation_msg.command;
-      if (world_model->isOurBall()) {
-        play_situation_msg.command = PlaySituation::OUR_INPLAY;
-      } else if (world_model->isTheirBall()) {
-        play_situation_msg.command = PlaySituation::THEIR_INPLAY;
-      } else {
-        play_situation_msg.command = PlaySituation::AMBIGUOUS_INPLAY;
-      }
-      play_situation_msg.header.stamp = now();
-      play_situation_pub->publish(play_situation_msg);
-    }
-  });
-
   RCLCPP_INFO(get_logger(), "PlaySwitcher is constructed.");
 
   play_situation_pub = create_publisher<crane_msgs::msg::PlaySituation>("/play_situation", 10);
@@ -113,7 +94,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
       // FORCE_START
       //-----------------------------------//
       // FORCE_STARTはインプレイをONにするだけ
-      next_play_situation = PlaySituation::AMBIGUOUS_INPLAY;
+      next_play_situation = PlaySituation::INPLAY;
       inplay_command_info.reason = "RAWコマンド変化＆FORCE_START：強制的にINPLAYに突入";
     } else {
       //-----------------------------------//
@@ -157,7 +138,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
       // play_situation_msg.command == PlaySituation::OUR_PENALTY_START or
       play_situation_msg.command == PlaySituation::OUR_INDIRECT_FREE) {
       if (0.05 <= (last_command_changed_state.ball_position - world_model->ball.pos).norm()) {
-        next_play_situation = PlaySituation::AMBIGUOUS_INPLAY;
+        next_play_situation = PlaySituation::INPLAY;
         inplay_command_info.reason =
           "INPLAY判定：敵ボールが少なくとも0.05m動いた(移動量: " +
           std::to_string(
@@ -173,32 +154,21 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
     if (
       play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START &&
       10.0 <= (now() - last_command_changed_state.stamp).seconds()) {
-      next_play_situation = PlaySituation::AMBIGUOUS_INPLAY;
+      next_play_situation = PlaySituation::INPLAY;
       inplay_command_info.reason = "INPLAY判定：敵キックオフから10秒経過";
     }
     // フリーキックからN秒経過（N=5 @DivA, N=10 @DivB）
     if (
       play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE or
       play_situation_msg.command == PlaySituation::THEIR_INDIRECT_FREE) {
       if (30.0 <= (now() - last_command_changed_state.stamp).seconds()) {
-        next_play_situation = PlaySituation::AMBIGUOUS_INPLAY;
+        next_play_situation = PlaySituation::INPLAY;
         inplay_command_info.reason =
           "INPLAY判定：敵フリーキックからN秒経過（N=5 @DivA, N=10 @DivB)";
       }
     }
   }
 
-  // INPLAYの詳細判定
-  if (play_situation_msg.command == PlaySituation::AMBIGUOUS_INPLAY) {
-    if (world_model->isOurBall() && not world_model->isTheirBall()) {
-      play_situation_msg.command = PlaySituation::OUR_INPLAY;
-    } else if (not world_model->isOurBall() && world_model->isTheirBall()) {
-      play_situation_msg.command = PlaySituation::THEIR_INPLAY;
-    } else {
-      play_situation_msg.command = PlaySituation::AMBIGUOUS_INPLAY;
-    }
-  }
-
   // コマンドが更新されているかを調べる
   if (
     next_play_situation != std::nullopt &&

crane_robot_skills/src/attacker.cpp

@@ -216,8 +216,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   });
 
   addTransition(AttackerState::GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool {
-    // 敵にボールを奪われた
-    return not world_model()->isOurBallByBallOwnerCalculator();
+    // ボールが早い
+    return world_model()->ball.isMoving(1.0);
   });
 
   addStateFunction(
@@ -242,7 +242,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     });
 
   addTransition(AttackerState::ENTRY_POINT, AttackerState::STANDARD_PASS, [this]() -> bool {
-    if (robot()->getDistance(world_model()->ball.pos) > 1.0) {
+    if (robot()->getDistance(world_model()->ball.pos) > 1.0 or world_model()->ball.isStopped(1.0)) {
       return false;
     }
 
@@ -287,8 +287,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   });
 
   addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool {
-    // 敵にボールを奪われた
-    return not world_model()->isOurBallByBallOwnerCalculator();
+    // ボールが早い
+    return world_model()->ball.isMoving(1.0);
   });
 
   addStateFunction(

crane_robot_skills/src/steal_ball.cpp

@@ -63,7 +63,8 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base)
             world_model()->ball.pos +
             getVerticalVec(world_model()->ball.pos - robot()->pose.pos) * 0.3);
           // ロボット半径より近くに来れば急回転して刈り取れる
-          //          command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2);
+          // command.setTargetTheta(
+          //  getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2);
         } else {
           command.setDribblerTargetPosition(world_model()->ball.pos);
         }

crane_robot_skills/src/sub_attacker.cpp

@@ -77,9 +77,9 @@ Status SubAttacker::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer
     double best_score = 0.0;
     for (const auto & dpps_point : dpps_points) {
       double score = getPointScore(dpps_point, world_model()->ball.pos, world_model());
-      //      visualizer->addPoint(
-      //        dpps_point.x(), dpps_point.y(), std::clamp(static_cast<int>(score * 100), 0, 20), "blue",
-      //        1.);
+      // visualizer->addPoint(
+      //  dpps_point.x(), dpps_point.y(), std::clamp(static_cast<int>(score * 100), 0, 20), "blue",
+      // 1.);
 
       if (score > best_score) {
         best_score = score;

session/crane_planner_plugins/src/defender_planner.cpp

@@ -160,7 +160,7 @@ std::vector<Point> DefenderPlanner::getDefenseLinePoints(
   std::vector<Point> defense_points;
 
   if (auto defense_parameter = getDefenseLinePointParameter(ball_line)) {
-    double upper_parameter, lower_parameter;
+    double upper_parameter = *defense_parameter, lower_parameter = *defense_parameter;
 
     auto add_parameter = [&](double parameter) -> bool {
       const double OFFSET_X = 0.1, OFFSET_Y = 0.1;

session/crane_session_controller/config/normal.yaml

@@ -11,12 +11,14 @@ events:
     session: formation
   - event: OUR_KICKOFF_START
     session: OUR_KICKOFF_START
+  - event: INPLAY
+    session: OUR_INPLAY
   - event: OUR_INPLAY
-    session: ATTACK_TEST
+    session: OUR_INPLAY
   - event: THEIR_INPLAY
-    session: ATTACK_TEST
+    session: THEIR_INPLAY
   - event: AMBIGUOUS_INPLAY
-    session: ATTACK_TEST
+    session: THEIR_INPLAY
   - event: OUR_BALL_PLACEMENT
     session: OUR_BALL_PLACEMENT
   - event: THEIR_BALL_PLACEMENT

session/crane_session_controller/config/play_situation/ATTACK_TEST.yaml → session/crane_session_controller/config/play_situation/OUR_INPLAY.yaml

@@ -1,15 +1,15 @@
-name: ATTACK_TEST
-description: ATTACK_TEST
+name: OUR_INPLAY
+description: OUR_INPLAY
 sessions:
   - name: goalie_skill
     capacity: 1
   - name: attacker_skill
     capacity: 1
+  - name: sub_attacker_skill
+    capacity: 1
   - name: defender
     capacity: 2
   - name: marker
     capacity: 2
-  - name: sub_attacker_skill
-    capacity: 1
   - name: waiter
     capacity: 20

session/crane_session_controller/config/play_situation/THEIR_INPLAY.yaml

@@ -3,11 +3,13 @@ description: THEIR_INPLAY
 sessions:
   - name: goalie_skill
     capacity: 1
-  - name: attacker
-    capacity: 1
-  - name: sub_attacker_skill
+  - name: attacker_skill
     capacity: 1
   - name: defender
-    capacity: 1
+    capacity: 2
   - name: marker
+    capacity: 3
+  - name: sub_attacker_skill
+    capacity: 1
+  - name: waiter
     capacity: 20

session/crane_session_controller/src/crane_session_controller.cpp

@@ -234,6 +234,9 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
       if (robot_changed) {
         RCLCPP_INFO(get_logger(), "ロボットの数か変動していますので再割当を行います");
         request(play_situation.getSituationCommandText(), world_model->ours.getAvailableRobotIds());
+      } else if (world_model->isOurBallOwnerChanged() or world_model->isBallOwnerTeamChanged()) {
+        RCLCPP_INFO(get_logger(), "ボールオーナーが変更されたので再割当を行います");
+        request(play_situation.getSituationCommandText(), world_model->ours.getAvailableRobotIds());
       }
 
       for (const auto & planner : available_planners) {
@@ -275,14 +278,25 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
 void SessionControllerComponent::request(
   const std::string & situation, std::vector<uint8_t> selectable_robot_ids)
 {
-  auto map = robot_selection_priority_map.find(situation);
+  const std::string situation_str = [&]() -> std::string {
+    if (situation == "INPLAY") {
+      if (world_model->isOurBallByBallOwnerCalculator()) {
+        return "OUR_INPLAY";
+      } else {
+        return "THEIR_INPLAY";
+      }
+    } else {
+      return situation;
+    }
+  }();
+  auto map = robot_selection_priority_map.find(situation_str);
   if (map == robot_selection_priority_map.end()) {
     RCLCPP_ERROR(
       get_logger(),
       "\t「%"
       "s」というSituationに対してロボット割当リクエストが発行されましたが，"
       "見つかりませんでした",
-      situation.c_str());
+      situation_str.c_str());
     return;
   }
 

utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp

@@ -297,6 +297,12 @@ struct WorldModelWrapper
       ball_owner_id_change_callback = callback;
     }
 
+    bool getIsOurBallOwnerChanged() const { return is_our_ball_owner_changed; }
+
+    bool getIsTheirBallOwnerChanged() const { return is_their_ball_owner_changed; }
+
+    bool getIsBallOwnerTeamChanged() const { return is_ball_owner_team_changed; }
+
   private:
     std::vector<RobotWithScore> sorted_our_robots;
 
@@ -306,6 +312,12 @@ struct WorldModelWrapper
 
     bool is_our_ball = false;
 
+    bool is_our_ball_owner_changed = false;
+
+    bool is_their_ball_owner_changed = false;
+
+    bool is_ball_owner_team_changed = false;
+
     std::uint8_t our_frontier = 255;
 
     std::function<void(bool)> ball_owner_team_change_callback = nullptr;
@@ -336,6 +348,21 @@ struct WorldModelWrapper
     return ball_owner_calculator.isOurBall();
   }
 
+  [[nodiscard]] auto isOurBallOwnerChanged() const
+  {
+    return ball_owner_calculator.getIsOurBallOwnerChanged();
+  }
+
+  [[nodiscard]] auto isTheirBallOwnerChanged() const
+  {
+    return ball_owner_calculator.getIsTheirBallOwnerChanged();
+  }
+
+  [[nodiscard]] auto isBallOwnerTeamChanged() const
+  {
+    return ball_owner_calculator.getIsBallOwnerTeamChanged();
+  }
+
   class PointChecker
   {
   public:

utility/crane_msg_wrappers/src/play_situation_wrapper.cpp

@@ -56,6 +56,7 @@ static std::map<int, std::string> situation_command_map = {
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, THEIR_INDIRECT_FREE),
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, OUR_BALL_PLACEMENT),
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, THEIR_BALL_PLACEMENT),
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, INPLAY),
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, OUR_INPLAY),
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, THEIR_INPLAY),
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, AMBIGUOUS_INPLAY)};

utility/crane_msg_wrappers/src/world_model_wrapper.cpp

@@ -460,15 +460,17 @@ auto WorldModelWrapper::BallOwnerCalculator::update() -> void
     }
   }());
 
-  if (is_our_ball_old != is_our_ball) {
+  is_ball_owner_team_changed = is_our_ball_old != is_our_ball;
+  if (is_ball_owner_team_changed) {
     std::cout << "ボールオーナーが" << (is_our_ball ? "我々" : "相手") << "チームに変更されました"
               << std::endl;
     if (ball_owner_team_change_callback) {
       ball_owner_team_change_callback(is_our_ball);
     }
   }
 
-  if (our_frontier_old != our_frontier) {
+  is_our_ball_owner_changed = our_frontier_old != our_frontier;
+  if (is_our_ball_owner_changed) {
     std::cout << "我々のボールオーナーが" << static_cast<int>(our_frontier_old) << "番から"
               << static_cast<int>(our_frontier) << "番に交代しました" << std::endl;
     if (ball_owner_id_change_callback) {