Merge remote-tracking branch 'origin/master' into feature/moveit_task…

…_constructor
kroshu · Sep 10, 2024 · 7a77f0d · 7a77f0d
2 parents 780814d + 81a2d23
commit 7a77f0d
Show file tree

Hide file tree

Showing 25 changed files with 72 additions and 27 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -1,4 +1,2 @@
 ## code changes will send PR to following users
-* @VX792
-* @Svastits
-* @kovacsge11
+* @Svastits @kovacsge11
diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml
@@ -34,7 +34,7 @@ jobs:
       ANALYZER: sonarqube
       TEST_COVERAGE: true
       UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka-external-control-sdk#master'
-      ROS_DISTRO: humble
+      ROS_DISTRO: jazzy
       CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)
       EVENT_NAME: ${{ github.event_name }}
       BRANCH: ${{ github.event.ref }}
@@ -43,7 +43,7 @@ jobs:
       PR_NUMBER: ${{ github.event.number }}
       ANALYZER_TOKEN: ${{ secrets.ANALYZER_TOKEN }}
       DEBUG_BASH: true
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-24.04
     steps:
       - name: Reset ANALYZER for scheduled and push runs
         run: |

diff --git a/README.md b/README.md
@@ -2,12 +2,16 @@
 
 This repository contains ROS2 drivers for all KUKA operating systems.
 
-Github CI | SonarCloud
-------------| ---------------
-[![Build Status](https://github.com/kroshu//kuka_drivers/workflows/CI/badge.svg?branch=master)](https://github.com/kroshu/ros2_kuka_sunrise_fri_driver/actions) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=kroshu_kuka_drivers&metric=alert_status)](https://sonarcloud.io/dashboard?id=kroshu_kuka_drivers)
+ROS2 Distro | Branch | Github CI | SonarCloud
+------------ | -------------- | -------------- | --------------
+**Jazzy** | [`master`](https://github.com/kroshu/kuka_drivers/tree/master) | [![Build Status](https://github.com/kroshu//kuka_drivers/actions/workflows/industrial_ci.yml/badge.svg?branch=master)](https://github.com/kroshu/kuka_drivers/actions) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=kroshu_kuka_drivers&metric=alert_status)](https://sonarcloud.io/dashboard?id=kroshu_kuka_drivers)
+**Humble** | [`humble`](https://github.com/kroshu/kuka_drivers/tree/humble) | [![Build Status](https://github.com/kroshu//kuka_drivers/actions/workflows/industrial_ci.yml/badge.svg?branch=humble)](https://github.com/kroshu/kuka_drivers/actions) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=kroshu_kuka_drivers&metric=alert_status&branch=humble)](https://sonarcloud.io/dashboard?id=kroshu_kuka_drivers)
 
 # Requirements
-The drivers require a system with ROS installed. It is recommended to use Ubuntu 22.04 with ROS humble. Iron Irwini has breaking changes in the moveit API, thus it is not yet supported.
+The drivers require a system with ROS installed. It is recommended to use Ubuntu 24.04 with ROS Jazzy.
+
+Additionally, there exists a ROS Humble version of the drivers, and its corresponding configuration can be found under the `humble` branch.
+
 It is also recommended to use a client machine with a real-time kernel, as all three drivers require cyclic, real-time communication. Due to the real-time requirement, Windows systems are not recommended and covered in the documentation.
 
 

diff --git a/doc/wiki/3_Sunrise_FRI.md b/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:

diff --git a/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp b/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";

diff --git a/kuka_iiqka_eac_driver/test/test_driver_activation.py b/kuka_iiqka_eac_driver/test/test_driver_activation.py
@@ -71,7 +71,7 @@ def generate_test_description():
 class TestDriverActivation(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment base", timeout=5)
+        proc_output.assertWaitFor("Robot initialized", timeout=5)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'lbr_iisy3_r760'", timeout=5
         )

diff --git a/kuka_iiqka_eac_driver/test/test_driver_startup.py b/kuka_iiqka_eac_driver/test/test_driver_startup.py
@@ -50,7 +50,7 @@ def generate_test_description():
 class TestDriverStartup(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment base", timeout=5)
+        proc_output.assertWaitFor("Robot initialized", timeout=5)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'lbr_iisy3_r760'", timeout=5
         )

diff --git a/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py b/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py
@@ -75,8 +75,7 @@ def generate_test_description():
 class TestMultiStartup(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment test1_base", timeout=20)
-        proc_output.assertWaitFor("got segment test2_base", timeout=20)
+        proc_output.assertWaitFor("Robot initialized", timeout=20)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'test1_lbr_iisy3_r760'", timeout=20
         )

diff --git a/kuka_kss_rsi_driver/test/test_driver_activation.py b/kuka_kss_rsi_driver/test/test_driver_activation.py
@@ -77,7 +77,7 @@ def generate_test_description():
 class TestDriverActivation(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment base", timeout=5)
+        proc_output.assertWaitFor("Robot initialized", timeout=5)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'kr6_r700_sixx'", timeout=5
         )

diff --git a/kuka_kss_rsi_driver/test/test_driver_startup.py b/kuka_kss_rsi_driver/test/test_driver_startup.py
@@ -50,7 +50,7 @@ def generate_test_description():
 class TestDriverStartup(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment base", timeout=5)
+        proc_output.assertWaitFor("Robot initialized", timeout=5)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'kr6_r700_sixx'", timeout=5
         )

diff --git a/kuka_kss_rsi_driver/test/test_multi_robot_startup.py b/kuka_kss_rsi_driver/test/test_multi_robot_startup.py
@@ -71,8 +71,7 @@ def generate_test_description():
 class TestMultiStartup(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment test1_base", timeout=20)
-        proc_output.assertWaitFor("got segment test2_base", timeout=20)
+        proc_output.assertWaitFor("Robot initialized", timeout=20)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'test1_kr6_r700_sixx'", timeout=20
         )

diff --git a/kuka_rsi_simulator/setup.cfg b/kuka_rsi_simulator/setup.cfg
@@ -2,3 +2,7 @@
 script_dir=$base/lib/kuka_rsi_simulator
 [install]
 install_scripts=$base/lib/kuka_rsi_simulator
+[tool:pytest]
+minversion = 6.0
+addopts = --strict-markers
+norecursedirs = .git build dist
diff --git a/kuka_rsi_simulator/setup.py b/kuka_rsi_simulator/setup.py
@@ -39,4 +39,6 @@
     entry_points={
         "console_scripts": ["rsi_simulator = kuka_rsi_simulator.rsi_simulator:main"],
     },
+    tests_require=["pytest"],
+    test_suite="test",
 )
diff --git a/kuka_rsi_simulator/test/__init__.py b/kuka_rsi_simulator/test/__init__.py
diff --git a/kuka_rsi_simulator/test/test_dummy.py b/kuka_rsi_simulator/test/test_dummy.py
@@ -0,0 +1,2 @@
+def test_dummy():
+    pass
diff --git a/kuka_sunrise_fri_driver/CMakeLists.txt b/kuka_sunrise_fri_driver/CMakeLists.txt
@@ -27,7 +27,7 @@ find_package(std_srvs)
 find_package(nanopb REQUIRED)
 
 find_path(NANOPB_INCLUDE_DIR
-  NAMES pb.h 
+  NAMES pb.h
   PATHS /usr/include/nanopb /usr/local/include/nanopb
 )
 

diff --git a/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml b/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
diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/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:

diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/tcp_connection.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/tcp_connection.hpp
@@ -21,6 +21,7 @@
 #include <unistd.h>
 
 #include <atomic>
+#include <cstdint>
 #include <functional>
 #include <stdexcept>
 #include <string>

diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/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)])
diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/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;
+    }
   }
 }
 

diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/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");

diff --git a/kuka_sunrise_fri_driver/test/test_driver_activation.py b/kuka_sunrise_fri_driver/test/test_driver_activation.py
@@ -71,7 +71,7 @@ def generate_test_description():
 class TestDriverActivation(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment base", timeout=5)
+        proc_output.assertWaitFor("Robot initialized", timeout=5)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'lbr_iiwa14_r820'", timeout=5
         )

diff --git a/kuka_sunrise_fri_driver/test/test_driver_startup.py b/kuka_sunrise_fri_driver/test/test_driver_startup.py
@@ -50,7 +50,7 @@ def generate_test_description():
 class TestDriverStartup(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment base", timeout=5)
+        proc_output.assertWaitFor("Robot initialized", timeout=5)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'lbr_iiwa14_r820'", timeout=5
         )

diff --git a/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py b/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py
@@ -71,8 +71,7 @@ def generate_test_description():
 class TestMultiStartup(unittest.TestCase):
     def test_read_stdout(self, proc_output):
         # Check for successful initialization
-        proc_output.assertWaitFor("got segment test1_base", timeout=20)
-        proc_output.assertWaitFor("got segment test2_base", timeout=20)
+        proc_output.assertWaitFor("Robot initialized", timeout=20)
         proc_output.assertWaitFor(
             "Successful initialization of hardware 'test1_lbr_iiwa14_r820'", timeout=20
         )