Skip to content

Commit f43bc3c

Browse files
ViktorCVSViktorCVS
committed
Adjust unit tests for new parameters
Add new parameters to the existing unit tests in the package.
1 parent 5f88af3 commit f43bc3c

File tree

2 files changed

+139
-11
lines changed

2 files changed

+139
-11
lines changed

‎control_toolbox/test/pid_ros_parameters_tests.cpp

+135-9
Original file line numberDiff line numberDiff line change
@@ -54,10 +54,16 @@ void check_set_parameters(
5454
const double D = 3.0;
5555
const double I_MAX = 10.0;
5656
const double I_MIN = -10.0;
57+
const double U_MAX = 10.0;
58+
const double U_MIN = -10.0;
59+
const double TRK_TC = 4.0;
60+
const bool SATURATION = true;
5761
const bool ANTIWINDUP = true;
62+
const std::string ANTIWINDUP_STRAT = "none";
5863
const bool SAVE_I_TERM = true;
5964

60-
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM));
65+
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
66+
ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM));
6167

6268
rclcpp::Parameter param;
6369

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

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

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

@@ -90,7 +111,12 @@ void check_set_parameters(
90111
ASSERT_EQ(gains.d_gain_, D);
91112
ASSERT_EQ(gains.i_max_, I_MAX);
92113
ASSERT_EQ(gains.i_min_, I_MIN);
114+
ASSERT_EQ(gains.u_max_, U_MAX);
115+
ASSERT_EQ(gains.u_min_, U_MIN);
116+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
117+
ASSERT_TRUE(gains.saturation_);
93118
ASSERT_TRUE(gains.antiwindup_);
119+
ASSERT_EQ(gains.antiwindup_strat_, "none");
94120
}
95121

96122
TEST(PidParametersTest, InitPidTest)
@@ -116,8 +142,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
116142
const double D = 3.0;
117143
const double I_MAX_BAD = -10.0;
118144
const double I_MIN_BAD = 10.0;
145+
const double U_MAX_BAD = -10.0;
146+
const double U_MIN_BAD = 10.0;
147+
const double TRK_TC = 4.0;
119148

120-
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, false));
149+
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD,
150+
TRK_TC, false, false, "none", false));
121151

122152
rclcpp::Parameter param;
123153

@@ -127,7 +157,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
127157
ASSERT_FALSE(node->get_parameter("d", param));
128158
ASSERT_FALSE(node->get_parameter("i_clamp_max", param));
129159
ASSERT_FALSE(node->get_parameter("i_clamp_min", param));
160+
ASSERT_FALSE(node->get_parameter("u_clamp_max", param));
161+
ASSERT_FALSE(node->get_parameter("u_clamp_min", param));
162+
ASSERT_FALSE(node->get_parameter("tracking_time_constant", param));
163+
ASSERT_FALSE(node->get_parameter("saturation", param));
130164
ASSERT_FALSE(node->get_parameter("antiwindup", param));
165+
ASSERT_FALSE(node->get_parameter("antiwindup_strategy", param));
131166

132167
// check gains were NOT set
133168
control_toolbox::Pid::Gains gains = pid.get_gains();
@@ -136,7 +171,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
136171
ASSERT_EQ(gains.d_gain_, 0.0);
137172
ASSERT_EQ(gains.i_max_, 0.0);
138173
ASSERT_EQ(gains.i_min_, 0.0);
174+
ASSERT_EQ(gains.u_max_, 0.0);
175+
ASSERT_EQ(gains.u_min_, 0.0);
176+
ASSERT_EQ(gains.trk_tc_, 0.0);
177+
ASSERT_FALSE(gains.saturation_);
139178
ASSERT_FALSE(gains.antiwindup_);
179+
ASSERT_EQ(gains.antiwindup_strat_, "none");
140180
}
141181

142182
TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot)
@@ -216,10 +256,16 @@ TEST(PidParametersTest, SetParametersTest)
216256
const double D = 3.0;
217257
const double I_MAX = 10.0;
218258
const double I_MIN = -10.0;
259+
const double U_MAX = 10.0;
260+
const double U_MIN = -10.0;
261+
const double TRK_TC = 4.0;
262+
const bool SATURATION = true;
219263
const bool ANTIWINDUP = true;
264+
const std::string ANTIWINDUP_STRAT = "none";
220265
const bool SAVE_I_TERM = false;
221266

222-
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM);
267+
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
268+
ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM);
223269

224270
rcl_interfaces::msg::SetParametersResult set_result;
225271

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

@@ -253,7 +311,12 @@ TEST(PidParametersTest, SetParametersTest)
253311
ASSERT_EQ(gains.d_gain_, D);
254312
ASSERT_EQ(gains.i_max_, I_MAX);
255313
ASSERT_EQ(gains.i_min_, I_MIN);
314+
ASSERT_EQ(gains.u_max_, U_MAX);
315+
ASSERT_EQ(gains.u_min_, U_MIN);
316+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
317+
ASSERT_TRUE(gains.saturation_);
256318
ASSERT_EQ(gains.antiwindup_, ANTIWINDUP);
319+
ASSERT_EQ(gains.antiwindup_strat_, "none");
257320
}
258321

259322
TEST(PidParametersTest, SetBadParametersTest)
@@ -269,9 +332,17 @@ TEST(PidParametersTest, SetBadParametersTest)
269332
const double I_MIN = -10.0;
270333
const double I_MAX_BAD = -20.0;
271334
const double I_MIN_BAD = 20.0;
335+
const double U_MAX = 10.0;
336+
const double U_MIN = -10.0;
337+
const double U_MAX_BAD = -20.0;
338+
const double U_MIN_BAD = 20.0;
339+
const double TRK_TC = 4.0;
340+
const bool SATURATION = true;
272341
const bool ANTIWINDUP = true;
342+
const std::string ANTIWINDUP_STRAT = "none";
273343

274-
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP);
344+
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
345+
ANTIWINDUP, ANTIWINDUP_STRAT, false);
275346

276347
rcl_interfaces::msg::SetParametersResult set_result;
277348

@@ -290,8 +361,20 @@ TEST(PidParametersTest, SetBadParametersTest)
290361
ASSERT_TRUE(set_result.successful);
291362
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN_BAD)));
292363
ASSERT_TRUE(set_result.successful);
364+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX_BAD)));
365+
ASSERT_TRUE(set_result.successful);
366+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN_BAD)));
367+
ASSERT_TRUE(set_result.successful);
368+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter(
369+
"tracking_time_constant", TRK_TC)));
370+
ASSERT_TRUE(set_result.successful);
371+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION)));
372+
ASSERT_TRUE(set_result.successful);
293373
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP)));
294374
ASSERT_TRUE(set_result.successful);
375+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter(
376+
"antiwindup_strategy", ANTIWINDUP_STRAT)));
377+
ASSERT_TRUE(set_result.successful);
295378

296379
// process callbacks
297380
rclcpp::spin_some(node->get_node_base_interface());
@@ -303,7 +386,12 @@ TEST(PidParametersTest, SetBadParametersTest)
303386
ASSERT_EQ(gains.d_gain_, D);
304387
ASSERT_EQ(gains.i_max_, I_MAX);
305388
ASSERT_EQ(gains.i_min_, I_MIN);
389+
ASSERT_EQ(gains.u_max_, U_MAX);
390+
ASSERT_EQ(gains.u_min_, U_MIN);
391+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
392+
ASSERT_TRUE(gains.saturation_);
306393
ASSERT_EQ(gains.antiwindup_, ANTIWINDUP);
394+
ASSERT_EQ(gains.antiwindup_strat_, "none");
307395
}
308396

309397
TEST(PidParametersTest, GetParametersTest)
@@ -317,10 +405,16 @@ TEST(PidParametersTest, GetParametersTest)
317405
const double D = 3.0;
318406
const double I_MAX = 10.0;
319407
const double I_MIN = -10.0;
408+
const double U_MAX = 10.0;
409+
const double U_MIN = -10.0;
410+
const double TRK_TC = 4.0;
411+
const bool SATURATION = true;
320412
const bool ANTIWINDUP = true;
413+
const std::string ANTIWINDUP_STRAT = "none";
321414

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

325419
rclcpp::Parameter param;
326420

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

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

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

378499
TEST(PidParametersTest, MultiplePidInstances)
@@ -387,9 +508,14 @@ TEST(PidParametersTest, MultiplePidInstances)
387508
const double D = 3.0;
388509
const double I_MAX = 10.0;
389510
const double I_MIN = -10.0;
390-
391-
ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, false, false));
392-
ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, true, false));
511+
const double U_MAX = 10.0;
512+
const double U_MIN = -10.0;
513+
const double TRK_TC = 4.0;
514+
515+
ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false,
516+
false, "none", false));
517+
ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true,
518+
true, "none", false));
393519

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

‎control_toolbox/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)