From 3178fd0bfdd9907a0f89c8c623095fdcb2c93bcd Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Fri, 9 Jun 2023 16:24:32 +0200 Subject: [PATCH] fix pre-commit --- .clang-format | 2 +- .github/workflows/ci-ros-lint.yml | 10 +- .pre-commit-config.yaml | 8 +- .../kuka_eki_hw_interface.h | 106 -------- .../kuka_eki_hw_interface.hpp | 105 ++++++++ kuka_eki_hw_interface/krl/README.md | 3 +- .../krl/kuka_eki_hw_interface.dat | 2 +- .../krl/kuka_eki_hw_interface.src | 4 +- .../src/kuka_eki_hw_interface.cpp | 246 +++++++++--------- .../src/kuka_eki_hw_interface_node.cpp | 73 +++--- kuka_kr10_support/urdf/kr10r900_2_macro.xacro | 2 +- kuka_kr5_support/urdf/kr5_arc_macro.xacro | 2 +- kuka_kr6_support/urdf/kr6r700sixx_macro.xacro | 2 +- kuka_resources/urdf/common_properties.xacro | 2 +- .../kuka_hardware_interface.h | 134 ---------- .../kuka_hardware_interface.hpp | 126 +++++++++ .../kuka_rsi_hw_interface/rsi_command.h | 89 ------- .../kuka_rsi_hw_interface/rsi_command.hpp | 86 ++++++ .../include/kuka_rsi_hw_interface/rsi_state.h | 129 --------- .../kuka_rsi_hw_interface/rsi_state.hpp | 125 +++++++++ .../kuka_rsi_hw_interface/udp_server.h | 178 ------------- .../kuka_rsi_hw_interface/udp_server.hpp | 171 ++++++++++++ kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src | 8 +- .../krl/KR_C2/ros_rsi_ethernet.xml | 2 +- kuka_rsi_hw_interface/krl/KR_C4/README.md | 1 - kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi | 4 +- .../krl/KR_C4/ros_rsi.rsi.diagram | 4 +- .../krl/KR_C4/ros_rsi.rsi.xml | 4 +- kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src | 12 +- .../krl/KR_C4/ros_rsi_ethernet.xml | 10 +- .../src/kuka_hardware_interface.cpp | 125 ++++----- .../src/kuka_hardware_interface_node.cpp | 78 +++--- .../test/ag1_hw_controllers.yaml | 2 - kuka_rsi_hw_interface/test/ag2_params.yaml | 1 - kuka_rsi_simulator/scripts/kuka_rsi_simulator | 86 ++++-- kuka_rsi_simulator/setup.py | 57 +++- 36 files changed, 1027 insertions(+), 972 deletions(-) delete mode 100644 kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h create mode 100644 kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.hpp delete mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp delete mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.hpp delete mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.hpp delete mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.hpp diff --git a/.clang-format b/.clang-format index ab8d07757..9e92cf780 100644 --- a/.clang-format +++ b/.clang-format @@ -10,5 +10,5 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false +ReflowComments: true IncludeBlocks: Preserve diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 0e98f5007..3a53a0f16 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.6.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -29,6 +29,9 @@ jobs: kuka_kr5_support kuka_kr3_support kuka_experimental + kuka_rsi_hw_interface + kuka_rsi_simulator + kuka_eki_hw_interface ament_lint_100: @@ -40,7 +43,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.6.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -58,3 +61,6 @@ jobs: kuka_kr5_support kuka_kr3_support kuka_experimental + kuka_rsi_hw_interface + kuka_rsi_simulator + kuka_eki_hw_interface diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ea4786e51..92407c3d4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -34,13 +34,13 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.3.1 + rev: v3.4.0 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 23.1.0 + rev: 23.3.0 hooks: - id: black args: ["--line-length=99"] @@ -56,7 +56,7 @@ repos: rev: 6.0.0 hooks: - id: flake8 - args: ["--ignore=E501"] + args: ["--ignore=E501,W503"] # CPP hooks - repo: local @@ -137,7 +137,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.2 + rev: v2.2.4 hooks: - id: codespell args: ['--write-changes', '--ignore-words-list=linz'] 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 deleted file mode 100644 index 707ddc807..000000000 --- a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h +++ /dev/null @@ -1,106 +0,0 @@ -/* - * 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 -#include - -#include - -#include -#include -#include -#include -#include - - -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 diff --git a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.hpp b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.hpp new file mode 100644 index 000000000..0bc4f2526 --- /dev/null +++ b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.hpp @@ -0,0 +1,105 @@ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2018, 3M +// +// 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 HOLDER 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__KUKA_EKI_HW_INTERFACE_HPP_ +#define KUKA_EKI_HW_INTERFACE__KUKA_EKI_HW_INTERFACE_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +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__KUKA_EKI_HW_INTERFACE_HPP_ diff --git a/kuka_eki_hw_interface/krl/README.md b/kuka_eki_hw_interface/krl/README.md index 27cc8be31..e55b3ea40 100644 --- a/kuka_eki_hw_interface/krl/README.md +++ b/kuka_eki_hw_interface/krl/README.md @@ -26,7 +26,7 @@ The files included in the `kuka_eki_hw_interface/krl` folder provide the KRL int Note that the `eki/robot_address` and `eki/robot_port` parameters of the `kuka_eki_hw_interface` must correspond to the `IP`and `PORT` set in this XML file. ##### Copy files to controller -The files `kuka_eki_hw_interface.dat` and `kuke_eki_hw_interface.src` should not be edited. All files are now ready to be copied to the Kuka controller. Using WorkVisual or a USB drive (with appropriate privleges): +The files `kuka_eki_hw_interface.dat` and `kuke_eki_hw_interface.src` should not be edited. All files are now ready to be copied to the Kuka controller. Using WorkVisual or a USB drive (with appropriate privileges): 1. Copy `kuka_eki_hw_interface.dat` and `kuka_eki_hw_interface.src` files to `KRC:\R1\Program`. 2. Copy `EkiHwInterface.xml` to `C:\KRC\ROBOTER\Config\User\Common\EthernetKRL\`. @@ -90,4 +90,3 @@ $ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller Choose **controller manager ns** and **controller** and you should be able to move each robot joint. * Note that T1-mode limits the robot movement velocity and is intended for testing purposes. - 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..30b15a326 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,4 @@ &ACCESS RVO defdat kuka_eki_hw_interface ext bas(bas_command :in,real :in ) -enddat \ No newline at end of file +enddat 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..8e3bb46f7 100644 --- a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src +++ b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src @@ -62,7 +62,7 @@ def kuka_eki_hw_interface() eki_hw_iface_init() ; BCO (Block COincidence) run to current position - ; (requied for below loop continue before first incoming command) + ; (required for below loop continue before first incoming command) joint_pos_tgt = $axis_act_meas ptp joint_pos_tgt @@ -157,7 +157,7 @@ endfct ; eki_hw_iface_get -; Tries to read most recent elemnt from buffer. q left unchanged if empty. +; Tries to read most recent element 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 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..5b1badf51 100644 --- a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp +++ b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp @@ -1,62 +1,61 @@ -/* - * 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. - */ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2018, 3M +// +// 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 HOLDER 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 -#include - +#include +#include +#include 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() +: 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 @@ -70,52 +69,49 @@ void KukaEkiHardwareInterface::eki_check_read_state_deadline() 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) +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) +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 + // 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)); + 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; + } while (ec == boost::asio::error::would_block); + if (ec) return false; // Update joint positions from XML packet (if received) - if (len == 0) - return false; + 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; + 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] @@ -139,19 +135,19 @@ bool KukaEkiHardwareInterface::eki_read_state(std::vector &joint_positio return true; } - -bool KukaEkiHardwareInterface::eki_write_command(const std::vector &joint_position_command) +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(""); + 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 ) + 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()); + pos->SetAttribute( + axis_name, std::to_string(angles::to_degrees(joint_position_command[i])).c_str()); axis_name[1]++; } xml_out.LinkEndChild(robot_command); @@ -160,20 +156,20 @@ bool KukaEkiHardwareInterface::eki_write_command(const std::vector &join 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_); + 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."); + throw std::runtime_error( + "Cannot find required parameter 'controller_joint_names' on the parameter server."); } // Get EKI parameters from parameter server @@ -182,56 +178,63 @@ void KukaEkiHardwareInterface::init() 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_)) + 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_); + 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 + "')"; + 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"); + 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"); + 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_); + 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_)); + 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_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])); + position_joint_interface_.registerHandle(hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); } // Register interfaces @@ -241,7 +244,6 @@ void KukaEkiHardwareInterface::init() 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..."); @@ -249,19 +251,24 @@ void KukaEkiHardwareInterface::start() // 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 + 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) + 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) + // 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 " + std::string msg = "Failed to read from robot EKI server within allotted 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); @@ -271,35 +278,34 @@ void KukaEkiHardwareInterface::start() ROS_INFO_NAMED("kuka_eki_hw_interface", "... done. EKI hardware interface started!"); } - -void KukaEkiHardwareInterface::read(const ros::Time &time, const ros::Duration &period) +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 " + std::string msg = "Failed to read from robot EKI server within allotted 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) +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_); + 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_ + // if (eki_cmd_buff_len_ >= eki_max_cmd_buff_len_) + // ROS_WARN_STREAM("eki_hw_iface RobotCommand buffer overflow (current size " << + // eki_cmd_buff_len_ // << " greater than or equal max allowed " << eki_max_cmd_buff_len_ << ")"); - //else if (eki_cmd_buff_len_ == 0) + // else if (eki_cmd_buff_len_ == 0) // ROS_WARN_STREAM("eki_hw_iface RobotCommand buffer empty"); } -} // namespace kuka_eki_hw_interface +} // namespace kuka_eki_hw_interface diff --git a/kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp b/kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp index b3f944dd9..65e28897a 100644 --- a/kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp +++ b/kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp @@ -1,41 +1,40 @@ -/* - * 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. - */ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2018, 3M +// +// 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 HOLDER 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 -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "kuka_eki_hw_interface"); @@ -60,7 +59,9 @@ int main(int argc, char** argv) // Get current time and elapsed time since last read timestamp = ros::Time::now(); stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); + period.fromSec( + std::chrono::duration_cast>(stopwatch_now - stopwatch_last) + .count()); stopwatch_last = stopwatch_now; while (ros::ok()) @@ -71,7 +72,9 @@ int main(int argc, char** argv) // Get current time and elapsed time since last read timestamp = ros::Time::now(); stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); + period.fromSec( + std::chrono::duration_cast>(stopwatch_now - stopwatch_last) + .count()); stopwatch_last = stopwatch_now; // Update the controllers diff --git a/kuka_kr10_support/urdf/kr10r900_2_macro.xacro b/kuka_kr10_support/urdf/kr10r900_2_macro.xacro index 88c5c96aa..20b9660f1 100644 --- a/kuka_kr10_support/urdf/kr10r900_2_macro.xacro +++ b/kuka_kr10_support/urdf/kr10r900_2_macro.xacro @@ -191,4 +191,4 @@ - \ No newline at end of file + diff --git a/kuka_kr5_support/urdf/kr5_arc_macro.xacro b/kuka_kr5_support/urdf/kr5_arc_macro.xacro index 02d09a513..8044b9ef1 100644 --- a/kuka_kr5_support/urdf/kr5_arc_macro.xacro +++ b/kuka_kr5_support/urdf/kr5_arc_macro.xacro @@ -284,4 +284,4 @@ - \ No newline at end of file + diff --git a/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro b/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro index d48d3ad7c..ddea62587 100644 --- a/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro +++ b/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro @@ -184,4 +184,4 @@ - \ No newline at end of file + diff --git a/kuka_resources/urdf/common_properties.xacro b/kuka_resources/urdf/common_properties.xacro index 2cb528ef1..31842e43d 100644 --- a/kuka_resources/urdf/common_properties.xacro +++ b/kuka_resources/urdf/common_properties.xacro @@ -8,5 +8,5 @@ - + diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h deleted file mode 100644 index 9e1f919b3..000000000 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h +++ /dev/null @@ -1,134 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * 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 Norwegian University of Science and - * Technology, 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: Lars Tingelstad - */ - -#ifndef KUKA_RSI_HARDWARE_INTERFACE_KUKA_HARDWARE_INTERFACE_ -#define KUKA_RSI_HARDWARE_INTERFACE_KUKA_HARDWARE_INTERFACE_ - -// STL -#include -#include - -// ROS -#include -#include - - -// ros_control -#include -#include -#include -#include -#include - -// Timers -#include - -// UDP server -#include - -// RSI -#include -#include - -namespace kuka_rsi_hw_interface -{ - -static const double RAD2DEG = 57.295779513082323; -static const double DEG2RAD = 0.017453292519943295; - -class KukaHardwareInterface : public hardware_interface::RobotHW -{ - -private: - - // ROS node handle - ros::NodeHandle nh_; - - unsigned int n_dof_; - - std::vector joint_names_; - - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - std::vector joint_effort_command_; - - // RSI - RSIState rsi_state_; - RSICommand rsi_command_; - std::vector rsi_initial_joint_positions_; - std::vector rsi_joint_position_corrections_; - unsigned long long ipoc_; - - std::unique_ptr > rt_rsi_pub_; - - std::unique_ptr server_; - std::string local_host_; - int local_port_; - std::string remote_host_; - std::string remote_port_; - std::string in_buffer_; - std::string out_buffer_; - - // 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_; - -public: - - KukaHardwareInterface(); - ~KukaHardwareInterface(); - - void start(); - void configure(); - bool read(const ros::Time time, const ros::Duration period); - bool write(const ros::Time time, const ros::Duration period); - -}; - -} // namespace kuka_rsi_hw_interface - -#endif diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp new file mode 100644 index 000000000..6483406e3 --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -0,0 +1,126 @@ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2014, Norwegian University of Science and Technology +// +// 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 HOLDER 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: Lars Tingelstad + */ + +#ifndef KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ +#define KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ + +// ROS +#include +#include + +// ros_control +#include +#include +#include +#include +#include + +// STL +#include +#include +#include +#include + +// Timers +#include + +// UDP server +#include + +// RSI +#include +#include + +namespace kuka_rsi_hw_interface +{ + +static const double RAD2DEG = 57.295779513082323; +static const double DEG2RAD = 0.017453292519943295; + +class KukaHardwareInterface : public hardware_interface::RobotHW +{ +private: + // ROS node handle + ros::NodeHandle nh_; + + unsigned int n_dof_; + + std::vector joint_names_; + + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_position_command_; + std::vector joint_velocity_command_; + std::vector joint_effort_command_; + + // RSI + RSIState rsi_state_; + RSICommand rsi_command_; + std::vector rsi_initial_joint_positions_; + std::vector rsi_joint_position_corrections_; + uint64_t ipoc_; + + std::unique_ptr > rt_rsi_pub_; + + std::unique_ptr server_; + std::string local_host_; + int local_port_; + std::string remote_host_; + std::string remote_port_; + std::string in_buffer_; + std::string out_buffer_; + + // 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_; + +public: + KukaHardwareInterface(); + ~KukaHardwareInterface(); + + void start(); + void configure(); + bool read(const ros::Time time, const ros::Duration period); + bool write(const ros::Time time, const ros::Duration period); +}; + +} // namespace kuka_rsi_hw_interface + +#endif // KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h deleted file mode 100644 index c1dea43a6..000000000 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h +++ /dev/null @@ -1,89 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * 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 Univ of CO, Boulder 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: Lars Tingelstad - */ - -#ifndef KUKA_RSI_HW_INTERFACE_RSI_COMMAND_ -#define KUKA_RSI_HW_INTERFACE_RSI_COMMAND_ - -#include -#include - -namespace kuka_rsi_hw_interface -{ - -class RSICommand -{ -public: - RSICommand(); - RSICommand(std::vector position_corrections, unsigned long long ipoc); - std::string xml_doc; -}; - -RSICommand::RSICommand() -{ - // Intentionally empty -} - -RSICommand::RSICommand(std::vector joint_position_correction, unsigned long long ipoc) -{ - TiXmlDocument doc; - TiXmlElement* root = new TiXmlElement("Sen"); - root->SetAttribute("Type", "ImFree"); - TiXmlElement* el = new TiXmlElement("AK"); - // Add string attribute - el->SetAttribute("A1", std::to_string(joint_position_correction[0])); - el->SetAttribute("A2", std::to_string(joint_position_correction[1])); - el->SetAttribute("A3", std::to_string(joint_position_correction[2])); - el->SetAttribute("A4", std::to_string(joint_position_correction[3])); - el->SetAttribute("A5", std::to_string(joint_position_correction[4])); - el->SetAttribute("A6", std::to_string(joint_position_correction[5])); - - root->LinkEndChild(el); - el = new TiXmlElement("IPOC"); - el->LinkEndChild(new TiXmlText(std::to_string(ipoc))); - root->LinkEndChild(el); - doc.LinkEndChild(root); - TiXmlPrinter printer; - printer.SetStreamPrinting(); - doc.Accept(&printer); - - xml_doc = printer.Str(); -} - -} // namespace kuka_rsi_hw_interface - -#endif diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.hpp new file mode 100644 index 000000000..57902ed18 --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.hpp @@ -0,0 +1,86 @@ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2014, Norwegian University of Science and Technology +// +// 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 HOLDER 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: Lars Tingelstad + */ + +#ifndef KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HPP_ +#define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HPP_ + +#include +#include +#include + +namespace kuka_rsi_hw_interface +{ + +class RSICommand +{ +public: + RSICommand(); + RSICommand(std::vector position_corrections, uint64_t ipoc); + std::string xml_doc; +}; + +RSICommand::RSICommand() +{ + // Intentionally empty +} + +RSICommand::RSICommand(std::vector joint_position_correction, uint64_t ipoc) +{ + TiXmlDocument doc; + TiXmlElement * root = new TiXmlElement("Sen"); + root->SetAttribute("Type", "ImFree"); + TiXmlElement * el = new TiXmlElement("AK"); + // Add string attribute + el->SetAttribute("A1", std::to_string(joint_position_correction[0])); + el->SetAttribute("A2", std::to_string(joint_position_correction[1])); + el->SetAttribute("A3", std::to_string(joint_position_correction[2])); + el->SetAttribute("A4", std::to_string(joint_position_correction[3])); + el->SetAttribute("A5", std::to_string(joint_position_correction[4])); + el->SetAttribute("A6", std::to_string(joint_position_correction[5])); + + root->LinkEndChild(el); + el = new TiXmlElement("IPOC"); + el->LinkEndChild(new TiXmlText(std::to_string(ipoc))); + root->LinkEndChild(el); + doc.LinkEndChild(root); + TiXmlPrinter printer; + printer.SetStreamPrinting(); + doc.Accept(&printer); + + xml_doc = printer.Str(); +} + +} // namespace kuka_rsi_hw_interface + +#endif // KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h deleted file mode 100644 index 3396543e7..000000000 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h +++ /dev/null @@ -1,129 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * 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 Univ of CO, Boulder 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: Lars Tingelstad -*/ - -#ifndef KUKA_RSI_HW_INTERFACE_RSI_STATE_ -#define KUKA_RSI_HW_INTERFACE_RSI_STATE_ - -#include -#include - -namespace kuka_rsi_hw_interface -{ - -class RSIState -{ - -private: - std::string xml_doc_; - -public: - RSIState() : - positions(6, 0.0), - initial_positions(6, 0.0), - cart_position(6, 0.0), - initial_cart_position(6, 0.0) - { - xml_doc_.resize(1024); - } - - RSIState(std::string xml_doc); - // AIPOS - std::vector positions; - // ASPos - std::vector initial_positions; - // RIst - std::vector cart_position; - // RSol - std::vector initial_cart_position; - // IPOC - unsigned long long ipoc; - -}; - -RSIState::RSIState(std::string xml_doc) : - xml_doc_(xml_doc), - positions(6, 0.0), - initial_positions(6, 0.0), - cart_position(6, 0.0), - initial_cart_position(6, 0.0) -{ - // Parse message from robot - TiXmlDocument bufferdoc; - bufferdoc.Parse(xml_doc_.c_str()); - // Get the Rob node: - TiXmlElement* rob = bufferdoc.FirstChildElement("Rob"); - // Extract axis specific actual position - TiXmlElement* AIPos_el = rob->FirstChildElement("AIPos"); - AIPos_el->Attribute("A1", &positions[0]); - AIPos_el->Attribute("A2", &positions[1]); - AIPos_el->Attribute("A3", &positions[2]); - AIPos_el->Attribute("A4", &positions[3]); - AIPos_el->Attribute("A5", &positions[4]); - AIPos_el->Attribute("A6", &positions[5]); - // Extract axis specific setpoint position - TiXmlElement* ASPos_el = rob->FirstChildElement("ASPos"); - ASPos_el->Attribute("A1", &initial_positions[0]); - ASPos_el->Attribute("A2", &initial_positions[1]); - ASPos_el->Attribute("A3", &initial_positions[2]); - ASPos_el->Attribute("A4", &initial_positions[3]); - ASPos_el->Attribute("A5", &initial_positions[4]); - ASPos_el->Attribute("A6", &initial_positions[5]); - // Extract cartesian actual position - TiXmlElement* RIst_el = rob->FirstChildElement("RIst"); - RIst_el->Attribute("X", &cart_position[0]); - RIst_el->Attribute("Y", &cart_position[1]); - RIst_el->Attribute("Z", &cart_position[2]); - RIst_el->Attribute("A", &cart_position[3]); - RIst_el->Attribute("B", &cart_position[4]); - RIst_el->Attribute("C", &cart_position[5]); - // Extract cartesian actual position - TiXmlElement* RSol_el = rob->FirstChildElement("RSol"); - RSol_el->Attribute("X", &initial_cart_position[0]); - RSol_el->Attribute("Y", &initial_cart_position[1]); - RSol_el->Attribute("Z", &initial_cart_position[2]); - RSol_el->Attribute("A", &initial_cart_position[3]); - RSol_el->Attribute("B", &initial_cart_position[4]); - RSol_el->Attribute("C", &initial_cart_position[5]); - // Get the IPOC timestamp - TiXmlElement* ipoc_el = rob->FirstChildElement("IPOC"); - ipoc = std::stoull(ipoc_el->FirstChild()->Value()); -} - -} // namespace kuka_rsi_hw_interface - -#endif diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.hpp new file mode 100644 index 000000000..3083ac0ee --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.hpp @@ -0,0 +1,125 @@ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2014, Norwegian University of Science and Technology +// +// 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 HOLDER 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: Lars Tingelstad + */ + +#ifndef KUKA_RSI_HW_INTERFACE__RSI_STATE_HPP_ +#define KUKA_RSI_HW_INTERFACE__RSI_STATE_HPP_ + +#include +#include +#include +#include + +namespace kuka_rsi_hw_interface +{ + +class RSIState +{ +private: + std::string xml_doc_; + +public: + RSIState() + : positions(6, 0.0), + initial_positions(6, 0.0), + cart_position(6, 0.0), + initial_cart_position(6, 0.0) + { + xml_doc_.resize(1024); + } + + explicit RSIState(std::string xml_doc); + // AIPOS + std::vector positions; + // ASPos + std::vector initial_positions; + // RIst + std::vector cart_position; + // RSol + std::vector initial_cart_position; + // IPOC + uint64_t ipoc; +}; + +RSIState::RSIState(std::string xml_doc) +: xml_doc_(xml_doc), + positions(6, 0.0), + initial_positions(6, 0.0), + cart_position(6, 0.0), + initial_cart_position(6, 0.0) +{ + // Parse message from robot + TiXmlDocument bufferdoc; + bufferdoc.Parse(xml_doc_.c_str()); + // Get the Rob node: + TiXmlElement * rob = bufferdoc.FirstChildElement("Rob"); + // Extract axis specific actual position + TiXmlElement * AIPos_el = rob->FirstChildElement("AIPos"); + AIPos_el->Attribute("A1", &positions[0]); + AIPos_el->Attribute("A2", &positions[1]); + AIPos_el->Attribute("A3", &positions[2]); + AIPos_el->Attribute("A4", &positions[3]); + AIPos_el->Attribute("A5", &positions[4]); + AIPos_el->Attribute("A6", &positions[5]); + // Extract axis specific setpoint position + TiXmlElement * ASPos_el = rob->FirstChildElement("ASPos"); + ASPos_el->Attribute("A1", &initial_positions[0]); + ASPos_el->Attribute("A2", &initial_positions[1]); + ASPos_el->Attribute("A3", &initial_positions[2]); + ASPos_el->Attribute("A4", &initial_positions[3]); + ASPos_el->Attribute("A5", &initial_positions[4]); + ASPos_el->Attribute("A6", &initial_positions[5]); + // Extract cartesian actual position + TiXmlElement * RIst_el = rob->FirstChildElement("RIst"); + RIst_el->Attribute("X", &cart_position[0]); + RIst_el->Attribute("Y", &cart_position[1]); + RIst_el->Attribute("Z", &cart_position[2]); + RIst_el->Attribute("A", &cart_position[3]); + RIst_el->Attribute("B", &cart_position[4]); + RIst_el->Attribute("C", &cart_position[5]); + // Extract cartesian actual position + TiXmlElement * RSol_el = rob->FirstChildElement("RSol"); + RSol_el->Attribute("X", &initial_cart_position[0]); + RSol_el->Attribute("Y", &initial_cart_position[1]); + RSol_el->Attribute("Z", &initial_cart_position[2]); + RSol_el->Attribute("A", &initial_cart_position[3]); + RSol_el->Attribute("B", &initial_cart_position[4]); + RSol_el->Attribute("C", &initial_cart_position[5]); + // Get the IPOC timestamp + TiXmlElement * ipoc_el = rob->FirstChildElement("IPOC"); + ipoc = std::stoull(ipoc_el->FirstChild()->Value()); +} + +} // namespace kuka_rsi_hw_interface + +#endif // KUKA_RSI_HW_INTERFACE__RSI_STATE_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h deleted file mode 100644 index 704ae09d0..000000000 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ /dev/null @@ -1,178 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * 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 Norwegian University of Science and - * Technology, 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: Lars Tingelstad - */ - -#ifndef KUKA_RSI_HARDWARE_INTERFACE_UDP_SERVER_ -#define KUKA_RSI_HARDWARE_INTERFACE_UDP_SERVER_ - -#include -#include -#include - -// Select includes -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define BUFSIZE 1024 - -class UDPServer -{ -public: - UDPServer(std::string host, unsigned short port) : local_host_(host), local_port_(port), timeout_(false) - { - sockfd_ = socket(AF_INET, SOCK_DGRAM, 0); - if (sockfd_ < 0) - throw std::runtime_error("Error opening socket: " + std::string(strerror(errno))); - optval = 1; - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *)&optval , sizeof(int)); - memset(&serveraddr_, 0, sizeof(serveraddr_)); - serveraddr_.sin_family = AF_INET; - serveraddr_.sin_addr.s_addr = inet_addr(local_host_.c_str()); - serveraddr_.sin_port = htons(local_port_); - if (bind(sockfd_, (struct sockaddr *) &serveraddr_, sizeof(serveraddr_)) < 0) - throw std::runtime_error("Error binding socket: " + std::string(strerror(errno))); - clientlen_ = sizeof(clientaddr_); - } - - ~UDPServer() - { - close(sockfd_); - } - - bool set_timeout(int millisecs) - { - if (millisecs != 0) - { - tv_.tv_sec = millisecs / 1000; - tv_.tv_usec = (millisecs % 1000) * 1000; - timeout_ = true; - return timeout_; - } - else - { - return timeout_; - } - } - - ssize_t send(std::string& buffer) - { - ssize_t bytes = 0; - bytes = sendto(sockfd_, buffer.c_str(), buffer.size(), 0, (struct sockaddr *) &clientaddr_, clientlen_); - if (bytes < 0) - { - std::cout << "ERROR in sendto" << std::endl; - } - - return bytes; - } - - ssize_t recv(std::string& buffer) - { - ssize_t bytes = 0; - - if (timeout_) - { - fd_set read_fds; - FD_ZERO(&read_fds); - FD_SET(sockfd_, &read_fds); - - struct timeval tv; - tv.tv_sec = tv_.tv_sec; - tv.tv_usec = tv_.tv_usec; - - if (select(sockfd_+1, &read_fds, NULL, NULL, &tv) < 0) - { - return 0; - } - - if (FD_ISSET(sockfd_, &read_fds)) - { - memset(buffer_, 0, BUFSIZE); - bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); - if (bytes < 0) - { - std::cout << "ERROR in recvfrom" << std::endl; - } - } - else - { - return 0; - } - - } - else - { - memset(buffer_, 0, BUFSIZE); - bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); - if (bytes < 0) - { - std::cout << "ERROR in recvfrom" << std::endl; - } - } - - buffer = std::string(buffer_); - - return bytes; - } - -private: - std::string local_host_; - unsigned short local_port_; - bool timeout_; - struct timeval tv_; - - int sockfd_; - socklen_t clientlen_; - struct sockaddr_in serveraddr_; - struct sockaddr_in clientaddr_; - char buffer_[BUFSIZE]; - int optval; - -}; - -#endif - diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.hpp new file mode 100644 index 000000000..ed17a2fb6 --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.hpp @@ -0,0 +1,171 @@ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2014, Norwegian University of Science and Technology +// +// 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 HOLDER 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: Lars Tingelstad + */ + +#ifndef KUKA_RSI_HW_INTERFACE__UDP_SERVER_HPP_ +#define KUKA_RSI_HW_INTERFACE__UDP_SERVER_HPP_ + +// Select includes +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#define BUFSIZE 1024 + +class UDPServer +{ +public: + UDPServer(std::string host, unsigned short port) + : local_host_(host), local_port_(port), timeout_(false) + { + sockfd_ = socket(AF_INET, SOCK_DGRAM, 0); + if (sockfd_ < 0) + throw std::runtime_error("Error opening socket: " + std::string(strerror(errno))); + optval = 1; + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *)&optval, sizeof(int)); + memset(&serveraddr_, 0, sizeof(serveraddr_)); + serveraddr_.sin_family = AF_INET; + serveraddr_.sin_addr.s_addr = inet_addr(local_host_.c_str()); + serveraddr_.sin_port = htons(local_port_); + if (bind(sockfd_, (struct sockaddr *)&serveraddr_, sizeof(serveraddr_)) < 0) + throw std::runtime_error("Error binding socket: " + std::string(strerror(errno))); + clientlen_ = sizeof(clientaddr_); + } + + ~UDPServer() { close(sockfd_); } + + bool set_timeout(int millisecs) + { + if (millisecs != 0) + { + tv_.tv_sec = millisecs / 1000; + tv_.tv_usec = (millisecs % 1000) * 1000; + timeout_ = true; + return timeout_; + } + else + { + return timeout_; + } + } + + ssize_t send(std::string & buffer) + { + ssize_t bytes = 0; + bytes = sendto( + sockfd_, buffer.c_str(), buffer.size(), 0, (struct sockaddr *)&clientaddr_, clientlen_); + if (bytes < 0) + { + std::cout << "ERROR in sendto" << std::endl; + } + + return bytes; + } + + ssize_t recv(std::string & buffer) + { + ssize_t bytes = 0; + + if (timeout_) + { + fd_set read_fds; + FD_ZERO(&read_fds); + FD_SET(sockfd_, &read_fds); + + struct timeval tv; + tv.tv_sec = tv_.tv_sec; + tv.tv_usec = tv_.tv_usec; + + if (select(sockfd_ + 1, &read_fds, NULL, NULL, &tv) < 0) + { + return 0; + } + + if (FD_ISSET(sockfd_, &read_fds)) + { + memset(buffer_, 0, BUFSIZE); + bytes = + recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *)&clientaddr_, &clientlen_); + if (bytes < 0) + { + std::cout << "ERROR in recvfrom" << std::endl; + } + } + else + { + return 0; + } + } + else + { + memset(buffer_, 0, BUFSIZE); + bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *)&clientaddr_, &clientlen_); + if (bytes < 0) + { + std::cout << "ERROR in recvfrom" << std::endl; + } + } + + buffer = std::string(buffer_); + + return bytes; + } + +private: + std::string local_host_; + uint16_t local_port_; + bool timeout_; + struct timeval tv_; + + int sockfd_; + socklen_t clientlen_; + struct sockaddr_in serveraddr_; + struct sockaddr_in clientaddr_; + char buffer_[BUFSIZE]; + int optval; +}; + +#endif // KUKA_RSI_HW_INTERFACE__UDP_SERVER_HPP_ diff --git a/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src b/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src index c8d384875..612e040f9 100644 --- a/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src +++ b/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi.src @@ -25,7 +25,7 @@ DEF ros_rsi( ) ; Technology, 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 @@ -54,7 +54,7 @@ DEF ros_rsi( ) ;FOLD INI ;FOLD BASISTECH INI GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) - INTERRUPT ON 3 + INTERRUPT ON 3 BAS (#INITMOV,0 ) ;ENDFOLD (BASISTECH INI) ;FOLD USER INI @@ -154,10 +154,10 @@ IF (err <> #RSIOK) THEN HALT ENDIF -; Runs untill RSI break +; Runs until RSI break ST_SKIPSENS() -; Turn off RSI +; Turn off RSI err = ST_OFF() IF (err <> #RSIOK) THEN HALT diff --git a/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi_ethernet.xml b/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi_ethernet.xml index 531e44926..35df8dd28 100644 --- a/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi_ethernet.xml +++ b/kuka_rsi_hw_interface/krl/KR_C2/ros_rsi_ethernet.xml @@ -5,7 +5,7 @@ UDP ImFree FALSE - + diff --git a/kuka_rsi_hw_interface/krl/KR_C4/README.md b/kuka_rsi_hw_interface/krl/KR_C4/README.md index 0aba235f8..58e21e546 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/README.md +++ b/kuka_rsi_hw_interface/krl/KR_C4/README.md @@ -112,4 +112,3 @@ $ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller Choose **controller manager ns** and **controller** and you should be able to move each robot joint. * Note that T1-mode limits the robot movement velocity and is intended for testing purposes. - diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi index 25bcf9840..fd00db4e3 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi @@ -1,4 +1,4 @@ - + @@ -109,4 +109,4 @@ - \ No newline at end of file + diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram index 6386e4208..f42f47b99 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram @@ -1,4 +1,4 @@ - + @@ -195,4 +195,4 @@ - \ No newline at end of file + diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml index 30f5715c6..ab64bd15b 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml @@ -1,4 +1,4 @@ - + @@ -48,4 +48,4 @@ - \ No newline at end of file + diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src index 9df9f6d6e..8dedecfcb 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src @@ -25,7 +25,7 @@ DEF RSI_AxisCorr( ) ; Technology, 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 @@ -42,9 +42,9 @@ DEF RSI_AxisCorr( ) ; ============================================= ; ============================================= -; +; ; ROS INDUSTRIAL: KUKA RSI HW INTERFACE -; Realtime UDP data exchange with +; Realtime UDP data exchange with ; kuka_rsi_hw_interface ; ; ============================================= @@ -56,7 +56,7 @@ DECL INT CONTID ; ContainerID ;FOLD INI ;FOLD BASISTECH INI GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) - INTERRUPT ON 3 + INTERRUPT ON 3 BAS (#INITMOV,0 ) ;ENDFOLD (BASISTECH INI) ;FOLD USER INI @@ -68,7 +68,7 @@ DECL INT CONTID ; ContainerID ; Move to start position PTP {A1 0, A2 -90, A3 90, A4 0, A5 90, A6 0} -; Create RSI Context +; Create RSI Context ret = RSI_CREATE("ros_rsi.rsi",CONTID,TRUE) IF (ret <> RSIOK) THEN HALT @@ -83,7 +83,7 @@ ENDIF ; Sensor guided movement RSI_MOVECORR() -; Turn off RSI +; Turn off RSI ret = RSI_OFF() IF (ret <> RSIOK) THEN HALT diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml index 806e89a90..3bdadd954 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml +++ b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml @@ -1,17 +1,17 @@ ip.address.of.rospc - 49152 - ImFree + 49152 + ImFree FALSE - - + + - + diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index f03f13bb6..a60f63432 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -1,54 +1,56 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * 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 Norwegian University of Science and - * Technology, 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. - *********************************************************************/ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2014, Norwegian University of Science and Technology +// +// 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 HOLDER 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: Lars Tingelstad */ -#include +#include #include - namespace kuka_rsi_hw_interface { -KukaHardwareInterface::KukaHardwareInterface() : - joint_position_(6, 0.0), joint_velocity_(6, 0.0), joint_effort_(6, 0.0), joint_position_command_(6, 0.0), joint_velocity_command_( - 6, 0.0), joint_effort_command_(6, 0.0), joint_names_(6), rsi_initial_joint_positions_(6, 0.0), rsi_joint_position_corrections_( - 6, 0.0), ipoc_(0), n_dof_(6) +KukaHardwareInterface::KukaHardwareInterface() +: joint_position_(6, 0.0), + joint_velocity_(6, 0.0), + joint_effort_(6, 0.0), + joint_position_command_(6, 0.0), + joint_velocity_command_(6, 0.0), + joint_effort_command_(6, 0.0), + joint_names_(6), + rsi_initial_joint_positions_(6, 0.0), + rsi_joint_position_corrections_(6, 0.0), + ipoc_(0), + n_dof_(6) { in_buffer_.resize(1024); out_buffer_.resize(1024); @@ -57,24 +59,24 @@ KukaHardwareInterface::KukaHardwareInterface() : if (!nh_.getParam("controller_joint_names", joint_names_)) { - ROS_ERROR("Cannot find required parameter 'controller_joint_names' " + ROS_ERROR( + "Cannot find required parameter 'controller_joint_names' " "on the parameter server."); - throw std::runtime_error("Cannot find required parameter " + throw std::runtime_error( + "Cannot find required parameter " "'controller_joint_names' on the parameter server."); } - //Create ros_control interfaces + // Create ros_control interfaces for (std::size_t i = 0; i < n_dof_; ++i) { // Create joint state interface for all joints - joint_state_interface_.registerHandle( - hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], &joint_velocity_[i], - &joint_effort_[i])); + joint_state_interface_.registerHandle(hardware_interface::JointStateHandle( + joint_names_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i])); // Create joint position control interface - position_joint_interface_.registerHandle( - hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[i]), - &joint_position_command_[i])); + position_joint_interface_.registerHandle(hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); } // Register interfaces @@ -84,10 +86,7 @@ KukaHardwareInterface::KukaHardwareInterface() : ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded kuka_rsi_hardware_interface"); } -KukaHardwareInterface::~KukaHardwareInterface() -{ - -} +KukaHardwareInterface::~KukaHardwareInterface() {} bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration period) { @@ -98,7 +97,8 @@ bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration perio return false; } - if (rt_rsi_pub_->trylock()){ + if (rt_rsi_pub_->trylock()) + { rt_rsi_pub_->msg_.data = in_buffer_; rt_rsi_pub_->unlockAndPublish(); } @@ -119,7 +119,8 @@ bool KukaHardwareInterface::write(const ros::Time time, const ros::Duration peri for (std::size_t i = 0; i < n_dof_; ++i) { - rsi_joint_position_corrections_[i] = (RAD2DEG * joint_position_command_[i]) - rsi_initial_joint_positions_[i]; + rsi_joint_position_corrections_[i] = + (RAD2DEG * joint_position_command_[i]) - rsi_initial_joint_positions_[i]; } out_buffer_ = RSICommand(rsi_joint_position_corrections_, ipoc_).xml_doc; @@ -156,7 +157,6 @@ void KukaHardwareInterface::start() // Set receive timeout to 1 second server_->set_timeout(1000); ROS_INFO_STREAM_NAMED("kuka_hardware_interface", "Got connection from robot"); - } void KukaHardwareInterface::configure() @@ -166,17 +166,20 @@ void KukaHardwareInterface::configure() if (nh_.getParam(param_addr, local_host_) && nh_.getParam(param_port, local_port_)) { - ROS_INFO_STREAM_NAMED("kuka_hardware_interface", - "Setting up RSI server on: (" << local_host_ << ", " << local_port_ << ")"); + ROS_INFO_STREAM_NAMED( + "kuka_hardware_interface", + "Setting up RSI server on: (" << local_host_ << ", " << local_port_ << ")"); } else { - std::string msg = "Failed to get RSI listen address or listen port from" - " parameter server (looking for '" + param_addr + "' and '" + param_port + "')"; + std::string msg = + "Failed to get RSI listen address or listen port from" + " parameter server (looking for '" + + param_addr + "' and '" + param_port + "')"; ROS_ERROR_STREAM(msg); throw std::runtime_error(msg); } rt_rsi_pub_.reset(new realtime_tools::RealtimePublisher(nh_, "rsi_xml_doc", 3)); } -} // namespace kuka_rsi_hardware_interface +} // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp index ba587e348..b504ccf0c 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp @@ -1,45 +1,40 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * 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 Norwegian University of Science and - * Technology, 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. - *********************************************************************/ +// Software License Agreement (3-Clause BSD License) +// +// Copyright (c) 2014, Norwegian University of Science and Technology +// +// 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 HOLDER 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: Lars Tingelstad */ -#include +#include -int main(int argc, char** argv) +int main(int argc, char ** argv) { ROS_INFO_STREAM_NAMED("hardware_interface", "Starting hardware interface..."); @@ -66,12 +61,14 @@ int main(int argc, char** argv) // Get current time and elapsed time since last read timestamp = ros::Time::now(); stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); + period.fromSec( + std::chrono::duration_cast>(stopwatch_now - stopwatch_last) + .count()); stopwatch_last = stopwatch_now; // Run as fast as possible while (ros::ok()) - //while (!g_quit) + // while (!g_quit) { // Receive current state from robot if (!kuka_rsi_hw_interface.read(timestamp, period)) @@ -83,7 +80,9 @@ int main(int argc, char** argv) // Get current time and elapsed time since last read timestamp = ros::Time::now(); stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); + period.fromSec( + std::chrono::duration_cast>(stopwatch_now - stopwatch_last) + .count()); stopwatch_last = stopwatch_now; // Update the controllers @@ -97,5 +96,4 @@ int main(int argc, char** argv) ROS_INFO_STREAM_NAMED("hardware_interface", "Shutting down."); return 0; - } diff --git a/kuka_rsi_hw_interface/test/ag1_hw_controllers.yaml b/kuka_rsi_hw_interface/test/ag1_hw_controllers.yaml index ba5e3067b..44f2d28a9 100644 --- a/kuka_rsi_hw_interface/test/ag1_hw_controllers.yaml +++ b/kuka_rsi_hw_interface/test/ag1_hw_controllers.yaml @@ -15,5 +15,3 @@ position_trajectory_controller: - ag1_joint_6 state_publish_rate: 50 # Defaults to 50 action_monitor_rate: 20 # Defaults to 20 - - diff --git a/kuka_rsi_hw_interface/test/ag2_params.yaml b/kuka_rsi_hw_interface/test/ag2_params.yaml index fbb11e86e..21869f3b4 100644 --- a/kuka_rsi_hw_interface/test/ag2_params.yaml +++ b/kuka_rsi_hw_interface/test/ag2_params.yaml @@ -1,4 +1,3 @@ rsi: listen_address: "192.168.1.11" listen_port: 49152 - diff --git a/kuka_rsi_simulator/scripts/kuka_rsi_simulator b/kuka_rsi_simulator/scripts/kuka_rsi_simulator index db1b47f34..b67e892e9 100755 --- a/kuka_rsi_simulator/scripts/kuka_rsi_simulator +++ b/kuka_rsi_simulator/scripts/kuka_rsi_simulator @@ -10,32 +10,57 @@ import errno import rclpy from std_msgs.msg import String + def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc): q = act_joint_pos qd = setpoint_joint_pos - root = ET.Element('Rob', {'TYPE':'KUKA'}) - ET.SubElement(root, 'RIst', {'X':'0.0', 'Y':'0.0', 'Z':'0.0', - 'A':'0.0', 'B':'0.0', 'C':'0.0'}) - ET.SubElement(root, 'RSol', {'X':'0.0', 'Y':'0.0', 'Z':'0.0', - 'A':'0.0', 'B':'0.0', 'C':'0.0'}) - ET.SubElement(root, 'AIPos', {'A1':str(q[0]), 'A2':str(q[1]), 'A3':str(q[2]), - 'A4':str(q[3]), 'A5':str(q[4]), 'A6':str(q[5])}) - ET.SubElement(root, 'ASPos', {'A1':str(qd[0]), 'A2':str(qd[1]), 'A3':str(qd[2]), - 'A4':str(qd[3]), 'A5':str(qd[4]), 'A6':str(qd[5])}) - ET.SubElement(root, 'Delay', {'D':str(timeout_count)}) - ET.SubElement(root, 'IPOC').text=str(ipoc) + root = ET.Element("Rob", {"TYPE": "KUKA"}) + ET.SubElement( + root, "RIst", {"X": "0.0", "Y": "0.0", "Z": "0.0", "A": "0.0", "B": "0.0", "C": "0.0"} + ) + ET.SubElement( + root, "RSol", {"X": "0.0", "Y": "0.0", "Z": "0.0", "A": "0.0", "B": "0.0", "C": "0.0"} + ) + ET.SubElement( + root, + "AIPos", + { + "A1": str(q[0]), + "A2": str(q[1]), + "A3": str(q[2]), + "A4": str(q[3]), + "A5": str(q[4]), + "A6": str(q[5]), + }, + ) + ET.SubElement( + root, + "ASPos", + { + "A1": str(qd[0]), + "A2": str(qd[1]), + "A3": str(qd[2]), + "A4": str(qd[3]), + "A5": str(qd[4]), + "A6": str(qd[5]), + }, + ) + ET.SubElement(root, "Delay", {"D": str(timeout_count)}) + ET.SubElement(root, "IPOC").text = str(ipoc) return ET.tostring(root) + def parse_rsi_xml_sen(data): root = ET.fromstring(data) - AK = root.find('AK').attrib - desired_joint_correction = np.array([AK['A1'], AK['A2'], AK['A3'], - AK['A4'], AK['A5'], AK['A6']]).astype(np.float64) - IPOC = root.find('IPOC').text + AK = root.find("AK").attrib + desired_joint_correction = np.array( + [AK["A1"], AK["A2"], AK["A3"], AK["A4"], AK["A5"], AK["A6"]] + ).astype(np.float64) + IPOC = root.find("IPOC").text return desired_joint_correction, int(IPOC) -node_name = 'kuka_rsi_simulation' +node_name = "kuka_rsi_simulation" cycle_time = 0.004 act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64) @@ -44,12 +69,23 @@ des_joint_correction_absolute = np.zeros(6) timeout_count = 0 ipoc = 0 -if __name__ == '__main__': +if __name__ == "__main__": import argparse - parser = argparse.ArgumentParser(description='KUKA RSI Simulation') - parser.add_argument('--rsi_hw_iface_ip', default="127.0.0.1", help='The ip address of the RSI control interface (default=127.0.0.1)') - parser.add_argument('--rsi_hw_iface_port', default=49152, help='The port of the RSI control interface (default=49152)') - parser.add_argument('--sen', default='ImFree', help='Type attribute in RSI XML doc. E.g. ') + + parser = argparse.ArgumentParser(description="KUKA RSI Simulation") + parser.add_argument( + "--rsi_hw_iface_ip", + default="127.0.0.1", + help="The ip address of the RSI control interface (default=127.0.0.1)", + ) + parser.add_argument( + "--rsi_hw_iface_port", + default=49152, + help="The port of the RSI control interface (default=49152)", + ) + parser.add_argument( + "--sen", default="ImFree", help='Type attribute in RSI XML doc. E.g. ' + ) # Only parse known arguments args, _ = parser.parse_known_args() host = args.rsi_hw_iface_ip @@ -61,14 +97,14 @@ if __name__ == '__main__': node.get_logger().info(f"Started '{node_name}'") - rsi_act_pub = node.create_publisher(String, '~/rsi/state') - rsi_cmd_pub = node.create_publisher(String, '~/rsi/command') + rsi_act_pub = node.create_publisher(String, "~/rsi/state") + rsi_cmd_pub = node.create_publisher(String, "~/rsi/command") try: s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) node.get_logger().info(f"[{node_name}] Successfully created socket.") s.settimeout(1) - except socket.error as e: + except OSError: node.get_logger().fatal(f"[{node_name}] Could not create socket.") sys.exit() @@ -88,7 +124,7 @@ if __name__ == '__main__': except socket.timeout: node.get_logger().warn(f"[{node_name}] Socket timed out.") timeout_count += 1 - except socket.error as e: + except OSError as e: if e.errno != errno.EINTR: raise diff --git a/kuka_rsi_simulator/setup.py b/kuka_rsi_simulator/setup.py index cf4d2de54..c2b430bbf 100644 --- a/kuka_rsi_simulator/setup.py +++ b/kuka_rsi_simulator/setup.py @@ -1,26 +1,57 @@ +# Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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. + from setuptools import setup -package_name = 'kuka_rsi_simulator' +package_name = "kuka_rsi_simulator" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='Denis Štogl', - maintainer_email='denis@stoglrobotics.de', - description='Python node that implements a minimal RSI interface simulator.', - license='BSD', - tests_require=['pytest'], + maintainer="Denis Štogl", + maintainer_email="denis@stoglrobotics.de", + description="Python node that implements a minimal RSI interface simulator.", + license="BSD", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'kuka_rsi_simulator = kuka_rsi_simulator.kuka_rsi_simulator:main', + "console_scripts": [ + "kuka_rsi_simulator = kuka_rsi_simulator.kuka_rsi_simulator:main", ], }, )