From 38ffd1a2681fc0485ab7e9adc92b8a4c2d2a4077 Mon Sep 17 00:00:00 2001 From: soge0001 Date: Tue, 28 Feb 2023 17:42:05 +0100 Subject: [PATCH 1/4] 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 From 79c848ad92830e77d2c45f1ad7268fa6cf88b608 Mon Sep 17 00:00:00 2001 From: giridhar Date: Thu, 9 Mar 2023 01:34:29 +0100 Subject: [PATCH 2/4] added eki io --- kuka_eki_hw_interface/CMakeLists.txt | 115 ++-- .../kuka_eki_hw_interface.h | 183 +++--- .../visibility_control.h | 56 ++ kuka_eki_hw_interface/krl/EkiHwInterface.xml | 142 ++--- kuka_eki_hw_interface/krl/EkiIOInterface.xml | 41 ++ .../krl/kuka_eki_hw_interface.dat | 9 +- .../krl/kuka_eki_hw_interface.src | 539 ++++++++++------ .../launch/controller_spawner.launch.py | 105 ++++ kuka_eki_hw_interface/package.xml | 33 +- .../ros2_control_kuka_eki.xml | 9 + .../src/kuka_eki_hw_interface.cpp | 593 +++++++++--------- kuka_eki_io_interface/CMakeLists.txt | 98 +++ kuka_eki_io_interface/README.md | 131 ++++ .../kuka_eki_io_interface.h | 36 ++ .../launch/eki_io_interface.launch.py | 45 ++ 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, 1646 insertions(+), 728 deletions(-) create mode 100644 kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h create mode 100644 kuka_eki_hw_interface/krl/EkiIOInterface.xml create mode 100644 kuka_eki_hw_interface/launch/controller_spawner.launch.py create mode 100644 kuka_eki_hw_interface/ros2_control_kuka_eki.xml create mode 100644 kuka_eki_io_interface/CMakeLists.txt create mode 100644 kuka_eki_io_interface/README.md create mode 100644 kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h create mode 100644 kuka_eki_io_interface/launch/eki_io_interface.launch.py create mode 100644 kuka_eki_io_interface/package.xml create mode 100644 kuka_eki_io_interface/src/kuka_eki_io_interface.cpp create mode 100644 kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp diff --git a/kuka_eki_hw_interface/CMakeLists.txt b/kuka_eki_hw_interface/CMakeLists.txt index 3435416d7..92372f906 100644 --- a/kuka_eki_hw_interface/CMakeLists.txt +++ b/kuka_eki_hw_interface/CMakeLists.txt @@ -1,64 +1,89 @@ -cmake_minimum_required(VERSION 3.1.0) +cmake_minimum_required(VERSION 3.5) project(kuka_eki_hw_interface) -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() -find_package(catkin REQUIRED COMPONENTS - angles - cmake_modules - controller_manager - hardware_interface - joint_limits_interface - roscpp -) +# 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) -catkin_package( - CATKIN_DEPENDS - controller_manager - hardware_interface - joint_limits_interface - roscpp - DEPENDS - TinyXML -) - +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() -include_directories( +add_library( + ${PROJECT_NAME} + SHARED + src/kuka_eki_hw_interface.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${TinyXML_INCLUDE_DIRS} ) - -add_library(kuka_eki_hw_interface - src/kuka_eki_hw_interface.cpp +ament_target_dependencies( + ${PROJECT_NAME} + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + TinyXML ) +pluginlib_export_plugin_description_file(hardware_interface ros2_control_kuka_eki.xml) -target_link_libraries(kuka_eki_hw_interface - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${TinyXML_LIBRARIES} +ament_export_include_directories( + include ) - -add_executable(kuka_eki_hw_interface_node - src/kuka_eki_hw_interface_node.cpp +ament_export_libraries( + ${PROJECT_NAME} ) - -target_link_libraries(kuka_eki_hw_interface_node - kuka_eki_hw_interface +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle ) +### ros2 control hardware plugin install( - TARGETS kuka_eki_hw_interface - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) - + TARGETS ${PROJECT_NAME} + DESTINATION lib +) +install( + DIRECTORY include/ + DESTINATION include +) install( - TARGETS kuka_eki_hw_interface_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) +ament_package() diff --git a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h index 707ddc807..0e8d7ee24 100644 --- a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h +++ b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h @@ -1,106 +1,91 @@ -/* - * 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 disclaimer. - * * 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) - - -#ifndef KUKA_EKI_HW_INTERFACE -#define KUKA_EKI_HW_INTERFACE - -#include +// 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 -#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::RobotHW -{ -private: - ros::NodeHandle nh_; - - const unsigned int n_dof_ = 6; - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - - // EKI - 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) - - // Timing - ros::Duration control_period_; - ros::Duration elapsed_time_; - double loop_hz_; - - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - - // EKI socket read/write - int eki_read_state_timeout_ = 5; // [s]; settable by parameter (default = 5) - 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(); - 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); - -public: - - KukaEkiHardwareInterface(); - ~KukaEkiHardwareInterface(); - - void init(); - void start(); - void read(const ros::Time &time, const ros::Duration &period); - void write(const ros::Time &time, const ros::Duration &period); -}; - -} // namespace kuka_eki_hw_interface - -#endif // 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_hw_interface/include/kuka_eki_hw_interface/visibility_control.h b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h new file mode 100644 index 000000000..90cacbbcb --- /dev/null +++ b/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_hw_interface/krl/EkiHwInterface.xml b/kuka_eki_hw_interface/krl/EkiHwInterface.xml index 4aea0aadc..05e6f37e7 100644 --- a/kuka_eki_hw_interface/krl/EkiHwInterface.xml +++ b/kuka_eki_hw_interface/krl/EkiHwInterface.xml @@ -1,71 +1,71 @@ - - - - Client - - - Program - - - address.of.robot.controller - 54600 - UDP - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + Client + + + Program + + + 10.181.116.41 + 54600 + UDP + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kuka_eki_hw_interface/krl/EkiIOInterface.xml b/kuka_eki_hw_interface/krl/EkiIOInterface.xml new file mode 100644 index 000000000..5d0f30053 --- /dev/null +++ b/kuka_eki_hw_interface/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_hw_interface/krl/kuka_eki_hw_interface.dat b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat index f47f888f3..479da3ee5 100644 --- a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat +++ b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat @@ -1,4 +1,7 @@ -&ACCESS RVO -defdat kuka_eki_hw_interface - ext bas(bas_command :in,real :in ) +&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_hw_interface/krl/kuka_eki_hw_interface.src b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src index 5a4edf7b9..412c7a32d 100644 --- a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src +++ b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src @@ -1,192 +1,347 @@ -&ACCESS RVO -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 disclaimer. - ; * 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 - - ; 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() - - ; BCO (Block COincidence) run to current position - ; (requied for below loop continue before first incoming command) - joint_pos_tgt = $axis_act_meas - ptp joint_pos_tgt - - ; Loop forever - $advance = 5 - loop - elements_read = eki_hw_iface_get(joint_pos_tgt) ; Get new command from buffer if present - ptp joint_pos_tgt c_ptp ; PTP to most recent commanded position - 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_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 - - if not $flag[1] then - return 0 - endif - - eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") - 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 - - - -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 +&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_hw_interface/launch/controller_spawner.launch.py b/kuka_eki_hw_interface/launch/controller_spawner.launch.py new file mode 100644 index 000000000..bf84ab792 --- /dev/null +++ b/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_hw_interface/package.xml b/kuka_eki_hw_interface/package.xml index 915074672..13d8cadf8 100644 --- a/kuka_eki_hw_interface/package.xml +++ b/kuka_eki_hw_interface/package.xml @@ -1,24 +1,23 @@ - + + kuka_eki_hw_interface - 0.0.1 - A ROS-Control hardware interface for use with KUKA EKI - Brett Hemes (3M) - Brett Hemes (3M) - G.A. vd. Hoorn (TU Delft Robotics Institute) - BSD + 0.0.0 + TODO: Package description + robot + TODO: License declaration - http://wiki.ros.org/kuka_eki_hw_interface - https://github.com/ros-industrial/kuka_experimental/issues - https://github.com/ros-industrial/kuka_experimental + ament_cmake - catkin + ament_lint_auto + ament_lint_common - angles - cmake_modules - - controller_manager hardware_interface - joint_limits_interface - roscpp + pluginlib + rclcpp + rclcpp_lifecycle + + + ament_cmake + diff --git a/kuka_eki_hw_interface/ros2_control_kuka_eki.xml b/kuka_eki_hw_interface/ros2_control_kuka_eki.xml new file mode 100644 index 000000000..0e50359c0 --- /dev/null +++ b/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_hw_interface/src/kuka_eki_hw_interface.cpp b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp index 9a59a03f1..0e0c0e43e 100644 --- a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp +++ b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp @@ -1,305 +1,296 @@ -/* - * 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 disclaimer. - * * 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) - - -#include -#include -#include - -#include - -#include - -#include - +// 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 { - -KukaEkiHardwareInterface::KukaEkiHardwareInterface() : joint_position_(n_dof_, 0.0), joint_velocity_(n_dof_, 0.0), - joint_effort_(n_dof_, 0.0), joint_position_command_(n_dof_, 0.0), joint_names_(n_dof_), deadline_(ios_), - eki_server_socket_(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)) -{ - -} - - -KukaEkiHardwareInterface::~KukaEkiHardwareInterface() {} - - -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, + 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 (int i = 0; i < n_dof_; ++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 (int i = 0; i < n_dof_; ++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); - - size_t len = eki_server_socket_.send_to(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()), - eki_server_endpoint_); - - return true; -} - - -void KukaEkiHardwareInterface::init() -{ - // Get controller joint names from parameter server - if (!nh_.getParam("controller_joint_names", joint_names_)) - { - ROS_ERROR("Cannot find required parameter 'controller_joint_names' on the parameter server."); - throw std::runtime_error("Cannot find required parameter 'controller_joint_names' on the parameter server."); - } - - // Get EKI parameters from parameter server - const std::string param_addr = "eki/robot_address"; - const std::string param_port = "eki/robot_port"; - const std::string param_socket_timeout = "eki/socket_timeout"; - const std::string param_max_cmd_buf_len = "eki/max_cmd_buf_len"; - - if (nh_.getParam(param_addr, eki_server_address_) && - nh_.getParam(param_port, eki_server_port_)) - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface on: " - << eki_server_address_ << ", " << eki_server_port_); - } - else - { - std::string msg = "Failed to get EKI address/port from parameter server (looking for '" + param_addr + - "', '" + param_port + "')"; - ROS_ERROR_STREAM(msg); - throw std::runtime_error(msg); - } - - if (nh_.getParam(param_socket_timeout, eki_read_state_timeout_)) - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface socket timeout to " - << eki_read_state_timeout_ << " seconds"); - } - else - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Failed to get EKI socket timeout from parameter server (looking " - "for '" + param_socket_timeout + "'), defaulting to " + - std::to_string(eki_read_state_timeout_) + " seconds"); - } - - if (nh_.getParam(param_max_cmd_buf_len, eki_max_cmd_buff_len_)) - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface maximum command buffer " - "length to " << eki_max_cmd_buff_len_); - } - else - { - ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Failed to get EKI hardware interface maximum command buffer length " - "from parameter server (looking for '" + param_max_cmd_buf_len + "'), defaulting to " + - std::to_string(eki_max_cmd_buff_len_)); - } - - // Create ros_control interfaces (joint state and position joint for all dof's) - for (std::size_t i = 0; i < n_dof_; ++i) - { - // Joint state interface - joint_state_interface_.registerHandle( - hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], &joint_velocity_[i], - &joint_effort_[i])); - - // Joint position control interface - position_joint_interface_.registerHandle( - hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[i]), - &joint_position_command_[i])); - } - - // Register interfaces - registerInterface(&joint_state_interface_); - registerInterface(&position_joint_interface_); - - ROS_INFO_STREAM_NAMED("kukw_eki_hw_interface", "Loaded Kuka EKI hardware interface"); -} - - -void KukaEkiHardwareInterface::start() -{ - ROS_INFO_NAMED("kuka_eki_hw_interface", "Starting Kuka EKI hardware interface..."); - - // Start client - ROS_INFO_NAMED("kuka_eki_hw_interface", "... connecting to robot's EKI server..."); - 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(); - - // Initialize joint_position_command_ from initial robot state (avoid bad (null) commands before controllers come up) - 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."; - ROS_ERROR_STREAM(msg); - throw std::runtime_error(msg); - } - joint_position_command_ = joint_position_; - - ROS_INFO_NAMED("kuka_eki_hw_interface", "... done. EKI hardware interface started!"); -} - - -void KukaEkiHardwareInterface::read(const ros::Time &time, const ros::Duration &period) -{ - 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."; - ROS_ERROR_STREAM(msg); - throw std::runtime_error(msg); - } -} - - -void KukaEkiHardwareInterface::write(const ros::Time &time, const ros::Duration &period) -{ - // only write if max will not be exceeded - if (eki_cmd_buff_len_ < eki_max_cmd_buff_len_) - eki_write_command(joint_position_command_); - - // underflow/overflow checking - // NOTE: this is commented as it results in a lot of logging output and the use of ROS_* - // logging macros breaks incurs (quite) some overhead. Uncomment and rebuild this - // if you'd like to use this anyway. - //if (eki_cmd_buff_len_ >= eki_max_cmd_buff_len_) - // ROS_WARN_STREAM("eki_hw_iface RobotCommand buffer overflow (curent size " << eki_cmd_buff_len_ - // << " greater than or equal max allowed " << eki_max_cmd_buff_len_ << ")"); - //else if (eki_cmd_buff_len_ == 0) - // ROS_WARN_STREAM("eki_hw_iface RobotCommand buffer empty"); -} - -} // namespace kuka_eki_hw_interface + { + *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_io_interface/CMakeLists.txt b/kuka_eki_io_interface/CMakeLists.txt new file mode 100644 index 000000000..db2bdb241 --- /dev/null +++ b/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_io_interface/README.md b/kuka_eki_io_interface/README.md new file mode 100644 index 000000000..0daed5151 --- /dev/null +++ b/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_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h b/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_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_io_interface/launch/eki_io_interface.launch.py b/kuka_eki_io_interface/launch/eki_io_interface.launch.py new file mode 100644 index 000000000..1abc4bc68 --- /dev/null +++ b/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_io_interface/package.xml b/kuka_eki_io_interface/package.xml new file mode 100644 index 000000000..c99c5b5cc --- /dev/null +++ b/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_io_interface/src/kuka_eki_io_interface.cpp b/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp new file mode 100644 index 000000000..071ea2bc6 --- /dev/null +++ b/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_io_interface/src/kuka_eki_io_interface_node.cpp b/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp new file mode 100644 index 000000000..a22f04c08 --- /dev/null +++ b/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 From 8b46dae5f2e8ba16b065098408a2f7855e69b5d6 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 9 Mar 2023 03:17:33 +0100 Subject: [PATCH 3/4] added read() write() arguments --- .../include/kuka_eki_hw_interface/kuka_eki_hw_interface.h | 4 ++-- kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h index 0e8d7ee24..10a3c3145 100644 --- a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h +++ b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h @@ -57,10 +57,10 @@ namespace kuka_eki_hw_interface hardware_interface::return_type stop() override; ROS2_CONTROL_KUKA_EKI_HW_PUBLIC - hardware_interface::return_type read() override; + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; ROS2_CONTROL_KUKA_EKI_HW_PUBLIC - hardware_interface::return_type write() override; + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; private: // Store the command for the simulated robot diff --git a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp index 0e0c0e43e..abe858810 100644 --- a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp +++ b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp @@ -259,7 +259,7 @@ namespace kuka_eki_hw_interface return hardware_interface::return_type::OK; } - hardware_interface::return_type KukaEkiHardwareInterface::read() + hardware_interface::return_type KukaEkiHardwareInterface::read(const rclcpp::Time & time, const rclcpp::Duration & period) { std::vector joint_position; std::vector joint_velocity; @@ -281,7 +281,7 @@ namespace kuka_eki_hw_interface return hardware_interface::return_type::OK; } - hardware_interface::return_type KukaEkiHardwareInterface::write() + hardware_interface::return_type KukaEkiHardwareInterface::write(const rclcpp::Time & time, const rclcpp::Duration & period) { if (eki_cmd_buff_len_ < eki_max_cmd_buff_len_) eki_write_command(hw_commands_); From e5cdff9c219c60eab057d29b990daa820227a705 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 9 Mar 2023 23:29:50 +0100 Subject: [PATCH 4/4] reorganized the files and removed duplicates --- 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 deletions(-) delete mode 100644 kuka_eki/krl/EkiHwInterface.xml delete mode 100644 kuka_eki/krl/EkiIOInterface.xml delete mode 100644 kuka_eki/krl/kuka_eki_hw_interface.dat delete mode 100644 kuka_eki/krl/kuka_eki_hw_interface.src delete mode 100644 kuka_eki/kuka_eki_hw_interface/CMakeLists.txt delete mode 100644 kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h delete mode 100644 kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h delete mode 100644 kuka_eki/kuka_eki_hw_interface/launch/controller_spawner.launch.py delete mode 100644 kuka_eki/kuka_eki_hw_interface/package.xml delete mode 100644 kuka_eki/kuka_eki_hw_interface/ros2_control_kuka_eki.xml delete mode 100644 kuka_eki/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp delete mode 100644 kuka_eki/kuka_eki_io_interface/CMakeLists.txt delete mode 100644 kuka_eki/kuka_eki_io_interface/README.md delete mode 100644 kuka_eki/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h delete mode 100644 kuka_eki/kuka_eki_io_interface/launch/eki_io_interface.launch.py delete mode 100644 kuka_eki/kuka_eki_io_interface/package.xml delete mode 100644 kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp delete 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 deleted file mode 100644 index 05e6f37e7..000000000 --- a/kuka_eki/krl/EkiHwInterface.xml +++ /dev/null @@ -1,71 +0,0 @@ - - - - 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 deleted file mode 100644 index 5d0f30053..000000000 --- a/kuka_eki/krl/EkiIOInterface.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - 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 deleted file mode 100644 index 479da3ee5..000000000 --- a/kuka_eki/krl/kuka_eki_hw_interface.dat +++ /dev/null @@ -1,7 +0,0 @@ -&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 deleted file mode 100644 index 412c7a32d..000000000 --- a/kuka_eki/krl/kuka_eki_hw_interface.src +++ /dev/null @@ -1,347 +0,0 @@ -&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 deleted file mode 100644 index 92372f906..000000000 --- a/kuka_eki/kuka_eki_hw_interface/CMakeLists.txt +++ /dev/null @@ -1,89 +0,0 @@ -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 deleted file mode 100644 index 0e8d7ee24..000000000 --- a/kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h +++ /dev/null @@ -1,91 +0,0 @@ -// 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 deleted file mode 100644 index 90cacbbcb..000000000 --- a/kuka_eki/kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// 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 deleted file mode 100644 index bf84ab792..000000000 --- a/kuka_eki/kuka_eki_hw_interface/launch/controller_spawner.launch.py +++ /dev/null @@ -1,105 +0,0 @@ -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 deleted file mode 100644 index 13d8cadf8..000000000 --- a/kuka_eki/kuka_eki_hw_interface/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - 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 deleted file mode 100644 index 0e50359c0..000000000 --- a/kuka_eki/kuka_eki_hw_interface/ros2_control_kuka_eki.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - 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 deleted file mode 100644 index 0e0c0e43e..000000000 --- a/kuka_eki/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp +++ /dev/null @@ -1,296 +0,0 @@ -// 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 deleted file mode 100644 index db2bdb241..000000000 --- a/kuka_eki/kuka_eki_io_interface/CMakeLists.txt +++ /dev/null @@ -1,98 +0,0 @@ -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 deleted file mode 100644 index 0daed5151..000000000 --- a/kuka_eki/kuka_eki_io_interface/README.md +++ /dev/null @@ -1,131 +0,0 @@ -# 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 deleted file mode 100644 index 52db78e64..000000000 --- a/kuka_eki/kuka_eki_io_interface/include/kuka_eki_io_interface/kuka_eki_io_interface.h +++ /dev/null @@ -1,36 +0,0 @@ -#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 deleted file mode 100644 index 1abc4bc68..000000000 --- a/kuka_eki/kuka_eki_io_interface/launch/eki_io_interface.launch.py +++ /dev/null @@ -1,45 +0,0 @@ -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 deleted file mode 100644 index c99c5b5cc..000000000 --- a/kuka_eki/kuka_eki_io_interface/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - 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 deleted file mode 100644 index 071ea2bc6..000000000 --- a/kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface.cpp +++ /dev/null @@ -1,156 +0,0 @@ -#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 deleted file mode 100644 index a22f04c08..000000000 --- a/kuka_eki/kuka_eki_io_interface/src/kuka_eki_io_interface_node.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#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