update: 復帰動作用

Hara1274 · Hara1274 · commit 04680a6966db · 2025-11-15T15:09:34.000+09:00
diff --git a/modules/MotionParser.cpp b/modules/MotionParser.cpp
@@ -434,38 +434,35 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
         break;
       }
 
-      // CRA: カメラ復帰動作
-      // [1]:int 回頭角度[deg], [2]:double 回頭スピード[mm/s], [3]:string 回頭の方向(clockwise or
-      // anticlockwise), [4-9]:int HSV値(lowerH,lowerS,lowerV,upperH,upperS,upperV), [10-13]int
-      // ROI座標[px]
-      // ([10]左上隅のx座標, [11]左上隅のy座標, [12]幅, [13]高さ), [14-15]int 解像度[px] ([14]幅,
-      // [15]高さ)
+      // CRA: カメラ検出失敗時の復帰動作
+      // [1]:int ラインの方向角度（絶対角度）[deg], [2]:int 基準パワー値,
+      // [3-5]:double ライン回頭用PIDゲイン(kp, ki, kd)
+      // [6]:int 首振り角度（deg）, [7]:int 最大首振り回数
+      // [8]:double PCIDS距離[mm], [9]:double PCIDS速度[mm/s],
+      // [10-12]:double PCIDS用PIDゲイン(kp, ki, kd)
+      // [13-18]:int HSV値(lowerH,lowerS,lowerV,upperH,upperS,upperV), [19-22]:int ROI座標[px]
+      // ([19]左上隅のx座標, [20]左上隅のy座標, [21]幅, [22]高さ), [23-24]:int 解像度[px] ([23]幅,
+      // [24]高さ)
       // 補足：ROI（Region of Interest: ライントレース用の画像内注目領域（四角形））
       case COMMAND::CRA: {
         CameraServer::BoundingBoxDetectorRequest detectionRequest;
 
         detectionRequest.command
             = CameraServer::Command::LINE_DETECTION;  // コマンドタイプをライン検出に設定
 
-        detectionRequest.lowerHSV = cv::Scalar(stoi(params[4]), stoi(params[5]), stoi(params[6]));
-        detectionRequest.upperHSV = cv::Scalar(stoi(params[7]), stoi(params[8]), stoi(params[9]));
-
-        // パラメータ配列のサイズによってROIと解像度を設定
-        if(params.size() > 15) {
-          detectionRequest.roi
-              = cv::Rect(stoi(params[10]), stoi(params[11]), stoi(params[12]), stoi(params[13]));
-          detectionRequest.resolution = cv::Size(stoi(params[14]), stoi(params[15]));
-        } else if(params.size() > 13) {
-          detectionRequest.roi
-              = cv::Rect(stoi(params[10]), stoi(params[11]), stoi(params[12]), stoi(params[13]));
-          detectionRequest.resolution = cv::Size(640, 480);
-        } else {
-          detectionRequest.roi = cv::Rect(50, 240, 540, 240);
-          detectionRequest.resolution = cv::Size(640, 480);
-        }
+        detectionRequest.lowerHSV
+            = cv::Scalar(stoi(params[13]), stoi(params[14]), stoi(params[15]));
+        detectionRequest.upperHSV
+            = cv::Scalar(stoi(params[16]), stoi(params[17]), stoi(params[18]));
+        detectionRequest.roi
+            = cv::Rect(stoi(params[19]), stoi(params[20]), stoi(params[21]), stoi(params[22]));
+        detectionRequest.resolution = cv::Size(stoi(params[23]), stoi(params[24]));
 
-        auto cra = new CameraRecoveryAction(robot, stoi(params[1]), stod(params[2]),
-                                            convertBool(params[0], params[3]), detectionRequest);
+        PidGain anglePidGain(stod(params[3]), stod(params[4]), stod(params[5]));
+        PidGain pcidsPidGain(stod(params[10]), stod(params[11]), stod(params[12]));
+        auto cra = new CameraRecoveryAction(robot, stoi(params[1]), stoi(params[2]), anglePidGain,
+                                            stoi(params[6]), stoi(params[7]), stod(params[8]),
+                                            stod(params[9]), pcidsPidGain, detectionRequest);
         motionList.push_back(cra);
         break;
       }
diff --git a/modules/motions/CameraRecoveryAction.cpp b/modules/motions/CameraRecoveryAction.cpp
@@ -5,17 +5,20 @@
  */
 
 #include "CameraRecoveryAction.h"
-#include <thread>
-#include <chrono>
-#include <iostream>
 
 CameraRecoveryAction::CameraRecoveryAction(
-    Robot& _robot, int _angle, double _speed, bool _isClockwise,
-    const CameraServer::BoundingBoxDetectorRequest& _detectionRequest)
+    Robot& _robot, int _lineDirectionAngle, int _basePower, const PidGain& _anglePidGain,
+    int _swingAngle, int _maxSwingCount, double _pcidsDistance, double _pcidsSpeed,
+    const PidGain& _pcidsPidGain, const CameraServer::BoundingBoxDetectorRequest& _detectionRequest)
   : CompositeMotion(_robot),
-    recoveryAngle(_angle),
-    speed(_speed),
-    isClockwise(_isClockwise),
+    lineDirectionAngle(_lineDirectionAngle),
+    basePower(_basePower),
+    anglePidGain(_anglePidGain),
+    swingAngle(_swingAngle),
+    maxSwingCount(_maxSwingCount),
+    pcidsDistance(_pcidsDistance),
+    pcidsSpeed(_pcidsSpeed),
+    pcidsPidGain(_pcidsPidGain),
     detectionRequest(_detectionRequest)
 {
 }
@@ -38,21 +41,112 @@ void CameraRecoveryAction::run()
     return;
   }
 
-  AngleRotation rotation(robot, recoveryAngle, speed, isClockwise);
-  rotation.run();
-  std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  // 停止処理
+  robot.getMotorControllerInstance().stopWheelsMotor();
+  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+  // IMUの角度計算が開始されているかチェック
+  if(!robot.getIMUControllerInstance().getShouldContinueCalculation()) {
+    std::cerr << "IS,startで角度計算を事前に開始する必要があります。" << std::endl;
+    return;
+  }
+
+  // 現在の角度を取得
+  double currentAngle = robot.getIMUControllerInstance().getAngle();
+
+  // 目標角度への最短回頭方向を決定
+  // fmodで(目標角度 - 現在角度)を0~360度に正規化すると「時計回りで回る場合の角度」を計算
+  // この値が180度以下なら時計回りの方が短く、180度より大きければ反時計回りの方が短い
+  bool isClockwise = (fmod(lineDirectionAngle - currentAngle + 360.0, 360.0) <= 180.0);
+
+  // ラインの方向へ絶対角度モードで回頭
+  IMUAngleRotation turnToLineDirection(robot, lineDirectionAngle, basePower, isClockwise,
+                                       anglePidGain, true);
+  turnToLineDirection.run();
+
+  // 最新フレームに更新するためにスナップショットを連続で取得
+  CameraServer::SnapshotActionRequest request{};
+  request.command = CameraServer::Command::TAKE_SNAPSHOT;
+  std::strncpy(request.fileName, "warmup", sizeof(request.fileName));
+  for(int i = 0; i < 5; ++i) {
+    CameraServer::SnapshotActionResponse response{};
+    robot.getSocketClient().executeSnapshotAction(request, response);
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+  }
+
+  // デバッグ用に復帰動作後の画像を保存
+  Snapshot snapshot(robot, "recovery_line_direction");
+  snapshot.run();
+
+  // 動作安定のためにスリープ
+  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 
   // 再検出
   success = client.executeLineDetection(detectionRequest, response);
 
   if(!success) {
-    std::cerr << "通信に失敗しました。" << std::endl;
+    std::cerr << "再検出時の通信に失敗しました。" << std::endl;
     return;
   }
 
   if(response.result.wasDetected) {
     std::cout << "復帰に成功しました。" << std::endl;
-  } else {
-    std::cout << "復帰できませんでした。" << std::endl;
+    return;
+  }
+
+  // 復帰できなかった場合、首振り動作で検出を試みる
+  int swingCount = 0;
+  bool currentDirection = isClockwise;  // 現在の首振り方向
+
+  while(true) {  // ラインを検出するまで無限ループ
+    // 首振り動作
+    for(int i = 0; i < maxSwingCount; ++i) {
+      // swingAngle分、現在の方向（currentDirection）に相対角度で首を振る
+      IMUAngleRotation swing(robot, swingAngle, basePower, currentDirection, anglePidGain, false);
+      swing.run();
+
+      // 最新フレームに更新するためにスナップショットを連続で取得
+      CameraServer::SnapshotActionRequest request{};
+      request.command = CameraServer::Command::TAKE_SNAPSHOT;
+      std::strncpy(request.fileName, "warmup", sizeof(request.fileName));
+      for(int j = 0; j < 5; ++j) {
+        CameraServer::SnapshotActionResponse response{};
+        robot.getSocketClient().executeSnapshotAction(request, response);
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+      }
+
+      // デバッグ用に復帰動作後の画像を保存
+      Snapshot snapshot(robot, "recovery_swing");
+      snapshot.run();
+
+      // 動作安定のためにスリープ
+      std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+      // 検出を試みる
+      success = client.executeLineDetection(detectionRequest, response);
+      if(success && response.result.wasDetected) {
+        std::cout << "首振りで復帰に成功しました。" << std::endl;
+        return;
+      }
+
+      swingCount++;
+    }
+
+    // 首振り動作後、直進
+    std::cout << "首振り " << swingCount << " 回実施後、直進します。" << std::endl;
+    PictureColorDistanceStraight straightMotion(robot, pcidsDistance, pcidsSpeed, pcidsPidGain,
+                                                detectionRequest);
+    straightMotion.run();
+
+    // 直進後に再検出
+    success = client.executeLineDetection(detectionRequest, response);
+    if(success && response.result.wasDetected) {
+      std::cout << "直進後に復帰に成功しました。" << std::endl;
+      return;
+    }
+
+    // 首振り方向を反転
+    currentDirection = !currentDirection;
+    std::cout << "首振り方向を反転して継続します。" << std::endl;
   }
 }
diff --git a/modules/motions/CameraRecoveryAction.h b/modules/motions/CameraRecoveryAction.h
@@ -8,20 +8,34 @@
 #define CAMERA_RECOVERY_ACTION_H
 
 #include "CompositeMotion.h"
-#include "AngleRotation.h"
+#include "IMUAngleRotation.h"
 #include "SocketProtocol.h"
+#include "Snapshot.h"
+#include "PictureColorDistanceStraight.h"
+#include "Pid.h"
+#include <iostream>
+#include <cmath>
+#include <thread>
+#include <chrono>
 
 class CameraRecoveryAction : public CompositeMotion {
  public:
   /**
    * コンストラクタ
    * @param _robot ロボットインスタンス
-   * @param _angle 回頭角度 (dig) 0~180
-   * @param _speed 回頭速度（mm/秒）
-   * @param _isClockwise 回頭方向（true: 右回り, false: 左回り）
+   * @param _lineDirectionAngle ラインの方向角度（絶対角度, deg） 0~360
+   * @param _basePower 基準パワー値
+   * @param _anglePidGain 角度制御用PIDゲイン
+   * @param _swingAngle 首振り角度（deg）
+   * @param _maxSwingCount 最大首振り回数
+   * @param _pcidsDistance PCIDS用の目標距離[mm]
+   * @param _pcidsSpeed PCIDS用の目標速度[mm/s]
+   * @param _pcidsPidGain PCIDS用の角度補正PIDゲイン
    * @param _detectionRequest 検出リクエスト
    */
-  CameraRecoveryAction(Robot& _robot, int _angle, double _speed, bool _isClockwise,
+  CameraRecoveryAction(Robot& _robot, int _lineDirectionAngle, int _basePower,
+                       const PidGain& _anglePidGain, int _swingAngle, int _maxSwingCount,
+                       double _pcidsDistance, double _pcidsSpeed, const PidGain& _pcidsPidGain,
                        const CameraServer::BoundingBoxDetectorRequest& _detectionRequest);
 
   /**
@@ -32,10 +46,15 @@ class CameraRecoveryAction : public CompositeMotion {
  private:
   CameraServer::BoundingBoxDetectorRequest detectionRequest;  // 検出リクエスト
   BoundingBoxDetectionResult result;                          // 検出結果
-  int recoveryAngle;                                          // 復帰回頭角度
-  double speed;                                               // 回頭スピード
-  bool isClockwise;                                           // 回頭方向
-  static constexpr int FRAME_NUMBER = 5;                      // フレーム取得回数
+  int lineDirectionAngle;                 // ラインの方向角度（絶対角度）
+  int basePower;                          // 基準パワー値
+  PidGain anglePidGain;                   // 角度制御用PIDゲイン
+  int swingAngle;                         // 首振り角度
+  int maxSwingCount;                      // 最大首振り回数
+  double pcidsDistance;                   // PCIDS用の目標距離[mm]
+  double pcidsSpeed;                      // PCIDS用の目標速度[mm/s]
+  PidGain pcidsPidGain;                   // PCIDS用の角度補正PIDゲイン
+  static constexpr int FRAME_NUMBER = 5;  // フレーム取得回数
 };
 
 #endif
diff --git a/tests/CameraRecoveryActionTest.cpp b/tests/CameraRecoveryActionTest.cpp
@@ -10,6 +10,7 @@
 #include "Robot.h"
 #include "MockSocketClient.h"
 #include "SystemInfo.h"
+#include "IMUSetting.h"
 
 using namespace std;
 
@@ -27,7 +28,10 @@ namespace etrobocon2025_test {
     mockSocketClient.setNextLineDetectionResponse(successResponse);
 
     CameraServer::BoundingBoxDetectorRequest dummyRequest;
-    CameraRecoveryAction action(robot, 20, 100.0, true, dummyRequest);
+    PidGain anglePidGain{ 0.3, 0.005, 0.15 };
+    PidGain pcidsPidGain{ 0.5, 0.01, 0.2 };
+    CameraRecoveryAction action(robot, 20, 100, anglePidGain, 10, 3, 100.0, 50.0, pcidsPidGain,
+                                dummyRequest);
 
     testing::internal::CaptureStdout();
     action.run();
@@ -41,6 +45,20 @@ namespace etrobocon2025_test {
     MockSocketClient mockSocketClient;
     Robot robot(mockSocketClient);
 
+    // オフセット計算前に静止状態に設定
+    IMUTestControl::rotationStateRef() = 0;
+
+    // オフセット計算と補正行列計算を事前実行
+    robot.getIMUControllerInstance().initializeOffset();
+    robot.getIMUControllerInstance().calculateCorrectionMatrix();
+
+    // 絶対角度モードのため角度計算を開始
+    IMUSetting imuStart(robot, true);
+    imuStart.run();
+
+    // ダミーIMUの回転状態を右回頭に設定
+    IMUTestControl::rotationStateRef() = 1;
+
     // モックを設定: 初回は失敗、2回目は成功
     CameraServer::BoundingBoxDetectorResponse failureResponse;
     failureResponse.result.wasDetected = false;
@@ -50,33 +68,64 @@ namespace etrobocon2025_test {
     mockSocketClient.setNextLineDetectionResponse(successResponse);
 
     CameraServer::BoundingBoxDetectorRequest dummyRequest;
-    CameraRecoveryAction action(robot, 20, 100.0, true, dummyRequest);
+    PidGain anglePidGain{ 0.3, 0.005, 0.15 };
+    PidGain pcidsPidGain{ 0.5, 0.01, 0.2 };
+    CameraRecoveryAction action(robot, 15, 100, anglePidGain, 10, 3, 100.0, 50.0, pcidsPidGain,
+                                dummyRequest);
 
     testing::internal::CaptureStdout();
     action.run();
     string output = testing::internal::GetCapturedStdout();
     ASSERT_NE(output.find("復帰に成功しました。"), string::npos);
+
+    // 角度計算を停止
+    IMUSetting imuStop(robot, false);
+    imuStop.run();
   }
 
-  // 復帰動作を行い、再検出でも失敗した場合のテスト
-  TEST(CameraRecoveryActionTest, DetectionFailureAfterRecovery)
+  // 復帰動作を行い、首振りで成功した場合のテスト
+  TEST(CameraRecoveryActionTest, DetectionSuccessAfterSwing)
   {
     MockSocketClient mockSocketClient;
     Robot robot(mockSocketClient);
 
-    // モックを設定: 2回とも失敗
+    // オフセット計算前に静止状態に設定
+    IMUTestControl::rotationStateRef() = 0;
+
+    // オフセット計算と補正行列計算を事前実行
+    robot.getIMUControllerInstance().initializeOffset();
+    robot.getIMUControllerInstance().calculateCorrectionMatrix();
+
+    // 絶対角度モードのため角度計算を開始
+    IMUSetting imuStart(robot, true);
+    imuStart.run();
+
+    // ダミーIMUの回転状態を右回頭に設定
+    IMUTestControl::rotationStateRef() = 1;
+
+    // モックを設定: 初回と再検出は失敗、首振り後に成功
     CameraServer::BoundingBoxDetectorResponse failureResponse;
     failureResponse.result.wasDetected = false;
-    mockSocketClient.setNextLineDetectionResponse(failureResponse);
-    mockSocketClient.setNextLineDetectionResponse(failureResponse);
+    CameraServer::BoundingBoxDetectorResponse successResponse;
+    successResponse.result.wasDetected = true;
+    mockSocketClient.setNextLineDetectionResponse(failureResponse);  // 初回失敗
+    mockSocketClient.setNextLineDetectionResponse(failureResponse);  // 再検出失敗
+    mockSocketClient.setNextLineDetectionResponse(successResponse);  // 首振り後成功
 
     CameraServer::BoundingBoxDetectorRequest dummyRequest;
-    CameraRecoveryAction action(robot, 90, 300.0, true, dummyRequest);
+    PidGain anglePidGain{ 0.3, 0.005, 0.15 };
+    PidGain pcidsPidGain{ 0.5, 0.01, 0.2 };
+    CameraRecoveryAction action(robot, 15, 100, anglePidGain, 10, 3, 100.0, 50.0, pcidsPidGain,
+                                dummyRequest);
 
     testing::internal::CaptureStdout();
     action.run();
     string output = testing::internal::GetCapturedStdout();
-    ASSERT_NE(output.find("復帰できませんでした"), string::npos);
+    ASSERT_NE(output.find("首振りで復帰に成功しました。"), string::npos);
+
+    // 角度計算を停止
+    IMUSetting imuStop(robot, false);
+    imuStop.run();
   }
 
 }  // namespace etrobocon2025_test
