diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 5a0a5ff573..7d9a50d899 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.17.0 (2024-09-11) +------------------- +* 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_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 2729f2f7fe..ab422cd13a 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `_) +* 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/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/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/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/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 56e80d736b..485af0d0f1 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/doc/release_notes.rst b/doc/release_notes.rst index 6402af6932..b116f78938 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -119,3 +119,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/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 405756a48c..19e0e6d980 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `_) +* 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/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/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 72e966fcab..c2593030c5 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 (...) @@ -1804,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()); @@ -1851,8 +1856,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 (...) @@ -1864,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())) { diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index f1cd9531d0..d70c618e2c 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 02f6673e88..62076925b9 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 f522eda772..6e4e2435cd 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 d5e898924b..5112940255 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.17.0 (2024-09-11) +------------------- +* [HWItfs] Add key-value-storage to the InterfaceInfo (`#1421 `_) +* Contributors: Manuel Muth + 4.16.1 (2024-08-24) ------------------- 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 639a295a06..0dc41eecdc 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.17.0 (2024-09-11) +------------------- +* [ros2controlcli] fix list_controllers when no controllers are loaded (`#1721 `_) +* Contributors: Sai Kishor Kothakota + 4.16.1 (2024-08-24) ------------------- 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/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/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) 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 abfab9c3c3..d28a2ab28f 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 523f72c1ee..2e523b066d 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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