diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index 4ca602a0b..a408168a5 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/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; diff --git a/crane_msgs/msg/analysis/PlaySituation.msg b/crane_msgs/msg/analysis/PlaySituation.msg index 478a87d66..a63e92174 100755 --- a/crane_msgs/msg/analysis/PlaySituation.msg +++ b/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 diff --git a/crane_play_switcher/src/play_switcher.cpp b/crane_play_switcher/src/play_switcher.cpp index c0dbfd170..84e685dea 100644 --- a/crane_play_switcher/src/play_switcher.cpp +++ b/crane_play_switcher/src/play_switcher.cpp @@ -37,7 +37,8 @@ PlaySwitcher::PlaySwitcher(const rclcpp::NodeOptions & options) session_injection_sub = create_subscription( "/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 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(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(play_situation_msg.command)) { - play_situation_msg.command = next_play_situation.value(); + next_play_situation.value() != static_cast(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()); diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index f3f93388d..132170f1b 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/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) { diff --git a/crane_robot_skills/src/ball_nearby_positioner.cpp b/crane_robot_skills/src/ball_nearby_positioner.cpp index e1bdf277b..d7de10c2c 100644 --- a/crane_robot_skills/src/ball_nearby_positioner.cpp +++ b/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: diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 76a71c1ee..87d11af7e 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/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("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); diff --git a/crane_robot_skills/src/penalty_kick.cpp b/crane_robot_skills/src/penalty_kick.cpp index 5ccaa9ebe..3b7cf09ea 100644 --- a/crane_robot_skills/src/penalty_kick.cpp +++ b/crane_robot_skills/src/penalty_kick.cpp @@ -33,7 +33,7 @@ PenaltyKick::PenaltyKick(RobotCommandWrapperBase::SharedPtr & base) if (getParameter("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; } }); diff --git a/crane_speaker/src/crane_speaker_node.cpp b/crane_speaker/src/crane_speaker_node.cpp index 80ef688a6..6106e7766 100644 --- a/crane_speaker/src/crane_speaker_node.cpp +++ b/crane_speaker/src/crane_speaker_node.cpp @@ -46,8 +46,8 @@ class SpeakClient : public rclcpp::Node play_situation_sub = create_subscription( "/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]); } }); } diff --git a/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp b/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp index 8c9ccd77b..5eed69163 100644 --- a/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp +++ b/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()); diff --git a/session/crane_session_controller/include/crane_session_controller/session_controller.hpp b/session/crane_session_controller/include/crane_session_controller/session_controller.hpp index 64ee1f0bd..e24f17eb4 100644 --- a/session/crane_session_controller/include/crane_session_controller/session_controller.hpp +++ b/session/crane_session_controller/include/crane_session_controller/session_controller.hpp @@ -74,7 +74,7 @@ class SessionControllerComponent : public rclcpp::Node std::vector available_planners; - PlaySituationWrapper play_situation; + crane_msgs::msg::PlaySituation play_situation; rclcpp::TimerBase::SharedPtr timer; diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index 6535dac53..4dc380c32 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -100,12 +100,12 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions play_situation_sub = create_subscription( "/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("~/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; diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp index a6f76f037..dd87e92c3 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp +++ b/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 +#include #include #include #include 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; - static auto getRefereeCommandText(uint32_t id) -> std::string; +auto getRefereeCommandText(uint32_t id) -> std::string; - static auto getRefereeCommandTextList() -> std::vector; +auto getRefereeCommandNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt; - static auto getSituationCommandText(uint32_t id) -> std::string; +auto getRefereeCommandTextList() -> std::vector; - static auto getSituationCommandTextList() -> std::vector; +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; } // namespace crane #endif // CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_ diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index 49b590112..ce2867139 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/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 { diff --git a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp index 2dea7699d..25f0e5587 100644 --- a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp +++ b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp @@ -16,6 +16,22 @@ namespace crane { #define CMD_STRING_MAPPING(TYPE, CMD) {TYPE::CMD, #CMD} +static std::map stage_map = { + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_NORMAL_FIRST_HALF_PRE), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_NORMAL_FIRST_HALF), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_NORMAL_HALF_TIME), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_NORMAL_SECOND_HALF_PRE), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_NORMAL_SECOND_HALF), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_EXTRA_TIME_BREAK), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_EXTRA_FIRST_HALF_PRE), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_EXTRA_FIRST_HALF), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_EXTRA_HALF_TIME), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_EXTRA_SECOND_HALF_PRE), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_EXTRA_SECOND_HALF), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_PENALTY_SHOOTOUT_BREAK), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_PENALTY_SHOOTOUT), + CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, STAGE_POST_GAME)}; + static std::map referee_command_map = { CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, COMMAND_HALT), CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, COMMAND_STOP), @@ -66,43 +82,54 @@ static std::map situation_command_map = { 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 -{ - referee_command_raw.id = msg.command_raw; - referee_command_raw.text = referee_command_map[msg.command_raw]; +auto getStageText(uint32_t id) -> std::string { return stage_map[id]; } - situation_command.id = msg.command; - situation_command.text = situation_command_map[msg.command]; +auto getStageNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt +{ + crane_msgs::msg::NamedInt named_int; + named_int.name = getStageText(id); + named_int.value = id; + return named_int; +} - placement_position << msg.placement_position.x / 1000., msg.placement_position.y / 1000.; +auto getStageTextList() -> std::vector +{ + return stage_map | ranges::views::transform([](const auto & elem) { return elem.second; }) | + ranges::to(); } -auto PlaySituationWrapper::getRefereeCommandText(uint32_t id) -> std::string +auto getRefereeCommandText(uint32_t id) -> std::string { return referee_command_map[id]; } + +auto getRefereeCommandNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt { - return referee_command_map[id]; + crane_msgs::msg::NamedInt named_int; + named_int.name = getRefereeCommandText(id); + named_int.value = id; + return named_int; } -auto PlaySituationWrapper::getRefereeCommandTextList() -> std::vector +auto getRefereeCommandTextList() -> std::vector { - std::vector ret; - for (auto & [id, text] : referee_command_map) { - ret.push_back(text); - } - return ret; + return referee_command_map | + ranges::views::transform([](const auto & elem) { return elem.second; }) | + ranges::to(); } -auto PlaySituationWrapper::getSituationCommandText(uint32_t id) -> std::string +auto getSituationCommandText(uint32_t id) -> std::string { return situation_command_map[id]; } + +auto getSituationCommandNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt { - return situation_command_map[id]; + crane_msgs::msg::NamedInt named_int; + named_int.name = getSituationCommandText(id); + named_int.value = id; + return named_int; } -auto PlaySituationWrapper::getSituationCommandTextList() -> std::vector +auto getSituationCommandTextList() -> std::vector { - std::vector ret; - for (auto & [id, text] : situation_command_map) { - ret.push_back(text); - } - return ret; + return situation_command_map | + ranges::views::transform([](const auto & elem) { return elem.second; }) | + ranges::to(); } } // namespace crane diff --git a/utility/crane_msg_wrappers/src/world_model_wrapper.cpp b/utility/crane_msg_wrappers/src/world_model_wrapper.cpp index dadfacbd0..9ddf4dd5f 100644 --- a/utility/crane_msg_wrappers/src/world_model_wrapper.cpp +++ b/utility/crane_msg_wrappers/src/world_model_wrapper.cpp @@ -82,8 +82,6 @@ WorldModelWrapper::WorldModelWrapper(rclcpp::Node & node) void WorldModelWrapper::update(const crane_msgs::msg::WorldModel & world_model) { - play_situation.update(world_model.play_situation); - for (auto & our_robot : ours.robots) { our_robot->available = false; } @@ -244,13 +242,15 @@ auto WorldModelWrapper::PointChecker::isPenaltyArea(const Point & p, double offs auto WorldModelWrapper::getBallPlacementTarget() const -> std::optional { if ( - play_situation.getSituationCommandID() == crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT or - play_situation.getSituationCommandID() == + latest_msg.play_situation.command.value == crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT or + latest_msg.play_situation.command.value == crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT) { - return play_situation.placement_position; - } else { - return std::nullopt; + const auto designated_position = latest_msg.play_situation.referee_raw.designated_position; + if (not designated_position.empty()) { + return Point(designated_position.front().x / 1000., designated_position.front().y / 1000.); + } } + return std::nullopt; } auto WorldModelWrapper::getBallPlacementArea(const double offset) const -> std::optional