Skip to content

Commit 8931acc

Browse files
ViktorCVSViktorCVS
committed
Adjust unit tests for new parameters
Add new parameters to the existing unit tests in the package.
1 parent 178031a commit 8931acc

File tree

2 files changed

+139
-11
lines changed

2 files changed

+139
-11
lines changed

‎test/pid_ros_parameters_tests.cpp

+135-9
Original file line numberDiff line numberDiff line change
@@ -55,10 +55,16 @@ void check_set_parameters(
5555
const double D = 3.0;
5656
const double I_MAX = 10.0;
5757
const double I_MIN = -10.0;
58+
const double U_MAX = 10.0;
59+
const double U_MIN = -10.0;
60+
const double TRK_TC = 4.0;
61+
const bool SATURATION = true;
5862
const bool ANTIWINDUP = true;
63+
const std::string ANTIWINDUP_STRAT = "none";
5964
const bool SAVE_I_TERM = true;
6065

61-
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM));
66+
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
67+
ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM));
6268

6369
rclcpp::Parameter param;
6470

@@ -78,9 +84,24 @@ void check_set_parameters(
7884
ASSERT_TRUE(node->get_parameter(prefix + "i_clamp_min", param));
7985
ASSERT_EQ(param.get_value<double>(), I_MIN);
8086

87+
ASSERT_TRUE(node->get_parameter(prefix + "u_clamp_max", param));
88+
ASSERT_EQ(param.get_value<double>(), U_MAX);
89+
90+
ASSERT_TRUE(node->get_parameter(prefix + "u_clamp_min", param));
91+
ASSERT_EQ(param.get_value<double>(), U_MIN);
92+
93+
ASSERT_TRUE(node->get_parameter(prefix + "tracking_time_constant", param));
94+
ASSERT_EQ(param.get_value<double>(), TRK_TC);
95+
96+
ASSERT_TRUE(node->get_parameter(prefix + "saturation", param));
97+
ASSERT_EQ(param.get_value<bool>(), SATURATION);
98+
8199
ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param));
82100
ASSERT_EQ(param.get_value<bool>(), ANTIWINDUP);
83101

102+
ASSERT_TRUE(node->get_parameter(prefix + "antiwindup_strategy", param));
103+
ASSERT_EQ(param.get_value<std::string>(), ANTIWINDUP_STRAT);
104+
84105
ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param));
85106
ASSERT_EQ(param.get_value<bool>(), SAVE_I_TERM);
86107

@@ -91,7 +112,12 @@ void check_set_parameters(
91112
ASSERT_EQ(gains.d_gain_, D);
92113
ASSERT_EQ(gains.i_max_, I_MAX);
93114
ASSERT_EQ(gains.i_min_, I_MIN);
115+
ASSERT_EQ(gains.u_max_, U_MAX);
116+
ASSERT_EQ(gains.u_min_, U_MIN);
117+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
118+
ASSERT_TRUE(gains.saturation_);
94119
ASSERT_TRUE(gains.antiwindup_);
120+
ASSERT_EQ(gains.antiwindup_strat_, "none");
95121
}
96122

97123
TEST(PidParametersTest, InitPidTest)
@@ -117,8 +143,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
117143
const double D = 3.0;
118144
const double I_MAX_BAD = -10.0;
119145
const double I_MIN_BAD = 10.0;
146+
const double U_MAX_BAD = -10.0;
147+
const double U_MIN_BAD = 10.0;
148+
const double TRK_TC = 4.0;
120149

121-
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, false));
150+
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD,
151+
TRK_TC, false, false, "none", false));
122152

123153
rclcpp::Parameter param;
124154

@@ -128,7 +158,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
128158
ASSERT_FALSE(node->get_parameter("d", param));
129159
ASSERT_FALSE(node->get_parameter("i_clamp_max", param));
130160
ASSERT_FALSE(node->get_parameter("i_clamp_min", param));
161+
ASSERT_FALSE(node->get_parameter("u_clamp_max", param));
162+
ASSERT_FALSE(node->get_parameter("u_clamp_min", param));
163+
ASSERT_FALSE(node->get_parameter("tracking_time_constant", param));
164+
ASSERT_FALSE(node->get_parameter("saturation", param));
131165
ASSERT_FALSE(node->get_parameter("antiwindup", param));
166+
ASSERT_FALSE(node->get_parameter("antiwindup_strategy", param));
132167

133168
// check gains were NOT set
134169
control_toolbox::Pid::Gains gains = pid.get_gains();
@@ -137,7 +172,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
137172
ASSERT_EQ(gains.d_gain_, 0.0);
138173
ASSERT_EQ(gains.i_max_, 0.0);
139174
ASSERT_EQ(gains.i_min_, 0.0);
175+
ASSERT_EQ(gains.u_max_, 0.0);
176+
ASSERT_EQ(gains.u_min_, 0.0);
177+
ASSERT_EQ(gains.trk_tc_, 0.0);
178+
ASSERT_FALSE(gains.saturation_);
140179
ASSERT_FALSE(gains.antiwindup_);
180+
ASSERT_EQ(gains.antiwindup_strat_, "none");
141181
}
142182

143183
TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot)
@@ -217,10 +257,16 @@ TEST(PidParametersTest, SetParametersTest)
217257
const double D = 3.0;
218258
const double I_MAX = 10.0;
219259
const double I_MIN = -10.0;
260+
const double U_MAX = 10.0;
261+
const double U_MIN = -10.0;
262+
const double TRK_TC = 4.0;
263+
const bool SATURATION = true;
220264
const bool ANTIWINDUP = true;
265+
const std::string ANTIWINDUP_STRAT = "none";
221266
const bool SAVE_I_TERM = false;
222267

223-
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM);
268+
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
269+
ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM);
224270

225271
rcl_interfaces::msg::SetParametersResult set_result;
226272

@@ -239,8 +285,20 @@ TEST(PidParametersTest, SetParametersTest)
239285
ASSERT_TRUE(set_result.successful);
240286
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN)));
241287
ASSERT_TRUE(set_result.successful);
288+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX)));
289+
ASSERT_TRUE(set_result.successful);
290+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN)));
291+
ASSERT_TRUE(set_result.successful);
292+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter(
293+
"tracking_time_constant", TRK_TC)));
294+
ASSERT_TRUE(set_result.successful);
295+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION)));
296+
ASSERT_TRUE(set_result.successful);
242297
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP)));
243298
ASSERT_TRUE(set_result.successful);
299+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter(
300+
"antiwindup_strategy", ANTIWINDUP_STRAT)));
301+
ASSERT_TRUE(set_result.successful);
244302
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_i_term", SAVE_I_TERM)));
245303
ASSERT_TRUE(set_result.successful);
246304

@@ -254,7 +312,12 @@ TEST(PidParametersTest, SetParametersTest)
254312
ASSERT_EQ(gains.d_gain_, D);
255313
ASSERT_EQ(gains.i_max_, I_MAX);
256314
ASSERT_EQ(gains.i_min_, I_MIN);
315+
ASSERT_EQ(gains.u_max_, U_MAX);
316+
ASSERT_EQ(gains.u_min_, U_MIN);
317+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
318+
ASSERT_TRUE(gains.saturation_);
257319
ASSERT_EQ(gains.antiwindup_, ANTIWINDUP);
320+
ASSERT_EQ(gains.antiwindup_strat_, "none");
258321
}
259322

260323
TEST(PidParametersTest, SetBadParametersTest)
@@ -270,9 +333,17 @@ TEST(PidParametersTest, SetBadParametersTest)
270333
const double I_MIN = -10.0;
271334
const double I_MAX_BAD = -20.0;
272335
const double I_MIN_BAD = 20.0;
336+
const double U_MAX = 10.0;
337+
const double U_MIN = -10.0;
338+
const double U_MAX_BAD = -20.0;
339+
const double U_MIN_BAD = 20.0;
340+
const double TRK_TC = 4.0;
341+
const bool SATURATION = true;
273342
const bool ANTIWINDUP = true;
343+
const std::string ANTIWINDUP_STRAT = "none";
274344

275-
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP);
345+
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
346+
ANTIWINDUP, ANTIWINDUP_STRAT, false);
276347

277348
rcl_interfaces::msg::SetParametersResult set_result;
278349

@@ -291,8 +362,20 @@ TEST(PidParametersTest, SetBadParametersTest)
291362
ASSERT_TRUE(set_result.successful);
292363
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN_BAD)));
293364
ASSERT_TRUE(set_result.successful);
365+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX_BAD)));
366+
ASSERT_TRUE(set_result.successful);
367+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN_BAD)));
368+
ASSERT_TRUE(set_result.successful);
369+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter(
370+
"tracking_time_constant", TRK_TC)));
371+
ASSERT_TRUE(set_result.successful);
372+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION)));
373+
ASSERT_TRUE(set_result.successful);
294374
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP)));
295375
ASSERT_TRUE(set_result.successful);
376+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter(
377+
"antiwindup_strategy", ANTIWINDUP_STRAT)));
378+
ASSERT_TRUE(set_result.successful);
296379

297380
// process callbacks
298381
rclcpp::spin_some(node->get_node_base_interface());
@@ -304,7 +387,12 @@ TEST(PidParametersTest, SetBadParametersTest)
304387
ASSERT_EQ(gains.d_gain_, D);
305388
ASSERT_EQ(gains.i_max_, I_MAX);
306389
ASSERT_EQ(gains.i_min_, I_MIN);
390+
ASSERT_EQ(gains.u_max_, U_MAX);
391+
ASSERT_EQ(gains.u_min_, U_MIN);
392+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
393+
ASSERT_TRUE(gains.saturation_);
307394
ASSERT_EQ(gains.antiwindup_, ANTIWINDUP);
395+
ASSERT_EQ(gains.antiwindup_strat_, "none");
308396
}
309397

310398
TEST(PidParametersTest, GetParametersTest)
@@ -318,10 +406,16 @@ TEST(PidParametersTest, GetParametersTest)
318406
const double D = 3.0;
319407
const double I_MAX = 10.0;
320408
const double I_MIN = -10.0;
409+
const double U_MAX = 10.0;
410+
const double U_MIN = -10.0;
411+
const double TRK_TC = 4.0;
412+
const bool SATURATION = true;
321413
const bool ANTIWINDUP = true;
414+
const std::string ANTIWINDUP_STRAT = "none";
322415

323-
pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, false, false);
324-
pid.set_gains(P, I, D, I_MAX, I_MIN, ANTIWINDUP);
416+
pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, false, "none", false);
417+
pid.set_gains(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
418+
ANTIWINDUP, ANTIWINDUP_STRAT);
325419

326420
rclcpp::Parameter param;
327421

@@ -340,9 +434,24 @@ TEST(PidParametersTest, GetParametersTest)
340434
ASSERT_TRUE(node->get_parameter("i_clamp_min", param));
341435
ASSERT_EQ(param.get_value<double>(), I_MIN);
342436

437+
ASSERT_TRUE(node->get_parameter("u_clamp_max", param));
438+
ASSERT_EQ(param.get_value<double>(), U_MAX);
439+
440+
ASSERT_TRUE(node->get_parameter("u_clamp_min", param));
441+
ASSERT_EQ(param.get_value<double>(), U_MIN);
442+
443+
ASSERT_TRUE(node->get_parameter("tracking_time_constant", param));
444+
ASSERT_EQ(param.get_value<double>(), TRK_TC);
445+
446+
ASSERT_TRUE(node->get_parameter("saturation", param));
447+
ASSERT_EQ(param.get_value<bool>(), SATURATION);
448+
343449
ASSERT_TRUE(node->get_parameter("antiwindup", param));
344450
ASSERT_EQ(param.get_value<bool>(), ANTIWINDUP);
345451

452+
ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param));
453+
ASSERT_EQ(param.get_value<std::string>(), ANTIWINDUP_STRAT);
454+
346455
ASSERT_TRUE(node->get_parameter("save_i_term", param));
347456
ASSERT_EQ(param.get_value<bool>(), false);
348457
}
@@ -374,6 +483,18 @@ TEST(PidParametersTest, GetParametersFromParams)
374483
rclcpp::Parameter param_i_clamp_min;
375484
ASSERT_TRUE(node->get_parameter("i_clamp_min", param_i_clamp_min));
376485
ASSERT_TRUE(std::isnan(param_i_clamp_min.get_value<double>()));
486+
487+
rclcpp::Parameter param_u_clamp_max;
488+
ASSERT_TRUE(node->get_parameter("u_clamp_max", param_u_clamp_max));
489+
ASSERT_TRUE(std::isnan(param_u_clamp_max.get_value<double>()));
490+
491+
rclcpp::Parameter param_u_clamp_min;
492+
ASSERT_TRUE(node->get_parameter("u_clamp_min", param_u_clamp_min));
493+
ASSERT_TRUE(std::isnan(param_u_clamp_min.get_value<double>()));
494+
495+
rclcpp::Parameter param_tracking_time_constant;
496+
ASSERT_TRUE(node->get_parameter("tracking_time_constant", param_tracking_time_constant));
497+
ASSERT_TRUE(std::isnan(param_tracking_time_constant.get_value<double>()));
377498
}
378499

379500
TEST(PidParametersTest, MultiplePidInstances)
@@ -388,9 +509,14 @@ TEST(PidParametersTest, MultiplePidInstances)
388509
const double D = 3.0;
389510
const double I_MAX = 10.0;
390511
const double I_MIN = -10.0;
391-
392-
ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, false, false));
393-
ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, true, false));
512+
const double U_MAX = 10.0;
513+
const double U_MIN = -10.0;
514+
const double TRK_TC = 4.0;
515+
516+
ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false,
517+
false, "none", false));
518+
ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true,
519+
true, "none", false));
394520

395521
rclcpp::Parameter param_1, param_2;
396522
ASSERT_TRUE(node->get_parameter("PID_1.p", param_1));

‎test/pid_ros_publisher_tests.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@ TEST(PidPublisherTest, PublishTest)
3939

4040
control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node);
4141

42-
pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false);
42+
pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0,
43+
false, false, "none", false);
4344

4445
bool callback_called = false;
4546
control_msgs::msg::PidState::SharedPtr last_state_msg;
@@ -76,7 +77,8 @@ TEST(PidPublisherTest, PublishTestLifecycle)
7677
std::dynamic_pointer_cast<rclcpp_lifecycle::LifecyclePublisher<control_msgs::msg::PidState>>(
7778
pid_ros.get_pid_state_publisher());
7879

79-
pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false);
80+
pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, 5.0, -5.0, 1.0,
81+
false, false, "none", false);
8082

8183
bool callback_called = false;
8284
control_msgs::msg::PidState::SharedPtr last_state_msg;

0 commit comments

Comments
 (0)