Skip to content

Commit

Permalink
Integrate kuka-external-control-sdk (#144)
Browse files Browse the repository at this point in the history
* use client lib in hwif-without control mode switch

* add control mode change

* add event subscriber to manager

* change type

* format

* add enums

* hack for LD_LIBRARY_PATH

* fake hw in activation test

* set server event

* recv timeout

* clean up

* fix control mode changes

* log fix

* log

* rollback launch

* modify access rights

* format

* recv timeout

* prev_control_mode type

* fix build

* mutex

* cleanup

* package xml update + ci

* sudo mkdir

* fix

* git

* pipe fix

* sudo

* home

* fix dir

* update name

* fix dir structure

* fix dir

* update env

* fix deactivate

* find_lib

* fix build

* debug

* try without path

* Revert "try without path"

This reverts commit a80ab26.

* fix multiple deact

* cleanup

* fix controller  configuration

* update build instructions

* fix

* remove unnecessary copy

* fix switch

* package xml

* install deps

* format

* try pre-build script

* remove sdk dep

* fix upstream

* sudo

* clean up control mode handling

* fix before_build

* try path fix

* before init

* install deps

* fix install path

* protobuf dep

* fix dep

* fix condition

* target include

* fix hack

* remove activation test

* spell

* fix tests with new config

* extend ld_lib_path

* fix

* syntax

* try extend again

* fix hack

* fix hack

* debug

* debug2

* Update industrial_ci.yml

* Remove after init

* debug

* fix env

* cleanup

* better driver init

* extend doc with torque difference

* remove mock from doc

* cleanup

* fix log

* spell

* typo

---------

Co-authored-by: Szitanics Márk <szitanics@gmail.com>
Co-authored-by: Aron Svastits <svastits1@gmail.com>
Co-authored-by: Gergely Kovacs <gergely.kovacs4@kuka.com>
  • Loading branch information
4 people authored Feb 28, 2024
1 parent 7ec32c8 commit 7506c7c
Show file tree
Hide file tree
Showing 55 changed files with 475 additions and 4,039 deletions.
3 changes: 3 additions & 0 deletions .github/workflows/industrial_ci.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 }}
13 changes: 13 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion doc/wiki/KSS_RSI.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion doc/wiki/Sunrise_FRI.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
6 changes: 3 additions & 3 deletions doc/wiki/iiQKA_EAC.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -126,6 +126,8 @@ class ControllerHandler
*
*/
bool ApproveControllerDeactivation();

std::vector<std::string> GetControllersForMode(ControlMode control_mode);
};
} // namespace kuka_drivers_core

Expand Down
35 changes: 35 additions & 0 deletions kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp
Original file line number Diff line number Diff line change
@@ -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_
16 changes: 15 additions & 1 deletion kuka_drivers_core/src/controller_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}

Expand Down Expand Up @@ -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
58 changes: 15 additions & 43 deletions kuka_iiqka_eac_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)

Expand All @@ -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()
4 changes: 2 additions & 2 deletions kuka_iiqka_eac_driver/config/ros2_controller_config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Original file line number Diff line number Diff line change
@@ -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_
Loading

0 comments on commit 7506c7c

Please sign in to comment.