Merge pull request #153 from ibis-ssl/fix/#152

PlaySwitcher内で前回コマンド遷移時の状態記録が抜けていたのを修正

crane_bringup/launch/play_switcher.launch.xml

@@ -47,9 +47,9 @@
 
   <node pkg="crane_sender" exec="sim_sender_node" output="screen">
     <param name="no_movement" value="false"/>
-    <param name="theta_kp" value="5.0"/>
+    <param name="theta_kp" value="3.0"/>
     <param name="theta_ki" value="0.0"/>
-    <param name="theta_kd" value="1.0"/>
+    <param name="theta_kd" value="0.1"/>
   </node>
 
   <!--    <node pkg="crane_sender" exec="ibis_sender_node" output="screen">-->

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);
   }