Skip to content

Commit 6c2dbc0

Browse files
VX792VX792
committed
add WIP tests to per-controller-update-rate and use unsigned integers for the update rate variables
1 parent 1f18afa commit 6c2dbc0

File tree

6 files changed

+129
-10
lines changed

6 files changed

+129
-10
lines changed

‎controller_interface/include/controller_interface/controller_interface.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -155,7 +155,7 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
155155
std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
156156
std::shared_ptr<rclcpp::Node> node_;
157157
rclcpp_lifecycle::State lifecycle_state_;
158-
int update_rate_ = 0;
158+
unsigned int update_rate_ = 0;
159159

160160
};
161161

‎controller_manager/include/controller_manager/controller_manager.hpp‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -144,8 +144,7 @@ class ControllerManager : public rclcpp::Node
144144
// rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
145145

146146
//Per controller update rate support
147-
int get_update_rate() const;
148-
147+
unsigned int get_update_rate() const;
149148
void configure();
150149

151150
protected:
@@ -223,6 +222,10 @@ class ControllerManager : public rclcpp::Node
223222
const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,
224223
std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);
225224

225+
//Per controller update rate support
226+
unsigned int update_loop_counter_ = 0;
227+
unsigned int update_rate_ = 100;
228+
226229
private:
227230
std::vector<std::string> get_controller_names();
228231

@@ -353,9 +356,7 @@ class ControllerManager : public rclcpp::Node
353356
};
354357

355358
SwitchParams switch_params_;
356-
//Per controller update rate support
357-
int update_loop_counter_ = 0;
358-
int update_rate_ = 100;
359+
359360

360361
};
361362

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1253,7 +1253,7 @@ void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using(
12531253
}
12541254
}
12551255

1256-
int ControllerManager::get_update_rate() const
1256+
unsigned int ControllerManager::get_update_rate() const
12571257
{
12581258
return update_rate_;
12591259
}

‎controller_manager/src/ros2_control_node.cpp‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -40,8 +40,7 @@ int main(int argc, char ** argv)
4040
std::thread cm_thread([cm]() {
4141
// load controller_manager update time parameter
4242
cm->configure();
43-
int update_rate = cm->get_update_rate();
44-
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", update_rate);
43+
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
4544

4645
while (rclcpp::ok())
4746
{
@@ -52,7 +51,7 @@ int main(int argc, char ** argv)
5251
std::chrono::system_clock::time_point end = std::chrono::system_clock::now();
5352
std::this_thread::sleep_for(std::max(
5453
std::chrono::nanoseconds(0),
55-
std::chrono::nanoseconds(1000000000 / update_rate) -
54+
std::chrono::nanoseconds(1000000000 / cm->get_update_rate()) -
5655
std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin)));
5756
}
5857
});

‎controller_manager/test/controller_manager_test_common.hpp‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,21 @@
3838
constexpr auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT;
3939
constexpr auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
4040

41+
class TestableControllerManager : public controller_manager::ControllerManager
42+
{
43+
public:
44+
TestableControllerManager(std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
45+
std::shared_ptr<rclcpp::Executor> executor,
46+
const std::string & manager_node_name = "controller_manager",
47+
const std::string & namespace_ = "") :
48+
controller_manager::ControllerManager(std::move(resource_manager),executor,manager_node_name,namespace_) {}
49+
50+
int get_internal_update_counter()
51+
{
52+
return update_loop_counter_;
53+
}
54+
};
55+
4156
class ControllerManagerFixture : public ::testing::Test
4257
{
4358
public:

‎controller_manager/test/test_controller_manager.cpp‎

Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,3 +107,107 @@ TEST_F(TestControllerManager, controller_lifecycle)
107107
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
108108
EXPECT_EQ(1, test_controller.use_count());
109109
}
110+
111+
112+
113+
TEST_F(TestControllerManager, per_controller_update_rate) {
114+
auto test_controller = std::make_shared<test_controller::TestController>();
115+
cm_->add_controller(
116+
test_controller, test_controller::TEST_CONTROLLER_NAME,
117+
test_controller::TEST_CONTROLLER_CLASS_NAME);
118+
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
119+
EXPECT_EQ(2, test_controller.use_count());
120+
121+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
122+
EXPECT_EQ(
123+
0u,
124+
test_controller->internal_counter) <<
125+
"Update should not reach an unconfigured controller";
126+
127+
EXPECT_EQ(
128+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
129+
test_controller->get_state().id());
130+
131+
test_controller->get_node()->set_parameter({"update_rate", 4});
132+
// configure controller
133+
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
134+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
135+
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
136+
137+
EXPECT_EQ(
138+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
139+
test_controller->get_state().id());
140+
141+
// Start controller, will take effect at the end of the update function
142+
std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
143+
std::vector<std::string> stop_controllers = {};
144+
auto switch_future = std::async(
145+
std::launch::async,
146+
&controller_manager::ControllerManager::switch_controller, cm_,
147+
start_controllers, stop_controllers,
148+
STRICT, true, rclcpp::Duration(0, 0));
149+
150+
ASSERT_EQ(
151+
std::future_status::timeout,
152+
switch_future.wait_for(std::chrono::milliseconds(100))) <<
153+
"switch_controller should be blocking until next update cycle";
154+
155+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
156+
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
157+
{
158+
ControllerManagerRunner cm_runner(this);
159+
EXPECT_EQ(
160+
controller_interface::return_type::OK,
161+
switch_future.get()
162+
);
163+
}
164+
165+
EXPECT_EQ(
166+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
167+
test_controller->get_state().id());
168+
169+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
170+
EXPECT_GE(test_controller->internal_counter, 1u);
171+
EXPECT_EQ(test_controller->get_update_rate(), 4);
172+
}
173+
174+
TEST_F(TestControllerManager, inactive_controller_does_not_get_updated)
175+
{
176+
//trying to get the cm's internal update rate, but not sure what is the right way to do that
177+
//TestableControllerManager test_cm(std::move(cm_->resource_manager_), cm_->executor_, "controller_manager", "");
178+
//int internal_update_counter = test_cm.get_internal_update_counter();
179+
180+
auto test_controller = std::make_shared<test_controller::TestController>();
181+
cm_->add_controller(
182+
test_controller, test_controller::TEST_CONTROLLER_NAME,
183+
test_controller::TEST_CONTROLLER_CLASS_NAME);
184+
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
185+
EXPECT_EQ(2, test_controller.use_count());
186+
187+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
188+
EXPECT_EQ(
189+
0u,
190+
test_controller->internal_counter) <<
191+
"Update should not reach an unconfigured controller";
192+
193+
EXPECT_EQ(
194+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
195+
test_controller->get_state().id());
196+
197+
test_controller->get_node()->set_parameter({"update_rate", 4});
198+
// configure controller
199+
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
200+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
201+
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
202+
203+
//"Attempt to update the controller a couple of times"
204+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
205+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
206+
207+
EXPECT_EQ(
208+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
209+
test_controller->get_state().id());
210+
211+
212+
//EXPECT_EQ(0, internal_update_counter);
213+
}

0 commit comments

Comments
 (0)