diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e54bf95..b0e600c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,29 +42,13 @@ target_include_directories(control_toolbox PUBLIC ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") -######################## -# Parameter handler -######################## -add_library(parameter_handler SHARED - src/parameter_handler.cpp -) - -target_include_directories(parameter_handler PUBLIC - $ - $ -) -ament_target_dependencies(parameter_handler - rcl_interfaces - rclcpp - rcutils -) - ######################## # Control filters ######################## find_package(filters REQUIRED) find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -73,6 +57,7 @@ set(CONTROL_FILTERS_INCLUDE_DEPENDS filters geometry_msgs pluginlib + generate_parameter_library rclcpp tf2_geometry_msgs tf2 @@ -86,7 +71,11 @@ target_include_directories(gravity_compensation PUBLIC $ $ ) -target_link_libraries(gravity_compensation parameter_handler) +generate_parameter_library( + gravity_compensation_filter_parameters + src/control_filters/gravity_compensation_filter_parameters.yaml +) +target_link_libraries(gravity_compensation gravity_compensation_filter_parameters) ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) ########## @@ -109,7 +98,7 @@ if(BUILD_TESTING) ## Control Filters ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp) - target_link_libraries(test_gravity_compensation gravity_compensation) + target_link_libraries(test_gravity_compensation gravity_compensation gravity_compensation_filter_parameters) ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() @@ -119,13 +108,13 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox parameter_handler +install(TARGETS control_toolbox EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) -install(TARGETS gravity_compensation +install(TARGETS gravity_compensation gravity_compensation_filter_parameters ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -142,7 +131,7 @@ ament_export_include_directories( ament_export_libraries( control_toolbox gravity_compensation - parameter_handler + gravity_compensation_filter_parameters ) ament_package() diff --git a/control_filters.xml b/control_filters.xml index 0fcb8922..1c60dbc5 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,8 +1,8 @@ + type="control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>" + base_class_type="filters::FilterBase<geometry_msgs::msg::WrenchStamped>"> This is a gravity compensation filter working with geometry_msgs::WrenchStamped. It subtracts the influence of a force caused by the gravitational force from force diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 0292599e..9d749376 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -19,7 +19,7 @@ #include #include -#include "control_toolbox/parameter_handler.hpp" +#include "gravity_compensation_filter_parameters.hpp" #include "filters/filter_base.hpp" #include "geometry_msgs/msg/vector3_stamped.hpp" #include "tf2_ros/buffer.h" @@ -27,72 +27,8 @@ namespace control_filters { -class GravityCompensationParameters : public control_toolbox::ParameterHandler -{ -public: - explicit GravityCompensationParameters(const std::string & params_prefix) - : control_toolbox::ParameterHandler(params_prefix, 0, 0, 4, 3) - { - add_string_parameter("world_frame", false); - add_string_parameter("sensor_frame", false); - add_string_parameter("force_frame", false); - - add_double_parameter("CoG.x", true); - add_double_parameter("CoG.y", true); - add_double_parameter("CoG.z", true); - add_double_parameter("force", true); - } - bool check_if_parameters_are_valid() override - { - bool ret = true; - - // Check if any string parameter is empty - ret = !empty_parameter_in_list(string_parameters_); - - for (size_t i = 0; i < 4; ++i) - { - if (std::isnan(double_parameters_[i].second)) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "Parameter '%s' has to be set", - double_parameters_[i].first.name.c_str()); - ret = false; - } - } - - return ret; - } - - void update_storage() override - { - world_frame_ = string_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "World frame: %s", world_frame_.c_str()); - sensor_frame_ = string_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Sensor frame: %s", sensor_frame_.c_str()); - force_frame_ = string_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Force frame: %s", force_frame_.c_str()); - - cog_.vector.x = double_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG X is %e", cog_.vector.x); - cog_.vector.y = double_parameters_[1].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG Y is %e", cog_.vector.y); - cog_.vector.z = double_parameters_[2].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG Z is %e", cog_.vector.z); - - force_z_ = double_parameters_[3].second; - RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Force is %e", force_z_); - } - // Frames for Transformation of Gravity / CoG Vector - std::string world_frame_; - std::string sensor_frame_; - std::string force_frame_; - - // Storage for Calibration Values - geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) - double force_z_; // Gravitational Force -}; template class GravityCompensation : public filters::FilterBase @@ -113,13 +49,30 @@ class GravityCompensation : public filters::FilterBase */ bool update(const T & data_in, T & data_out) override; +protected: + void compute_internal_params() + { + cog_.vector.x = parameters_.CoG.pos[0]; + cog_.vector.y = parameters_.CoG.pos[1]; + cog_.vector.z = parameters_.CoG.pos[2]; + // for now always consider the force is along z + force_z_ = parameters_.CoG.force[2]; + }; + private: rclcpp::Clock::SharedPtr clock_; std::shared_ptr logger_; - std::unique_ptr parameters_; + std::shared_ptr parameter_handler_; + gravity_compensation_filter::Params parameters_; - // Callback for updating dynamic parameters - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; + // Frames for Transformation of Gravity / CoG Vector + std::string world_frame_; + std::string sensor_frame_; + std::string force_frame_; + + // Storage for Calibration Values + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) + double force_z_; // Gravitational Force // Filter objects std::unique_ptr p_tf_Buffer_; @@ -146,22 +99,25 @@ bool GravityCompensation::configure() logger_.reset( new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); - parameters_.reset(new GravityCompensationParameters(this->param_prefix_)); - - parameters_->initialize(this->params_interface_, logger_->get_name()); - - parameters_->declare_parameters(); - if (!parameters_->get_parameters()) + // Initialize the parameters + try { + parameter_handler_ = + std::make_shared(this->params_interface_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); return false; } - - // Add callback to dynamically update parameters - on_set_callback_handle_ = this->params_interface_->add_on_set_parameters_callback( - [this](const std::vector & parameters) { - return parameters_->set_parameter_callback(parameters); - }); + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); return true; } diff --git a/include/control_toolbox/parameter_handler.hpp b/include/control_toolbox/parameter_handler.hpp deleted file mode 100644 index f38fd093..00000000 --- a/include/control_toolbox/parameter_handler.hpp +++ /dev/null @@ -1,387 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/// \author: Denis Stogl - -#ifndef CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ -#define CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/node.hpp" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" -#include "rclcpp/parameter_value.hpp" -#include "rcutils/logging_macros.h" - -namespace control_toolbox -{ -using rclcpp::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, const uint8_t type, const bool configurable = false) - : name(name), type(type), configurable(configurable) - { - } - - std::string name; - uint8_t type; - bool configurable; -}; - -class ParameterHandler -{ -public: - ParameterHandler( - const std::string & params_prefix = "", int nr_bool_params = 0, int nr_integer_params = 0, - int nr_double_params = 0, int nr_string_params = 0, int nr_byte_array_params = 0, - int nr_bool_array_params = 0, int nr_integer_array_params = 0, int nr_double_array_params = 0, - int nr_string_array_params = 0); - - virtual ~ParameterHandler() = default; - - void initialize(rclcpp::Node::SharedPtr node) - { - initialize(node->get_node_parameters_interface(), node->get_logger().get_name()); - } - - void initialize( - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, - const std::string & logger_name = "::ControllerParameters") - { - params_interface_ = node_params; - logger_name_ = logger_name + "." + "parameters"; - } - - void declare_parameters(); - - /** - * 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: - /** - * Override this method to implement custom parameters check. - * Default implementation does not checks anything, always returns true. - * - * \return **true** if parameters are valid, **false** otherwise. - */ - virtual bool check_if_parameters_are_valid() { return true; } - - /** - * 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, const bool & configurable = false, const bool & default_value = false) - { - bool_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), - default_value}); - } - - void add_integer_parameter( - const std::string & name, const bool & configurable = false, - const int & default_value = std::numeric_limits::quiet_NaN()) - { - integer_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER, configurable), - default_value}); - } - - void add_double_parameter( - const std::string & name, const bool & configurable = false, - const double & default_value = std::numeric_limits::quiet_NaN()) - { - double_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), - default_value}); - } - - void add_string_parameter( - const std::string & name, const bool & configurable = false, - const std::string & default_value = "") - { - string_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), - default_value}); - } - - void add_byte_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - byte_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BYTE_ARRAY, configurable), - default_value}); - } - - void add_bool_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - bool_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL_ARRAY, configurable), - default_value}); - } - - void add_integer_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - integer_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER_ARRAY, configurable), - default_value}); - } - void add_double_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - double_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), - default_value}); - } - - void add_string_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - string_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), - default_value}); - } - - template - bool empty_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (parameter.second.empty()) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - return empty_numeric_parameter_in_list(parameters); - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - return empty_numeric_parameter_in_list(parameters); - } - - template - bool find_and_assign_parameter_value( - std::vector> & parameter_list, - const rclcpp::Parameter & input_parameter) - { - auto is_in_list = [&](const auto & parameter) { - return parameter.first.name == input_parameter.get_name(); - }; - - auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); - - if (it != parameter_list.end()) - { - if (!it->first.configurable) - { - throw std::runtime_error( - "Parameter " + input_parameter.get_name() + " is not configurable."); - } - else - { - it->second = input_parameter.get_value(); - RCUTILS_LOG_ERROR_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; - } - } - else - { - return false; - } - } - - // Storage members - std::vector> bool_parameters_; - std::vector> integer_parameters_; - std::vector> double_parameters_; - std::vector> string_parameters_; - std::vector>> byte_array_parameters_; - std::vector>> bool_array_parameters_; - std::vector>> integer_array_parameters_; - std::vector>> double_array_parameters_; - std::vector>> string_array_parameters_; - - // Functional members - bool declared_; - bool up_to_date_; - std::string params_prefix_; - - // Input/Output members to ROS 2 - std::string logger_name_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; - -private: - template - void declare_parameters_from_list( - 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) - { - // 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 empty_numeric_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (std::isnan(parameter.second)) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - 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; - } -}; - -} // namespace control_toolbox - -#endif // CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ diff --git a/package.xml b/package.xml index f6a98f64..8446cf48 100644 --- a/package.xml +++ b/package.xml @@ -22,7 +22,7 @@ filters geometry_msgs pluginlib - rcl_interfaces + generate_parameter_library rclcpp rcutils realtime_tools diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index ca5f41f9..f005db56 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -30,16 +30,21 @@ bool GravityCompensation::update( return false; } - parameters_->update(); + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); + } try { transform_ = p_tf_Buffer_->lookupTransform( - parameters_->world_frame_, data_in.header.frame_id, rclcpp::Time()); + parameters_.world_frame, data_in.header.frame_id, rclcpp::Time()); transform_back_ = p_tf_Buffer_->lookupTransform( - data_in.header.frame_id, parameters_->world_frame_, rclcpp::Time()); + data_in.header.frame_id, parameters_.world_frame, rclcpp::Time()); transform_cog_ = p_tf_Buffer_->lookupTransform( - parameters_->world_frame_, parameters_->force_frame_, rclcpp::Time()); + parameters_.world_frame, parameters_.force_frame, rclcpp::Time()); } catch (const tf2::TransformException & ex) { @@ -58,13 +63,14 @@ bool GravityCompensation::update( // Transform CoG Vector geometry_msgs::msg::Vector3Stamped cog_transformed; - tf2::doTransform(parameters_->cog_, cog_transformed, transform_cog_); + tf2::doTransform(cog_, cog_transformed, transform_cog_); + // TODO(guihomework): use the full force vector and not only its z component // Compensate for gravity force - temp_force_transformed.vector.z += parameters_->force_z_; + temp_force_transformed.vector.z -= force_z_; // Compensation Values for torque result from cross-product of cog Vector and (0 0 G) - temp_torque_transformed.vector.x += (parameters_->force_z_ * cog_transformed.vector.y); - temp_torque_transformed.vector.y -= (parameters_->force_z_ * cog_transformed.vector.x); + temp_torque_transformed.vector.x -= (force_z_ * cog_transformed.vector.y); + temp_torque_transformed.vector.y += (force_z_ * cog_transformed.vector.x); // Copy Message and Compensate values for Gravity Force and Resulting Torque data_out = data_in; diff --git a/src/control_filters/gravity_compensation_filter_parameters.yaml b/src/control_filters/gravity_compensation_filter_parameters.yaml new file mode 100644 index 00000000..69d9cb68 --- /dev/null +++ b/src/control_filters/gravity_compensation_filter_parameters.yaml @@ -0,0 +1,43 @@ +gravity_compensation_filter: + world_frame: { + type: string, + # default_value: world, + description: "World frame", + read_only: true, + validation: { + not_empty<>: [] + }, + } + sensor_frame: { + type: string, + # default_value: ft_sensor, + description: "Sensor frame", + read_only: true, + validation: { + not_empty<>: [] + }, + } + force_frame: { + type: string, + # default_value: world, + description: "Force frame", + read_only: true, + validation: { + not_empty<>: [] + }, + } + CoG: + pos: { + type: double_array, + description: "Specifies the position of the center of gravity (CoG) of the end effector in the sensor frame.", + validation: { + fixed_size<>: 3 + }, + } + force: { + type: double_array, + description: "Specifies the constant force applied at the CoG of the end effector defined in world frame, e.g [0, 0, -mass * 9.81].", + validation: { + fixed_size<>: 3 + }, + } diff --git a/src/parameter_handler.cpp b/src/parameter_handler.cpp deleted file mode 100644 index 52ca742e..00000000 --- a/src/parameter_handler.cpp +++ /dev/null @@ -1,216 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/// \author: Denis Stogl - -#include "control_toolbox/parameter_handler.hpp" - -#include -#include - -#include "rcutils/logging_macros.h" - -namespace control_toolbox -{ -ParameterHandler::ParameterHandler( - const std::string & params_prefix, int nr_bool_params, int nr_integer_params, - int nr_double_params, int nr_string_params, int nr_byte_array_params, int nr_bool_array_params, - int nr_integer_array_params, int nr_double_array_params, int nr_string_array_params) -: declared_(false), up_to_date_(false), params_prefix_("") -{ - params_prefix_ = impl::normalize_params_prefix(params_prefix); - - bool_parameters_.reserve(nr_bool_params); - integer_parameters_.reserve(nr_integer_params); - double_parameters_.reserve(nr_double_params); - string_parameters_.reserve(nr_string_params); - byte_array_parameters_.reserve(nr_byte_array_params); - bool_array_parameters_.reserve(nr_bool_array_params); - integer_array_parameters_.reserve(nr_integer_array_params); - double_array_parameters_.reserve(nr_double_array_params); - string_array_parameters_.reserve(nr_string_array_params); -} - -void ParameterHandler::declare_parameters() -{ - if (!declared_) - { - declare_parameters_from_list(bool_parameters_); - declare_parameters_from_list(integer_parameters_); - declare_parameters_from_list(double_parameters_); - declare_parameters_from_list(string_parameters_); - declare_parameters_from_list(byte_array_parameters_); - declare_parameters_from_list(bool_array_parameters_); - declare_parameters_from_list(integer_array_parameters_); - declare_parameters_from_list(double_array_parameters_); - declare_parameters_from_list(string_array_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 ParameterHandler::get_parameters(bool check_validity, bool update) -{ - // TODO(destogl): Should we "auto-declare" parameters here? - if (!declared_) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "Can not get parameters. Please declare them first."); - return false; - } - - bool ret = false; - - // 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(integer_parameters_) && - get_parameters_from_list(double_parameters_) && get_parameters_from_list(string_parameters_) && - get_parameters_from_list(byte_array_parameters_) && - get_parameters_from_list(bool_array_parameters_) && - get_parameters_from_list(integer_array_parameters_) && - get_parameters_from_list(double_array_parameters_) && - get_parameters_from_list(string_array_parameters_); - - if (ret && check_validity) - { - 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 ParameterHandler::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; -} - -rcl_interfaces::msg::SetParametersResult ParameterHandler::set_parameter_callback( - const std::vector & parameters) -{ - 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; - - try - { - found = find_and_assign_parameter_value(bool_parameters_, input_parameter); - if (!found) - { - found = find_and_assign_parameter_value(integer_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(double_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(byte_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(bool_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(integer_array_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_array_parameters_, input_parameter); - } - - RCUTILS_LOG_INFO_EXPRESSION_NAMED( - found, logger_name_.c_str(), - "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) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - catch (const std::runtime_error & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - } - - return result; -} - -} // namespace control_toolbox diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp index e90b53dd..538890fe 100644 --- a/test/control_filters/test_gravity_compensation.cpp +++ b/test/control_filters/test_gravity_compensation.cpp @@ -13,22 +13,62 @@ // limitations under the License. #include "test_gravity_compensation.hpp" +#include + +TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + node_->declare_parameter("world_frame", "world"); + node_->declare_parameter("sensor_frame", "sensor"); + + // one mandatory param missing, should fail + ASSERT_FALSE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + /* NOTE: one cannot declare or set the missing param afterwards, to then test if configure works, + * because the param is read only and cannot be set anymore. + */ +} + +TEST_F(GravityCompensationTest, TestGravityCompensationInvalidParameters) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + double gravity_acc = 9.81; + double mass = 5.0; + node_->declare_parameter("world_frame", "world"); + node_->declare_parameter("sensor_frame", "sensor"); + node_->declare_parameter("force_frame", "world"); + node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); + + node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0})); + // wrong vector size, should fail + ASSERT_FALSE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.0}))); + // all parameters correctly set + ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} TEST_F(GravityCompensationTest, TestGravityCompensation) { - std::shared_ptr> filter_ = + std::shared_ptr> filter_ = std::make_shared>(); + double gravity_acc = 9.81; + double mass = 5.0; node_->declare_parameter("world_frame", "world"); node_->declare_parameter("sensor_frame", "sensor"); node_->declare_parameter("force_frame", "world"); - node_->declare_parameter("CoG.x", 0.0); - node_->declare_parameter("CoG.y", 0.0); - node_->declare_parameter("CoG.z", 0.0); - node_->declare_parameter("force", 50.0); + node_->declare_parameter("CoG.pos", std::vector({0.0, 0.0, 0.0})); + node_->declare_parameter("CoG.force", std::vector({0.0, 0.0, -gravity_acc * mass})); ASSERT_TRUE(filter_->configure("", "TestGravityCompensationFilter", - node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); geometry_msgs::msg::WrenchStamped in, out; in.header.frame_id = "world"; @@ -39,7 +79,7 @@ TEST_F(GravityCompensationTest, TestGravityCompensation) ASSERT_EQ(out.wrench.force.x, 1.0); ASSERT_EQ(out.wrench.force.y, 0.0); - ASSERT_EQ(out.wrench.force.z, 50.0); + ASSERT_EQ(out.wrench.force.z, gravity_acc * mass); ASSERT_EQ(out.wrench.torque.x, 10.0); ASSERT_EQ(out.wrench.torque.y, 0.0); diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp index 7e048e14..daa766df 100644 --- a/test/control_filters/test_gravity_compensation.hpp +++ b/test/control_filters/test_gravity_compensation.hpp @@ -40,7 +40,7 @@ class GravityCompensationTest : public ::testing::Test executor_thread_ = std::thread([this]() { executor_->spin(); }); } - GravityCompensationTest() + GravityCompensationTest() { rclcpp::init(0, nullptr); node_ = std::make_shared("test_gravity_compensation");