Skip to content

Commit 85140ed

Browse files
Hara1274Hara1274
authored
Merge branch 'work-KL25-107-2' into ticket-KL25-149
2 parents 363871d + 632ee8c commit 85140ed

20 files changed

+256
-79
lines changed

‎modules/MotionParser.cpp‎

Lines changed: 95 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,8 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
105105
// [1]:double 距離[mm], [2]:double 速度[mm/s], [3]:int X座標[px], [4-6]:double PIDゲイン,
106106
// [7-9]int lowerHSV, [10-12]int upperHSV,
107107
// [13-16]int ROI座標[px] ([13]左上隅のx座標, [14]左上隅のy座標, [15]幅, [16]高さ),
108-
// [17-18]int 解像度[px] ([17]幅, [18]高さ)
108+
// [17-18]int 解像度[px] ([17]幅, [18]高さ),
109+
// [19]string 停止制御 (continue ならモータを停止せず、stop ならモータを停止) [オプション]
109110
// 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））
110111
case COMMAND::DCL: {
111112
CameraServer::BoundingBoxDetectorRequest detectionRequest;
@@ -121,9 +122,15 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
121122
= cv::Rect(stoi(params[13]), stoi(params[14]), stoi(params[15]), stoi(params[16]));
122123
detectionRequest.resolution = cv::Size(stoi(params[17]), stoi(params[18]));
123124

125+
bool isStopMotorPower = true;
126+
if(params.size() >= 20) {
127+
isStopMotorPower = convertBool("DCL", params[19]);
128+
}
129+
124130
auto dcl = new DistanceCameraLineTrace(
125131
robot, stod(params[1]), stod(params[2]), stoi(params[3]),
126-
PidGain(stod(params[4]), stod(params[5]), stod(params[6])), detectionRequest);
132+
PidGain(stod(params[4]), stod(params[5]), stod(params[6])), detectionRequest,
133+
isStopMotorPower);
127134
motionList.push_back(dcl);
128135
break;
129136
}
@@ -132,7 +139,8 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
132139
// [1]:string 色, [2]:double 距離[mm], [3]:double 速度[mm/s], [4]:int X座標[px],
133140
// [5-7]:double PIDゲイン, [8-10]int lowerHSV, [11-13]int upperHSV, [14-17]int ROI座標[px]
134141
// ([14]左上隅のx座標, [15]左上隅のy座標, [16]幅, [17]高さ), [18-19]int 解像度[px] ([18]幅,
135-
// [19]高さ) 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））
142+
// [19]高さ) 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））,
143+
// [20]string 停止制御 (continue ならモータを停止せず、stop ならモータを停止) [オプション]
136144
case COMMAND::CDCL: {
137145
CameraServer::BoundingBoxDetectorRequest detectionRequest;
138146

@@ -147,10 +155,15 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
147155
= cv::Rect(stoi(params[14]), stoi(params[15]), stoi(params[16]), stoi(params[17]));
148156
detectionRequest.resolution = cv::Size(stoi(params[18]), stoi(params[19]));
149157

158+
bool isStopMotorPower = true;
159+
if(params.size() >= 21) {
160+
isStopMotorPower = convertBool("CDCL", params[20]);
161+
}
162+
150163
auto cdcl = new ColorDistanceCameraLineTrace(
151164
robot, ColorJudge::convertStringToColor(params[1]), stod(params[2]), stod(params[3]),
152165
stoi(params[4]), PidGain(stod(params[5]), stod(params[6]), stod(params[7])),
153-
detectionRequest);
166+
detectionRequest, isStopMotorPower);
154167
motionList.push_back(cdcl);
155168
break;
156169
}
@@ -160,7 +173,8 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
160173
// [5-7]:double PIDゲイン, [8-10]int lowerHSV, [11-13]int upperHSV, [14-17]int ROI座標[px]
161174
// ([14]左上隅のx座標, [15]左上隅のy座標, [16]幅, [17]高さ), [18-19]int 解像度[px] ([18]幅,
162175
// [19]高さ)
163-
// 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））
176+
// 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））,
177+
// [20]string 停止制御 (continue ならモータを停止せず、stop ならモータを停止) [オプション]
164178
case COMMAND::UDCL: {
165179
CameraServer::BoundingBoxDetectorRequest detectionRequest;
166180

@@ -179,9 +193,15 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
179193
detectionRequest.resolution = cv::Size(stoi(params[18]), stoi(params[19]));
180194
}
181195

196+
bool isStopMotorPower = true;
197+
if(params.size() >= 21) {
198+
isStopMotorPower = convertBool("UDCL", params[20]);
199+
}
200+
182201
auto udcl = new UltrasonicDistanceCameraLineTrace(
183202
robot, stod(params[1]), stod(params[2]), stod(params[3]), stoi(params[4]),
184-
PidGain(stod(params[5]), stod(params[6]), stod(params[7])), detectionRequest);
203+
PidGain(stod(params[5]), stod(params[6]), stod(params[7])), detectionRequest,
204+
isStopMotorPower);
185205
motionList.push_back(udcl);
186206
break;
187207
}
@@ -192,7 +212,8 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
192212
// [13-15]int 2色目lowerHSV (H, S, V), [16-18]int 2色目upperHSV (H, S, V),
193213
// [19-22]int ROI座標[px] ([19]左上隅のx座標, [20]左上隅のy座標, [21]幅, [22]高さ),
194214
// [23-24]int 解像度[px] ([23]幅, [24]高さ)
195-
// 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））
215+
// 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））,
216+
// [25]string 停止制御 (continue ならモータを停止せず、stop ならモータを停止) [オプション]
196217
case COMMAND::DTCCL: {
197218
CameraServer::TwoColorBoundingBoxDetectorRequest detectionRequest;
198219

@@ -212,9 +233,15 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
212233
= cv::Rect(stoi(params[19]), stoi(params[20]), stoi(params[21]), stoi(params[22]));
213234
detectionRequest.resolution = cv::Size(stoi(params[23]), stoi(params[24]));
214235

236+
bool isStopMotorPower = true;
237+
if(params.size() >= 26) {
238+
isStopMotorPower = convertBool("DTCCL", params[25]);
239+
}
240+
215241
auto dtccl = new DistanceTwoColorCameraLineTrace(
216242
robot, stod(params[1]), stod(params[2]), stoi(params[3]),
217-
PidGain(stod(params[4]), stod(params[5]), stod(params[6])), detectionRequest);
243+
PidGain(stod(params[4]), stod(params[5]), stod(params[6])), detectionRequest,
244+
isStopMotorPower);
218245
motionList.push_back(dtccl);
219246
break;
220247
}
@@ -226,7 +253,8 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
226253
// [14-16]int 2色目lowerHSV (H, S, V), [17-19]int 2色目upperHSV (H, S, V),
227254
// [20-23]int ROI座標[px] ([20]左上隅のx座標, [21]左上隅のy座標, [22]幅, [23]高さ),
228255
// [24-25]int 解像度[px] ([24]幅, [25]高さ)
229-
// 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））
256+
// 補足：ROI（Region of Interest:ライントレース用の画像内注目領域（四角形））,
257+
// [26]string 停止制御 (continue ならモータを停止せず、stop ならモータを停止) [オプション]
230258
case COMMAND::CDTCCL: {
231259
CameraServer::TwoColorBoundingBoxDetectorRequest detectionRequest;
232260

@@ -246,10 +274,15 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
246274
= cv::Rect(stoi(params[20]), stoi(params[21]), stoi(params[22]), stoi(params[23]));
247275
detectionRequest.resolution = cv::Size(stoi(params[24]), stoi(params[25]));
248276

277+
bool isStopMotorPower = true;
278+
if(params.size() >= 27) {
279+
isStopMotorPower = convertBool("CDTCCL", params[26]);
280+
}
281+
249282
auto cdtccl = new ColorDistanceTwoColorCameraLineTrace(
250283
robot, ColorJudge::convertStringToColor(params[1]), stod(params[2]), stod(params[3]),
251284
stoi(params[4]), PidGain(stod(params[5]), stod(params[6]), stod(params[7])),
252-
detectionRequest);
285+
detectionRequest, isStopMotorPower);
253286
motionList.push_back(cdtccl);
254287
break;
255288
}
@@ -345,38 +378,45 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
345378
// [2]:int preTargetAngle
346379
// [3]:int postTargetAngle
347380
// [4]:int 回頭基準パワー値
348-
// [5]:double threshold（動体検出用）
349-
// [6]:double minArea（動体矩形とみなす最小面積）
350-
// [7]:int ROIの左上X座標
351-
// [8]:int ROIの左上Y座標
352-
// [9]:int ROIの幅
353-
// [10]:int ROIの高さ
354-
// [11]:int position（0=初期位置）
355-
// [12]:string 回頭方法(relative or absolute)
356-
// [13]:double kp（回頭PIDのP値）[オプション]
357-
// [14]:double ki（回頭PIDのI値）[オプション]
358-
// [15]:double kd（回頭PIDのD値）[オプション]
381+
// [5]:double preTargetDistance（撮影前の直進距離）
382+
// [6]:double postTargetDistance（撮影後の後退距離）
383+
// [7]:double preTargetSpeed（撮影前の直進速度）
384+
// [8]:double postTargetSpeed（撮影後の後退速度）
385+
// [9]:int armPower（アームを上げるpower値）
386+
// [10]:double threshold（動体検出用）
387+
// [11]:double minArea（動体矩形とみなす最小面積）
388+
// [12]:int ROIの左上X座標
389+
// [13]:int ROIの左上Y座標
390+
// [14]:int ROIの幅
391+
// [15]:int ROIの高さ
392+
// [16]:int position（0=初期位置）
393+
// [17]:string 回頭方法(relative or absolute)
394+
// [18]:double kp（回頭PIDのP値）[オプション]
395+
// [19]:double ki（回頭PIDのI値）[オプション]
396+
// [20]:double kd（回頭PIDのD値）[オプション]
359397

360398
case COMMAND::BCA: {
361399
cv::Rect roi;
362400

363401
bool isClockwise = convertBool("BCA", params[1]);
364-
roi = cv::Rect(stoi(params[7]), stoi(params[8]), stoi(params[9]), stoi(params[10]));
402+
roi = cv::Rect(stoi(params[12]), stoi(params[13]), stoi(params[14]), stoi(params[15]));
365403

366404
BackgroundPlaCameraAction* bca;
367-
if(params.size() >= 16) {
405+
if(params.size() >= 21) {
368406
// PID値が指定されている場合
369-
bca = new BackgroundPlaCameraAction(robot, isClockwise, stoi(params[2]), stoi(params[3]),
370-
stoi(params[4]), stod(params[5]), stod(params[6]),
371-
roi, stoi(params[11]),
372-
convertRotationModeToBool(params[12]),
373-
stod(params[13]), stod(params[14]), stod(params[15]));
407+
bca = new BackgroundPlaCameraAction(
408+
robot, isClockwise, stoi(params[2]), stoi(params[3]), stoi(params[4]),
409+
stod(params[5]), stod(params[6]), stod(params[7]), stod(params[8]), stoi(params[9]),
410+
stod(params[10]), stod(params[11]), roi, stoi(params[16]),
411+
convertRotationModeToBool(params[17]), stod(params[18]), stod(params[19]),
412+
stod(params[20]));
374413
} else {
375414
// PID値が指定されていない場合、デフォルト値を使用
376-
bca = new BackgroundPlaCameraAction(robot, isClockwise, stoi(params[2]), stoi(params[3]),
377-
stoi(params[4]), stod(params[5]), stod(params[6]),
378-
roi, stoi(params[11]),
379-
convertRotationModeToBool(params[12]));
415+
bca = new BackgroundPlaCameraAction(
416+
robot, isClockwise, stoi(params[2]), stoi(params[3]), stoi(params[4]),
417+
stod(params[5]), stod(params[6]), stod(params[7]), stod(params[8]), stoi(params[9]),
418+
stod(params[10]), stod(params[11]), roi, stoi(params[16]),
419+
convertRotationModeToBool(params[17]));
380420
}
381421

382422
motionList.push_back(bca);
@@ -416,6 +456,13 @@ vector<Motion*> MotionParser::createMotions(Robot& robot, string& commandFilePat
416456
break;
417457
}
418458

459+
// STOP: 走行体を停止させる動作
460+
case COMMAND::STOP: {
461+
auto stop = new Stop(robot);
462+
motionList.push_back(stop);
463+
break;
464+
}
465+
419466
// PCIDS: IMU角度補正直進に、カメラ画像の色検出による停止条件を追加した動作
420467
// [1]:double 距離[mm], [2]:double 速度[mm/s], [3-5]:double 角度補正PIDゲイン(kp, ki, kd),
421468
// [6-8]:int HSV下限, [9-11]:int HSV上限, [12-15]:int ROI座標[px] ([12]左上x, [13]左上y,
@@ -479,7 +526,8 @@ COMMAND MotionParser::convertCommand(const string& str)
479526
{ "SS", COMMAND::SS }, // カメラ撮影動作
480527
{ "MCA", COMMAND::MCA }, // ミニフィグのカメラ撮影動作
481528
{ "BCA", COMMAND::BCA }, // 風景・プラレールのカメラ撮影動作
482-
{ "CRA", COMMAND::CRA }, // カメラ検出失敗時の復帰動作
529+
{ "CRA", COMMAND::CRA }, // カメラ復帰動作
530+
{ "STOP", COMMAND::STOP }, // 走行体を停止させる動作
483531
{ "PCIDS", COMMAND::PCIDS }, // 画像ラインを用いた距離直進
484532
{ "IS", COMMAND::IS }, // IMU設定
485533
{ "DTCCL", COMMAND::DTCCL }, // 2色指定距離カメラライントレース
@@ -500,7 +548,20 @@ bool MotionParser::convertBool(const string& command, const string& stringParame
500548
// 末尾の改行を削除
501549
string param = StringOperator::removeEOL(stringParameter);
502550

503-
// 回転動作(AR,IMUR,MCA,BCA,CRA)の場合、"clockwise"ならtrue（時計回り）、"anticlockwise"ならfalse（反時計回り）に変換
551+
// カメラPIDトラッキング系の停止制御（continueなら継続、stopなら停止）
552+
if(command == "DCL" || command == "CDCL" || command == "UDCL" || command == "DTCCL"
553+
|| command == "CDTCCL") {
554+
if(param == "continue") {
555+
return false;
556+
} else if(param == "stop") {
557+
return true;
558+
} else {
559+
cout << "'continue' か 'stop'を入力してください" << endl;
560+
return true;
561+
}
562+
}
563+
564+
// 回転動作(AR,IMUR,MCA,BCA)の場合、"clockwise"ならtrue（時計回り）、"anticlockwise"ならfalse（反時計回り）に変換
504565
if(command == "AR" || command == "IMUR" || command == "MCA" || command == "BCA"
505566
|| command == "CRA") {
506567
if(param == "clockwise") {

‎modules/MotionParser.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535
#include "DistanceTwoColorCameraLineTrace.h"
3636
#include "ColorDistanceTwoColorCameraLineTrace.h"
3737
#include "PictureColorDistanceStraight.h"
38+
#include "Stop.h"
3839

3940
enum class COMMAND {
4041
AR, // 角度指定回頭
@@ -55,6 +56,7 @@ enum class COMMAND {
5556
BCA, // 背景のカメラ撮影動作
5657
CRA, // カメラ検出失敗時の復帰動作
5758
IS, // IMUの角度計算の設定を行う動作
59+
STOP, // 走行体を停止させる動作
5860
PCIDS, // カメラ画像を用いた色距離直進
5961
DTCCL, // 2色指定距離カメラライントレース
6062
CDTCCL, // 色距離指定2色カメラライントレース

‎modules/motions/BackgroundPlaCameraAction.cpp‎

Lines changed: 43 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -11,17 +11,21 @@
1111

1212
using namespace std;
1313

14-
BackgroundPlaCameraAction::BackgroundPlaCameraAction(Robot& _robot, bool _isClockwise,
15-
int _preTargetAngle, int _postTargetAngle,
16-
int _basePower, double _threshold,
17-
double _minArea, const cv::Rect _roi,
18-
int _position, bool _isAbsoluteAngleMode,
19-
double _kp, double _ki, double _kd)
14+
BackgroundPlaCameraAction::BackgroundPlaCameraAction(
15+
Robot& _robot, bool _isClockwise, int _preTargetAngle, int _postTargetAngle, int _basePower,
16+
double _preTargetDistance, double _postTargetDistance, double _preTargetSpeed,
17+
double _postTargetSpeed, int _armPower, double _threshold, double _minArea, const cv::Rect _roi,
18+
int _position, bool _isAbsoluteAngleMode, double _kp, double _ki, double _kd)
2019
: CompositeMotion(_robot),
2120
isClockwise(_isClockwise),
2221
preTargetAngle(_preTargetAngle),
2322
postTargetAngle(_postTargetAngle),
2423
basePower(_basePower),
24+
preTargetDistance(_preTargetDistance),
25+
postTargetDistance(_postTargetDistance),
26+
preTargetSpeed(_preTargetSpeed),
27+
postTargetSpeed(_postTargetSpeed),
28+
armPower(_armPower),
2529
threshold(_threshold),
2630
minArea(_minArea),
2731
roi(_roi),
@@ -60,12 +64,27 @@ void BackgroundPlaCameraAction::run()
6064
{
6165
if(!isMetPreCondition()) return;
6266

63-
// 撮影のため回頭
6467
PidGain prePidGain = { kp, ki, kd };
68+
// 撮影のため回頭
6569
IMUAngleRotation preRotation(robot, preTargetAngle, basePower, isClockwise, prePidGain,
6670
isAbsoluteAngleMode);
6771
preRotation.run();
6872

73+
// 動作安定のためのスリープ
74+
this_thread::sleep_for(chrono::milliseconds(10));
75+
76+
// アームを上げる
77+
robot.getMotorControllerInstance().setArmMotorPower(armPower);
78+
79+
// 動作安定のためのスリープ
80+
this_thread::sleep_for(chrono::milliseconds(10));
81+
82+
// 撮影のため直進
83+
IMUDistanceStraight preStraight(robot, preTargetDistance, preTargetSpeed, prePidGain);
84+
preStraight.run();
85+
86+
robot.getMotorControllerInstance().stopWheelsMotor();
87+
6988
// 綺麗な写真の撮影のためのスリープ
7089
this_thread::sleep_for(chrono::milliseconds(100));
7190

@@ -96,8 +115,24 @@ void BackgroundPlaCameraAction::run()
96115
// 動作安定のためのスリープ
97116
this_thread::sleep_for(chrono::milliseconds(10));
98117

99-
// 黒線復帰のための回頭をする
100118
PidGain postPidGain = { kp, ki, kd };
119+
120+
// 黒線復帰のための後退をする
121+
IMUDistanceStraight postStraight(robot, postTargetDistance, postTargetSpeed, postPidGain);
122+
postStraight.run();
123+
124+
// 動作安定のためのスリープ
125+
this_thread::sleep_for(chrono::milliseconds(10));
126+
127+
robot.getMotorControllerInstance().stopWheelsMotor();
128+
129+
// アームを下げる
130+
robot.getMotorControllerInstance().setArmMotorPower(-armPower);
131+
132+
// 動作安定のためのスリープ
133+
this_thread::sleep_for(chrono::milliseconds(10));
134+
135+
// 黒線復帰のための回頭をする
101136
IMUAngleRotation postRotation(robot, postTargetAngle, basePower, !isClockwise, postPidGain,
102137
isAbsoluteAngleMode);
103138
postRotation.run();

0 commit comments

Comments
 (0)