Integrate kuka-external-control-sdk

.github/workflows/industrial_ci.yml

@@ -59,6 +59,24 @@ jobs:
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
+
+      # Install and build kuka-external-control-sdk
+      - name: Install dependencies
+        run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev
+      - name: Clone sdk lib
+        run: git clone https://github.com/kroshu/kuka-external-control-sdk.git
+        working-directory: /home/runner/work
+      - name: Create build directory
+        run: mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build
+      - name: Build and install sdk
+        run: cmake .. && sudo make install
+        working-directory: /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build
+
       # Run industrial_ci
+      - name: Extend LD_LIBRARY_PATH
+        run: echo "LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib" >> $GITHUB_ENV
+      - name: Debug
+        run: ls -l /usr/local/lib && ls -l /usr/include/kuka/external-control-sdk/iiqka/ && echo $LD_LIBRARY_PATH
+
       - 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 ..
+sudo make install
+```
+
 Build KUKA packages.
 ```bash
 cd ~/ros2_ws

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_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,47 +28,10 @@ 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(NAMES kuka-external-control-sdk PATHS "/usr/local/lib" REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
   src/hardware_interface.cpp
@@ -80,14 +42,13 @@ add_library(${PROJECT_NAME} SHARED
 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} kuka-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 kuka-external-control-sdk)
 
 pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml)
 
@@ -108,4 +69,8 @@ ament_export_libraries(
   ${PROJECT_NAME}
 )
 
+# Hack to extend $LD_LIBRARY_PATH with /usr/local/lib when sourcing
+file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_environment_hooks/library_path.dsv
+  "prepend-non-duplicate;LD_LIBRARY_PATH;/usr/local/lib")
+
 ament_package()

kuka_iiqka_eac_driver/config/ros2_controller_config.yaml

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

kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp

@@ -0,0 +1,62 @@
+// 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 "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:
+  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");
+  }
+  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_

kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp

@@ -27,13 +27,12 @@
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/rclcpp.hpp"
+
+#include "kuka/external-control-sdk/iiqka/message_builder.h"
+#include "kuka/external-control-sdk/iiqka/robot.h"
 #include "rclcpp_lifecycle/state.hpp"
 
-#include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h"
-#include "nanopb/kuka/core/motion/joint.pb.hh"
-#include "nanopb/kuka/ecs/v1/control_signal_external.pb.hh"
-#include "nanopb/kuka/ecs/v1/motion_state_external.pb.hh"
-#include "os-core-udp-communication/replier.h"
+#include "kuka_drivers_core/hardware_event.hpp"
 
 #include "kuka_iiqka_eac_driver/visibility_control.h"
 
@@ -42,7 +41,6 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
 
 namespace kuka_eac
 {
-
 class KukaEACHardwareInterface : public hardware_interface::SystemInterface
 {
 public:
@@ -72,39 +70,35 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface
   KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type
   write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_server_event(kuka_drivers_core::HardwareEvent event);
+
+  KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_stop_flag() { stop_requested_ = true; };
+
 private:
-  KUKA_IIQKA_EAC_DRIVER_LOCAL void ObserveControl();
+  KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot();
+  KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupQoS();
 
-  bool is_active_ = false;
-  bool msg_received_ = false;
+  std::unique_ptr<kuka::external::control::iiqka::Robot> robot_ptr_;
 
   std::vector<double> hw_position_commands_;
   std::vector<double> hw_torque_commands_;
   std::vector<double> hw_stiffness_commands_;
   std::vector<double> hw_damping_commands_;
-
   std::vector<double> hw_position_states_;
   std::vector<double> hw_torque_states_;
 
-  double hw_control_mode_command_;
-
-#ifdef NON_MOCK_SETUP
-  kuka::ecs::v1::CommandState command_state_;
-  std::unique_ptr<kuka::ecs::v1::ExternalControlService::Stub> stub_;
-  std::unique_ptr<grpc::ClientContext> context_;
-#endif
-
-  std::thread observe_thread_;
+  double hw_control_mode_command_ = 0;
+  double server_state_ = 0;
 
-  std::unique_ptr<os::core::udp::communication::Replier> udp_replier_;
-  std::chrono::milliseconds receive_timeout_{100};
+  std::mutex event_mutex_;
 
-  uint8_t out_buff_arr_[1500];
+  kuka_drivers_core::ControlMode prev_control_mode_ =
+    kuka_drivers_core::ControlMode::CONTROL_MODE_UNSPECIFIED;
+  kuka_drivers_core::HardwareEvent last_event_ =
+    kuka_drivers_core::HardwareEvent::HARDWARE_EVENT_UNSPECIFIED;
 
-  nanopb::kuka::ecs::v1::ControlSignalExternal control_signal_ext_{
-    nanopb::kuka::ecs::v1::ControlSignalExternal_init_default};
-  nanopb::kuka::ecs::v1::MotionStateExternal motion_state_external_{
-    nanopb::kuka::ecs::v1::MotionStateExternal_init_default};
+  bool msg_received_;
+  std::atomic<bool> stop_requested_{false};
 };
 }  // namespace kuka_eac
 

kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp

@@ -26,19 +26,17 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/bool.hpp"
 #include "std_msgs/msg/u_int32.hpp"
+#include "std_msgs/msg/u_int8.hpp"
 
 #include "kuka_drivers_core/controller_handler.hpp"
 #include "kuka_drivers_core/ros2_base_lc_node.hpp"
 
-#include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h"
-
 namespace kuka_eac
 {
 class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode
 {
 public:
   RobotManagerNode();
-  ~RobotManagerNode();
 
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State &) override;
@@ -53,7 +51,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode
     const rclcpp_lifecycle::State &) override;
 
 private:
-  void ObserveControl();
+  void EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg);
   bool onControlModeChangeRequest(int control_mode);
   bool onRobotModelChangeRequest(const std::string & robot_model);
 
@@ -62,32 +60,23 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode
   rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr
     change_controller_state_client_;
   rclcpp::CallbackGroup::SharedPtr cbg_;
+  rclcpp::CallbackGroup::SharedPtr event_cbg_;
   std::string robot_model_;
 
   kuka_drivers_core::ControllerHandler controller_handler_;
 
-  std::thread observe_thread_;
   std::atomic<bool> terminate_{false};
   bool param_declared_ = false;
-#ifdef NON_MOCK_SETUP
-  std::unique_ptr<kuka::ecs::v1::ExternalControlService::Stub> stub_;
-  std::unique_ptr<grpc::ClientContext> context_;
 
   std::condition_variable control_mode_cv_;
   std::mutex control_mode_cv_m_;
   bool control_mode_change_finished_;
-#endif
-
   rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr control_mode_pub_;
 
-  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>> is_configured_pub_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr is_configured_pub_;
   std_msgs::msg::Bool is_configured_msg_;
 
-  // There are two kinds of control modes with different number of necessary interfaces to be set:
-  //  - in standard modes (position, torque), only the control signal to the used interface (1)
-  //  - in impedance modes, the setpoint and the parameters describing the behaviour (2)
-  static constexpr int STANDARD_MODE_IF_SIZE = 1;
-  static constexpr int IMPEDANCE_MODE_IF_SIZE = 2;
+  rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr event_subscriber_;
 };
 
 }  // namespace kuka_eac