diff --git a/crane_bringup/launch/play_switcher.launch.xml b/crane_bringup/launch/play_switcher.launch.xml index 294c5acf0..dbb8d6c3c 100644 --- a/crane_bringup/launch/play_switcher.launch.xml +++ b/crane_bringup/launch/play_switcher.launch.xml @@ -47,9 +47,9 @@ - + - + diff --git a/crane_play_switcher/src/play_switcher.cpp b/crane_play_switcher/src/play_switcher.cpp index 7f7308ca2..b60a05173 100644 --- a/crane_play_switcher/src/play_switcher.cpp +++ b/crane_play_switcher/src/play_switcher.cpp @@ -132,7 +132,11 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg) 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 = "INPLAY判定:敵ボールが少なくとも0.05m動いた"; + inplay_command_info.reason = + "INPLAY判定:敵ボールが少なくとも0.05m動いた(移動量: " + + std::to_string( + (last_command_changed_state.ball_position - world_model->ball.pos).norm()) + + "m)"; } } @@ -175,6 +179,9 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg) RCLCPP_INFO( get_logger(), "PREV_CMD_TIME: %f", (now() - last_command_changed_state.stamp).seconds()); + last_command_changed_state.stamp = now(); + last_command_changed_state.ball_position = world_model->ball.pos; + // パブリッシュはコマンド更新時のみ play_situation_pub->publish(play_situation_msg); }