Feature/fri external torque broadcast

doc/wiki/3_Sunrise_FRI.md

@@ -49,7 +49,7 @@ This starts the 3 core components of every driver (described in the [Non-real-ti
 - `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)
-
+- `external_torque_broadcaster` (of type `JointStateBroadcaster`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml), publishes a `JointState` message type on the topic `external_torque_broadcaster/joint_states` containing the measured external torques for every joint)
 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`):
     ```
     ros2 lifecycle set robot_manager configure
@@ -74,6 +74,7 @@ Both launch files support the following argument:
 - `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`)
+- `etb_config`: the location of the configuration file for the `external_torque_broadcaster` (defaults to `kuka_sunrise_fri_driver/config/external_torque_broadcaster._config.yaml`)
 
 
 The `startup_with_rviz.launch.py` additionally contains one argument:

kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp

@@ -19,6 +19,8 @@ namespace kuka_drivers_core
 {
 /* Fixed controller names */
 static constexpr char JOINT_STATE_BROADCASTER[] = "joint_state_broadcaster";
+static constexpr char EXTERNAL_TORQUE_BROADCASTER[] = "external_torque_broadcaster";
+
 static constexpr char CONTROL_MODE_HANDLER[] = "control_mode_handler";
 static constexpr char EVENT_BROADCASTER[] = "event_broadcaster";
 static constexpr char FRI_CONFIGURATION_CONTROLLER[] = "fri_configuration_controller";

kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml

@@ -0,0 +1,15 @@
+external_torque_broadcaster:
+  ros__parameters:
+    interfaces:
+     - external_torque
+    joints:
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+    - joint_7
+    map_interface_to_joint_state:
+      effort: external_torque
+    use_local_topics: true

kuka_sunrise_fri_driver/config/ros2_controller_config.yaml

@@ -12,6 +12,8 @@ controller_manager:
       type: kuka_controllers/FRIStateBroadcaster
     event_broadcaster:
       type: kuka_controllers/EventBroadcaster
+    external_torque_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
 
     # Configuration controllers
     joint_group_impedance_controller:

kuka_sunrise_fri_driver/launch/startup.launch.py

@@ -39,6 +39,7 @@ def launch_setup(context, *args, **kwargs):
     jtc_config = LaunchConfiguration("jtc_config")
     jic_config = LaunchConfiguration("jic_config")
     ec_config = LaunchConfiguration("ec_config")
+    etb_config = LaunchConfiguration("etb_config")
     if ns.perform(context) == "":
         tf_prefix = ""
     else:
@@ -116,6 +117,7 @@ def launch_setup(context, *args, **kwargs):
             jtc_config,
             jic_config,
             ec_config,
+            etb_config,
             {
                 "hardware_components_initial_state": {
                     "unconfigured": [tf_prefix + robot_model.perform(context)]
@@ -159,6 +161,7 @@ def controller_spawner(controller_names, activate=False):
 
     controller_names = [
         "joint_state_broadcaster",
+        "external_torque_broadcaster",
         "joint_trajectory_controller",
         "fri_configuration_controller",
         "fri_state_broadcaster",
@@ -222,4 +225,11 @@ def generate_launch_description():
             + "/config/effort_controller_config.yaml",
         )
     )
+    launch_arguments.append(
+        DeclareLaunchArgument(
+            "etb_config",
+            default_value=get_package_share_directory("kuka_sunrise_fri_driver")
+            + "/config/external_torque_broadcaster_config.yaml",
+        )
+    )
     return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])

kuka_sunrise_fri_driver/src/hardware_interface.cpp

@@ -468,7 +468,14 @@ void KukaFRIHardwareInterface::activateFrictionCompensation(double * values) con
 {
   for (int i = 0; i < DOF; i++)
   {
-    values[i] -= (values[i] / fabs(values[i]) * 0.1);
+    if (values[i] != 0.0)
+    {
+      values[i] -= (values[i] / fabs(values[i]) * 0.1);
+    }
+    else
+    {
+      values[i] -= 0.1;
+    }
   }
 }
 

kuka_sunrise_fri_driver/src/robot_manager_node.cpp

@@ -216,8 +216,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
   // Activate joint state broadcaster and controller for given control mode
   if (!kuka_drivers_core::changeControllerState(
         change_controller_state_client_,
-        {kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER,
-         GetControllerName()},
+        {kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::EXTERNAL_TORQUE_BROADCASTER,
+         kuka_drivers_core::FRI_STATE_BROADCASTER, GetControllerName()},
         {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER}))
   {
     RCLCPP_ERROR(get_logger(), "Could not activate RT controllers");
@@ -245,7 +245,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
   if (!kuka_drivers_core::changeControllerState(
         change_controller_state_client_, {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER},
         {GetControllerName(), kuka_drivers_core::JOINT_STATE_BROADCASTER,
-         kuka_drivers_core::FRI_STATE_BROADCASTER},
+         kuka_drivers_core::EXTERNAL_TORQUE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER},
         SwitchController::Request::BEST_EFFORT))
   {
     RCLCPP_ERROR(get_logger(), "Could not deactivate RT controllers");