diff --git a/include/rosparam_shortcuts/parameter.h b/include/rosparam_shortcuts/parameter.h new file mode 100644 index 0000000..19788dc --- /dev/null +++ b/include/rosparam_shortcuts/parameter.h @@ -0,0 +1,257 @@ +#pragma once + +#include +#include + +#include + +namespace rosparam_shortcuts { + +/* + * Thread-safe container for generic parameter values, providing callback functions. + * This class implements a tree structure of parameters which are declared recursively. + * All leafs are of type Parameter which are used for interacting with + * the rclcpp node parameter interface using the NodeConfig class further down. + */ +template +class Parameter +{ +public: + Parameter(const std::string& name); + Parameter(const std::string& name, const ParameterT& default_value); + + // Fields + const std::string& name() const noexcept; + std::string description() const noexcept; + std::string additional_constraints() const noexcept; + bool isNodeParameter() const noexcept; // true if this parameter contains an rclcpp::Parameter + + // Properties + Parameter& describe(const std::string& description, const std::string& additional_constraints = ""); + // Allow setting additional properties. i.e. read_only, dynamic_typing? + // void setProperties(...); + + // Validity Status + const bool isInitialized() const noexcept; // true when created with default + const bool isValid() const noexcept; // errors empty? + const bool hasErrors() const noexcept; + const bool hasWarnings() const noexcept; + // TODO: Insert additional thread-safe access to errors/warnings + + // Read/write-locked data access using std::shared_mutex + // See: https://www.youtube.com/watch?v=ozOgzlxIsdg + ReadLockedPtr operator->const; + WriteLockedPtr operator->; + // optional: (useful for synchronized vector/map operations) + void applyRW(std::function); + void applyRO(std::function); + + // Copy-set value using read/write lock internally + ParameterT& operator=(const ParameterT& value); + + // Copy-get value, throws exception if isInitialized is false, uses read/write lock interally + ParameterT operator() const; // or: 'operator ParameterT() const;' + + // Validity Callbacks (could also use rcl_interfaces::msg::SetParametersResult like in node_parameters.h). + // Callbacks are stored in ordered hash map, could also possibly return hash for deleting later on. + // Alternatively, return Parameter& for method chaining + using ParameterValidityCallback = std::function; + Parameter& warnIf(ParameterValidityCallback warn_if_callback); + Parameter& errorIf(ParameterValidityCallback error_if_callback); // or rejectIf()/failIf().. + // Predefined generic (type-specific?) conditions, i.e. 'NO_OVERRIDE', 'EMPTY', 'NOT_NORMALIZED' + enum ParameterCondition + { + ... + }; + Parameter& warnIf(ParameterCondition condition); + Parameter& errorIf(ParameterCondition condition); + + // Callback for reacting to simple parameter changes (read-locked) + // NOTE: this should be called after validity callbacks so that the parameter status can be evaluated + using ParameterChangedCallback = std::function; + void onChanged(ParameterChangedCallback callback); + +protected: + // Recursively declare parameters and populate child_params_, the actual node parameters (leafs) are + // collected and returned so that they can be maintained by NodeConfig. Should call declareChildParam() + // internally. NodeParameter is Parameter and needs to be forward declared.. + virtual std::map> declare(); + + // Constructor used for child parameters in the tree that share the same data, so basically all parameters + // that are neither root nor leaf. + // NOTE: not sure if this is really a good solution or if we should simply copy the data and synchronize + // using child/parent callbacks only + Parameter(const std::string& name, ParameterT& value, std::shared_mutex parent_mutex); + +private: + // Initialize child parameter and call declare() function recursively + template + void declareChildParam(const std::string, std::map>& node_parameters); + + // Internal value protected by mutex_. Alternatively use a struct ParameterValue that manages + // locked acces and use that for internal data (this would allow sharing references between child + // parameters) + ParameterT value_; + mutable std::shared_mutex mutex_; + + std::map> child_params_; + + const std::string name_; + std::string description_; + std::string additional_constraints_; + + // Store callbacks, use alternative data structurs for ordered hashes + std::map warn_if_callbacks_; + std::map error_if_callbacks_; + std::map on_changed_callbacks_; + + // Store messages of failed validity callbacks + std::vector errors_; + std::vector warnings_; +} + +// namespace Parameter +using NodeParameter = Parameter; +std::map> +NodeParameter::declare() override{ /* returns shared_from_this */ }; + +/* + * Maintains all Parameter instances and does the heavy lifting with actually declaring and interfacing with dynamic + * rclcpp Parameters. Provides additional convenience functions for validating all parameters, printing, etc... + */ +class NodeConfig +{ +public: + NodeConfig(const rclcpp::Node::SharedPtr& node, const std::string base_name = ""); + +protected: + /* + * Declare Parameter types, internally calls Parameter::declare() to get + * all actual node parameters which are then declared using the node parameter interface. + * + * This eventually calls the ros interfaces for declaring and getting the parameter. + */ + template + Parameter& declare(Parameter& parameter); + + /* + * Undeclare a parameter, remove all callbacks that have been registered using the declare() call + */ + template + bool undeclare(const Parameter& parameter); + bool undeclare(const std::string& name); + + /* + * Return the parameter for a given name, if not found parameter is nullptr + */ + // Not possible when working with references, maybe only for actual node parameters? + // std::shared_ptr> get(const std::string& name); + // std::shared_ptr> get(const std::string& name) const noexcept; + + // + // TODO: insert utility functions: validate(), print, ostream<<, errors(), warnings()... + // + +private: + const rclcpp::Node::SharedPtr node_; + const std::string base_name_; + + // Class similar to the one in rosparam_shortcuts/node_parameters.h + // implements ROS calls/checks/callbacks for node_parameters_ + NodeParametersInterface node_parameters_interface_; + + // Store parameters, only node parameters (rclcpp::Parameters) are stored inside NodeConfig + std::map parameters_; + std::map> node_parameters_; + + // Needed? keep track of registered callbacks that are required for linking SetParameters requests or + // exposing high-level update changes (invalid/valid states) + // std::string registered_node_parameter_callbacks_ids_; + // std::string registered_parameter_callbacks_ids_; +} + +struct ExampleConfig : public NodeConfig +{ + ExampleConfig(const rclcpp::Node::SharedPtr& node) : NodeConfig(node) { + // Build and declare the parameters + + // declare simple parameters just like that + declare(int1_param.describe("This is the first int parameter")); + declare(int2_param.describe("This is the second int parameter")); + declare(int3_param.describe("This is the third int parameter")); + + // "double_param", add some description and validity checks + declare(double_param + .describe("This is a very important double parameter", "should be greater than 0 and not 7") + .warnIf([](const double& val, std::string& message) { + message = "Value is 7, are you sure this is a good value?"; + return val == 7; + }) + .errorIf([](const double& val, std::string& message) { + message = "Value must not be negative!"; + return val < 0; + }) + ); + + // "string_param", use a parameter error condition "EMPTY" + declare(string_param + .describe("meh") + .warnIf([](const std::string& val, std::string& message) { + message = "string is much too long"; + return val.size() > 100; + }) + .errorIf(EMPTY) + ); + + // "double_map_param", validate keys and values, reject if not valid + declare(double_map_param + .describe("6-dof joint state") + .errorIf([](const auto& values, std::string& message) { + message = "Invalid joint state map values, expected 6-dof"; + return values.size() != 6 || checkJointNamesInRobotModel(values); // from somewhere + }) + ); + + // + // can already check for errors/warnings here... + // + } + + Parameter double_param{ "double_param", 0 }; + Parameter> string_param{ "string_param", {"first", "second"} }; + Parameter> double_map_param{ "double_map_param"}; + Parameter int1_param{ "int1_param", 0 }; + Parameter int2_param{ "int2_param", 1 }; + Parameter int3_param{ "int3_param", 2 }; +} + +void useConfig(const rclcpp::Node::SharedPtr& node) +{ + // use non-const config for initializing callbacks + // For safety, it should only be accessed as const in nodes + const ExampleConfig config(node); + + // access copy of value + double value = config.double_param(); + + // ... + + // React to parameter changes (sub is a bad example, but planning pipeline, plugin etc would work well) + auto sub = rclcpp::create_subscription(config.string_param, ...); // sub managed somewhere else + config.string_param->onChanged( + [&](const std::string& value) { sub = rclcpp::create_subscription(config.value, ...); }); + + // ... + + auto robot_state = /* get robot state copy from somewhere else */; + auto update_robot_state = [this, robot_state](const std::map values) { + for (const auto& [name, val] : values) + robot_state->setVariablePosition(name, val); + + // other thread-safe member function somewhere + this->updateRobotState(robot_state); + }; + config.double_map_param.apply(update_robot_state); + config.double_map_param.onChanged(update_robot_state); +} +} // namespace rosparam_shortcuts