diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 76a90ab314..036d27ea29 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp_lifecycle REQUIRED) diff --git a/controller_interface/include/controller_interface/controller_parameters.hpp b/controller_interface/include/controller_interface/controller_parameters.hpp index eccf6cf16c..f812a754fa 100644 --- a/controller_interface/include/controller_interface/controller_parameters.hpp +++ b/controller_interface/include/controller_interface/controller_parameters.hpp @@ -27,16 +27,32 @@ namespace controller_interface { +using rcl_interfaces::msg::ParameterType; + +namespace impl +{ + inline std::string normalize_params_prefix(std::string prefix) + { + if (!prefix.empty()) { + if ('.' != prefix.back()) { + prefix += '.'; + } + } + return prefix; + } +} // namespace impl + struct Parameter { Parameter() = default; - explicit Parameter(const std::string & name, bool configurable = false) - : name(name), configurable(configurable) + explicit Parameter(const std::string & name, const uint8_t type, const bool configurable = false) + : name(name), type(type), configurable(configurable) { } std::string name; + uint8_t type; bool configurable; }; @@ -44,89 +60,114 @@ class ControllerParameters { public: ControllerParameters( + const std::string & params_prefix = "", int nr_bool_params = 0, int nr_double_params = 0, int nr_string_params = 0, - int nr_string_list_params = 0); + int nr_double_array_params = 0, int nr_string_list_params = 0); virtual ~ControllerParameters() = default; - virtual void declare_parameters(rclcpp::Node::SharedPtr node); + void declare_parameters(rclcpp::Node::SharedPtr node); + + void declare_parameters( + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params); /** - * Gets all defined parameters from. + * Gets all defined parameters from parameter server. If parameters are not declared, they will + * be declared first. + * See also documentation of another `get_parameters` method. * - * \param[node] shared pointer to the node where parameters should be read. - * \return true if all parameters are read Successfully, false if a parameter is not provided or + * \param[in] node shared pointer to the node where parameters should be read. + * \return true if all parameters are read successfully, false if a parameter is not provided or * parameter configuration is wrong. */ - virtual bool get_parameters(rclcpp::Node::SharedPtr node, bool check_validity = true); + bool get_parameters(rclcpp::Node::SharedPtr node, bool check_validity = true, bool update = true); - virtual rcl_interfaces::msg::SetParametersResult set_parameter_callback( + /** + * Gets all defined parameters from parameter server after parameters are declared. + * Optionally, check parameters' validity and update storage. + * + * \param[in] check_validity check validity after getting parameters (default: **true**). + * \param[in] update update parameters in storage after getting them (default: **true**). + * \return true if all parameters are read successfully, false if a parameter is not provided or + * parameter configuration is wrong. + */ + bool get_parameters(bool check_validity = true, bool update = true); + + /** + * Update all storage variables from the parameter maps. + * Parameters will be only updated if they are previously read from parameter server or dynamic + * reconfigure callback occurred. + * + * \param[in] check_validity check validity before updating parameters (default: **true**). + * \return is update was successful, i.e., parameters are valid. If \p check_validity is set + * **false**, the result is always **true**. + */ + bool update(bool check_validity = true); + + rcl_interfaces::msg::SetParametersResult set_parameter_callback( const std::vector & parameters); +protected: /** - * Default implementation of parameter check. No check is done. Always returns true. + * Override this method to implement custom parameters check. + * Default implementation does not checks anything, always returns true. * - * \return true + * \return **true** if parameters are valid, **false** otherwise. */ virtual bool check_if_parameters_are_valid() { return true; } - virtual void update() = 0; - - // TODO(destogl): Test this: "const rclcpp::Node::SharedPtr & node" + /** + * Mapping between parameter storage and implementation members. + * The order of parameter in storage is the order of adding parameters to the storage. + * E.g., to access the value of 5-th string parameter added to storage use: + * `fifth_string_param_ = string_parameters_[4].second; + * where `fifth_string_param_` is the name of the member of a child class. + */ + virtual void update_storage() = 0; protected: void add_bool_parameter( - const std::string & name, bool configurable = false, bool default_value = false) + const std::string & name, const bool & configurable = false, const bool & default_value = false) { - bool_parameters_.push_back({Parameter(name, configurable), default_value}); + bool_parameters_.push_back({ + Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), default_value}); } void add_double_parameter( - const std::string & name, bool configurable = false, - double default_value = std::numeric_limits::quiet_NaN()) + const std::string & name, const bool & configurable = false, + const double & default_value = std::numeric_limits::quiet_NaN()) { - double_parameters_.push_back({Parameter(name, configurable), default_value}); + double_parameters_.push_back({ + Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), default_value}); } void add_string_parameter( - const std::string & name, bool configurable = false, const std::string & default_value = "") + const std::string & name, const bool & configurable = false, + const std::string & default_value = "") { - string_parameters_.push_back({Parameter(name, configurable), default_value}); + string_parameters_.push_back({ + Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), default_value}); } - void add_string_list_parameter( - const std::string & name, bool configurable = false, - const std::vector & default_value = {}) + void add_double_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {std::numeric_limits::quiet_NaN()}) { - string_list_parameters_.push_back({Parameter(name, configurable), default_value}); + double_array_parameters_.push_back({ + Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), default_value}); } - template - bool get_parameters_from_list( - const rclcpp::Node::SharedPtr node, std::vector> & parameters) + void add_string_list_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) { - bool ret = true; - for (auto & parameter : parameters) - { - try - { - rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? - ret = node->get_parameter(parameter.first.name, input_parameter); - parameter.second = input_parameter.get_value(); - } - catch (rclcpp::ParameterTypeException & e) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); - ret = false; - break; - } - } - return ret; + string_list_parameters_.push_back({ + Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), default_value}); } - template - bool empty_parameter_in_list(const std::vector> & parameters) + template + bool empty_parameter_in_list(const std::vector> & parameters) { bool ret = false; for (const auto & parameter : parameters) @@ -156,9 +197,9 @@ class ControllerParameters return ret; } - template + template bool find_and_assign_parameter_value( - std::vector> & parameter_list, + std::vector> & parameter_list, const rclcpp::Parameter & input_parameter) { auto is_in_list = [&](const auto & parameter) { @@ -176,8 +217,8 @@ class ControllerParameters } else { - it->second = input_parameter.get_value(); - RCUTILS_LOG_ERROR_NAMED( + it->second = input_parameter.get_value(); + RCUTILS_LOG_INFO_NAMED( logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), input_parameter.value_to_string().c_str()); return true; @@ -189,22 +230,97 @@ class ControllerParameters } } + // Storage members std::vector> bool_parameters_; std::vector> double_parameters_; + std::vector>> double_array_parameters_; std::vector> string_parameters_; std::vector>> string_list_parameters_; + // Input/Output members to ROS 2 std::string logger_name_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; + + bool declared_; + bool up_to_date_; + std::string params_prefix_; private: - template + template void declare_parameters_from_list( - rclcpp::Node::SharedPtr node, const std::vector> & parameters) + rclcpp::Node::SharedPtr node, const std::vector> & parameters) + { + for (const auto & parameter : parameters) + { + node->declare_parameter(parameter.first.name, parameter.second); + } + } + + template + void declare_parameters_from_list(const std::vector> & parameters) { for (const auto & parameter : parameters) { - node->declare_parameter(parameter.first.name, parameter.second); + // declare parameter only if it does not already exists + if (!params_interface_->has_parameter(parameter.first.name)) + { + rclcpp::ParameterValue default_parameter_value(parameter.second); + rcl_interfaces::msg::ParameterDescriptor descr; + descr.name = parameter.first.name; + descr.type = parameter.first.type; + descr.read_only = false; + + params_interface_->declare_parameter(parameter.first.name, default_parameter_value, descr); + } + } + } + + template + bool get_parameters_from_list( + const rclcpp::Node::SharedPtr node, std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = node->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } + } + return ret; + } + + template + bool get_parameters_from_list(std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = params_interface_->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } } + return ret; } }; diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 22944f63dd..03cbe396c9 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -10,10 +10,12 @@ ament_cmake + backward_ros hardware_interface rclcpp_lifecycle sensor_msgs + backward_ros hardware_interface rclcpp_lifecycle sensor_msgs diff --git a/controller_interface/src/controller_parameters.cpp b/controller_interface/src/controller_parameters.cpp index efb87c2dba..8c4029f4ff 100644 --- a/controller_interface/src/controller_parameters.cpp +++ b/controller_interface/src/controller_parameters.cpp @@ -24,43 +24,125 @@ namespace controller_interface { ControllerParameters::ControllerParameters( - int nr_bool_params, int nr_double_params, int nr_string_params, int nr_string_list_params) + const std::string & params_prefix, + int nr_bool_params, int nr_double_params, int nr_string_params, int nr_double_array_params, + int nr_string_list_params) : declared_(false), up_to_date_(false), params_prefix_("") { + params_prefix_ = impl::normalize_params_prefix(params_prefix); + bool_parameters_.reserve(nr_bool_params); double_parameters_.reserve(nr_double_params); string_parameters_.reserve(nr_string_params); + double_array_parameters_.reserve(nr_double_array_params); string_list_parameters_.reserve(nr_string_list_params); } void ControllerParameters::declare_parameters(rclcpp::Node::SharedPtr node) { - logger_name_ = std::string(node->get_name()) + "::parameters"; + declare_parameters(node->get_node_logging_interface(), node->get_node_parameters_interface()); +} + +void ControllerParameters::declare_parameters( + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params) +{ + if (!declared_) + { + logger_name_ = std::string(node_logger->get_logger_name()) + + (params_prefix_.empty() ? "" : ("::" + params_prefix_)) + + "::parameters"; + logging_interface_ = node_logger; + params_interface_ = node_params; + + declare_parameters_from_list(bool_parameters_); + declare_parameters_from_list(double_parameters_); + declare_parameters_from_list(string_parameters_); + declare_parameters_from_list(double_array_parameters_); + declare_parameters_from_list(string_list_parameters_); + + declared_ = true; + } + else + { + RCUTILS_LOG_WARN_NAMED( + logger_name_.c_str(), + "Parameters already declared. Declaration should be done only once. " + "Nothing bad will happen, but please correct your code-logic."); + } +} + +bool ControllerParameters::get_parameters( + rclcpp::Node::SharedPtr node, bool check_validity, bool update) +{ + if (!declared_) + { + declare_parameters(node); + } - declare_parameters_from_list(node, bool_parameters_); - declare_parameters_from_list(node, double_parameters_); - declare_parameters_from_list(node, string_parameters_); - declare_parameters_from_list(node, string_list_parameters_); + return get_parameters(check_validity, update); } -/** - * Gets all defined parameters from. - * - * \param[node] shared pointer to the node where parameters should be read. - * \return true if all parameters are read Successfully, false if a parameter is not provided or - * parameter configuration is wrong. - */ -bool ControllerParameters::get_parameters(rclcpp::Node::SharedPtr node, bool check_validity) +bool ControllerParameters::get_parameters(bool check_validity, bool update) { + if (!declared_) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Can not get parameters. Please declare them first."); + return false; + } + bool ret = false; - ret = get_parameters_from_list(node, bool_parameters_) && - get_parameters_from_list(node, double_parameters_) && - get_parameters_from_list(node, string_parameters_) && - get_parameters_from_list(node, string_list_parameters_); + // If parameters are updated using dynamic reconfigure callback then there is no need to read + // them again. To ignore multiple manual reads + ret = get_parameters_from_list(bool_parameters_) && + get_parameters_from_list(double_parameters_) && + get_parameters_from_list(string_parameters_) && + get_parameters_from_list(double_array_parameters_) && + get_parameters_from_list(string_list_parameters_); if (ret && check_validity) { - ret = check_if_parameters_are_valid(); + ret = this->check_if_parameters_are_valid(); + } + + // If it is all good until now the parameters are not up to date anymore + if (ret) + { + up_to_date_ = false; + } + + if (ret && update) + { + ret = this->update(false); + } + + return ret; +} + +bool ControllerParameters::update(bool check_validity) +{ + bool ret = true; + + // Let's make this efficient and execute code only if parameters are updated + if (!up_to_date_) + { + if (check_validity) + { + ret = this->check_if_parameters_are_valid(); + } + + if (ret) + { + this->update_storage(); + } + else + { + RCUTILS_LOG_WARN_NAMED( + logger_name_.c_str(), "Parameters are not valid and therefore will not be updated"); + } + // reset variable to update parameters only when this is needed + up_to_date_ = true; } return ret; @@ -72,6 +154,7 @@ rcl_interfaces::msg::SetParametersResult ControllerParameters::set_parameter_cal rcl_interfaces::msg::SetParametersResult result; result.successful = true; + // TODO(destogl): this is probably executed in another thread --> mutex protection is needed. for (const auto & input_parameter : parameters) { bool found = false; @@ -88,14 +171,23 @@ rcl_interfaces::msg::SetParametersResult ControllerParameters::set_parameter_cal found = find_and_assign_parameter_value(string_parameters_, input_parameter); } if (!found) + { + found = find_and_assign_parameter_value(double_array_parameters_, input_parameter); + } + if (!found) { found = find_and_assign_parameter_value(string_list_parameters_, input_parameter); } RCUTILS_LOG_INFO_EXPRESSION_NAMED( found, logger_name_.c_str(), - "Dynamic parameters got changed! To update the parameters internally please " - "restart the controller."); + "Dynamic parameters got changed! Maybe you have to restart controller to update the " + "parameters internally."); + + if (found) + { + up_to_date_ = false; + } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 795f9cb0ff..b5e7d4e00b 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -8,6 +8,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_core REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(backward_ros REQUIRED) find_package(controller_interface REQUIRED) find_package(controller_manager_msgs REQUIRED) find_package(hardware_interface REQUIRED) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c8796486cf..53deb286f1 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -10,6 +10,7 @@ ament_cmake ament_index_cpp + backward_ros controller_interface controller_manager_msgs hardware_interface diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f4002ee338..65a71d55aa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -30,6 +30,20 @@ namespace controller_manager static constexpr const char * kControllerInterfaceName = "controller_interface"; static constexpr const char * kControllerInterface = "controller_interface::ControllerInterface"; +// Changed services history QOS to keep all so we don't lose any client service calls +static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = +{ + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false +}; + inline bool is_controller_inactive(const controller_interface::ControllerInterface & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -114,83 +128,83 @@ void ControllerManager::init_services() using namespace std::placeholders; list_controllers_service_ = create_service( "~/list_controllers", std::bind(&ControllerManager::list_controllers_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); list_controller_types_service_ = create_service( "~/list_controller_types", std::bind(&ControllerManager::list_controller_types_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); load_controller_service_ = create_service( "~/load_controller", std::bind(&ControllerManager::load_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); configure_controller_service_ = create_service( "~/configure_controller", std::bind(&ControllerManager::configure_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); load_and_configure_controller_service_ = create_service( "~/load_and_configure_controller", std::bind(&ControllerManager::load_and_configure_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); load_and_start_controller_service_ = create_service( "~/load_and_start_controller", std::bind(&ControllerManager::load_and_start_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); configure_and_start_controller_service_ = create_service( "~/configure_and_start_controller", std::bind(&ControllerManager::configure_and_start_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); reload_controller_libraries_service_ = create_service( "~/reload_controller_libraries", std::bind(&ControllerManager::reload_controller_libraries_service_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); switch_controller_service_ = create_service( "~/switch_controller", std::bind(&ControllerManager::switch_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); unload_controller_service_ = create_service( "~/unload_controller", std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); list_hardware_components_service_ = create_service( "~/list_hardware_components", std::bind(&ControllerManager::list_hardware_components_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); list_hardware_interfaces_service_ = create_service( "~/list_hardware_interfaces", std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); configure_hardware_component_service_ = create_service( "~/configure_hardware_component", std::bind(&ControllerManager::configure_hardware_component_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); cleanup_hardware_component_service_ = create_service( "~/cleanup_hardware_component", std::bind(&ControllerManager::cleanup_hardware_component_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); shutdown_hardware_component_service_ = create_service( "~/shutdown_hardware_component", std::bind(&ControllerManager::shutdown_hardware_component_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); manage_hardware_activity_service_ = create_service( "~/manage_hardware_activity", std::bind(&ControllerManager::manage_hardware_activity_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); set_hardware_component_state_service_ = create_service( "~/set_hardware_component_state", std::bind(&ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); + rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); } controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller( diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index fe28e1f070..41dce2e4a2 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(control_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp_lifecycle REQUIRED) diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index 36c857f6f6..89bd017e8b 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -7,6 +7,16 @@ There are three types of hardware Actuator, Sensor and System. For details on each type check `Hardware Components description `_. +Handling of errors that happen during read() and write() calls +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware interface, ``on_error(previous_state)`` method will be called to handle any error that happened. + +Error handling follows the `node lifecycle `_. +If successful ``CallbackReturn::SUCCESS`` is returned and hardware is again in ``UNCONFIGURED`` state, if any ``ERROR`` or ``FAILURE`` happens the hardware ends in ``FINALIZED`` state and can not be recovered. +The only option is to reload the complete plugin, but there is currently no service for this in the Controller Manager. + + Life cycle of Hardware Components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -50,9 +60,6 @@ After successful call to ``on_configure``, Resource Manager makes all state inte NOTE: If using "non-movement" command interfaces to parameterize robot in ``lifecycle_msgs::msg::State::PRIMARY_STATE_CONFIGURED`` state please take care about current state in the ``write`` method of your Hardware Interface implementation. - - - Migration from Foxy to Galactic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index c06e535c72..4af52ccd05 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -9,6 +9,7 @@ ament_cmake + backward_ros control_msgs pluginlib rclcpp_lifecycle diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 774b769995..7f56f4b81a 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -218,24 +218,34 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ return_type Actuator::read() { + return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - return impl_->read(); + result = impl_->read(); + if (result == return_type::ERROR) + { + error(); + } } - return return_type::ERROR; + return result; } return_type Actuator::write() { + return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - return impl_->write(); + result = impl_->write(); + if (result == return_type::ERROR) + { + error(); + } } - return return_type::ERROR; + return result; } } // namespace hardware_interface diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 63e420cecd..5904bf5ef4 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -195,13 +195,18 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st return_type Sensor::read() { + return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - return impl_->read(); + result = impl_->read(); + if (result == return_type::ERROR) + { + error(); + } } - return return_type::ERROR; + return result; } } // namespace hardware_interface diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index ba6558d9cf..7ad01e6c71 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -214,24 +214,34 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st return_type System::read() { + return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - return impl_->read(); + result = impl_->read(); + if (result == return_type::ERROR) + { + error(); + } } - return return_type::ERROR; + return result; } return_type System::write() { + return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - return impl_->write(); + result = impl_->write(); + if (result == return_type::ERROR) + { + error(); + } } - return return_type::ERROR; + return result; } } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index f0c1df1065..1444c54f76 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -34,6 +34,10 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +// Values to send over command interface to trigger error in write and read methods +static constexpr double RECOVERABLE_ERROR = 1.23456e20; +static constexpr double UNRECOVERABLE_ERROR = 9.87654e20; + using namespace ::testing; // NOLINT namespace test_components @@ -48,6 +52,20 @@ class DummyActuator : public hardware_interface::ActuatorInterface return CallbackReturn::SUCCESS; } + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + position_state_ = 0.0; + velocity_state_ = 0.0; + + // reset command only if error is initiated + if (velocity_command_ == RECOVERABLE_ERROR || velocity_command_ == UNRECOVERABLE_ERROR) + { + velocity_command_ = 0.0; + } + + return CallbackReturn::SUCCESS; + } + std::vector export_state_interfaces() override { // We can read a position and a velocity @@ -60,14 +78,6 @@ class DummyActuator : public hardware_interface::ActuatorInterface return state_interfaces; } - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - position_state_ = 0.0; - velocity_state_ = 0.0; - - return CallbackReturn::SUCCESS; - } - std::vector export_command_interfaces() override { // We can command in velocity @@ -82,12 +92,22 @@ class DummyActuator : public hardware_interface::ActuatorInterface hardware_interface::return_type read() override { + if (velocity_command_ == RECOVERABLE_ERROR || velocity_command_ == UNRECOVERABLE_ERROR) + { + return hardware_interface::return_type::ERROR; + } + // no-op, state is getting propagated within write. return hardware_interface::return_type::OK; } hardware_interface::return_type write() override { + if (velocity_command_ == RECOVERABLE_ERROR || velocity_command_ == UNRECOVERABLE_ERROR) + { + return hardware_interface::return_type::ERROR; + } + position_state_ += velocity_command_; velocity_state_ = velocity_command_; @@ -100,6 +120,19 @@ class DummyActuator : public hardware_interface::ActuatorInterface return CallbackReturn::SUCCESS; } + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (velocity_command_ == RECOVERABLE_ERROR) + { + return CallbackReturn::SUCCESS; + } + if (velocity_command_ == UNRECOVERABLE_ERROR) + { + return CallbackReturn::FAILURE; + } + return CallbackReturn::ERROR; + } + private: double position_state_ = std::numeric_limits::quiet_NaN(); double velocity_state_ = std::numeric_limits::quiet_NaN(); @@ -117,6 +150,7 @@ class DummySensor : public hardware_interface::SensorInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { voltage_level_ = 0.0; + read_calls_ = 0; return CallbackReturn::SUCCESS; } @@ -134,14 +168,38 @@ class DummySensor : public hardware_interface::SensorInterface hardware_interface::return_type read() override { + ++read_calls_; + if (read_calls_ == 100) + { + return hardware_interface::return_type::ERROR; + } + // no-op, static value voltage_level_ = voltage_level_hw_value_; return hardware_interface::return_type::OK; } + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + private: double voltage_level_ = std::numeric_limits::quiet_NaN(); double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; }; class DummySystem : public hardware_interface::SystemInterface @@ -159,6 +217,14 @@ class DummySystem : public hardware_interface::SystemInterface position_state_[i] = 0.0; velocity_state_[i] = 0.0; } + // reset command only if error is initiated + if (velocity_command_[0] == RECOVERABLE_ERROR || velocity_command_[0] == UNRECOVERABLE_ERROR) + { + for (auto i = 0ul; i < 3; ++i) + { + velocity_command_[i] = 0.0; + } + } return CallbackReturn::SUCCESS; } @@ -200,12 +266,22 @@ class DummySystem : public hardware_interface::SystemInterface hardware_interface::return_type read() override { + if (velocity_command_[0] == RECOVERABLE_ERROR || velocity_command_[0] == UNRECOVERABLE_ERROR) + { + return hardware_interface::return_type::ERROR; + } + // no-op, state is getting propagated within write. return hardware_interface::return_type::OK; } hardware_interface::return_type write() override { + if (velocity_command_[0] == RECOVERABLE_ERROR || velocity_command_[0] == UNRECOVERABLE_ERROR) + { + return hardware_interface::return_type::ERROR; + } + for (auto i = 0; i < 3; ++i) { position_state_[i] += velocity_command_[0]; @@ -223,6 +299,19 @@ class DummySystem : public hardware_interface::SystemInterface return CallbackReturn::SUCCESS; } + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (velocity_command_[0] == RECOVERABLE_ERROR) + { + return CallbackReturn::SUCCESS; + } + if (velocity_command_[0] == UNRECOVERABLE_ERROR) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + private: std::array position_state_ = { std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), @@ -553,3 +642,269 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) hardware_interface::return_type::ERROR, system_hw.perform_command_mode_switch(two_keys, one_key)); } + +TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) +{ + hardware_interface::Actuator actuator_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; + auto state = actuator_hw.initialize(mock_hw_info); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate recoverable error + command_interfaces[0].set_value(RECOVERABLE_ERROR); + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read()); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read()); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write()); + + // Initiate unrecoverable error + command_interfaces[0].set_value(UNRECOVERABLE_ERROR); + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read()); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) +{ + hardware_interface::Actuator actuator_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; + auto state = actuator_hw.initialize(mock_hw_info); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate recoverable error + command_interfaces[0].set_value(RECOVERABLE_ERROR); + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write()); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read()); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write()); + + // Initiate unrecoverable error + command_interfaces[0].set_value(UNRECOVERABLE_ERROR); + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write()); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; + auto state = sensor_hw.initialize(mock_hw_info); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate recoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < 100; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read()); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read()); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = sensor_hw.configure(); + EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < 100; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read()); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read()); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_system_read_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; + auto state = system_hw.initialize(mock_hw_info); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read()); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); + + // Initiate recoverable error + command_interfaces[0].set_value(RECOVERABLE_ERROR); + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read()); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index].get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index].get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read()); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); + + // Initiate unrecoverable error + command_interfaces[0].set_value(UNRECOVERABLE_ERROR); + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read()); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_system_write_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; + auto state = system_hw.initialize(mock_hw_info); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read()); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); + + // Initiate recoverable error + command_interfaces[0].set_value(RECOVERABLE_ERROR); + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write()); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index].get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index].get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read()); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); + + // Initiate unrecoverable error + command_interfaces[0].set_value(UNRECOVERABLE_ERROR); + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write()); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index b50834b8e0..916262e8a3 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) +find_package(backward_ros REQUIRED) add_library(joint_limiter_interface SHARED src/joint_limiter_interface.cpp) target_include_directories( @@ -39,6 +40,7 @@ target_link_libraries( ) ament_target_dependencies( simple_joint_limiter + pluginlib rclcpp rcutils ) diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 0aaf3ee57e..85425ff8a1 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -84,6 +84,7 @@ class JointLimiterInterface protected: size_t number_of_joints_; + std::vector joint_names_; std::vector joint_limits_; rclcpp::Node::SharedPtr node_; }; diff --git a/joint_limits/package.xml b/joint_limits/package.xml index b66a16e928..e61b4fc658 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -17,6 +17,7 @@ rclcpp pluginlib rcutils + backward_ros ament_cmake_gmock diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp index b0d109b0cd..b3b4d65bb6 100644 --- a/joint_limits/src/joint_limiter_interface.cpp +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -30,6 +30,7 @@ bool JointLimiterInterface::init( const std::string & /*robot_description_topic*/) { number_of_joints_ = joint_names.size(); + joint_names_ = joint_names; joint_limits_.resize(number_of_joints_); node_ = node; diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp index 5106615ea8..8011a9e9bb 100644 --- a/joint_limits/src/simple_joint_limiter.cpp +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -45,119 +45,148 @@ bool SimpleJointLimiter::on_enforce( current_joint_states.velocities.resize(num_joints, 0.0); } - // Clamp velocities to limits + // check for required inputs + if ((desired_joint_states.positions.size() < num_joints) || + (desired_joint_states.velocities.size() < num_joints) || + (current_joint_states.positions.size() < num_joints)) + { + return false; + } + + std::vector desired_accel(num_joints); + std::vector desired_vel(num_joints); + std::vector desired_pos(num_joints); + std::vector pos_limit_trig_jnts(num_joints, false); + std::vector limited_jnts_vel, limited_jnts_acc; + + bool position_limit_triggered = false; + for (auto index = 0u; index < num_joints; ++index) { + desired_pos[index] = desired_joint_states.positions[index]; + + // limit position + if (joint_limits_[index].has_position_limits) + { + auto pos = std::max(std::min(joint_limits_[index].max_position, desired_pos[index]), joint_limits_[index].min_position); + if (pos != desired_pos[index]) + { + pos_limit_trig_jnts[index] = true; + desired_pos[index] = pos; + } + } + + desired_vel[index] = desired_joint_states.velocities[index]; + + // limit velocity if (joint_limits_[index].has_velocity_limits) { - if (std::abs(desired_joint_states.velocities[index]) > joint_limits_[index].max_velocity) + if (std::abs(desired_vel[index]) > joint_limits_[index].max_velocity) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed velocity limits, limiting"); - desired_joint_states.velocities[index] = - copysign(joint_limits_[index].max_velocity, desired_joint_states.velocities[index]); - double accel = - (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / - dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * accel * dt_seconds * dt_seconds; + desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); + limited_jnts_vel.emplace_back(joint_names_[index]); } } - } - // Clamp acclerations to limits - for (auto index = 0u; index < num_joints; ++index) - { + desired_accel[index] = (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + + // limit acceleration if (joint_limits_[index].has_acceleration_limits) { - double accel = - (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / - dt_seconds; - if (std::abs(accel) > joint_limits_[index].max_acceleration) + if (std::abs(desired_accel[index]) > joint_limits_[index].max_acceleration) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed acceleration limits, limiting"); - desired_joint_states.velocities[index] = - current_joint_states.velocities[index] + - copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds * dt_seconds; + desired_accel[index] = std::copysign(joint_limits_[index].max_acceleration, desired_accel[index]); + desired_vel[index] = current_joint_states.velocities[index] + desired_accel[index] * dt_seconds; + // recalc desired position after acceleration limiting + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_accel[index] * dt_seconds * dt_seconds; + limited_jnts_acc.emplace_back(joint_names_[index]); } } - } - // Check that stopping distance is within joint limits - // - In joint mode, slow down only joints whose stopping distance isn't inside joint limits, - // at maximum decel - // - In Cartesian mode, slow down all joints at maximum decel if any don't have stopping distance - // within joint limits - bool position_limit_triggered = false; - for (auto index = 0u; index < num_joints; ++index) - { - if (joint_limits_[index].has_acceleration_limits) + // Check that stopping distance is within joint limits + // Slow down all joints at maximum decel if any don't have stopping distance within joint limits + if (joint_limits_[index].has_position_limits) { // delta_x = (v2*v2 - v1*v1) / (2*a) // stopping_distance = (- v1*v1) / (2*max_acceleration) // Here we assume we will not trigger velocity limits while maximally decelerating. // This is a valid assumption if we are not currently at a velocity limit since we are just // coming to a rest. + double stopping_accel = joint_limits_[index].has_acceleration_limits ? joint_limits_[index].max_acceleration : + std::abs(desired_vel[index] / dt_seconds); double stopping_distance = std::abs( - (-desired_joint_states.velocities[index] * desired_joint_states.velocities[index]) / - (2 * joint_limits_[index].max_acceleration)); + (-desired_vel[index] * desired_vel[index]) / (2 * stopping_accel)); // Check that joint limits are beyond stopping_distance and desired_velocity is towards // that limit - // TODO(anyone): Should we consider sign on acceleration here? if ( - (desired_joint_states.velocities[index] < 0 && - (current_joint_states.positions[index] - joint_limits_[index].min_position < - stopping_distance)) || - (desired_joint_states.velocities[index] > 0 && - (joint_limits_[index].max_position - current_joint_states.positions[index] < - stopping_distance))) + (desired_vel[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < stopping_distance)) || + (desired_vel[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < stopping_distance))) { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed position limits, limiting"); + pos_limit_trig_jnts[index] = true; position_limit_triggered = true; - - // We will limit all joints - break; } } } if (position_limit_triggered) { - // In Cartesian admittance mode, stop all joints if one would exceed limit - for (auto index = 0u; index < num_joints; ++index) + std::ostringstream ostr; + for (auto index = 0u; index < num_joints; ++index) { + // Compute accel to stop + // Here we aren't explicitly maximally decelerating, but for joints near their limits this + // should still result in max decel being used + desired_accel[index] = -current_joint_states.velocities[index] / dt_seconds; if (joint_limits_[index].has_acceleration_limits) { - // Compute accel to stop - // Here we aren't explicitly maximally decelerating, but for joints near their limits this - // should still result in max decel being used - double accel_to_stop = -current_joint_states.velocities[index] / dt_seconds; - double limited_accel = copysign( - std::min(std::abs(accel_to_stop), joint_limits_[index].max_acceleration), accel_to_stop); - - desired_joint_states.velocities[index] = - current_joint_states.velocities[index] + limited_accel * dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * limited_accel * dt_seconds * dt_seconds; + desired_accel[index] = std::copysign( + std::min(std::abs(desired_accel[index]), joint_limits_[index].max_acceleration), desired_accel[index]); } + + // Recompute velocity and position + desired_vel[index] = current_joint_states.velocities[index] + desired_accel[index] * dt_seconds; + desired_pos[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + 0.5 * desired_accel[index] * dt_seconds * dt_seconds; } } + + if (std::count_if(pos_limit_trig_jnts.begin(), pos_limit_trig_jnts.end(), [](bool trig) { return trig; }) > 0) + { + std::ostringstream ostr; + for (auto index = 0u; index < num_joints; ++index) + { + if (pos_limit_trig_jnts[index]) ostr << joint_names_[index] << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed position limits, limiting"); + } + + if (limited_jnts_vel.size() > 0) + { + std::ostringstream ostr; + for (auto jnt: limited_jnts_vel) ostr << jnt << " "; + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting"); + } + + if (limited_jnts_acc.size() > 0) + { + std::ostringstream ostr; + for (auto jnt: limited_jnts_acc) ostr << jnt << " "; + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting"); + } + + desired_joint_states.positions = desired_pos; + desired_joint_states.velocities = desired_vel; return true; } diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt index 302c3562f3..05900eaec5 100644 --- a/joint_limits_enforcement_plugins/CMakeLists.txt +++ b/joint_limits_enforcement_plugins/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(joint_limits REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -29,6 +30,7 @@ target_link_libraries( ament_target_dependencies( ruckig_joint_limiter joint_limits + pluginlib rclcpp rcutils ruckig diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml index 50fecd417d..1e3eacdacf 100644 --- a/joint_limits_enforcement_plugins/package.xml +++ b/joint_limits_enforcement_plugins/package.xml @@ -15,6 +15,7 @@ ament_cmake + backward_ros joint_limits rclcpp pluginlib diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt index 1e0362e4ec..848b785fa1 100644 --- a/joint_limits_interface/CMakeLists.txt +++ b/joint_limits_interface/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(ament_cmake REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(urdf REQUIRED) +find_package(backward_ros REQUIRED) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml index 2170268c12..c31911ea0e 100644 --- a/joint_limits_interface/package.xml +++ b/joint_limits_interface/package.xml @@ -18,6 +18,7 @@ rclcpp urdf + backward_ros hardware_interface diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 15d4475003..b84dfb763f 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(hardware_interface REQUIRED) install( diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 674e0a10ef..e050607ba3 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -11,6 +11,7 @@ ament_cmake hardware_interface + backward_ros ament_cmake_gmock ament_lint_auto