From 84a79543364414ef0783447f4e3cc415dd969264 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Tue, 30 May 2023 15:49:43 +0200 Subject: [PATCH 1/4] [kuka_ros2_control_support] Fix pre-commit --- .clang-format | 2 +- .pre-commit-config.yaml | 8 +- .../kuka_eki_hw_interface.h | 106 -------- .../kuka_eki_hw_interface.hpp | 107 ++++++++ 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 | 250 +++++++++--------- .../src/kuka_eki_hw_interface_node.cpp | 77 +++--- .../launch/bringup.launch.py | 56 ++-- .../launch/test_bringup.launch.py | 56 ++-- .../test/ros2_control_support.py | 62 +++-- .../robot_control_interface.hpp | 68 ++--- .../kuka_rsi_hw_interface/rsi_command.hpp | 81 +++--- .../kuka_rsi_hw_interface/rsi_state.hpp | 82 +++--- .../kuka_rsi_hw_interface/udp_server.hpp | 92 +++---- .../visibility_control.hpp | 70 +++-- .../src/robot_control_interface.cpp | 64 +++-- kuka_rsi_simulator/scripts/kuka_rsi_simulator | 86 ++++-- kuka_rsi_simulator/setup.py | 57 +++- 20 files changed, 699 insertions(+), 634 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 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/.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..cd784008c --- /dev/null +++ b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.hpp @@ -0,0 +1,107 @@ +// Copyright (c) 2018, 3M +// 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 3M 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__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..5550b81db 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,63 @@ -/* - * 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. - */ - +// Copyright (c) 2018, 3M +// 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 3M nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// // Author: Brett Hemes (3M) - -#include -#include -#include +#include #include #include -#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 +71,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 +137,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 +158,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 +180,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 +246,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 +253,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 +280,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..41eac087d 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,42 @@ -/* - * 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. - */ - +// Copyright (c) 2018, 3M +// 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 3M nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// // Author: Brett Hemes (3M) -#include +#include -#include +#include -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "kuka_eki_hw_interface"); @@ -60,7 +61,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 +74,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_ros2_control_support/launch/bringup.launch.py b/kuka_ros2_control_support/launch/bringup.launch.py index c752088b9..8eecf1b4e 100644 --- a/kuka_ros2_control_support/launch/bringup.launch.py +++ b/kuka_ros2_control_support/launch/bringup.launch.py @@ -1,34 +1,34 @@ -# Software License Agreement (BSD License) +# Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +# All rights reserved. # -# 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: +# 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. +# * 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 3M 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. +# 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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler @@ -169,7 +169,7 @@ def generate_launch_description(): DeclareLaunchArgument( "start_rviz", default_value="true", - description="Start vizualization in Rviz2.", + description="Start visualization in Rviz2.", ) ) declared_arguments.append( diff --git a/kuka_ros2_control_support/launch/test_bringup.launch.py b/kuka_ros2_control_support/launch/test_bringup.launch.py index 6aad17d1c..cb5efa3d0 100644 --- a/kuka_ros2_control_support/launch/test_bringup.launch.py +++ b/kuka_ros2_control_support/launch/test_bringup.launch.py @@ -1,34 +1,34 @@ -# Software License Agreement (BSD License) +# Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +# All rights reserved. # -# 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: +# 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. +# * 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 3M 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. +# 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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription @@ -160,7 +160,7 @@ def generate_launch_description(): DeclareLaunchArgument( "start_rviz", default_value="true", - description="Start vizualization in Rviz2.", + description="Start visualization in Rviz2.", ) ) diff --git a/kuka_ros2_control_support/test/ros2_control_support.py b/kuka_ros2_control_support/test/ros2_control_support.py index 1b83d6835..d8e3f0bd6 100755 --- a/kuka_ros2_control_support/test/ros2_control_support.py +++ b/kuka_ros2_control_support/test/ros2_control_support.py @@ -1,37 +1,35 @@ #!/usr/bin/env python - -# Software License Agreement (BSD License) +# Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +# All rights reserved. # -# 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: +# 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. +# * 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 3M 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. - +# 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. import time import unittest @@ -132,10 +130,10 @@ def get_possible_combinations( return possible_combinations -### For test debugging ### +# For test debugging # test_only_these_robot_names = None # e.g. ["lbr_iiwa_14_r820"] start_rviz = False -### For test debugging ### +# For test debugging # package_name = "kuka_ros2_control_support" launch_file_name = "test_bringup.launch.py" @@ -282,7 +280,7 @@ def get_joint_states_from_trajectory_state(): point.time_from_start = Duration(sec=time_from_start) trajectory.points.append(point) - self.node.get_logger().info(f"Publishing trajectory") + self.node.get_logger().info("Publishing trajectory") self.trajectory_publisher.publish(trajectory) time.sleep(2 * time_from_start) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp index 53f2e2b4a..b9bee21d6 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp @@ -1,41 +1,41 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) - * 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. - *********************************************************************/ +// 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 3M 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. +// #ifndef KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_INTERFACE_HPP_ #define KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_INTERFACE_HPP_ #include +#include #include #include #include @@ -104,7 +104,7 @@ class RobotControlnterface : public hardware_interface::SystemInterface kuka_rsi_hw_interface::RSICommand rsi_command_; std::vector rsi_initial_joint_positions_; std::vector rsi_joint_position_corrections_; - unsigned long long ipoc_; + uint64_t ipoc_; std::unique_ptr server_; std::string local_host_; 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 index 644b990bf..55063d91b 100644 --- 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 @@ -1,45 +1,42 @@ -/********************************************************************* - * 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. - *********************************************************************/ +// Copyright (c) 2014, Norwegian University of Science and Technology +// 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 3M 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 -/* - * Author: Lars Tingelstad - */ - -#ifndef KUKA_RSI_HW_INTERFACE_RSI_COMMAND_ -#define KUKA_RSI_HW_INTERFACE_RSI_COMMAND_ +#ifndef KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HPP_ +#define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HPP_ #include +#include #include namespace kuka_rsi_hw_interface @@ -49,7 +46,7 @@ class RSICommand { public: RSICommand(); - RSICommand(std::vector position_corrections, unsigned long long ipoc); + RSICommand(std::vector position_corrections, uint64_t ipoc); std::string xml_doc; }; @@ -58,7 +55,7 @@ RSICommand::RSICommand() // Intentionally empty } -RSICommand::RSICommand(std::vector joint_position_correction, unsigned long long ipoc) +RSICommand::RSICommand(std::vector joint_position_correction, uint64_t ipoc) { TiXmlDocument doc; TiXmlElement * root = new TiXmlElement("Sen"); @@ -86,4 +83,4 @@ RSICommand::RSICommand(std::vector joint_position_correction, unsigned l } // namespace kuka_rsi_hw_interface -#endif +#endif // KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HPP_ 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 index 73c3150bd..1474f3cc8 100644 --- 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 @@ -1,46 +1,44 @@ -/********************************************************************* - * 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. - *********************************************************************/ +// Copyright (c) 2014, Norwegian University of Science and Technology +// 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 3M 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 -/* - * Author: Lars Tingelstad -*/ - -#ifndef KUKA_RSI_HW_INTERFACE_RSI_STATE_ -#define KUKA_RSI_HW_INTERFACE_RSI_STATE_ +#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 { @@ -60,7 +58,7 @@ class RSIState xml_doc_.resize(1024); } - RSIState(std::string xml_doc); + explicit RSIState(std::string xml_doc); // AIPOS std::vector positions; // ASPos @@ -70,7 +68,7 @@ class RSIState // RSol std::vector initial_cart_position; // IPOC - unsigned long long ipoc; + uint64_t ipoc; }; RSIState::RSIState(std::string xml_doc) @@ -124,4 +122,4 @@ RSIState::RSIState(std::string xml_doc) } // namespace kuka_rsi_hw_interface -#endif +#endif // KUKA_RSI_HW_INTERFACE__RSI_STATE_HPP_ 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 index 8541cf068..6bef0eb3e 100644 --- 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 @@ -1,48 +1,39 @@ -/********************************************************************* - * 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 +// Copyright (c) 2014, Norwegian University of Science and Technology +// 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 3M 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__UDP_SERVER_HPP_ +#define KUKA_RSI_HW_INTERFACE__UDP_SERVER_HPP_ // Select includes #include @@ -57,6 +48,11 @@ #include #include +#include +#include +#include +#include + #define BUFSIZE 1024 class UDPServer @@ -157,14 +153,14 @@ class UDPServer } } - buffer = std::string(buffer_); //this probably allocates, use string.assign()? + buffer = std::string(buffer_); // this probably allocates, use string.assign()? return bytes; } private: std::string local_host_; - unsigned short local_port_; + uint16_t local_port_; bool timeout_; struct timeval tv_; @@ -176,4 +172,4 @@ class UDPServer int optval; }; -#endif +#endif // KUKA_RSI_HW_INTERFACE__UDP_SERVER_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp index b32e116bb..8c5550d90 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp @@ -1,36 +1,34 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) - * 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. - *********************************************************************/ +// 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 3M 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. /* This header must be included by all rclcpp headers which declare symbols * which are defined in the rclcpp library. When not building the rclcpp @@ -39,8 +37,8 @@ * library cannot have, but the consuming code must have inorder to link. */ -#ifndef ROS2_CONTROL_DRIVER__VISIBILITY_CONTROL_H_ -#define ROS2_CONTROL_DRIVER__VISIBILITY_CONTROL_H_ +#ifndef KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_HPP_ +#define KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility @@ -73,4 +71,4 @@ #define ROS2_CONTROL_DRIVER_PUBLIC_TYPE #endif -#endif // ROS2_CONTROL_DRIVER__VISIBILITY_CONTROL_H_ +#endif // KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/kuka_rsi_hw_interface/src/robot_control_interface.cpp b/kuka_rsi_hw_interface/src/robot_control_interface.cpp index c91d7508f..7b7acc46a 100644 --- a/kuka_rsi_hw_interface/src/robot_control_interface.cpp +++ b/kuka_rsi_hw_interface/src/robot_control_interface.cpp @@ -1,36 +1,34 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) - * 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. - *********************************************************************/ +// 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 3M 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. #include "kuka_rsi_hw_interface/robot_control_interface.hpp" 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..236927caf 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 3M 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", ], }, ) From 456d5eb9d2ea027a545ddf8221703f05c7720237 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Wed, 31 May 2023 10:53:49 +0200 Subject: [PATCH 2/4] check ci-ros-lint.yml from ros2_control --- .github/workflows/ci-ros-lint.yml | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 0e98f5007..1f09285c5 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,10 @@ jobs: kuka_kr5_support kuka_kr3_support kuka_experimental + kuka_ros2_control_support + kuka_rsi_hw_interface + kuka_rsi_simulator + kuka_eki_hw_interface ament_lint_100: @@ -40,7 +44,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 +62,7 @@ jobs: kuka_kr5_support kuka_kr3_support kuka_experimental + kuka_ros2_control_support + kuka_rsi_hw_interface + kuka_rsi_simulator + kuka_eki_hw_interface From 9a74be411acfcbaf2124a3962a40cce7919bbcf2 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 1 Jun 2023 12:54:56 +0200 Subject: [PATCH 3/4] Update license name and text to be correct. --- .../include/kuka_eki_hw_interface/kuka_eki_hw_interface.hpp | 4 ++-- kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp | 4 ++-- kuka_ros2_control_support/launch/bringup.launch.py | 4 ++-- kuka_ros2_control_support/launch/test_bringup.launch.py | 4 ++-- kuka_ros2_control_support/test/ros2_control_support.py | 4 ++-- .../include/kuka_rsi_hw_interface/robot_control_interface.hpp | 4 ++-- .../include/kuka_rsi_hw_interface/rsi_command.hpp | 4 ++-- .../include/kuka_rsi_hw_interface/rsi_state.hpp | 4 ++-- .../include/kuka_rsi_hw_interface/udp_server.hpp | 4 ++-- .../include/kuka_rsi_hw_interface/visibility_control.hpp | 4 ++-- kuka_rsi_hw_interface/src/robot_control_interface.cpp | 4 ++-- kuka_rsi_simulator/setup.py | 2 +- 12 files changed, 23 insertions(+), 23 deletions(-) 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 index cd784008c..f8b63a2f5 100644 --- 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 @@ -1,7 +1,7 @@ // Copyright (c) 2018, 3M // All rights reserved. // -// Software License Agreement (BSD License 2.0) +// Software License Agreement (3-Clause BSD License) // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * Neither the name of the 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. // 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 41eac087d..9278241dc 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,7 +1,7 @@ // Copyright (c) 2018, 3M // All rights reserved. // -// Software License Agreement (BSD License 2.0) +// Software License Agreement (3-Clause BSD License) // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * 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. // diff --git a/kuka_ros2_control_support/launch/bringup.launch.py b/kuka_ros2_control_support/launch/bringup.launch.py index 8eecf1b4e..4183c1c14 100644 --- a/kuka_ros2_control_support/launch/bringup.launch.py +++ b/kuka_ros2_control_support/launch/bringup.launch.py @@ -1,7 +1,7 @@ # Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) # All rights reserved. # -# Software License Agreement (BSD License 2.0) +# Software License Agreement (3-Clause BSD License) # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ # 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 3M nor the names of its +# * 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. # diff --git a/kuka_ros2_control_support/launch/test_bringup.launch.py b/kuka_ros2_control_support/launch/test_bringup.launch.py index cb5efa3d0..d467bf126 100644 --- a/kuka_ros2_control_support/launch/test_bringup.launch.py +++ b/kuka_ros2_control_support/launch/test_bringup.launch.py @@ -1,7 +1,7 @@ # Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) # All rights reserved. # -# Software License Agreement (BSD License 2.0) +# Software License Agreement (3-Clause BSD License) # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ # 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 3M nor the names of its +# * 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. # diff --git a/kuka_ros2_control_support/test/ros2_control_support.py b/kuka_ros2_control_support/test/ros2_control_support.py index d8e3f0bd6..e11ca86de 100755 --- a/kuka_ros2_control_support/test/ros2_control_support.py +++ b/kuka_ros2_control_support/test/ros2_control_support.py @@ -2,7 +2,7 @@ # Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) # All rights reserved. # -# Software License Agreement (BSD License 2.0) +# Software License Agreement (3-Clause BSD License) # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -14,7 +14,7 @@ # 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 3M nor the names of its +# * 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. # diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp index b9bee21d6..fdd808a71 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_interface.hpp @@ -1,7 +1,7 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // All rights reserved. // -// Software License Agreement (BSD License 2.0) +// Software License Agreement (3-Clause BSD License) // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * 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. // 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 index 55063d91b..f839a8b53 100644 --- 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 @@ -1,7 +1,7 @@ // Copyright (c) 2014, Norwegian University of Science and Technology // All rights reserved. // -// Software License Agreement (BSD License 2.0) +// Software License Agreement (3-Clause BSD License) // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * 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. // 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 index 1474f3cc8..a8e90d108 100644 --- 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 @@ -1,7 +1,7 @@ // Copyright (c) 2014, Norwegian University of Science and Technology // All rights reserved. // -// Software License Agreement (BSD License 2.0) +// Software License Agreement (3-Clause BSD License) // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * 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. // 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 index 6bef0eb3e..2ee3e0f34 100644 --- 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 @@ -1,7 +1,7 @@ // Copyright (c) 2014, Norwegian University of Science and Technology // All rights reserved. // -// Software License Agreement (BSD License 2.0) +// Software License Agreement (3-Clause BSD License) // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * 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. // diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp index 8c5550d90..f3f4beffd 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.hpp @@ -1,7 +1,7 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // All rights reserved. // -// Software License Agreement (BSD License 2.0) +// Software License Agreement (3-Clause BSD License) // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * 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. // diff --git a/kuka_rsi_hw_interface/src/robot_control_interface.cpp b/kuka_rsi_hw_interface/src/robot_control_interface.cpp index 7b7acc46a..75f8ef255 100644 --- a/kuka_rsi_hw_interface/src/robot_control_interface.cpp +++ b/kuka_rsi_hw_interface/src/robot_control_interface.cpp @@ -1,7 +1,7 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // All rights reserved. // -// Software License Agreement (BSD License 2.0) +// Software License Agreement (3-Clause BSD License) // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * 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. // diff --git a/kuka_rsi_simulator/setup.py b/kuka_rsi_simulator/setup.py index 236927caf..c2b430bbf 100644 --- a/kuka_rsi_simulator/setup.py +++ b/kuka_rsi_simulator/setup.py @@ -13,7 +13,7 @@ # 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 3M nor the names of its +# * 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. # From da5a3555cd251748ab1472c039094c73611595fe Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 1 Jun 2023 12:55:17 +0200 Subject: [PATCH 4/4] Update kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp --- kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 5550b81db..4f4847c9a 100644 --- a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp +++ b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp @@ -13,7 +13,7 @@ // 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 3M nor the names of its +// * Neither the name of 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. //