Skip to content

Commit bb087e2

Browse files
saikishorsaikishor
authored
Fix the spawner to support full wildcard parameter entries (#1933)
1 parent d62be07 commit bb087e2

File tree

5 files changed

+144
-33
lines changed

5 files changed

+144
-33
lines changed

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