Skip to content

Commit 682aa93

Browse files
saikishorsaikishor
authored
Merge branch 'master' into integrate/pal_statistics
2 parents 553659e + 7374c43 commit 682aa93

File tree

8 files changed

+160
-50
lines changed

8 files changed

+160
-50
lines changed

‎controller_interface/include/controller_interface/controller_interface_base.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -130,7 +130,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
130130
* The configuration is used to check if controller can be activated and to claim interfaces from
131131
* hardware.
132132
* The claimed interfaces are populated in the
133-
* \ref ControllerInterfaceBase::state_interface_ "state_interface_" member.
133+
* \ref ControllerInterfaceBase::state_interfaces_ "state_interfaces_" member.
134134
*
135135
* \returns configuration of state interfaces.
136136
*/

‎controller_manager/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -222,6 +222,9 @@ if(BUILD_TESTING)
222222
install(FILES test/test_controller_overriding_parameters.yaml
223223
DESTINATION test)
224224

225+
install(FILES test/test_controller_spawner_wildcard_entries.yaml
226+
DESTINATION test)
227+
225228
ament_add_gmock(test_hardware_management_srvs
226229
test/test_hardware_management_srvs.cpp
227230
)

‎controller_manager/controller_manager/controller_manager_services.py

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -296,6 +296,7 @@ def get_params_files_with_controller_parameters(
296296
f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
297297
)
298298
WILDCARD_KEY = "/**"
299+
ROS_PARAMS_KEY = "ros__parameters"
299300
parameters = yaml.safe_load(f)
300301
# check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
301302
for key in [
@@ -304,6 +305,8 @@ def get_params_files_with_controller_parameters(
304305
f"{WILDCARD_KEY}/{controller_name}",
305306
f"{WILDCARD_KEY}{namespaced_controller}",
306307
]:
308+
if parameter_file in controller_parameter_files:
309+
break
307310
if key in parameters:
308311
if key == controller_name and namespace != "/":
309312
node.get_logger().fatal(
@@ -314,6 +317,8 @@ def get_params_files_with_controller_parameters(
314317

315318
if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
316319
controller_parameter_files.append(parameter_file)
320+
if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]:
321+
controller_parameter_files.append(parameter_file)
317322
return controller_parameter_files
318323

319324

@@ -346,6 +351,8 @@ def get_parameter_from_param_files(
346351

347352
if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
348353
controller_param_dict = parameters[WILDCARD_KEY][key]
354+
if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]:
355+
controller_param_dict = parameters[WILDCARD_KEY]
349356

350357
if controller_param_dict and (
351358
not isinstance(controller_param_dict, dict)

‎controller_manager/doc/userdoc.rst

Lines changed: 53 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -171,57 +171,77 @@ There are two scripts to interact with controller manager from launch files:
171171
172172
The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files:
173173

174+
.. code-block:: yaml
175+
176+
/**:
177+
ros__parameters:
178+
type: joint_trajectory_controller/JointTrajectoryController
179+
180+
command_interfaces:
181+
- position
182+
.....
183+
184+
position_trajectory_controller_joint1:
185+
ros__parameters:
186+
joints:
187+
- joint1
188+
189+
position_trajectory_controller_joint2:
190+
ros__parameters:
191+
joints:
192+
- joint2
193+
174194
.. code-block:: yaml
175195
176196
/**/position_trajectory_controller:
177-
ros__parameters:
178-
type: joint_trajectory_controller/JointTrajectoryController
179-
joints:
180-
- joint1
181-
- joint2
197+
ros__parameters:
198+
type: joint_trajectory_controller/JointTrajectoryController
199+
joints:
200+
- joint1
201+
- joint2
182202
183-
command_interfaces:
184-
- position
185-
.....
203+
command_interfaces:
204+
- position
205+
.....
186206
187207
.. code-block:: yaml
188208
189209
/position_trajectory_controller:
190-
ros__parameters:
191-
type: joint_trajectory_controller/JointTrajectoryController
192-
joints:
193-
- joint1
194-
- joint2
210+
ros__parameters:
211+
type: joint_trajectory_controller/JointTrajectoryController
212+
joints:
213+
- joint1
214+
- joint2
195215
196-
command_interfaces:
197-
- position
198-
.....
216+
command_interfaces:
217+
- position
218+
.....
199219
200220
.. code-block:: yaml
201221
202222
position_trajectory_controller:
203-
ros__parameters:
204-
type: joint_trajectory_controller/JointTrajectoryController
205-
joints:
206-
- joint1
207-
- joint2
223+
ros__parameters:
224+
type: joint_trajectory_controller/JointTrajectoryController
225+
joints:
226+
- joint1
227+
- joint2
208228
209-
command_interfaces:
210-
- position
211-
.....
229+
command_interfaces:
230+
- position
231+
.....
212232
213233
.. code-block:: yaml
214234
215235
/rrbot_1/position_trajectory_controller:
216-
ros__parameters:
217-
type: joint_trajectory_controller/JointTrajectoryController
218-
joints:
219-
- joint1
220-
- joint2
221-
222-
command_interfaces:
223-
- position
224-
.....
236+
ros__parameters:
237+
type: joint_trajectory_controller/JointTrajectoryController
238+
joints:
239+
- joint1
240+
- joint2
241+
242+
command_interfaces:
243+
- position
244+
.....
225245
226246
``unspawner``
227247
^^^^^^^^^^^^^^^^

‎controller_manager/src/controller_manager.cpp

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -2374,17 +2374,16 @@ controller_interface::return_type ControllerManager::update(
23742374
update_loop_counter_ %= update_rate_;
23752375

23762376
// Check for valid time
2377-
if (
2378-
!get_clock()->started() &&
2379-
time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
2380-
{
2381-
throw std::runtime_error(
2382-
"No clock received, and time argument is zero. Check your controller_manager node's "
2383-
"clock configuration (use_sim_time parameter) and if a valid clock source is "
2384-
"available. Also pass a proper time argument to the update method.");
2385-
}
2386-
else
2377+
if (!get_clock()->started())
23872378
{
2379+
if (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
2380+
{
2381+
throw std::runtime_error(
2382+
"No clock received, and time argument is zero. Check your controller_manager node's "
2383+
"clock configuration (use_sim_time parameter) and if a valid clock source is "
2384+
"available. Also pass a proper time argument to the update method.");
2385+
}
2386+
23882387
// this can happen with use_sim_time=true until the /clock is received
23892388
rclcpp::Clock clock = rclcpp::Clock();
23902389
RCLCPP_WARN_THROTTLE(

‎controller_manager/test/test_controller_manager.cpp

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -607,11 +607,11 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
607607
EXPECT_THAT(
608608
cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
609609
testing::AllOf(
610-
testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate()))));
610+
testing::Ge(0.75 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate()))));
611611
EXPECT_THAT(
612612
cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
613613
testing::AllOf(
614-
testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate()))));
614+
testing::Ge(0.75 * cm_->get_update_rate()), testing::Lt((2.0 * cm_->get_update_rate()))));
615615
loop_rate.sleep();
616616
}
617617
// if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -778,10 +778,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
778778
testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity))));
779779
EXPECT_THAT(
780780
cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
781-
testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
781+
testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
782782
EXPECT_THAT(
783783
cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
784-
testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
784+
testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((2.0 * exp_periodicity))));
785785
EXPECT_LT(
786786
cm_->get_loaded_controllers()[0].execution_time_statistics->Average(),
787787
50.0); // 50 microseconds
@@ -924,10 +924,10 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an
924924
testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity))));
925925
EXPECT_THAT(
926926
cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
927-
testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
927+
testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
928928
EXPECT_THAT(
929929
cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
930-
testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
930+
testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((2.0 * exp_periodicity))));
931931
EXPECT_LT(
932932
cm_->get_loaded_controllers()[0].execution_time_statistics->Average(),
933933
12000); // more or less 12 milliseconds considering the waittime in the controller

‎controller_manager/test/test_controller_spawner_wildcard_entries.yaml

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
/**:
2+
ros__parameters:
3+
type: "controller_manager/test_controller"
4+
joint_names: ["joint1"]
5+
param1: 1.0
6+
param2: 2.0
7+
8+
wildcard_ctrl_3:
9+
ros__parameters:
10+
param3: 3.0

‎controller_manager/test/test_spawner_unspawner.cpp

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -366,6 +366,77 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
366366
test_file_path);
367367
}
368368

369+
TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name)
370+
{
371+
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
372+
"/test/test_controller_spawner_wildcard_entries.yaml";
373+
374+
ControllerManagerRunner cm_runner(this);
375+
// Provide controller type via the parsed file
376+
EXPECT_EQ(
377+
call_spawner(
378+
"wildcard_ctrl_1 wildcard_ctrl_2 wildcard_ctrl_3 -c test_controller_manager "
379+
"--controller-manager-timeout 1.0 "
380+
"-p " +
381+
test_file_path),
382+
0);
383+
384+
auto verify_ctrl_parameter = [](const auto & ctrl_node, bool has_param_3)
385+
{
386+
if (!ctrl_node->has_parameter("joint_names"))
387+
{
388+
ctrl_node->declare_parameter("joint_names", std::vector<std::string>({"random_joint"}));
389+
}
390+
ASSERT_THAT(
391+
ctrl_node->get_parameter("joint_names").as_string_array(),
392+
std::vector<std::string>({"joint1"}));
393+
394+
if (!ctrl_node->has_parameter("param1"))
395+
{
396+
ctrl_node->declare_parameter("param1", -10.0);
397+
}
398+
ASSERT_THAT(ctrl_node->get_parameter("param1").as_double(), 1.0);
399+
400+
if (!ctrl_node->has_parameter("param2"))
401+
{
402+
ctrl_node->declare_parameter("param2", -10.0);
403+
}
404+
ASSERT_THAT(ctrl_node->get_parameter("param2").as_double(), 2.0);
405+
406+
if (!ctrl_node->has_parameter("param3"))
407+
{
408+
ctrl_node->declare_parameter("param3", -10.0);
409+
}
410+
ASSERT_THAT(ctrl_node->get_parameter("param3").as_double(), has_param_3 ? 3.0 : -10.0);
411+
};
412+
413+
ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
414+
415+
auto wildcard_ctrl_3 = cm_->get_loaded_controllers()[0];
416+
ASSERT_EQ(wildcard_ctrl_3.info.name, "wildcard_ctrl_3");
417+
ASSERT_EQ(wildcard_ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
418+
ASSERT_EQ(
419+
wildcard_ctrl_3.c->get_lifecycle_state().id(),
420+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
421+
verify_ctrl_parameter(wildcard_ctrl_3.c->get_node(), true);
422+
423+
auto wildcard_ctrl_2 = cm_->get_loaded_controllers()[1];
424+
ASSERT_EQ(wildcard_ctrl_2.info.name, "wildcard_ctrl_2");
425+
ASSERT_EQ(wildcard_ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
426+
ASSERT_EQ(
427+
wildcard_ctrl_2.c->get_lifecycle_state().id(),
428+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
429+
verify_ctrl_parameter(wildcard_ctrl_2.c->get_node(), false);
430+
431+
auto wildcard_ctrl_1 = cm_->get_loaded_controllers()[2];
432+
ASSERT_EQ(wildcard_ctrl_1.info.name, "wildcard_ctrl_1");
433+
ASSERT_EQ(wildcard_ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
434+
ASSERT_EQ(
435+
wildcard_ctrl_1.c->get_lifecycle_state().id(),
436+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
437+
verify_ctrl_parameter(wildcard_ctrl_1.c->get_node(), false);
438+
}
439+
369440
TEST_F(TestLoadController, unload_on_kill)
370441
{
371442
// Launch spawner with unload on kill

0 commit comments

Comments
 (0)