Skip to content

Commit 4cfe574

Browse files
HansRoboHansRobo
authored
Merge branch 'develop' into erforce-sim
2 parents 088d12a + 041a87c commit 4cfe574

File tree

16 files changed

+126
-103
lines changed

16 files changed

+126
-103
lines changed

‎.pre-commit-config.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ repos:
6060
# - id: isort
6161

6262
- repo: https://github.com/astral-sh/ruff-pre-commit
63-
rev: v0.9.3
63+
rev: v0.9.6
6464
hooks:
6565
- id: ruff
6666
args: [--fix]

‎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_msgs/msg/analysis/PlaySituation.msg

+10-4
Original file line numberDiff line numberDiff line change
@@ -78,16 +78,22 @@ uint8 STOP_PRE_THEIR_DIRECT_FREE = 65
7878
# NO_PROGRESS_IN_GAME / TOO_MANY_ROBOTS イベント後のSTOP
7979
uint8 STOP_PRE_FORCE_START = 66
8080

81-
uint32 stage
81+
NamedInt stage
8282

83-
uint32 command_raw
83+
NamedInt command_raw
8484

85-
uint32 command
85+
NamedInt command
8686

87-
string referee_text
87+
NamedInt[<=1] next_command_raw
88+
89+
NamedInt[<=1] next_command
8890

8991
string reason_text
9092

9193
geometry_msgs/Point placement_position
9294

95+
robocup_ssl_msgs/TeamInfo our_team_info
96+
97+
robocup_ssl_msgs/TeamInfo their_team_info
98+
9399
robocup_ssl_msgs/Referee referee_raw

‎crane_play_switcher/src/play_switcher.cpp

+30-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,20 @@ 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+
play_situation_msg.next_command_raw.clear();
87+
if (not msg.next_command.empty()) {
88+
play_situation_msg.next_command_raw.push_back(
89+
getRefereeCommandNamedInt(msg.next_command.front()));
90+
}
91+
if (bool is_yellow = msg.yellow.name == team_name; is_yellow) {
92+
play_situation_msg.our_team_info = msg.yellow;
93+
play_situation_msg.their_team_info = msg.blue;
94+
} else {
95+
play_situation_msg.our_team_info = msg.blue;
96+
play_situation_msg.their_team_info = msg.yellow;
97+
}
8598
play_situation_msg.referee_raw = msg;
8699

87100
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)
95108
// start_command_map[PlaySituation::THEIR_KICKOFF_START] = {}
96109

97110
if (msg.command == Referee::COMMAND_NORMAL_START) {
98-
next_play_situation = start_command_map[play_situation_msg.command];
111+
next_play_situation = start_command_map[play_situation_msg.command.value];
99112
inplay_command_info.reason =
100113
"RAWコマンド変化＆NORMAL_START：KICKOFF/"
101114
"PENALTYはPREPARATIONからSTARTに移行";
@@ -171,14 +184,14 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
171184

172185
// キックオフ・フリーキック・ペナルティーキック開始後，ボールが少なくとも0.05m動いた
173186
if (
174-
play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START or
175-
play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE or
187+
play_situation_msg.command.value == PlaySituation::THEIR_KICKOFF_START or
188+
play_situation_msg.command.value == PlaySituation::THEIR_DIRECT_FREE or
176189
// 敵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
190+
// play_situation_msg.command.value == PlaySituation::THEIR_PENALTY_START or
191+
play_situation_msg.command.value == PlaySituation::OUR_KICKOFF_START or
192+
play_situation_msg.command.value == PlaySituation::OUR_DIRECT_FREE
180193
// 味方PKのINPLAYはOUR_PENALTY_STARTとして実装しているのでINPLAY遷移はしない
181-
// play_situation_msg.command == PlaySituation::OUR_PENALTY_START
194+
// play_situation_msg.command.value == PlaySituation::OUR_PENALTY_START
182195
) {
183196
if (0.05 <= (last_command_changed_state.ball_position - world_model->ball.pos).norm()) {
184197
next_play_situation = PlaySituation::INPLAY;
@@ -195,13 +208,13 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
195208

196209
// キックオフから10秒経過
197210
if (
198-
play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START &&
211+
play_situation_msg.command.value == PlaySituation::THEIR_KICKOFF_START &&
199212
10.0 <= (now() - last_command_changed_state.stamp).seconds()) {
200213
next_play_situation = PlaySituation::INPLAY;
201214
inplay_command_info.reason = "INPLAY判定：敵キックオフから10秒経過";
202215
}
203216
// フリーキックからN秒経過（N=5 @DivA, N=10 @DivB）
204-
if (play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE) {
217+
if (play_situation_msg.command.value == PlaySituation::THEIR_DIRECT_FREE) {
205218
if (30.0 <= (now() - last_command_changed_state.stamp).seconds()) {
206219
next_play_situation = PlaySituation::INPLAY;
207220
inplay_command_info.reason =
@@ -213,17 +226,17 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
213226
// コマンドが更新されているかを調べる
214227
if (
215228
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();
229+
next_play_situation.value() != static_cast<int>(play_situation_msg.command.value)) {
230+
play_situation_msg.command = getSituationCommandNamedInt(next_play_situation.value());
218231
play_situation_msg.reason_text = inplay_command_info.reason;
219232

220233
RCLCPP_INFO(get_logger(), "---");
221234
RCLCPP_INFO(
222235
get_logger(), "RAW_CMD : %d (%s)", msg.command,
223-
PlaySituationWrapper::getRefereeCommandText(msg.command).c_str());
236+
getRefereeCommandText(msg.command).c_str());
224237
RCLCPP_INFO(
225-
get_logger(), "INPLAY_CMD : %d (%s)", play_situation_msg.command,
226-
PlaySituationWrapper::getSituationCommandText(play_situation_msg.command).c_str());
238+
get_logger(), "INPLAY_CMD : %d (%s)", play_situation_msg.command.value,
239+
play_situation_msg.command.name.c_str());
227240
RCLCPP_INFO(get_logger(), "REASON : %s", inplay_command_info.reason.c_str());
228241
RCLCPP_INFO(
229242
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;

‎utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp

+10-31
Original file line numberDiff line numberDiff line change
@@ -8,50 +8,29 @@
88
#define CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_
99

1010
#include <crane_basics/boost_geometry.hpp>
11+
#include <crane_msgs/msg/named_int.hpp>
1112
#include <crane_msgs/msg/play_situation.hpp>
1213
#include <string>
1314
#include <vector>
1415

1516
namespace crane
1617
{
17-
struct PlaySituationWrapper
18-
{
19-
struct IDWithText
20-
{
21-
uint32_t id;
22-
23-
std::string text;
24-
};
25-
26-
auto isInplay() const -> bool
27-
{
28-
return situation_command.id >= crane_msgs::msg::PlaySituation::OUR_INPLAY;
29-
}
30-
31-
Point placement_position;
32-
33-
auto update(const crane_msgs::msg::PlaySituation & msg) -> void;
34-
35-
auto getRefereeCommandID() const -> uint32_t { return referee_command_raw.id; }
36-
37-
auto getRefereeCommandText() const -> std::string { return referee_command_raw.text; }
18+
auto getStageText(uint32_t id) -> std::string;
3819

39-
auto getSituationCommandID() const -> uint32_t { return situation_command.id; }
20+
auto getStageNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt;
4021

41-
auto getSituationCommandText() const -> std::string { return situation_command.text; }
22+
auto getStageTextList() -> std::vector<std::string>;
4223

43-
static auto getRefereeCommandText(uint32_t id) -> std::string;
24+
auto getRefereeCommandText(uint32_t id) -> std::string;
4425

45-
static auto getRefereeCommandTextList() -> std::vector<std::string>;
26+
auto getRefereeCommandNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt;
4627

47-
static auto getSituationCommandText(uint32_t id) -> std::string;
28+
auto getRefereeCommandTextList() -> std::vector<std::string>;
4829

49-
static auto getSituationCommandTextList() -> std::vector<std::string>;
30+
auto getSituationCommandText(uint32_t id) -> std::string;
5031

51-
private:
52-
IDWithText referee_command_raw;
32+
auto getSituationCommandNamedInt(uint32_t id) -> crane_msgs::msg::NamedInt;
5333

54-
IDWithText situation_command;
55-
};
34+
auto getSituationCommandTextList() -> std::vector<std::string>;
5635
} // namespace crane
5736
#endif // CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_

‎utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -279,8 +279,6 @@ struct WorldModelWrapper
279279

280280
Ball ball;
281281

282-
PlaySituationWrapper play_situation;
283-
284282
private:
285283
class BallOwnerCalculator
286284
{

0 commit comments

Comments
 (0)