diff --git a/.github/dependabot.yml b/.github/dependabot.yml
index aafd67c236..f5e9921f23 100644
--- a/.github/dependabot.yml
+++ b/.github/dependabot.yml
@@ -18,10 +18,3 @@ updates:
schedule:
interval: "weekly"
target-branch: "humble"
- - package-ecosystem: "github-actions"
- # Workflow files stored in the
- # default location of `.github/workflows`
- directory: "/"
- schedule:
- interval: "weekly"
- target-branch: "iron"
diff --git a/.github/workflows/README.md b/.github/workflows/README.md
index 62007ffc2d..2ad6a249fe 100644
--- a/.github/workflows/README.md
+++ b/.github/workflows/README.md
@@ -3,5 +3,4 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages
:---------: | :----: | :----------: | :-----------: | :---------------:
**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-debian-build.yml)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-rhel-binary-build.yml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling)
**Jazzy** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-source-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-debian-build.yml)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-rhel-binary-build.yml) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#jazzy)
-**Iron** | [`iron`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-source-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-debian-build.yml)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-rhel-binary-build.yml) | [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron)
**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-debian-build.yml)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-rhel-binary-build.yml) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble)
diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml
deleted file mode 100644
index c2d9c19110..0000000000
--- a/.github/workflows/iron-abi-compatibility.yml
+++ /dev/null
@@ -1,27 +0,0 @@
-name: Iron - ABI Compatibility Check
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-abi-compatibility.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control-not-released.iron.repos'
-
-jobs:
- abi_check:
- runs-on: ubuntu-latest
- steps:
- - uses: actions/checkout@v4
- - uses: ros-industrial/industrial_ci@master
- env:
- ROS_DISTRO: iron
- ROS_REPO: testing
- ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }}
- NOT_TEST_BUILD: true
diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml
deleted file mode 100644
index ef90e256a0..0000000000
--- a/.github/workflows/iron-binary-build.yml
+++ /dev/null
@@ -1,47 +0,0 @@
-name: Iron Binary Build
-# author: Denis Štogl
-# description: 'Build & test all dependencies from released (binary) packages.'
-
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control-not-released.iron.repos'
- push:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control-not-released.iron.repos'
- schedule:
- # Run every morning to detect flakiness and broken dependencies
- - cron: '03 1 * * *'
-
-jobs:
- binary:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
- strategy:
- fail-fast: false
- matrix:
- ROS_DISTRO: [iron]
- ROS_REPO: [main, testing]
- with:
- ros_distro: ${{ matrix.ROS_DISTRO }}
- ros_repo: ${{ matrix.ROS_REPO }}
- upstream_workspace: ros2_control-not-released.iron.repos
- ref_for_scheduled_build: iron
diff --git a/.github/workflows/iron-check-docs.yml b/.github/workflows/iron-check-docs.yml
deleted file mode 100644
index e9295dad44..0000000000
--- a/.github/workflows/iron-check-docs.yml
+++ /dev/null
@@ -1,18 +0,0 @@
-name: Iron Check Docs
-
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.rst'
- - '**.md'
- - '**.yaml'
-
-jobs:
- check-docs:
- name: Check Docs
- uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron
- with:
- ROS2_CONTROL_PR: ${{ github.ref }}
diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml
deleted file mode 100644
index ff5be81d7d..0000000000
--- a/.github/workflows/iron-coverage-build.yml
+++ /dev/null
@@ -1,36 +0,0 @@
-name: Coverage Build - Iron
-on:
- workflow_dispatch:
- push:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-coverage-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- - 'codecov.yml'
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-coverage-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- - 'codecov.yml'
-
-jobs:
- coverage_iron:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master
- secrets: inherit
- with:
- ros_distro: iron
diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml
deleted file mode 100644
index 3cbe0c5127..0000000000
--- a/.github/workflows/iron-debian-build.yml
+++ /dev/null
@@ -1,33 +0,0 @@
-name: Debian Iron Source Build
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-debian-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- schedule:
- # Run every day to detect flakiness and broken dependencies
- - cron: '03 1 * * *'
-
-
-jobs:
- debian_source_build:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master
- strategy:
- fail-fast: false
- matrix:
- ROS_DISTRO: [iron]
- with:
- ros_distro: ${{ matrix.ROS_DISTRO }}
- upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
- ref_for_scheduled_build: master
- skip_packages: rqt_controller_manager
- skip_packages_test: controller_manager_msgs
diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml
deleted file mode 100644
index a128958031..0000000000
--- a/.github/workflows/iron-pre-commit.yml
+++ /dev/null
@@ -1,13 +0,0 @@
-name: Pre-Commit - Iron
-
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
-
-jobs:
- pre-commit:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master
- with:
- ros_distro: iron
diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml
deleted file mode 100644
index f308c495f3..0000000000
--- a/.github/workflows/iron-rhel-binary-build.yml
+++ /dev/null
@@ -1,31 +0,0 @@
-name: RHEL Iron Semi-Binary Build
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-rhel-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- schedule:
- # Run every day to detect flakiness and broken dependencies
- - cron: '03 1 * * *'
-
-jobs:
- rhel_semi_binary_build:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master
- strategy:
- fail-fast: false
- matrix:
- ROS_DISTRO: [iron]
- with:
- ros_distro: ${{ matrix.ROS_DISTRO }}
- upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
- ref_for_scheduled_build: iron
- skip_packages: rqt_controller_manager
diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml
deleted file mode 100644
index c2d137bde0..0000000000
--- a/.github/workflows/iron-semi-binary-build.yml
+++ /dev/null
@@ -1,47 +0,0 @@
-name: Iron Semi-Binary Build
-# author: Denis Štogl
-# description: 'Build & test all dependencies from released (binary) packages.'
-
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-semi-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- push:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-semi-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- schedule:
- # Run every morning to detect flakiness and broken dependencies
- - cron: '03 1 * * *'
-
-jobs:
- binary:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
- strategy:
- fail-fast: false
- matrix:
- ROS_DISTRO: [iron]
- ROS_REPO: [testing]
- with:
- ros_distro: ${{ matrix.ROS_DISTRO }}
- ros_repo: ${{ matrix.ROS_REPO }}
- upstream_workspace: ros2_control.iron.repos
- ref_for_scheduled_build: iron
diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml
deleted file mode 100644
index 3b7c53f6ff..0000000000
--- a/.github/workflows/iron-source-build.yml
+++ /dev/null
@@ -1,27 +0,0 @@
-name: Iron Source Build
-on:
- workflow_dispatch:
- push:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-source-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- schedule:
- # Run every day to detect flakiness and broken dependencies
- - cron: '03 3 * * *'
-
-jobs:
- source:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master
- with:
- ros_distro: iron
- ref: iron
- ros2_repo_branch: iron
- os_name: ubuntu-22.04
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 205e0f63ab..75c5402ffe 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -63,7 +63,7 @@ repos:
# CPP hooks
- repo: https://github.com/pre-commit/mirrors-clang-format
- rev: v19.1.3
+ rev: v19.1.4
hooks:
- id: clang-format
args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
- repo: https://github.com/python-jsonschema/check-jsonschema
- rev: 0.29.4
+ rev: 0.30.0
hooks:
- id: check-github-workflows
args: ["--verbose"]
diff --git a/README.md b/README.md
index 40d2a3c189..47c6ad1523 100644
--- a/README.md
+++ b/README.md
@@ -13,7 +13,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages
:---------: | :----: | :----------: | :-----------: | :---------------:
**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling)
**Jazzy** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#jazzy)
-**Iron** | [`iron`](https://github.com/ros-controls/ros2_control/tree/iron) | [](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build.yml?branch=iron)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build.yml?branch=iron) | [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron)
**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble)
[Detailed build status](.github/workflows/README.md)
diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
index ae3804919c..e41779cf7b 100644
--- a/controller_interface/include/controller_interface/controller_interface_base.hpp
+++ b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -71,6 +71,25 @@ struct ControllerUpdateStats
unsigned int total_triggers;
unsigned int failed_triggers;
};
+
+/**
+ * Struct to store the status of the controller update method.
+ * The status contains information if the update was triggered successfully, the result of the
+ * update method and the execution duration of the update method. The status is used to provide
+ * feedback to the controller_manager.
+ * @var successful: true if the update was triggered successfully, false if not.
+ * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR.
+ * @var execution_time: duration of the execution of the update method.
+ * @var period: period of the update method.
+ */
+struct ControllerUpdateStatus
+{
+ bool successful = true;
+ return_type result = return_type::OK;
+ std::optional execution_time = std::nullopt;
+ std::optional period = std::nullopt;
+};
+
/**
* Base interface class for an controller. The interface may not be used to implement a controller.
* The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
@@ -175,13 +194,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
*
* \param[in] time The time at the start of this control loop iteration
* \param[in] period The measured time taken by the last control loop iteration
- * \returns A pair with the first element being a boolean indicating if the async callback method
- * was triggered and the second element being the last return value of the async function. For
- * more details check the AsyncFunctionHandler implementation in `realtime_tools` package.
+ * \returns ControllerUpdateStatus. The status contains information if the update was triggered
+ * successfully, the result of the update method and the execution duration of the update method.
*/
CONTROLLER_INTERFACE_PUBLIC
- std::pair trigger_update(
- const rclcpp::Time & time, const rclcpp::Duration & period);
+ ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period);
CONTROLLER_INTERFACE_PUBLIC
std::shared_ptr get_node();
diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
index 0713ec3c25..065fe77061 100644
--- a/controller_interface/src/controller_interface_base.cpp
+++ b/controller_interface/src/controller_interface_base.cpp
@@ -159,12 +159,14 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
return node_->get_current_state();
}
-std::pair ControllerInterfaceBase::trigger_update(
+ControllerUpdateStatus ControllerInterfaceBase::trigger_update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
+ ControllerUpdateStatus status;
trigger_stats_.total_triggers++;
if (is_async())
{
+ const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time();
const auto result = async_handler_->trigger_async_callback(time, period);
if (!result.first)
{
@@ -174,12 +176,28 @@ std::pair ControllerInterfaceBase::trigger_update(
"The controller missed %u update cycles out of %u total triggers.",
trigger_stats_.failed_triggers, trigger_stats_.total_triggers);
}
- return result;
+ status.successful = result.first;
+ status.result = result.second;
+ const auto execution_time = async_handler_->get_last_execution_time();
+ if (execution_time.count() > 0)
+ {
+ status.execution_time = execution_time;
+ }
+ if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
+ {
+ status.period = time - last_trigger_time;
+ }
}
else
{
- return std::make_pair(true, update(time, period));
+ const auto start_time = std::chrono::steady_clock::now();
+ status.successful = true;
+ status.result = update(time, period);
+ status.execution_time = std::chrono::duration_cast(
+ std::chrono::steady_clock::now() - start_time);
+ status.period = period;
}
+ return status;
}
std::shared_ptr ControllerInterfaceBase::get_node()
diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index 1bb84eb32c..dba9531e45 100644
--- a/controller_manager/CMakeLists.txt
+++ b/controller_manager/CMakeLists.txt
@@ -16,6 +16,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rclcpp
realtime_tools
std_msgs
+ libstatistics_collector
+ generate_parameter_library
)
find_package(ament_cmake REQUIRED)
@@ -27,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()
+generate_parameter_library(controller_manager_parameters
+ src/controller_manager_parameters.yaml
+)
+
add_library(controller_manager SHARED
src/controller_manager.cpp
)
@@ -35,6 +41,9 @@ target_include_directories(controller_manager PUBLIC
$
$
)
+target_link_libraries(controller_manager PUBLIC
+ controller_manager_parameters
+)
ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
# Causes the visibility macros to use dllexport rather than dllimport,
@@ -210,6 +219,9 @@ if(BUILD_TESTING)
install(FILES test/test_controller_spawner_with_type.yaml
DESTINATION test)
+ install(FILES test/test_controller_overriding_parameters.yaml
+ DESTINATION test)
+
ament_add_gmock(test_hardware_management_srvs
test/test_hardware_management_srvs.cpp
)
@@ -233,7 +245,7 @@ install(
DESTINATION include/controller_manager
)
install(
- TARGETS controller_manager
+ TARGETS controller_manager controller_manager_parameters
EXPORT export_controller_manager
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py
index 4a8d7daee5..638a28ce86 100644
--- a/controller_manager/controller_manager/__init__.py
+++ b/controller_manager/controller_manager/__init__.py
@@ -23,9 +23,9 @@
set_hardware_component_state,
switch_controllers,
unload_controller,
- get_parameter_from_param_file,
+ get_parameter_from_param_files,
set_controller_parameters,
- set_controller_parameters_from_param_file,
+ set_controller_parameters_from_param_files,
bcolors,
)
@@ -40,8 +40,8 @@
"set_hardware_component_state",
"switch_controllers",
"unload_controller",
- "get_parameter_from_param_file",
+ "get_parameter_from_param_files",
"set_controller_parameters",
- "set_controller_parameters_from_param_file",
+ "set_controller_parameters_from_param_files",
"bcolors",
]
diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py
index 909e681ce6..16dee623e7 100644
--- a/controller_manager/controller_manager/controller_manager_services.py
+++ b/controller_manager/controller_manager/controller_manager_services.py
@@ -123,7 +123,9 @@ def service_caller(
)
-def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
+def configure_controller(
+ node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0
+):
request = ConfigureController.Request()
request.name = controller_name
return service_caller(
@@ -132,10 +134,11 @@ def configure_controller(node, controller_manager_name, controller_name, service
ConfigureController,
request,
service_timeout,
+ call_timeout,
)
-def list_controllers(node, controller_manager_name, service_timeout=0.0):
+def list_controllers(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0):
request = ListControllers.Request()
return service_caller(
node,
@@ -143,10 +146,11 @@ def list_controllers(node, controller_manager_name, service_timeout=0.0):
ListControllers,
request,
service_timeout,
+ call_timeout,
)
-def list_controller_types(node, controller_manager_name, service_timeout=0.0):
+def list_controller_types(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0):
request = ListControllerTypes.Request()
return service_caller(
node,
@@ -154,10 +158,13 @@ def list_controller_types(node, controller_manager_name, service_timeout=0.0):
ListControllerTypes,
request,
service_timeout,
+ call_timeout,
)
-def list_hardware_components(node, controller_manager_name, service_timeout=0.0):
+def list_hardware_components(
+ node, controller_manager_name, service_timeout=0.0, call_timeout=10.0
+):
request = ListHardwareComponents.Request()
return service_caller(
node,
@@ -165,10 +172,13 @@ def list_hardware_components(node, controller_manager_name, service_timeout=0.0)
ListHardwareComponents,
request,
service_timeout,
+ call_timeout,
)
-def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0):
+def list_hardware_interfaces(
+ node, controller_manager_name, service_timeout=0.0, call_timeout=10.0
+):
request = ListHardwareInterfaces.Request()
return service_caller(
node,
@@ -176,10 +186,13 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0)
ListHardwareInterfaces,
request,
service_timeout,
+ call_timeout,
)
-def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
+def load_controller(
+ node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0
+):
request = LoadController.Request()
request.name = controller_name
return service_caller(
@@ -188,10 +201,13 @@ def load_controller(node, controller_manager_name, controller_name, service_time
LoadController,
request,
service_timeout,
+ call_timeout,
)
-def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0):
+def reload_controller_libraries(
+ node, controller_manager_name, force_kill, service_timeout=0.0, call_timeout=10.0
+):
request = ReloadControllerLibraries.Request()
request.force_kill = force_kill
return service_caller(
@@ -200,11 +216,17 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi
ReloadControllerLibraries,
request,
service_timeout,
+ call_timeout,
)
def set_hardware_component_state(
- node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0
+ node,
+ controller_manager_name,
+ component_name,
+ lifecyle_state,
+ service_timeout=0.0,
+ call_timeout=10.0,
):
request = SetHardwareComponentState.Request()
request.name = component_name
@@ -215,6 +237,7 @@ def set_hardware_component_state(
SetHardwareComponentState,
request,
service_timeout,
+ call_timeout,
)
@@ -226,6 +249,7 @@ def switch_controllers(
strict,
activate_asap,
timeout,
+ call_timeout=10.0,
):
request = SwitchController.Request()
request.activate_controllers = activate_controllers
@@ -237,11 +261,17 @@ def switch_controllers(
request.activate_asap = activate_asap
request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg()
return service_caller(
- node, f"{controller_manager_name}/switch_controller", SwitchController, request
+ node,
+ f"{controller_manager_name}/switch_controller",
+ SwitchController,
+ request,
+ call_timeout=call_timeout,
)
-def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
+def unload_controller(
+ node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0
+):
request = UnloadController.Request()
request.name = controller_name
return service_caller(
@@ -250,60 +280,94 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
UnloadController,
request,
service_timeout,
+ call_timeout,
)
-def get_parameter_from_param_file(
- node, controller_name, namespace, parameter_file, parameter_name
+def get_params_files_with_controller_parameters(
+ node, controller_name: str, namespace: str, parameter_files: list
):
- with open(parameter_file) as f:
- namespaced_controller = (
- f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
- )
- WILDCARD_KEY = "/**"
- ROS_PARAMS_KEY = "ros__parameters"
- parameters = yaml.safe_load(f)
- 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}"
+ controller_parameter_files = []
+ for parameter_file in parameter_files:
+ if parameter_file in controller_parameter_files:
+ continue
+ with open(parameter_file) as f:
+ namespaced_controller = (
+ f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
+ )
+ WILDCARD_KEY = "/**"
+ parameters = yaml.safe_load(f)
+ # 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_parameter_files.append(parameter_file)
+
+ if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
+ controller_parameter_files.append(parameter_file)
+ return controller_parameter_files
+
+
+def get_parameter_from_param_files(
+ node, controller_name: str, namespace: str, parameter_files: list, parameter_name: str
+):
+ for parameter_file in parameter_files:
+ with open(parameter_file) as f:
+ namespaced_controller = (
+ f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
+ )
+ WILDCARD_KEY = "/**"
+ ROS_PARAMS_KEY = "ros__parameters"
+ parameters = yaml.safe_load(f)
+ 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 node : {namespaced_controller}"
)
+ if (
+ controller_param_dict
+ and ROS_PARAMS_KEY in controller_param_dict
+ and parameter_name in controller_param_dict[ROS_PARAMS_KEY]
+ ):
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 node : {namespaced_controller}"
- )
- 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
+ if controller_param_dict and parameter_name in controller_param_dict[ROS_PARAMS_KEY]:
+ return controller_param_dict[ROS_PARAMS_KEY][parameter_name]
+ if controller_param_dict is None:
+ node.get_logger().fatal(
+ f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter files : {parameter_files}{bcolors.ENDC}"
+ )
+ return None
def set_controller_parameters(
@@ -347,26 +411,36 @@ def set_controller_parameters(
return True
-def set_controller_parameters_from_param_file(
- node, controller_manager_name, controller_name, parameter_file, namespace=None
+def set_controller_parameters_from_param_files(
+ node, controller_manager_name: str, controller_name: str, parameter_files: list, namespace=None
):
- if parameter_file:
- spawner_namespace = namespace if namespace else node.get_namespace()
+ spawner_namespace = namespace if namespace else node.get_namespace()
+ controller_parameter_files = get_params_files_with_controller_parameters(
+ node, controller_name, spawner_namespace, parameter_files
+ )
+ if controller_parameter_files:
set_controller_parameters(
- node, controller_manager_name, controller_name, "params_file", parameter_file
+ node,
+ controller_manager_name,
+ controller_name,
+ "params_file",
+ controller_parameter_files,
)
- controller_type = get_parameter_from_param_file(
- node, controller_name, spawner_namespace, parameter_file, "type"
+ controller_type = get_parameter_from_param_files(
+ node, controller_name, spawner_namespace, controller_parameter_files, "type"
)
- if controller_type:
- if not set_controller_parameters(
- node, controller_manager_name, controller_name, "type", controller_type
- ):
- return False
+ if controller_type and not set_controller_parameters(
+ node, controller_manager_name, controller_name, "type", controller_type
+ ):
+ return False
- fallback_controllers = get_parameter_from_param_file(
- node, controller_name, spawner_namespace, parameter_file, "fallback_controllers"
+ fallback_controllers = get_parameter_from_param_files(
+ node,
+ controller_name,
+ spawner_namespace,
+ controller_parameter_files,
+ "fallback_controllers",
)
if fallback_controllers:
if not set_controller_parameters(
diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py
index c64b893156..e10096b675 100644
--- a/controller_manager/controller_manager/launch_utils.py
+++ b/controller_manager/controller_manager/launch_utils.py
@@ -20,7 +20,7 @@
def generate_controllers_spawner_launch_description(
- controller_names: list, controller_params_file=None, extra_spawner_args=[]
+ controller_names: list, controller_params_files=None, extra_spawner_args=[]
):
"""
Generate launch description for loading a controller using spawner.
@@ -37,8 +37,8 @@ def generate_controllers_spawner_launch_description(
# Passing controller parameter file to load the controller (Controller type is retrieved from config file)
generate_controllers_spawner_launch_description(
['joint_state_broadcaster'],
- controller_params_file=os.path.join(get_package_share_directory('my_pkg'),
- 'config', 'controller_params.yaml'),
+ controller_params_files=[os.path.join(get_package_share_directory('my_pkg'),
+ 'config', 'controller_params.yaml')],
extra_spawner_args=[--load-only]
)
@@ -62,8 +62,10 @@ def generate_controllers_spawner_launch_description(
]
)
- if controller_params_file:
- spawner_arguments += ["--param-file", controller_params_file]
+ if controller_params_files:
+ for controller_params_file in controller_params_files:
+ if controller_params_file:
+ spawner_arguments += ["--param-file", controller_params_file]
# Setting --unload-on-kill if launch arg unload_on_kill is "true"
# See https://github.com/ros2/launch/issues/290
@@ -98,11 +100,51 @@ def generate_controllers_spawner_launch_description(
)
+def generate_controllers_spawner_launch_description_from_dict(
+ controller_info_dict: dict, extra_spawner_args=[]
+):
+ """
+ Generate launch description for loading a controller using spawner.
+
+ controller_info_dict: dict
+ A dictionary with the following info:
+ - controller_name: str
+ The name of the controller to load as the key
+ - controller_params_file: str or list or None
+ The path to the controller parameter file or a list of paths to multiple parameter files
+ or None if no parameter file is needed as the value of the key
+ If a list is passed, the controller parameters will be overloaded in same order
+ extra_spawner_args: list
+ A list of extra arguments to pass to the controller spawner
+ """
+ if not type(controller_info_dict) is dict:
+ raise ValueError(f"Invalid controller_info_dict type parsed {controller_info_dict}")
+ controller_names = controller_info_dict.keys()
+ controller_params_files = []
+ for controller_name in controller_names:
+ controller_params_file = controller_info_dict[controller_name]
+ if controller_params_file:
+ if type(controller_params_file) is list:
+ controller_params_files.extend(controller_params_file)
+ elif type(controller_params_file) is str:
+ controller_params_files.append(controller_params_file)
+ else:
+ raise ValueError(
+ f"Invalid controller_params_file type parsed in the dict {controller_params_file}"
+ )
+ return generate_controllers_spawner_launch_description(
+ controller_names=controller_names,
+ controller_params_files=controller_params_files,
+ extra_spawner_args=extra_spawner_args,
+ )
+
+
def generate_load_controller_launch_description(
controller_name: str, controller_params_file=None, extra_spawner_args=[]
):
+ controller_params_files = [controller_params_file] if controller_params_file else None
return generate_controllers_spawner_launch_description(
controller_names=[controller_name],
- controller_params_file=controller_params_file,
+ controller_params_file=controller_params_files,
extra_spawner_args=extra_spawner_args,
)
diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
index 7e9fe3443b..c5a23defe4 100644
--- a/controller_manager/controller_manager/spawner.py
+++ b/controller_manager/controller_manager/spawner.py
@@ -26,7 +26,7 @@
load_controller,
switch_controllers,
unload_controller,
- set_controller_parameters_from_param_file,
+ set_controller_parameters_from_param_files,
bcolors,
)
from controller_manager.controller_manager_services import ServiceNotFoundError
@@ -60,8 +60,12 @@ def has_service_names(node, node_name, node_namespace, service_names):
return all(service in client_names for service in service_names)
-def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0):
- controllers = list_controllers(node, controller_manager, service_timeout).controller
+def is_controller_loaded(
+ node, controller_manager, controller_name, service_timeout=0.0, call_timeout=10.0
+):
+ controllers = list_controllers(
+ node, controller_manager, service_timeout, call_timeout
+ ).controller
return any(c.name == controller_name for c in controllers)
@@ -79,8 +83,11 @@ def main(args=None):
parser.add_argument(
"-p",
"--param-file",
- help="Controller param file to be loaded into controller node before configure",
+ help="Controller param file to be loaded into controller node before configure. "
+ "Pass multiple times to load different files for different controllers or to "
+ "override the parameters of the same controller.",
default=None,
+ action="append",
required=False,
)
parser.add_argument(
@@ -110,7 +117,7 @@ def main(args=None):
)
parser.add_argument(
"--controller-manager-timeout",
- help="Time to wait for the controller manager",
+ help="Time to wait for the controller manager service to be available",
required=False,
default=0.0,
type=float,
@@ -124,6 +131,13 @@ def main(args=None):
default=5.0,
type=float,
)
+ parser.add_argument(
+ "--service-call-timeout",
+ help="Time to wait for the service response from the controller manager",
+ required=False,
+ default=10.0,
+ type=float,
+ )
parser.add_argument(
"--activate-as-group",
help="Activates all the parsed controllers list together instead of one by one."
@@ -136,12 +150,15 @@ def main(args=None):
args = parser.parse_args(command_line_args)
controller_names = args.controller_names
controller_manager_name = args.controller_manager
- param_file = args.param_file
+ param_files = args.param_file
controller_manager_timeout = args.controller_manager_timeout
+ service_call_timeout = args.service_call_timeout
switch_timeout = args.switch_timeout
- if param_file and not os.path.isfile(param_file):
- raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
+ if param_files:
+ for param_file in param_files:
+ if not os.path.isfile(param_file):
+ raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
node = Node("spawner_" + controller_names[0])
@@ -174,7 +191,11 @@ def main(args=None):
for controller_name in controller_names:
if is_controller_loaded(
- node, controller_manager_name, controller_name, controller_manager_timeout
+ node,
+ controller_manager_name,
+ controller_name,
+ controller_manager_timeout,
+ service_call_timeout,
):
node.get_logger().warn(
bcolors.WARNING
@@ -182,12 +203,12 @@ def main(args=None):
+ bcolors.ENDC
)
else:
- if param_file:
- if not set_controller_parameters_from_param_file(
+ if param_files:
+ if not set_controller_parameters_from_param_files(
node,
controller_manager_name,
controller_name,
- param_file,
+ param_files,
spawner_namespace,
):
return 1
@@ -207,7 +228,13 @@ def main(args=None):
)
if not args.load_only:
- ret = configure_controller(node, controller_manager_name, controller_name)
+ ret = configure_controller(
+ node,
+ controller_manager_name,
+ controller_name,
+ controller_manager_timeout,
+ service_call_timeout,
+ )
if not ret.ok:
node.get_logger().error(
bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC
@@ -223,6 +250,7 @@ def main(args=None):
True,
True,
switch_timeout,
+ service_call_timeout,
)
if not ret.ok:
node.get_logger().error(
@@ -247,6 +275,7 @@ def main(args=None):
True,
True,
switch_timeout,
+ service_call_timeout,
)
if not ret.ok:
node.get_logger().error(
@@ -279,6 +308,7 @@ def main(args=None):
True,
True,
switch_timeout,
+ service_call_timeout,
)
if not ret.ok:
node.get_logger().error(
diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml
new file mode 100644
index 0000000000..a015765c79
--- /dev/null
+++ b/controller_manager/doc/parameters_context.yaml
@@ -0,0 +1,21 @@
+hardware_components_initial_state: |
+ Map of parameters for controlled lifecycle management of hardware components.
+ The names of the components are defined as attribute of ````-tag in ``robot_description``.
+ Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated.
+ Detailed explanation of each parameter is given below.
+ The full structure of the map is given in the following example:
+
+ .. code-block:: yaml
+
+ hardware_components_initial_state:
+ unconfigured:
+ - "arm1"
+ - "arm2"
+ inactive:
+ - "base3"
+
+diagnostics.threshold.controllers.periodicity: |
+ The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.
+
+diagnostics.threshold.controllers.execution_time: |
+ The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle.
diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst
index ca222d68c0..872a695d0f 100644
--- a/controller_manager/doc/userdoc.rst
+++ b/controller_manager/doc/userdoc.rst
@@ -57,32 +57,6 @@ robot_description [std_msgs::msg::String]
Parameters
-----------
-hardware_components_initial_state
- Map of parameters for controlled lifecycle management of hardware components.
- The names of the components are defined as attribute of ````-tag in ``robot_description``.
- Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated.
- Detailed explanation of each parameter is given below.
- The full structure of the map is given in the following example:
-
-.. code-block:: yaml
-
- hardware_components_initial_state:
- unconfigured:
- - "arm1"
- - "arm2"
- inactive:
- - "base3"
-
-hardware_components_initial_state.unconfigured (optional; list; default: empty)
- Defines which hardware components will be only loaded immediately when controller manager is started.
-
-hardware_components_initial_state.inactive (optional; list; default: empty)
- Defines which hardware components will be configured immediately when controller manager is started.
-
-update_rate (mandatory; integer)
- The frequency of controller manager's real-time update loop.
- This loop reads states from hardware, updates controller and writes commands to hardware.
-
.type
Name of a plugin exported using ``pluginlib`` for a controller.
This is a class from which controller's instance with name "``controller_name``" is created.
@@ -99,6 +73,16 @@ update_rate (mandatory; integer)
The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation.
It is recommended to test the fallback strategy in simulation before deploying it on the real robot.
+.. generate_parameter_library_details::
+ ../src/controller_manager_parameters.yaml
+ parameters_context.yaml
+
+**An example parameter file:**
+
+.. generate_parameter_library_default::
+ ../src/controller_manager_parameters.yaml
+
+
Handling Multiple Controller Managers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -158,7 +142,7 @@ There are two scripts to interact with controller manager from launch files:
$ ros2 run controller_manager spawner -h
usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
- [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group]
+ [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT]
controller_names [controller_names ...]
positional arguments:
@@ -169,14 +153,16 @@ There are two scripts to interact with controller manager from launch files:
-c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
Name of the controller manager ROS node
-p PARAM_FILE, --param-file PARAM_FILE
- Controller param file to be loaded into controller node before configure
+ Controller param file to be loaded into controller node before configure. Pass multiple times to load different files for different controllers or to override the parameters of the same controller.
-n NAMESPACE, --namespace NAMESPACE
DEPRECATED Namespace for the controller_manager and the controller(s)
--load-only Only load the controller and leave unconfigured.
--inactive Load and configure the controller, however do not activate them
-u, --unload-on-kill Wait until this application is interrupted and unload controller
--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
- Time to wait for the controller manager
+ Time to wait for the controller manager service to be available
+ --service-call-timeout SERVICE_CALL_TIMEOUT
+ Time to wait for the service response from the controller manager
--switch-timeout SWITCH_TIMEOUT
Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused
simulations at startup
@@ -323,6 +309,38 @@ The workaround for this is to specify another node name remap rule in the ``Node
auto cm = std::make_shared(
executor, "_target_node_name", "some_optional_namespace", options);
+Launching controller_manager with ros2_control_node
+---------------------------------------------------
+
+The controller_manager can be launched with the ros2_control_node executable. The following example shows how to launch the controller_manager with the ros2_control_node executable:
+
+.. code-block:: python
+
+ control_node = Node(
+ package="controller_manager",
+ executable="ros2_control_node",
+ parameters=[robot_controllers],
+ output="both",
+ )
+
+The ros2_control_node executable uses the following parameters from the ``controller_manager`` node:
+
+lock_memory (optional; bool; default: false)
+ Locks the memory of the ``controller_manager`` node at startup to physical RAM in order to avoid page faults
+ and to prevent the node from being swapped out to disk.
+ Find more information about the setup for memory locking in the following link : `How to set ulimit values `_
+ The following command can be used to set the memory locking limit temporarily : ``ulimit -l unlimited``.
+
+cpu_affinity (optional; int; default: -1)
+ Sets the CPU affinity of the ``controller_manager`` node to the specified CPU core.
+ The value of -1 means that the CPU affinity is not set.
+
+thread_priority (optional; int; default: 50)
+ Sets the thread priority of the ``controller_manager`` node to the specified value. The value must be between 0 and 99.
+
+use_sim_time (optional; bool; default: false)
+ Enables the use of simulation time in the ``controller_manager`` node.
+
Concepts
-----------
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
index 13708c1af4..98ea5a843d 100644
--- a/controller_manager/include/controller_manager/controller_manager.hpp
+++ b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -50,6 +50,8 @@
namespace controller_manager
{
+class ParamListener;
+class Params;
using ControllersListIterator = std::vector::const_iterator;
CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options();
@@ -346,7 +348,7 @@ class ControllerManager : public rclcpp::Node
// Per controller update rate support
unsigned int update_loop_counter_ = 0;
- unsigned int update_rate_ = 100;
+ unsigned int update_rate_;
std::vector> chained_controllers_configuration_;
std::unique_ptr resource_manager_;
@@ -357,6 +359,8 @@ class ControllerManager : public rclcpp::Node
const std::string & command_interface);
void init_controller_manager();
+ void initialize_parameters();
+
/**
* Clear request lists used when switching controllers. The lists are shared between "callback"
* and "control loop" threads.
@@ -473,6 +477,8 @@ class ControllerManager : public rclcpp::Node
*/
rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;
+ std::shared_ptr cm_param_listener_;
+ std::shared_ptr params_;
diagnostic_updater::Updater diagnostics_updater_;
std::shared_ptr executor_;
@@ -603,6 +609,8 @@ class ControllerManager : public rclcpp::Node
rclcpp::Subscription::SharedPtr robot_description_subscription_;
rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
+ controller_manager::MovingAverageStatistics periodicity_stats_;
+
struct SwitchParams
{
void reset()
diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp
index 0f44867814..9cc508772d 100644
--- a/controller_manager/include/controller_manager/controller_spec.hpp
+++ b/controller_manager/include/controller_manager/controller_spec.hpp
@@ -24,9 +24,13 @@
#include
#include "controller_interface/controller_interface_base.hpp"
#include "hardware_interface/controller_info.hpp"
+#include "libstatistics_collector/moving_average_statistics/moving_average.hpp"
namespace controller_manager
{
+
+using MovingAverageStatistics =
+ libstatistics_collector::moving_average_statistics::MovingAverageStatistics;
/// Controller Specification
/**
* This struct contains both a pointer to a given controller, \ref c, as well
@@ -35,9 +39,18 @@ namespace controller_manager
*/
struct ControllerSpec
{
+ ControllerSpec()
+ {
+ last_update_cycle_time = std::make_shared(0, 0, RCL_CLOCK_UNINITIALIZED);
+ execution_time_statistics = std::make_shared();
+ periodicity_statistics = std::make_shared();
+ }
+
hardware_interface::ControllerInfo info;
controller_interface::ControllerInterfaceBaseSharedPtr c;
std::shared_ptr last_update_cycle_time;
+ std::shared_ptr execution_time_statistics;
+ std::shared_ptr periodicity_statistics;
};
struct ControllerChainSpec
diff --git a/controller_manager/package.xml b/controller_manager/package.xml
index 18189d5d16..42211e4e40 100644
--- a/controller_manager/package.xml
+++ b/controller_manager/package.xml
@@ -28,6 +28,8 @@
ros2param
ros2run
std_msgs
+ libstatistics_collector
+ generate_parameter_library
ament_cmake_gmock
ament_cmake_pytest
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 63435582ce..08f15f536c 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -28,6 +28,8 @@
#include "rclcpp/version.h"
#include "rclcpp_lifecycle/state.hpp"
+#include "controller_manager_parameters.hpp"
+
namespace // utility
{
static constexpr const char * kControllerInterfaceNamespace = "controller_interface";
@@ -237,6 +239,7 @@ ControllerManager::ControllerManager(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
cm_node_options_(options)
{
+ initialize_parameters();
init_controller_manager();
}
@@ -245,10 +248,6 @@ ControllerManager::ControllerManager(
bool activate_all_hw_components, const std::string & manager_node_name,
const std::string & node_namespace, const rclcpp::NodeOptions & options)
: rclcpp::Node(manager_node_name, node_namespace, options),
- update_rate_(get_parameter_or("update_rate", 100)),
- resource_manager_(std::make_unique(
- urdf, this->get_node_clock_interface(), this->get_node_logging_interface(),
- activate_all_hw_components, update_rate_)),
diagnostics_updater_(this),
executor_(executor),
loader_(std::make_shared>(
@@ -258,6 +257,10 @@ ControllerManager::ControllerManager(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
cm_node_options_(options)
{
+ initialize_parameters();
+ resource_manager_ = std::make_unique(
+ urdf, this->get_node_clock_interface(), this->get_node_logging_interface(),
+ activate_all_hw_components, params_->update_rate);
init_controller_manager();
}
@@ -276,18 +279,13 @@ ControllerManager::ControllerManager(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
cm_node_options_(options)
{
+ initialize_parameters();
init_controller_manager();
}
void ControllerManager::init_controller_manager()
{
// Get parameters needed for RT "update" loop to work
- if (!get_parameter("update_rate", update_rate_))
- {
- RCLCPP_WARN(
- get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
- }
-
if (is_resource_manager_initialized())
{
init_services();
@@ -313,6 +311,7 @@ void ControllerManager::init_controller_manager()
robot_description_subscription_->get_topic_name());
// Setup diagnostics
+ periodicity_stats_.Reset();
diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
@@ -324,6 +323,25 @@ void ControllerManager::init_controller_manager()
&ControllerManager::controller_manager_diagnostic_callback);
}
+void ControllerManager::initialize_parameters()
+{
+ // Initialize parameters
+ try
+ {
+ cm_param_listener_ = std::make_shared(
+ this->get_node_parameters_interface(), this->get_logger());
+ params_ = std::make_shared(cm_param_listener_->get_params());
+ update_rate_ = static_cast(params_->update_rate);
+ }
+ catch (const std::exception & e)
+ {
+ RCLCPP_ERROR(
+ this->get_logger(),
+ "Exception thrown while initializing controller manager parameters: %s \n", e.what());
+ throw e;
+ }
+}
+
void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
{
RCLCPP_INFO(get_logger(), "Received robot description from topic.");
@@ -365,51 +383,52 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
using lifecycle_msgs::msg::State;
auto set_components_to_state =
- [&](const std::string & parameter_name, rclcpp_lifecycle::State state)
+ [&](const std::vector & components_to_set, rclcpp_lifecycle::State state)
{
- std::vector components_to_set = std::vector({});
- if (get_parameter(parameter_name, components_to_set))
+ for (const auto & component : components_to_set)
{
- for (const auto & component : components_to_set)
+ if (component.empty())
{
- if (component.empty())
- {
- continue;
- }
- if (components_to_activate.find(component) == components_to_activate.end())
- {
- RCLCPP_WARN(
- get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.",
- component.c_str(), state.label().c_str());
- }
- else
+ continue;
+ }
+ if (components_to_activate.find(component) == components_to_activate.end())
+ {
+ RCLCPP_WARN(
+ get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.",
+ component.c_str(), state.label().c_str());
+ }
+ else
+ {
+ RCLCPP_INFO(
+ get_logger(), "Setting component '%s' to '%s' state.", component.c_str(),
+ state.label().c_str());
+ if (
+ resource_manager_->set_component_state(component, state) ==
+ hardware_interface::return_type::ERROR)
{
- RCLCPP_INFO(
- get_logger(), "Setting component '%s' to '%s' state.", component.c_str(),
- state.label().c_str());
- if (
- resource_manager_->set_component_state(component, state) ==
- hardware_interface::return_type::ERROR)
- {
- throw std::runtime_error(
- "Failed to set the initial state of the component : " + component + " to " +
- state.label());
- }
- components_to_activate.erase(component);
+ throw std::runtime_error(
+ "Failed to set the initial state of the component : " + component + " to " +
+ state.label());
}
+ components_to_activate.erase(component);
}
}
};
+ if (cm_param_listener_->is_old(*params_))
+ {
+ *params_ = cm_param_listener_->get_params();
+ }
+
// unconfigured (loaded only)
set_components_to_state(
- "hardware_components_initial_state.unconfigured",
+ params_->hardware_components_initial_state.unconfigured,
rclcpp_lifecycle::State(
State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED));
// inactive (configured)
set_components_to_state(
- "hardware_components_initial_state.inactive",
+ params_->hardware_components_initial_state.inactive,
rclcpp_lifecycle::State(
State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE));
@@ -545,22 +564,36 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
controller_spec.info.type = controller_type;
controller_spec.last_update_cycle_time = std::make_shared(
0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
+ controller_spec.execution_time_statistics = std::make_shared();
+ controller_spec.periodicity_statistics = std::make_shared();
// We have to fetch the parameters_file at the time of loading the controller, because this way we
// can load them at the creation of the LifeCycleNode and this helps in using the features such as
// read_only params, dynamic maps lists etc
// Now check if the parameters_file parameter exist
const std::string param_name = controller_name + ".params_file";
- std::string parameters_file;
+ controller_spec.info.parameters_files.clear();
- // Check if parameter has been declared
- if (!has_parameter(param_name))
- {
- declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING);
- }
- if (get_parameter(param_name, parameters_file) && !parameters_file.empty())
+ // get_parameter checks if parameter has been declared/set
+ rclcpp::Parameter params_files_parameter;
+ if (get_parameter(param_name, params_files_parameter))
{
- controller_spec.info.parameters_file = parameters_file;
+ if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY)
+ {
+ controller_spec.info.parameters_files = params_files_parameter.as_string_array();
+ }
+ else if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
+ {
+ controller_spec.info.parameters_files.push_back(params_files_parameter.as_string());
+ }
+ else
+ {
+ RCLCPP_ERROR(
+ get_logger(),
+ "The 'params_file' param needs to be a string or a string array for '%s', but it is of "
+ "type %s",
+ controller_name.c_str(), params_files_parameter.get_type_name().c_str());
+ }
}
const std::string fallback_ctrl_param = controller_name + ".fallback_controllers";
@@ -1762,6 +1795,8 @@ void ControllerManager::activate_controllers(
try
{
+ found_it->periodicity_statistics->Reset();
+ found_it->execution_time_statistics->Reset();
const auto new_state = controller->get_node()->activate();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
@@ -2243,6 +2278,7 @@ std::vector ControllerManager::get_controller_names()
void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
+ periodicity_stats_.AddMeasurement(1.0 / period.seconds());
auto [ok, failed_hardware_names] = resource_manager_->read(time, period);
if (!ok)
@@ -2345,12 +2381,14 @@ controller_interface::return_type ControllerManager::update(
: rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
const rclcpp::Time current_time = get_clock()->now();
+ bool first_update_cycle = false;
if (
*loaded_controller.last_update_cycle_time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
{
+ first_update_cycle = true;
// it is zero after activation
- *loaded_controller.last_update_cycle_time = current_time - controller_period;
+ *loaded_controller.last_update_cycle_time = current_time;
RCLCPP_DEBUG(
get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s",
loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str());
@@ -2368,7 +2406,7 @@ controller_interface::return_type ControllerManager::update(
run_controller_at_cm_rate ||
(time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
- (controller_actual_period.seconds() * controller_update_rate >= 0.99);
+ (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle;
RCLCPP_DEBUG(
get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
@@ -2382,8 +2420,20 @@ controller_interface::return_type ControllerManager::update(
// Catch exceptions thrown by the controller update function
try
{
- std::tie(trigger_status, controller_ret) =
- loaded_controller.c->trigger_update(time, controller_actual_period);
+ const auto trigger_result =
+ loaded_controller.c->trigger_update(current_time, controller_actual_period);
+ trigger_status = trigger_result.successful;
+ controller_ret = trigger_result.result;
+ if (trigger_status && trigger_result.execution_time.has_value())
+ {
+ loaded_controller.execution_time_statistics->AddMeasurement(
+ static_cast(trigger_result.execution_time.value().count()) / 1.e3);
+ }
+ if (!first_update_cycle && trigger_status && trigger_result.period.has_value())
+ {
+ loaded_controller.periodicity_statistics->AddMeasurement(
+ 1.0 / trigger_result.period.value().seconds());
+ }
}
catch (const std::exception & e)
{
@@ -3011,34 +3061,140 @@ void ControllerManager::controller_activity_diagnostic_callback(
std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_);
const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard);
bool all_active = true;
+ const std::string periodicity_suffix = ".periodicity";
+ const std::string exec_time_suffix = ".execution_time";
+ const std::string state_suffix = ".state";
+
+ if (cm_param_listener_->is_old(*params_))
+ {
+ *params_ = cm_param_listener_->get_params();
+ }
+
+ auto make_stats_string =
+ [](const auto & statistics_data, const std::string & measurement_unit) -> std::string
+ {
+ std::ostringstream oss;
+ oss << std::fixed << std::setprecision(2);
+ oss << "Avg: " << statistics_data.average << " [" << statistics_data.min << " - "
+ << statistics_data.max << "] " << measurement_unit
+ << ", StdDev: " << statistics_data.standard_deviation;
+ return oss.str();
+ };
+
+ // Variable to define the overall status of the controller diagnostics
+ auto level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+
+ std::vector high_exec_time_controllers;
+ std::vector bad_periodicity_async_controllers;
for (size_t i = 0; i < controllers.size(); ++i)
{
+ const bool is_async = controllers[i].c->is_async();
if (!is_controller_active(controllers[i].c))
{
all_active = false;
}
- stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label());
+ stat.add(
+ controllers[i].info.name + state_suffix, controllers[i].c->get_lifecycle_state().label());
+ if (is_controller_active(controllers[i].c))
+ {
+ const auto periodicity_stats = controllers[i].periodicity_statistics->GetStatistics();
+ const auto exec_time_stats = controllers[i].execution_time_statistics->GetStatistics();
+ stat.add(
+ controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us"));
+ if (is_async)
+ {
+ stat.add(
+ controllers[i].info.name + periodicity_suffix,
+ make_stats_string(periodicity_stats, "Hz") +
+ " -> Desired : " + std::to_string(controllers[i].c->get_update_rate()) + " Hz");
+ const double periodicity_error = std::abs(
+ periodicity_stats.average - static_cast(controllers[i].c->get_update_rate()));
+ if (
+ periodicity_error >
+ params_->diagnostics.threshold.controllers.periodicity.mean_error.error ||
+ periodicity_stats.standard_deviation >
+ params_->diagnostics.threshold.controllers.periodicity.standard_deviation.error)
+ {
+ level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+ add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name);
+ }
+ else if (
+ periodicity_error >
+ params_->diagnostics.threshold.controllers.periodicity.mean_error.warn ||
+ periodicity_stats.standard_deviation >
+ params_->diagnostics.threshold.controllers.periodicity.standard_deviation.warn)
+ {
+ if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR)
+ {
+ level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+ }
+ add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name);
+ }
+ }
+ const double max_exp_exec_time = is_async ? 1.e6 / controllers[i].c->get_update_rate() : 0.0;
+ if (
+ (exec_time_stats.average - max_exp_exec_time) >
+ params_->diagnostics.threshold.controllers.execution_time.mean_error.error ||
+ exec_time_stats.standard_deviation >
+ params_->diagnostics.threshold.controllers.execution_time.standard_deviation.error)
+ {
+ level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+ high_exec_time_controllers.push_back(controllers[i].info.name);
+ }
+ else if (
+ (exec_time_stats.average - max_exp_exec_time) >
+ params_->diagnostics.threshold.controllers.execution_time.mean_error.warn ||
+ exec_time_stats.standard_deviation >
+ params_->diagnostics.threshold.controllers.execution_time.standard_deviation.warn)
+ {
+ if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR)
+ {
+ level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+ }
+ high_exec_time_controllers.push_back(controllers[i].info.name);
+ }
+ }
+ }
+
+ stat.summary(
+ diagnostic_msgs::msg::DiagnosticStatus::OK,
+ all_active ? "All controllers are active" : "Not all controllers are active");
+
+ if (!high_exec_time_controllers.empty())
+ {
+ std::string high_exec_time_controllers_string;
+ for (const auto & controller : high_exec_time_controllers)
+ {
+ high_exec_time_controllers_string.append(controller);
+ high_exec_time_controllers_string.append(" ");
+ }
+ stat.mergeSummary(
+ level,
+ "\nControllers with high execution time : [ " + high_exec_time_controllers_string + "]");
+ }
+ if (!bad_periodicity_async_controllers.empty())
+ {
+ std::string bad_periodicity_async_controllers_string;
+ for (const auto & controller : bad_periodicity_async_controllers)
+ {
+ bad_periodicity_async_controllers_string.append(controller);
+ bad_periodicity_async_controllers_string.append(" ");
+ }
+ stat.mergeSummary(
+ level,
+ "\nControllers with bad periodicity : [ " + bad_periodicity_async_controllers_string + "]");
}
if (!atleast_one_hw_active)
{
- stat.summary(
+ stat.mergeSummary(
diagnostic_msgs::msg::DiagnosticStatus::ERROR,
"No hardware components are currently active to activate controllers");
}
- else
+ else if (controllers.empty())
{
- if (controllers.empty())
- {
- stat.summary(
- diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded");
- }
- else
- {
- stat.summary(
- diagnostic_msgs::msg::DiagnosticStatus::OK,
- all_active ? "All controllers are active" : "Not all controllers are active");
- }
+ stat.mergeSummary(
+ diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded");
}
}
@@ -3091,7 +3247,14 @@ void ControllerManager::hardware_components_diagnostic_callback(
void ControllerManager::controller_manager_diagnostic_callback(
diagnostic_updater::DiagnosticStatusWrapper & stat)
{
+ const std::string periodicity_stat_name = "periodicity";
+ const auto cm_stats = periodicity_stats_.GetStatistics();
stat.add("update_rate", std::to_string(get_update_rate()));
+ stat.add(periodicity_stat_name + ".average", std::to_string(cm_stats.average));
+ stat.add(
+ periodicity_stat_name + ".standard_deviation", std::to_string(cm_stats.standard_deviation));
+ stat.add(periodicity_stat_name + ".min", std::to_string(cm_stats.min));
+ stat.add(periodicity_stat_name + ".max", std::to_string(cm_stats.max));
if (is_resource_manager_initialized())
{
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running");
@@ -3110,6 +3273,27 @@ void ControllerManager::controller_manager_diagnostic_callback(
"Resource Manager is not initialized properly!");
}
}
+
+ const double periodicity_error = std::abs(cm_stats.average - get_update_rate());
+ const std::string diag_summary =
+ "Controller Manager has bad periodicity : " + std::to_string(cm_stats.average) +
+ " Hz. Expected consistent " + std::to_string(get_update_rate()) + " Hz";
+ if (
+ periodicity_error >
+ params_->diagnostics.threshold.controller_manager.periodicity.mean_error.error ||
+ cm_stats.standard_deviation >
+ params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.error)
+ {
+ stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_summary);
+ }
+ else if (
+ periodicity_error >
+ params_->diagnostics.threshold.controller_manager.periodicity.mean_error.warn ||
+ cm_stats.standard_deviation >
+ params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn)
+ {
+ stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_summary);
+ }
}
void ControllerManager::update_list_with_controller_chain(
@@ -3211,14 +3395,14 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
node_options_arguments.push_back(arg);
}
- if (controller.info.parameters_file.has_value())
+ for (const auto & parameters_file : controller.info.parameters_files)
{
if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG))
{
node_options_arguments.push_back(RCL_ROS_ARGS_FLAG);
}
node_options_arguments.push_back(RCL_PARAM_FILE_FLAG);
- node_options_arguments.push_back(controller.info.parameters_file.value());
+ node_options_arguments.push_back(parameters_file);
}
// ensure controller's `use_sim_time` parameter matches controller_manager's
diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml
new file mode 100644
index 0000000000..1bb9b152b1
--- /dev/null
+++ b/controller_manager/src/controller_manager_parameters.yaml
@@ -0,0 +1,136 @@
+controller_manager:
+ update_rate: {
+ type: int,
+ default_value: 100,
+ read_only: true,
+ description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware."
+ }
+
+ hardware_components_initial_state:
+ unconfigured: {
+ type: string_array,
+ default_value: [],
+ description: "Defines which hardware components will be only loaded when controller manager is started. These hardware components will need to be configured and activated manually or via a hardware spawner.",
+ validation: {
+ unique<>: null,
+ }
+ }
+
+ inactive: {
+ type: string_array,
+ default_value: [],
+ description: "Defines which hardware components will be configured when controller manager is started. These hardware components will need to be activated manually or via a hardware spawner.",
+ validation: {
+ unique<>: null,
+ }
+ }
+
+ diagnostics:
+ threshold:
+ controller_manager:
+ periodicity:
+ mean_error:
+ warn: {
+ type: double,
+ default_value: 5.0,
+ description: "The warning threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, a warning diagnostic will be published.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ error: {
+ type: double,
+ default_value: 10.0,
+ description: "The error threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, an error diagnostic will be published.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ standard_deviation:
+ warn: {
+ type: double,
+ default_value: 5.0,
+ description: "The warning threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, a warning diagnostic will be published.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ error: {
+ type: double,
+ default_value: 10.0,
+ description: "The error threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, an error diagnostic will be published.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ controllers:
+ periodicity:
+ mean_error:
+ warn: {
+ type: double,
+ default_value: 5.0,
+ description: "The warning threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ error: {
+ type: double,
+ default_value: 10.0,
+ description: "The error threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ standard_deviation:
+ warn: {
+ type: double,
+ default_value: 5.0,
+ description: "The warning threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ error: {
+ type: double,
+ default_value: 10.0,
+ description: "The error threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ execution_time:
+ mean_error:
+ warn: {
+ type: double,
+ default_value: 1000.0,
+ description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ error: {
+ type: double,
+ default_value: 2000.0,
+ description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ standard_deviation:
+ warn: {
+ type: double,
+ default_value: 100.0,
+ description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
+ error: {
+ type: double,
+ default_value: 200.0,
+ description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.",
+ validation: {
+ gt<>: 0.0,
+ }
+ }
diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index f8347df952..da79de76a3 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -59,7 +59,7 @@ int main(int argc, char ** argv)
const bool use_sim_time = cm->get_parameter_or("use_sim_time", false);
- const bool lock_memory = cm->get_parameter_or("lock_memory", true);
+ const bool lock_memory = cm->get_parameter_or("lock_memory", false);
std::string message;
if (lock_memory && !realtime_tools::lock_memory(message))
{
@@ -109,7 +109,7 @@ int main(int argc, char ** argv)
next_iteration_time{cm_now};
// for calculating the measured period of the loop
- rclcpp::Time previous_time = cm->now();
+ rclcpp::Time previous_time = cm->now() - period;
while (rclcpp::ok())
{
diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
index 0c4e51985f..b715bc23dc 100644
--- a/controller_manager/test/test_controller_manager.cpp
+++ b/controller_manager/test/test_controller_manager.cpp
@@ -366,6 +366,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
+ time_ = cm_->get_clock()->now();
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -373,6 +374,21 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
std::this_thread::sleep_for(
std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
EXPECT_EQ(test_controller->internal_counter, 1u);
+ EXPECT_EQ(test_controller->update_period_.seconds(), 0.0)
+ << "The first trigger cycle should have zero period";
+
+ const double exp_period = (cm_->get_clock()->now() - time_).seconds();
+ time_ = cm_->get_clock()->now();
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(test_controller->internal_counter, 1u);
+ std::this_thread::sleep_for(
+ std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
+ EXPECT_EQ(test_controller->internal_counter, 2u);
+ EXPECT_THAT(
+ test_controller->update_period_.seconds(),
+ testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period))));
size_t last_internal_counter = test_controller->internal_counter;
// Stop controller, will take effect at the end of the update function
@@ -576,7 +592,26 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
// [cm_update_rate, 2*cm_update_rate)
EXPECT_THAT(
test_controller->update_period_.seconds(),
- testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate))));
+ testing::AllOf(testing::Ge(0.85 / cm_update_rate), testing::Lt((1.15 / cm_update_rate))));
+ ASSERT_EQ(
+ test_controller->internal_counter,
+ cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount());
+ ASSERT_EQ(
+ test_controller->internal_counter - 1,
+ cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount())
+ << "The first update is not counted in periodicity statistics";
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Average(),
+ testing::AllOf(
+ testing::Ge(0.95 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate()))));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
+ testing::AllOf(
+ testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate()))));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
+ testing::AllOf(
+ testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate()))));
loop_rate.sleep();
}
// if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -683,6 +718,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
const auto initial_counter = test_controller->internal_counter;
// don't start with zero to check if the period is correct if controller is activated anytime
rclcpp::Time time = time_;
+ const auto exp_periodicity =
+ cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate);
for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
{
rclcpp::Time old_time = time;
@@ -691,19 +728,36 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
- // In case of a non perfect divisor, the update period should respect the rule
- // [controller_update_rate, 2*controller_update_rate)
- EXPECT_THAT(
- test_controller->update_period_.seconds(),
- testing::AllOf(
- testing::Gt(0.99 * controller_period),
- testing::Lt((1.05 * controller_period) + PERIOD.seconds())))
- << "update_counter: " << update_counter << " desired controller period: " << controller_period
- << " actual controller period: " << test_controller->update_period_.seconds();
- if (update_counter % cm_update_rate == 0)
+ if (test_controller->internal_counter - initial_counter > 0)
+ {
+ // In case of a non perfect divisor, the update period should respect the rule
+ // [controller_update_rate, 2*controller_update_rate)
+ EXPECT_THAT(
+ test_controller->update_period_.seconds(),
+ testing::AllOf(
+ testing::Gt(0.99 * controller_period),
+ testing::Lt((1.05 * controller_period) + PERIOD.seconds())))
+ << "update_counter: " << update_counter
+ << " desired controller period: " << controller_period
+ << " actual controller period: " << test_controller->update_period_.seconds();
+ }
+ else
+ {
+ // Check that the first cycle update period is zero
+ EXPECT_EQ(test_controller->update_period_.seconds(), 0.0);
+ }
+
+ if (update_counter > 0 && update_counter % cm_update_rate == 0)
{
const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate;
+ const auto actual_counter = test_controller->internal_counter - initial_counter;
+ const unsigned int exp_counter =
+ static_cast(exp_periodicity * no_of_secs_passed);
+ SCOPED_TRACE(
+ "The internal counter is : " + std::to_string(actual_counter) + " [" +
+ std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) +
+ "] and number of seconds passed : " + std::to_string(no_of_secs_passed));
// NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole
// cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it
// is clearly tracking, so adding 1 here won't affect the final count.
@@ -711,10 +765,26 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
// cycle and then on accumulating 37 on every other update cycle so at the end of the 10
// cycles it will have 369 instead of 370.
EXPECT_THAT(
- test_controller->internal_counter - initial_counter,
- testing::AnyOf(
- testing::Ge((controller_update_rate - 1) * no_of_secs_passed),
- testing::Lt((controller_update_rate * no_of_secs_passed))));
+ actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1)));
+ ASSERT_EQ(
+ test_controller->internal_counter,
+ cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount());
+ ASSERT_EQ(
+ test_controller->internal_counter - 1,
+ cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount())
+ << "The first update is not counted in periodicity statistics";
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Average(),
+ testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity))));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
+ testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
+ testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
+ EXPECT_LT(
+ cm_->get_loaded_controllers()[0].execution_time_statistics->Average(),
+ 50.0); // 50 microseconds
}
}
}
@@ -723,6 +793,148 @@ INSTANTIATE_TEST_SUITE_P(
per_controller_update_rate_check, TestControllerUpdateRates,
testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90));
+class TestAsyncControllerUpdateRates
+: public ControllerManagerFixture
+{
+};
+
+TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_and_stats)
+{
+ const unsigned int ctrl_update_rate = cm_->get_update_rate() / 2;
+ auto test_controller = std::make_shared();
+ cm_->add_controller(
+ test_controller, test_controller::TEST_CONTROLLER_NAME,
+ test_controller::TEST_CONTROLLER_CLASS_NAME);
+ EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+ EXPECT_EQ(2, test_controller.use_count());
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+ test_controller->get_lifecycle_state().id());
+
+ test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)});
+ test_controller->get_node()->set_parameter({"is_async", true});
+ // configure controller
+ {
+ ControllerManagerRunner cm_runner(this);
+ cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+ }
+ ASSERT_TRUE(test_controller->is_async());
+ time_ = test_controller->get_node()->now(); // set to something nonzero
+ cm_->get_clock()->sleep_until(time_ + PERIOD);
+ time_ = cm_->get_clock()->now();
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+ test_controller->get_lifecycle_state().id());
+
+ // Start controller, will take effect at the end of the update function
+ const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+ std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME};
+ std::vector stop_controllers = {};
+ auto switch_future = std::async(
+ std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+ start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));
+
+ ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+ << "switch_controller should be blocking until next update cycle";
+
+ cm_->get_clock()->sleep_until(time_ + PERIOD);
+ time_ = cm_->get_clock()->now();
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
+ {
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+ }
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
+
+ EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
+ const auto cm_update_rate = cm_->get_update_rate();
+ const auto controller_update_rate = test_controller->get_update_rate();
+ const double controller_period = 1.0 / controller_update_rate;
+
+ const auto initial_counter = test_controller->internal_counter;
+ // don't start with zero to check if the period is correct if controller is activated anytime
+ rclcpp::Time time = time_;
+ const auto exp_periodicity =
+ cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate);
+ for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
+ {
+ rclcpp::Time old_time = time;
+ cm_->get_clock()->sleep_until(old_time + PERIOD);
+ time = cm_->get_clock()->now();
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
+
+ // the async controllers will have to wait for one cycle to have the correct update period in
+ // the controller
+ if (test_controller->internal_counter - initial_counter > 1)
+ {
+ EXPECT_THAT(
+ test_controller->update_period_.seconds(),
+ testing::AllOf(
+ testing::Gt(0.99 * controller_period),
+ testing::Lt((1.05 * controller_period) + PERIOD.seconds())))
+ << "update_counter: " << update_counter
+ << " desired controller period: " << controller_period
+ << " actual controller period: " << test_controller->update_period_.seconds();
+ }
+ // else
+ // {
+ // // Check that the first cycle update period is zero
+ // EXPECT_EQ(test_controller->update_period_.seconds(), 0.0);
+ // }
+
+ if (update_counter > 0 && update_counter % cm_update_rate == 0)
+ {
+ const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate;
+ const auto actual_counter = test_controller->internal_counter - initial_counter;
+ const unsigned int exp_counter =
+ static_cast(exp_periodicity * no_of_secs_passed);
+ SCOPED_TRACE(
+ "The internal counter is : " + std::to_string(actual_counter) + " [" +
+ std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) +
+ "] and number of seconds passed : " + std::to_string(no_of_secs_passed));
+ // NOTE: here EXPECT_THAT is used because it is observed that in the first iteration of whole
+ // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it
+ // is clearly tracking, so adding 1 here won't affect the final count.
+ // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update
+ // cycle and then on accumulating 37 on every other update cycle so at the end of the 10
+ // cycles it will have 369 instead of 370.
+ EXPECT_THAT(
+ actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1)));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount(),
+ testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter)));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount(),
+ testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter)));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Average(),
+ testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity))));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
+ testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
+ EXPECT_THAT(
+ cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
+ testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
+ EXPECT_LT(
+ cm_->get_loaded_controllers()[0].execution_time_statistics->Average(),
+ 12000); // more or less 12 milliseconds considering the waittime in the controller
+ }
+ }
+}
+
class TestControllerManagerFallbackControllers
: public ControllerManagerFixture,
public testing::WithParamInterface
diff --git a/controller_manager/test/test_controller_overriding_parameters.yaml b/controller_manager/test/test_controller_overriding_parameters.yaml
new file mode 100644
index 0000000000..115d0ed993
--- /dev/null
+++ b/controller_manager/test/test_controller_overriding_parameters.yaml
@@ -0,0 +1,5 @@
+ctrl_with_parameters_and_type:
+ ros__parameters:
+ interface_name: "impedance"
+ joint_offset: 0.2
+ joint_names: ["joint10"]
diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml
index 087994bd23..23fd69b216 100644
--- a/controller_manager/test/test_controller_spawner_with_type.yaml
+++ b/controller_manager/test/test_controller_spawner_with_type.yaml
@@ -2,6 +2,7 @@ ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_controller"
joint_names: ["joint0"]
+ interface_name: "position"
/**:
chainable_ctrl_with_parameters_and_type:
diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp
index c7b67e0cfe..bdd48f15ae 100644
--- a/controller_manager/test/test_hardware_management_srvs.cpp
+++ b/controller_manager/test/test_hardware_management_srvs.cpp
@@ -65,6 +65,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
cm_ = std::make_shared(executor_, TEST_CM_NAME);
run_updater_ = false;
+ SetUpSrvsCMExecutor();
cm_->set_parameter(rclcpp::Parameter(
"hardware_components_initial_state.unconfigured",
std::vector({TEST_SYSTEM_HARDWARE_NAME})));
@@ -72,11 +73,10 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
"hardware_components_initial_state.inactive",
std::vector({TEST_SENSOR_HARDWARE_NAME})));
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto msg = std_msgs::msg::String();
msg.data = ros2_control_test_assets::minimal_robot_urdf;
cm_->robot_description_callback(msg);
-
- SetUpSrvsCMExecutor();
}
void check_component_fileds(
diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp
index 74e1efeeed..70a5480a06 100644
--- a/controller_manager/test/test_spawner_unspawner.cpp
+++ b/controller_manager/test/test_spawner_unspawner.cpp
@@ -255,6 +255,50 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param)
}
}
+TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter)
+{
+ const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_type.yaml";
+
+ cm_->set_parameter(rclcpp::Parameter(
+ "ctrl_with_parameters_and_type.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+ cm_->set_parameter(
+ rclcpp::Parameter("ctrl_with_parameters_and_type.params_file", test_file_path));
+
+ 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"), 0);
+
+ 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, "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);
+ ASSERT_EQ(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
+ auto ctrl_node = ctrl_with_parameters_and_type.c->get_node();
+ ASSERT_THAT(
+ ctrl_with_parameters_and_type.info.parameters_files,
+ std::vector({test_file_path}));
+ if (!ctrl_node->has_parameter("joint_names"))
+ {
+ ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"}));
+ }
+ ASSERT_THAT(
+ ctrl_node->get_parameter("joint_names").as_string_array(),
+ std::vector({"joint0"}));
+
+ if (!ctrl_node->has_parameter("interface_name"))
+ {
+ ctrl_node->declare_parameter("interface_name", "invalid_interface");
+ }
+ ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position");
+}
+
TEST_F(TestLoadController, spawner_test_type_in_params_file)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
@@ -278,7 +322,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
ctrl_with_parameters_and_type.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);
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array()[0],
+ test_file_path);
auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
ASSERT_EQ(
@@ -290,7 +335,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
chain_ctrl_with_parameters_and_type.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(),
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0],
test_file_path);
EXPECT_EQ(
@@ -308,7 +353,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
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);
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array()[0],
+ test_file_path);
auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type");
@@ -316,7 +362,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file)
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(),
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0],
test_file_path);
}
@@ -357,6 +403,114 @@ TEST_F(TestLoadController, unload_on_kill_activate_as_group)
ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
}
+TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding)
+{
+ const std::string main_test_file_path =
+ ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_type.yaml";
+ const std::string overriding_test_file_path =
+ ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_overriding_parameters.yaml";
+
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_parameters_and_type --load-only -c "
+ "test_controller_manager -p " +
+ main_test_file_path + " -p " + overriding_test_file_path),
+ 0);
+
+ 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, "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);
+ ASSERT_THAT(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(),
+ std::vector({main_test_file_path, overriding_test_file_path}));
+ auto ctrl_node = ctrl_with_parameters_and_type.c->get_node();
+ ASSERT_THAT(
+ ctrl_with_parameters_and_type.info.parameters_files,
+ std::vector({main_test_file_path, overriding_test_file_path}));
+ if (!ctrl_node->has_parameter("joint_names"))
+ {
+ ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"}));
+ }
+ ASSERT_THAT(
+ ctrl_node->get_parameter("joint_names").as_string_array(),
+ std::vector({"joint10"}));
+
+ if (!ctrl_node->has_parameter("interface_name"))
+ {
+ ctrl_node->declare_parameter("interface_name", "invalid_interface");
+ }
+ ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "impedance")
+ << "The parameter should be overridden";
+
+ if (!ctrl_node->has_parameter("joint_offset"))
+ {
+ ctrl_node->declare_parameter("joint_offset", -M_PI);
+ }
+ ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2);
+}
+
+TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding_reverse)
+{
+ const std::string main_test_file_path =
+ ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_overriding_parameters.yaml";
+ const std::string overriding_test_file_path =
+ ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_type.yaml";
+
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_parameters_and_type --load-only -c "
+ "test_controller_manager -p " +
+ main_test_file_path + " -p " + overriding_test_file_path),
+ 0);
+
+ 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, "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);
+ ASSERT_THAT(
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(),
+ std::vector({main_test_file_path, overriding_test_file_path}));
+ auto ctrl_node = ctrl_with_parameters_and_type.c->get_node();
+ ASSERT_THAT(
+ ctrl_with_parameters_and_type.info.parameters_files,
+ std::vector({main_test_file_path, overriding_test_file_path}));
+ if (!ctrl_node->has_parameter("joint_names"))
+ {
+ ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"}));
+ }
+ ASSERT_THAT(
+ ctrl_node->get_parameter("joint_names").as_string_array(),
+ std::vector({"joint0"}));
+
+ if (!ctrl_node->has_parameter("interface_name"))
+ {
+ ctrl_node->declare_parameter("interface_name", "invalid_interface");
+ }
+ ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position")
+ << "The parameter should be overridden";
+
+ if (!ctrl_node->has_parameter("joint_offset"))
+ {
+ ctrl_node->declare_parameter("joint_offset", -M_PI);
+ }
+ ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2);
+}
+
TEST_F(TestLoadController, spawner_test_fallback_controllers)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
@@ -377,7 +531,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers)
ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty());
ASSERT_EQ(
ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
- ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string(), test_file_path);
+ ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path);
}
// Try to spawn now the controller with fallback controllers inside the yaml
@@ -392,7 +546,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers)
ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty());
ASSERT_EQ(
ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
- ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string(), test_file_path);
+ ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path);
auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
@@ -401,7 +555,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers)
ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
ASSERT_EQ(
ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
- ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string(), test_file_path);
+ ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string_array()[0], test_file_path);
auto ctrl_3 = cm_->get_loaded_controllers()[2];
ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
@@ -409,7 +563,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers)
ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
ASSERT_EQ(
ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
- ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string(), test_file_path);
+ ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string_array()[0], test_file_path);
}
}
@@ -695,7 +849,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
- cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(),
+ cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file")
+ .as_string_array()[0],
test_file_path);
auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
@@ -708,7 +863,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
- cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(),
+ cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file")
+ .as_string_array()[0],
test_file_path);
EXPECT_EQ(
@@ -725,14 +881,16 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
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_1.info.name + ".params_file").as_string(), test_file_path);
+ ASSERT_EQ(
+ cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], test_file_path);
auto ctrl_2 = cm_->get_loaded_controllers()[1];
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(ctrl_2.info.name + ".params_file").as_string(), test_file_path);
+ ASSERT_EQ(
+ cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path);
}
TEST_F(
@@ -781,7 +939,8 @@ TEST_F(
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
- cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(),
+ cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file")
+ .as_string_array()[0],
test_file_path);
auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
@@ -794,7 +953,8 @@ TEST_F(
chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
- cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(),
+ cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file")
+ .as_string_array()[0],
test_file_path);
EXPECT_EQ(
@@ -812,14 +972,16 @@ TEST_F(
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_1.info.name + ".params_file").as_string(), test_file_path);
+ ASSERT_EQ(
+ cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], test_file_path);
auto ctrl_2 = cm_->get_loaded_controllers()[1];
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(ctrl_2.info.name + ".params_file").as_string(), test_file_path);
+ ASSERT_EQ(
+ cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path);
}
TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file)
@@ -928,3 +1090,138 @@ TEST_F(
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}
+
+TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file)
+{
+ const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_type.yaml";
+ const std::string fallback_test_file_path =
+ ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_fallback_controllers.yaml";
+
+ cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+ cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+ ControllerManagerRunner cm_runner(this);
+ // Provide controller type via the parsed file
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 "
+ "--load-only -c "
+ "test_controller_manager -p " +
+ test_file_path + " -p" + fallback_test_file_path),
+ 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul);
+
+ 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.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 params_file_info =
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array();
+ ASSERT_EQ(params_file_info.size(), 1ul);
+ ASSERT_EQ(params_file_info[0], test_file_path);
+
+ 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");
+ 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);
+ params_file_info =
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array();
+ ASSERT_EQ(params_file_info.size(), 1ul);
+ ASSERT_EQ(params_file_info[0], test_file_path);
+
+ auto ctrl_2 = cm_->get_loaded_controllers()[2];
+ ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
+ ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(
+ ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array();
+ ASSERT_EQ(params_file_info.size(), 1ul);
+ ASSERT_EQ(params_file_info[0], fallback_test_file_path);
+
+ auto ctrl_1 = cm_->get_loaded_controllers()[3];
+ ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+ 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);
+ params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array();
+ ASSERT_EQ(params_file_info.size(), 1ul);
+ ASSERT_EQ(params_file_info[0], fallback_test_file_path);
+}
+
+TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times)
+{
+ const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_type.yaml";
+ const std::string fallback_test_file_path =
+ ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_fallback_controllers.yaml";
+
+ cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+ cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+ ControllerManagerRunner cm_runner(this);
+ // Provide controller type via the parsed file
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 "
+ "--load-only -c "
+ "test_controller_manager -p " +
+ test_file_path + " -p" + fallback_test_file_path + " -p" + fallback_test_file_path + " -p" +
+ test_file_path),
+ 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul);
+
+ 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.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 params_file_info =
+ cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array();
+ ASSERT_EQ(params_file_info.size(), 1ul);
+ ASSERT_EQ(params_file_info[0], test_file_path);
+
+ 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");
+ 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);
+ params_file_info =
+ cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array();
+ ASSERT_EQ(params_file_info.size(), 1ul);
+ ASSERT_EQ(params_file_info[0], test_file_path);
+
+ auto ctrl_2 = cm_->get_loaded_controllers()[2];
+ ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
+ ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(
+ ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array();
+ ASSERT_EQ(params_file_info.size(), 1ul);
+ ASSERT_EQ(params_file_info[0], fallback_test_file_path);
+
+ auto ctrl_1 = cm_->get_loaded_controllers()[3];
+ ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+ 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);
+ params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array();
+ ASSERT_EQ(params_file_info.size(), 1ul);
+ ASSERT_EQ(params_file_info[0], fallback_test_file_path);
+}
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index 09a9236d79..b7c4b934c4 100644
--- a/doc/release_notes.rst
+++ b/doc/release_notes.rst
@@ -76,11 +76,13 @@ controller_manager
* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_).
* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_).
* Added support for the wildcard entries for the controller configuration files (`#1724 `_).
+* The spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 `_).
* ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_).
* ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_).
* The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_).
* The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_).
* The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_).
+* The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_).
hardware_interface
******************
diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp
index a38fb99cb3..3ad89551d5 100644
--- a/hardware_interface/include/hardware_interface/controller_info.hpp
+++ b/hardware_interface/include/hardware_interface/controller_info.hpp
@@ -34,7 +34,7 @@ struct ControllerInfo
std::string type;
/// Controller param file
- std::optional parameters_file;
+ std::vector parameters_files;
/// List of claimed interfaces by the controller.
std::vector claimed_interfaces;
diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
index 18da0e4012..32d0b8a48a 100644
--- a/hardware_interface/include/hardware_interface/system_interface.hpp
+++ b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -178,7 +178,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*
* \return vector of shared pointers to the created and stored StateInterfaces
*/
- std::vector on_export_state_interfaces()
+ virtual std::vector on_export_state_interfaces()
{
// import the unlisted interfaces
std::vector unlisted_interface_descriptions =
@@ -270,7 +270,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*
* \return vector of shared pointers to the created and stored CommandInterfaces
*/
- std::vector on_export_command_interfaces()
+ virtual std::vector on_export_command_interfaces()
{
// import the unlisted interfaces
std::vector unlisted_interface_descriptions =
diff --git a/ros2_control-not-released.iron.repos b/ros2_control-not-released.iron.repos
deleted file mode 100644
index 56f46b6f79..0000000000
--- a/ros2_control-not-released.iron.repos
+++ /dev/null
@@ -1 +0,0 @@
-repositories:
diff --git a/ros2_control.iron.repos b/ros2_control.iron.repos
deleted file mode 100644
index c93d8f4ef6..0000000000
--- a/ros2_control.iron.repos
+++ /dev/null
@@ -1,9 +0,0 @@
-repositories:
- ros-controls/realtime_tools:
- type: git
- url: https://github.com/ros-controls/realtime_tools.git
- version: master
- ros-controls/control_msgs:
- type: git
- url: https://github.com/ros-controls/control_msgs.git
- version: master
diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py
index e47540a1df..5e29058a31 100644
--- a/ros2controlcli/ros2controlcli/verb/load_controller.py
+++ b/ros2controlcli/ros2controlcli/verb/load_controller.py
@@ -17,7 +17,7 @@
load_controller,
list_controllers,
switch_controllers,
- set_controller_parameters_from_param_file,
+ set_controller_parameters_from_param_files,
bcolors,
)
@@ -68,11 +68,11 @@ def main(self, *, args):
if not os.path.isabs(args.param_file):
args.param_file = os.path.join(os.getcwd(), args.param_file)
- if not set_controller_parameters_from_param_file(
+ if not set_controller_parameters_from_param_files(
node,
args.controller_manager,
args.controller_name,
- args.param_file,
+ [args.param_file],
node.get_namespace(),
):
return 1