Merge branch 'master' into feature/fri_wrench_control

kroshu · Oct 14, 2024 · f1753e9 · f1753e9
2 parents bfacd30 + bd4ffa5
commit f1753e9
Show file tree

Hide file tree

Showing 30 changed files with 134 additions and 72 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#feature/fri_wrench_control 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/.gitignore b/.gitignore
@@ -1 +1,2 @@
 __pycache__/
+.vscode
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
@@ -50,9 +50,10 @@ 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)
 - `wrench_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/feature/fri_wrench_control/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml))
 
-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`):
+1. 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
     ros2 lifecycle set robot_manager activate
@@ -76,6 +77,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`)
 - `cic_config`: the location of the configuration file for the `cartesian_impedance_controller` (defaults to `kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml`)
 - `wc_config`: the location of the configuration file for the `wrench_controller` (defaults to `kuka_sunrise_fri_driver/config/wrench_controller_config.yaml`)
 

diff --git a/examples/iiqka_moveit_example/include/iiqka_moveit_example/moveit_example.hpp b/examples/iiqka_moveit_example/include/iiqka_moveit_example/moveit_example.hpp
@@ -109,7 +109,7 @@ class MoveitExample : public rclcpp::Node
     else
     {
       RCLCPP_INFO(LOGGER, "Planning successful");
-      return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory_);
+      return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory);
     }
   }
 
@@ -127,7 +127,7 @@ class MoveitExample : public rclcpp::Node
     else
     {
       RCLCPP_INFO(LOGGER, "Planning successful");
-      return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory_);
+      return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory);
     }
   }
 
@@ -153,7 +153,7 @@ class MoveitExample : public rclcpp::Node
     auto stop = std::chrono::high_resolution_clock::now();
     auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(stop - start);
     RCLCPP_INFO(LOGGER, "Planning successful after %li ms", duration.count());
-    return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory_);
+    return std::make_shared<moveit_msgs::msg::RobotTrajectory>(plan.trajectory);
   }
 
   void AddObject(const moveit_msgs::msg::CollisionObject & object)

diff --git a/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp b/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp
@@ -15,6 +15,8 @@
 #ifndef KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_
 #define KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_
 
+#include <cstdint>
+
 namespace kuka_drivers_core
 {
 /**

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/launch/startup.launch.py b/kuka_iiqka_eac_driver/launch/startup.launch.py
@@ -113,9 +113,6 @@ def launch_setup(context, *args, **kwargs):
         parameters=[
             robot_description,
             controller_config,
-            jtc_config,
-            jic_config,
-            ec_config,
             {
                 "hardware_components_initial_state": {
                     "unconfigured": [tf_prefix + robot_model.perform(context)]
@@ -145,29 +142,37 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # Spawn controllers
-    def controller_spawner(controller_names, activate=False):
+    def controller_spawner(controller_name, param_file=None, activate=False):
         arg_list = [
-            controller_names,
+            controller_name,
             "-c",
             controller_manager_node,
             "-n",
             ns,
         ]
+
+        # Add param-file if it's provided
+        if param_file:
+            arg_list.extend(["--param-file", param_file])
+
         if not activate:
             arg_list.append("--inactive")
+
         return Node(package="controller_manager", executable="spawner", arguments=arg_list)
 
-    controller_names = [
-        "joint_state_broadcaster",
-        "joint_trajectory_controller",
-        "joint_group_impedance_controller",
-        "effort_controller",
-        "control_mode_handler",
-        "event_broadcaster",
+    controllers = {
+        "joint_state_broadcaster": None,
+        "joint_trajectory_controller": jtc_config,
+        "joint_group_impedance_controller": jic_config,
+        "effort_controller": ec_config,
+        "control_mode_handler": None,
+        "event_broadcaster": None,
+    }
+
+    controller_spawners = [
+        controller_spawner(name, param_file) for name, param_file in controllers.items()
     ]
 
-    controller_spawners = [controller_spawner(name) for name in controller_names]
-
     nodes_to_start = [
         control_node,
         robot_manager_node,

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/launch/startup.launch.py b/kuka_kss_rsi_driver/launch/startup.launch.py
@@ -104,7 +104,6 @@ def launch_setup(context, *args, **kwargs):
         parameters=[
             robot_description,
             controller_config,
-            jtc_config,
             {
                 "hardware_components_initial_state": {
                     "unconfigured": [tf_prefix + robot_model.perform(context)]
@@ -128,24 +127,32 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # Spawn controllers
-    def controller_spawner(controller_names, activate=False):
+    def controller_spawner(controller_name, param_file=None, activate=False):
         arg_list = [
-            controller_names,
+            controller_name,
             "-c",
             controller_manager_node,
             "-n",
             ns,
         ]
+
+        # Add param-file if it's provided
+        if param_file:
+            arg_list.extend(["--param-file", param_file])
+
         if not activate:
             arg_list.append("--inactive")
+
         return Node(package="controller_manager", executable="spawner", arguments=arg_list)
 
-    controller_names = [
-        "joint_state_broadcaster",
-        "joint_trajectory_controller",
-    ]
+    controllers = {
+        "joint_state_broadcaster": None,
+        "joint_trajectory_controller": jtc_config,
+    }
 
-    controller_spawners = [controller_spawner(name) for name in controller_names]
+    controller_spawners = [
+        controller_spawner(name, param_file) for name, param_file in controllers.items()
+    ]
 
     nodes_to_start = [
         control_node,

diff --git a/kuka_kss_rsi_driver/src/hardware_interface.cpp b/kuka_kss_rsi_driver/src/hardware_interface.cpp
@@ -168,7 +168,7 @@ return_type KukaRSIHardwareInterface::read(const rclcpp::Time &, const rclcpp::D
   if (server_->recv(in_buffer_) == 0)
   {
     RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "No data received from robot");
-    this->on_deactivate(this->get_state());
+    this->on_deactivate(this->get_lifecycle_state());
     return return_type::ERROR;
   }
   rsi_state_ = RSIState(in_buffer_);

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
@@ -24,8 +24,14 @@ find_package(pluginlib REQUIRED)
 find_package(controller_manager_msgs)
 find_package(std_msgs)
 find_package(std_srvs)
+find_package(nanopb REQUIRED)
 
-include_directories(include src/fri_client_sdk)
+find_path(NANOPB_INCLUDE_DIR
+  NAMES pb.h
+  PATHS /usr/include/nanopb /usr/local/include/nanopb
+)
+
+include_directories(include src/fri_client_sdk ${NANOPB_INCLUDE_DIR})
 
 add_library(fri_connection SHARED
   src/connection_helpers/fri_connection.cpp

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
     wrench_controller:
       type: kuka_controllers/WrenchController
 

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>