From 38ffd1a2681fc0485ab7e9adc92b8a4c2d2a4077 Mon Sep 17 00:00:00 2001 From: soge0001 Date: Tue, 28 Feb 2023 17:42:05 +0100 Subject: [PATCH] added EKI interfaces --- kuka_eki/krl/EkiHwInterface.xml | 71 ++++ kuka_eki/krl/EkiIOInterface.xml | 41 +++ kuka_eki/krl/kuka_eki_hw_interface.dat | 7 + kuka_eki/krl/kuka_eki_hw_interface.src | 347 ++++++++++++++++++ kuka_eki/kuka_eki_hw_interface/CMakeLists.txt | 89 +++++ .../kuka_eki_hw_interface.h | 91 +++++ .../visibility_control.h | 56 +++ .../launch/controller_spawner.launch.py | 105 ++++++ kuka_eki/kuka_eki_hw_interface/package.xml | 23 ++ .../ros2_control_kuka_eki.xml | 9 + .../src/kuka_eki_hw_interface.cpp | 296 +++++++++++++++ kuka_eki/kuka_eki_io_interface/CMakeLists.txt | 98 +++++ kuka_eki/kuka_eki_io_interface/README.md | 131 +++++++ .../kuka_eki_io_interface.h | 36 ++ .../launch/eki_io_interface.launch.py | 45 +++ kuka_eki/kuka_eki_io_interface/package.xml | 23 ++ .../src/kuka_eki_io_interface.cpp | 156 ++++++++ .../src/kuka_eki_io_interface_node.cpp | 60 +++ 18 files changed, 1684 insertions(+) create mode 100644 kuka_eki/krl/EkiHwInterface.xml create mode 100644 kuka_eki/krl/EkiIOInterface.xml create mode 100644 kuka_eki/krl/kuka_eki_hw_interface.dat create mode 100644 kuka_eki/krl/kuka_eki_hw_interface.src create mode 100644 kuka_eki/kuka_eki_hw_interface/CMakeLists.txt create mode 100644 kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h create mode 100644 kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h create mode 100644 kuka_eki/kuka_eki_hw_interface/launch/controller_spawner.launch.py create mode 100644 kuka_eki/kuka_eki_hw_interface/package.xml create mode 100644 kuka_eki/kuka_eki_hw_interface/ros2_control_kuka_eki.xml create mode 100644 kuka_eki/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp create mode 100644 kuka_eki/kuka_eki_io_interface/CMakeLists.txt create mode 100644 kuka_eki/kuka_eki_io_interface/README.md create mode 100644 kuka_eki/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h create mode 100644 kuka_eki/kuka_eki_io_interface/launch/eki_io_interface.launch.py create mode 100644 kuka_eki/kuka_eki_io_interface/package.xml create mode 100644 kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp create mode 100644 kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp diff --git a/kuka_eki/krl/EkiHwInterface.xml b/kuka_eki/krl/EkiHwInterface.xml new file mode 100644 index 000000000..05e6f37e7 --- /dev/null +++ b/kuka_eki/krl/EkiHwInterface.xml @@ -0,0 +1,71 @@ + + + + Client + + + Program + + + 10.181.116.41 + 54600 + UDP + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kuka_eki/krl/EkiIOInterface.xml b/kuka_eki/krl/EkiIOInterface.xml new file mode 100644 index 000000000..5d0f30053 --- /dev/null +++ b/kuka_eki/krl/EkiIOInterface.xml @@ -0,0 +1,41 @@ + + + + Client + + + Program + + + 10.181.116.41 + 54601 + UDP + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kuka_eki/krl/kuka_eki_hw_interface.dat b/kuka_eki/krl/kuka_eki_hw_interface.dat new file mode 100644 index 000000000..479da3ee5 --- /dev/null +++ b/kuka_eki/krl/kuka_eki_hw_interface.dat @@ -0,0 +1,7 @@ +&ACCESS RVP +defdat kuka_eki_hw_interface + ext bas(bas_command :in,real :in ) + decl int io_pins[2] + decl int io_types[2] + decl int io_cmd[2] +enddat \ No newline at end of file diff --git a/kuka_eki/krl/kuka_eki_hw_interface.src b/kuka_eki/krl/kuka_eki_hw_interface.src new file mode 100644 index 000000000..412c7a32d --- /dev/null +++ b/kuka_eki/krl/kuka_eki_hw_interface.src @@ -0,0 +1,347 @@ +&ACCESS RVP +def kuka_eki_hw_interface() + + ; Software License Agreement (BSD License) + ; + ; Copyright (c) 2018, 3M + ; All rights reserved. + ; + ; Redistribution and use in source and binary forms, with or without + ; modification, are permitted provided that the following conditions are met: + ; + ; * Redistributions of source code must retain the above copyright + ; notice, this list of conditions and the following disclQaimer. + ; * Redistributions in binary form must reproduce the above copyright + ; notice, this list of conditions and the following disclaimer in the + ; documentation and/or other materials provided with the distribution. + ; * Neither the name of the copyright holder, nor the names of its + ; contributors may be used to endorse or promote products derived + ; from this software without specific prior written permission. + ; + ; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + ; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + ; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + ; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + ; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + ; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + ; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + ; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + ; POSSIBILITY OF SUCH DAMAGE. + + ; Author: Brett Hemes (3M) + + ; Declarations + decl axis joint_pos_tgt + decl int elements_read + decl int io_elements_read + decl int io_elements_set + decl int index + + ; INI + bas(#initmov, 0) ; Basic initializasion of axes + + ; Initialize eki_hw_interface server + ; Config located in C:/ROBOTER/Config/User/Common/EthernetKRL/EkiHwInterface.xml + ; Starts a TCP state sever on xml-specified IP address/port. + ; + ; State transmission is periodic (currently determined by $timer[1]=-## line in + ; eki_hw_iface_send() + ; + ; Joint position commands execute as they come in. + ; PTP motions are blended in joint-space via c_ptp approximation when possible + ; via the advance run. The advance run will read and blend command points as + ; soon as the become available up to value set in $advance (in range [0, 5]) + ; + ; Utilized system resources: + ; Flags: + ; $flag[1]: Indicates active client connection + ; $timer_flag[1]: Used to trigger periodic send of robot state + ; Interrupts: + ; 15: Calls eki_hw_iface_reset() on falling edge of $flag[1] + ; 16: Calls eki_hw_iface_send() on rising edge of $timer_flag[1] + eki_hw_iface_init() + eki_io_iface_init() + + ; BCO (Block COincidence) run to current position + ; (requied for below loop continue before first incoming command) + joint_pos_tgt = $axis_act_meas + ; additional comment + ptp joint_pos_tgt + + for index = 1 to 2 + io_pins[index] = index + io_types[index] = 1 + io_cmd[index] = -1 + endfor + + eki_io_iface_send() + + ; Loop forever + $advance = 5 + loop + ; TODO: check return values + elements_read = eki_hw_iface_get(joint_pos_tgt) ; Get new command from buffer if present + if elements_read == 1 then + ptp joint_pos_tgt c_ptp ; PTP to most recent commanded position + endif + io_elements_read = eki_io_iface_get() + if io_elements_read == 1 then + set_io_values() + eki_io_iface_send() + endif + endloop + + ; Note: EKI channels delete on reset or deselect of this program + ; See Program EKI config element +end + +def eki_hw_iface_init() + decl eki_status eki_ret + + ; Setup interrupts + ; Interrupt 15 - Connection cleanup on client disconnect + global interrupt decl 15 when $flag[1]==false do eki_hw_iface_reset() + interrupt on 15 + ; Interrupt 16 - Timer interrupt for periodic state transmission + global interrupt decl 16 when $timer_flag[1]==true do eki_hw_iface_send() + interrupt on 16 + wait sec 0.012 ; Wait for next interpolation cycle + $timer[1] = -200 ; Time in [ms] before first interrupt call + $timer_stop[1] = false ; Start timer 1 + + ; Create and open EKI interface + eki_ret = eki_init("EkiHwInterface") + eki_ret = eki_open("EkiHwInterface") +end + +def eki_io_iface_init() + decl eki_status eki_ret + + ; Setup interrupts + ; Interrupt 15 - Connection cleanup on client disconnect + ;global interrupt decl 17 when $flag[2]==false do eki_io_iface_reset() + ;interrupt on 17 + ; Interrupt 16 - Timer interrupt for periodic state transmission + ;global interrupt decl 18 when $timer_flag[2]==true do eki_io_iface_send() + ;interrupt on 18 + ;wait sec 0.012 ; Wait for next interpolation cycle + ;$timer[2] = -200 ; Time in [ms] before first interrupt call + ;$timer_stop[2] = false ; Start timer 1 + + ; Create and open EKI interface + eki_ret = eki_init("EkiIOInterface") + eki_ret = eki_open("EkiIOInterface") +end + +def eki_io_iface_send() + decl eki_status eki_ret + decl int index + if $flag[2] then + index = 1 + eki_ret = eki_setint("EkiIOInterface", "IOState/IO1/@Type", io_types[index]) + eki_ret = eki_setint("EkiIOInterface", "IOState/IO1/@Pin", io_pins[index]) + switch io_types[index] + case 1 + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO1/@State", $in[io_pins[index]]) + case 2 + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO1/@State", $out[io_pins[index]]) + default + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO1/@State", False) + endswitch + + index = 2 + eki_ret = eki_setint("EkiIOInterface", "IOState/IO2/@Type", io_types[index]) + eki_ret = eki_setint("EkiIOInterface", "IOState/IO2/@Pin", io_pins[index]) + switch io_types[index] + case 1 + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO2/@State", $in[io_pins[index]]) + case 2 + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO2/@State", $out[io_pins[index]]) + default + eki_ret = eki_setbool("EkiIOInterface", "IOState/IO2/@State", False) + endswitch + eki_ret = eki_checkbuffer("EkiIOInterface", "IOCommand/IO1/@Mode") + eki_ret = eki_setint("EkiIOInterface", "IOState/IOCommand/@Size", eki_ret.buff) + ; Send xml structure + if $flag[2] then ; Make sure connection hasn't died while updating xml structure + eki_ret = eki_send("EkiIOInterface", "IOState") + endif + endif + + + ; Set timer for next interrupt [ms] + $timer[2] = -10 ; ~10 ms for above send + 10 ms interrupt timer -> ~50 Hz state transmission +end + +def eki_hw_iface_send() + decl eki_status eki_ret + decl real vel_percent + if $flag[1] then ; If connection alive + ; Load state values into xml structure + ; position + eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A1", $axis_act_meas.a1) + eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A2", $axis_act_meas.a2) + eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A3", $axis_act_meas.a3) + eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A4", $axis_act_meas.a4) + eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A5", $axis_act_meas.a5) + eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A6", $axis_act_meas.a6) + ; velocity + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A1", $vel_axis_act[1] * $vel_axis_ma[1] / 100.0) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A2", $vel_axis_act[2] * $vel_axis_ma[2] / 100.0) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A3", $vel_axis_act[3] * $vel_axis_ma[3] / 100.0) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A4", $vel_axis_act[4] * $vel_axis_ma[4] / 100.0) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A5", $vel_axis_act[5] * $vel_axis_ma[5] / 100.0) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A6", $vel_axis_act[6] * $vel_axis_ma[6] / 100.0) + ; effort + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A1", $torque_axis_act[1]) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A2", $torque_axis_act[2]) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A3", $torque_axis_act[3]) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A4", $torque_axis_act[4]) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A5", $torque_axis_act[5]) + ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A6", $torque_axis_act[6]) + ; interface state + eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") + eki_ret = eki_setint("EkiHwInterface", "RobotState/RobotCommand/@Size", eki_ret.buff) + + ; Send xml structure + if $flag[1] then ; Make sure connection hasn't died while updating xml structure + eki_ret = eki_send("EkiHwInterface", "RobotState") + endif + endif + + ; Set timer for next interrupt [ms] + $timer[1] = -10 ; ~10 ms for above send + 10 ms interrupt timer -> ~50 Hz state transmission +end + +deffct int eki_hw_iface_available() + decl eki_status eki_ret + continue + if not $flag[2] then + return 0 + endif + + eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") + $timer[2] = -10 + return eki_ret.buff +endfct + +; eki_hw_iface_get +; Tries to read most recent elemnt from buffer. q left unchanged if empty. +; Returns number of elements read. +deffct int eki_hw_iface_get(joint_pos_cmd :out) + decl eki_status eki_ret + decl axis joint_pos_cmd + + if not $flag[1] then + return 0 + endif + + eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") + + if eki_ret.buff <= 0 then + return 0 + endif + + eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A1", joint_pos_cmd.a1) + eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A2", joint_pos_cmd.a2) + eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A3", joint_pos_cmd.a3) + eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A4", joint_pos_cmd.a4) + eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A5", joint_pos_cmd.a5) + eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A6", joint_pos_cmd.a6) + return 1 +endfct + +; eki_io_iface_get +; Tries to read most recent elemnt from buffer. q left unchanged if empty. +; Returns number of elements read. +deffct int eki_io_iface_get() + decl eki_status eki_ret + decl int index + decl int io_mode + decl int value + + if not $flag[2] then + return 0 + endif + + eki_ret = eki_checkbuffer("EkiIOInterface", "IOCommand/IO1/@Mode") + + if eki_ret.buff <= 0 then + return 0 + endif + + io_mode = 0 + index = 1 + value = -1 + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO1/@Mode", io_mode) + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO1/@Pin", io_pins[index]) + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO1/@Value", value) + switch io_mode + case 1 + io_types[index] = 1 + io_cmd[index] = -1 + case 2 + io_types[index] = 2 + io_cmd[index] = value + case 3 + io_types[index] = 2 + io_cmd[index] = -1 + default + io_types[index] = 0 + io_cmd[index] = -1 + endswitch + index = 2 + value = -1 + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO2/@Mode", io_mode) + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO2/@Pin", io_pins[index]) + eki_ret = eki_getint("EkiIOInterface", "IOCommand/IO2/@Value", value) + switch io_mode + case 1 + io_types[index] = 1 + io_cmd[index] = -1 + case 2 + io_types[index] = 2 + io_cmd[index] = value + case 3 + io_types[index] = 2 + io_cmd[index] = -1 + default + io_types[index] = 0 + io_cmd[index] = -1 + endswitch + + + return 1 +endfct + +; set_io_values +; Tries to read most recent elemnt from buffer. q left unchanged if empty. +; Returns number of elements read. +def set_io_values() + decl int index + for index = 1 to 2 + if (io_cmd[index] <> -1) then + REPEAT + $out[io_pins[index]] = (io_cmd[index] <> 0) + UNTIL $out[io_pins[index]] == (io_cmd[index] <> 0) + endif + endfor +end + +def eki_hw_iface_reset() + decl eki_status eki_ret + + eki_ret = eki_clear("EkiHwInterface") + eki_ret = eki_init("EkiHwInterface") + eki_ret = eki_open("EkiHwInterface") +end + +def eki_io_iface_reset() + decl eki_status eki_ret + + eki_ret = eki_clear("EkiIOInterface") + eki_ret = eki_init("EkiIOInterface") + eki_ret = eki_open("EkiIOInterface") +end \ No newline at end of file diff --git a/kuka_eki/kuka_eki_hw_interface/CMakeLists.txt b/kuka_eki/kuka_eki_hw_interface/CMakeLists.txt new file mode 100644 index 000000000..92372f906 --- /dev/null +++ b/kuka_eki/kuka_eki_hw_interface/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.5) +project(kuka_eki_hw_interface) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) +find_package(tinyxml_vendor REQUIRED) +find_package(TinyXML REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +add_library( + ${PROJECT_NAME} + SHARED + src/kuka_eki_hw_interface.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include +) +ament_target_dependencies( + ${PROJECT_NAME} + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + TinyXML +) +pluginlib_export_plugin_description_file(hardware_interface ros2_control_kuka_eki.xml) + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +### ros2 control hardware plugin +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib +) +install( + DIRECTORY include/ + DESTINATION include +) +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) +ament_package() diff --git a/kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h b/kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h new file mode 100644 index 000000000..0e8d7ee24 --- /dev/null +++ b/kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h @@ -0,0 +1,91 @@ +// Copyright 2020 ros2_control Development Team +// +// 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 ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_ +#define ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "tinyxml.h" +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "rclcpp/macros.hpp" +#include "kuka_eki_hw_interface/visibility_control.h" + + +namespace kuka_eki_hw_interface +{ + class KukaEkiHardwareInterface : public hardware_interface::BaseInterface + { + public: + RCLCPP_SHARED_PTR_DEFINITIONS(KukaEkiHardwareInterface) + + ROS2_CONTROL_KUKA_EKI_HW_PUBLIC + hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_KUKA_EKI_HW_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_KUKA_EKI_HW_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_KUKA_EKI_HW_PUBLIC + hardware_interface::return_type start() override; + + ROS2_CONTROL_KUKA_EKI_HW_PUBLIC + hardware_interface::return_type stop() override; + + ROS2_CONTROL_KUKA_EKI_HW_PUBLIC + hardware_interface::return_type read() override; + + ROS2_CONTROL_KUKA_EKI_HW_PUBLIC + hardware_interface::return_type write() override; + + private: + // Store the command for the simulated robot + std::vector hw_commands_; + std::vector hw_states_; + + std::string eki_server_address_; + std::string eki_server_port_; + int eki_cmd_buff_len_; + int eki_max_cmd_buff_len_ = 5; // by default, limit command buffer to 5 (size of advance run in KRL) +// +// // EKI socket read/write + int eki_read_state_timeout_ = 5; // [s]; settable by parameter (default = 5) + boost::asio::io_service ios_; + std::unique_ptr deadline_; + boost::asio::ip::udp::endpoint eki_server_endpoint_; + std::unique_ptr eki_server_socket_; + void eki_check_read_state_deadline(); + static void eki_handle_receive(const boost::system::error_code &ec, size_t length, + boost::system::error_code* out_ec, size_t* out_length); + bool eki_read_state(std::vector &joint_position, std::vector &joint_velocity, + std::vector &joint_effort, int &cmd_buff_len); + bool eki_write_command(const std::vector &joint_position); + }; + +} // namespace kuka_eki_hw_interface + +#endif // ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_ \ No newline at end of file diff --git a/kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h b/kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h new file mode 100644 index 000000000..90cacbbcb --- /dev/null +++ b/kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROS2_CONTROL_KUKA_EKI_HW__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_KUKA_EKI_HW__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 ROS2_CONTROL_KUKA_EKI_HW_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_KUKA_EKI_HW_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_KUKA_EKI_HW_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_KUKA_EKI_HW_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_KUKA_EKI_HW_BUILDING_DLL +#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC ROS2_CONTROL_KUKA_EKI_HW_EXPORT +#else +#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC ROS2_CONTROL_KUKA_EKI_HW_IMPORT +#endif +#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC_TYPE ROS2_CONTROL_KUKA_EKI_HW_PUBLIC +#define ROS2_CONTROL_KUKA_EKI_HW_LOCAL +#else +#define ROS2_CONTROL_KUKA_EKI_HW_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_KUKA_EKI_HW_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_KUKA_EKI_HW_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC +#define ROS2_CONTROL_KUKA_EKI_HW_LOCAL +#endif +#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_KUKA_EKI_HW__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/kuka_eki/kuka_eki_hw_interface/launch/controller_spawner.launch.py b/kuka_eki/kuka_eki_hw_interface/launch/controller_spawner.launch.py new file mode 100644 index 000000000..bf84ab792 --- /dev/null +++ b/kuka_eki/kuka_eki_hw_interface/launch/controller_spawner.launch.py @@ -0,0 +1,105 @@ +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append(DeclareLaunchArgument( + "robot_description_package", + default_value="kuka_kr3_support", + description="Robot description package.", + )) + + declared_arguments.append(DeclareLaunchArgument( + "robot_description_file", + default_value="kr3r540.xacro", + description="Robot description file located in /urdf/ .", + )) + + declared_arguments.append(DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with fake hardware mirroring command to its states.", + )) + + declared_arguments.append(DeclareLaunchArgument( + "robot_ip", + default_value="10.181.116.41", + description="IP address by which the robot can be reached." + )) + + declared_arguments.append(DeclareLaunchArgument( + "robot_port", + default_value="54600", + description="Port by which the robot can be reached." + )) + + robot_description_package = LaunchConfiguration("robot_description_package") + robot_description_file = LaunchConfiguration("robot_description_file") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + robot_ip = LaunchConfiguration("robot_ip") + robot_port = LaunchConfiguration("robot_port") + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(robot_description_package), "urdf", robot_description_file]), + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "robot_ip:=", + robot_ip, + " ", + "robot_port:=", + robot_port, + ] + ) + robot_description = {"robot_description": robot_description_content} + + controllers_file = os.path.join( + get_package_share_directory("kuka_resources"), + 'config', + 'kuka_6dof_controllers.yaml' + ) + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + robot_description, + controllers_file + ], + output='screen' + ) + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[ + robot_description + ], + output='screen' + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + initial_joint_controller_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["position_trajectory_controller", "-c", "/controller_manager"], + ) + return LaunchDescription(declared_arguments + [ + controller_manager, robot_state_publisher, joint_state_broadcaster_spawner, + initial_joint_controller_spawner + ]) diff --git a/kuka_eki/kuka_eki_hw_interface/package.xml b/kuka_eki/kuka_eki_hw_interface/package.xml new file mode 100644 index 000000000..13d8cadf8 --- /dev/null +++ b/kuka_eki/kuka_eki_hw_interface/package.xml @@ -0,0 +1,23 @@ + + + + kuka_eki_hw_interface + 0.0.0 + TODO: Package description + robot + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + + ament_cmake + + diff --git a/kuka_eki/kuka_eki_hw_interface/ros2_control_kuka_eki.xml b/kuka_eki/kuka_eki_hw_interface/ros2_control_kuka_eki.xml new file mode 100644 index 000000000..0e50359c0 --- /dev/null +++ b/kuka_eki/kuka_eki_hw_interface/ros2_control_kuka_eki.xml @@ -0,0 +1,9 @@ + + + + Minimal hardware interface implementation for use with KUKA EKI. + + + \ No newline at end of file diff --git a/kuka_eki/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp b/kuka_eki/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp new file mode 100644 index 000000000..0e0c0e43e --- /dev/null +++ b/kuka_eki/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp @@ -0,0 +1,296 @@ +// Copyright 2020 ros2_control Development Team +// +// 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_eki_hw_interface/kuka_eki_hw_interface.h" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace kuka_eki_hw_interface +{ + hardware_interface::return_type KukaEkiHardwareInterface::configure(const hardware_interface::HardwareInfo & info) + { + if (configure_default(info) != hardware_interface::return_type::OK) + { + return hardware_interface::return_type::ERROR; + } + + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // RRBotSystemPositionOnly has exactly one state and command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaEkiHardwareInterface"), + "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::return_type::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaEkiHardwareInterface"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::return_type::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaEkiHardwareInterface"), + "Joint '%s' has %d state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::return_type::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL(rclcpp::get_logger("KukaEkiHardwareInterface"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::return_type::ERROR; + } + } + + status_ = hardware_interface::status::CONFIGURED; + return hardware_interface::return_type::OK; + } + + std::vector KukaEkiHardwareInterface::export_state_interfaces() + { + std::vector state_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + } + + return state_interfaces; + } + + std::vector KukaEkiHardwareInterface::export_command_interfaces() + { + std::vector command_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + + return command_interfaces; + } + + void KukaEkiHardwareInterface::eki_check_read_state_deadline() + { + // Check if deadline has already passed + if (deadline_->expires_at() <= boost::asio::deadline_timer::traits_type::now()) + { + eki_server_socket_->cancel(); + deadline_->expires_at(boost::posix_time::pos_infin); + } + + // Sleep until deadline exceeded + deadline_->async_wait(boost::bind(&KukaEkiHardwareInterface::eki_check_read_state_deadline, this)); + } + + void KukaEkiHardwareInterface::eki_handle_receive(const boost::system::error_code &ec, size_t length, + boost::system::error_code* out_ec, size_t* out_length) + { + *out_ec = ec; + *out_length = length; + } + + bool KukaEkiHardwareInterface::eki_read_state(std::vector &joint_position, + std::vector &joint_velocity, + std::vector &joint_effort, + int &cmd_buff_len) + { + static boost::array in_buffer; + // Read socket buffer (with timeout) + // Based off of Boost documentation example: doc/html/boost_asio/example/timeouts/blocking_udp_client.cpp + deadline_->expires_from_now(boost::posix_time::seconds(eki_read_state_timeout_)); // set deadline + boost::system::error_code ec = boost::asio::error::would_block; + size_t len = 0; + eki_server_socket_->async_receive(boost::asio::buffer(in_buffer), + boost::bind(&KukaEkiHardwareInterface::eki_handle_receive, _1, _2, &ec, &len)); + do + ios_.run_one(); + while (ec == boost::asio::error::would_block); + if (ec) + return false; + + // Update joint positions from XML packet (if received) + if (len == 0) + return false; + + // Parse XML + TiXmlDocument xml_in; + in_buffer[len] = '\0'; // null-terminate data buffer for parsing (expects c-string) + xml_in.Parse(in_buffer.data()); + TiXmlElement* robot_state = xml_in.FirstChildElement("RobotState"); + if (!robot_state) + return false; + TiXmlElement* pos = robot_state->FirstChildElement("Pos"); + TiXmlElement* vel = robot_state->FirstChildElement("Vel"); + TiXmlElement* eff = robot_state->FirstChildElement("Eff"); + TiXmlElement* robot_command = robot_state->FirstChildElement("RobotCommand"); + if (!pos || !vel || !eff || !robot_command) + return false; + + // Extract axis positions + double joint_pos; // [deg] + double joint_vel; // [%max] + double joint_eff; // [Nm] + char axis_name[] = "A1"; + for (long unsigned int i = 0; i < hw_states_.size(); ++i) + { + pos->Attribute(axis_name, &joint_pos); + joint_position[i] = angles::from_degrees(joint_pos); // convert deg to rad + vel->Attribute(axis_name, &joint_vel); + joint_velocity[i] = joint_vel; + eff->Attribute(axis_name, &joint_eff); + joint_effort[i] = joint_eff; + axis_name[1]++; + } + + // Extract number of command elements buffered on robot + robot_command->Attribute("Size", &cmd_buff_len); + return true; + } + + bool KukaEkiHardwareInterface::eki_write_command(const std::vector &joint_position_command) + { + TiXmlDocument xml_out; + TiXmlElement* robot_command = new TiXmlElement("RobotCommand"); + TiXmlElement* pos = new TiXmlElement("Pos"); + TiXmlText* empty_text = new TiXmlText(""); + robot_command->LinkEndChild(pos); + pos->LinkEndChild(empty_text); // force format (vs ) + char axis_name[] = "A1"; + for (long unsigned int i = 0; i < hw_states_.size(); ++i) + { + pos->SetAttribute(axis_name, std::to_string(angles::to_degrees(joint_position_command[i])).c_str()); + axis_name[1]++; + } + xml_out.LinkEndChild(robot_command); + TiXmlPrinter xml_printer; + xml_printer.SetStreamPrinting(); // no linebreaks + xml_out.Accept(&xml_printer); + + eki_server_socket_->send_to(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()), + eki_server_endpoint_); + + return true; + } + + hardware_interface::return_type KukaEkiHardwareInterface::start() + { + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "Starting ...please wait......................................................................................................................."); + + eki_server_address_ = info_.hardware_parameters["robot_ip"]; + eki_server_port_ = info_.hardware_parameters["robot_port"]; + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), eki_server_address_); + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), eki_server_port_); + deadline_.reset(new boost::asio::deadline_timer(ios_)); + eki_server_socket_.reset(new boost::asio::ip::udp::socket(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0))); + + boost::asio::ip::udp::resolver resolver(ios_); + eki_server_endpoint_ = *resolver.resolve({boost::asio::ip::udp::v4(), eki_server_address_, eki_server_port_}); + + boost::array ini_buf = { 0 }; + eki_server_socket_->send_to(boost::asio::buffer(ini_buf), eki_server_endpoint_); // initiate contact to start server + + // Start persistent actor to check for eki_read_state timeouts + deadline_->expires_at(boost::posix_time::pos_infin); // do nothing unit a read is invoked (deadline_ = +inf) + eki_check_read_state_deadline(); + + std::vector joint_position; + std::vector joint_velocity; + std::vector joint_effort; + joint_position.resize(hw_states_.size()); + joint_velocity.resize(hw_states_.size()); + joint_effort.resize(hw_states_.size()); + if (!eki_read_state(joint_position, joint_velocity, joint_effort, eki_cmd_buff_len_)) + { + std::string msg = "Failed to read from robot EKI server within alloted time of " + + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running " + "on the robot controller and all configurations are correct."; + RCLCPP_ERROR( + rclcpp::get_logger("KukaEkiHardwareInterface"), msg); + throw std::runtime_error(msg); + } + hw_commands_ = joint_position; + hw_states_ = joint_position; + + status_ = hardware_interface::status::STARTED; + + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "System Successfully started!......................................................................................................................."); + + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type KukaEkiHardwareInterface::stop() + { + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "Stopping ...please wait..."); + + status_ = hardware_interface::status::STOPPED; + + RCLCPP_INFO(rclcpp::get_logger("KukaEkiHardwareInterface"), "System successfully stopped!"); + + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type KukaEkiHardwareInterface::read() + { + std::vector joint_position; + std::vector joint_velocity; + std::vector joint_effort; + joint_position.resize(hw_states_.size()); + joint_velocity.resize(hw_states_.size()); + joint_effort.resize(hw_states_.size()); + if (!eki_read_state(joint_position, joint_velocity, joint_effort, eki_cmd_buff_len_)) + { + std::string msg = "Failed to read from robot EKI server within alloted time of " + + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running " + "on the robot controller and all configurations are correct."; + RCLCPP_ERROR( + rclcpp::get_logger("KukaEkiHardwareInterface"), msg); + throw std::runtime_error(msg); + } + hw_states_ = joint_position; + + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type KukaEkiHardwareInterface::write() + { + if (eki_cmd_buff_len_ < eki_max_cmd_buff_len_) + eki_write_command(hw_commands_); + return hardware_interface::return_type::OK; + } + +} // namespace kuka_eki_hw_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + kuka_eki_hw_interface::KukaEkiHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/kuka_eki/kuka_eki_io_interface/CMakeLists.txt b/kuka_eki/kuka_eki_io_interface/CMakeLists.txt new file mode 100644 index 000000000..db2bdb241 --- /dev/null +++ b/kuka_eki/kuka_eki_io_interface/CMakeLists.txt @@ -0,0 +1,98 @@ +cmake_minimum_required(VERSION 3.5) +project(kuka_eki_io_interface) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +add_compile_options(-std=c++11) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) +find_package(tinyxml_vendor REQUIRED) +find_package(TinyXML REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +add_library( + ${PROJECT_NAME} + SHARED + src/kuka_eki_io_interface.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include +) +ament_target_dependencies( + ${PROJECT_NAME} + rclcpp + rclcpp_lifecycle + TinyXML +) + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + rclcpp + rclcpp_lifecycle +) + + +### ros2 control hardware plugin +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib +) +install( + DIRECTORY include/ + DESTINATION include +) + +add_executable(eki_io_node src/kuka_eki_io_interface_node.cpp src/kuka_eki_io_interface.cpp) +target_include_directories( + eki_io_node + PRIVATE + include) +ament_target_dependencies(eki_io_node ${${PROJECT_NAME}_EXPORTED_TARGETS} rclcpp std_msgs rclcpp_lifecycle TinyXML) +install(TARGETS + eki_io_node + DESTINATION lib/${PROJECT_NAME}) + + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/kuka_eki/kuka_eki_io_interface/README.md b/kuka_eki/kuka_eki_io_interface/README.md new file mode 100644 index 000000000..0daed5151 --- /dev/null +++ b/kuka_eki/kuka_eki_io_interface/README.md @@ -0,0 +1,131 @@ +# KUKA EKI IO Interface +This package interfaces KUKA IOs over EKI. + +The number of simultaneously supported operations is currently set to 2. If you need more, you have to alter +the KRL code and upload it to the controller. + +A command takes a pin, a mode and a state for each operation. +The pin is the number of the IO pin on the controller, the mode is the type of the IO (0: do nothing, 1: read input, +2: write output, 3: read output) and the state is the value of the IO (true: high, false: low). +State is only relevant for writing output pins. + +## Example usage as a ROS node + +io_example.h + +```cpp +#ifndef IO_EXAMPLE +#define IO_EXAMPLE + +#include "rclcpp/rclcpp.hpp" +#include +#include + +namespace io_example +{ + class IOExample : public rclcpp::Node + { + public: + IOExample(); + ~IOExample() {}; + + private: + std::shared_ptr _kuka_eki_io_interface; + rclcpp::Service::SharedPtr _example_srv; + void example_io(const std::shared_ptr request, + std::shared_ptr response); + void eki_command(int pin1, int pin2, int mode1, int mode2, bool state1, bool state2); + }; +} +#endif +``` + +io_example.cpp + +```cpp +#include + +using std::placeholders::_1; +using std::placeholders::_2; + +namespace io_example +{ + Gripper::IOExample() : Node("io_example_node") + { + // get parameters + this->declare_parameter("robot_ip"); + this->declare_parameter("eki_io_port"); + this->declare_parameter("n_io"); + + std::string robot_ip; + int robot_ip; + this->get_parameter("robot_ip", robot_ip); + int eki_io_port; + std::string eki_io_port_str; + this->get_parameter("eki_io_port", eki_io_port); + eki_io_port_str = std::to_string(eki_io_port); + int n_io; + this->get_parameter("n_io",n_io); + + // initialize kuka_eki_io_interface + _kuka_eki_io_interface.reset(new kuka_eki_io_interface::KukaEkiIOInterface(robot_ip.c_str(), eki_io_port_str.c_str(), n_io)); + + // initialize example service + _example_srv = this->create_service("example_srv", std::bind(&IOExample::example_io, this, _1, _2)); + RCLCPP_INFO(rclcpp::get_logger("io_example_node"), "Ready to receive commands."); + + } + + void Gripper::eki_command(int pin1, int pin2, int mode1, int mode2, bool state1, bool state2) + { + rclcpp::Rate loop_rate(50); + // set up flags for checking if command was received and executed + bool f_pin_set = false; + bool s_pin_set = false; + bool f_command_received = false; + bool s_command_received = false; + bool f_mode_set = false; + bool s_mode_set = false; + // set up vectors for reading state + std::vector io_states; + std::vector io_pins; + std::vector io_types; + int buff_len; + // set up vectors for writing command + const std::vector set_io_pins_cmd{pin1, pin2}; + const std::vector set_io_modes_cmd{mode1, mode2}; + const std::vector set_target_ios_cmd{ state1, state2}; + + // wait until command was received and executed + bool command_received = false; + while (!command_received) + { + // write command + _kuka_eki_io_interface->eki_write_command(set_io_pins_cmd, set_io_modes_cmd, set_target_ios_cmd); + loop_rate.sleep(); + // read state + _kuka_eki_io_interface->eki_read_state(io_states, io_pins, io_types, buff_len); + // check if command was received and executed + f_pin_set = io_pins[0] == set_io_pins_cmd[0]; + s_pin_set = io_pins[1] == set_io_pins_cmd[1]; + f_command_received = io_states[0] == set_target_ios_cmd[0]; + s_command_received = io_states[1] == set_target_ios_cmd[1]; + f_mode_set = io_types[0] == set_io_modes_cmd[0]; + s_mode_set = io_types[1] == set_io_modes_cmd[1]; + command_received = f_pin_set && s_pin_set && f_command_received && s_command_received && f_mode_set && s_mode_set; + } + } + + void Gripper::example_io(const std::shared_ptr request, + std::shared_ptr response) + { + // set out pin 502 to false, pin 504 to true + eki_command(502, 504, 2, 2, false, true); + // read in pin 501 and 503, the state do not matter when reading + eki_command(501, 503, 1, 1, true, true); + // set out pin 501 to false, pin 503 to true + eki_command(501, 503, 2, 2, true, false); + response->success = true; + } +} +``` \ No newline at end of file diff --git a/kuka_eki/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h b/kuka_eki/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h new file mode 100644 index 000000000..52db78e64 --- /dev/null +++ b/kuka_eki/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h @@ -0,0 +1,36 @@ +#ifndef KUKA_EKI_IO_INTERFACE +#define KUKA_EKI_IO_INTERFACE + +#include +#include + +#include + +namespace kuka_eki_io_interface +{ + class KukaEkiIOInterface + { + private: + std::string eki_server_address_; + std::string eki_server_port_; + int eki_cmd_buff_len_; + int eki_max_cmd_buff_len_ = 5; + int n_io_; + + int eki_read_state_timeout_ = 5; // [s]; settable by parameter (default = 5) + static void eki_handle_receive(const boost::system::error_code &ec, size_t length, + boost::system::error_code* out_ec, size_t* out_length); + boost::asio::io_service ios_; + boost::asio::deadline_timer deadline_; + boost::asio::ip::udp::endpoint eki_server_endpoint_; + boost::asio::ip::udp::socket eki_server_socket_; + void eki_check_read_state_deadline(); + + public: + KukaEkiIOInterface(const std::string& eki_server_address, const std::string& eki_server_port, int n_io); + ~KukaEkiIOInterface(){}; + bool eki_read_state(std::vector &io_states, std::vector &io_pins, std::vector &io_types, int &cmd_buff_len); + bool eki_write_command(const std::vector &io_pins, const std::vector &io_modes, const std::vector &target_ios); + }; +} +#endif \ No newline at end of file diff --git a/kuka_eki/kuka_eki_io_interface/launch/eki_io_interface.launch.py b/kuka_eki/kuka_eki_io_interface/launch/eki_io_interface.launch.py new file mode 100644 index 000000000..1abc4bc68 --- /dev/null +++ b/kuka_eki/kuka_eki_io_interface/launch/eki_io_interface.launch.py @@ -0,0 +1,45 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append(DeclareLaunchArgument( + "robot_ip", + default_value="10.181.116.41", + description="IP address by which the robot can be reached." + )) + + declared_arguments.append(DeclareLaunchArgument( + "eki_port", + default_value="54601", + description="Port by which the robot can be reached." + )) + + declared_arguments.append(DeclareLaunchArgument( + "n_io", + default_value="2", + description="Port by which the robot can be reached." + )) + + robot_ip = LaunchConfiguration("robot_ip") + eki_port = LaunchConfiguration("eki_port") + n_io = LaunchConfiguration("n_io") + + eki_io_node = Node( + package="kuka_eki_io_interface", + executable="eki_io_node", + parameters=[ + {"robot_ip": robot_ip, + "eki_port": eki_port, + "n_io": n_io} + ], + emulate_tty=True, + output='screen' + ) + return LaunchDescription(declared_arguments + [ + eki_io_node + ]) diff --git a/kuka_eki/kuka_eki_io_interface/package.xml b/kuka_eki/kuka_eki_io_interface/package.xml new file mode 100644 index 000000000..c99c5b5cc --- /dev/null +++ b/kuka_eki/kuka_eki_io_interface/package.xml @@ -0,0 +1,23 @@ + + + + kuka_eki_io_interface + 0.0.0 + TODO: Package description + robot + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_lifecycle + rclcpp_components + std_msgs + + + ament_cmake + + diff --git a/kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp b/kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp new file mode 100644 index 000000000..071ea2bc6 --- /dev/null +++ b/kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp @@ -0,0 +1,156 @@ +#include +#include +#include +#include + +#include + +namespace kuka_eki_io_interface +{ + KukaEkiIOInterface::KukaEkiIOInterface(const std::string& eki_server_address, const std::string& eki_server_port, int n_io) : deadline_(ios_), + eki_server_socket_(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)) + { + eki_server_address_ = eki_server_address; + eki_server_port_ = eki_server_port; + n_io_ = n_io; + + std::cout << eki_server_address_ << std::endl; + std::cout << eki_server_port_ << std::endl; + std::cout << n_io_ << std::endl; + + boost::asio::ip::udp::resolver resolver(ios_); + eki_server_endpoint_ = *resolver.resolve({boost::asio::ip::udp::v4(), eki_server_address_, eki_server_port_}); + + boost::array ini_buf = { 0 }; + eki_server_socket_.send_to(boost::asio::buffer(ini_buf), eki_server_endpoint_); + + deadline_.expires_at(boost::posix_time::pos_infin); // do nothing unit a read is invoked (deadline_ = +inf) + eki_check_read_state_deadline(); + +// std::vector io_states; +// std::vector io_pins; +// std::vector io_types; +// int buff_len; +// if (!eki_read_state(io_states, io_pins, io_types, buff_len)) +// { +// std::string msg = "Failed to read from robot EKI server within alloted time of " +// + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running " +// "on the robot controller and all configurations are correct."; +// std::cout << msg << std::endl; +// throw std::runtime_error(msg); +// } + } +// KukaEkiIOInterface::~KukaEkiIOInterface() {} + + void KukaEkiIOInterface::eki_check_read_state_deadline() + { + // Check if deadline has already passed + if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) + { + eki_server_socket_.cancel(); + deadline_.expires_at(boost::posix_time::pos_infin); + } + // Sleep until deadline exceeded + deadline_.async_wait(boost::bind(&KukaEkiIOInterface::eki_check_read_state_deadline, this)); + } + + bool KukaEkiIOInterface::eki_read_state(std::vector &io_states, std::vector &io_pins, std::vector &io_types, int &cmd_buff_len) + { + io_states.resize(n_io_); + io_pins.resize(n_io_); + io_types.resize(n_io_); + static boost::array in_buffer; + deadline_.expires_from_now(boost::posix_time::seconds(eki_read_state_timeout_)); // set deadline + boost::system::error_code ec = boost::asio::error::would_block; + size_t len = 0; + eki_server_socket_.async_receive(boost::asio::buffer(in_buffer), + boost::bind(&KukaEkiIOInterface::eki_handle_receive, _1, _2, &ec, &len)); + + do { + ios_.run_one(); + } + while (ec == boost::asio::error::would_block); + if (ec) + { + std::cout << ec << std::endl; + return false; + } + if (len == 0) + { + std::cout << "2" << std::endl; + return false; + } + + TiXmlDocument xml_in; + in_buffer[len] = '\0'; // null-terminate data buffer for parsing (expects c-string) + xml_in.Parse(in_buffer.data()); + TiXmlElement* robot_state = xml_in.FirstChildElement("IOState"); + if (!robot_state) + { + std::cout << "3" << std::endl; + return false; + } + xml_in.Print(); + char io_name[] = "IO1"; + for (int i = 0; i < n_io_; ++i) + { + TiXmlElement* state = robot_state->FirstChildElement(io_name); + if (!state) + { + std::cout << "4" << std::endl; + return false; + } + int io_state; // [Nm] + state->Attribute("State", &io_state); + int io_pin; + state->Attribute("Pin", &io_pin); + int io_type; + state->Attribute("Type", &io_type); + io_states[i] = io_state; + io_pins[i] = io_pin; + io_types[i] = io_type; + io_name[2]++; + } + TiXmlElement* robot_command = robot_state->FirstChildElement("IOCommand"); + robot_command->Attribute("Size", &cmd_buff_len); + return true; + } + + bool KukaEkiIOInterface::eki_write_command(const std::vector &io_pins, const std::vector &io_modes, const std::vector &target_ios) + { + // TODO: assert vectors' lengths + // TODO: extend vector if length < n_io_ + TiXmlDocument xml_out; + TiXmlElement* io_command = new TiXmlElement("IOCommand"); + char io_name[] = "IO1"; + for (int i = 0; i < n_io_; i++) + { + TiXmlElement* io_element = new TiXmlElement(io_name); + TiXmlText* empty_text = new TiXmlText(""); + io_command->LinkEndChild(io_element); + io_element->LinkEndChild(empty_text); + + io_element->SetAttribute("Pin", io_pins[i]); + io_element->SetAttribute("Mode", io_modes[i]); + io_element->SetAttribute("Value", (int)target_ios[i]); + + io_name[2]++; + } + xml_out.LinkEndChild(io_command); + xml_out.Print(); + TiXmlPrinter xml_printer; + xml_printer.SetStreamPrinting(); // no linebreaks + xml_out.Accept(&xml_printer); + + eki_server_socket_.send_to(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()), + eki_server_endpoint_); + return true; + } + + void KukaEkiIOInterface::eki_handle_receive(const boost::system::error_code &ec, size_t length, + boost::system::error_code* out_ec, size_t* out_length) + { + *out_ec = ec; + *out_length = length; + } +} \ No newline at end of file diff --git a/kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp b/kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp new file mode 100644 index 000000000..a22f04c08 --- /dev/null +++ b/kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp @@ -0,0 +1,60 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "kuka_eki_io_interface/kuka_eki_io_interface.h" + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + std::shared_ptr node = rclcpp::Node::make_shared("eki_io_node"); + + rclcpp::Rate loop_rate(2); + node->declare_parameter("robot_ip"); + node->declare_parameter("eki_port"); + node->declare_parameter("n_io"); + + std::string robot_ip; + node->get_parameter("robot_ip", robot_ip); + int eki_port; + std::string eki_port_; + node->get_parameter("eki_port", eki_port); + eki_port_ = std::to_string(eki_port); + int n_io; + node->get_parameter("n_io",n_io); + kuka_eki_io_interface::KukaEkiIOInterface io_interface(robot_ip.c_str(), eki_port_.c_str(), n_io); + + const std::vector io_pins_cmd{7, 8}; + const std::vector io_modes_cmd{2, 2}; + const std::vector target_ios_cmd{ false, true}; +// io_interface.eki_write_command(io_pins_cmd, io_modes_cmd, target_ios_cmd); + + loop_rate.sleep(); + while (rclcpp::ok()) + { + rclcpp::spin_some(node); + io_interface.eki_write_command(io_pins_cmd, io_modes_cmd, target_ios_cmd); + + rclcpp::spin_some(node); + std::vector io_states; + std::vector io_pins; + std::vector io_types; + int buff_len; + io_interface.eki_read_state(io_states, io_pins, io_types, buff_len); +// for(int i = 0; i < n_io; i++) +// { +// RCLCPP_INFO(node->get_logger(), "IO state: %d, %d, %d", io_pins[i], io_types[1], io_states[i]); +// } + loop_rate.sleep(); + } + for(int i = 0; i<10; i++) { + io_interface.eki_write_command(io_pins_cmd, io_modes_cmd, target_ios_cmd); + } + + RCLCPP_INFO(node->get_logger(), "Shutting down."); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file