Skip to content

Commit 812dab4

Browse files
ViktorCVSViktorCVS
committed
Adjust unit tests for new parameters
Add new parameters to the existing unit tests in the package.
1 parent bd7c8f0 commit 812dab4

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
@@ -56,10 +56,16 @@ void check_set_parameters(
5656
const double D = 3.0;
5757
const double I_MAX = 10.0;
5858
const double I_MIN = -10.0;
59+
const double U_MAX = 10.0;
60+
const double U_MIN = -10.0;
61+
const double TRK_TC = 4.0;
62+
const bool SATURATION = true;
5963
const bool ANTIWINDUP = true;
64+
const std::string ANTIWINDUP_STRAT = "none";
6065
const bool SAVE_I_TERM = true;
6166

62-
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM));
67+
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
68+
ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM));
6369

6470
rclcpp::Parameter param;
6571

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

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

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

@@ -92,7 +113,12 @@ void check_set_parameters(
92113
ASSERT_EQ(gains.d_gain_, D);
93114
ASSERT_EQ(gains.i_max_, I_MAX);
94115
ASSERT_EQ(gains.i_min_, I_MIN);
116+
ASSERT_EQ(gains.u_max_, U_MAX);
117+
ASSERT_EQ(gains.u_min_, U_MIN);
118+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
119+
ASSERT_TRUE(gains.saturation_);
95120
ASSERT_TRUE(gains.antiwindup_);
121+
ASSERT_EQ(gains.antiwindup_strat_, "none");
96122
}
97123

98124
TEST(PidParametersTest, InitPidTest)
@@ -118,8 +144,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
118144
const double D = 3.0;
119145
const double I_MAX_BAD = -10.0;
120146
const double I_MIN_BAD = 10.0;
147+
const double U_MAX_BAD = -10.0;
148+
const double U_MIN_BAD = 10.0;
149+
const double TRK_TC = 4.0;
121150

122-
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, false));
151+
ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, I_MAX_BAD, I_MIN_BAD, U_MAX_BAD, U_MIN_BAD,
152+
TRK_TC, false, false, "none", false));
123153

124154
rclcpp::Parameter param;
125155

@@ -129,7 +159,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
129159
ASSERT_FALSE(node->get_parameter("d", param));
130160
ASSERT_FALSE(node->get_parameter("i_clamp_max", param));
131161
ASSERT_FALSE(node->get_parameter("i_clamp_min", param));
162+
ASSERT_FALSE(node->get_parameter("u_clamp_max", param));
163+
ASSERT_FALSE(node->get_parameter("u_clamp_min", param));
164+
ASSERT_FALSE(node->get_parameter("tracking_time_constant", param));
165+
ASSERT_FALSE(node->get_parameter("saturation", param));
132166
ASSERT_FALSE(node->get_parameter("antiwindup", param));
167+
ASSERT_FALSE(node->get_parameter("antiwindup_strategy", param));
133168

134169
// check gains were NOT set
135170
control_toolbox::Pid::Gains gains = pid.get_gains();
@@ -138,7 +173,12 @@ TEST(PidParametersTest, InitPidTestBadParameter)
138173
ASSERT_EQ(gains.d_gain_, 0.0);
139174
ASSERT_EQ(gains.i_max_, 0.0);
140175
ASSERT_EQ(gains.i_min_, 0.0);
176+
ASSERT_EQ(gains.u_max_, 0.0);
177+
ASSERT_EQ(gains.u_min_, 0.0);
178+
ASSERT_EQ(gains.trk_tc_, 0.0);
179+
ASSERT_FALSE(gains.saturation_);
141180
ASSERT_FALSE(gains.antiwindup_);
181+
ASSERT_EQ(gains.antiwindup_strat_, "none");
142182
}
143183

144184
TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot)
@@ -218,10 +258,16 @@ TEST(PidParametersTest, SetParametersTest)
218258
const double D = 3.0;
219259
const double I_MAX = 10.0;
220260
const double I_MIN = -10.0;
261+
const double U_MAX = 10.0;
262+
const double U_MIN = -10.0;
263+
const double TRK_TC = 4.0;
264+
const bool SATURATION = true;
221265
const bool ANTIWINDUP = true;
266+
const std::string ANTIWINDUP_STRAT = "none";
222267
const bool SAVE_I_TERM = false;
223268

224-
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_I_TERM);
269+
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
270+
ANTIWINDUP, ANTIWINDUP_STRAT, SAVE_I_TERM);
225271

226272
rcl_interfaces::msg::SetParametersResult set_result;
227273

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

@@ -255,7 +313,12 @@ TEST(PidParametersTest, SetParametersTest)
255313
ASSERT_EQ(gains.d_gain_, D);
256314
ASSERT_EQ(gains.i_max_, I_MAX);
257315
ASSERT_EQ(gains.i_min_, I_MIN);
316+
ASSERT_EQ(gains.u_max_, U_MAX);
317+
ASSERT_EQ(gains.u_min_, U_MIN);
318+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
319+
ASSERT_TRUE(gains.saturation_);
258320
ASSERT_EQ(gains.antiwindup_, ANTIWINDUP);
321+
ASSERT_EQ(gains.antiwindup_strat_, "none");
259322
}
260323

261324
TEST(PidParametersTest, SetBadParametersTest)
@@ -271,9 +334,17 @@ TEST(PidParametersTest, SetBadParametersTest)
271334
const double I_MIN = -10.0;
272335
const double I_MAX_BAD = -20.0;
273336
const double I_MIN_BAD = 20.0;
337+
const double U_MAX = 10.0;
338+
const double U_MIN = -10.0;
339+
const double U_MAX_BAD = -20.0;
340+
const double U_MIN_BAD = 20.0;
341+
const double TRK_TC = 4.0;
342+
const bool SATURATION = true;
274343
const bool ANTIWINDUP = true;
344+
const std::string ANTIWINDUP_STRAT = "none";
275345

276-
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, ANTIWINDUP);
346+
pid.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, SATURATION,
347+
ANTIWINDUP, ANTIWINDUP_STRAT, false);
277348

278349
rcl_interfaces::msg::SetParametersResult set_result;
279350

@@ -292,8 +363,20 @@ TEST(PidParametersTest, SetBadParametersTest)
292363
ASSERT_TRUE(set_result.successful);
293364
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN_BAD)));
294365
ASSERT_TRUE(set_result.successful);
366+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_max", U_MAX_BAD)));
367+
ASSERT_TRUE(set_result.successful);
368+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("u_clamp_min", U_MIN_BAD)));
369+
ASSERT_TRUE(set_result.successful);
370+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter(
371+
"tracking_time_constant", TRK_TC)));
372+
ASSERT_TRUE(set_result.successful);
373+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", SATURATION)));
374+
ASSERT_TRUE(set_result.successful);
295375
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP)));
296376
ASSERT_TRUE(set_result.successful);
377+
ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter(
378+
"antiwindup_strategy", ANTIWINDUP_STRAT)));
379+
ASSERT_TRUE(set_result.successful);
297380

298381
// process callbacks
299382
rclcpp::spin_some(node->get_node_base_interface());
@@ -305,7 +388,12 @@ TEST(PidParametersTest, SetBadParametersTest)
305388
ASSERT_EQ(gains.d_gain_, D);
306389
ASSERT_EQ(gains.i_max_, I_MAX);
307390
ASSERT_EQ(gains.i_min_, I_MIN);
391+
ASSERT_EQ(gains.u_max_, U_MAX);
392+
ASSERT_EQ(gains.u_min_, U_MIN);
393+
ASSERT_EQ(gains.trk_tc_, TRK_TC);
394+
ASSERT_TRUE(gains.saturation_);
308395
ASSERT_EQ(gains.antiwindup_, ANTIWINDUP);
396+
ASSERT_EQ(gains.antiwindup_strat_, "none");
309397
}
310398

311399
TEST(PidParametersTest, GetParametersTest)
@@ -319,10 +407,16 @@ TEST(PidParametersTest, GetParametersTest)
319407
const double D = 3.0;
320408
const double I_MAX = 10.0;
321409
const double I_MIN = -10.0;
410+
const double U_MAX = 10.0;
411+
const double U_MIN = -10.0;
412+
const double TRK_TC = 4.0;
413+
const bool SATURATION = true;
322414
const bool ANTIWINDUP = true;
415+
const std::string ANTIWINDUP_STRAT = "none";
323416

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

327421
rclcpp::Parameter param;
328422

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

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

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

380501
TEST(PidParametersTest, MultiplePidInstances)
@@ -389,9 +510,14 @@ TEST(PidParametersTest, MultiplePidInstances)
389510
const double D = 3.0;
390511
const double I_MAX = 10.0;
391512
const double I_MIN = -10.0;
392-
393-
ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, false, false));
394-
ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, true, false));
513+
const double U_MAX = 10.0;
514+
const double U_MIN = -10.0;
515+
const double TRK_TC = 4.0;
516+
517+
ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, false,
518+
false, "none", false));
519+
ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, I_MAX, I_MIN, U_MAX, U_MIN, TRK_TC, true,
520+
true, "none", false));
395521

396522
rclcpp::Parameter param_1, param_2;
397523
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)