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..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 @@ -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(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_KUKA_EKI_HW_PUBLIC + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) 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..abe858810 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(const rclcpp::Time & time, const rclcpp::Duration & period) + { + 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(const rclcpp::Time & time, const rclcpp::Duration & period) + { + 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