merge controllers branch

controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp

@@ -20,7 +20,7 @@
 #include <vector>
 
 #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<kuka_driver_interfaces::srv::SetInt>::SharedPtr receive_multiplier_service_;
+  rclcpp::Service<kuka_driver_interfaces::srv::SetFriConfiguration>::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_

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<kuka_driver_interfaces::srv::SetInt>(
-    "~/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<kuka_driver_interfaces::srv::SetFriConfiguration>(
+    "~/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;
 }

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

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)
 

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;