Skip to content

Commit 5ce2e65

Browse files
HansRoboHansRobo
committed
その他移行
1 parent e7a5270 commit 5ce2e65

File tree

10 files changed

+46
-35
lines changed

10 files changed

+46
-35
lines changed

‎crane_local_planner/src/rvo2_planner.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,7 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node)
6262
void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg)
6363
{
6464
if (
65-
world_model->play_situation.getRefereeCommandID() ==
65+
world_model->getMsg().play_situation.command_raw.value ==
6666
robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
6767
// 1.5m/sだとたまに超えるので1.0m/sにしておく
6868
for (int i = 0; i < 40; i++) {
@@ -149,7 +149,7 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
149149
max_vel = std::min(max_vel, max_vel_by_decel);
150150
max_vel = std::min(max_vel, max_vel_by_acc);
151151
if (
152-
world_model->play_situation.getRefereeCommandID() ==
152+
world_model->getMsg().play_situation.command_raw.value ==
153153
robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
154154
// 1.5m/sだとたまに超えるので1.0m/sにしておく
155155
max_vel = std::min(max_vel, 1.0);
@@ -259,7 +259,7 @@ crane_msgs::msg::RobotCommands RVO2Planner::calculateRobotCommand(
259259
{
260260
crane_msgs::msg::RobotCommands commands = msg;
261261
if (
262-
world_model->play_situation.getRefereeCommandID() !=
262+
world_model->getMsg().play_situation.command_raw.value !=
263263
robocup_ssl_msgs::msg::Referee::COMMAND_HALT) {
264264
overrideTargetPosition(commands);
265265
}
@@ -298,7 +298,7 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
298298
double SURROUNDING_OFFSET = 0.3;
299299
double PENALTY_AREA_OFFSET = 0.1;
300300
if (
301-
world_model->play_situation.getRefereeCommandID() ==
301+
world_model->getMsg().play_situation.command_raw.value ==
302302
robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
303303
PENALTY_AREA_OFFSET = 0.5;
304304
SURROUNDING_OFFSET = 0.6;

‎crane_play_switcher/src/play_switcher.cpp

+28-17
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,8 @@ PlaySwitcher::PlaySwitcher(const rclcpp::NodeOptions & options)
3737
session_injection_sub = create_subscription<std_msgs::msg::String>(
3838
"/session_injection", 1, [&](const std_msgs::msg::String & msg) {
3939
// イベント注入（次のレフェリーイベント発生まで有効）
40-
play_situation_msg.command = crane_msgs::msg::PlaySituation::INJECTION;
40+
play_situation_msg.command =
41+
getSituationCommandNamedInt(crane_msgs::msg::PlaySituation::INJECTION);
4142
play_situation_msg.header.stamp = now();
4243
play_situation_pub->publish(play_situation_msg);
4344
});
@@ -80,8 +81,18 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
8081
std::optional<int> next_play_situation = std::nullopt;
8182

8283
// TODO(HansRobo): robocup_ssl_msgs/msg/Refereeをもう少しわかりやすい形式にする必要あり
83-
play_situation_msg.stage = msg.stage;
84-
play_situation_msg.command_raw = msg.command;
84+
play_situation_msg.stage = getStageNamedInt(msg.stage);
85+
play_situation_msg.command_raw = getRefereeCommandNamedInt(msg.command);
86+
if (not msg.next_command.empty()) {
87+
play_situation_msg.next_command_raw = getRefereeCommandNamedInt(msg.next_command.front());
88+
}
89+
if (bool is_yellow = msg.yellow.name == team_name; is_yellow) {
90+
play_situation_msg.our_team_info = msg.yellow;
91+
play_situation_msg.their_team_info = msg.blue;
92+
} else {
93+
play_situation_msg.our_team_info = msg.blue;
94+
play_situation_msg.their_team_info = msg.yellow;
95+
}
8596
play_situation_msg.referee_raw = msg;
8697

8798
if (latest_raw_referee_command != static_cast<int>(msg.command)) {
@@ -95,7 +106,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
95106
// start_command_map[PlaySituation::THEIR_KICKOFF_START] = {}
96107

97108
if (msg.command == Referee::COMMAND_NORMAL_START) {
98-
next_play_situation = start_command_map[play_situation_msg.command];
109+
next_play_situation = start_command_map[play_situation_msg.command.value];
99110
inplay_command_info.reason =
100111
"RAWコマンド変化＆NORMAL_START：KICKOFF/"
101112
"PENALTYはPREPARATIONからSTARTに移行";
@@ -171,14 +182,14 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
171182

172183
// キックオフ・フリーキック・ペナルティーキック開始後，ボールが少なくとも0.05m動いた
173184
if (
174-
play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START or
175-
play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE or
185+
play_situation_msg.command.value == PlaySituation::THEIR_KICKOFF_START or
186+
play_situation_msg.command.value == PlaySituation::THEIR_DIRECT_FREE or
176187
// 敵PKのINPLAYはOUR_PENALTY_STARTとして実装しているのでINPLAY遷移はしない
177-
// play_situation_msg.command == PlaySituation::THEIR_PENALTY_START or
178-
play_situation_msg.command == PlaySituation::OUR_KICKOFF_START or
179-
play_situation_msg.command == PlaySituation::OUR_DIRECT_FREE
188+
// play_situation_msg.command.value == PlaySituation::THEIR_PENALTY_START or
189+
play_situation_msg.command.value == PlaySituation::OUR_KICKOFF_START or
190+
play_situation_msg.command.value == PlaySituation::OUR_DIRECT_FREE
180191
// 味方PKのINPLAYはOUR_PENALTY_STARTとして実装しているのでINPLAY遷移はしない
181-
// play_situation_msg.command == PlaySituation::OUR_PENALTY_START
192+
// play_situation_msg.command.value == PlaySituation::OUR_PENALTY_START
182193
) {
183194
if (0.05 <= (last_command_changed_state.ball_position - world_model->ball.pos).norm()) {
184195
next_play_situation = PlaySituation::INPLAY;
@@ -195,13 +206,13 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
195206

196207
// キックオフから10秒経過
197208
if (
198-
play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START &&
209+
play_situation_msg.command.value == PlaySituation::THEIR_KICKOFF_START &&
199210
10.0 <= (now() - last_command_changed_state.stamp).seconds()) {
200211
next_play_situation = PlaySituation::INPLAY;
201212
inplay_command_info.reason = "INPLAY判定：敵キックオフから10秒経過";
202213
}
203214
// フリーキックからN秒経過（N=5 @DivA, N=10 @DivB）
204-
if (play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE) {
215+
if (play_situation_msg.command.value == PlaySituation::THEIR_DIRECT_FREE) {
205216
if (30.0 <= (now() - last_command_changed_state.stamp).seconds()) {
206217
next_play_situation = PlaySituation::INPLAY;
207218
inplay_command_info.reason =
@@ -213,17 +224,17 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
213224
// コマンドが更新されているかを調べる
214225
if (
215226
next_play_situation != std::nullopt &&
216-
next_play_situation.value() != static_cast<int>(play_situation_msg.command)) {
217-
play_situation_msg.command = next_play_situation.value();
227+
next_play_situation.value() != static_cast<int>(play_situation_msg.command.value)) {
228+
play_situation_msg.command = getSituationCommandNamedInt(next_play_situation.value());
218229
play_situation_msg.reason_text = inplay_command_info.reason;
219230

220231
RCLCPP_INFO(get_logger(), "---");
221232
RCLCPP_INFO(
222233
get_logger(), "RAW_CMD : %d (%s)", msg.command,
223-
PlaySituationWrapper::getRefereeCommandText(msg.command).c_str());
234+
getRefereeCommandText(msg.command).c_str());
224235
RCLCPP_INFO(
225-
get_logger(), "INPLAY_CMD : %d (%s)", play_situation_msg.command,
226-
PlaySituationWrapper::getSituationCommandText(play_situation_msg.command).c_str());
236+
get_logger(), "INPLAY_CMD : %d (%s)", play_situation_msg.command.value,
237+
play_situation_msg.command.name.c_str());
227238
RCLCPP_INFO(get_logger(), "REASON : %s", inplay_command_info.reason.c_str());
228239
RCLCPP_INFO(
229240
get_logger(), "PREV_CMD_TIME: %f", (now() - last_command_changed_state.stamp).seconds());

‎crane_robot_skills/src/attacker.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
2626

2727
addTransition(AttackerState::ENTRY_POINT, AttackerState::FORCED_PASS, [this]() -> bool {
2828
// セットプレイのときは強制パス
29-
auto game_command = world_model()->play_situation.getSituationCommandID();
29+
auto game_command = world_model()->getMsg().play_situation.command.value;
3030
if (
3131
game_command == crane_msgs::msg::PlaySituation::OUR_DIRECT_FREE ||
3232
game_command == crane_msgs::msg::PlaySituation::OUR_KICKOFF_START) {

‎crane_robot_skills/src/ball_nearby_positioner.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ BallNearByPositioner::BallNearByPositioner(RobotCommandWrapperBase::SharedPtr &
2525

2626
Status BallNearByPositioner::update()
2727
{
28-
auto situation = world_model()->play_situation.getSituationCommandID();
28+
auto situation = world_model()->getMsg().play_situation.command.value;
2929
double distance_from_ball = [&]() {
3030
switch (situation) {
3131
case crane_msgs::msg::PlaySituation::THEIR_DIRECT_FREE:

‎crane_robot_skills/src/goalie.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ Goalie::Goalie(RobotCommandWrapperBase::SharedPtr & base)
2020

2121
Status Goalie::update()
2222
{
23-
auto situation = world_model()->play_situation.getSituationCommandID();
23+
auto situation = world_model()->getMsg().play_situation.command.value;
2424
if (getParameter<bool>("run_inplay")) {
2525
situation = crane_msgs::msg::PlaySituation::OUR_INPLAY;
2626
}
@@ -38,7 +38,7 @@ Status Goalie::update()
3838
break;
3939
default: {
4040
if (
41-
world_model()->play_situation.getRefereeCommandID() ==
41+
world_model()->getMsg().play_situation.command_raw.value ==
4242
robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
4343
// STOPのときにはボールを排出しない
4444
inplay(false);

‎crane_robot_skills/src/penalty_kick.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ PenaltyKick::PenaltyKick(RobotCommandWrapperBase::SharedPtr & base)
3333
if (getParameter<bool>("start_from_kick")) {
3434
return true;
3535
} else {
36-
return world_model()->play_situation.getSituationCommandID() ==
36+
return world_model()->getMsg().play_situation.command.value ==
3737
crane_msgs::msg::PlaySituation::OUR_PENALTY_START;
3838
}
3939
});

‎crane_speaker/src/crane_speaker_node.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -46,8 +46,8 @@ class SpeakClient : public rclcpp::Node
4646

4747
play_situation_sub = create_subscription<crane_msgs::msg::PlaySituation>(
4848
"/play_situation", 10, [this](const crane_msgs::msg::PlaySituation::SharedPtr msg) {
49-
if (play_situation_map.find(msg->command) != play_situation_map.end()) {
50-
sendGoal(play_situation_map[msg->command]);
49+
if (play_situation_map.find(msg->command.value) != play_situation_map.end()) {
50+
sendGoal(play_situation_map[msg->command.value]);
5151
}
5252
});
5353
}

‎session/crane_planner_plugins/src/their_penalty_kick_planner.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ TheirPenaltyKickPlanner::calculateRobotCommand(
2828
}
2929
if (goalie) {
3030
if (
31-
world_model->play_situation.getSituationCommandID() ==
31+
world_model->getMsg().play_situation.command.value ==
3232
crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION) {
3333
auto & cmd = goalie->commander();
3434
cmd.setTargetPosition(world_model->getOurGoalCenter());

‎session/crane_session_controller/include/crane_session_controller/session_controller.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,7 @@ class SessionControllerComponent : public rclcpp::Node
7474

7575
std::vector<PlannerBase::SharedPtr> available_planners;
7676

77-
PlaySituationWrapper play_situation;
77+
crane_msgs::msg::PlaySituation play_situation;
7878

7979
rclcpp::TimerBase::SharedPtr timer;
8080

‎session/crane_session_controller/src/crane_session_controller.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -100,12 +100,12 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
100100

101101
play_situation_sub = create_subscription<crane_msgs::msg::PlaySituation>(
102102
"/play_situation", 1, [this](const crane_msgs::msg::PlaySituation & msg) {
103+
play_situation = msg;
103104
// TODO(HansRobo): 実装
104105
if (not world_model_ready) {
105106
return;
106107
}
107-
play_situation.update(msg);
108-
assign(play_situation.getSituationCommandText());
108+
assign(play_situation.command.name);
109109
});
110110

111111
timer_process_time_pub = create_publisher<std_msgs::msg::Float32>("~/timer/process_time", 10);
@@ -116,7 +116,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
116116
timer = rclcpp::create_timer(this, get_clock(), 100ms, [&]() {
117117
ScopedTimer timer(timer_process_time_pub);
118118
PlannerContext planner_context;
119-
auto it = event_map.find(play_situation.getSituationCommandText());
119+
auto it = event_map.find(play_situation.command.name);
120120
if (it != event_map.end()) {
121121
try {
122122
request(it->second, world_model->ours.getAvailableRobotIds(), planner_context);
@@ -189,10 +189,10 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
189189
}();
190190

191191
if (robot_changed) {
192-
assign(play_situation.getSituationCommandText());
192+
assign(play_situation.command.name);
193193
} else if (world_model->isOurBallOwnerChanged() or world_model->isBallOwnerTeamChanged()) {
194194
RCLCPP_INFO(get_logger(), "ボールオーナーが変更されたので再割当を行います");
195-
assign(play_situation.getSituationCommandText());
195+
assign(play_situation.command.name);
196196
}
197197

198198
PlannerContext planner_context;

0 commit comments

Comments
 (0)