From fcbc0a6700dee64981cc47c414d79ddfd84018bf Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Tue, 16 Apr 2024 08:51:28 +0200 Subject: [PATCH] merge controllers branch --- .../fri_configuration_controller.hpp | 7 +++-- .../src/fri_configuration_controller.cpp | 29 ++++++++++--------- doc/wiki/4_Controllers.md | 2 +- kuka_sunrise_fri_driver/CMakeLists.txt | 4 ++- .../src/robot_manager_node.cpp | 3 +- 5 files changed, 24 insertions(+), 21 deletions(-) diff --git a/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp b/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp index c33058fe..6780abb4 100644 --- a/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp +++ b/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp @@ -20,7 +20,7 @@ #include #include "controller_interface/controller_interface.hpp" -#include "kuka_driver_interfaces/srv/set_int.hpp" +#include "kuka_driver_interfaces/srv/set_fri_configuration.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" @@ -53,9 +53,10 @@ class FRIConfigurationController : public controller_interface::ControllerInterf FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; private: - rclcpp::Service::SharedPtr receive_multiplier_service_; + rclcpp::Service::SharedPtr receive_multiplier_service_; int receive_multiplier_ = 1; - bool resend_multiplier_ = false; + int send_period_ms_= 10; + bool update_config_ = false; }; } // namespace kuka_controllers #endif // FRI_CONFIGURATION_CONTROLLER__FRI_CONFIGURATION_CONTROLLER_HPP_ diff --git a/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp b/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp index 91144568..315c7336 100644 --- a/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp +++ b/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp @@ -21,18 +21,16 @@ namespace kuka_controllers controller_interface::CallbackReturn FRIConfigurationController::on_init() { auto callback = [this]( - kuka_driver_interfaces::srv::SetInt::Request::SharedPtr request, - kuka_driver_interfaces::srv::SetInt::Response::SharedPtr response) + kuka_driver_interfaces::srv::SetFriConfiguration::Request::SharedPtr request, + kuka_driver_interfaces::srv::SetFriConfiguration::Response::SharedPtr response) { - resend_multiplier_ = true; - receive_multiplier_ = request->data; + update_config_ = true; + receive_multiplier_ = request->receive_multiplier; + send_period_ms_= request->send_period_ms; response->success = true; }; - receive_multiplier_service_ = get_node()->create_service( - "~/set_receive_multiplier", callback); - // TODO(Svastits): create service to get multiplier changes (or perpaps - // parameter??) - // and set resend_multiplier_ to true in the callback + receive_multiplier_service_ = get_node()->create_service( + "~/set_fri_config", callback); return controller_interface::CallbackReturn::SUCCESS; } @@ -43,6 +41,8 @@ FRIConfigurationController::command_interface_configuration() const config.type = controller_interface::interface_configuration_type::INDIVIDUAL; config.names.emplace_back( std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::RECEIVE_MULTIPLIER); + config.names.emplace_back( + std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::SEND_PERIOD); return config; } @@ -75,13 +75,14 @@ controller_interface::return_type FRIConfigurationController::update( const rclcpp::Time &, const rclcpp::Duration &) { // TODO(Svastits): disable changes if HWIF is active - if (resend_multiplier_) + if (update_config_) { RCLCPP_INFO( - get_node()->get_logger(), "Changing receive multiplier of hardware interface to %i", - receive_multiplier_); - command_interfaces_[0].set_value(receive_multiplier_); - resend_multiplier_ = false; + get_node()->get_logger(), "Updating FRI configuration of hardware interface: receive multiplier is %i, send period is %i [ms]", + receive_multiplier_, send_period_ms_); + command_interfaces_[0].set_value(receive_multiplier_); + command_interfaces_[1].set_value(send_period_ms_); + update_config_ = false; } return controller_interface::return_type::OK; } diff --git a/doc/wiki/4_Controllers.md b/doc/wiki/4_Controllers.md index 1ec94a59..17b4aa1b 100644 --- a/doc/wiki/4_Controllers.md +++ b/doc/wiki/4_Controllers.md @@ -53,6 +53,6 @@ __Required parameters__: None #### `fri_configuration_controller` -The `receive_multiplier` parameter of FRI defines the answer rate factor (ratio of receiving states and sending commands). This is a parameter of the hardware interface, which can be modified in connected state, when control is not active. To support changing this parameter after startup, the `FRIConfigurationController` implements a service named `~/set_receive_multiplier`. Sending a request containing the desired integer value of the `receive_multiplier` updates the parameter of the hardware interface. +The `SendPeriodMilliSec` parameter of FRI defines the period with which the controller sends state updates, while the `ReceiveMultiplier` defines the answer rate factor (ratio of receiving states and sending commands). These are parameters of the hardware interface, which can be modified in connected state, when control is not active. To support changing these parameters after startup, the `FRIConfigurationController` implements a service named `~/set_fri_config`. Sending a request containing the desired integer values of `send_period_ms` and `receive_multiplier` updates the parameters of the hardware interface. __Required parameters__: None diff --git a/kuka_sunrise_fri_driver/CMakeLists.txt b/kuka_sunrise_fri_driver/CMakeLists.txt index 02558987..c5f8163e 100644 --- a/kuka_sunrise_fri_driver/CMakeLists.txt +++ b/kuka_sunrise_fri_driver/CMakeLists.txt @@ -81,8 +81,10 @@ target_link_libraries(${PROJECT_NAME} fri_client_sdk) add_executable(robot_manager_node src/robot_manager_node.cpp) -ament_target_dependencies(robot_manager_node kuka_driver_interfaces kuka_drivers_core std_srvs +ament_target_dependencies(robot_manager_node kuka_driver_interfaces kuka_drivers_core std_msgs std_srvs controller_manager_msgs) +target_link_libraries(robot_manager_node fri_connection) + pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 2b0f7aef..b7b1647a 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -241,8 +241,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {"joint_group_impedance_controller"}, {GetControllerName(), kuka_drivers_core::JOINT_STATE_BROADCASTER, - kuka_drivers_core::FRI_STATE_BROADCASTER}, - change_controller_state_client_, {}, SwitchController::Request::BEST_EFFORT)) + kuka_drivers_core::FRI_STATE_BROADCASTER}, SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not deactivate RT controllers"); return ERROR;