Merge branch 'work-KL25-143' into ticket-KL25-143

Author: nishijima515

datafiles/commands/SmartCarryLeft.csv

@@ -11,4 +11,4 @@ DCL,800.0,400.0,200,0.002,0.0005,0.001,35,50,50,85,255,255,0,0,600,600,800,600
 IMUR,55,70.0,clockwise,0.036,0.012,0.03
 DCL,700.0,300.0,280,0.002,0.0005,0.001,0,0,0,179,255,30,0,0,600,600,800,600
 DCL,300.0,300.0,400,0.002,0.0005,0.001,0,0,0,179,255,30,300,0,500,600,800,600
-CDCL,BLUE,400.0,350.0,400,0.002,0.0005,0.001,100,50,50,130,255,255,0,0,700,600,800,600
+CDCL,BLUE,400.0,350.0,400,0.002,0.0005,0.001,100,50,50,130,255,255,0,0,700,600,800,600

modules/MotionParser.cpp

@@ -476,7 +476,8 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
         // 角度[deg] [9]:double 回転パワー [10-12]:double udclの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:
+        // ([23]幅,[24]高さ), [25]:int PCIDS用ROI狭め値[px](オプション)
+        // 補足：ROI（Region of Interest:
         // ライントレース用の画像内注目領域（四角形））
       case COMMAND::BTCA: {
         CameraServer::BoundingBoxDetectorRequest detectionRequest;
@@ -489,8 +490,15 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
         detectionRequest.upperHSV
             = cv::Scalar(stoi(params[16]), stoi(params[17]), stoi(params[18]));
 
+        int roiShrinkValue = 0;  // ROI狭め値のデフォルト値
+
         // パラメータ配列のサイズによってROIと解像度を設定
-        if(params.size() > 24) {
+        if(params.size() > 25) {
+          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]));
+          roiShrinkValue = stoi(params[25]);
+        } else if(params.size() > 24) {
           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]));
@@ -507,7 +515,8 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
             robot, stod(params[1]), stod(params[2]),
             PidGain(stod(params[3]), stod(params[4]), stod(params[5])), stod(params[6]),
             stod(params[7]), stod(params[8]), stod(params[9]),
-            PidGain(stod(params[10]), stod(params[11]), stod(params[12])), detectionRequest);
+            PidGain(stod(params[10]), stod(params[11]), stod(params[12])), detectionRequest,
+            roiShrinkValue);
         motionList.push_back(btca);
         break;
       }

modules/motions/BottleTwoCatchAction.cpp

@@ -10,7 +10,8 @@
 BottleTwoCatchAction::BottleTwoCatchAction(
     Robot& _robot, double _forwardDistance, double _idsSpeed, const PidGain& _idsPidGain,
     double _ultrasonicDistance, double _udclSpeed, double _angle, double _rotatePower,
-    const PidGain& _udclPidGain, const CameraServer::BoundingBoxDetectorRequest& _detectionRequest)
+    const PidGain& _udclPidGain, const CameraServer::BoundingBoxDetectorRequest& _detectionRequest,
+    int _roiShrinkValue)
   : CompositeMotion(_robot),
     forwardDistance(_forwardDistance),
     idsSpeed(_idsSpeed),
@@ -20,7 +21,8 @@ BottleTwoCatchAction::BottleTwoCatchAction(
     angle(_angle),
     rotatePower(_rotatePower),
     udclPidGain(_udclPidGain),
-    detectionRequest(_detectionRequest)
+    detectionRequest(_detectionRequest),
+    roiShrinkValue(_roiShrinkValue)
 {
 }
 
@@ -31,9 +33,16 @@ void BottleTwoCatchAction::run()
   double initialLeftMotorCount = robot.getMotorControllerInstance().getLeftMotorCount();
   double initialDistance = Mileage::calculateMileage(initialRightMotorCount, initialLeftMotorCount);
 
+  // PCIDS用にROIを狭める
+  CameraServer::BoundingBoxDetectorRequest pcidsDetectionRequest = detectionRequest;
+  pcidsDetectionRequest.roi.x += roiShrinkValue;
+  pcidsDetectionRequest.roi.y += roiShrinkValue;
+  pcidsDetectionRequest.roi.width -= roiShrinkValue * 2;
+  pcidsDetectionRequest.roi.height -= roiShrinkValue * 2;
+
   // PCIDSを動かす。ボトル探索。
   PictureColorDistanceStraight pcids(robot, forwardDistance, idsSpeed, idsPidGain,
-                                     detectionRequest);
+                                     pcidsDetectionRequest);
   pcids.run();
 
   UltrasonicDistanceCameraLineTrace udcl(robot, ultrasonicDistance, forwardDistance, udclSpeed, 400,

modules/motions/BottleTwoCatchAction.h

@@ -30,11 +30,13 @@ class BottleTwoCatchAction : public CompositeMotion {
    * @param _rotatePower 回頭パワー
    * @param _udclPidGain udclのPIDゲイン
    * @param _detectionRequest 検出リクエスト
+   * @param _roiShrinkValue PCIDS用のROIを狭める値[px] (デフォルト: 0)
    */
   BottleTwoCatchAction(Robot& _robot, double _forwardDistance, double _idsSpeed,
                        const PidGain& _idsPidGain, double _ultrasonicDistance, double _udclSpeed,
                        double _angle, double _rotatePower, const PidGain& _udclPidGain,
-                       const CameraServer::BoundingBoxDetectorRequest& _detectionRequest);
+                       const CameraServer::BoundingBoxDetectorRequest& _detectionRequest,
+                       int _roiShrinkValue = 0);
 
   /**
    * @brief 2本目のボトルのキャッチ動作を行う
@@ -51,6 +53,7 @@ class BottleTwoCatchAction : public CompositeMotion {
   double angle;                                               // 回頭角度[度]
   double rotatePower;                                         // 回頭パワー
   PidGain udclPidGain;                                        // udclのPIDゲイン
+  int roiShrinkValue;                                         // PCIDS用のROIを狭める値[px]
 };
 
 #endif