Skip to content

Commit

Permalink
Merge branch 'humble' into mergify/bp/humble/pr-1759
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor authored Nov 4, 2024
2 parents 291e344 + ac9f005 commit 7161cf4
Show file tree
Hide file tree
Showing 7 changed files with 375 additions and 53 deletions.
10 changes: 5 additions & 5 deletions .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.6.0
rev: v5.0.0
hooks:
- id: check-added-large-files
- id: check-ast
Expand All @@ -37,7 +37,7 @@ repos:

# Python hooks
- repo: https://github.com/asottile/pyupgrade
rev: v3.17.0
rev: v3.19.0
hooks:
- id: pyupgrade
args: [--py36-plus]
Expand All @@ -50,7 +50,7 @@ repos:
args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]

- repo: https://github.com/psf/black
rev: 24.8.0
rev: 24.10.0
hooks:
- id: black
args: ["--line-length=99"]
Expand All @@ -63,7 +63,7 @@ repos:

# CPP hooks
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v19.1.0
rev: v19.1.3
hooks:
- id: clang-format
args: ['-fallback-style=none', '-i']
Expand Down Expand Up @@ -133,7 +133,7 @@ repos:
exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$

- repo: https://github.com/python-jsonschema/check-jsonschema
rev: 0.29.3
rev: 0.29.4
hooks:
- id: check-github-workflows
args: ["--verbose"]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -244,24 +244,57 @@ 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:
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]

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

if controller_param_dict and (
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 : {namespaced_controller}"
f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {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
if (
controller_param_dict
and ROS_PARAMS_KEY in controller_param_dict
and parameter_name in controller_param_dict[ROS_PARAMS_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 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 @@ -311,11 +344,11 @@ def set_controller_parameters_from_param_file(
if parameter_file:
spawner_namespace = namespace if namespace else node.get_namespace()
set_controller_parameters(
node, controller_manager_name, controller_name, "param_file", parameter_file
node, controller_manager_name, controller_name, "params_file", parameter_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 Down
29 changes: 21 additions & 8 deletions controller_manager/controller_manager/spawner.py
Original file line number Diff line number Diff line change
Expand Up @@ -244,7 +244,7 @@ def main(args=None):

node.get_logger().info(
bcolors.OKGREEN
+ "Configured and activated all the parsed controllers list!"
+ f"Configured and activated all the parsed controllers list : {controller_names}!"
+ bcolors.ENDC
)
if args.stopped:
Expand All @@ -265,20 +265,33 @@ def main(args=None):
node, controller_manager_name, controller_names, [], True, True, 5.0
)
if not ret.ok:
node.get_logger().error("Failed to deactivate controller")
node.get_logger().error(
bcolors.FAIL + "Failed to deactivate controller" + bcolors.ENDC
)
return 1

node.get_logger().info("Deactivated controller")
node.get_logger().info(
f"Successfully deactivated controllers : {controller_names}"
)

elif args.stopped:
node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead')

ret = unload_controller(node, controller_manager_name, controller_name)
if not ret.ok:
node.get_logger().error("Failed to unload controller")
return 1
unload_status = True
for controller_name in controller_names:
ret = unload_controller(node, controller_manager_name, controller_name)
if not ret.ok:
unload_status = False
node.get_logger().error(
bcolors.FAIL
+ f"Failed to unload controller : {controller_name}"
+ bcolors.ENDC
)

node.get_logger().info("Unloaded controller")
if unload_status:
node.get_logger().info(f"Successfully unloaded controllers : {controller_names}")
else:
return 1
return 0
except KeyboardInterrupt:
pass
Expand Down
54 changes: 54 additions & 0 deletions controller_manager/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,60 @@ There are two scripts to interact with controller manager from launch files:
Time to wait for the controller manager
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
/**/position_trajectory_controller:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
.. code-block:: yaml
/position_trajectory_controller:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
.. code-block:: yaml
position_trajectory_controller:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
.. code-block:: yaml
/rrbot_1/position_trajectory_controller:
ros__parameters:
type: joint_trajectory_controller/JointTrajectoryController
joints:
- joint1
- joint2
command_interfaces:
- position
.....
``unspawner``
^^^^^^^^^^^^^^^^

Expand Down
29 changes: 22 additions & 7 deletions controller_manager/test/test_controller_spawner_with_type.yaml
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"]
Loading

0 comments on commit 7161cf4

Please sign in to comment.