Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[Spawner] Add support for wildcard entries in the controller param files #1724

Merged
merged 15 commits into from
Oct 17, 2024
Merged

[Spawner] Add support for wildcard entries in the controller param files #1724

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
c1fe929
added logic to support the param files with wildcard entries
saikishor Aug 27, 2024
1346b53
rename controllers within the same yaml file to be differentiable
saikishor Aug 27, 2024
0491a02
Add test case for controllers with wildcard entries in the params_file
saikishor Aug 27, 2024
8a9ae26
throw errors when trying to access controller info from a non namespa…
saikishor Aug 27, 2024
0212812
add better error message logging and cleanup
saikishor Aug 27, 2024
90b353d
Minor fix to also find the parameters from global parameter namespace
saikishor Sep 2, 2024
031c21c
Merge branch 'master' into support/wilcard_entries/param_files
saikishor Sep 2, 2024
a8e1efc
Merge branch 'master' into support/wilcard_entries/param_files
saikishor Sep 18, 2024
21872b7
fix tests after rebasing
saikishor Sep 24, 2024
ba4e91a
Merge branch 'master' into support/wilcard_entries/param_files
saikishor Oct 8, 2024
1b524a2
add documentation and release_notes
saikishor Oct 9, 2024
318da2d
Merge branch 'master' into support/wilcard_entries/param_files
saikishor Oct 9, 2024
5b1fe44
fix typo
saikishor Oct 9, 2024
9ff8450
update the logic to work for overriding parameters as well
saikishor Oct 10, 2024
705535a
Merge branch 'master' into support/wilcard_entries/param_files
saikishor Oct 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 43 additions & 16 deletions controller_manager/controller_manager/controller_manager_services.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -253,24 +253,51 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
)


def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
def get_parameter_from_param_file(
node, controller_name, namespace, parameter_file, parameter_name
):
with open(parameter_file) as f:
namespaced_controller = (
controller_name if namespace == "/" else f"{namespace}/{controller_name}"
f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
)
WILDCARD_KEY = "/**"
ROS_PARAMS_KEY = "ros__parameters"
parameters = yaml.safe_load(f)
if namespaced_controller in parameters:
value = parameters[namespaced_controller]
if not isinstance(value, dict) or "ros__parameters" not in value:
raise RuntimeError(
f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
)
if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
return parameters[namespaced_controller]["ros__parameters"][parameter_name]
else:
return None
else:
return None
controller_param_dict = None
# check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
for key in [
controller_name,
namespaced_controller,
f"{WILDCARD_KEY}/{controller_name}",
f"{WILDCARD_KEY}{namespaced_controller}",
]:
if key in parameters:
if key == controller_name and namespace != "/":
node.get_logger().fatal(
f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}"
)
break
controller_param_dict = parameters[key]
break
if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
controller_param_dict = parameters[WILDCARD_KEY][key]
break

if controller_param_dict is None:
node.get_logger().fatal(
f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}"
)
if (
not isinstance(controller_param_dict, dict)
or ROS_PARAMS_KEY not in controller_param_dict
):
raise RuntimeError(
f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}"
)
if parameter_name in controller_param_dict[ROS_PARAMS_KEY]:
return controller_param_dict[ROS_PARAMS_KEY][parameter_name]

return None


def set_controller_parameters(
Expand Down Expand Up @@ -324,7 +351,7 @@ def set_controller_parameters_from_param_file(
)

controller_type = get_parameter_from_param_file(
controller_name, spawner_namespace, parameter_file, "type"
node, controller_name, spawner_namespace, parameter_file, "type"
)
if controller_type:
if not set_controller_parameters(
Expand All @@ -333,7 +360,7 @@ def set_controller_parameters_from_param_file(
return False

fallback_controllers = get_parameter_from_param_file(
controller_name, spawner_namespace, parameter_file, "fallback_controllers"
node, controller_name, spawner_namespace, parameter_file, "fallback_controllers"
)
if fallback_controllers:
if not set_controller_parameters(
Expand Down
29 changes: 22 additions & 7 deletions controller_manager/test/test_controller_spawner_with_type.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,25 +3,40 @@ ctrl_with_parameters_and_type:
type: "controller_manager/test_controller"
joint_names: ["joint0"]

chainable_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_chainable_controller"
joint_names: ["joint1"]
/**:
chainable_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_chainable_controller"
joint_names: ["joint1"]

wildcard_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_controller"
joint_names: ["joint1"]

ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]

/foo_namespace/ctrl_with_parameters_and_type:
/foo_namespace/ns_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_controller"
joint_names: ["joint1"]

/foo_namespace/chainable_ctrl_with_parameters_and_type:
/foo_namespace/ns_chainable_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_chainable_controller"
joint_names: ["joint1"]

/foo_namespace/ns_ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]

/**/wildcard_chainable_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_chainable_controller"
joint_names: ["joint1"]

/foo_namespace/ctrl_with_parameters_and_no_type:
/**/wildcard_ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]
140 changes: 124 additions & 16 deletions controller_manager/test/test_spawner_unspawner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -674,22 +674,22 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
// Provide controller type via the parsed file
EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
"ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --controller-manager-timeout 1.0 -p " +
test_file_path),
256)
<< "Should fail without the namespacing it";
EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
"ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager -p " +
test_file_path + " --ros-args -r __ns:=/foo_namespace"),
0);

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

auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
Expand All @@ -699,7 +699,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)

auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type");
chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.type,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
Expand All @@ -712,23 +712,23 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)

EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path +
"ns_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path +
" --ros-args -r __ns:=/foo_namespace"),
256)
<< "Should fail as no type is defined!";
// Will still be same as the current call will fail
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);

auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);

auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
Expand All @@ -747,28 +747,28 @@ TEST_F(
// Provide controller type via the parsed file
EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
"ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --controller-manager-timeout 1.0 -p " +
test_file_path),
256)
<< "Should fail without the namespacing it";
EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
"ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " +
test_file_path + " --ros-args -r __ns:=/random_namespace"),
256)
<< "Should fail when parsed namespace through both way with different namespaces";
EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
"ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" +
test_file_path + " --ros-args -r __ns:=/foo_namespace"),
256)
<< "Should fail when parsed namespace through both ways even with same namespacing name";
EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
"ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --namespace foo_namespace -p " +
test_file_path),
0)
Expand All @@ -777,7 +777,7 @@ TEST_F(
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);

auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
Expand All @@ -787,7 +787,7 @@ TEST_F(

auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type");
chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.type,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
Expand All @@ -800,27 +800,135 @@ TEST_F(

EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace -p " +
"ns_ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace "
"-p " +
test_file_path),
256)
<< "Should fail as no type is defined!";
// Will still be same as the current call will fail
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);

auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);

auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
test_file_path);
}

TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
"/test/test_controller_spawner_with_type.yaml";

ControllerManagerRunner cm_runner(this);
// Provide controller type via the parsed file
EXPECT_EQ(
call_spawner(
"wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type "
"--load-only -c "
"test_controller_manager --controller-manager-timeout 1.0 -p " +
test_file_path),
256)
<< "Should fail without the namespacing it due to timeout but can find the parameters";
EXPECT_EQ(
call_spawner(
"wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type "
"--load-only -c "
"test_controller_manager -p " +
test_file_path + " --ros-args -r __ns:=/foo_namespace"),
0);

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

auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "wildcard_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.name,
"wildcard_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.type,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

EXPECT_EQ(
call_spawner(
"wildcard_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path +
" --ros-args -r __ns:=/foo_namespace"),
256)
<< "Should fail as no type is defined!";
// Will still be same as the current call will fail
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);

auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "wildcard_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "wildcard_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}

TEST_F(
TestLoadControllerWithNamespacedCM,
spawner_test_fail_namespaced_controllers_with_non_wildcard_entries)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
"/test/test_controller_spawner_with_type.yaml";

ControllerManagerRunner cm_runner(this);
// Provide controller type via the parsed file
EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --controller-manager-timeout 1.0 -p " +
test_file_path),
256)
<< "Should fail without the namespacing it";
EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --namespace foo_namespace -p " +
test_file_path),
256)
<< "Should fail even namespacing it as ctrl_with_parameters_and_type is not a wildcard entry";
EXPECT_EQ(
call_spawner(
"chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --namespace foo_namespace -p " +
test_file_path),
0)
<< "Should work as chainable_ctrl_with_parameters_and_type is a wildcard entry";

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

auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(
ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}
Loading