Add controllers to drivers repo (#156)

* merge controllers to drivers

* fix codeowners

* readd effort dep

* fix sonar

* fix sonar

* fix wiki names

* test timing

---------

Co-authored-by: Aron Svastits <svastits1@gmail.com>

.github/CODEOWNERS

@@ -1,2 +1,4 @@
 ## code changes will send PR to following users
 * @VX792
+* @Svastits
+* @kovacsge11

.github/workflows/industrial_ci.yml

@@ -9,7 +9,6 @@ on:
   push:
     branches:
       - master
-      - sunrise_driver_original
       - feature/**
       - fix/**
     paths-ignore:
@@ -36,9 +35,7 @@ jobs:
           BUILDER: colcon
           ANALYZER: sonarqube
           TEST_COVERAGE: true
-          # Hacky solution needed to be able to build upstream WS:
-          #  kuka_driver_interfaces and kuka_drivers_core must be also added to upstream
-          UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka-external-control-sdk#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver'
+          UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka-external-control-sdk#master'
           ROS_DISTRO: humble
     env:
       CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)

controllers/control_mode_handler/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.5)
+project(control_mode_handler)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(kuka_drivers_core REQUIRED)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED
+  src/control_mode_handler.cpp)
+
+target_include_directories(${PROJECT_NAME} PRIVATE
+  include
+)
+
+ament_target_dependencies(${PROJECT_NAME} controller_interface std_msgs kuka_drivers_core
+)
+
+# 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 "CONTROL_MODE_HANDLER_BUILDING_LIBRARY")
+# prevent pluginlib from using boost
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(FILES controller_plugins.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_export_include_directories(
+  include
+)
+
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+ament_package()

controllers/control_mode_handler/controller_plugins.xml

@@ -0,0 +1,7 @@
+<library path="control_mode_handler">
+  <class name="kuka_controllers/ControlModeHandler" type="kuka_controllers::ControlModeHandler" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller sets the control mode of KUKA robots in runtime
+    </description>
+  </class>
+</library>

controllers/control_mode_handler/include/control_mode_handler/control_mode_handler.hpp

@@ -0,0 +1,62 @@
+// Copyright 2022 Aron 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 CONTROL_MODE_HANDLER__CONTROL_MODE_HANDLER_HPP_
+#define CONTROL_MODE_HANDLER__CONTROL_MODE_HANDLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "kuka_drivers_core/control_mode.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/time.hpp"
+#include "std_msgs/msg/u_int32.hpp"
+
+#include "control_mode_handler/visibility_control.h"
+
+namespace kuka_controllers
+{
+class ControlModeHandler : public controller_interface::ControllerInterface
+{
+public:
+  CONTROL_MODE_HANDLER_PUBLIC controller_interface::InterfaceConfiguration
+  command_interface_configuration() const override;
+
+  CONTROL_MODE_HANDLER_PUBLIC controller_interface::InterfaceConfiguration
+  state_interface_configuration() const override;
+
+  CONTROL_MODE_HANDLER_PUBLIC controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_init() override;
+
+private:
+  rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr control_mode_subscriber_;
+  kuka_drivers_core::ControlMode control_mode_ =
+    kuka_drivers_core::ControlMode::CONTROL_MODE_UNSPECIFIED;
+};
+}  // namespace kuka_controllers
+#endif  // CONTROL_MODE_HANDLER__CONTROL_MODE_HANDLER_HPP_

controllers/control_mode_handler/include/control_mode_handler/visibility_control.h

@@ -0,0 +1,49 @@
+//    Copyright 2023 Aron 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 CONTROL_MODE_HANDLER__VISIBILITY_CONTROL_H_
+#define CONTROL_MODE_HANDLER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define CONTROL_MODE_HANDLER_EXPORT __attribute__((dllexport))
+#define CONTROL_MODE_HANDLER_IMPORT __attribute__((dllimport))
+#else
+#define CONTROL_MODE_HANDLER_EXPORT __declspec(dllexport)
+#define CONTROL_MODE_HANDLER_IMPORT __declspec(dllimport)
+#endif
+#ifdef CONTROL_MODE_HANDLER_BUILDING_LIBRARY
+#define CONTROL_MODE_HANDLER_PUBLIC CONTROL_MODE_HANDLER_EXPORT
+#else
+#define CONTROL_MODE_HANDLER_PUBLIC CONTROL_MODE_HANDLER_IMPORT
+#endif
+#define CONTROL_MODE_HANDLER_PUBLIC_TYPE CONTROL_MODE_HANDLER_PUBLIC
+#define CONTROL_MODE_HANDLER_LOCAL
+#else
+#define CONTROL_MODE_HANDLER_EXPORT __attribute__((visibility("default")))
+#define CONTROL_MODE_HANDLER_IMPORT
+#if __GNUC__ >= 4
+#define CONTROL_MODE_HANDLER_PUBLIC __attribute__((visibility("default")))
+#define CONTROL_MODE_HANDLER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define CONTROL_MODE_HANDLER_PUBLIC
+#define CONTROL_MODE_HANDLER_LOCAL
+#endif
+#define CONTROL_MODE_HANDLER_PUBLIC_TYPE
+#endif
+
+#endif  // CONTROL_MODE_HANDLER__VISIBILITY_CONTROL_H_

controllers/control_mode_handler/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>control_mode_handler</name>
+  <version>0.9.0</version>
+  <description>Controller for setting the control mode of KUKA robots in runtime</description>
+
+  <maintainer email="svastits1@gmail.com">Aron Svastits</maintainer>
+
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>kuka_drivers_core</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

controllers/control_mode_handler/src/control_mode_handler.cpp

@@ -0,0 +1,85 @@
+// Copyright 2022 Aron 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.
+
+#include "kuka_drivers_core/hardware_interface_types.hpp"
+
+#include "control_mode_handler/control_mode_handler.hpp"
+
+namespace kuka_controllers
+{
+controller_interface::CallbackReturn ControlModeHandler::on_init()
+{
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration ControlModeHandler::command_interface_configuration()
+  const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  config.names.emplace_back(
+    std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::CONTROL_MODE);
+  return config;
+}
+
+controller_interface::InterfaceConfiguration ControlModeHandler::state_interface_configuration()
+  const
+{
+  return controller_interface::InterfaceConfiguration{
+    controller_interface::interface_configuration_type::NONE};
+}
+
+controller_interface::CallbackReturn ControlModeHandler::on_configure(
+  const rclcpp_lifecycle::State &)
+{
+  // TODO(Svastits): consider server instead of simple subscription
+  control_mode_subscriber_ = get_node()->create_subscription<std_msgs::msg::UInt32>(
+    "~/control_mode", rclcpp::SystemDefaultsQoS(),
+    [this](const std_msgs::msg::UInt32::SharedPtr msg)
+    {
+      control_mode_ = kuka_drivers_core::ControlMode(msg->data);
+      RCLCPP_INFO(get_node()->get_logger(), "Control mode changed to %u", msg->data);
+    });
+  RCLCPP_INFO(get_node()->get_logger(), "Control mode handler configured");
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn ControlModeHandler::on_activate(
+  const rclcpp_lifecycle::State &)
+{
+  if (control_mode_ <= kuka_drivers_core::ControlMode::CONTROL_MODE_UNSPECIFIED)
+  {
+    throw std::runtime_error("Control mode unspecified");
+  }
+
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn ControlModeHandler::on_deactivate(
+  const rclcpp_lifecycle::State &)
+{
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type ControlModeHandler::update(
+  const rclcpp::Time &, const rclcpp::Duration &)
+{
+  command_interfaces_[0].set_value(static_cast<double>(control_mode_));
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace kuka_controllers
+
+PLUGINLIB_EXPORT_CLASS(
+  kuka_controllers::ControlModeHandler, controller_interface::ControllerInterface)

controllers/event_broadcaster/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.5)
+project(event_broadcaster)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(kuka_drivers_core REQUIRED)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED
+  src/event_broadcaster.cpp)
+
+target_include_directories(${PROJECT_NAME} PRIVATE
+  include
+)
+
+ament_target_dependencies(${PROJECT_NAME} controller_interface kuka_drivers_core
+)
+
+# 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 "EVENT_BROADCASTER_BUILDING_LIBRARY")
+# prevent pluginlib from using boost
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(FILES controller_plugins.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_export_include_directories(
+  include
+)
+
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+ament_package()

controllers/event_broadcaster/controller_plugins.xml

@@ -0,0 +1,7 @@
+<library path="event_broadcaster">
+  <class name="kuka_controllers/EventBroadcaster" type="kuka_controllers::EventBroadcaster" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This broadcaster publishes the state changes of ECI
+    </description>
+  </class>
+</library>