STOPをnext_commandで細分化して扱う

crane_msgs/msg/analysis/PlaySituation.msg

@@ -8,7 +8,6 @@
 # 13 : OUR_PENALTY_PREPARATION
 # 14 : OUR_PENALTY_START
 # 15 : OUR_DIRECT_FREE
-# 16 : OUR_INDIRECT_FREE
 # 17 : OUR_TIMEOUT
 # 18 : OUR_GOAL
 # 19 : OUR_BALL_PLACEMENT
@@ -18,7 +17,6 @@
 # 23 : THEIR_PENALTY_PREPARATION
 # 24 : THEIR_PENALTY_START
 # 25 : THEIR_DIRECT_FREE
-# 26 : THEIR_INDIRECT_FREE
 # 27 : THEIR_TIMEOUT
 # 28 : THEIR_GOAL
 # 29 : THEIR_BALL_PLACEMENT
@@ -33,7 +31,7 @@ uint8 OUR_KICKOFF_START = 12
 uint8 OUR_PENALTY_PREPARATION = 13
 uint8 OUR_PENALTY_START = 14
 uint8 OUR_DIRECT_FREE = 15
-uint8 OUR_INDIRECT_FREE = 16
+# uint8 OUR_INDIRECT_FREE = 16　現在は非推奨（使われていない）
 # uint8 OUR_TIMEOUT = 17  use halt
 # uint8 OUR_GOAL = 18 use halt
 uint8 OUR_BALL_PLACEMENT = 19
@@ -43,7 +41,7 @@ uint8 THEIR_KICKOFF_START = 22
 uint8 THEIR_PENALTY_PREPARATION = 23
 uint8 THEIR_PENALTY_START = 24
 uint8 THEIR_DIRECT_FREE = 25
-uint8 THEIR_INDIRECT_FREE = 26
+# uint8 THEIR_INDIRECT_FREE = 26 現在は非推奨（使われていない）
 # uint8 THEIR_TIMEOUT = 27  use halt
 # uint8 THEIR_GOAL = 28 use halt
 uint8 THEIR_BALL_PLACEMENT = 29
@@ -56,6 +54,30 @@ uint8 OUR_INPLAY = 51
 uint8 THEIR_INPLAY = 52
 uint8 AMBIGUOUS_INPLAY = 53
 
+
+# STOPの場合分け
+# 参考：https://github.com/RoboCup-SSL/ssl-game-controller/blob/master/internal/app/statemachine/change_gameevent.go#L329
+
+# DEFENDER_IN_DEFENSE_AREAの後のSTOP
+uint8 STOP_PRE_OUR_PENALTY_PREPARATION = 60
+uint8 STOP_PRE_THEIR_PENALTY_PREPARATION = 61
+
+# 得点後のSTOP
+uint8 STOP_PRE_OUR_KICKOFF_PREPARATION = 62
+uint8 STOP_PRE_THEIR_KICKOFF_PREPARATION = 63
+
+# 以下のいずれかのイベント後のSTOP。ボールプレイスメントが挟まるかも
+# BALL_LEFT_FIELD_GOAL_LINE / BALL_LEFT_FIELD_TOUCH_LINE / AIMLESS_KICK
+# ATTACKER_TOO_CLOSE_TO_DEFENSE_AREA / BOT_PUSHED_BOT / BOT_HELD_BALL_DELIBERATELY
+# BOT_TIPPED_OVER / BOT_DROPPED_PARTS / KEEPER_HELD_BALL / BOUNDARY_CROSSING
+# BOT_DRIBBLED_BALL_TOO_FAR / ATTACKER_DOUBLE_TOUCHED_BALL / PENALTY_KICK_FAILED
+# INVALID_GOAL
+uint8 STOP_PRE_OUR_DIRECT_FREE = 64
+uint8 STOP_PRE_THEIR_DIRECT_FREE = 65
+
+# NO_PROGRESS_IN_GAME / TOO_MANY_ROBOTS イベント後のSTOP
+uint8 STOP_PRE_FORCE_START = 66
+
 uint32 stage
 
 uint32 command_raw

crane_play_switcher/src/play_switcher.cpp

@@ -104,9 +104,43 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
       // FORCE_STARTはインプレイをONにするだけ
       next_play_situation = PlaySituation::INPLAY;
       inplay_command_info.reason = "RAWコマンド変化＆FORCE_START：強制的にINPLAYに突入";
+    } else if (msg.command == Referee::COMMAND_STOP) {
+      //-----------------------------------//
+      // STOP
+      //-----------------------------------//
+      static std::map<int, int> stop_command_map = [&]() {
+#define NEXT_CMD_MAPPING(is_yellow, NEXT_RAW_CMD, CMD)                                             \
+  if (is_yellow) {                                                                                 \
+    command_map[Referee::COMMAND_##NEXT_RAW_CMD##_YELLOW] = {PlaySituation::STOP_PRE_OUR_##CMD};   \
+    command_map[Referee::COMMAND_##NEXT_RAW_CMD##_BLUE] = {PlaySituation::STOP_PRE_THEIR_##CMD};   \
+  } else {                                                                                         \
+    command_map[Referee::COMMAND_##NEXT_RAW_CMD##_YELLOW] = {PlaySituation::STOP_PRE_THEIR_##CMD}; \
+    command_map[Referee::COMMAND_##NEXT_RAW_CMD##_BLUE] = {PlaySituation::STOP_PRE_OUR_##CMD};     \
+  }
+        std::map<int, int> command_map;
+        bool is_yellow = msg.yellow.name == team_name;
+        NEXT_CMD_MAPPING(is_yellow, PREPARE_PENALTY, PENALTY_PREPARATION);
+        NEXT_CMD_MAPPING(is_yellow, PREPARE_KICKOFF, KICKOFF_PREPARATION);
+        NEXT_CMD_MAPPING(is_yellow, DIRECT_FREE, DIRECT_FREE);
+
+#undef NEXT_CMD_MAPPING
+
+        command_map[Referee::COMMAND_FORCE_START] = {PlaySituation::STOP_PRE_FORCE_START};
+
+        return command_map;
+      }();
+
+      if (
+        not msg.next_command.empty() &&
+        stop_command_map.find(msg.next_command.front()) != stop_command_map.end()) {
+        next_play_situation = stop_command_map.find(msg.next_command.front())->second;
+        inplay_command_info.reason = "RAWコマンド変化 & STOP：STOPの場合分け";
+      } else {
+        next_play_situation = PlaySituation::STOP;
+      }
     } else {
       //-----------------------------------//
-      // その他：HALT/STOP/KICKOFF/PENALTY/DIRECT/INDIRECT/PLACEMENT
+      // その他：HALT/STOP/KICKOFF/PENALTY/DIRECT/PLACEMENT
       //-----------------------------------//
       // raw command -> crane command
       std::map<int, int> command_map;
@@ -121,7 +155,6 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
       CMD_MAPPING(is_yellow, PREPARE_KICKOFF, KICKOFF_PREPARATION)
       CMD_MAPPING(is_yellow, PREPARE_PENALTY, PENALTY_PREPARATION)
       CMD_MAPPING(is_yellow, DIRECT_FREE, DIRECT_FREE)
-      CMD_MAPPING(is_yellow, INDIRECT_FREE, INDIRECT_FREE)
       CMD_MAPPING(is_yellow, BALL_PLACEMENT, BALL_PLACEMENT)
 
       next_play_situation = command_map[msg.command];
@@ -137,14 +170,13 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
     if (
       play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START or
       play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE or
-      play_situation_msg.command == PlaySituation::THEIR_INDIRECT_FREE or
       // 敵PKのINPLAYはOUR_PENALTY_STARTとして実装しているのでINPLAY遷移はしない
       // play_situation_msg.command == PlaySituation::THEIR_PENALTY_START or
       play_situation_msg.command == PlaySituation::OUR_KICKOFF_START or
-      play_situation_msg.command == PlaySituation::OUR_DIRECT_FREE or
+      play_situation_msg.command == PlaySituation::OUR_DIRECT_FREE
       // 味方PKのINPLAYはOUR_PENALTY_STARTとして実装しているのでINPLAY遷移はしない
-      // play_situation_msg.command == PlaySituation::OUR_PENALTY_START or
-      play_situation_msg.command == PlaySituation::OUR_INDIRECT_FREE) {
+      // play_situation_msg.command == PlaySituation::OUR_PENALTY_START
+    ) {
       if (0.05 <= (last_command_changed_state.ball_position - world_model->ball.pos).norm()) {
         next_play_situation = PlaySituation::INPLAY;
         inplay_command_info.reason =
@@ -166,9 +198,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
       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 (play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE) {
       if (30.0 <= (now() - last_command_changed_state.stamp).seconds()) {
         next_play_situation = PlaySituation::INPLAY;
         inplay_command_info.reason =

crane_robot_skills/src/attacker.cpp

@@ -31,7 +31,6 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     auto game_command = world_model()->play_situation.getSituationCommandID();
     if (
       game_command == crane_msgs::msg::PlaySituation::OUR_DIRECT_FREE ||
-      game_command == crane_msgs::msg::PlaySituation::OUR_INDIRECT_FREE ||
       game_command == crane_msgs::msg::PlaySituation::OUR_KICKOFF_START) {
       auto best_receiver = selectPassReceiver();
       forced_pass_receiver_id = best_receiver->id;

crane_robot_skills/src/ball_nearby_positioner.cpp

@@ -30,8 +30,6 @@ Status BallNearByPositioner::update()
     switch (situation) {
       case crane_msgs::msg::PlaySituation::THEIR_DIRECT_FREE:
         return 0.5;
-      case crane_msgs::msg::PlaySituation::THEIR_INDIRECT_FREE:
-        return 0.5;
       case crane_msgs::msg::PlaySituation::STOP:
         return 0.5;
       case crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT:

crane_speaker/src/crane_speaker_node.cpp

@@ -20,14 +20,12 @@ map play_situation_map = {
   {crane_msgs::msg::PlaySituation::OUR_PENALTY_PREPARATION, "味方PK準備"},
   {crane_msgs::msg::PlaySituation::OUR_PENALTY_START, "味方PK開始"},
   {crane_msgs::msg::PlaySituation::OUR_DIRECT_FREE, "味方フリーキック"},
-  {crane_msgs::msg::PlaySituation::OUR_INDIRECT_FREE, "味方インダイレクトフリーキック"},
   {crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT, "味方ボールプレイスメント"},
   {crane_msgs::msg::PlaySituation::THEIR_KICKOFF_PREPARATION, "敵キックオフ準備"},
   {crane_msgs::msg::PlaySituation::THEIR_KICKOFF_START, "敵キックオフ開始"},
   {crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION, "敵PK準備"},
   {crane_msgs::msg::PlaySituation::THEIR_PENALTY_START, "敵PK開始"},
   {crane_msgs::msg::PlaySituation::THEIR_DIRECT_FREE, "敵フリーキック"},
-  {crane_msgs::msg::PlaySituation::THEIR_INDIRECT_FREE, "敵インダイレクトフリーキック"},
   {crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT, "敵ボールプレイスメント"},
   {crane_msgs::msg::PlaySituation::OUR_INPLAY, "味方ボール"},
   {crane_msgs::msg::PlaySituation::THEIR_INPLAY, "敵ボール"},

session/crane_session_controller/config/normal.yaml

@@ -3,6 +3,20 @@ events:
     session: HALT
   - event: STOP
     session: STOP
+  - event: STOP_PRE_OUR_KICKOFF_PREPARATION
+    session: PRE_KICK_OFF
+  - event: STOP_PRE_THEIR_KICKOFF_PREPARATION
+    session: PRE_KICK_OFF
+  - event: STOP_PRE_OUR_PENALTY_PREPARATION
+    session: OUR_PENALTY_KICK
+  - event: STOP_PRE_THEIR_PENALTY_PREPARATION
+    session: THEIR_PENALTY_KICK
+  - event: STOP_PRE_OUR_DIRECT_FREE
+    session: STOP
+  - event: STOP_PRE_THEIR_DIRECT_FREE
+    session: STOP
+  - event: STOP_PRE_FORCE_START
+    session: STOP
   - event: THEIR_KICKOFF_PREPARATION
     session: formation
   - event: THEIR_KICKOFF_START

session/crane_session_controller/config/play_situation/PRE_KICK_OFF.yaml

@@ -0,0 +1,7 @@
+name: PRE_KICK_OFF
+description: PRE_KICK_OFF
+sessions:
+  - name: goalie_skill
+    capacity: 1
+  - name: formation
+    capacity: 20

utility/crane_msg_wrappers/src/play_situation_wrapper.cpp

@@ -52,15 +52,20 @@ static std::map<int, std::string> situation_command_map = {
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, THEIR_PENALTY_START),
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, OUR_DIRECT_FREE),
   CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, THEIR_DIRECT_FREE),
-  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, OUR_INDIRECT_FREE),
-  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, INJECTION),
   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)};
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, AMBIGUOUS_INPLAY),
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, STOP_PRE_OUR_PENALTY_PREPARATION),
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, STOP_PRE_THEIR_PENALTY_PREPARATION),
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, STOP_PRE_OUR_KICKOFF_PREPARATION),
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, STOP_PRE_THEIR_KICKOFF_PREPARATION),
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, STOP_PRE_OUR_DIRECT_FREE),
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, STOP_PRE_THEIR_DIRECT_FREE),
+  CMD_STRING_MAPPING(crane_msgs::msg::PlaySituation, STOP_PRE_FORCE_START)};
 
 auto PlaySituationWrapper::update(const crane_msgs::msg::PlaySituation & msg) -> void
 {