From 2ee0001d83460168ab0ed43b1d56b6d8f3f2fbc9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Sep 2024 13:12:07 +0200 Subject: [PATCH 1/7] Log exception type when catching the exception (#1749) --- controller_manager/src/controller_manager.cpp | 40 +++++++----- hardware_interface/src/resource_manager.cpp | 62 ++++++++++--------- 2 files changed, 57 insertions(+), 45 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e769ea06ce..b4f18dd90e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -470,8 +470,9 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Caught exception while loading the controller '%s' of plugin type '%s':\n%s", - controller_name.c_str(), controller_type.c_str(), e.what()); + get_logger(), + "Caught exception of type : %s while loading the controller '%s' of plugin type '%s':\n%s", + typeid(e).name(), controller_name.c_str(), controller_type.c_str(), e.what()); return nullptr; } catch (...) @@ -615,8 +616,9 @@ controller_interface::return_type ControllerManager::unload_controller( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Failed to clean-up the controller '%s' before unloading: %s", - controller_name.c_str(), e.what()); + get_logger(), + "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s", + typeid(e).name(), controller_name.c_str(), e.what()); } catch (...) { @@ -720,8 +722,8 @@ controller_interface::return_type ControllerManager::configure_controller( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Caught exception while configuring controller '%s': %s", - controller_name.c_str(), e.what()); + get_logger(), "Caught exception of type : %s while configuring controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); return controller_interface::return_type::ERROR; } catch (...) @@ -1432,8 +1434,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co { to.clear(); RCLCPP_ERROR( - get_logger(), "Caught exception while initializing controller '%s': %s", - controller.info.name.c_str(), e.what()); + get_logger(), "Caught exception of type : %s while initializing controller '%s': %s", + typeid(e).name(), controller.info.name.c_str(), e.what()); return nullptr; } catch (...) @@ -1508,8 +1510,8 @@ void ControllerManager::deactivate_controllers( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Caught exception while deactivating the controller '%s': %s", - controller_name.c_str(), e.what()); + get_logger(), "Caught exception of type : %s while deactivating the controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); continue; } catch (...) @@ -1626,7 +1628,10 @@ void ControllerManager::activate_controllers( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what()); + get_logger(), + "Caught exception of type : %s while claiming the command interfaces. Can't activate " + "controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); assignment_successful = false; break; } @@ -1661,7 +1666,10 @@ void ControllerManager::activate_controllers( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what()); + get_logger(), + "Caught exception of type : %s while claiming the state interfaces. Can't activate " + "controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); assignment_successful = false; break; } @@ -1689,8 +1697,8 @@ void ControllerManager::activate_controllers( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Caught exception while activating the controller '%s': %s", - controller_name.c_str(), e.what()); + get_logger(), "Caught exception of type : %s while activating the controller '%s': %s", + typeid(e).name(), controller_name.c_str(), e.what()); continue; } catch (...) @@ -2295,8 +2303,8 @@ controller_interface::return_type ControllerManager::update( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Caught exception while updating controller '%s': %s", - loaded_controller.info.name.c_str(), e.what()); + get_logger(), "Caught exception of type : %s while updating controller '%s': %s", + typeid(e).name(), loaded_controller.info.name.c_str(), e.what()); controller_ret = controller_interface::return_type::ERROR; } catch (...) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 72e966fcab..f368f2a00a 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -160,13 +160,15 @@ class ResourceStorage } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_ERROR(get_logger(), "Exception while loading hardware: %s", ex.what()); + RCLCPP_ERROR( + get_logger(), "Caught exception of type : %s while loading hardware: %s", typeid(ex).name(), + ex.what()); } catch (const std::exception & ex) { RCLCPP_ERROR( - get_logger(), "Exception occurred while loading hardware '%s': %s", - hardware_info.name.c_str(), ex.what()); + get_logger(), "Exception of type : %s occurred while loading hardware '%s': %s", + typeid(ex).name(), hardware_info.name.c_str(), ex.what()); } catch (...) { @@ -203,8 +205,8 @@ class ResourceStorage catch (const std::exception & ex) { RCLCPP_ERROR( - get_logger(), "Exception occurred while initializing hardware '%s': %s", - hardware_info.name.c_str(), ex.what()); + get_logger(), "Exception of type : %s occurred while initializing hardware '%s': %s", + typeid(ex).name(), hardware_info.name.c_str(), ex.what()); } catch (...) { @@ -229,8 +231,8 @@ class ResourceStorage catch (const std::exception & ex) { RCLCPP_ERROR( - get_logger(), "Exception occurred while configuring hardware '%s': %s", - hardware.get_name().c_str(), ex.what()); + get_logger(), "Exception of type : %s occurred while configuring hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); } catch (...) { @@ -378,8 +380,8 @@ class ResourceStorage catch (const std::exception & ex) { RCLCPP_ERROR( - get_logger(), "Exception occurred while cleaning up hardware '%s': %s", - hardware.get_name().c_str(), ex.what()); + get_logger(), "Exception of type : %s occurred while cleaning up hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); } catch (...) { @@ -412,8 +414,8 @@ class ResourceStorage catch (const std::exception & ex) { RCLCPP_ERROR( - get_logger(), "Exception occurred while shutting down hardware '%s': %s", - hardware.get_name().c_str(), ex.what()); + get_logger(), "Exception of type : %s occurred while shutting down hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); } catch (...) { @@ -451,8 +453,8 @@ class ResourceStorage catch (const std::exception & ex) { RCLCPP_ERROR( - get_logger(), "Exception occurred while activating hardware '%s': %s", - hardware.get_name().c_str(), ex.what()); + get_logger(), "Exception of type : %s occurred while activating hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); } catch (...) { @@ -486,8 +488,8 @@ class ResourceStorage catch (const std::exception & ex) { RCLCPP_ERROR( - get_logger(), "Exception occurred while deactivating hardware '%s': %s", - hardware.get_name().c_str(), ex.what()); + get_logger(), "Exception of type : %s occurred while deactivating hardware '%s': %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); } catch (...) { @@ -625,8 +627,9 @@ class ResourceStorage { RCLCPP_ERROR( get_logger(), - "Exception occurred while importing state interfaces for the hardware '%s' : %s", - hardware.get_name().c_str(), e.what()); + "Exception of type : %s occurred while importing state interfaces for the hardware '%s' : " + "%s", + typeid(e).name(), hardware.get_name().c_str(), e.what()); } catch (...) { @@ -650,8 +653,9 @@ class ResourceStorage { RCLCPP_ERROR( get_logger(), - "Exception occurred while importing command interfaces for the hardware '%s' : %s", - hardware.get_name().c_str(), ex.what()); + "Exception of type : %s occurred while importing command interfaces for the hardware '%s' " + ": %s", + typeid(ex).name(), hardware.get_name().c_str(), ex.what()); } catch (...) { @@ -1568,9 +1572,9 @@ bool ResourceManager::prepare_command_mode_switch( { RCLCPP_ERROR( logger, - "Exception occurred while preparing command mode switch for component '%s' for the " - "interfaces: \n %s : %s", - component.get_name().c_str(), + "Exception of type : %s occurred while preparing command mode switch for component " + "'%s' for the interfaces: \n %s : %s", + typeid(e).name(), component.get_name().c_str(), interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what()); ret = false; } @@ -1632,9 +1636,9 @@ bool ResourceManager::perform_command_mode_switch( { RCLCPP_ERROR( logger, - "Exception occurred while performing command mode switch for component '%s' for the " - "interfaces: \n %s : %s", - component.get_name().c_str(), + "Exception of type : %s occurred while performing command mode switch for component " + "'%s' for the interfaces: \n %s : %s", + typeid(e).name(), component.get_name().c_str(), interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what()); ret = false; } @@ -1791,8 +1795,8 @@ HardwareReadWriteStatus ResourceManager::read( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Exception thrown durind read of the component '%s': %s", - component.get_name().c_str(), e.what()); + get_logger(), "Exception of type : %s thrown during read of the component '%s': %s", + typeid(e).name(), component.get_name().c_str(), e.what()); ret_val = return_type::ERROR; } catch (...) @@ -1851,8 +1855,8 @@ HardwareReadWriteStatus ResourceManager::write( catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Exception thrown during write of the component '%s': %s", - component.get_name().c_str(), e.what()); + get_logger(), "Exception of type : %s thrown during write of the component '%s': %s", + typeid(e).name(), component.get_name().c_str(), e.what()); ret_val = return_type::ERROR; } catch (...) From e0109d2be6aa1a6a086ac0acde0d92be8f30d605 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 11 Sep 2024 12:14:01 +0100 Subject: [PATCH 2/7] Update changelogs --- controller_interface/CHANGELOG.rst | 5 +++++ controller_manager/CHANGELOG.rst | 9 +++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 8 ++++++++ hardware_interface_testing/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 5 +++++ ros2controlcli/CHANGELOG.rst | 5 +++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 50 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 5a0a5ff573..0543bd1033 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 `_) +* Contributors: Manuel Muth + 4.16.1 (2024-08-24) ------------------- diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 2729f2f7fe..59d2b6424f 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Log exception type when catching the exception (`#1749 `_) +* [CM] Handle other exceptions while loading the controller plugin (`#1731 `_) +* remove unnecessary log of the CM args (`#1720 `_) +* Fix unload of controllers when spawned with `--unload-on-kill` (`#1717 `_) +* Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + 4.16.1 (2024-08-24) ------------------- * propage a portion of global args to the controller nodes (`#1712 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 56e80d736b..eecd49e4a3 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.1 (2024-08-24) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 405756a48c..380b77cd96 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Log exception type when catching the exception (`#1749 `_) +* Fix spam of logs on failed hardware component initialization (`#1719 `_) +* [HWItfs] Add key-value-storage to the InterfaceInfo (`#1421 `_) +* Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + 4.16.1 (2024-08-24) ------------------- diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index f1cd9531d0..5a45c913da 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.1 (2024-08-24) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 02f6673e88..c5edbde2b3 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.1 (2024-08-24) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index f522eda772..2a914e13ba 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.1 (2024-08-24) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index d5e898924b..af760bcd9c 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [HWItfs] Add key-value-storage to the InterfaceInfo (`#1421 `_) +* Contributors: Manuel Muth + 4.16.1 (2024-08-24) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 639a295a06..044094be1f 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ros2controlcli] fix list_controllers when no controllers are loaded (`#1721 `_) +* Contributors: Sai Kishor Kothakota + 4.16.1 (2024-08-24) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index abfab9c3c3..472df2ddc0 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.1 (2024-08-24) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 523f72c1ee..feebd90269 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.1 (2024-08-24) ------------------- From aefbad38e4371aff499c39f53c8b296a557142cd Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 11 Sep 2024 12:14:23 +0100 Subject: [PATCH 3/7] 4.17.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 0543bd1033..7d9a50d899 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- * Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 `_) * Contributors: Manuel Muth diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 39d15a6f20..e7719e053c 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.16.1 + 4.17.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 59d2b6424f..ab422cd13a 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- * Log exception type when catching the exception (`#1749 `_) * [CM] Handle other exceptions while loading the controller plugin (`#1731 `_) * remove unnecessary log of the CM args (`#1720 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index cb6b380092..d17db9c4ae 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.16.1 + 4.17.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index eecd49e4a3..485af0d0f1 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- 4.16.1 (2024-08-24) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 195fda32f1..6f967ebda8 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.16.1 + 4.17.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 380b77cd96..19e0e6d980 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- * Log exception type when catching the exception (`#1749 `_) * Fix spam of logs on failed hardware component initialization (`#1719 `_) * [HWItfs] Add key-value-storage to the InterfaceInfo (`#1421 `_) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 8c19535675..48bb6ae030 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.16.1 + 4.17.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 5a45c913da..d70c618e2c 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- 4.16.1 (2024-08-24) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index a88ae6ba93..7c8e8e0b25 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.16.1 + 4.17.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index c5edbde2b3..62076925b9 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- 4.16.1 (2024-08-24) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 8305cff9e5..68bc4fc0a3 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.16.1 + 4.17.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2a914e13ba..6e4e2435cd 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- 4.16.1 (2024-08-24) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index a8c423658d..c0cf9ae4e1 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.16.1 + 4.17.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index af760bcd9c..5112940255 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- * [HWItfs] Add key-value-storage to the InterfaceInfo (`#1421 `_) * Contributors: Manuel Muth diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index ea22ab6b11..bb2c67dc3d 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.16.1 + 4.17.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 044094be1f..0dc41eecdc 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- * [ros2controlcli] fix list_controllers when no controllers are loaded (`#1721 `_) * Contributors: Sai Kishor Kothakota diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 176334ad54..a7c714877c 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.16.1 + 4.17.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index c0395ec63d..41e2ac2c18 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.16.1", + version="4.17.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 472df2ddc0..d28a2ab28f 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- 4.16.1 (2024-08-24) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 90e9fd1462..b6fd5c2fe8 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.16.1 + 4.17.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index cd0b3141a7..e08ed3d41d 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.16.1", + version="4.17.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index feebd90269..2e523b066d 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-09-11) +------------------- 4.16.1 (2024-08-24) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 6a982ddef9..77bd1c22f8 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.16.1 + 4.17.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 111321c11feadf2cab68a0d229d8963d121dd967 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Sep 2024 16:42:34 +0200 Subject: [PATCH 4/7] [RM] Execute `error` callback of component on returning ERROR or with exception (#1730) --- hardware_interface/src/actuator.cpp | 4 +++- hardware_interface/src/resource_manager.cpp | 2 ++ hardware_interface/src/sensor.cpp | 4 +++- hardware_interface/src/system.cpp | 4 +++- 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 13f2090d1f..653b25aac3 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -183,7 +183,9 @@ const rclcpp_lifecycle::State & Actuator::deactivate() const rclcpp_lifecycle::State & Actuator::error() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_error(impl_->get_lifecycle_state())) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index f368f2a00a..c2593030c5 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1808,6 +1808,7 @@ HardwareReadWriteStatus ResourceManager::read( } if (ret_val == return_type::ERROR) { + component.error(); read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); @@ -1868,6 +1869,7 @@ HardwareReadWriteStatus ResourceManager::write( } if (ret_val == return_type::ERROR) { + component.error(); read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 83b4c9face..b5484ee233 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -182,7 +182,9 @@ const rclcpp_lifecycle::State & Sensor::deactivate() const rclcpp_lifecycle::State & Sensor::error() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_error(impl_->get_lifecycle_state())) { diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 6e96814c24..044eea9d6b 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -181,7 +181,9 @@ const rclcpp_lifecycle::State & System::deactivate() const rclcpp_lifecycle::State & System::error() { std::unique_lock lock(system_mutex_); - if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_error(impl_->get_lifecycle_state())) { From 447228704ecdf953fefd40d4ea64f8c14d4c5f8a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Sep 2024 16:46:15 +0200 Subject: [PATCH 5/7] [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (#1703) --- .../controller_manager_services.py | 20 ++- doc/release_notes.rst | 12 ++ ros2controlcli/doc/userdoc.rst | 137 +++++++++++------- ros2controlcli/ros2controlcli/api/__init__.py | 47 ++---- .../verb/list_controller_types.py | 2 +- .../ros2controlcli/verb/list_controllers.py | 2 +- .../verb/list_hardware_components.py | 2 +- .../verb/list_hardware_interfaces.py | 2 +- .../ros2controlcli/verb/load_controller.py | 93 +++++++++--- .../verb/reload_controller_libraries.py | 2 +- .../verb/set_controller_state.py | 2 +- .../verb/set_hardware_component_state.py | 2 +- .../ros2controlcli/verb/switch_controllers.py | 13 +- .../ros2controlcli/verb/unload_controller.py | 15 +- .../verb/view_controller_chains.py | 2 +- 15 files changed, 223 insertions(+), 130 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a09be5d14b..a1d3c0f5ad 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -88,15 +88,23 @@ def service_caller( @return The service response """ - cli = node.create_client(service_type, service_name) + namespace = "" if node.get_namespace() == "/" else node.get_namespace() + fully_qualified_service_name = ( + f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name + ) + cli = node.create_client(service_type, fully_qualified_service_name) while not cli.service_is_ready(): - node.get_logger().info(f"waiting for service {service_name} to become available...") + node.get_logger().info( + f"waiting for service {fully_qualified_service_name} to become available..." + ) if service_timeout: if not cli.wait_for_service(service_timeout): - raise ServiceNotFoundError(f"Could not contact service {service_name}") + raise ServiceNotFoundError( + f"Could not contact service {fully_qualified_service_name}" + ) elif not cli.wait_for_service(10.0): - node.get_logger().warn(f"Could not contact service {service_name}") + node.get_logger().warn(f"Could not contact service {fully_qualified_service_name}") node.get_logger().debug(f"requester: making request: {request}\n") future = None @@ -105,13 +113,13 @@ def service_caller( rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) if future.result() is None: node.get_logger().warning( - f"Failed getting a result from calling {service_name} in " + f"Failed getting a result from calling {fully_qualified_service_name} in " f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" ) else: return future.result() raise RuntimeError( - f"Could not successfully call service {service_name} after {max_attempts} attempts." + f"Could not successfully call service {fully_qualified_service_name} after {max_attempts} attempts." ) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 097ab208b0..bb9c10abe0 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -118,3 +118,15 @@ ros2controlcli .. code-block:: bash ros2 control set_hardware_component_state + +* The ``load_controller`` now supports parsing of the params file (`#1703 `_). + + .. code-block:: bash + + ros2 control load_controller + +* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 `_). + + .. code-block:: bash + + ros2 control --ros-args -r __ns:= diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 5dd88cd6d9..8449583d61 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -28,18 +28,30 @@ list_controllers .. code-block:: console $ ros2 control list_controllers -h - usage: ros2 control list_controllers [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control list_controllers [-h] [--spin-time SPIN_TIME] [-s] [--claimed-interfaces] [--required-state-interfaces] [--required-command-interfaces] [--chained-interfaces] [--reference-interfaces] [--verbose] + [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the list of loaded controllers, their type and status - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --claimed-interfaces List controller's claimed interfaces + --required-state-interfaces + List controller's required state interfaces + --required-command-interfaces + List controller's required command interfaces + --chained-interfaces List interfaces that the controllers are chained to + --reference-interfaces + List controller's exported references + --verbose, -v List controller's claimed interfaces, required state interfaces and required command interfaces -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable Example output: @@ -55,18 +67,20 @@ list_controller_types .. code-block:: console $ ros2 control list_controller_types -h - usage: ros2 control list_controller_types [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control list_controller_types [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the available controller types and their base classes - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable Example output: @@ -84,21 +98,21 @@ list_hardware_components .. code-block:: console $ ros2 control list_hardware_components -h - usage: ros2 control list_hardware_components [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control list_hardware_components [-h] [--spin-time SPIN_TIME] [-s] [--verbose] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the list of available hardware components options: - -h, --help show this help message and exit - --spin-time SPIN_TIME + -h, --help show this help message and exit + --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - -s, --use-sim-time Enable ROS simulation time - --verbose, -v List hardware components with command and state interfaces - -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node - --include-hidden-nodes + -s, --use-sim-time Enable ROS simulation time + --verbose, -v List hardware components with command and state interfaces + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes Consider hidden nodes as well - + --ros-args ... Pass arbitrary arguments to the executable Example output: @@ -118,18 +132,20 @@ list_hardware_interfaces .. code-block:: console $ ros2 control list_hardware_interfaces -h - usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the list of available command and state interfaces - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable .. code-block:: console @@ -149,23 +165,26 @@ load_controller .. code-block:: console $ ros2 control load_controller -h - usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set-state {inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [-s] [--set-state {inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name [param_file] Load a controller in a controller manager positional arguments: controller_name Name of the controller + param_file The YAML file with the controller parameters - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time --set-state {inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable reload_controller_libraries --------------------------- @@ -173,19 +192,21 @@ reload_controller_libraries .. code-block:: console $ ros2 control reload_controller_libraries -h - usage: ros2 control reload_controller_libraries [-h] [--spin-time SPIN_TIME] [--force-kill] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control reload_controller_libraries [-h] [--spin-time SPIN_TIME] [-s] [--force-kill] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Reload controller libraries - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time --force-kill Force stop of loaded controllers -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable set_controller_state -------------------- @@ -193,23 +214,24 @@ set_controller_state .. code-block:: console $ ros2 control set_controller_state -h - usage: ros2 control set_controller_state [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name {inactive,active} + usage: ros2 control set_controller_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name {inactive,active} Adjust the state of the controller positional arguments: controller_name Name of the controller to be changed - {inactive,active} - State in which the controller should be changed to + {inactive,active} State in which the controller should be changed to - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable set_hardware_component_state ---------------------------- @@ -217,8 +239,7 @@ set_hardware_component_state .. code-block:: console $ ros2 control set_hardware_component_state -h - usage: ros2 control set_hardware_component_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] - hardware_component_name {unconfigured,inactive,active} + usage: ros2 control set_hardware_component_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] hardware_component_name {unconfigured,inactive,active} Adjust the state of the hardware component @@ -234,9 +255,10 @@ set_hardware_component_state Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable switch_controllers ------------------ @@ -244,27 +266,29 @@ switch_controllers .. code-block:: console $ ros2 control switch_controllers -h - usage: ros2 control switch_controllers [-h] [--spin-time SPIN_TIME] [--deactivate [CTRL1 [CTRL2 ...]]] [--activate [CTRL1 [CTRL2 ...]]] [--strict] [--activate-asap] [--switch-timeout SWITCH_TIMEOUT] [-c CONTROLLER_MANAGER] - [--include-hidden-nodes] + usage: ros2 control switch_controllers [-h] [--spin-time SPIN_TIME] [-s] [--deactivate [DEACTIVATE ...]] [--activate [ACTIVATE ...]] [--strict] [--activate-asap] [--switch-timeout SWITCH_TIMEOUT] + [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Switch controllers in a controller manager - optional arguments: - -h, --help show this help message and exit - --spin-time SPIN_TIME - Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - --deactivate [CTRL1 [CTRL2 ...]] - Name of the controllers to be deactivate - --activate [CTRL1 [CTRL2 ...]] - Name of the controllers to be activated - --strict Strict switch - --activate-asap Activate asap controllers - --switch-timeout SWITCH_TIMEOUT - Timeout for switching controllers - -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node - --include-hidden-nodes - Consider hidden nodes as well + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --deactivate [DEACTIVATE ...] + Name of the controllers to be deactivated + --activate [ACTIVATE ...] + Name of the controllers to be activated + --strict Strict switch + --activate-asap Start asap controllers + --switch-timeout SWITCH_TIMEOUT + Timeout for switching controllers + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable unload_controller ----------------- @@ -272,21 +296,23 @@ unload_controller .. code-block:: console $ ros2 control unload_controller -h - usage: ros2 control unload_controller [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control unload_controller [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name Unload a controller in a controller manager positional arguments: controller_name Name of the controller - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable view_controller_chains ---------------------- @@ -294,7 +320,7 @@ view_controller_chains .. code-block:: console $ ros2 control view_controller_chains -h - usage: ros2 view_controller_chains + usage: ros2 control view_controller_chains [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Generates a diagram of the loaded chained controllers into /tmp/controller_diagram.gv.pdf @@ -304,6 +330,7 @@ view_controller_chains Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index 2b4b805980..7ff78c3ed0 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -15,40 +15,13 @@ from controller_manager import list_controllers, list_hardware_components -import rclpy - from ros2cli.node.direct import DirectNode from ros2node.api import NodeNameCompleter from ros2param.api import call_list_parameters - -def service_caller(service_name, service_type, request): - try: - rclpy.init() - - node = rclpy.create_node(f"ros2controlcli_{service_name.replace('/', '')}_requester") - - cli = node.create_client(service_type, service_name) - - if not cli.service_is_ready(): - node.get_logger().debug(f"waiting for service {service_name} to become available...") - - if not cli.wait_for_service(2.0): - raise RuntimeError(f"Could not contact service {service_name}") - - node.get_logger().debug(f"requester: making request: {repr(request)}\n") - future = cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - if future.result() is not None: - return future.result() - else: - future_exception = future.exception() - raise RuntimeError(f"Exception while calling service: {repr(future_exception)}") - finally: - node.destroy_node() - rclpy.shutdown() +import argparse class ControllerNameCompleter: @@ -89,16 +62,28 @@ def __call__(self, prefix, parsed_args, **kwargs): return [c.name for c in hardware_components if c.state.label in self.valid_states] +class ParserROSArgs(argparse.Action): + def __call__(self, parser, namespace, values, option_string=None): + values = [option_string] + values + setattr(namespace, "argv", values) + + def add_controller_mgr_parsers(parser): - """Parser arguments to get controller manager node name, defaults to /controller_manager.""" + """Parser arguments to get controller manager node name, defaults to controller_manager.""" arg = parser.add_argument( "-c", "--controller-manager", - help="Name of the controller manager ROS node", - default="/controller_manager", + help="Name of the controller manager ROS node (default: controller_manager)", + default="controller_manager", required=False, ) arg.completer = NodeNameCompleter(include_hidden_nodes_key="include_hidden_nodes") parser.add_argument( "--include-hidden-nodes", action="store_true", help="Consider hidden nodes as well" ) + parser.add_argument( + "--ros-args", + nargs=argparse.REMAINDER, + help="Pass arbitrary arguments to the executable", + action=ParserROSArgs, + ) diff --git a/ros2controlcli/ros2controlcli/verb/list_controller_types.py b/ros2controlcli/ros2controlcli/verb/list_controller_types.py index 086b820124..370c8cdc0d 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controller_types.py +++ b/ros2controlcli/ros2controlcli/verb/list_controller_types.py @@ -29,7 +29,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = list_controller_types(node, args.controller_manager) types_and_classes = zip(response.types, response.base_classes) for c in types_and_classes: diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index b4ebff94cd..6b6a42e3ce 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -95,7 +95,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = list_controllers(node, args.controller_manager) if not response.controller: diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index e3e2269920..3194fee73f 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -36,7 +36,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: hardware_components = list_hardware_components(node, args.controller_manager) for idx, component in enumerate(hardware_components.component): diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 4510998ad9..a0bf8a8403 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -28,7 +28,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: hardware_interfaces = list_hardware_interfaces(node, args.controller_manager) command_interfaces = sorted( hardware_interfaces.command_interfaces, key=lambda hwi: hwi.name diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index b5a155de94..e47540a1df 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -12,13 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import configure_controller, load_controller, switch_controllers +from controller_manager import ( + configure_controller, + load_controller, + list_controllers, + switch_controllers, + set_controller_parameters_from_param_file, + bcolors, +) from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy from ros2cli.verb import VerbExtension from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter +import os +from argparse import OPTIONAL class LoadControllerVerb(VerbExtension): @@ -28,37 +37,79 @@ def add_arguments(self, parser, cli_name): add_arguments(parser) arg = parser.add_argument("controller_name", help="Name of the controller") arg.completer = ControllerNameCompleter() + arg = parser.add_argument( + "param_file", + help="The YAML file with the controller parameters", + nargs=OPTIONAL, + default=None, + ) arg = parser.add_argument( "--set-state", choices=["inactive", "active"], help="Set the state of the loaded controller", + default=None, ) add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: - response = load_controller(node, args.controller_manager, args.controller_name) - if not response.ok: - return "Error loading controller, check controller_manager logs" + with NodeStrategy(args).direct_node as node: + controllers = list_controllers(node, args.controller_manager, 20.0).controller + if any(c.name == args.controller_name for c in controllers): + print( + f"{bcolors.WARNING}Controller : {args.controller_name} already loaded, skipping load_controller!{bcolors.ENDC}" + ) + else: + if args.param_file: + if not os.path.exists(args.param_file): + print( + f"{bcolors.FAIL}Controller parameter file : {args.param_file} does not exist, Aborting!{bcolors.ENDC}" + ) + return 1 + if not os.path.isabs(args.param_file): + args.param_file = os.path.join(os.getcwd(), args.param_file) - if not args.set_state: - print(f"Successfully loaded controller {args.controller_name}") - return 0 + if not set_controller_parameters_from_param_file( + node, + args.controller_manager, + args.controller_name, + args.param_file, + node.get_namespace(), + ): + return 1 - # we in any case configure the controller - response = configure_controller(node, args.controller_manager, args.controller_name) - if not response.ok: - return "Error configuring controller" + ret = load_controller(node, args.controller_manager, args.controller_name) + if not ret.ok: + print( + f"{bcolors.FAIL}Failed loading controller {args.controller_name} check controller_manager logs{bcolors.ENDC}" + ) + return 1 + print( + f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name}{bcolors.ENDC}" + ) - if args.set_state == "active": - response = switch_controllers( - node, args.controller_manager, [], [args.controller_name], True, True, 5.0 + if args.set_state: + + # we in any case configure the controller + response = configure_controller( + node, args.controller_manager, args.controller_name ) if not response.ok: - return "Error activating controller, check controller_manager logs" + print( + f"{bcolors.FAIL}Error configuring controller : {args.controller_name}{bcolors.ENDC}" + ) + return 1 + + if args.set_state == "active": + response = switch_controllers( + node, args.controller_manager, [], [args.controller_name], True, True, 5.0 + ) + if not response.ok: + print( + f"{bcolors.FAIL}Error activating controller : {args.controller_name}, check controller_manager logs{bcolors.ENDC}" + ) + return 1 - print( - f"Successfully loaded controller {args.controller_name} into " - f'state {"inactive" if args.set_state == "inactive" else "active"}' - ) - return 0 + print( + f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name} into state {args.set_state}{bcolors.ENDC}" + ) + return 0 diff --git a/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py b/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py index 82bc2e480f..e60c9987a0 100644 --- a/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py +++ b/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py @@ -32,7 +32,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = reload_controller_libraries( node, args.controller_manager, force_kill=args.force_kill ) diff --git a/ros2controlcli/ros2controlcli/verb/set_controller_state.py b/ros2controlcli/ros2controlcli/verb/set_controller_state.py index 7cc44775a1..830d309b13 100644 --- a/ros2controlcli/ros2controlcli/verb/set_controller_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_controller_state.py @@ -37,7 +37,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: controllers = list_controllers(node, args.controller_manager).controller try: diff --git a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py index 4b1093f4f7..00f62f3e4c 100644 --- a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py @@ -39,7 +39,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: if args.state == "unconfigured": diff --git a/ros2controlcli/ros2controlcli/verb/switch_controllers.py b/ros2controlcli/ros2controlcli/verb/switch_controllers.py index 1c798c29b8..60ceb000c4 100644 --- a/ros2controlcli/ros2controlcli/verb/switch_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/switch_controllers.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import switch_controllers +from controller_manager import switch_controllers, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -52,7 +52,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = switch_controllers( node, args.controller_manager, @@ -63,7 +63,12 @@ def main(self, *, args): args.switch_timeout, ) if not response.ok: - return "Error switching controllers, check controller_manager logs" + print( + bcolors.FAIL + + "Error switching controllers, check controller_manager logs" + + bcolors.ENDC + ) + return 1 - print("Successfully switched controllers") + print(bcolors.OKBLUE + "Successfully switched controllers" + bcolors.ENDC) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/unload_controller.py b/ros2controlcli/ros2controlcli/verb/unload_controller.py index 81612eb3ad..cd445cbb70 100644 --- a/ros2controlcli/ros2controlcli/verb/unload_controller.py +++ b/ros2controlcli/ros2controlcli/verb/unload_controller.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import unload_controller +from controller_manager import unload_controller, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -31,10 +31,15 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = unload_controller(node, args.controller_manager, args.controller_name) if not response.ok: - return "Error unloading controllers, check controller_manager logs" - - print(f"Successfully unloaded controller {args.controller_name}") + print( + f"{bcolors.FAIL}Error unloading controller {args.controller_name}, check controller_manager logs{bcolors.ENDC}" + ) + return 1 + + print( + f"{bcolors.OKBLUE}Successfully unloaded controller {args.controller_name}{bcolors.ENDC}" + ) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py index f838a96e1c..dafe067539 100644 --- a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py +++ b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py @@ -202,7 +202,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: list_controllers_response = list_controllers(node, args.controller_manager) list_hardware_response = list_hardware_interfaces(node, args.controller_manager) parse_response(list_controllers_response, list_hardware_response) From eb4c19d9edb846a3ad31b932b78339115cae52ce Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Sep 2024 20:07:29 +0200 Subject: [PATCH 6/7] [Controller Interface] Make assign and release interfaces virtual (#1743) --- .../controller_interface_base.hpp | 17 +++++++++++++++-- doc/release_notes.rst | 1 + 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index fbb2111cc9..c4a19fd8e3 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -104,13 +104,26 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual InterfaceConfiguration state_interface_configuration() const = 0; + /// Method that assigns the Loaned interfaces to the controller. + /** + * Method used by the controller_manager to assign the interfaces to the controller. + * \note When this method is overridden, the user has to also implement the `release_interfaces` + * method by overriding it to release the interfaces. + * + * \param[in] command_interfaces vector of command interfaces to be assigned to the controller. + * \param[in] state_interfaces vector of state interfaces to be assigned to the controller. + */ CONTROLLER_INTERFACE_PUBLIC - void assign_interfaces( + virtual void assign_interfaces( std::vector && command_interfaces, std::vector && state_interfaces); + /// Method that releases the Loaned interfaces from the controller. + /** + * Method used by the controller_manager to release the interfaces from the controller. + */ CONTROLLER_INTERFACE_PUBLIC - void release_interfaces(); + virtual void release_interfaces(); CONTROLLER_INTERFACE_PUBLIC return_type init( diff --git a/doc/release_notes.rst b/doc/release_notes.rst index bb9c10abe0..b116f78938 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -24,6 +24,7 @@ For details see the controller_manager section. * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. * The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. +* The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) controller_manager ****************** From 84e85f9f0bbd67af9eebbe40bf5eef89792945ff Mon Sep 17 00:00:00 2001 From: Santosh Govindaraj <75157452+SantoshGovindaraj@users.noreply.github.com> Date: Tue, 17 Sep 2024 13:10:49 +0100 Subject: [PATCH 7/7] Add test coverage for `params_file` parameter in spawner/unspawner tests (#1754) --- .../test/test_spawner_unspawner.cpp | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 0812d7ffa4..cba832f8bf 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -277,6 +277,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) 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 chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( @@ -287,6 +289,9 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ( 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(), + test_file_path); EXPECT_EQ( call_spawner( @@ -302,12 +307,17 @@ TEST_F(TestLoadController, 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_with_parameters_and_type.params_file").as_string(), test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + test_file_path); } TEST_F(TestLoadController, unload_on_kill) @@ -367,6 +377,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); } // Try to spawn now the controller with fallback controllers inside the yaml @@ -381,6 +392,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); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); @@ -389,6 +401,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); auto ctrl_3 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); @@ -396,6 +409,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); } } @@ -680,6 +694,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) 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 chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( @@ -690,6 +706,9 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ( 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(), + test_file_path); EXPECT_EQ( call_spawner( @@ -705,12 +724,17 @@ 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_with_parameters_and_type.params_file").as_string(), test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + test_file_path); } TEST_F( @@ -758,6 +782,8 @@ TEST_F( 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 chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( @@ -768,6 +794,9 @@ TEST_F( ASSERT_EQ( 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(), + test_file_path); EXPECT_EQ( call_spawner( @@ -783,10 +812,15 @@ 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_with_parameters_and_type.params_file").as_string(), test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + test_file_path); }