PlaySituation.msgの列挙体をNamedIntで構成 (#719)

crane_local_planner/src/rvo2_planner.cpp

@@ -62,7 +62,7 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node)
 void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg)
 {
   if (
-    world_model->play_situation.getRefereeCommandID() ==
+    world_model->getMsg().play_situation.command_raw.value ==
     robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
     // 1.5m/sだとたまに超えるので1.0m/sにしておく
     for (int i = 0; i < 40; i++) {
@@ -149,7 +149,7 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
         max_vel = std::min(max_vel, max_vel_by_decel);
         max_vel = std::min(max_vel, max_vel_by_acc);
         if (
-          world_model->play_situation.getRefereeCommandID() ==
+          world_model->getMsg().play_situation.command_raw.value ==
           robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
           // 1.5m/sだとたまに超えるので1.0m/sにしておく
           max_vel = std::min(max_vel, 1.0);
@@ -259,7 +259,7 @@ crane_msgs::msg::RobotCommands RVO2Planner::calculateRobotCommand(
 {
   crane_msgs::msg::RobotCommands commands = msg;
   if (
-    world_model->play_situation.getRefereeCommandID() !=
+    world_model->getMsg().play_situation.command_raw.value !=
     robocup_ssl_msgs::msg::Referee::COMMAND_HALT) {
     overrideTargetPosition(commands);
   }
@@ -298,7 +298,7 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
         double SURROUNDING_OFFSET = 0.3;
         double PENALTY_AREA_OFFSET = 0.1;
         if (
-          world_model->play_situation.getRefereeCommandID() ==
+          world_model->getMsg().play_situation.command_raw.value ==
           robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
           PENALTY_AREA_OFFSET = 0.5;
           SURROUNDING_OFFSET = 0.6;

crane_msgs/msg/analysis/PlaySituation.msg

@@ -78,16 +78,22 @@ uint8 STOP_PRE_THEIR_DIRECT_FREE = 65
 # NO_PROGRESS_IN_GAME / TOO_MANY_ROBOTS イベント後のSTOP
 uint8 STOP_PRE_FORCE_START = 66
 
-uint32 stage
+NamedInt stage
 
-uint32 command_raw
+NamedInt command_raw
 
-uint32 command
+NamedInt command
 
-string referee_text
+NamedInt[<=1] next_command_raw
+
+NamedInt[<=1] next_command
 
 string reason_text
 
 geometry_msgs/Point placement_position
 
+robocup_ssl_msgs/TeamInfo our_team_info
+
+robocup_ssl_msgs/TeamInfo their_team_info
+
 robocup_ssl_msgs/Referee referee_raw

crane_play_switcher/src/play_switcher.cpp

@@ -37,7 +37,8 @@ PlaySwitcher::PlaySwitcher(const rclcpp::NodeOptions & options)
   session_injection_sub = create_subscription<std_msgs::msg::String>(
     "/session_injection", 1, [&](const std_msgs::msg::String & msg) {
       // イベント注入（次のレフェリーイベント発生まで有効）
-      play_situation_msg.command = crane_msgs::msg::PlaySituation::INJECTION;
+      play_situation_msg.command =
+        getSituationCommandNamedInt(crane_msgs::msg::PlaySituation::INJECTION);
       play_situation_msg.header.stamp = now();
       play_situation_pub->publish(play_situation_msg);
     });
@@ -80,8 +81,20 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
   std::optional<int> next_play_situation = std::nullopt;
 
   // TODO(HansRobo): robocup_ssl_msgs/msg/Refereeをもう少しわかりやすい形式にする必要あり
-  play_situation_msg.stage = msg.stage;
-  play_situation_msg.command_raw = msg.command;
+  play_situation_msg.stage = getStageNamedInt(msg.stage);
+  play_situation_msg.command_raw = getRefereeCommandNamedInt(msg.command);
+  play_situation_msg.next_command_raw.clear();
+  if (not msg.next_command.empty()) {
+    play_situation_msg.next_command_raw.push_back(
+      getRefereeCommandNamedInt(msg.next_command.front()));
+  }
+  if (bool is_yellow = msg.yellow.name == team_name; is_yellow) {
+    play_situation_msg.our_team_info = msg.yellow;
+    play_situation_msg.their_team_info = msg.blue;
+  } else {
+    play_situation_msg.our_team_info = msg.blue;
+    play_situation_msg.their_team_info = msg.yellow;
+  }
   play_situation_msg.referee_raw = msg;
 
   if (latest_raw_referee_command != static_cast<int>(msg.command)) {
@@ -95,7 +108,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
     //  start_command_map[PlaySituation::THEIR_KICKOFF_START] = {}
 
     if (msg.command == Referee::COMMAND_NORMAL_START) {
-      next_play_situation = start_command_map[play_situation_msg.command];
+      next_play_situation = start_command_map[play_situation_msg.command.value];
       inplay_command_info.reason =
         "RAWコマンド変化＆NORMAL_START：KICKOFF/"
         "PENALTYはPREPARATIONからSTARTに移行";
@@ -171,14 +184,14 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
 
     // キックオフ・フリーキック・ペナルティーキック開始後，ボールが少なくとも0.05m動いた
     if (
-      play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START or
-      play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE or
+      play_situation_msg.command.value == PlaySituation::THEIR_KICKOFF_START or
+      play_situation_msg.command.value == PlaySituation::THEIR_DIRECT_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
+      // play_situation_msg.command.value == PlaySituation::THEIR_PENALTY_START or
+      play_situation_msg.command.value == PlaySituation::OUR_KICKOFF_START or
+      play_situation_msg.command.value == PlaySituation::OUR_DIRECT_FREE
       // 味方PKのINPLAYはOUR_PENALTY_STARTとして実装しているのでINPLAY遷移はしない
-      // play_situation_msg.command == PlaySituation::OUR_PENALTY_START
+      // play_situation_msg.command.value == PlaySituation::OUR_PENALTY_START
     ) {
       if (0.05 <= (last_command_changed_state.ball_position - world_model->ball.pos).norm()) {
         next_play_situation = PlaySituation::INPLAY;
@@ -195,13 +208,13 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
 
     // キックオフから10秒経過
     if (
-      play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START &&
+      play_situation_msg.command.value == PlaySituation::THEIR_KICKOFF_START &&
       10.0 <= (now() - last_command_changed_state.stamp).seconds()) {
       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) {
+    if (play_situation_msg.command.value == PlaySituation::THEIR_DIRECT_FREE) {
       if (30.0 <= (now() - last_command_changed_state.stamp).seconds()) {
         next_play_situation = PlaySituation::INPLAY;
         inplay_command_info.reason =
@@ -213,17 +226,17 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
   // コマンドが更新されているかを調べる
   if (
     next_play_situation != std::nullopt &&
-    next_play_situation.value() != static_cast<int>(play_situation_msg.command)) {
-    play_situation_msg.command = next_play_situation.value();
+    next_play_situation.value() != static_cast<int>(play_situation_msg.command.value)) {
+    play_situation_msg.command = getSituationCommandNamedInt(next_play_situation.value());
     play_situation_msg.reason_text = inplay_command_info.reason;
 
     RCLCPP_INFO(get_logger(), "---");
     RCLCPP_INFO(
       get_logger(), "RAW_CMD      : %d (%s)", msg.command,
-      PlaySituationWrapper::getRefereeCommandText(msg.command).c_str());
+      getRefereeCommandText(msg.command).c_str());
     RCLCPP_INFO(
-      get_logger(), "INPLAY_CMD   : %d (%s)", play_situation_msg.command,
-      PlaySituationWrapper::getSituationCommandText(play_situation_msg.command).c_str());
+      get_logger(), "INPLAY_CMD   : %d (%s)", play_situation_msg.command.value,
+      play_situation_msg.command.name.c_str());
     RCLCPP_INFO(get_logger(), "REASON       : %s", inplay_command_info.reason.c_str());
     RCLCPP_INFO(
       get_logger(), "PREV_CMD_TIME: %f", (now() - last_command_changed_state.stamp).seconds());

crane_robot_skills/src/attacker.cpp

@@ -26,7 +26,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
 
   addTransition(AttackerState::ENTRY_POINT, AttackerState::FORCED_PASS, [this]() -> bool {
     // セットプレイのときは強制パス
-    auto game_command = world_model()->play_situation.getSituationCommandID();
+    auto game_command = world_model()->getMsg().play_situation.command.value;
     if (
       game_command == crane_msgs::msg::PlaySituation::OUR_DIRECT_FREE ||
       game_command == crane_msgs::msg::PlaySituation::OUR_KICKOFF_START) {

crane_robot_skills/src/ball_nearby_positioner.cpp

@@ -25,7 +25,7 @@ BallNearByPositioner::BallNearByPositioner(RobotCommandWrapperBase::SharedPtr &
 
 Status BallNearByPositioner::update()
 {
-  auto situation = world_model()->play_situation.getSituationCommandID();
+  auto situation = world_model()->getMsg().play_situation.command.value;
   double distance_from_ball = [&]() {
     switch (situation) {
       case crane_msgs::msg::PlaySituation::THEIR_DIRECT_FREE:

crane_robot_skills/src/goalie.cpp

@@ -20,7 +20,7 @@ Goalie::Goalie(RobotCommandWrapperBase::SharedPtr & base)
 
 Status Goalie::update()
 {
-  auto situation = world_model()->play_situation.getSituationCommandID();
+  auto situation = world_model()->getMsg().play_situation.command.value;
   if (getParameter<bool>("run_inplay")) {
     situation = crane_msgs::msg::PlaySituation::OUR_INPLAY;
   }
@@ -38,7 +38,7 @@ Status Goalie::update()
       break;
     default: {
       if (
-        world_model()->play_situation.getRefereeCommandID() ==
+        world_model()->getMsg().play_situation.command_raw.value ==
         robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
         // STOPのときにはボールを排出しない
         inplay(false);

crane_robot_skills/src/penalty_kick.cpp

@@ -33,7 +33,7 @@ PenaltyKick::PenaltyKick(RobotCommandWrapperBase::SharedPtr & base)
     if (getParameter<bool>("start_from_kick")) {
       return true;
     } else {
-      return world_model()->play_situation.getSituationCommandID() ==
+      return world_model()->getMsg().play_situation.command.value ==
              crane_msgs::msg::PlaySituation::OUR_PENALTY_START;
     }
   });

crane_speaker/src/crane_speaker_node.cpp

@@ -46,8 +46,8 @@ class SpeakClient : public rclcpp::Node
 
     play_situation_sub = create_subscription<crane_msgs::msg::PlaySituation>(
       "/play_situation", 10, [this](const crane_msgs::msg::PlaySituation::SharedPtr msg) {
-        if (play_situation_map.find(msg->command) != play_situation_map.end()) {
-          sendGoal(play_situation_map[msg->command]);
+        if (play_situation_map.find(msg->command.value) != play_situation_map.end()) {
+          sendGoal(play_situation_map[msg->command.value]);
         }
       });
   }

session/crane_planner_plugins/src/their_penalty_kick_planner.cpp

@@ -28,7 +28,7 @@ TheirPenaltyKickPlanner::calculateRobotCommand(
   }
   if (goalie) {
     if (
-      world_model->play_situation.getSituationCommandID() ==
+      world_model->getMsg().play_situation.command.value ==
       crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION) {
       auto & cmd = goalie->commander();
       cmd.setTargetPosition(world_model->getOurGoalCenter());

session/crane_session_controller/include/crane_session_controller/session_controller.hpp

@@ -74,7 +74,7 @@ class SessionControllerComponent : public rclcpp::Node
 
   std::vector<PlannerBase::SharedPtr> available_planners;
 
-  PlaySituationWrapper play_situation;
+  crane_msgs::msg::PlaySituation play_situation;
 
   rclcpp::TimerBase::SharedPtr timer;
 

session/crane_session_controller/src/crane_session_controller.cpp

@@ -100,12 +100,12 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
 
   play_situation_sub = create_subscription<crane_msgs::msg::PlaySituation>(
     "/play_situation", 1, [this](const crane_msgs::msg::PlaySituation & msg) {
+      play_situation = msg;
       // TODO(HansRobo): 実装
       if (not world_model_ready) {
         return;
       }
-      play_situation.update(msg);
-      assign(play_situation.getSituationCommandText());
+      assign(play_situation.command.name);
     });
 
   timer_process_time_pub = create_publisher<std_msgs::msg::Float32>("~/timer/process_time", 10);
@@ -116,7 +116,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
   timer = rclcpp::create_timer(this, get_clock(), 100ms, [&]() {
     ScopedTimer timer(timer_process_time_pub);
     PlannerContext planner_context;
-    auto it = event_map.find(play_situation.getSituationCommandText());
+    auto it = event_map.find(play_situation.command.name);
     if (it != event_map.end()) {
       try {
         request(it->second, world_model->ours.getAvailableRobotIds(), planner_context);
@@ -189,10 +189,10 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
     }();
 
     if (robot_changed) {
-      assign(play_situation.getSituationCommandText());
+      assign(play_situation.command.name);
     } else if (world_model->isOurBallOwnerChanged() or world_model->isBallOwnerTeamChanged()) {
       RCLCPP_INFO(get_logger(), "ボールオーナーが変更されたので再割当を行います");
-      assign(play_situation.getSituationCommandText());
+      assign(play_situation.command.name);
     }
 
     PlannerContext planner_context;

utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp

@@ -8,50 +8,29 @@
 #define CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_
 
 #include <crane_basics/boost_geometry.hpp>
+#include <crane_msgs/msg/named_int.hpp>
 #include <crane_msgs/msg/play_situation.hpp>
 #include <string>
 #include <vector>
 
 namespace crane
 {
-struct PlaySituationWrapper
-{
-  struct IDWithText
-  {
-    uint32_t id;
-
-    std::string text;
-  };
-
-  auto isInplay() const -> bool
-  {
-    return situation_command.id >= crane_msgs::msg::PlaySituation::OUR_INPLAY;
-  }
-
-  Point placement_position;
-
-  auto update(const crane_msgs::msg::PlaySituation & msg) -> void;
-
-  auto getRefereeCommandID() const -> uint32_t { return referee_command_raw.id; }
-
-  auto getRefereeCommandText() const -> std::string { return referee_command_raw.text; }
+auto getStageText(uint32_t id) -> std::string;
 
-  auto getSituationCommandID() const -> uint32_t { return situation_command.id; }
+auto getStageNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt;
 
-  auto getSituationCommandText() const -> std::string { return situation_command.text; }
+auto getStageTextList() -> std::vector<std::string>;
 
-  static auto getRefereeCommandText(uint32_t id) -> std::string;
+auto getRefereeCommandText(uint32_t id) -> std::string;
 
-  static auto getRefereeCommandTextList() -> std::vector<std::string>;
+auto getRefereeCommandNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt;
 
-  static auto getSituationCommandText(uint32_t id) -> std::string;
+auto getRefereeCommandTextList() -> std::vector<std::string>;
 
-  static auto getSituationCommandTextList() -> std::vector<std::string>;
+auto getSituationCommandText(uint32_t id) -> std::string;
 
-private:
-  IDWithText referee_command_raw;
+auto getSituationCommandNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt;
 
-  IDWithText situation_command;
-};
+auto getSituationCommandTextList() -> std::vector<std::string>;
 }  // namespace crane
 #endif  // CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_

utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp

@@ -279,8 +279,6 @@ struct WorldModelWrapper
 
   Ball ball;
 
-  PlaySituationWrapper play_situation;
-
 private:
   class BallOwnerCalculator
   {