diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d0f9cfdc..4e5c6cf7 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,2 +1,4 @@ ## code changes will send PR to following users * @VX792 +* @Svastits +* @kovacsge11 diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 1f6f3193..5ccbf26e 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -9,7 +9,6 @@ on: push: branches: - master - - sunrise_driver_original - feature/** - fix/** paths-ignore: @@ -29,20 +28,14 @@ on: jobs: industrial_ci: name: ROS-Industrial CI - strategy: - matrix: - env: - - ROS_REPO: ros - 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_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' - ROS_DISTRO: humble env: + ROS_REPO: ros + BUILDER: colcon + ANALYZER: sonarqube + TEST_COVERAGE: true + UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka-external-control-sdk#master' + ROS_DISTRO: humble 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 }} @@ -50,9 +43,13 @@ jobs: 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: + - name: Reset ANALYZER for scheduled and push runs + run: | + if [[ "${{ github.event_name }}" == "schedule" ]] || { [[ "${{ github.event_name }}" == "push" ]] && [[ "${{ github.ref }}" != "refs/heads/master" ]]; }; then + echo "ANALYZER=" >> $GITHUB_ENV + fi - uses: actions/checkout@v2 with: fetch-depth: 0 @@ -64,4 +61,3 @@ jobs: # Run industrial_ci - uses: 'kroshu/industrial_ci@master' - env: ${{ matrix.env }} diff --git a/README.md b/README.md index 979d59d6..54346fd8 100644 --- a/README.md +++ b/README.md @@ -40,26 +40,13 @@ 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. +Build all packages in workspace. ```bash cd ~/ros2_ws colcon build ``` -Source built KUKA packages. +Source workspace. ```bash # Replace ".bash" with your shell if you're not using bash # Possible values are: setup.bash, setup.sh, setup.zsh diff --git a/controllers/control_mode_handler/CMakeLists.txt b/controllers/control_mode_handler/CMakeLists.txt new file mode 100644 index 00000000..de115c17 --- /dev/null +++ b/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() diff --git a/controllers/control_mode_handler/controller_plugins.xml b/controllers/control_mode_handler/controller_plugins.xml new file mode 100644 index 00000000..9559360a --- /dev/null +++ b/controllers/control_mode_handler/controller_plugins.xml @@ -0,0 +1,7 @@ + + + + This controller sets the control mode of KUKA robots in runtime + + + diff --git a/controllers/control_mode_handler/include/control_mode_handler/control_mode_handler.hpp b/controllers/control_mode_handler/include/control_mode_handler/control_mode_handler.hpp new file mode 100644 index 00000000..3fa559c8 --- /dev/null +++ b/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 +#include +#include + +#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::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_ diff --git a/controllers/control_mode_handler/include/control_mode_handler/visibility_control.h b/controllers/control_mode_handler/include/control_mode_handler/visibility_control.h new file mode 100644 index 00000000..2db1ce12 --- /dev/null +++ b/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_ diff --git a/controllers/control_mode_handler/package.xml b/controllers/control_mode_handler/package.xml new file mode 100644 index 00000000..558b1882 --- /dev/null +++ b/controllers/control_mode_handler/package.xml @@ -0,0 +1,21 @@ + + + + control_mode_handler + 0.9.0 + Controller for setting the control mode of KUKA robots in runtime + + Aron Svastits + + Apache-2.0 + + ament_cmake + + controller_interface + pluginlib + kuka_drivers_core + + + ament_cmake + + diff --git a/controllers/control_mode_handler/src/control_mode_handler.cpp b/controllers/control_mode_handler/src/control_mode_handler.cpp new file mode 100644 index 00000000..16ea448a --- /dev/null +++ b/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( + "~/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(control_mode_)); + return controller_interface::return_type::OK; +} + +} // namespace kuka_controllers + +PLUGINLIB_EXPORT_CLASS( + kuka_controllers::ControlModeHandler, controller_interface::ControllerInterface) diff --git a/controllers/event_broadcaster/CMakeLists.txt b/controllers/event_broadcaster/CMakeLists.txt new file mode 100644 index 00000000..b2a86670 --- /dev/null +++ b/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() diff --git a/controllers/event_broadcaster/controller_plugins.xml b/controllers/event_broadcaster/controller_plugins.xml new file mode 100644 index 00000000..66105bdc --- /dev/null +++ b/controllers/event_broadcaster/controller_plugins.xml @@ -0,0 +1,7 @@ + + + + This broadcaster publishes the state changes of ECI + + + diff --git a/controllers/event_broadcaster/include/event_broadcaster/event_broadcaster.hpp b/controllers/event_broadcaster/include/event_broadcaster/event_broadcaster.hpp new file mode 100644 index 00000000..507538ca --- /dev/null +++ b/controllers/event_broadcaster/include/event_broadcaster/event_broadcaster.hpp @@ -0,0 +1,61 @@ +// Copyright 2024 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 EVENT_BROADCASTER__EVENT_BROADCASTER_HPP_ +#define EVENT_BROADCASTER__EVENT_BROADCASTER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" +#include "std_msgs/msg/u_int8.hpp" + +#include "event_broadcaster/visibility_control.h" + +namespace kuka_controllers +{ +class EventBroadcaster : public controller_interface::ControllerInterface +{ +public: + EVENT_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + EVENT_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + EVENT_BROADCASTER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + EVENT_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + EVENT_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + EVENT_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + EVENT_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + +private: + rclcpp::Publisher::SharedPtr event_publisher_; + std_msgs::msg::UInt8 event_msg_; + int last_event_ = 0; +}; +} // namespace kuka_controllers +#endif // EVENT_BROADCASTER__EVENT_BROADCASTER_HPP_ diff --git a/controllers/event_broadcaster/include/event_broadcaster/visibility_control.h b/controllers/event_broadcaster/include/event_broadcaster/visibility_control.h new file mode 100644 index 00000000..8374e754 --- /dev/null +++ b/controllers/event_broadcaster/include/event_broadcaster/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2024 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 EVENT_BROADCASTER__VISIBILITY_CONTROL_H_ +#define EVENT_BROADCASTER__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 EVENT_BROADCASTER_EXPORT __attribute__((dllexport)) +#define EVENT_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define EVENT_BROADCASTER_EXPORT __declspec(dllexport) +#define EVENT_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef EVENT_BROADCASTER_BUILDING_LIBRARY +#define EVENT_BROADCASTER_PUBLIC EVENT_BROADCASTER_EXPORT +#else +#define EVENT_BROADCASTER_PUBLIC EVENT_BROADCASTER_IMPORT +#endif +#define EVENT_BROADCASTER_PUBLIC_TYPE EVENT_BROADCASTER_PUBLIC +#define EVENT_BROADCASTER_LOCAL +#else +#define EVENT_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define EVENT_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define EVENT_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define EVENT_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define EVENT_BROADCASTER_PUBLIC +#define EVENT_BROADCASTER_LOCAL +#endif +#define EVENT_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // EVENT_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/controllers/event_broadcaster/package.xml b/controllers/event_broadcaster/package.xml new file mode 100644 index 00000000..e851fe48 --- /dev/null +++ b/controllers/event_broadcaster/package.xml @@ -0,0 +1,21 @@ + + + + event_broadcaster + 0.9.0 + Broadcaster of hardware events of KUKA robots + + Aron Svastits + + Apache-2.0 + + ament_cmake + + controller_interface + pluginlib + kuka_drivers_core + + + ament_cmake + + diff --git a/controllers/event_broadcaster/src/event_broadcaster.cpp b/controllers/event_broadcaster/src/event_broadcaster.cpp new file mode 100644 index 00000000..34af4422 --- /dev/null +++ b/controllers/event_broadcaster/src/event_broadcaster.cpp @@ -0,0 +1,75 @@ +// Copyright 2024 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 "event_broadcaster/event_broadcaster.hpp" + +namespace kuka_controllers +{ +controller_interface::CallbackReturn EventBroadcaster::on_init() +{ + event_publisher_ = get_node()->create_publisher( + "~/hardware_event", rclcpp::SystemDefaultsQoS()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration EventBroadcaster::command_interface_configuration() + const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration EventBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.emplace_back( + std::string(hardware_interface::STATE_PREFIX) + "/" + hardware_interface::SERVER_STATE); + return config; +} + +controller_interface::CallbackReturn EventBroadcaster::on_configure(const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn EventBroadcaster::on_activate(const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn EventBroadcaster::on_deactivate( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type EventBroadcaster::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + auto current_event = static_cast(state_interfaces_[0].get_value()); + if (current_event != last_event_) + { + event_msg_.data = current_event; + event_publisher_->publish(event_msg_); + last_event_ = current_event; + } + return controller_interface::return_type::OK; +} +} // namespace kuka_controllers + +PLUGINLIB_EXPORT_CLASS( + kuka_controllers::EventBroadcaster, controller_interface::ControllerInterface) diff --git a/controllers/fri_configuration_controller/CMakeLists.txt b/controllers/fri_configuration_controller/CMakeLists.txt new file mode 100644 index 00000000..be847e90 --- /dev/null +++ b/controllers/fri_configuration_controller/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.5) +project(fri_configuration_controller) + +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_driver_interfaces REQUIRED) +find_package(kuka_drivers_core REQUIRED) + +include_directories(include) + +add_library(${PROJECT_NAME} SHARED + src/fri_configuration_controller.cpp) + +target_include_directories(${PROJECT_NAME} PRIVATE + include +) + +ament_target_dependencies(${PROJECT_NAME} controller_interface kuka_driver_interfaces 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 "FRI_CONFIGURATION_CONTROLLER_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() diff --git a/controllers/fri_configuration_controller/controller_plugins.xml b/controllers/fri_configuration_controller/controller_plugins.xml new file mode 100644 index 00000000..ef6d9c03 --- /dev/null +++ b/controllers/fri_configuration_controller/controller_plugins.xml @@ -0,0 +1,7 @@ + + + + This controller sets the receive multiplier of the hardware interface + + + diff --git a/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp b/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp new file mode 100644 index 00000000..c33058fe --- /dev/null +++ b/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp @@ -0,0 +1,61 @@ +// 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 FRI_CONFIGURATION_CONTROLLER__FRI_CONFIGURATION_CONTROLLER_HPP_ +#define FRI_CONFIGURATION_CONTROLLER__FRI_CONFIGURATION_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "kuka_driver_interfaces/srv/set_int.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" + +#include "fri_configuration_controller/visibility_control.h" + +namespace kuka_controllers +{ +class FRIConfigurationController : public controller_interface::ControllerInterface +{ +public: + FRI_CONFIGURATION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + FRI_CONFIGURATION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; + +private: + rclcpp::Service::SharedPtr receive_multiplier_service_; + int receive_multiplier_ = 1; + bool resend_multiplier_ = false; +}; +} // namespace kuka_controllers +#endif // FRI_CONFIGURATION_CONTROLLER__FRI_CONFIGURATION_CONTROLLER_HPP_ diff --git a/controllers/fri_configuration_controller/include/fri_configuration_controller/visibility_control.h b/controllers/fri_configuration_controller/include/fri_configuration_controller/visibility_control.h new file mode 100644 index 00000000..9368a675 --- /dev/null +++ b/controllers/fri_configuration_controller/include/fri_configuration_controller/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 FRI_CONFIGURATION_CONTROLLER__VISIBILITY_CONTROL_H_ +#define FRI_CONFIGURATION_CONTROLLER__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 FRI_CONFIGURATION_CONTROLLER_EXPORT __attribute__((dllexport)) +#define FRI_CONFIGURATION_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define FRI_CONFIGURATION_CONTROLLER_EXPORT __declspec(dllexport) +#define FRI_CONFIGURATION_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef FRI_CONFIGURATION_CONTROLLER_BUILDING_LIBRARY +#define FRI_CONFIGURATION_CONTROLLER_PUBLIC FRI_CONFIGURATION_CONTROLLER_EXPORT +#else +#define FRI_CONFIGURATION_CONTROLLER_PUBLIC FRI_CONFIGURATION_CONTROLLER_IMPORT +#endif +#define FRI_CONFIGURATION_CONTROLLER_PUBLIC_TYPE FRI_CONFIGURATION_CONTROLLER_PUBLIC +#define FRI_CONFIGURATION_CONTROLLER_LOCAL +#else +#define FRI_CONFIGURATION_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define FRI_CONFIGURATION_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define FRI_CONFIGURATION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define FRI_CONFIGURATION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define FRI_CONFIGURATION_CONTROLLER_PUBLIC +#define FRI_CONFIGURATION_CONTROLLER_LOCAL +#endif +#define FRI_CONFIGURATION_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // FRI_CONFIGURATION_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/controllers/fri_configuration_controller/package.xml b/controllers/fri_configuration_controller/package.xml new file mode 100644 index 00000000..87a4d8fd --- /dev/null +++ b/controllers/fri_configuration_controller/package.xml @@ -0,0 +1,22 @@ + + + + fri_configuration_controller + 0.9.0 + Controller for configuration of FRI + + Aron Svastits + + Apache-2.0 + + ament_cmake + + controller_interface + pluginlib + kuka_driver_interfaces + kuka_drivers_core + + + ament_cmake + + diff --git a/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp b/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp new file mode 100644 index 00000000..91144568 --- /dev/null +++ b/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp @@ -0,0 +1,92 @@ +// 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 "fri_configuration_controller/fri_configuration_controller.hpp" + +namespace kuka_controllers +{ +controller_interface::CallbackReturn FRIConfigurationController::on_init() +{ + auto callback = [this]( + kuka_driver_interfaces::srv::SetInt::Request::SharedPtr request, + kuka_driver_interfaces::srv::SetInt::Response::SharedPtr response) + { + resend_multiplier_ = true; + receive_multiplier_ = request->data; + response->success = true; + }; + receive_multiplier_service_ = get_node()->create_service( + "~/set_receive_multiplier", callback); + // TODO(Svastits): create service to get multiplier changes (or perpaps + // parameter??) + // and set resend_multiplier_ to true in the callback + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +FRIConfigurationController::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::RECEIVE_MULTIPLIER); + return config; +} + +controller_interface::InterfaceConfiguration +FRIConfigurationController::state_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn FRIConfigurationController::on_configure( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn FRIConfigurationController::on_activate( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn FRIConfigurationController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type FRIConfigurationController::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + // TODO(Svastits): disable changes if HWIF is active + if (resend_multiplier_) + { + RCLCPP_INFO( + get_node()->get_logger(), "Changing receive multiplier of hardware interface to %i", + receive_multiplier_); + command_interfaces_[0].set_value(receive_multiplier_); + resend_multiplier_ = false; + } + return controller_interface::return_type::OK; +} + +} // namespace kuka_controllers + +PLUGINLIB_EXPORT_CLASS( + kuka_controllers::FRIConfigurationController, controller_interface::ControllerInterface) diff --git a/controllers/fri_state_broadcaster/CMakeLists.txt b/controllers/fri_state_broadcaster/CMakeLists.txt new file mode 100644 index 00000000..7fb5f76e --- /dev/null +++ b/controllers/fri_state_broadcaster/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.5) +project(fri_state_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_driver_interfaces REQUIRED) +find_package(kuka_drivers_core REQUIRED) + +include_directories(include) + +add_library(${PROJECT_NAME} SHARED + src/fri_state_broadcaster.cpp) + +target_include_directories(${PROJECT_NAME} PRIVATE + include +) + +ament_target_dependencies(${PROJECT_NAME} controller_interface kuka_driver_interfaces 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 "FRI_STATE_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() diff --git a/controllers/fri_state_broadcaster/controller_plugins.xml b/controllers/fri_state_broadcaster/controller_plugins.xml new file mode 100644 index 00000000..2de0987c --- /dev/null +++ b/controllers/fri_state_broadcaster/controller_plugins.xml @@ -0,0 +1,7 @@ + + + + This broadcaster publishes the state changes of ECI + + + diff --git a/controllers/fri_state_broadcaster/include/fri_state_broadcaster/fri_state_broadcaster.hpp b/controllers/fri_state_broadcaster/include/fri_state_broadcaster/fri_state_broadcaster.hpp new file mode 100644 index 00000000..e6370fba --- /dev/null +++ b/controllers/fri_state_broadcaster/include/fri_state_broadcaster/fri_state_broadcaster.hpp @@ -0,0 +1,61 @@ +// 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 FRI_STATE_BROADCASTER__FRI_STATE_BROADCASTER_HPP_ +#define FRI_STATE_BROADCASTER__FRI_STATE_BROADCASTER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "kuka_driver_interfaces/msg/fri_state.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" + +#include "fri_state_broadcaster/visibility_control.h" + +namespace kuka_controllers +{ +class FRIStateBroadcaster : public controller_interface::ControllerInterface +{ +public: + FRI_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + FRI_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + FRI_STATE_BROADCASTER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + FRI_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FRI_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FRI_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + FRI_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + +private: + int counter_ = 0; + rclcpp::Publisher::SharedPtr robot_state_publisher_; + kuka_driver_interfaces::msg::FRIState state_msg_; +}; +} // namespace kuka_controllers +#endif // FRI_STATE_BROADCASTER__FRI_STATE_BROADCASTER_HPP_ diff --git a/controllers/fri_state_broadcaster/include/fri_state_broadcaster/visibility_control.h b/controllers/fri_state_broadcaster/include/fri_state_broadcaster/visibility_control.h new file mode 100644 index 00000000..24e4ccb5 --- /dev/null +++ b/controllers/fri_state_broadcaster/include/fri_state_broadcaster/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 FRI_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ +#define FRI_STATE_BROADCASTER__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 FRI_STATE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define FRI_STATE_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define FRI_STATE_BROADCASTER_EXPORT __declspec(dllexport) +#define FRI_STATE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef FRI_STATE_BROADCASTER_BUILDING_LIBRARY +#define FRI_STATE_BROADCASTER_PUBLIC FRI_STATE_BROADCASTER_EXPORT +#else +#define FRI_STATE_BROADCASTER_PUBLIC FRI_STATE_BROADCASTER_IMPORT +#endif +#define FRI_STATE_BROADCASTER_PUBLIC_TYPE FRI_STATE_BROADCASTER_PUBLIC +#define FRI_STATE_BROADCASTER_LOCAL +#else +#define FRI_STATE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define FRI_STATE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define FRI_STATE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define FRI_STATE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define FRI_STATE_BROADCASTER_PUBLIC +#define FRI_STATE_BROADCASTER_LOCAL +#endif +#define FRI_STATE_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // FRI_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/controllers/fri_state_broadcaster/package.xml b/controllers/fri_state_broadcaster/package.xml new file mode 100644 index 00000000..1450c78c --- /dev/null +++ b/controllers/fri_state_broadcaster/package.xml @@ -0,0 +1,22 @@ + + + + fri_state_broadcaster + 0.9.0 + Broadcaster for FRI state + + Aron Svastits + + Apache-2.0 + + ament_cmake + + controller_interface + pluginlib + kuka_driver_interfaces + kuka_drivers_core + + + ament_cmake + + diff --git a/controllers/fri_state_broadcaster/src/fri_state_broadcaster.cpp b/controllers/fri_state_broadcaster/src/fri_state_broadcaster.cpp new file mode 100644 index 00000000..1719cbbd --- /dev/null +++ b/controllers/fri_state_broadcaster/src/fri_state_broadcaster.cpp @@ -0,0 +1,106 @@ +// 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 "fri_state_broadcaster/fri_state_broadcaster.hpp" + +namespace kuka_controllers +{ +controller_interface::CallbackReturn FRIStateBroadcaster::on_init() +{ + robot_state_publisher_ = get_node()->create_publisher( + "~/fri_state", rclcpp::SystemDefaultsQoS()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration FRIStateBroadcaster::command_interface_configuration() + const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration FRIStateBroadcaster::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + hardware_interface::SESSION_STATE); + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + + hardware_interface::CONNECTION_QUALITY); + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + hardware_interface::SAFETY_STATE); + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + hardware_interface::COMMAND_MODE); + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + hardware_interface::CONTROL_MODE); + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + hardware_interface::OPERATION_MODE); + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + hardware_interface::DRIVE_STATE); + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + hardware_interface::OVERLAY_TYPE); + config.names.emplace_back( + std::string(hardware_interface::FRI_STATE_PREFIX) + "/" + + hardware_interface::TRACKING_PERFORMANCE); + return config; +} + +controller_interface::CallbackReturn FRIStateBroadcaster::on_configure( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn FRIStateBroadcaster::on_activate( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn FRIStateBroadcaster::on_deactivate( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type FRIStateBroadcaster::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + state_msg_.session_state = static_cast(state_interfaces_[0].get_value()); + state_msg_.connection_quality = static_cast(state_interfaces_[1].get_value()); + state_msg_.safety_state = static_cast(state_interfaces_[2].get_value()); + state_msg_.command_mode = static_cast(state_interfaces_[3].get_value()); + state_msg_.control_mode = static_cast(state_interfaces_[4].get_value()); + state_msg_.operation_mode = static_cast(state_interfaces_[5].get_value()); + state_msg_.drive_state = static_cast(state_interfaces_[6].get_value()); + state_msg_.overlay_type = static_cast(state_interfaces_[7].get_value()); + state_msg_.tracking_performance = state_interfaces_[8].get_value(); + + if (counter_++ == 10) + { + robot_state_publisher_->publish(state_msg_); + counter_ = 0; + } + + return controller_interface::return_type::OK; +} + +} // namespace kuka_controllers + +PLUGINLIB_EXPORT_CLASS( + kuka_controllers::FRIStateBroadcaster, controller_interface::ControllerInterface) diff --git a/controllers/joint_group_impedance_controller/CMakeLists.txt b/controllers/joint_group_impedance_controller/CMakeLists.txt new file mode 100644 index 00000000..9b13e39b --- /dev/null +++ b/controllers/joint_group_impedance_controller/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.5) +project(joint_group_impedance_controller) + +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(pluginlib REQUIRED) +find_package(forward_command_controller) +find_package(kuka_drivers_core) +find_package(generate_parameter_library) + +include_directories(include) + +generate_parameter_library( + joint_group_impedance_controller_parameters + src/joint_group_impedance_controller_parameters.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/joint_group_impedance_controller.cpp) + +target_include_directories(${PROJECT_NAME} PRIVATE + include +) + +ament_target_dependencies(${PROJECT_NAME} forward_command_controller kuka_drivers_core +) +target_link_libraries(${PROJECT_NAME} joint_group_impedance_controller_parameters) + +# 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 "JOINT_GROUP_IMPEDANCE_CONTROLLER_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() diff --git a/controllers/joint_group_impedance_controller/controller_plugins.xml b/controllers/joint_group_impedance_controller/controller_plugins.xml new file mode 100644 index 00000000..fb026282 --- /dev/null +++ b/controllers/joint_group_impedance_controller/controller_plugins.xml @@ -0,0 +1,7 @@ + + + + This controller sets the joint impedance parameters in real time + + + diff --git a/controllers/joint_group_impedance_controller/include/joint_group_impedance_controller/joint_group_impedance_controller.hpp b/controllers/joint_group_impedance_controller/include/joint_group_impedance_controller/joint_group_impedance_controller.hpp new file mode 100644 index 00000000..1f8cd730 --- /dev/null +++ b/controllers/joint_group_impedance_controller/include/joint_group_impedance_controller/joint_group_impedance_controller.hpp @@ -0,0 +1,47 @@ +// 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 JOINT_GROUP_IMPEDANCE_CONTROLLER__JOINT_GROUP_IMPEDANCE_CONTROLLER_HPP_ +#define JOINT_GROUP_IMPEDANCE_CONTROLLER__JOINT_GROUP_IMPEDANCE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "forward_command_controller/multi_interface_forward_command_controller.hpp" + +#include "joint_group_impedance_controller/visibility_control.h" +#include "joint_group_impedance_controller_parameters.hpp" + +namespace kuka_controllers +{ +class JointGroupImpedanceController : public forward_command_controller::ForwardControllersBase +{ +public: + JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC JointGroupImpedanceController(); + JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; + +private: + JOINT_GROUP_IMPEDANCE_CONTROLLER_LOCAL void declare_parameters() override; + JOINT_GROUP_IMPEDANCE_CONTROLLER_LOCAL controller_interface::CallbackReturn read_parameters() + override; + + using Params = joint_group_impedance_controller::Params; + using ParamListener = joint_group_impedance_controller::ParamListener; + + std::shared_ptr param_listener_; + Params params_; +}; +} // namespace kuka_controllers +#endif // JOINT_GROUP_IMPEDANCE_CONTROLLER__JOINT_GROUP_IMPEDANCE_CONTROLLER_HPP_ diff --git a/controllers/joint_group_impedance_controller/include/joint_group_impedance_controller/visibility_control.h b/controllers/joint_group_impedance_controller/include/joint_group_impedance_controller/visibility_control.h new file mode 100644 index 00000000..3e0645fb --- /dev/null +++ b/controllers/joint_group_impedance_controller/include/joint_group_impedance_controller/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 JOINT_GROUP_IMPEDANCE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define JOINT_GROUP_IMPEDANCE_CONTROLLER__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 JOINT_GROUP_IMPEDANCE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_EXPORT __declspec(dllexport) +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_GROUP_IMPEDANCE_CONTROLLER_BUILDING_LIBRARY +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC JOINT_GROUP_IMPEDANCE_CONTROLLER_EXPORT +#else +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC JOINT_GROUP_IMPEDANCE_CONTROLLER_IMPORT +#endif +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC_TYPE JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_LOCAL +#else +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_LOCAL +#endif +#define JOINT_GROUP_IMPEDANCE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // JOINT_GROUP_IMPEDANCE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/controllers/joint_group_impedance_controller/package.xml b/controllers/joint_group_impedance_controller/package.xml new file mode 100644 index 00000000..86bb58e0 --- /dev/null +++ b/controllers/joint_group_impedance_controller/package.xml @@ -0,0 +1,22 @@ + + + + joint_group_impedance_controller + 0.9.0 + Controller for modifying impedance (stiffness and damping) interfaces of a joint group + + Aron Svastits + + Apache-2.0 + + ament_cmake + + forward_command_controller + pluginlib + kuka_drivers_core + generate_parameter_library + + + ament_cmake + + diff --git a/controllers/joint_group_impedance_controller/src/joint_group_impedance_controller.cpp b/controllers/joint_group_impedance_controller/src/joint_group_impedance_controller.cpp new file mode 100644 index 00000000..d6034431 --- /dev/null +++ b/controllers/joint_group_impedance_controller/src/joint_group_impedance_controller.cpp @@ -0,0 +1,91 @@ +// 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 "pluginlib/class_list_macros.hpp" + +#include "kuka_drivers_core/hardware_interface_types.hpp" + +#include "joint_group_impedance_controller/joint_group_impedance_controller.hpp" + +namespace kuka_controllers +{ + +JointGroupImpedanceController::JointGroupImpedanceController() : ForwardControllersBase() {} + +void JointGroupImpedanceController::declare_parameters() +{ + param_listener_ = std::make_shared(get_node()); +} + +controller_interface::CallbackReturn JointGroupImpedanceController::read_parameters() +{ + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + if (params_.joints.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.interface_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & joint : params_.joints) + { + for (const auto & interface : params_.interface_names) + { + command_interface_types_.push_back(joint + "/" + interface); + } + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn JointGroupImpedanceController::on_init() +{ + auto ret = ForwardControllersBase::on_init(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + try + { + // Explicitly set the interfaces parameter declared by the + // forward_command_controller + get_node()->set_parameter(rclcpp::Parameter( + "interface_names", + std::vector{ + hardware_interface::HW_IF_STIFFNESS, hardware_interface::HW_IF_DAMPING})); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} +} // namespace kuka_controllers + +PLUGINLIB_EXPORT_CLASS( + kuka_controllers::JointGroupImpedanceController, controller_interface::ControllerInterface) diff --git a/controllers/joint_group_impedance_controller/src/joint_group_impedance_controller_parameters.yaml b/controllers/joint_group_impedance_controller/src/joint_group_impedance_controller_parameters.yaml new file mode 100644 index 00000000..626fc72d --- /dev/null +++ b/controllers/joint_group_impedance_controller/src/joint_group_impedance_controller_parameters.yaml @@ -0,0 +1,11 @@ +joint_group_impedance_controller: + joints: { + type: string_array, + default_value: [], + description: "Name of the joints to control", + } + interface_names: { + type: string_array, + default_value: [], + description: "Names of the interfaces to command", + } diff --git a/controllers/kuka_controllers/CMakeLists.txt b/controllers/kuka_controllers/CMakeLists.txt new file mode 100644 index 00000000..71bcebad --- /dev/null +++ b/controllers/kuka_controllers/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.16) +project(kuka_controllers LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/controllers/kuka_controllers/package.xml b/controllers/kuka_controllers/package.xml new file mode 100644 index 00000000..7d6112f9 --- /dev/null +++ b/controllers/kuka_controllers/package.xml @@ -0,0 +1,20 @@ + + + kuka_controllers + 0.9.0 + ROS2 controllers for KUKA robots + Aron Svastits + Apache-2.0 + + ament_cmake + + control_mode_handler + event_broadcaster + fri_configuration_controller + fri_state_broadcaster + joint_group_impedance_controller + + + ament_cmake + + diff --git a/doc/wiki/iiQKA_EAC.md b/doc/wiki/1_iiQKA_EAC.md similarity index 91% rename from doc/wiki/iiQKA_EAC.md rename to doc/wiki/1_iiQKA_EAC.md index 399c7cc7..f830478f 100644 --- a/doc/wiki/iiQKA_EAC.md +++ b/doc/wiki/1_iiQKA_EAC.md @@ -3,12 +3,13 @@ ### Setup #### 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)). +- 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/5_Realtime)). - Set a fixed IP in the subnet of the KONI interface for the real-time machine. #### Controller side - Install ExternalAPI.Control toolbox (requires iiQKA 1.2) + - The toolbox is not available by default and should be requested from KUKA Customer Support - Set KONI IP (System Settings -> Network -> KONI) + restart - Connect external client to KONI port (XF7) - Release SPOC on the Teach Pendant @@ -53,7 +54,7 @@ This starts the 3 core components of every driver (described in the [Non-real-ti - `joint_trajectory_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml)) - `joint_group_impedance_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml)) - `effort_controller` (of type `JointGroupPositionController`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_iiqka_eac_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) +- [`control_mode_handler`](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#control_mode_handler) (no configuration file) 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`): ``` @@ -61,7 +62,7 @@ After successful startup, the `robot_manager` node has to be activated to start ros2 lifecycle set robot_manager activate ``` -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)). +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](https://github.com/kroshu/kuka_drivers/wiki#additional-packages) section of the project overview). 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). @@ -76,6 +77,7 @@ Both launch files support the following arguments: - `namespace`: adds a namespace to all nodes and controllers of the driver, and modifies the `prefix` argument of the robot description macro to `namespace_` - `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame (default: [0, 0, 0]) - `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame (default: [0, 0, 0]) +- `roundtrip_time`: The roundtrip time (in microseconds) to be enforced by the [KUKA mock hardware interface](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#custom-mock-hardware), (defaults to 2500 us, only used if `use_fake_hardware` is true) - `qos_config`: the location of the QoS configuration file (defaults to `kuka_iiqka_eac_driver/config/qos_config.yaml`) - `controller_config`: the location of the `ros2_control` configuration file (defaults to `kuka_iiqka_eac_driver/config/ros2_controller_config.yaml`) - `jtc_config`: the location of the configuration file for the `joint_trajectory_controller` (defaults to `kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml`) diff --git a/doc/wiki/KSS_RSI.md b/doc/wiki/2_KSS_RSI.md similarity index 95% rename from doc/wiki/KSS_RSI.md rename to doc/wiki/2_KSS_RSI.md index 51bb886e..02c5b58b 100644 --- a/doc/wiki/KSS_RSI.md +++ b/doc/wiki/2_KSS_RSI.md @@ -3,7 +3,7 @@ ### Setup #### 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)). +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/5_Realtime)). To set up the controller with WorkVisual (which is necessary if RSI is not yet installed), a Windows machine is also required. @@ -103,7 +103,7 @@ The IP address of the client machine must be provided as a launch argument. For 3. Start the `KRC:\R1\Program\ros_rsi.src` program on the controller and execute the line of `RSI_MOVECORR()` - in T1, a warning (*!!! Attention - Sensor correction goes active !!!*) should be visible after reaching `RSI_MOVECORR()`, which should be confirmed to start this step -On successful activation the brakes of the robot will be released and external control is started. 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)). +On successful activation the brakes of the robot will be released and external control is started. 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](https://github.com/kroshu/kuka_drivers/wiki#additional-packages) section of the project overview). ##### Launch arguments @@ -116,6 +116,7 @@ Both launch files support the following arguments: - `namespace`: adds a namespace to all nodes and controllers of the driver, and modifies the `prefix` argument of the robot description macro to `namespace_` - `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame (default: [0, 0, 0]) - `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame (default: [0, 0, 0]) +- `roundtrip_time`: The roundtrip time (in microseconds) to be enforced by the [KUKA mock hardware interface](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#custom-mock-hardware), (defaults to 4000 us, only used if `use_fake_hardware` is true) - `controller_config`: the location of the `ros2_control` configuration file (defaults to `kuka_kss_rsi_driver/config/ros2_controller_config.yaml`) - `jtc_config`: the location of the configuration file for the `joint_trajectory_controller` (defaults to `kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml`) diff --git a/doc/wiki/3_Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md new file mode 100644 index 00000000..dc3d0f3f --- /dev/null +++ b/doc/wiki/3_Sunrise_FRI.md @@ -0,0 +1,108 @@ +## Sunrise driver (FRI) + +### Setup + +#### 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/5_Realtime)). +- Set a fixed IP in the subnet of the controller for the real-time machine. + +#### Controller side + +- Upload the robot application under `robot_application/src` to the controller using Sunrise Workbench + +### Configuration + +#### Startup configuration + +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. +- 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 + +##### IP configuration +The IP address of robot controller must be provided as a launch argument. For further information see section [launch arguments](#launch-arguments). + +#### Runtime parameters +The parameters in the driver configuration file can be also changed during runtime using the parameter interface of the `robot_manager` node: +- `send_period_ms` (integer): this parameter defines the send rate in milliseconds (with which the controller sends robot state updates). It must be between 1 and 10 for control and can be only changed in `inactive` and `configuring` states. +- `receive_multiplier` (integer): this parameter defines the answer rate factor (the client should sends commands in every `receive_multiplier`*`send_period_ms` milliseconds). It must be at least 1 and can be only changed in `inactive` and `configuring` states. +- `control_mode`: The enum value of the control mode should be given, which updates the `ControlMode` and `ClientCommandMode` parameters of FRI. It cannot be changed in active state. +- `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. The updated values are sent to the hardware interface using the `joint_group_impedance_controller` to adapt to conventions, but it is not possible to change them in active state due to the constraints of FRI. (Therefore the `joint_group_impedance_controller` is deactivated at driver activation.) +- `position_controller_name`: The name of the controller (string) that controls the `position` interface of the robot. It can't be changed in active state. +- `torque_controller_name`: The name of the controller (string) that controls the `effort` interface of the robot. It can't be changed in active state. + +### Usage + +#### Starting the driver + +1. On the controller, start the uploaded robot application (ROS2_Control). +2. To start the driver, two launch file are available, with and without `rviz`. To launch (without `rviz`), run +<<<<<<< HEAD:doc/wiki/Sunrise_FRI.md +``` +ros2 launch kuka_sunrise_fri_driver startup.launch.py controller_ip:=0.0.0.0 client_ip:=0.0.0.0 +``` +This starts the 3 core components of every driver (described in the *Non-real-time interface* section of the [project overview](Project%20overview.md)) and the following controllers: +- `joint_state_broadcaster` (no configuration file, all state interfaces are published) +- `joint_trajectory_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml)) +- [`fri_configuration_controller`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_configuration_controller) (no configuration file) +- [`fri_state_broadcaster`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_state_broadcaster) (no configuration file) +- `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 `JointGroupPositionController`, [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) +======= + ``` + ros2 launch kuka_sunrise_fri_driver startup.launch.py controller_ip:=0.0.0.0 + ``` + - This starts the 3 core components of every driver (described in the [Non-real-time interface](https://github.com/kroshu/kuka_drivers/wiki#non-real-time-interface) section of the project overview) and the following controllers: + - `joint_state_broadcaster` (no configuration file, all state interfaces are published) + - `joint_trajectory_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml)) + - [`fri_configuration_controller`](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#fri_configuration_controller) (no configuration file) + - [`fri_state_broadcaster`](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#fri_state_broadcaster) (no configuration file) +>>>>>>> master:doc/wiki/3_Sunrise_FRI.md + +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`): + ``` + ros2 lifecycle set robot_manager configure + ros2 lifecycle set robot_manager activate + ``` +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](https://github.com/kroshu/kuka_drivers/wiki#additional-packages) section of the project overview). + + +##### Launch arguments + +Both launch files support the following argument: +- `controller_ip`: IP address of the robot controller +- `client_ip`: IP address of the client PC +- `client_port`: port of the client machine (default: 30200) +- `robot_model`: defines which LBR iiwa robot to use. Available options: `lbr_iiwa14_r820` (default) +- `use_fake_hardware`: if true, the `KukaMockHardwareInterface` will be used instead of the `KukaFRIHardwareInterface`. This enables trying out the driver without actual hardware. +- `namespace`: adds a namespace to all nodes and controllers of the driver, and modifies the `prefix` argument of the robot description macro to `namespace_` +- `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame (default: [0, 0, 0]) +- `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame (default: [0, 0, 0]) +- `roundtrip_time`: The roundtrip time (in microseconds) to be enforced by the [KUKA mock hardware interface](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#custom-mock-hardware), (defaults to 5000 us, only used if `use_fake_hardware` is true) +- `controller_config`: the location of the `ros2_control` configuration file (defaults to `kuka_sunrise_fri_driver/config/ros2_controller_config.yaml`) +- `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`) + + +The `startup_with_rviz.launch.py` additionally contains one argument: +- `rviz_config`: the location of the `rviz` configuration file (defaults to `kuka_resources/config/view_6_axis_urdf.rviz`) + + +#### Stopping external control + +To stop external control, all components have to be deactivated with `ros2 lifecycle set robot_manager deactivate` + +BEWARE, that this is a non-realtime process including lifecycle management, so the connection is not terminated immediately, in cases where an abrupt stop is needed, the safety stop of the Teach Pendant should be used! (This will also deactivate all components to allow reactivation without a restart.) + + +### Known issues and limitations + +- Not all hardware-related communication is implemented in the hardware interface, therefore the mock hardware option is not working properly +- The control mode handling for the driver is not the one defined in the `kuka_drivers_core` package + - enum definition and controller switching logic is not used + - joint impedance control is not implemented properly using command interfaces +- I/O control was not tested +- Cartesian modes are not yet supported diff --git a/doc/wiki/4_Controllers.md b/doc/wiki/4_Controllers.md new file mode 100644 index 00000000..1ec94a59 --- /dev/null +++ b/doc/wiki/4_Controllers.md @@ -0,0 +1,58 @@ +## KUKA-specific controllers + +The controllers needed for KUKA drivers can be divided into three categories. + +### Traditional controllers + +These controllers update the command interfaces of a hardware cyclically. + +#### `joint_group_impedance_controller` +The joint impedance controller listens on the `~/command` topic and updates the `stiffness` and `damping` interfaces of the hardware accordingly. +The command must be of `std_msgs::Float64MultiArray` type and must contain the values for all configured joints. The order of the values should match the `stifness_1, damping_1, stiffness_2, ...` pattern. The controller only processes the commands received on this topic, when it is in active state. + +Example cli command to set damping to 0.7 and stiffness to 100 for all joints of a 6 DOF robot: + +``` +ros2 topic pub /joint_group_impedance_controller/commands std_msgs/msg/Float64MultiArray "{data: [100, 0.7, 100, 0.7, 100, 0.7, 100, 0.7, 100, 0.7, 100, 0.7]}" --once +``` + +__Required parameters__: +- `joints` [string_array]: Names of joints used by the controller + +### Broadcasters + +Broadcasters receive the state interfaces of a hardware and publish it to a ROS2 topic. +#### `fri_state_broadcaster` + +The `FRIStateBroadcaster` publishes the actual state of FRI to the `~/fri_state` topic, using the custom [FRIState](https://github.com/kroshu/kuka_drivers/blob/master/kuka_driver_interfaces/msg/FRIState.msg) message. + +__Required parameters__: None + + +#### `event_broadcaster` + +The `EventBroadcaster` publishes server state change events as integers (enum values) on the `~/hardware_event` topic. The enum values are equivalent with the following events: +- 2: Start command was accepted by the robot controller +- 3: External control started +- 4: External control stopped (by user) +- 5: Control mode switch was successful (only relevant for drivers, where control mode can be changed in active state) +- 6: External control stopped by an error (Error message is only available in the hardware interface) + +__Required parameters__: None + +### Configuration controllers + +Hardware interfaces do not support parameters that can be changed in runtime. To provide this behaviour, configuration controllers can be used, which update specific command interfaces of a hardware, that are exported as a workaround instead of parameters. + +#### `control_mode_handler` + +The `ControlModeHandler` can update the `control_mode` command interface of a hardware. It listens on the `~/control_mode` topic and makes control mode changes possible without having to reactivate the driver. +The control mode is [defined as an enum](https://github.com/kroshu/kuka_drivers/blob/master/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp) in the `kuka_drivers_core` package, the subscription therefore is of an unsigned integer type. + +__Required parameters__: None + +#### `fri_configuration_controller` + +The `receive_multiplier` parameter of FRI defines the answer rate factor (ratio of receiving states and sending commands). This is a parameter of the hardware interface, which can be modified in connected state, when control is not active. To support changing this parameter after startup, the `FRIConfigurationController` implements a service named `~/set_receive_multiplier`. Sending a request containing the desired integer value of the `receive_multiplier` updates the parameter of the hardware interface. + +__Required parameters__: None diff --git a/doc/wiki/Realtime.md b/doc/wiki/5_Realtime.md similarity index 77% rename from doc/wiki/Realtime.md rename to doc/wiki/5_Realtime.md index 0d8d7f53..0d8cfb58 100644 --- a/doc/wiki/Realtime.md +++ b/doc/wiki/5_Realtime.md @@ -11,7 +11,7 @@ 2. Find and download the real-time patch with matching major and minor versions from [here](https://cdn.kernel.org/pub/linux/kernel/projects/rt/) (*patch-\-rt\.patch.gz*) -3. Download the kernel with identical version from [here](https://mirrors.edge.kernel.org/pub/linux/kernel/) (*linux-\.tar.gz*) +3. Download the kernel with identical version as the real-time patch from [here](https://mirrors.edge.kernel.org/pub/linux/kernel/) (*linux-\.tar.gz*) 4. Create directory to build kernel ``` @@ -34,7 +34,7 @@ 7. Install dependencies needed for building the kernel ``` - sudo apt install libncurses-dev flex bison openssl libssl-dev dkms libelf-dev libudev-dev libpci-dev libiberty-dev autoconf fakeroot + sudo apt install libncurses-dev flex bison openssl libssl-dev dkms libelf-dev libudev-dev libpci-dev libiberty-dev autoconf fakeroot debhelper ``` ### Kernel configuration @@ -97,9 +97,14 @@ Save (without modifying the name) and exit menuconfig. ### Build and install kernel 1. Build the kernel (which will take quite some time): + - Below kernel version 6.3: ``` make -j `getconf _NPROCESSORS_ONLN` deb-pkg ``` + - From kernel version 6.3: + ``` + make -j `getconf _NPROCESSORS_ONLN` bindeb-pkg + ``` After successful completion, there should be 4 debian packages in the `~/kernel` directory 2. Install all kernel debian packages: @@ -121,3 +126,23 @@ After installing the real-time kernel, the setting of scheduling priorities must username - rtprio 98 ``` - Restart the system + +## Possible issues of building the kernel +### SSL error at signing +**Error**: +`SSL error:FFFFFFFF80000002:system library::No such file or directory` + +**Solution**: disable ZSTD module compression and rebuild + ``` + scripts/config --disable CONFIG_MODULE_COMPRESS_ZSTD + scripts/config --enable CONFIG_MODULE_COMPRESS_NONE + ``` + +### Segmentation fault while building a specific driver + +**Error example** +``` +CC [M] drivers/net/wireless/mediatek/mt76/mt7921/mac.occ1: +internal compiler error: Segmentation fault +``` +**Solution**: remove the problematic driver (mt7921 in example) from the kernel configuration file diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index 5115d9fd..d53d6d91 100644 --- a/doc/wiki/Home.md +++ b/doc/wiki/Home.md @@ -34,7 +34,7 @@ The startup procedure for any system in ROS can be defined using a launch file, The last issue should be certainly prevented from happening, therefore it was decided to extend the default startup procedure with a [lifecycle interface](https://design.ros2.org/articles/node_lifecycle.html), that synchronizes all components of the driver. The hardware interfaces and controllers already have a lifecycle interface, but by default they are loaded and activated at startup. This configuration was modified to only load the hardware interfaces and controllers, configuration and activation is handled by a custom a lifecycle node, called `robot_manager`. The 3 states of the `robot_manager` node have the following meaning: - `unconfigured`: all necessary components are started, but no connection is needed to the robot -- `configured`: The driver has configured the parameters necessary to start external control. Connection to the robot might be needed. (All of the parameters have default values in the driver, which are set on the robot controller during configuration.) A few [configuration controllers](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#configuration-controllers) might be active, that handle the runtime parameters of the hardware interface. +- `configured`: The driver has configured the parameters necessary to start external control. Connection to the robot might be needed. (All of the parameters have default values in the driver, which are set on the robot controller during configuration.) A few [configuration controllers](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#configuration-controllers) might be active, that handle the runtime parameters of the hardware interface. - `active`: external control is running with cyclic real-time communication, controllers are active To achieve these synchronized states, the state transitions of the system do the following steps (implemented by the launch file and the `robot_manager` node): @@ -102,7 +102,7 @@ The package contains a [launch file](https://github.com/kroshu/kuka_drivers/blob After activation, the Motion Planning plugin can be added (`Add` -> `moveit_ros_visualisation` -> `MotionPlanning`) to plan trajectories from the `rviz` GUI. (`Planning group` in the `Planning` tab should be changed to `manipulator`.) -The package also contains examples of sending planning requests from C++ code, in which case the `rviz` plugin is not necessary. The `MoveitExample` class implements a wrapper around the `MoveGroupInterface`, the example executables in the package use this class to interact with Moveit. Four examples are provided: +The package also contains examples of sending planning requests from C++ code, in which case the `rviz` plugin is not necessary. The `MoveitExample` class implements a wrapper around the `MoveGroupInterface`, the example executables in the package use this class to interact with `Moveit`. Four examples are provided: - `moveit_basic_planners_example`: the example uses the `PILZ` motion planner to plan a `PTP` and a `LIN` trajectory. It also demonstrates that planning with collision avoidance is not possible with the `PILZ` planner by adding a collision box that invalidates the planned trajectory. - `moveit_collision_avoidance_example`: the example adds a collision box to the scene and demonstrates, that path planning with collision avoidance is possible using the `OMPL` planning pipeline. - `moveit_constrained_planning_example`: this example demonstrates constrained planning capabilities, as the planner can find a valid path around the obstacle with the end effector orientation remaining constant (with small tolerance). @@ -112,10 +112,31 @@ Note: the first three examples should be executed consequently (without restarti Note: The examples need user interaction in `rviz`, the `Next` button (`RvizVisualToolsGui` tab) should be pressed each time the logs indicate it. + +### Planning in impedance mode + +Planning with `Moveit`, or simply moving the robot in impedance mode is quite tricky, as often there is a mismatch between the actual and commanded positions. +Both `Moveit` and the `joint_trajectory_controller` start planning/execution from the actually measured values, creating a jump in the commands (from the previous command to the measured state), which can trigger a joint limit violation. To solve this issue, the planning request must be modified to start from the commanded position instead of the measured one. +For example in the `moveit_basic_planners_example` the following lines must be added before each planning request: +```C++ + moveit_msgs::msg::RobotState start_state; + std::vector commanded_pos; // TODO: fill this vector with the actually commanded positions + start_state.joint_state.name = example_node->moveGroupInterface()->getJointNames(); + start_state.joint_state.position = commanded_pos; + example_node->moveGroupInterface()->setStartState(start_state); +``` +Additionally the `trajectory_execution.allowed_start_tolerfance` parameter in `moveit_controllers.yaml` (found in the moveit support packages) should be increased based on the actual displacement between commanded and measured joint values. + +If you would like to only move the robot by sending a goal to the `joint_trajectory_controller` for interpolation (e.g. with the [example trajectory publisher](https://github.com/kroshu/kuka_drivers/blob/master/examples/iiqka_moveit_example/launch/launch_trajectory_publisher.launch.py)), the following line should be added to the controller configuration file: +```yaml +open_loop_control: true +``` + + ## Multi-robot scenario As the robot controllers manage the timing of the drivers, it does not make sense to add more robots to the same control loop, as it would only lead to timeouts on the controller side. Therefore to start the drivers of multiple robots, all components has to be duplicated. This can be achieved by using the `namespace` argument of the launch files (which is available for all drivers) to avoid name collisions. This will add a namespace to all nodes and controllers of the driver and will also modify the `prefix` argument of the robot description macro to `namespace_`. To adapt to the new namespace and prefix, the configurations files of the driver must also be modified to reflect the new node and joint names. An example of this can found in the `iiqka_moveit_example` package, that starts two robots with `test1` and `test2` namespaces and modified configuration files. ## Detailed setup and startup instructions -For more detailed information about the drivers, visit the dedicated wiki pages for [KSS](https://github.com/kroshu/kuka_drivers/wiki/KSS_RSI), [Sunrise](https://github.com/kroshu/kuka_drivers/wiki/Sunrise_FRI) or [iiQKA](https://github.com/kroshu/kuka_drivers/wiki/iiQKA_EAC) robots. +For more detailed information about the drivers, visit the dedicated wiki pages for [KSS](https://github.com/kroshu/kuka_drivers/wiki/2_KSS_RSI), [Sunrise](https://github.com/kroshu/kuka_drivers/wiki/3_Sunrise_FRI) or [iiQKA](https://github.com/kroshu/kuka_drivers/wiki/1_iiQKA_EAC) robots. diff --git a/examples/iiqka_moveit_example/CMakeLists.txt b/examples/iiqka_moveit_example/CMakeLists.txt index c9e82446..e9903dbd 100755 --- a/examples/iiqka_moveit_example/CMakeLists.txt +++ b/examples/iiqka_moveit_example/CMakeLists.txt @@ -1,61 +1,39 @@ cmake_minimum_required(VERSION 3.5) project(iiqka_moveit_example) -# Common cmake code applied to all moveit packages -find_package(moveit_common REQUIRED) -moveit_package() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(moveit_common REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) -find_package(rosidl_default_runtime REQUIRED) -find_package(rclcpp REQUIRED) find_package(moveit_visual_tools REQUIRED) -find_package(rviz_visual_tools REQUIRED) -find_package(moveit_msgs REQUIRED) -find_package(kuka_driver_interfaces REQUIRED) include_directories(include) add_executable(moveit_basic_planners_example src/moveit_basic_planners_example.cpp) ament_target_dependencies(moveit_basic_planners_example moveit_ros_planning_interface - rclcpp - rviz_visual_tools moveit_visual_tools - kuka_driver_interfaces ) add_executable(moveit_collision_avoidance_example src/moveit_collision_avoidance_example.cpp) ament_target_dependencies(moveit_collision_avoidance_example moveit_ros_planning_interface - rclcpp - rviz_visual_tools moveit_visual_tools - kuka_driver_interfaces ) add_executable(moveit_constrained_planning_example src/moveit_constrained_planning_example.cpp) ament_target_dependencies(moveit_constrained_planning_example moveit_ros_planning_interface - rclcpp - rviz_visual_tools moveit_visual_tools - kuka_driver_interfaces ) add_executable(moveit_depalletizing_example src/moveit_depalletizing_example.cpp) ament_target_dependencies(moveit_depalletizing_example moveit_ros_planning_interface - rclcpp - rviz_visual_tools moveit_visual_tools - kuka_driver_interfaces ) install(TARGETS diff --git a/examples/iiqka_moveit_example/CONTRIBUTING.md b/examples/iiqka_moveit_example/CONTRIBUTING.md deleted file mode 100644 index 6f63de9e..00000000 --- a/examples/iiqka_moveit_example/CONTRIBUTING.md +++ /dev/null @@ -1,13 +0,0 @@ -Any contribution that you make to this repository will -be under the Apache 2 License, as dictated by that -[license](http://www.apache.org/licenses/LICENSE-2.0.html): - -~~~ -5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. -~~~ diff --git a/examples/iiqka_moveit_example/LICENSE b/examples/iiqka_moveit_example/LICENSE deleted file mode 100644 index 261eeb9e..00000000 --- a/examples/iiqka_moveit_example/LICENSE +++ /dev/null @@ -1,201 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/examples/iiqka_moveit_example/launch/multi_robot_startup.launch.py b/examples/iiqka_moveit_example/launch/multi_robot_startup.launch.py index 6da796f8..86d973c7 100644 --- a/examples/iiqka_moveit_example/launch/multi_robot_startup.launch.py +++ b/examples/iiqka_moveit_example/launch/multi_robot_startup.launch.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/examples/iiqka_moveit_example/package.xml b/examples/iiqka_moveit_example/package.xml index 4e2fe719..158ee51f 100644 --- a/examples/iiqka_moveit_example/package.xml +++ b/examples/iiqka_moveit_example/package.xml @@ -2,22 +2,19 @@ iiqka_moveit_example - 0.0.0 + 0.9.0 ROS example package using the iiQKA driver and moveit Aron Svastits - Apache 2.0 + Gergely Kovacs + Mark Szitanics + Apache-2.0 ament_cmake - moveit_common moveit_ros_planning_interface - moveit_msgs - rclcpp moveit_visual_tools - moveit_configs_utils - kuka_driver_interfaces - ros2_controllers + kuka_iiqka_eac_driver moveit ros2_controllers_test_nodes diff --git a/kuka_driver_interfaces/CMakeLists.txt b/kuka_driver_interfaces/CMakeLists.txt index 4170094f..f17912cd 100644 --- a/kuka_driver_interfaces/CMakeLists.txt +++ b/kuka_driver_interfaces/CMakeLists.txt @@ -18,18 +18,14 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(geometry_msgs REQUIRED) - rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetFriConfiguration.srv" "srv/GetInt.srv" "srv/SetDouble.srv" "msg/FRIState.msg" - DEPENDENCIES geometry_msgs ) - if(BUILD_TESTING) endif() diff --git a/kuka_driver_interfaces/package.xml b/kuka_driver_interfaces/package.xml index 57e737f9..a948b87a 100644 --- a/kuka_driver_interfaces/package.xml +++ b/kuka_driver_interfaces/package.xml @@ -2,16 +2,14 @@ kuka_driver_interfaces - 0.0.0 - TODO: Package description - rosdeveloper - Apache 2.0 + 0.9.0 + Message definitions necessary for using KUKA drivers + Aron Svastits + Apache-2.0 ament_cmake rosidl_default_generators - geometry_msgs - rosidl_default_runtime rosidl_interface_packages diff --git a/kuka_drivers/package.xml b/kuka_drivers/package.xml index aac4982b..7c0409ea 100644 --- a/kuka_drivers/package.xml +++ b/kuka_drivers/package.xml @@ -1,24 +1,27 @@ kuka_drivers - 0.0.1 + 0.9.0 ROS2 drivers for KUKA robots - Aron Svastits Aron Svastits - Apache 2.0 + Gergely Kovacs + Mark Szitanics + Apache-2.0 https://github.com/kroshu/kuka_drivers/issues https://github.com/kroshu/kuka_drivers ament_cmake - kuka_drivers_core + iiqka_moveit_example kuka_driver_interfaces + kuka_drivers_core kuka_iiqka_eac_driver kuka_kss_rsi_driver - kuka_sunrise_fri_driver - iiqka_moveit_example kuka_rsi_simulator + kuka_sunrise_fri_driver + + kuka_controllers ament_cmake diff --git a/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp b/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp index 4054ada3..a37bbc7c 100644 --- a/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp +++ b/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Áron Svastits +// Copyright 2020 Aron Svastits // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp b/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp new file mode 100644 index 00000000..58f4b0ae --- /dev/null +++ b/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp @@ -0,0 +1,31 @@ +// Copyright 2024 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 KUKA_DRIVERS_CORE__CONTROLLER_NAMES_HPP_ +#define KUKA_DRIVERS_CORE__CONTROLLER_NAMES_HPP_ + +namespace kuka_drivers_core +{ +/* Fixed controller names */ +static constexpr char JOINT_STATE_BROADCASTER[] = "joint_state_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"; +static constexpr char FRI_STATE_BROADCASTER[] = "fri_state_broadcaster"; + +// Controller names with default values +static constexpr char JOINT_TRAJECTORY_CONTROLLER[] = "joint_trajectory_controller"; +} // namespace kuka_drivers_core + +#endif // KUKA_DRIVERS_CORE__CONTROLLER_NAMES_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp index 90fac1c2..150c3fc7 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Áron Svastits +// Copyright 2024 Aron Svastits // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp index 0ce4ade3..3338082c 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Áron Svastits +// 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. diff --git a/kuka_drivers_core/package.xml b/kuka_drivers_core/package.xml index 4b0d9d56..92a29a98 100644 --- a/kuka_drivers_core/package.xml +++ b/kuka_drivers_core/package.xml @@ -2,10 +2,12 @@ kuka_drivers_core - 0.0.0 + 0.9.0 Package containing ROS2 core functions for KUKA robots + Aron Svastits Gergely Kovacs - Apache 2.0 + Mark Szitanics + Apache-2.0 ament_cmake rosidl_default_generators diff --git a/kuka_drivers_core/src/control_node.cpp b/kuka_drivers_core/src/control_node.cpp index 7ebf36f9..9b5926a3 100644 --- a/kuka_drivers_core/src/control_node.cpp +++ b/kuka_drivers_core/src/control_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Áron Svastits +// 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. diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index fdc3194e..12003654 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -14,47 +14,34 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# A few (transitive) dependencies are available only in one of the dependencies -# Set flag to suppress linker error -# TODO: find better solution -set(CMAKE_EXE_LINKER_FLAGS "-Wl,--copy-dt-needed-entries") - +list(APPEND CMAKE_PREFIX_PATH "~/.local/lib/cmake") find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) find_package(kuka_drivers_core REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) +find_package(std_msgs REQUIRED) find_package(controller_manager_msgs REQUIRED) - +find_package(kuka-external-control-sdk CONFIG REQUIRED) include_directories(include) -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} ${EXTERNAL_CONTROL_SDK}) +ament_target_dependencies(${PROJECT_NAME} hardware_interface kuka_drivers_core kuka-external-control-sdk) +target_link_libraries(${PROJECT_NAME} Kuka::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 ${EXTERNAL_CONTROL_SDK}) +ament_target_dependencies(robot_manager_node std_msgs kuka_drivers_core controller_manager_msgs) +target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers Kuka::kuka-external-control-sdk) pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) @@ -74,9 +61,4 @@ 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() diff --git a/kuka_iiqka_eac_driver/config/effort_controller_config.yaml b/kuka_iiqka_eac_driver/config/effort_controller_config.yaml index 99873d72..504c4f38 100644 --- a/kuka_iiqka_eac_driver/config/effort_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/effort_controller_config.yaml @@ -7,28 +7,3 @@ effort_controller: - joint_4 - joint_5 - joint_6 - gains: - joint_1: - p: 5.0 - i: 0.0 - d: 0.1 - joint_2: - p: 5.0 - i: 0.0 - d: 0.1 - joint_3: - p: 5.0 - i: 0.0 - d: 0.1 - joint_4: - p: 5.0 - i: 0.0 - d: 0.1 - joint_5: - p: 5.0 - i: 0.0 - d: 0.1 - joint_6: - p: 5.0 - i: 0.0 - d: 0.1 diff --git a/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml b/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml index 3f727ef4..0462493d 100644 --- a/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml @@ -14,4 +14,4 @@ joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml index b4f8f503..00ad8aac 100644 --- a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml @@ -9,7 +9,7 @@ controller_manager: joint_group_impedance_controller: type: kuka_controllers/JointGroupImpedanceController effort_controller: - type: effort_controllers/JointGroupPositionController + type: effort_controllers/JointGroupEffortController control_mode_handler: type: kuka_controllers/ControlModeHandler event_broadcaster: diff --git a/kuka_iiqka_eac_driver/include/CPPLINT.cfg b/kuka_iiqka_eac_driver/include/CPPLINT.cfg index 15fee855..19074bf5 100644 --- a/kuka_iiqka_eac_driver/include/CPPLINT.cfg +++ b/kuka_iiqka_eac_driver/include/CPPLINT.cfg @@ -1,4 +1,4 @@ -# Copyright 2020 Áron Svastits +# Copyright 2020 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index 3f1947c2..2d23d91e 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -18,7 +18,7 @@ #include #include "rclcpp/macros.hpp" -#include "kuka/external-control-sdk/common/irobot.h" +#include "kuka/external-control-sdk/iiqka/sdk.h" #include "kuka_drivers_core/hardware_event.hpp" #include "kuka_iiqka_eac_driver/hardware_interface.hpp" diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index f59f178b..dc5811d9 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Áron Svastits +// 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. @@ -28,8 +28,7 @@ #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 "kuka/external-control-sdk/iiqka/sdk.h" #include "rclcpp_lifecycle/state.hpp" #include "kuka_drivers_core/hardware_event.hpp" diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h index 5640c7f4..c373f61c 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2023 Áron Svastits +// 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. diff --git a/kuka_iiqka_eac_driver/launch/startup.launch.py b/kuka_iiqka_eac_driver/launch/startup.launch.py index d87934c9..2d207aeb 100644 --- a/kuka_iiqka_eac_driver/launch/startup.launch.py +++ b/kuka_iiqka_eac_driver/launch/startup.launch.py @@ -1,4 +1,4 @@ -# Copyright 2022 Áron Svastits +# 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. @@ -33,6 +33,7 @@ def launch_setup(context, *args, **kwargs): roll = LaunchConfiguration("roll") pitch = LaunchConfiguration("pitch") yaw = LaunchConfiguration("yaw") + roundtrip_time = LaunchConfiguration("roundtrip_time") qos_config = LaunchConfiguration("qos_config") controller_config = LaunchConfiguration("controller_config") jtc_config = LaunchConfiguration("jtc_config") @@ -86,6 +87,9 @@ def launch_setup(context, *args, **kwargs): "yaw:=", yaw, " ", + "roundtrip_time:=", + roundtrip_time, + " ", "qos_config_file:=", qos_config, ], @@ -186,6 +190,7 @@ def generate_launch_description(): launch_arguments.append(DeclareLaunchArgument("roll", default_value="0")) launch_arguments.append(DeclareLaunchArgument("pitch", default_value="0")) launch_arguments.append(DeclareLaunchArgument("yaw", default_value="0")) + launch_arguments.append(DeclareLaunchArgument("roundtrip_time", default_value="2500")) launch_arguments.append( DeclareLaunchArgument( "qos_config", diff --git a/kuka_iiqka_eac_driver/launch/startup_with_rviz.launch.py b/kuka_iiqka_eac_driver/launch/startup_with_rviz.launch.py index 5c88f954..a11a5c1a 100644 --- a/kuka_iiqka_eac_driver/launch/startup_with_rviz.launch.py +++ b/kuka_iiqka_eac_driver/launch/startup_with_rviz.launch.py @@ -1,4 +1,4 @@ -# Copyright 2022 Áron Svastits +# 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. diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index 5a7d9c4f..bbe5e6d1 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -2,29 +2,30 @@ kuka_iiqka_eac_driver - 0.0.0 + 0.9.0 A ROS2 hardware interface for use with KUKA iiQKA OS Aron Svastits - Apache 2.0 + Gergely Kovacs + Mark Szitanics + Apache-2.0 ament_cmake - ament_cmake_python - rclcpp - rclcpp_lifecycle - std_msgs - std_srvs - sensor_msgs kuka_drivers_core hardware_interface pluginlib + kuka_external_control_sdk + std_msgs + controller_manager_msgs kuka_lbr_iisy_support ros2_control - ros2_controllers - joint_group_impedance_controller + joint_trajectory_controller effort_controllers + joint_state_broadcaster control_mode_handler + event_broadcaster + joint_group_impedance_controller ros2lifecycle diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index bb6c6b02..e7b9d677 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Áron Svastits +// 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. @@ -22,7 +22,6 @@ #include "kuka_drivers_core/control_mode.hpp" #include "kuka_drivers_core/hardware_interface_types.hpp" -#include "kuka/external-control-sdk/iiqka/configuration.h" #include "kuka_iiqka_eac_driver/event_observer.hpp" #include "kuka_iiqka_eac_driver/hardware_interface.hpp" @@ -188,7 +187,7 @@ CallbackReturn KukaEACHardwareInterface::on_configure(const rclcpp_lifecycle::St CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { - kuka::external::control::OperationStatus create_event_observer = + kuka::external::control::Status create_event_observer = robot_ptr_->RegisterEventHandler(std::make_unique(this)); if (create_event_observer.return_code == kuka::external::control::ReturnCode::ERROR) { @@ -197,7 +196,7 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta "Creating event observer failed, error message: %s", create_event_observer.message); } - kuka::external::control::OperationStatus start_control = robot_ptr_->StartControlling( + kuka::external::control::Status start_control = robot_ptr_->StartControlling( static_cast(hw_control_mode_command_)); if (start_control.return_code == kuka::external::control::ReturnCode::ERROR) { @@ -214,6 +213,7 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta "External control session started successfully"); stop_requested_ = false; + cycle_count_ = 0; return CallbackReturn::SUCCESS; } @@ -229,7 +229,7 @@ CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::S return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { // Bigger timeout blocks controller configuration - kuka::external::control::OperationStatus receive_state = + kuka::external::control::Status receive_state = robot_ptr_->ReceiveMotionState(std::chrono::milliseconds(10)); if ((msg_received_ = receive_state.return_code == kuka::external::control::ReturnCode::OK)) @@ -271,7 +271,7 @@ return_type KukaEACHardwareInterface::write(const rclcpp::Time &, const rclcpp:: robot_ptr_->GetControlSignal().AddStiffnessAndDampingValues( hw_stiffness_commands_, hw_damping_commands_); - kuka::external::control::OperationStatus send_reply; + kuka::external::control::Status send_reply; if (stop_requested_) { RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Sending stop signal"); @@ -311,7 +311,7 @@ bool KukaEACHardwareInterface::SetupRobot() robot_ptr_ = std::make_unique(config); - kuka::external::control::OperationStatus setup = robot_ptr_->Setup(); + kuka::external::control::Status setup = robot_ptr_->Setup(); if (setup.return_code != kuka::external::control::ReturnCode::OK) { @@ -333,7 +333,7 @@ bool KukaEACHardwareInterface::SetupQoS() std::stoi(info_.hardware_parameters.at("consequent_lost_packets")); qos_config.timeframe_ms = std::stoi(info_.hardware_parameters.at("timeframe_ms")); - kuka::external::control::OperationStatus set_qos_status = robot_ptr_->SetQoSProfile(qos_config); + kuka::external::control::Status set_qos_status = robot_ptr_->SetQoSProfile(qos_config); if (set_qos_status.return_code != kuka::external::control::ReturnCode::OK) { diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 1c7c6d8c..6bc520bd 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -20,6 +20,7 @@ #include "kuka_iiqka_eac_driver/robot_manager_node.hpp" #include "kuka_drivers_core/control_mode.hpp" +#include "kuka_drivers_core/controller_names.hpp" #include "kuka_drivers_core/hardware_event.hpp" using namespace controller_manager_msgs::srv; // NOLINT @@ -57,7 +58,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ // Register parameters this->registerParameter( - "position_controller_name", "joint_trajectory_controller", + "position_controller_name", kuka_drivers_core::JOINT_TRAJECTORY_CONTROLLER, kuka_drivers_core::ParameterSetAccessRights{true, true, false}, [this](const std::string & controller_name) { @@ -118,7 +119,8 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Activate control mode handler and event broadcaster if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {"control_mode_handler", "event_broadcaster"}, {})) + change_controller_state_client_, + {kuka_drivers_core::CONTROL_MODE_HANDLER, kuka_drivers_core::EVENT_BROADCASTER}, {})) { RCLCPP_ERROR(get_logger(), "Could not activate control mode handler or event broadcaster"); // Rollback @@ -136,7 +138,8 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { // Deactivate control mode handler and event broadcaster if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, {"control_mode_handler", "event_broadcaster"})) + change_controller_state_client_, {}, + {kuka_drivers_core::CONTROL_MODE_HANDLER, kuka_drivers_core::EVENT_BROADCASTER})) { RCLCPP_ERROR(get_logger(), "Could not deactivate control mode handler and event broadcaster"); } @@ -209,9 +212,9 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) std::this_thread::sleep_for(std::chrono::milliseconds(20)); // Activate RT controller(s) - if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, controller_handler_.GetControllersForMode(control_mode_), - {})) + auto controllers = controller_handler_.GetControllersForMode(control_mode_); + controllers.push_back(kuka_drivers_core::JOINT_STATE_BROADCASTER); + if (!kuka_drivers_core::changeControllerState(change_controller_state_client_, controllers, {})) { RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); this->on_deactivate(get_current_state()); @@ -243,10 +246,10 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) } // Stop RT controllers + auto controllers = controller_handler_.GetControllersForMode(control_mode_); + controllers.push_back(kuka_drivers_core::JOINT_STATE_BROADCASTER); if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, - controller_handler_.GetControllersForMode(control_mode_), - SwitchController::Request::BEST_EFFORT)) + change_controller_state_client_, {}, controllers, SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop RT controllers"); return ERROR; diff --git a/kuka_iiqka_eac_driver/test/config/test1_joint_trajectory_controller_config.yaml b/kuka_iiqka_eac_driver/test/config/test1_joint_trajectory_controller_config.yaml index 5d37962b..ef982dd0 100644 --- a/kuka_iiqka_eac_driver/test/config/test1_joint_trajectory_controller_config.yaml +++ b/kuka_iiqka_eac_driver/test/config/test1_joint_trajectory_controller_config.yaml @@ -14,4 +14,4 @@ test1/joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml b/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml index 7383c386..734d4428 100644 --- a/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml @@ -12,3 +12,5 @@ test1/controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler + event_broadcaster: + type: kuka_controllers/EventBroadcaster diff --git a/kuka_iiqka_eac_driver/test/config/test2_joint_trajectory_controller_config.yaml b/kuka_iiqka_eac_driver/test/config/test2_joint_trajectory_controller_config.yaml index e9f93524..03fd32a8 100644 --- a/kuka_iiqka_eac_driver/test/config/test2_joint_trajectory_controller_config.yaml +++ b/kuka_iiqka_eac_driver/test/config/test2_joint_trajectory_controller_config.yaml @@ -14,4 +14,4 @@ test2/joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml b/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml index 0a8547e6..1231fa8b 100644 --- a/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml @@ -12,3 +12,5 @@ test2/controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler + event_broadcaster: + type: kuka_controllers/EventBroadcaster diff --git a/kuka_iiqka_eac_driver/test/test_driver_activation.py b/kuka_iiqka_eac_driver/test/test_driver_activation.py index 35da23bd..ff2a2f3c 100644 --- a/kuka_iiqka_eac_driver/test/test_driver_activation.py +++ b/kuka_iiqka_eac_driver/test/test_driver_activation.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -46,7 +46,7 @@ def generate_test_description(): }.items(), ), launch.actions.TimerAction( - period=2.0, + period=10.0, actions=[ launch.actions.ExecuteProcess( cmd=["ros2", "lifecycle", "set", "robot_manager", "configure"], @@ -55,7 +55,7 @@ def generate_test_description(): ], ), launch.actions.TimerAction( - period=4.0, + period=15.0, actions=[ launch.actions.ExecuteProcess( cmd=["ros2", "lifecycle", "set", "robot_manager", "activate"], @@ -81,6 +81,6 @@ def test_read_stdout(self, proc_output): ) # Check for successful configuration and activation proc_output.assertWaitFor( - "Successful 'configure' of hardware 'lbr_iisy3_r760'", timeout=10 + "Successful 'configure' of hardware 'lbr_iisy3_r760'", timeout=15 ) - proc_output.assertWaitFor("Successful 'activate' of hardware 'lbr_iisy3_r760'", timeout=15) + proc_output.assertWaitFor("Successful 'activate' of hardware 'lbr_iisy3_r760'", timeout=20) diff --git a/kuka_iiqka_eac_driver/test/test_driver_startup.py b/kuka_iiqka_eac_driver/test/test_driver_startup.py index 22c5ee3c..ceb3d46e 100644 --- a/kuka_iiqka_eac_driver/test/test_driver_startup.py +++ b/kuka_iiqka_eac_driver/test/test_driver_startup.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py b/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py index c8220fdf..85243b6c 100644 --- a/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py +++ b/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/kuka_kss_rsi_driver/CMakeLists.txt b/kuka_kss_rsi_driver/CMakeLists.txt index 20b2fd91..cca22896 100644 --- a/kuka_kss_rsi_driver/CMakeLists.txt +++ b/kuka_kss_rsi_driver/CMakeLists.txt @@ -16,10 +16,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) find_package(kuka_drivers_core REQUIRED) +find_package(std_msgs REQUIRED) find_package(hardware_interface REQUIRED) find_package(controller_manager_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -39,14 +37,12 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_KSS_RSI_DRIVER_BUILDING # prevent pluginlib from using boost target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface) +ament_target_dependencies(${PROJECT_NAME} hardware_interface) target_link_libraries(${PROJECT_NAME} tinyxml) - - 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) +ament_target_dependencies(robot_manager_node std_msgs kuka_drivers_core controller_manager_msgs) target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers) pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) diff --git a/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml b/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml index 3f727ef4..0462493d 100644 --- a/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml @@ -14,4 +14,4 @@ joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp index ee94efbc..29f6ae5a 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/hardware_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Áron Svastits +// 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. diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp index c35ba604..9b2b3a3e 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/robot_manager_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Svastits Áron +// 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. diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h index a91aaa35..6d20ac0e 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2023 Áron Svastits +// 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. diff --git a/kuka_kss_rsi_driver/launch/startup.launch.py b/kuka_kss_rsi_driver/launch/startup.launch.py index 30be4b86..7f03ca07 100644 --- a/kuka_kss_rsi_driver/launch/startup.launch.py +++ b/kuka_kss_rsi_driver/launch/startup.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Áron Svastits +# 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. @@ -35,6 +35,7 @@ def launch_setup(context, *args, **kwargs): roll = LaunchConfiguration("roll") pitch = LaunchConfiguration("pitch") yaw = LaunchConfiguration("yaw") + roundtrip_time = LaunchConfiguration("roundtrip_time") ns = LaunchConfiguration("namespace") controller_config = LaunchConfiguration("controller_config") jtc_config = LaunchConfiguration("jtc_config") @@ -85,6 +86,9 @@ def launch_setup(context, *args, **kwargs): " ", "yaw:=", yaw, + " ", + "roundtrip_time:=", + roundtrip_time, ], on_stderr="capture", ) @@ -166,6 +170,7 @@ def generate_launch_description(): launch_arguments.append(DeclareLaunchArgument("roll", default_value="0")) launch_arguments.append(DeclareLaunchArgument("pitch", default_value="0")) launch_arguments.append(DeclareLaunchArgument("yaw", default_value="0")) + launch_arguments.append(DeclareLaunchArgument("roundtrip_time", default_value="4000")) launch_arguments.append( DeclareLaunchArgument( "controller_config", diff --git a/kuka_kss_rsi_driver/launch/startup_with_rviz.launch.py b/kuka_kss_rsi_driver/launch/startup_with_rviz.launch.py index f96234ec..83acebeb 100644 --- a/kuka_kss_rsi_driver/launch/startup_with_rviz.launch.py +++ b/kuka_kss_rsi_driver/launch/startup_with_rviz.launch.py @@ -1,4 +1,4 @@ -# Copyright 2022 Áron Svastits +# 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. diff --git a/kuka_kss_rsi_driver/package.xml b/kuka_kss_rsi_driver/package.xml index 4b4b1385..c851c3ab 100644 --- a/kuka_kss_rsi_driver/package.xml +++ b/kuka_kss_rsi_driver/package.xml @@ -2,27 +2,28 @@ kuka_kss_rsi_driver - 0.0.0 + 0.9.0 A ROS2 hardware interface for use with KUKA RSI Aron Svastits - Apache 2.0 + Gergely Kovacs + Mark Szitanics + Apache-2.0 ament_cmake ament_cmake_python - rclcpp - rclcpp_lifecycle - std_msgs - std_srvs - sensor_msgs kuka_drivers_core - tinyxml_vendor hardware_interface pluginlib controller_manager_msgs + std_msgs + + tinyxml_vendor ros2_control - ros2_controllers + joint_trajectory_controller + joint_state_broadcaster + kuka_robot_descriptions kuka_rsi_simulator ros2lifecycle diff --git a/kuka_kss_rsi_driver/src/hardware_interface.cpp b/kuka_kss_rsi_driver/src/hardware_interface.cpp index 5c3dc1e9..ceb33412 100644 --- a/kuka_kss_rsi_driver/src/hardware_interface.cpp +++ b/kuka_kss_rsi_driver/src/hardware_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Áron Svastits +// 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. diff --git a/kuka_kss_rsi_driver/src/robot_manager_node.cpp b/kuka_kss_rsi_driver/src/robot_manager_node.cpp index c80f49de..0d7bcf45 100644 --- a/kuka_kss_rsi_driver/src/robot_manager_node.cpp +++ b/kuka_kss_rsi_driver/src/robot_manager_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Svastits Áron +// 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. @@ -15,6 +15,7 @@ #include "communication_helpers/ros2_control_tools.hpp" #include "communication_helpers/service_tools.hpp" +#include "kuka_drivers_core/controller_names.hpp" #include "kuka_kss_rsi_driver/robot_manager_node.hpp" using namespace controller_manager_msgs::srv; // NOLINT @@ -96,7 +97,9 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate RT controller(s) if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {"joint_state_broadcaster", "joint_trajectory_controller"}, + change_controller_state_client_, + {kuka_drivers_core::JOINT_STATE_BROADCASTER, + kuka_drivers_core::JOINT_TRAJECTORY_CONTROLLER}, {})) { RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); @@ -124,7 +127,8 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // With best effort strictness, deactivation succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {}, - {"joint_state_broadcaster", "joint_trajectory_controller"}, + {kuka_drivers_core::JOINT_STATE_BROADCASTER, + kuka_drivers_core::JOINT_TRAJECTORY_CONTROLLER}, SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); diff --git a/kuka_kss_rsi_driver/test/config/test1_joint_trajectory_controller_config.yaml b/kuka_kss_rsi_driver/test/config/test1_joint_trajectory_controller_config.yaml index 5d37962b..ef982dd0 100644 --- a/kuka_kss_rsi_driver/test/config/test1_joint_trajectory_controller_config.yaml +++ b/kuka_kss_rsi_driver/test/config/test1_joint_trajectory_controller_config.yaml @@ -14,4 +14,4 @@ test1/joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_kss_rsi_driver/test/config/test2_joint_trajectory_controller_config.yaml b/kuka_kss_rsi_driver/test/config/test2_joint_trajectory_controller_config.yaml index e9f93524..03fd32a8 100644 --- a/kuka_kss_rsi_driver/test/config/test2_joint_trajectory_controller_config.yaml +++ b/kuka_kss_rsi_driver/test/config/test2_joint_trajectory_controller_config.yaml @@ -14,4 +14,4 @@ test2/joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_kss_rsi_driver/test/test_driver_activation.py b/kuka_kss_rsi_driver/test/test_driver_activation.py index 78ffd9c7..2bedb4eb 100644 --- a/kuka_kss_rsi_driver/test/test_driver_activation.py +++ b/kuka_kss_rsi_driver/test/test_driver_activation.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -52,7 +52,7 @@ def generate_test_description(): ) ), launch.actions.TimerAction( - period=2.0, + period=10.0, actions=[ launch.actions.ExecuteProcess( cmd=["ros2", "lifecycle", "set", "robot_manager", "configure"], @@ -61,7 +61,7 @@ def generate_test_description(): ], ), launch.actions.TimerAction( - period=4.0, + period=15.0, actions=[ launch.actions.ExecuteProcess( cmd=["ros2", "lifecycle", "set", "robot_manager", "activate"], @@ -86,5 +86,5 @@ def test_read_stdout(self, proc_output): "Setting component 'kr6_r700_sixx' to 'unconfigured' state.", timeout=5 ) # Check for successful configuration and activation - proc_output.assertWaitFor("Successful 'configure' of hardware 'kr6_r700_sixx'", timeout=10) - proc_output.assertWaitFor("Successful 'activate' of hardware 'kr6_r700_sixx'", timeout=15) + proc_output.assertWaitFor("Successful 'configure' of hardware 'kr6_r700_sixx'", timeout=15) + proc_output.assertWaitFor("Successful 'activate' of hardware 'kr6_r700_sixx'", timeout=20) diff --git a/kuka_kss_rsi_driver/test/test_driver_startup.py b/kuka_kss_rsi_driver/test/test_driver_startup.py index 045ef6da..ede3aaa0 100644 --- a/kuka_kss_rsi_driver/test/test_driver_startup.py +++ b/kuka_kss_rsi_driver/test/test_driver_startup.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/kuka_kss_rsi_driver/test/test_multi_robot_startup.py b/kuka_kss_rsi_driver/test/test_multi_robot_startup.py index da9e3ff1..2f0994f6 100644 --- a/kuka_kss_rsi_driver/test/test_multi_robot_startup.py +++ b/kuka_kss_rsi_driver/test/test_multi_robot_startup.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/kuka_rsi_simulator/package.xml b/kuka_rsi_simulator/package.xml index aeb5a03c..ec81d829 100644 --- a/kuka_rsi_simulator/package.xml +++ b/kuka_rsi_simulator/package.xml @@ -2,10 +2,10 @@ kuka_rsi_simulator - 0.0.0 + 0.9.0 Simple package for simulating the KUKA RSI interface Aron Svastits - Apache 2.0 + Apache-2.0 ros2launch diff --git a/kuka_rsi_simulator/setup.py b/kuka_rsi_simulator/setup.py index a06a7928..fa2a0252 100644 --- a/kuka_rsi_simulator/setup.py +++ b/kuka_rsi_simulator/setup.py @@ -35,7 +35,7 @@ maintainer="Marton Antal", maintainer_email="antal.marci@gmail.com", description="Simple package for simulating the KUKA RSI interface.", - license="BSD", + license="Apache-2.0", entry_points={ "console_scripts": ["rsi_simulator = kuka_rsi_simulator.rsi_simulator:main"], }, diff --git a/kuka_sunrise_fri_driver/CMakeLists.txt b/kuka_sunrise_fri_driver/CMakeLists.txt index 59f0ca71..02558987 100644 --- a/kuka_sunrise_fri_driver/CMakeLists.txt +++ b/kuka_sunrise_fri_driver/CMakeLists.txt @@ -17,17 +17,13 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(sensor_msgs REQUIRED) find_package(kuka_driver_interfaces REQUIRED) find_package(kuka_drivers_core REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(controller_manager_msgs) +find_package(std_msgs) +find_package(std_srvs) include_directories(include src/fri_client_sdk) @@ -80,15 +76,13 @@ add_library(${PROJECT_NAME} SHARED # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_SUNRISE_FRI_DRIVER_BUILDING_LIBRARY") -ament_target_dependencies(${PROJECT_NAME} kuka_driver_interfaces rclcpp rclcpp_lifecycle hardware_interface kuka_drivers_core) +ament_target_dependencies(${PROJECT_NAME} kuka_driver_interfaces hardware_interface kuka_drivers_core) target_link_libraries(${PROJECT_NAME} fri_client_sdk) add_executable(robot_manager_node src/robot_manager_node.cpp) -ament_target_dependencies(robot_manager_node kuka_driver_interfaces rclcpp rclcpp_lifecycle kuka_drivers_core std_srvs +ament_target_dependencies(robot_manager_node kuka_driver_interfaces kuka_drivers_core std_srvs controller_manager_msgs) -target_link_libraries(robot_manager_node - fri_connection) pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) diff --git a/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml b/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml index ce5af6bb..2094b3eb 100644 --- a/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml @@ -15,4 +15,4 @@ joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h index 2d986a30..81e17ab8 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2023 Áron Svastits +// 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. diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 889a549a..4290da6a 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -1,4 +1,4 @@ -# Copyright 2022 Áron Svastits +# 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. @@ -34,6 +34,7 @@ def launch_setup(context, *args, **kwargs): roll = LaunchConfiguration("roll") pitch = LaunchConfiguration("pitch") yaw = LaunchConfiguration("yaw") + roundtrip_time = LaunchConfiguration("roundtrip_time") controller_config = LaunchConfiguration("controller_config") jtc_config = LaunchConfiguration("jtc_config") jic_config = LaunchConfiguration("jic_config") @@ -88,6 +89,9 @@ def launch_setup(context, *args, **kwargs): " ", "yaw:=", yaw, + " ", + "roundtrip_time:=", + roundtrip_time, ], on_stderr="capture", ) @@ -188,6 +192,7 @@ def generate_launch_description(): launch_arguments.append(DeclareLaunchArgument("roll", default_value="0")) launch_arguments.append(DeclareLaunchArgument("pitch", default_value="0")) launch_arguments.append(DeclareLaunchArgument("yaw", default_value="0")) + launch_arguments.append(DeclareLaunchArgument("roundtrip_time", default_value="5000")) launch_arguments.append( DeclareLaunchArgument( "controller_config", diff --git a/kuka_sunrise_fri_driver/launch/startup_with_rviz.launch.py b/kuka_sunrise_fri_driver/launch/startup_with_rviz.launch.py index 74584f43..d876ac6f 100644 --- a/kuka_sunrise_fri_driver/launch/startup_with_rviz.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup_with_rviz.launch.py @@ -1,4 +1,4 @@ -# Copyright 2022 Áron Svastits +# 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. diff --git a/kuka_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index 855faac0..7deb6ce7 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -2,31 +2,30 @@ kuka_sunrise_fri_driver - 0.0.0 + 0.9.0 ROS2 KUKA sunrise interface - Zoltan Resi + Aron Svastits Gergely Kovacs - Apache 2.0 + Mark Szitanics + Apache-2.0 ament_cmake - ament_cmake_python - rosidl_default_generators - rclcpp - rclcpp_lifecycle std_msgs std_srvs - sensor_msgs kuka_driver_interfaces kuka_drivers_core hardware_interface controller_manager_msgs + libnanopb-dev ros2_control - ros2_controllers - fri_state_broadcaster + joint_trajectory_controller + joint_state_broadcaster fri_configuration_controller + fri_state_broadcaster + kuka_lbr_iiwa_support ament_cmake diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index b29b1057..080cfba8 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -16,6 +16,7 @@ #include "communication_helpers/ros2_control_tools.hpp" #include "communication_helpers/service_tools.hpp" +#include "kuka_drivers_core/controller_names.hpp" #include "kuka_sunrise_fri_driver/robot_manager_node.hpp" @@ -132,6 +133,8 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {"fri_configuration_controller", "joint_group_impedance_controller"}, {})) + {kuka_drivers_core::FRI_CONFIGURATION_CONTROLLER, kuka_drivers_core::FRI_STATE_BROADCASTER}, + {})) { RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); return FAILURE; @@ -151,7 +154,8 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) // With best effort strictness, cleanup succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {}, - {"fri_configuration_controller", "joint_group_impedance_controller"}, + // TODO(Svastits): const char for impedance + {kuka_drivers_core::FRI_CONFIGURATION_CONTROLLER, kuka_drivers_core::FRI_STATE_BROADCASTER, "joint_group_impedance_controller"}, SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); @@ -210,7 +214,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate joint state broadcaster and controller for given control mode if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, - {"joint_state_broadcaster", "fri_state_broadcaster", GetControllerName()}, + {kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER, GetControllerName()}, {"joint_group_impedance_controller"})) { RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); @@ -237,7 +241,8 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // With best effort strictness, deactivation succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {"joint_group_impedance_controller"}, - {GetControllerName(), "joint_state_broadcaster", "fri_state_broadcaster"}, + {GetControllerName(), kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER}, + change_controller_state_client_, {}, SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not deactivate RT controllers"); diff --git a/kuka_sunrise_fri_driver/test/config/test1_joint_trajectory_controller_config.yaml b/kuka_sunrise_fri_driver/test/config/test1_joint_trajectory_controller_config.yaml index 5d37962b..ef982dd0 100644 --- a/kuka_sunrise_fri_driver/test/config/test1_joint_trajectory_controller_config.yaml +++ b/kuka_sunrise_fri_driver/test/config/test1_joint_trajectory_controller_config.yaml @@ -14,4 +14,4 @@ test1/joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_sunrise_fri_driver/test/config/test2_joint_trajectory_controller_config.yaml b/kuka_sunrise_fri_driver/test/config/test2_joint_trajectory_controller_config.yaml index e9f93524..03fd32a8 100644 --- a/kuka_sunrise_fri_driver/test/config/test2_joint_trajectory_controller_config.yaml +++ b/kuka_sunrise_fri_driver/test/config/test2_joint_trajectory_controller_config.yaml @@ -14,4 +14,4 @@ test2/joint_trajectory_controller: - position state_publish_rate: 50.0 action_monitor_rate: 20.0 - allow_nonzero_velocity_at_trajectory_end: True + allow_nonzero_velocity_at_trajectory_end: true diff --git a/kuka_sunrise_fri_driver/test/test_driver_startup.py b/kuka_sunrise_fri_driver/test/test_driver_startup.py index 02ee48bc..2cb9b941 100644 --- a/kuka_sunrise_fri_driver/test/test_driver_startup.py +++ b/kuka_sunrise_fri_driver/test/test_driver_startup.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py b/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py index c9c5f84c..b256b304 100644 --- a/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py +++ b/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py @@ -1,4 +1,4 @@ -# Copyright 2024 Áron Svastits +# Copyright 2024 Aron Svastits # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/sonar-project.properties b/sonar-project.properties index f01f0107..1323fdaa 100644 --- a/sonar-project.properties +++ b/sonar-project.properties @@ -4,7 +4,7 @@ sonar.projectKey=kroshu_kuka_drivers sonar.host.url=https://sonarcloud.io -sonar.modules=examples,kuka_sunrise_fri_driver, kuka_kss_rsi_driver, kuka_iiqka_eac_driver, kuka_drivers_core +sonar.modules=examples,kuka_sunrise_fri_driver, kuka_kss_rsi_driver, kuka_iiqka_eac_driver, kuka_drivers_core, controllers examples.sonar.projectName=examples examples.sonar.sources=./iiqka_moveit_example/src, ./iiqka_moveit_example/launch @@ -18,6 +18,8 @@ kuka_iiqka_eac_driver.sonar.sources=./src,./include/kuka_iiqka_eac_driver kuka_iiqka_eac_driver.sonar.exclusions=**/mock/** kuka_drivers_core.sonar.projectName=kuka_drivers_core kuka_drivers_core.sonar.sources=./src,./include +controllers.sonar.projectName=controllers +controllers.sonar.sources=./control_mode_handler, ./event_broadcaster, ./fri_configuration_controller, ./fri_state_broadcaster, ./joint_group_impedance_controller # Encoding of the source code. Default is default system encoding sonar.sourceEncoding=UTF-8 diff --git a/upstream.repos b/upstream.repos index 7aec6253..6fab7a57 100644 --- a/upstream.repos +++ b/upstream.repos @@ -4,7 +4,7 @@ repositories: type: git url: https://github.com/kroshu/kuka_robot_descriptions.git version: master - kuka_controllers: + kuka-external-control-sdk: type: git - url: https://github.com/kroshu/kuka_controllers.git + url: https://github.com/kroshu/kuka-external-control-sdk.git version: master