From 48a83db04264f6f19f4b1f2ceed75b82fcb6379e Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Tue, 30 Apr 2024 09:03:21 +0200 Subject: [PATCH] always allow unconfigured param changes --- kuka_drivers_core/README.md | 8 +++---- .../kuka_drivers_core/parameter_handler.hpp | 3 +-- .../src/robot_manager_node.cpp | 13 ++++++------ .../src/robot_manager_node.cpp | 2 +- .../src/robot_manager_node.cpp | 21 ++++++++----------- 5 files changed, 21 insertions(+), 26 deletions(-) diff --git a/kuka_drivers_core/README.md b/kuka_drivers_core/README.md index 9c5b3dc9..f7530b2e 100644 --- a/kuka_drivers_core/README.md +++ b/kuka_drivers_core/README.md @@ -80,7 +80,7 @@ Improvements: - The parameter type is enforced automatically. - The registered callback has access over all of the registered parameters, therefore the parameter server and the node is always in sync - It is easy to register a callback for additional checks before requested parameter value is accepted. -- The user can define the lifecycle states, in which parameter changes are allowed. +- The user can define the lifecycle states, in which parameter changes are allowed using the `ParameterSetAccessRights` structure. - There is a different endpoint for static parameters, which cannot be changed after initialization. The `Parameter` class supports the following parameter types (identical to the types supported by `rclcpp::Parameter`): @@ -98,7 +98,7 @@ The `Parameter` class supports the following parameter types (identical to th The nodes provide the `registerParameter()` and `registerStaticParameter()` endpoints with the following arguments: - `name` [std::string]: name of the parameter - `value` [T]: default value of the parameter - - `rights` [ParameterAccessRights]: structure defining in which states is the setting of the parameter allowed (only for ROS2BaseLCNode) + - `rights` [ParameterSetAccessRights]: structure defining whether modifying the parameter value is allowed in `inactive` and `active` states (only for `ROS2BaseLCNode`, value changes are always allowed in `unconfigured` state) - `on_change_callback` [std::function]: the callback to call when determining the validity of a parameter change request Both methods register a parameter with the given `name` and `type`, and the `on_change_callback` is called if a parameter change is requested to check validity. In case of the `registerStaticParameter()`, the callback always returns false after initializing the value. @@ -107,8 +107,8 @@ Example code for registering an integer parameter for both base nodes (`onRateCh ```C++ // For class derived from ROS2BaseLCNode registerParameter( - "rate", 2, kuka_drivers_core::ParameterSetAccessRights {true, true, - false}, [this](int rate) { + "rate", 2, kuka_drivers_core::ParameterSetAccessRights {true, false}, + [this](int rate) { return this->onRateChangeRequest(rate); }); diff --git a/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp b/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp index 4a976ae3..7be3743c 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp @@ -28,7 +28,6 @@ namespace kuka_drivers_core { struct ParameterSetAccessRights { - bool unconfigured; bool inactive; bool active; bool isSetAllowed(std::uint8_t current_state) const @@ -36,7 +35,7 @@ struct ParameterSetAccessRights switch (current_state) { case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED: - return unconfigured; + return true; case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE: return inactive; case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 51ec534a..2d12fd7f 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -59,7 +59,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ // Register parameters this->registerParameter( "position_controller_name", kuka_drivers_core::JOINT_TRAJECTORY_CONTROLLER, - kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) { return this->controller_handler_.UpdateControllerName( @@ -67,7 +67,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); this->registerParameter( "impedance_controller_name", "joint_group_impedance_controllers", - kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) { return this->controller_handler_.UpdateControllerName( @@ -75,7 +75,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); this->registerParameter( "torque_controller_name", "effort_controller", - kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) { return this->controller_handler_.UpdateControllerName( @@ -83,14 +83,13 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); this->registerParameter( "control_mode", static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL), - kuka_drivers_core::ParameterSetAccessRights{true, true, true}, + kuka_drivers_core::ParameterSetAccessRights{true, true}, [this](int control_mode) { return this->onControlModeChangeRequest(control_mode); }); this->registerStaticParameter( - "controller_ip", "", kuka_drivers_core::ParameterSetAccessRights{true, false, false}, + "controller_ip", "", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string &) { return true; }); this->registerStaticParameter( - "robot_model", "lbr_iisy3_r760", - kuka_drivers_core::ParameterSetAccessRights{true, false, false}, + "robot_model", "lbr_iisy3_r760", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string & robot_model) { return this->onRobotModelChangeRequest(robot_model); }); } diff --git a/kuka_kss_rsi_driver/src/robot_manager_node.cpp b/kuka_kss_rsi_driver/src/robot_manager_node.cpp index 0d7bcf45..4f61a4c1 100644 --- a/kuka_kss_rsi_driver/src/robot_manager_node.cpp +++ b/kuka_kss_rsi_driver/src/robot_manager_node.cpp @@ -40,7 +40,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ this->create_publisher("robot_manager/is_configured", is_configured_qos); this->registerStaticParameter( - "robot_model", "kr6_r700_sixx", kuka_drivers_core::ParameterSetAccessRights{true, false, false}, + "robot_model", "kr6_r700_sixx", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string & robot_model) { return this->onRobotModelChangeRequest(robot_model); }); } diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index bb7bf1ef..adc0bdaa 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -68,31 +68,30 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ sub_options); registerStaticParameter( - "robot_model", "lbr_iiwa14_r820", - kuka_drivers_core::ParameterSetAccessRights{true, false, false}, + "robot_model", "lbr_iiwa14_r820", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string & robot_model) { return this->onRobotModelChangeRequest(robot_model); }); registerStaticParameter( - "controller_ip", "", kuka_drivers_core::ParameterSetAccessRights{true, false, false}, + "controller_ip", "", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string & controller_ip) { return this->ValidateIPAdress(controller_ip); }); registerParameter( - "send_period_ms", 10, kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + "send_period_ms", 10, kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const int & send_period) { return this->onSendPeriodChangeRequest(send_period); }); registerParameter( - "receive_multiplier", 1, kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + "receive_multiplier", 1, kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const int & receive_multiplier) { return this->onReceiveMultiplierChangeRequest(receive_multiplier); }); registerParameter( "control_mode", static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL), - kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](int control_mode) { return this->onControlModeChangeRequest(control_mode); }); registerParameter( - "position_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + "position_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) { return this->onControllerNameChangeRequest( @@ -100,7 +99,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); registerParameter( - "torque_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + "torque_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) { return this->onControllerNameChangeRequest( @@ -108,13 +107,12 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); registerParameter>( - "joint_stiffness", joint_stiffness_, - kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + "joint_stiffness", joint_stiffness_, kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::vector & joint_stiffness) { return this->onJointStiffnessChangeRequest(joint_stiffness); }); registerParameter>( - "joint_damping", joint_damping_, kuka_drivers_core::ParameterSetAccessRights{true, true, false}, + "joint_damping", joint_damping_, kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::vector & joint_damping) { return this->onJointDampingChangeRequest(joint_damping); }); } @@ -376,7 +374,6 @@ bool RobotManagerNode::ValidateIPAdress(std::string_view controller_ip) const return true; } - bool RobotManagerNode::onControllerNameChangeRequest( std::string_view controller_name, kuka_drivers_core::ControllerType controller_type) {