Skip to content

Commit

Permalink
Fix the spawner to support full wildcard parameter entries (#1933)
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor authored Dec 12, 2024
1 parent d62be07 commit bb087e2
Show file tree
Hide file tree
Showing 5 changed files with 144 additions and 33 deletions.
3 changes: 3 additions & 0 deletions controller_manager/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -222,6 +222,9 @@ if(BUILD_TESTING)
install(FILES test/test_controller_overriding_parameters.yaml
DESTINATION test)

install(FILES test/test_controller_spawner_wildcard_entries.yaml
DESTINATION test)

ament_add_gmock(test_hardware_management_srvs
test/test_hardware_management_srvs.cpp
)
Expand Down
7 changes: 7 additions & 0 deletions controller_manager/controller_manager/controller_manager_services.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,6 +296,7 @@ def get_params_files_with_controller_parameters(
f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
)
WILDCARD_KEY = "/**"
ROS_PARAMS_KEY = "ros__parameters"
parameters = yaml.safe_load(f)
# check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
for key in [
Expand All @@ -304,6 +305,8 @@ def get_params_files_with_controller_parameters(
f"{WILDCARD_KEY}/{controller_name}",
f"{WILDCARD_KEY}{namespaced_controller}",
]:
if parameter_file in controller_parameter_files:
break
if key in parameters:
if key == controller_name and namespace != "/":
node.get_logger().fatal(
Expand All @@ -314,6 +317,8 @@ def get_params_files_with_controller_parameters(

if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
controller_parameter_files.append(parameter_file)
if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]:
controller_parameter_files.append(parameter_file)
return controller_parameter_files


Expand Down Expand Up @@ -346,6 +351,8 @@ def get_parameter_from_param_files(

if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
controller_param_dict = parameters[WILDCARD_KEY][key]
if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]:
controller_param_dict = parameters[WILDCARD_KEY]

if controller_param_dict and (
not isinstance(controller_param_dict, dict)
Expand Down
86 changes: 53 additions & 33 deletions controller_manager/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,57 +171,77 @@ There are two scripts to interact with controller manager from launch files:
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:

.. code-block:: yaml
/**:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
command_interfaces:
- position
.....
position_trajectory_controller_joint1:
ros__parameters:
joints:
- joint1
position_trajectory_controller_joint2:
ros__parameters:
joints:
- joint2
.. code-block:: yaml
/**/position_trajectory_controller:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
command_interfaces:
- position
.....
.. code-block:: yaml
/position_trajectory_controller:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
command_interfaces:
- position
.....
.. code-block:: yaml
position_trajectory_controller:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
command_interfaces:
- position
.....
.. code-block:: yaml
/rrbot_1/position_trajectory_controller:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
``unspawner``
^^^^^^^^^^^^^^^^
Expand Down
10 changes: 10 additions & 0 deletions controller_manager/test/test_controller_spawner_wildcard_entries.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
/**:
ros__parameters:
type: "controller_manager/test_controller"
joint_names: ["joint1"]
param1: 1.0
param2: 2.0

wildcard_ctrl_3:
ros__parameters:
param3: 3.0
71 changes: 71 additions & 0 deletions controller_manager/test/test_spawner_unspawner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -366,6 +366,77 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
test_file_path);
}

TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
"/test/test_controller_spawner_wildcard_entries.yaml";

ControllerManagerRunner cm_runner(this);
// Provide controller type via the parsed file
EXPECT_EQ(
call_spawner(
"wildcard_ctrl_1 wildcard_ctrl_2 wildcard_ctrl_3 -c test_controller_manager "
"--controller-manager-timeout 1.0 "
"-p " +
test_file_path),
0);

auto verify_ctrl_parameter = [](const auto & ctrl_node, bool has_param_3)
{
if (!ctrl_node->has_parameter("joint_names"))
{
ctrl_node->declare_parameter("joint_names", std::vector<std::string>({"random_joint"}));
}
ASSERT_THAT(
ctrl_node->get_parameter("joint_names").as_string_array(),
std::vector<std::string>({"joint1"}));

if (!ctrl_node->has_parameter("param1"))
{
ctrl_node->declare_parameter("param1", -10.0);
}
ASSERT_THAT(ctrl_node->get_parameter("param1").as_double(), 1.0);

if (!ctrl_node->has_parameter("param2"))
{
ctrl_node->declare_parameter("param2", -10.0);
}
ASSERT_THAT(ctrl_node->get_parameter("param2").as_double(), 2.0);

if (!ctrl_node->has_parameter("param3"))
{
ctrl_node->declare_parameter("param3", -10.0);
}
ASSERT_THAT(ctrl_node->get_parameter("param3").as_double(), has_param_3 ? 3.0 : -10.0);
};

ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);

auto wildcard_ctrl_3 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(wildcard_ctrl_3.info.name, "wildcard_ctrl_3");
ASSERT_EQ(wildcard_ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
wildcard_ctrl_3.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
verify_ctrl_parameter(wildcard_ctrl_3.c->get_node(), true);

auto wildcard_ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(wildcard_ctrl_2.info.name, "wildcard_ctrl_2");
ASSERT_EQ(wildcard_ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
wildcard_ctrl_2.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
verify_ctrl_parameter(wildcard_ctrl_2.c->get_node(), false);

auto wildcard_ctrl_1 = cm_->get_loaded_controllers()[2];
ASSERT_EQ(wildcard_ctrl_1.info.name, "wildcard_ctrl_1");
ASSERT_EQ(wildcard_ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
wildcard_ctrl_1.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
verify_ctrl_parameter(wildcard_ctrl_1.c->get_node(), false);
}

TEST_F(TestLoadController, unload_on_kill)
{
// Launch spawner with unload on kill
Expand Down

0 comments on commit bb087e2

Please sign in to comment.