Integrate kuka-external-control-sdk

.github/workflows/industrial_ci.yml

@@ -42,13 +42,15 @@ jobs:
           ROS_DISTRO: humble
     env:
       CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)
+      BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install'
       EVENT_NAME: ${{ github.event_name }}
       BRANCH: ${{ github.event.ref }}
       PR_BRANCH: ${{ github.event.pull_request.head.ref }}
       PR_BASE: ${{ github.event.pull_request.base.ref }}
       PR_NUMBER: ${{ github.event.number }}
       ANALYZER_TOKEN: ${{ secrets.ANALYZER_TOKEN }}
       DEBUG_BASH: true
+      DOCKER_RUN_OPTS: '-e LD_LIBRARY_PATH=/root/.local/lib'
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v2
@@ -59,6 +61,7 @@ jobs:
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
+
       # Run industrial_ci
       - uses: 'kroshu/industrial_ci@master'
         env: ${{ matrix.env }}

README.md

@@ -40,6 +40,19 @@ sudo apt upgrade
 rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y
 ```
 
+Clone and build kuka-external-control-sdk in a different workspace.
+ - This library is necessary for the iiQKA ExternalAPI.Control driver
+ - The library is not a ROS2 package, therefore a different workspace is necessary, otherwise colcon will fail to build it
+```bash
+mkdir -p ~/sdk_ws/src
+cd ~/sdk_ws/src
+git clone https://github.com/kroshu/kuka-external-control-sdk.git
+mkdir -p ~/sdk_ws/src/kuka-external-control-sdk/kuka-external-control-sdk/build
+cd ~/sdk_ws/src/kuka-external-control-sdk/kuka-external-control-sdk/build
+cmake ..
+make install
+```
+
 Build KUKA packages.
 ```bash
 cd ~/ros2_ws

doc/wiki/KSS_RSI.md

@@ -65,7 +65,7 @@ Method 2:
 
 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. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup.
+- `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
 
 ##### IP configuration

doc/wiki/Sunrise_FRI.md

@@ -17,7 +17,7 @@
 
 The following configuration files are available in the `config` directory of the package:
 - `driver_config.yaml`: : contains runtime parameters of the `robot_manager` node
-- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup.
+- `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
 - `gpio_config.xacro`: contains the I/O setup of the system, but this was not tested yet
 

doc/wiki/iiQKA_EAC.md

@@ -4,8 +4,6 @@
 
 #### Client side
 - It is recommended to use the driver on a real-time capable client machine (further information about setting up the PREEMPT_RT patch can be found [here](https://github.com/kroshu/kuka_drivers/wiki/Realtime)).
-- The driver depends on some KUKA-specific packages, which are only available with the real robot, therefore a mock mode is provided to enable trying out solutions with the same components running.
-  - By default, the mock libraries are used, this can be changed in the `CmakeLists.txt` file by setting `MOCK_KUKA_LIBS` to `FALSE` before building.
 - Set a fixed IP in the subnet of the KONI interface for the real-time machine.
 
 #### Controller side
@@ -22,7 +20,7 @@
 The following configuration files are available in the `config` directory of the package:
 - `driver_config.yaml`: contains runtime parameters of the `robot_manager` node
 - `qos_config.yaml`: contains the configuration options for the QoS profile defining the connection quality (description later)
-- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup.
+- `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)
 
 ##### QoS profile configuration
@@ -65,6 +63,8 @@ After successful startup, the `robot_manager` node has to be activated to start
 
 On successful activation the brakes of the robot will be released and external control is started using the requested control mode. To test moving the robot, the `rqt_joint_trajectory_controller` is not recommended, use the launch file in the `iiqka_moveit_example` package instead (usage is described in the *Additional packages* section of the [project overview](Project%20overview.md)).
 
+It is important to note, that the commanded and measures torques have a different meaning: the `effort` command interface accepts values that should be superimposed on internal gravity compensation, while the state interface provides the actually measured torques (sum on internal and external effects).
+
 
 ##### Launch arguments
 

kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp

@@ -76,7 +76,7 @@ class ControllerHandler
    *
    * @param fixed_controllers: Controllers that have to be active in all control modes
    */
-  explicit ControllerHandler(std::vector<std::string> fixed_controllers);
+  explicit ControllerHandler(std::vector<std::string> fixed_controllers = {});
 
   /**
    * @brief Destroy the control mode handler object
@@ -126,6 +126,8 @@ class ControllerHandler
    *
    */
   bool ApproveControllerDeactivation();
+
+  std::vector<std::string> GetControllersForMode(ControlMode control_mode);
 };
 }  // namespace kuka_drivers_core
 

kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp

@@ -0,0 +1,35 @@
+// Copyright 2024 Áron Svastits
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef KUKA_DRIVERS_CORE__HARDWARE_EVENT_HPP_
+#define KUKA_DRIVERS_CORE__HARDWARE_EVENT_HPP_
+
+namespace kuka_drivers_core
+{
+/**
+ * @brief Enum for controller-side events
+ */
+enum class HardwareEvent : std::uint8_t
+{
+  HARDWARE_EVENT_UNSPECIFIED = 0,
+  COMMAND_ACCEPTED = 2,
+  CONTROL_STARTED = 3,
+  CONTROL_STOPPED = 4,
+  CONTROL_MODE_SWITCH = 5,
+  ERROR = 6
+};
+
+}  // namespace kuka_drivers_core
+
+#endif  // KUKA_DRIVERS_CORE__HARDWARE_EVENT_HPP_

kuka_drivers_core/src/controller_handler.cpp

@@ -72,7 +72,7 @@ ControllerHandler::GetControllersForSwitch(ControlMode new_control_mode)
 {
   if (control_mode_map_.find(new_control_mode) == control_mode_map_.end())
   {
-    // Not valid control mode, through error
+    // Not valid control mode, throw exception
     throw std::out_of_range("Attribute new_control_mode is out of range");
   }
 
@@ -148,4 +148,18 @@ bool ControllerHandler::ApproveControllerDeactivation()
 
   return true;
 }
+
+std::vector<std::string> ControllerHandler::GetControllersForMode(ControlMode control_mode)
+{
+  std::vector<std::string> controllers;
+
+  auto controller_types = control_mode_map_.at(control_mode);
+  controllers.push_back(controller_types.standard_controller);
+  if (!controller_types.impedance_controller.empty())
+  {
+    controllers.push_back(controller_types.impedance_controller);
+  }
+  return controllers;
+}
+
 }  // namespace kuka_drivers_core

kuka_iiqka_eac_driver/CMakeLists.txt

@@ -19,7 +19,6 @@ endif()
 # TODO: find better solution
 set(CMAKE_EXE_LINKER_FLAGS "-Wl,--copy-dt-needed-entries")
 
-set(MOCK_KUKA_LIBS TRUE)
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
@@ -29,65 +28,33 @@ find_package(kuka_drivers_core REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(controller_manager_msgs REQUIRED)
-find_package(yaml-cpp REQUIRED)
 
 include_directories(include)
 
-if(NOT MOCK_KUKA_LIBS)
-  find_package(os-core-udp-communication REQUIRED)
-  find_package(motion-services-ecs-proto-api-cpp REQUIRED)
-  find_package(motion-external-proto-api-nanopb REQUIRED)
-  find_package(motion-services-ecs-proto-api-nanopb REQUIRED)
-  find_package(nanopb-helpers REQUIRED)
-  message("Using real kuka libs")
-  add_definitions(-DNON_MOCK_SETUP)
-else()
-  find_package(Protobuf)
-  include_directories(include/mock)
-  add_library(os-core-udp-communication SHARED
-    src/mock/os-core-udp-communication/udp_replier_mock.cpp
-    src/mock/os-core-udp-communication/udp_socket_mock.cpp)
-  add_library(kuka::os-core-udp-communication ALIAS os-core-udp-communication)
-  install(TARGETS os-core-udp-communication
-    LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
-  )
-
-  add_library(nanopb-helpers INTERFACE)
-  target_include_directories(nanopb-helpers INTERFACE include/mock/nanopb-helpers)
-  target_link_libraries(nanopb-helpers INTERFACE protobuf::libprotobuf)
-  add_library(kuka::nanopb-helpers ALIAS nanopb-helpers)
-
-  add_library(motion-services-ecs-proto-api-cpp INTERFACE)
-  target_include_directories(motion-services-ecs-proto-api-cpp INTERFACE include/mock/kuka)
-
-  add_library(motion-external-proto-api-nanopb INTERFACE)
-  target_include_directories(motion-external-proto-api-nanopb INTERFACE include/mock/nanopb/kuka/motion/external)
-  target_link_libraries(motion-external-proto-api-nanopb INTERFACE protobuf::libprotobuf)
-
-  add_library(motion-services-ecs-proto-api-nanopb INTERFACE)
-  target_include_directories(motion-services-ecs-proto-api-nanopb INTERFACE include/mock/nanopb/kuka/ecs/v1)
-  target_link_libraries(motion-external-proto-api-nanopb INTERFACE motion-external-proto-api-nanopb)
-  message("Using mock kuka libs")
-  remove_definitions(-DNON_MOCK_SETUP)
-endif()
+find_library(EXTERNAL_CONTROL_SDK kuka-external-control-sdk PATHS "~/.local/lib" REQUIRED)
+find_library(EXTERNAL_CONTROL_SDK_PROTOBUF kuka-external-control-sdk-protobuf PATHS "~/.local/lib" REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
   src/hardware_interface.cpp
 )
 
+target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      "~/.local/include"
+)
+
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_IIQKA_EAC_DRIVER_BUILDING_LIBRARY")
 
 ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface kuka_drivers_core)
-target_link_libraries(${PROJECT_NAME} motion-external-proto-api-nanopb motion-services-ecs-proto-api-cpp
-  motion-services-ecs-proto-api-nanopb yaml-cpp kuka::os-core-udp-communication kuka::nanopb-helpers)
+target_link_libraries(${PROJECT_NAME} ${EXTERNAL_CONTROL_SDK})
 
 
 add_executable(robot_manager_node
   src/robot_manager_node.cpp)
 ament_target_dependencies(robot_manager_node rclcpp kuka_drivers_core sensor_msgs controller_manager_msgs)
-target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers motion-services-ecs-proto-api-cpp)
+target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers ${EXTERNAL_CONTROL_SDK})
 
 pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml)
 
@@ -100,12 +67,17 @@ install(DIRECTORY config launch test
 if(BUILD_TESTING)
   find_package(launch_testing_ament_cmake)
   add_launch_test(test/test_driver_startup.py)
-  add_launch_test(test/test_driver_activation.py)
+  # TODO: re-add activation test, if KUKA mock hardware is finished
+  # add_launch_test(test/test_driver_activation.py)
   add_launch_test(test/test_multi_robot_startup.py)
 endif()
 
 ament_export_libraries(
   ${PROJECT_NAME}
 )
 
+# Hack to extend $LD_LIBRARY_PATH with ~/.local/lib when sourcing
+file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_environment_hooks/library_path.dsv
+  "prepend-non-duplicate;LD_LIBRARY_PATH;$ENV{HOME}/.local/lib")
+
 ament_package()

kuka_iiqka_eac_driver/config/ros2_controller_config.yaml

@@ -12,5 +12,5 @@ controller_manager:
       type: effort_controllers/JointGroupPositionController
     control_mode_handler:
       type: kuka_controllers/ControlModeHandler
-
-    configure_components_on_start: [""]
+    event_broadcaster:
+      type: kuka_controllers/EventBroadcaster

kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp

@@ -0,0 +1,67 @@
+// Copyright 2024 Márk Szitanics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_
+#define KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_
+
+#include <string>
+#include "rclcpp/macros.hpp"
+
+#include "kuka/external-control-sdk/common/irobot.h"
+#include "kuka_drivers_core/hardware_event.hpp"
+#include "kuka_iiqka_eac_driver/hardware_interface.hpp"
+
+namespace kuka_eac
+{
+class KukaEACEventObserver : public kuka::external::control::EventHandler
+{
+public:
+  explicit KukaEACEventObserver(KukaEACHardwareInterface * hw_interface)
+  : hw_interface_(hw_interface)
+  {
+  }
+  void OnSampling() override
+  {
+    hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STARTED);
+    RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control is active");
+  }
+  void OnControlModeSwitch(const std::string &) override
+  {
+    hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH);
+    RCLCPP_INFO(
+      rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress");
+    hw_interface_->reset_cycle_count();
+  }
+  void OnStopped(const std::string &) override
+  {
+    hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STOPPED);
+    RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished");
+    hw_interface_->set_stop_flag();
+  }
+  void OnError(const std::string & reason) override
+  {
+    hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::ERROR);
+    RCLCPP_ERROR(
+      rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error");
+    RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str());
+    hw_interface_->set_stop_flag();
+  }
+
+private:
+  KukaEACHardwareInterface * hw_interface_;
+};
+
+}  // namespace kuka_eac
+
+#endif  // KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_