Restructure Sunrise driver (#141)

* controller_ip to launch

* use controller_ip

* define setfriconfig srv

* move hw-related stuff to hwif step1

* format

* cleanup

* configuration manager cleanup

* hw if cleanup

* extend launch and config

* remove config manager

* clean up access rights

* remove configuring from rights

* if and controller cleanup

* change control and command modes in hwif

* add new args to robot app

* fix relative links in wiki

* fix file

* extend serialization

* client ip and port params

* remove params

* launch args

* update mds

* control mode handler

* extend md

* update params

* damping and stiffness as params

* update md

* update comment

* fix controller config

* fix multi-install

* friction compensation

* format

* fix merge

* fix doc merge

* merge controllers branch

* add event_broadcaster

* add activation test

* fix linking

* fix ip transmission

* fix ip on server side

* app folder

* format

* is_active and controller fixes

* fix kss doc

* add sleep necessary for jtc

* fix impedance settings

* extend log

* fri config fix

* rework fri configuration

* fix comparison

* remove unnecessary check + add comments

* add comment

* format + update md

* string_view

* str_view 2

* always allow unconfigured param changes

* fix build

* udpate package xml

* sonar fixes

---------

Co-authored-by: Aron Svastits <svastits1@gmail.com>
Co-authored-by: Gergely Kovacs <gergely.kovacs4@kuka.com>

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/msg/fri_configuration.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
@@ -53,9 +53,9 @@ 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::Subscription<kuka_driver_interfaces::msg::FriConfiguration>::SharedPtr fri_config_sub_;
   int receive_multiplier_ = 1;
-  bool resend_multiplier_ = false;
+  int send_period_ms_ = 10;
 };
 }  // namespace kuka_controllers
 #endif  // FRI_CONFIGURATION_CONTROLLER__FRI_CONFIGURATION_CONTROLLER_HPP_

controllers/fri_configuration_controller/src/fri_configuration_controller.cpp

@@ -20,19 +20,13 @@ 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)
+  auto callback = [this](const kuka_driver_interfaces::msg::FriConfiguration::SharedPtr msg)
   {
-    resend_multiplier_ = true;
-    receive_multiplier_ = request->data;
-    response->success = true;
+    receive_multiplier_ = msg->receive_multiplier;
+    send_period_ms_ = msg->send_period_ms;
   };
-  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
+  fri_config_sub_ = get_node()->create_subscription<kuka_driver_interfaces::msg::FriConfiguration>(
+    "~/set_fri_config", rclcpp::SystemDefaultsQoS(), callback);
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -43,6 +37,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,14 +71,9 @@ controller_interface::return_type FRIConfigurationController::update(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
   // TODO(Svastits): disable changes if HWIF is active
-  if (resend_multiplier_)
-  {
-    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;
-  }
+  command_interfaces_[0].set_value(receive_multiplier_);
+  command_interfaces_[1].set_value(send_period_ms_);
+
   return controller_interface::return_type::OK;
 }
 

doc/wiki/2_KSS_RSI.md

@@ -64,7 +64,6 @@ Method 2:
 #### Startup configuration
 
 The following configuration files are available in the `config` directory of the package:
-- `driver_config.yaml`: contains the IP address of the client machine
 - `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used.
 - configuration files for specific controllers, for further information, see the documentation of the given controller
 

doc/wiki/3_Sunrise_FRI.md

@@ -8,7 +8,6 @@
 
 #### Controller side
 
-- Modify the `_remoteIP` variable of the [FRIConfigurationParams.java](https://github.com/kroshu/kuka_drivers/blob/master/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java) file to the IP address of the client machine
 - Upload the robot application under `robot_application/src` to the controller using Sunrise Workbench
 
 ### Configuration
@@ -28,8 +27,8 @@ The IP address of robot controller must be provided as a launch argument. For fu
 The parameters in the driver configuration file can be also changed during runtime using the parameter interface of the `robot_manager` node:
 - `send_period_ms` (integer): this parameter defines the send rate in milliseconds (with which the controller sends robot state updates). It must be between 1 and 10 for control and can be only changed in `inactive` and `configuring` states.
 - `receive_multiplier` (integer): this parameter defines the answer rate factor (the client should sends commands in every `receive_multiplier`*`send_period_ms` milliseconds). It must be at least 1 and can be only changed in `inactive` and `configuring` states.
-- `control_mode`, `command_mode` (strings): control mode related parameters, which will be combined to support the defined enums. They cannot be changed in active state.
-- `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. They will be removed after changing to using the `joint_group_impedance_controller` to adapt to conventions.
+- `control_mode`: The enum value of the control mode should be given, which updates the `ControlMode` and `ClientCommandMode` parameters of FRI. It cannot be changed in active state.
+- `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. The updated values are sent to the hardware interface using the `joint_group_impedance_controller` to adapt to conventions, but it is not possible to change them in active state due to the constraints of FRI. (Therefore the `joint_group_impedance_controller` is deactivated at driver activation.)
 - `position_controller_name`: The name of the controller (string) that controls the `position` interface of the robot. It can't be changed in active state.
 - `torque_controller_name`: The name of the controller (string) that controls the `effort` interface of the robot. It can't be changed in active state.
 
@@ -39,14 +38,17 @@ The parameters in the driver configuration file can be also changed during runti
 
 1. On the controller, start the uploaded robot application (ROS2_Control).
 2. To start the driver, two launch file are available, with and without `rviz`. To launch (without `rviz`), run
-    ```
-    ros2 launch kuka_sunrise_fri_driver startup.launch.py controller_ip:=0.0.0.0
-    ```
-    - This starts the 3 core components of every driver (described in the [Non-real-time interface](https://github.com/kroshu/kuka_drivers/wiki#non-real-time-interface) section of the project overview) and the following controllers:
-      - `joint_state_broadcaster` (no configuration file, all state interfaces are published)
-      - `joint_trajectory_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml))
-      - [`fri_configuration_controller`](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#fri_configuration_controller) (no configuration file)
-      - [`fri_state_broadcaster`](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#fri_state_broadcaster) (no configuration file)
+```
+ros2 launch kuka_sunrise_fri_driver startup.launch.py controller_ip:=0.0.0.0 client_ip:=0.0.0.0
+```
+This starts the 3 core components of every driver (described in the [Non-real-time interface](https://github.com/kroshu/kuka_drivers/wiki#non-real-time-interface) section of the project overview) and the following controllers:
+- `joint_state_broadcaster` (no configuration file, all state interfaces are published)
+- `joint_trajectory_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml))
+- [`fri_configuration_controller`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_configuration_controller) (no configuration file)
+- [`fri_state_broadcaster`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_state_broadcaster) (no configuration file)
+- `joint_group_impedance_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml))
+- `effort_controller` (of type `JointGroupEffortController`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/effort_controller_config.yaml))
+- [`control_mode_handler`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#control_mode_handler) (no configuration file)
 
 3. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`):
     ```
@@ -60,6 +62,8 @@ On successful activation the brakes of the robot will be released and external c
 
 Both launch files support the following argument:
 - `controller_ip`: IP address of the robot controller
+- `client_ip`: IP address of the client PC
+- `client_port`: port of the client machine (default: 30200)
 - `robot_model`: defines which LBR iiwa robot to use. Available options: `lbr_iiwa14_r820` (default)
 - `use_fake_hardware`: if true, the `KukaMockHardwareInterface` will be used instead of the `KukaFRIHardwareInterface`. This enables trying out the driver without actual hardware.
 - `namespace`: adds a namespace to all nodes and controllers of the driver, and modifies the `prefix` argument of the robot description macro to `namespace_`
@@ -68,6 +72,8 @@ Both launch files support the following argument:
 - `roundtrip_time`: The roundtrip time (in microseconds) to be enforced by the [KUKA mock hardware interface](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#custom-mock-hardware), (defaults to 5000 us, only used if `use_fake_hardware` is true)
 - `controller_config`: the location of the `ros2_control` configuration file (defaults to `kuka_sunrise_fri_driver/config/ros2_controller_config.yaml`)
 - `jtc_config`: the location of the configuration file for the `joint_trajectory_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml`)
+- `jic_config`: the location of the configuration file for the `joint_impedance_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml`)
+- `ec_config`: the location of the configuration file for the `effort_controller` (defaults to `kuka_sunrise_fri_driver/config/effort_controller_config.yaml`)
 
 
 The `startup_with_rviz.launch.py` additionally contains one argument:
@@ -83,9 +89,5 @@ BEWARE, that this is a non-realtime process including lifecycle management, so t
 
 ### Known issues and limitations
 
-- Not all hardware-related communication is implemented in the hardware interface, therefore the mock hardware option is not working properly
-- The control mode handling for the driver is not the one defined in the `kuka_drivers_core` package
-  - enum definition and controller switching logic is not used
-  - joint impedance control is not implemented properly using command interfaces
 - I/O control was not tested
 - Cartesian modes are not yet supported

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` advertises the topic `~/set_fri_config`. Sending a message containing the desired integer values of `send_period_ms` and `receive_multiplier` updates the parameters of the hardware interface.
 
 __Required parameters__: None

doc/wiki/Home.md

@@ -34,7 +34,7 @@ The startup procedure for any system in ROS can be defined using a launch file,
 The last issue should be certainly prevented from happening, therefore it was decided to extend the default startup procedure with a [lifecycle interface](https://design.ros2.org/articles/node_lifecycle.html), that synchronizes all components of the driver. The hardware interfaces and controllers already have a lifecycle interface, but by default they are loaded and activated at startup. This configuration was modified to only load the hardware interfaces and controllers, configuration and activation is handled by a custom a lifecycle node, called `robot_manager`. The 3 states of the `robot_manager` node have the following meaning:
 
 - `unconfigured`: all necessary components are started, but no connection is needed to the robot
-- `configured`: The driver has configured the parameters necessary to start external control. Connection to the robot might be needed. (All of the parameters have default values in the driver, which are set on the robot controller during configuration.) A few [configuration controllers](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#configuration-controllers) might be active, that handle the runtime parameters of the hardware interface.
+- `configured`: The driver has valid parameters configured, external control can be initiated. It is possible to change most parameters (with the exception of IP addresses and robot model) in this state without having to clean up the `robot_manager` node. Connection to the robot might be needed. (All of the parameters have default values in the driver, which are set on the robot controller during configuration.) A few [configuration controllers](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#configuration-controllers) might be active, that handle the runtime parameters of the hardware interface.
 - `active`: external control is running with cyclic real-time communication, controllers are active
 
 To achieve these synchronized states, the state transitions of the system do the following steps (implemented by the launch file and the `robot_manager` node):
@@ -61,7 +61,7 @@ The control mode specifications are also part of the common API. They are define
 
 - joint position control: the driver streams cyclic position updates for every joint.
     - Needed command interface(s): `position`
-- joint impedance control: the driver streams cyclic position updates for every joint and additionally stiffness [Nm/rad] and normalized damping [-] attributes, which define how the joint reacts to external effects (around the setpoint position). The effect of gravity is compensated internally.
+- joint impedance control: the driver streams cyclic position updates for every joint and additionally stiffness [Nm/rad] and normalized damping [-] attributes, which define how the joint reacts to external effects (around the setpoint position). The effect of gravity is compensated internally. (FRI does not allow changing the impedance attributes in runtime, therefore the initial damping and stiffness values are valid for the whole motion.)
     - Needed command interface(s): `position`, `stiffness`, `damping`
 - joint velocity control: the driver streams cyclic velocity updates for every joint.
     - Needed command interface(s): `velocity`

kuka_driver_interfaces/CMakeLists.txt

@@ -20,9 +20,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "srv/SetInt.srv"
-  "srv/GetInt.srv"
-  "srv/SetDouble.srv"
+  "msg/FriConfiguration.msg"
   "msg/FRIState.msg"
 )
 

kuka_driver_interfaces/msg/FriConfiguration.msg

@@ -0,0 +1,3 @@
+# Message with the modifiable values of FRI configuration
+int32 receive_multiplier
+int32 send_period_ms

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<T>` class supports the following parameter types (identical to the types supported by `rclcpp::Parameter`):
@@ -98,7 +98,7 @@ The `Parameter<T>` 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<bool(const T &)>]: 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<int>(
-    "rate", 2, kuka_drivers_core::ParameterSetAccessRights {true, true,
-      false, false}, [this](int rate) {
+    "rate", 2, kuka_drivers_core::ParameterSetAccessRights {true, false},
+      [this](int rate) {
       return this->onRateChangeRequest(rate);
     });
 

kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp

@@ -26,6 +26,7 @@ static constexpr char FRI_STATE_BROADCASTER[] = "fri_state_broadcaster";
 
 // Controller names with default values
 static constexpr char JOINT_TRAJECTORY_CONTROLLER[] = "joint_trajectory_controller";
+static constexpr char JOINT_GROUP_IMPEDANCE_CONTROLLER[] = "joint_group_impedance_controller";
 }  // namespace kuka_drivers_core
 
 #endif  // KUKA_DRIVERS_CORE__CONTROLLER_NAMES_HPP_

kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp

@@ -40,6 +40,7 @@ static constexpr char STATE_PREFIX[] = "state";
 static constexpr char CONTROL_MODE[] = "control_mode";
 // Constant defining the receive multiplier interface needed for FRI
 static constexpr char RECEIVE_MULTIPLIER[] = "receive_multiplier";
+static constexpr char SEND_PERIOD[] = "send_period_ms";
 
 /* FRI state interfaces */
 static constexpr char SESSION_STATE[] = "session_state";