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 d1f1cc2f..6d5cc64e 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: @@ -36,9 +35,7 @@ jobs: BUILDER: colcon ANALYZER: sonarqube TEST_COVERAGE: true - # Hacky solution needed to be able to build upstream WS: - # kuka_driver_interfaces and kuka_drivers_core must be also added to upstream - UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka-external-control-sdk#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' + UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka-external-control-sdk#master' ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) 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 98% rename from doc/wiki/iiQKA_EAC.md rename to doc/wiki/1_iiQKA_EAC.md index 5c5dcda4..daebbece 100644 --- a/doc/wiki/iiQKA_EAC.md +++ b/doc/wiki/1_iiQKA_EAC.md @@ -54,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`): ``` diff --git a/doc/wiki/KSS_RSI.md b/doc/wiki/2_KSS_RSI.md similarity index 100% rename from doc/wiki/KSS_RSI.md rename to doc/wiki/2_KSS_RSI.md diff --git a/doc/wiki/Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md similarity index 97% rename from doc/wiki/Sunrise_FRI.md rename to doc/wiki/3_Sunrise_FRI.md index b46f81de..b81d339b 100644 --- a/doc/wiki/Sunrise_FRI.md +++ b/doc/wiki/3_Sunrise_FRI.md @@ -45,8 +45,8 @@ The parameters in the driver configuration file can be also changed during runti - 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_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) + - [`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) 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`): ``` 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 95% rename from doc/wiki/Realtime.md rename to doc/wiki/5_Realtime.md index 0d8d7f53..f728f63a 100644 --- a/doc/wiki/Realtime.md +++ b/doc/wiki/5_Realtime.md @@ -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: diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index 226e0070..e4ac17fc 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): diff --git a/kuka_drivers/package.xml b/kuka_drivers/package.xml index a6a5d857..7c0409ea 100644 --- a/kuka_drivers/package.xml +++ b/kuka_drivers/package.xml @@ -20,6 +20,8 @@ kuka_kss_rsi_driver kuka_rsi_simulator kuka_sunrise_fri_driver + + kuka_controllers ament_cmake diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index f178ead7..bbe5e6d1 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -20,9 +20,12 @@ kuka_lbr_iisy_support ros2_control - ros2_controllers - kuka_controllers + joint_trajectory_controller effort_controllers + joint_state_broadcaster + control_mode_handler + event_broadcaster + joint_group_impedance_controller ros2lifecycle diff --git a/kuka_kss_rsi_driver/package.xml b/kuka_kss_rsi_driver/package.xml index fd48fb1c..c851c3ab 100644 --- a/kuka_kss_rsi_driver/package.xml +++ b/kuka_kss_rsi_driver/package.xml @@ -21,8 +21,10 @@ 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/test/test_driver_activation.py b/kuka_kss_rsi_driver/test/test_driver_activation.py index f24af363..2bedb4eb 100644 --- a/kuka_kss_rsi_driver/test/test_driver_activation.py +++ b/kuka_kss_rsi_driver/test/test_driver_activation.py @@ -52,7 +52,7 @@ def generate_test_description(): ) ), launch.actions.TimerAction( - period=5.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=10.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_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index 5b983aa0..7deb6ce7 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -21,8 +21,10 @@ libnanopb-dev ros2_control - ros2_controllers - kuka_controllers + joint_trajectory_controller + joint_state_broadcaster + fri_configuration_controller + fri_state_broadcaster kuka_lbr_iiwa_support 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 f3e59fb6..6fab7a57 100644 --- a/upstream.repos +++ b/upstream.repos @@ -4,10 +4,6 @@ repositories: type: git url: https://github.com/kroshu/kuka_robot_descriptions.git version: master - kuka_controllers: - type: git - url: https://github.com/kroshu/kuka_controllers.git - version: master kuka-external-control-sdk: type: git url: https://github.com/kroshu/kuka-external-control-sdk.git