From 376c4bc357f694a8a90f7c934907b588f495a487 Mon Sep 17 00:00:00 2001 From: Marton Antal Date: Thu, 23 Jun 2022 14:26:21 +0200 Subject: [PATCH 01/97] initial portation of the HW to ROS2_CONTROL --- kuka_rsi_hw_interface/CMakeLists.txt | 4 +- .../kuka_hardware_interface.hpp | 57 ++- .../robot_control_node.hpp | 48 +-- .../visibility_control.h | 56 +++ kuka_rsi_hw_interface/package.xml | 2 + .../src/kuka_hardware_interface.cpp | 387 +++++++++++++----- .../src/robot_control_node.cpp | 224 +++++----- 7 files changed, 533 insertions(+), 245 deletions(-) create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.h diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 8b934422..1a4b32b7 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -20,6 +20,8 @@ find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(kroshu_ros2_core REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) find_package(tinyxml_vendor REQUIRED) find_package(TinyXML REQUIRED) @@ -29,7 +31,7 @@ include_directories(include ${TinyXML2_INCLUDE_DIRS}) add_library(kuka_hardware_interface src/kuka_hardware_interface.cpp ) -ament_target_dependencies(kuka_hardware_interface rclcpp sensor_msgs) +ament_target_dependencies(kuka_hardware_interface rclcpp sensor_msgs hardware_interface) target_link_libraries(kuka_hardware_interface tinyxml) add_executable(robot_control_node diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index ab3f0fbd..285f27b3 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -52,17 +52,53 @@ #include #include +#include "rclcpp/macros.hpp" + +#include "kuka_rsi_hw_interface/visibility_control.h" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + + + +using hardware_interface::return_type; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + namespace kuka_rsi_hw_interface { -class KukaHardwareInterface + +class KukaRSIHardwareInterface : public hardware_interface::SystemInterface { public: - explicit KukaHardwareInterface(const std::string & rsi_ip_address, int rsi_port, uint8_t n_dof); - void start(std::vector & joint_state_msg_position); - void stop(); - bool read(std::vector & joint_state_msg_position); - bool write(const std::vector & joint_pos_correction_deg_); + RCLCPP_SHARED_PTR_DEFINITIONS(KukaRSIHardwareInterface); + + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + std::vector export_state_interfaces() override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + std::vector export_command_interfaces() override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + // return_type start() override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + // return_type stop() override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; bool isActive() const; @@ -75,6 +111,11 @@ class KukaHardwareInterface std::vector initial_joint_pos_ = std::vector(n_dof_, 0.0); std::vector joint_pos_correction_deg_ = std::vector(n_dof_, 0.0); + // Dummy parameters + double hw_start_sec_, hw_stop_sec_, hw_slowdown_; + // Store the command for the simulated robot + std::vector hw_commands_, hw_states_; + uint64_t ipoc_ = 0; RSIState rsi_state_; RSICommand rsi_command_; @@ -83,6 +124,10 @@ class KukaHardwareInterface std::string out_buffer_; std::mutex m_; + double loop_hz_; + std::chrono::steady_clock::time_point time_now_, time_last_; + std::chrono::duration control_period_, elapsed_time_; + static constexpr double R2D = 180 / M_PI; static constexpr double D2R = M_PI / 180; }; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp index 545ab0e5..b77d99a5 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp @@ -33,38 +33,38 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class RobotControlNode : public kroshu_ros2_core::ROS2BaseLCNode { public: - RobotControlNode(const std::string & node_name, const rclcpp::NodeOptions & options); +RobotControlNode(const std::string & node_name, const rclcpp::NodeOptions & options); private: - CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; - CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; +CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; +CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; +CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; +CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; - std::unique_ptr kuka_rsi_hw_interface_; +std::unique_ptr kuka_rsi_hw_interface_; - void commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg); - bool onRSIIPAddressChange(const std::string & rsi_ip_address); - bool onRSIPortAddressChange(int rsi_port); - bool onNDOFChange(uint8_t n_dof); +void commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg); +bool onRSIIPAddressChange(const std::string & rsi_ip_address); +bool onRSIPortAddressChange(int rsi_port); +bool onNDOFChange(uint8_t n_dof); - void ControlLoop(); +void ControlLoop(); - std::thread control_thread_; +std::thread control_thread_; - std::string rsi_ip_address_ = ""; - int rsi_port_ = 0; - uint8_t n_dof_ = DEFAULT_N_DOF; - std::vector controller_joint_names_; +std::string rsi_ip_address_ = ""; +int rsi_port_ = 0; +uint8_t n_dof_ = DEFAULT_N_DOF; +std::vector controller_joint_names_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr - joint_state_publisher_; - rclcpp::Subscription::SharedPtr joint_command_subscription_; - rclcpp::CallbackGroup::SharedPtr cbg_; - sensor_msgs::msg::JointState::SharedPtr joint_command_msg_; - sensor_msgs::msg::JointState joint_state_msg_; - std::mutex m_; - std::condition_variable cv_; +rclcpp_lifecycle::LifecyclePublisher::SharedPtr + joint_state_publisher_; +rclcpp::Subscription::SharedPtr joint_command_subscription_; +rclcpp::CallbackGroup::SharedPtr cbg_; +sensor_msgs::msg::JointState::SharedPtr joint_command_msg_; +sensor_msgs::msg::JointState joint_state_msg_; +std::mutex m_; +std::condition_variable cv_; }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.h new file mode 100644 index 00000000..e008e7cc --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_H_ +#define KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define KUKA_RSI_HW_INTERFACE_EXPORT __attribute__((dllexport)) +#define KUKA_RSI_HW_INTERFACE_IMPORT __attribute__((dllimport)) +#else +#define KUKA_RSI_HW_INTERFACE_EXPORT __declspec(dllexport) +#define KUKA_RSI_HW_INTERFACE_IMPORT __declspec(dllimport) +#endif +#ifdef KUKA_RSI_HW_INTERFACE_BUILDING_DLL +#define KUKA_RSI_HW_INTERFACE_PUBLIC KUKA_RSI_HW_INTERFACE_EXPORT +#else +#define KUKA_RSI_HW_INTERFACE_PUBLIC KUKA_RSI_HW_INTERFACE_IMPORT +#endif +#define KUKA_RSI_HW_INTERFACE_PUBLIC_TYPE KUKA_RSI_HW_INTERFACE_PUBLIC +#define KUKA_RSI_HW_INTERFACE_LOCAL +#else +#define KUKA_RSI_HW_INTERFACE_EXPORT __attribute__((visibility("default"))) +#define KUKA_RSI_HW_INTERFACE_IMPORT +#if __GNUC__ >= 4 +#define KUKA_RSI_HW_INTERFACE_PUBLIC __attribute__((visibility("default"))) +#define KUKA_RSI_HW_INTERFACE_LOCAL __attribute__((visibility("hidden"))) +#else +#define KUKA_RSI_HW_INTERFACE_PUBLIC +#define KUKA_RSI_HW_INTERFACE_LOCAL +#endif +#define KUKA_RSI_HW_INTERFACE_PUBLIC_TYPE +#endif + +#endif // KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_H_ diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml index 671d03b6..b4bed93e 100644 --- a/kuka_rsi_hw_interface/package.xml +++ b/kuka_rsi_hw_interface/package.xml @@ -17,6 +17,8 @@ sensor_msgs kroshu_ros2_core tinyxml_vendor + hardware_interface + pluginlib ament_cmake_copyright ament_cmake_cppcheck diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 0288b2a1..829d2333 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -1,37 +1,37 @@ /********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Norwegian University of Science and - * Technology, nor the names of its contributors may be used to - * endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ +* Software License Agreement (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 @@ -44,98 +44,275 @@ #include #include "kuka_rsi_hw_interface/kuka_hardware_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" namespace kuka_rsi_hw_interface { -KukaHardwareInterface::KukaHardwareInterface( - const std::string & rsi_ip_address, int rsi_port, uint8_t n_dof) -: rsi_ip_address_(rsi_ip_address), - rsi_port_(rsi_port), - n_dof_(n_dof) +// KukaRSIHardwareInterface::KukaRSIHardwareInterface( +// const std::string & rsi_ip_address, int rsi_port, uint8_t n_dof) +// : rsi_ip_address_(rsi_ip_address), +// rsi_port_(rsi_port), +// n_dof_(n_dof) +// { +// in_buffer_.resize(1024); +// out_buffer_.resize(1024); +// } + + +CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { - in_buffer_.resize(1024); - out_buffer_.resize(1024); + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_init()"); + + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo &joint : info_.joints) { + + if(joint.command_interfaces.size() != 1) { + RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting exactly 1 command interface"); + return CallbackReturn::ERROR; + } + + if(joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting only POSITION command interface"); + return CallbackReturn::ERROR; + } + + if(joint.state_interfaces.size() != 1) { + RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting exactly 1 state interface"); + return CallbackReturn::ERROR; + } + + if(joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting only POSITION state interface"); + return CallbackReturn::ERROR; + } + + } + + //RSI + in_buffer_.resize(1024); //udp_server.h --> #define BUFSIZE 1024 + out_buffer_.resize(1024); + + initial_joint_pos_.resize(n_dof_, 0.0); + joint_pos_correction_deg_.resize(n_dof_, 0.0), + ipoc_ = 0; + + rsi_ip_address_ = info_.hardware_parameters["rsi_ip_address_"]; + rsi_port_ = std::stoi(info_.hardware_parameters["rsi_port_"]); + + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), + "robot location: %s:%d", rsi_ip_address_.c_str(), rsi_port_); + + //done + // return return_type::OK; + //lifecycle_state_ = rclcpp_lifecycle::State::CONFIGURED; + return CallbackReturn::SUCCESS; } -void KukaHardwareInterface::start(std::vector & joint_state_msg_position) + +CallbackReturn KukaRSIHardwareInterface::on_configure(const rclcpp_lifecycle::State & previous_state) { - std::lock_guard lock(m_); - // Wait for connection from robot - server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); - // TODO(Marton): use any logger - std::cout << "Waiting for robot connection\n"; - int bytes = server_->recv(in_buffer_); - - // Drop empty frame with RSI <= 2.3 - if (bytes < 100) { - bytes = server_->recv(in_buffer_); - } - - rsi_state_ = RSIState(in_buffer_); - for (std::size_t i = 0; i < n_dof_; ++i) { - joint_state_msg_position[i] = rsi_state_.positions[i] * KukaHardwareInterface::D2R; - initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaHardwareInterface::D2R; - } - ipoc_ = rsi_state_.ipoc; - out_buffer_ = RSICommand(std::vector(n_dof_, 0), ipoc_).xml_doc; - server_->send(out_buffer_); - // Set receive timeout to 1 second - server_->set_timeout(1000); - // TODO(Marton): use any logger - std::cout << "Got connection from robot\n"; - is_active_ = true; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_configure()"); + + //just in case - not 100% sure this is the right thing to do . . . + for (size_t i = 0; i < hw_states_.size(); ++i) { + hw_states_[i] = std::numeric_limits::quiet_NaN(); + hw_commands_[i] = std::numeric_limits::quiet_NaN(); + initial_joint_pos_[i] = 0.0; + joint_pos_correction_deg_[i] = 0.0; + } + + return CallbackReturn::SUCCESS; } -bool KukaHardwareInterface::read(std::vector & joint_state_msg_position) + +std::vector KukaRSIHardwareInterface::export_state_interfaces() { - std::lock_guard lock(m_); - if (!is_active_) { - return false; - } - - if (server_->recv(in_buffer_) == 0) { - return false; - } - rsi_state_ = RSIState(in_buffer_); - for (std::size_t i = 0; i < n_dof_; ++i) { - joint_state_msg_position[i] = rsi_state_.positions[i] * KukaHardwareInterface::D2R; - } - ipoc_ = rsi_state_.ipoc; - return true; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "export_state_interfaces()"); + + std::vector state_interfaces; + for(size_t i=0; i & joint_command_position_msg_) +std::vector KukaRSIHardwareInterface::export_command_interfaces() { - std::lock_guard lock(m_); - if (!is_active_) { - std::cout << "Controller deactivated\n"; - return false; - } - - for (size_t i = 0; i < n_dof_; i++) { - joint_pos_correction_deg_[i] = (joint_command_position_msg_[i] - initial_joint_pos_[i]) * - KukaHardwareInterface::R2D; - } - - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; - server_->send(out_buffer_); - return true; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "export_command_interfaces()"); + + std::vector command_interfaces; + for(size_t i=0; i lock(m_); - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; - server_->send(out_buffer_); - server_.reset(); - is_active_ = false; - std::cout << "Connection to robot terminated\n"; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_activate()"); + + // Wait for connection from robot + server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); + + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Connecting to robot . . ."); + + int bytes = server_->recv(in_buffer_); + + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "got some bytes"); + + // Drop empty frame with RSI <= 2.3 + if(bytes < 100) { + bytes = server_->recv(in_buffer_); + } + + if(bytes < 100) { + RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "not enough data received"); + return CallbackReturn::ERROR; + } + + rsi_state_ = RSIState(in_buffer_); + + for (size_t i = 0; i < n_dof_; ++i) { + hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + hw_commands_[i] = hw_states_[i]; + initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; + // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + } + ipoc_ = rsi_state_.ipoc; + + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; + server_->send(out_buffer_); + server_->set_timeout(1000); // Set receive timeout to 1 second + + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System Sucessfully started!"); + is_active_ = true; + + // status_ = hardware_interface::status::STARTED; + // return return_type::OK; + return CallbackReturn::SUCCESS; +} + +CallbackReturn KukaRSIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_deactivate()"); + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; + server_->send(out_buffer_); + server_.reset(); + is_active_ = false; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System sucessfully stopped!"); + return CallbackReturn::SUCCESS; } -bool KukaHardwareInterface::isActive() const + +// void KukaRSIHardwareInterface::start(std::vector & joint_state_msg_position) +// { +// std::lock_guard lock(m_); +// // Wait for connection from robot +// server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); +// // TODO(Marton): use any logger +// std::cout << "Waiting for robot connection\n"; +// int bytes = server_->recv(in_buffer_); + +// // Drop empty frame with RSI <= 2.3 +// if (bytes < 100) { +// bytes = server_->recv(in_buffer_); +// } + +// rsi_state_ = RSIState(in_buffer_); +// for (std::size_t i = 0; i < n_dof_; ++i) { +// joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; +// initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; +// } +// ipoc_ = rsi_state_.ipoc; +// out_buffer_ = RSICommand(std::vector(n_dof_, 0), ipoc_).xml_doc; +// server_->send(out_buffer_); +// // Set receive timeout to 1 second +// server_->set_timeout(1000); +// // TODO(Marton): use any logger +// std::cout << "Got connection from robot\n"; +// is_active_ = true; +// } + +return_type KukaRSIHardwareInterface::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - return is_active_; + std::lock_guard lock(m_); + if (!is_active_) { + return return_type::ERROR; + } + + if (server_->recv(in_buffer_) == 0) { + return return_type::ERROR; + } + rsi_state_ = RSIState(in_buffer_); + + for (std::size_t i = 0; i < n_dof_; ++i) { + hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Got state %.5f for joint %ld!", hw_states_[i], i); + // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + } + ipoc_ = rsi_state_.ipoc; + return return_type::OK; +} + +return_type KukaRSIHardwareInterface::write(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "write()"); + + std::lock_guard lock(m_); + if (!is_active_) { + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Controller deactivated"); + return return_type::ERROR; + } + + for (size_t i = 0; i < n_dof_; i++) { + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Got command %.5f for joint %ld!",hw_commands_[i], i); + joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * KukaRSIHardwareInterface::R2D; + } + + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; + server_->send(out_buffer_); + return return_type::OK; + +} + +// void KukaRSIHardwareInterface::stop() +// { +// std::lock_guard lock(m_); +// out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; +// server_->send(out_buffer_); +// server_.reset(); +// is_active_ = false; +// std::cout << "Connection to robot terminated\n"; +// } + +bool KukaRSIHardwareInterface::isActive() const +{ + return is_active_; } } // namespace namespace kuka_rsi_hw_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + kuka_rsi_hw_interface::KukaRSIHardwareInterface, + hardware_interface::SystemInterface + ) + diff --git a/kuka_rsi_hw_interface/src/robot_control_node.cpp b/kuka_rsi_hw_interface/src/robot_control_node.cpp index f096b75c..fbf39c9a 100644 --- a/kuka_rsi_hw_interface/src/robot_control_node.cpp +++ b/kuka_rsi_hw_interface/src/robot_control_node.cpp @@ -26,155 +26,161 @@ namespace kuka_rsi_hw_interface { RobotControlNode::RobotControlNode( - const std::string & node_name, - const rclcpp::NodeOptions & options) -: kroshu_ros2_core::ROS2BaseLCNode(node_name, options) + const std::string & node_name, + const rclcpp::NodeOptions & options) + : kroshu_ros2_core::ROS2BaseLCNode(node_name, options) { - auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); - qos.best_effort(); - auto callback = - [this](sensor_msgs::msg::JointState::SharedPtr msg) - { - this->commandReceivedCallback(msg); - }; - - joint_command_msg_ = std::make_shared(); - - registerParameter( - "rsi_ip_address_", "127.0.0.1", - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](const std::string & rsi_ip_address) - { - return this->onRSIIPAddressChange(rsi_ip_address); - }); - - registerParameter( - "rsi_port_", 59152, - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](int rsi_port) - { - return this->onRSIPortAddressChange(rsi_port); - }); - - registerParameter( - "n_dof_", 6, - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](uint8_t n_dof) - { - return this->onNDOFChange(n_dof); - }); - - joint_command_subscription_ = this->create_subscription( - "rsi_joint_command", qos, callback); - - joint_state_publisher_ = - this->create_publisher("rsi_joint_state", 1); - joint_command_msg_ = std::make_shared(); - controller_joint_names_ = std::vector( - {"joint_a1", "joint_a2", "joint_a3", - "joint_a4", "joint_a5", "joint_a6"}); + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); + qos.best_effort(); + auto callback = + [this](sensor_msgs::msg::JointState::SharedPtr msg) + { + this->commandReceivedCallback(msg); + }; + + joint_command_msg_ = std::make_shared(); + + registerParameter( + "rsi_ip_address_", "127.0.0.1", + kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, + [this](const std::string & rsi_ip_address) + { + return this->onRSIIPAddressChange(rsi_ip_address); + }); + + registerParameter( + "rsi_port_", 59152, + kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, + [this](int rsi_port) + { + return this->onRSIPortAddressChange(rsi_port); + }); + + registerParameter( + "n_dof_", 6, + kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, + [this](uint8_t n_dof) + { + return this->onNDOFChange(n_dof); + }); + + joint_command_subscription_ = this->create_subscription( + "rsi_joint_command", qos, callback); + + joint_state_publisher_ = + this->create_publisher("rsi_joint_state", 1); + joint_command_msg_ = std::make_shared(); + controller_joint_names_ = std::vector( + {"joint_a1", "joint_a2", "joint_a3", + "joint_a4", "joint_a5", "joint_a6"}); } -CallbackReturn RobotControlNode::on_configure(const rclcpp_lifecycle::State &) +CallbackReturn RobotControlNode::on_configure(const rclcpp_lifecycle::State& previous_state) { - kuka_rsi_hw_interface_ = std::make_unique( - rsi_ip_address_, rsi_port_, n_dof_); - return SUCCESS; + hardware_interface::HardwareInfo info; + info.name = "KukaRSIHardwareInterface"; + info.type = "System Interface"; + info.hardware_parameters["rsi_ip_address_"] = rsi_ip_address_; + info.hardware_parameters["rsi_port_"] = std::to_string(rsi_port_); + info.hardware_parameters["n_dof_"] = std::to_string(n_dof_); + + kuka_rsi_hw_interface_ = std::make_unique(); + kuka_rsi_hw_interface_->on_init(info); + return SUCCESS; } -CallbackReturn RobotControlNode::on_activate(const rclcpp_lifecycle::State &) +CallbackReturn RobotControlNode::on_activate(const rclcpp_lifecycle::State& previous_state) { - kuka_rsi_hw_interface_->start(joint_state_msg_.position); - joint_command_msg_->position = joint_state_msg_.position; - joint_state_msg_.name = controller_joint_names_; - joint_state_msg_.header.frame_id = "base"; + kuka_rsi_hw_interface_->on_activate(previous_state); + joint_command_msg_->position = joint_state_msg_.position; + joint_state_msg_.name = controller_joint_names_; + joint_state_msg_.header.frame_id = "base"; - joint_state_publisher_->on_activate(); - joint_state_publisher_->publish(joint_state_msg_); + joint_state_publisher_->on_activate(); + joint_state_publisher_->publish(joint_state_msg_); - control_thread_ = std::thread(&RobotControlNode::ControlLoop, this); - return SUCCESS; + control_thread_ = std::thread(&RobotControlNode::ControlLoop, this); + return SUCCESS; } -CallbackReturn RobotControlNode::on_deactivate(const rclcpp_lifecycle::State &) +CallbackReturn RobotControlNode::on_deactivate(const rclcpp_lifecycle::State& previous_state) { - kuka_rsi_hw_interface_->stop(); - joint_state_publisher_->on_deactivate(); - if (control_thread_.joinable()) { - control_thread_.join(); - } - - return SUCCESS; + kuka_rsi_hw_interface_->on_deactivate(previous_state); + joint_state_publisher_->on_deactivate(); + if (control_thread_.joinable()) { + control_thread_.join(); + } + return SUCCESS; } -CallbackReturn RobotControlNode::on_cleanup(const rclcpp_lifecycle::State &) +CallbackReturn RobotControlNode::on_cleanup(const rclcpp_lifecycle::State& previous_state) { - kuka_rsi_hw_interface_ = nullptr; - return SUCCESS; + kuka_rsi_hw_interface_ = nullptr; + return SUCCESS; } void RobotControlNode::ControlLoop() { - while (kuka_rsi_hw_interface_->isActive()) { - std::unique_lock lock(m_); - if (!kuka_rsi_hw_interface_->read(joint_state_msg_.position)) { - RCLCPP_ERROR(get_logger(), "Failed to read state from robot. Shutting down!"); - rclcpp::shutdown(); - return; - } - joint_state_msg_.header.stamp = this->now(); - joint_state_publisher_->publish(joint_state_msg_); - - cv_.wait(lock); - kuka_rsi_hw_interface_->write(joint_command_msg_->position); - } + while (kuka_rsi_hw_interface_->isActive()) { + std::unique_lock lock(m_); + if (kuka_rsi_hw_interface_->read(this->now(), rclcpp::Duration(0, 0)) == return_type::ERROR) { + RCLCPP_ERROR(get_logger(), "Failed to read state from robot. Shutting down!"); + rclcpp::shutdown(); + return; + } + joint_state_msg_.header.stamp = this->now(); + joint_state_publisher_->publish(joint_state_msg_); + + cv_.wait(lock); + kuka_rsi_hw_interface_->write(this->now(), rclcpp::Duration(0, 0)); + } } void RobotControlNode::commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg) { - std::lock_guard lock(m_); - joint_command_msg_ = msg; + std::lock_guard lock(m_); + joint_command_msg_ = msg; - cv_.notify_one(); + cv_.notify_one(); } bool RobotControlNode::onRSIIPAddressChange(const std::string & rsi_ip_address) { - rsi_ip_address_ = rsi_ip_address; - return true; + rsi_ip_address_ = rsi_ip_address; + return true; } bool RobotControlNode::onRSIPortAddressChange(int rsi_port) { - rsi_port_ = rsi_port; - return true; + rsi_port_ = rsi_port; + return true; } bool RobotControlNode::onNDOFChange(uint8_t n_dof) { - n_dof_ = n_dof; - joint_command_msg_->position.resize(n_dof_); - joint_command_msg_->effort.resize(n_dof_); - joint_command_msg_->velocity.resize(n_dof_); - joint_state_msg_.position.resize(n_dof_); - joint_state_msg_.effort.resize(n_dof_); - joint_state_msg_.velocity.resize(n_dof_); - return true; + n_dof_ = n_dof; + joint_command_msg_->position.resize(n_dof_); + joint_command_msg_->effort.resize(n_dof_); + joint_command_msg_->velocity.resize(n_dof_); + joint_state_msg_.position.resize(n_dof_); + joint_state_msg_.effort.resize(n_dof_); + joint_state_msg_.velocity.resize(n_dof_); + return true; } } // namespace kuka_rsi_hw_interface int main(int argc, char * argv[]) { - setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared( - "robot_control_node", - rclcpp::NodeOptions()); - executor.add_node(node->get_node_base_interface()); - executor.spin(); - rclcpp::shutdown(); - return 0; + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared( + "robot_control_node", + rclcpp::NodeOptions()); + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + return 0; } From ec9d93bb84ac072ea25b304785206e185997d9f3 Mon Sep 17 00:00:00 2001 From: Marton Antal Date: Thu, 23 Jun 2022 17:04:58 +0200 Subject: [PATCH 02/97] adding setup launch file, not yet working example --- kuka_rsi_hw_interface/CMakeLists.txt | 5 ++ .../config/controller_joint_names.yaml | 1 + .../config/hardware_controllers.yaml | 18 +++++ .../kuka_rsi_hw_interface.xml | 9 +++ kuka_rsi_hw_interface/package.xml | 1 + kuka_rsi_hw_interface/test/startup.launch.py | 66 +++++++++++++++++++ .../test/test_hardware_interface.launch.py | 57 ++++++++++++++++ kuka_rsi_hw_interface/test/test_params.yaml | 3 + .../test/test_params_sim.yaml | 3 + 9 files changed, 163 insertions(+) create mode 100644 kuka_rsi_hw_interface/config/controller_joint_names.yaml create mode 100644 kuka_rsi_hw_interface/config/hardware_controllers.yaml create mode 100644 kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml create mode 100644 kuka_rsi_hw_interface/test/startup.launch.py create mode 100644 kuka_rsi_hw_interface/test/test_hardware_interface.launch.py create mode 100644 kuka_rsi_hw_interface/test/test_params.yaml create mode 100644 kuka_rsi_hw_interface/test/test_params_sim.yaml diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 1a4b32b7..958d508f 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(sensor_msgs REQUIRED) find_package(kroshu_ros2_core REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) +find_package(controller_manager REQUIRED) find_package(tinyxml_vendor REQUIRED) find_package(TinyXML REQUIRED) @@ -39,6 +40,7 @@ add_executable(robot_control_node ament_target_dependencies(robot_control_node rclcpp kroshu_ros2_core sensor_msgs) target_link_libraries(robot_control_node kuka_hardware_interface) +pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml) install(TARGETS kuka_hardware_interface robot_control_node DESTINATION lib/${PROJECT_NAME}) @@ -62,4 +64,7 @@ if(BUILD_TESTING) ament_xmllint(--exclude ros_rsi.rsi.xml) endif() +install(DIRECTORY config test + DESTINATION share/${PROJECT_NAME}) + ament_package() diff --git a/kuka_rsi_hw_interface/config/controller_joint_names.yaml b/kuka_rsi_hw_interface/config/controller_joint_names.yaml new file mode 100644 index 00000000..5f8f0343 --- /dev/null +++ b/kuka_rsi_hw_interface/config/controller_joint_names.yaml @@ -0,0 +1 @@ +controller_joint_names: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"] diff --git a/kuka_rsi_hw_interface/config/hardware_controllers.yaml b/kuka_rsi_hw_interface/config/hardware_controllers.yaml new file mode 100644 index 00000000..991bb3c9 --- /dev/null +++ b/kuka_rsi_hw_interface/config/hardware_controllers.yaml @@ -0,0 +1,18 @@ +#Publish all joint states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +# Joint trajectory controller +position_trajectory_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + + state_publish_rate: 50 # Defaults to 50 + action_monitor_rate: 20 # Defaults to 20 diff --git a/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml b/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml new file mode 100644 index 00000000..58f4fb45 --- /dev/null +++ b/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml @@ -0,0 +1,9 @@ + + + + ROS2 control Kuka driver. + + + diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml index b4bed93e..c4099fe8 100644 --- a/kuka_rsi_hw_interface/package.xml +++ b/kuka_rsi_hw_interface/package.xml @@ -19,6 +19,7 @@ tinyxml_vendor hardware_interface pluginlib + controller_manager ament_cmake_copyright ament_cmake_cppcheck diff --git a/kuka_rsi_hw_interface/test/startup.launch.py b/kuka_rsi_hw_interface/test/startup.launch.py new file mode 100644 index 00000000..ae33182e --- /dev/null +++ b/kuka_rsi_hw_interface/test/startup.launch.py @@ -0,0 +1,66 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + # Get URDF via xacro + robot_description_path = os.path.join( + get_package_share_directory('kuka_kr6_support'), + 'urdf', + 'kr6r700sixx_ros2_control.xacro') + robot_description_config = xacro.process_file(robot_description_path) + robot_description = {'robot_description': robot_description_config.toxml()} + + robot_forward_controller = os.path.join( + get_package_share_directory('kuka_kr6_support'), + 'config', + 'kr6r700sixx_ros2_controller_config.yaml' + ) + + rviz_config_file = os.path.join( + get_package_share_directory('kuka_kr6_support'), + 'rviz', + 'rviz.rviz') + + return LaunchDescription([ + Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, robot_forward_controller], + # output={ + # 'stdout': 'screen', + # 'stderr': 'screen', + # }, + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[robot_description] + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["forward_command_controller_position", "--controller-manager", "/controller_manager"], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + ]) diff --git a/kuka_rsi_hw_interface/test/test_hardware_interface.launch.py b/kuka_rsi_hw_interface/test/test_hardware_interface.launch.py new file mode 100644 index 00000000..ef4e154d --- /dev/null +++ b/kuka_rsi_hw_interface/test/test_hardware_interface.launch.py @@ -0,0 +1,57 @@ +# from launch import LaunchDescription +# from launch.substitutions import EnvironmentVariable +# import os +# import launch_ros.actions +# import pathlib + +# test_params_sim_file_name = 'test_params_sim.yaml' +# test_params_file_name = 'test_params.yaml' +# hardware_controllers_file_name = 'hardware_controllers.yaml' +# controller_joint_names_file_name = 'controller_joint_names.yaml' + + +# def generate_launch_description(): +# test_params_sim_file_path = Path(get_package_share_directory('the_package'), 'config', test_params_sim_file_name) +# test_params_file_path = Path(get_package_share_directory('the_package'), 'config', test_params_file_name) + +# kuka_rsi_hw_interface_node = launch_ros.actions.Node( +# package='kuka_rsi_hw_interface', +# node_executable='kuka_hardware_interface', +# output='screen', +# parameters=[ +# test_params_sim_file_path +# ], +# ) + +# return LaunchDescription([ +# kuka_rsi_hw_interface_node, +# ]) + +# +# +# + +# +# + +# +# + +# +# +# +# + +# +# +# +# + +# +# + +# diff --git a/kuka_rsi_hw_interface/test/test_params.yaml b/kuka_rsi_hw_interface/test/test_params.yaml new file mode 100644 index 00000000..462b6158 --- /dev/null +++ b/kuka_rsi_hw_interface/test/test_params.yaml @@ -0,0 +1,3 @@ +rsi: + listen_address: "192.168.1.67" + listen_port: 59152 diff --git a/kuka_rsi_hw_interface/test/test_params_sim.yaml b/kuka_rsi_hw_interface/test/test_params_sim.yaml new file mode 100644 index 00000000..de2580a3 --- /dev/null +++ b/kuka_rsi_hw_interface/test/test_params_sim.yaml @@ -0,0 +1,3 @@ +rsi: + listen_address: "127.0.0.1" + listen_port: 59152 From 338bfc8c25b771f3c7145026b48e20b1962a6a73 Mon Sep 17 00:00:00 2001 From: Marton Antal Date: Fri, 24 Jun 2022 13:30:44 +0200 Subject: [PATCH 03/97] some fixes to pluginazation, still not working --- kuka_rsi_hw_interface/CMakeLists.txt | 31 +++- .../kuka_hardware_interface.hpp | 162 +++++++++--------- 2 files changed, 107 insertions(+), 86 deletions(-) diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 958d508f..19940c80 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -29,20 +29,27 @@ find_package(TinyXML REQUIRED) include_directories(include ${TinyXML2_INCLUDE_DIRS}) -add_library(kuka_hardware_interface +add_library(${PROJECT_NAME} src/kuka_hardware_interface.cpp ) -ament_target_dependencies(kuka_hardware_interface rclcpp sensor_msgs hardware_interface) -target_link_libraries(kuka_hardware_interface tinyxml) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface) +target_link_libraries(${PROJECT_NAME} tinyxml) add_executable(robot_control_node src/robot_control_node.cpp) ament_target_dependencies(robot_control_node rclcpp kroshu_ros2_core sensor_msgs) -target_link_libraries(robot_control_node kuka_hardware_interface) +target_link_libraries(robot_control_node ${PROJECT_NAME}) pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml) -install(TARGETS kuka_hardware_interface robot_control_node +install(TARGETS ${PROJECT_NAME} robot_control_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) @@ -64,6 +71,20 @@ if(BUILD_TESTING) ament_xmllint(--exclude ros_rsi.rsi.xml) endif() +## EXPORTS +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + install(DIRECTORY config test DESTINATION share/${PROJECT_NAME}) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 285f27b3..6ee6cccf 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -1,37 +1,37 @@ /********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Norwegian University of Science and - * Technology, nor the names of its contributors may be used to - * endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ +* Software License Agreement (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 @@ -67,69 +67,69 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface namespace kuka_rsi_hw_interface { - + class KukaRSIHardwareInterface : public hardware_interface::SystemInterface { public: - RCLCPP_SHARED_PTR_DEFINITIONS(KukaRSIHardwareInterface); +RCLCPP_SHARED_PTR_DEFINITIONS(KukaRSIHardwareInterface); - KUKA_RSI_HW_INTERFACE_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; +KUKA_RSI_HW_INTERFACE_PUBLIC +CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - KUKA_RSI_HW_INTERFACE_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; +KUKA_RSI_HW_INTERFACE_PUBLIC +CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - KUKA_RSI_HW_INTERFACE_PUBLIC - std::vector export_state_interfaces() override; +KUKA_RSI_HW_INTERFACE_PUBLIC +std::vector export_state_interfaces() override; - KUKA_RSI_HW_INTERFACE_PUBLIC - std::vector export_command_interfaces() override; +KUKA_RSI_HW_INTERFACE_PUBLIC +std::vector export_command_interfaces() override; - KUKA_RSI_HW_INTERFACE_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - // return_type start() override; +KUKA_RSI_HW_INTERFACE_PUBLIC +CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; +// return_type start() override; - KUKA_RSI_HW_INTERFACE_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - // return_type stop() override; +KUKA_RSI_HW_INTERFACE_PUBLIC +CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; +// return_type stop() override; - KUKA_RSI_HW_INTERFACE_PUBLIC - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; +KUKA_RSI_HW_INTERFACE_PUBLIC +return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; - KUKA_RSI_HW_INTERFACE_PUBLIC - return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; +KUKA_RSI_HW_INTERFACE_PUBLIC +return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; - bool isActive() const; +bool isActive() const; private: - bool is_active_ = false; - const uint8_t n_dof_ = 6; - std::string rsi_ip_address_ = ""; - int rsi_port_ = 0; - - std::vector initial_joint_pos_ = std::vector(n_dof_, 0.0); - std::vector joint_pos_correction_deg_ = std::vector(n_dof_, 0.0); - - // Dummy parameters - double hw_start_sec_, hw_stop_sec_, hw_slowdown_; - // Store the command for the simulated robot - std::vector hw_commands_, hw_states_; - - uint64_t ipoc_ = 0; - RSIState rsi_state_; - RSICommand rsi_command_; - std::unique_ptr server_; - std::string in_buffer_; - std::string out_buffer_; - std::mutex m_; - - double loop_hz_; - std::chrono::steady_clock::time_point time_now_, time_last_; - std::chrono::duration control_period_, elapsed_time_; - - static constexpr double R2D = 180 / M_PI; - static constexpr double D2R = M_PI / 180; +bool is_active_ = false; +const uint8_t n_dof_ = 6; +std::string rsi_ip_address_ = ""; +int rsi_port_ = 0; + +std::vector initial_joint_pos_ = std::vector(n_dof_, 0.0); +std::vector joint_pos_correction_deg_ = std::vector(n_dof_, 0.0); + +// Dummy parameters +double hw_start_sec_, hw_stop_sec_, hw_slowdown_; +// Store the command for the simulated robot +std::vector hw_commands_, hw_states_; + +uint64_t ipoc_ = 0; +RSIState rsi_state_; +RSICommand rsi_command_; +std::unique_ptr server_; +std::string in_buffer_; +std::string out_buffer_; +std::mutex m_; + +double loop_hz_; +std::chrono::steady_clock::time_point time_now_, time_last_; +std::chrono::duration control_period_, elapsed_time_; + +static constexpr double R2D = 180 / M_PI; +static constexpr double D2R = M_PI / 180; }; } // namespace kuka_rsi_hw_interface From a7294aad421f248c591a047e0996ff08ad91877a Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 27 Jun 2022 16:49:12 +0200 Subject: [PATCH 04/97] change CI to humble --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 6d7f7f40..f1f6bfde 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -39,7 +39,7 @@ jobs: TEST_COVERAGE: true UPSTREAM_WORKSPACE: 'github:kroshu/kroshu_ros2_core#master' CMAKE_ARGS: '-DMOCK_FRI=ON' - ROS_DISTRO: foxy + ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) EVENT_NAME: ${{ github.event_name }} From 1c54952759531dab121deb4269df227a38369d88 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 27 Jun 2022 17:23:45 +0200 Subject: [PATCH 05/97] hwinterface to SHARED library --- kuka_rsi_hw_interface/CMakeLists.txt | 2 +- kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 19940c80..f9f694c0 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -29,7 +29,7 @@ find_package(TinyXML REQUIRED) include_directories(include ${TinyXML2_INCLUDE_DIRS}) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/kuka_hardware_interface.cpp ) diff --git a/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml b/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml index 58f4fb45..f2fec41c 100644 --- a/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml +++ b/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml @@ -1,6 +1,6 @@ + - ROS2 control Kuka driver. From c5a6c83d68e5beefa5d96b751675afdad0d630a5 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 27 Jun 2022 17:38:05 +0200 Subject: [PATCH 06/97] fix parameter names --- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 829d2333..c5b73276 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -105,8 +105,8 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw joint_pos_correction_deg_.resize(n_dof_, 0.0), ipoc_ = 0; - rsi_ip_address_ = info_.hardware_parameters["rsi_ip_address_"]; - rsi_port_ = std::stoi(info_.hardware_parameters["rsi_port_"]); + rsi_ip_address_ = info_.hardware_parameters["rsi_ip_address"]; + rsi_port_ = std::stoi(info_.hardware_parameters["rsi_port"]); RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "robot location: %s:%d", rsi_ip_address_.c_str(), rsi_port_); From 4f169c28cfe4ebc13a81d42a4725d53a79e66543 Mon Sep 17 00:00:00 2001 From: Marton Antal Date: Wed, 13 Jul 2022 17:29:50 +0200 Subject: [PATCH 07/97] changed to joint_trajectory controller, will fix for forward controller, too --- kuka_rsi_hw_interface/test/startup.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/test/startup.launch.py b/kuka_rsi_hw_interface/test/startup.launch.py index ae33182e..00e73b4a 100644 --- a/kuka_rsi_hw_interface/test/startup.launch.py +++ b/kuka_rsi_hw_interface/test/startup.launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): Node( package="controller_manager", executable="spawner", - arguments=["forward_command_controller_position", "--controller-manager", "/controller_manager"], + arguments=["joint_trajectory_controller", "--controller-manager", "/controller_manager"] ), Node( package="rviz2", From 3f4c45ca44f46395f39ee8ef03f536dea5530cb9 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 10:01:26 +0200 Subject: [PATCH 08/97] launch file rework --- .../joint_trajectory_controller_config.yaml | 16 ++++++ .../config/ros2_controller_config.yaml | 11 +++++ kuka_rsi_hw_interface/test/startup.launch.py | 49 +++++++++---------- 3 files changed, 51 insertions(+), 25 deletions(-) create mode 100644 kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml create mode 100644 kuka_rsi_hw_interface/config/ros2_controller_config.yaml diff --git a/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml b/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml new file mode 100644 index 00000000..fcb94336 --- /dev/null +++ b/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml @@ -0,0 +1,16 @@ +/joint_trajectory_controller: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + + command_interfaces: + - position + state_interfaces: + - position + state_publish_rate: 50.0 + action_monitor_rate: 20.0 diff --git a/kuka_rsi_hw_interface/config/ros2_controller_config.yaml b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml new file mode 100644 index 00000000..aacf2b03 --- /dev/null +++ b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml @@ -0,0 +1,11 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + configure_components_on_start: ["KukaRSIHardwareInterface"] diff --git a/kuka_rsi_hw_interface/test/startup.launch.py b/kuka_rsi_hw_interface/test/startup.launch.py index 00e73b4a..eba51d5f 100644 --- a/kuka_rsi_hw_interface/test/startup.launch.py +++ b/kuka_rsi_hw_interface/test/startup.launch.py @@ -18,11 +18,13 @@ def generate_launch_description(): robot_description_config = xacro.process_file(robot_description_path) robot_description = {'robot_description': robot_description_config.toxml()} - robot_forward_controller = os.path.join( - get_package_share_directory('kuka_kr6_support'), - 'config', - 'kr6r700sixx_ros2_controller_config.yaml' - ) + controller_config = (get_package_share_directory('kuka_rsi_hw_interface') + + "/config/ros2_controller_config.yaml") + + joint_traj_controller_config = (get_package_share_directory('kuka_rsi_hw_interface') + + "/config/joint_trajectory_controller_config.yaml") + + controller_manager_node = '/controller_manager' rviz_config_file = os.path.join( get_package_share_directory('kuka_kr6_support'), @@ -33,34 +35,31 @@ def generate_launch_description(): Node( package='controller_manager', executable='ros2_control_node', - parameters=[robot_description, robot_forward_controller], - # output={ - # 'stdout': 'screen', - # 'stderr': 'screen', - # }, - ), - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[robot_description] + parameters=[robot_description, controller_config] ), + # Node( + # package='robot_state_publisher', + # executable='robot_state_publisher', + # output='both', + # parameters=[robot_description] + # ), Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=["joint_state_broadcaster", "-c", controller_manager_node], ), Node( package="controller_manager", executable="spawner", - arguments=["joint_trajectory_controller", "--controller-manager", "/controller_manager"] + arguments=["joint_trajectory_controller", "-c", controller_manager_node, "-p", + joint_traj_controller_config, "--inactive"] ), - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - ) + # Node( + # package="rviz2", + # executable="rviz2", + # name="rviz2", + # output="log", + # arguments=["-d", rviz_config_file], + # ) ]) From be9d756c77072c202dcc38cc387682933a7035fe Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 10:02:57 +0200 Subject: [PATCH 09/97] uncrustify --- .../kuka_hardware_interface.hpp | 84 ++-- .../robot_control_node.hpp | 48 +-- .../src/kuka_hardware_interface.cpp | 406 +++++++++--------- .../src/robot_control_node.cpp | 230 +++++----- 4 files changed, 393 insertions(+), 375 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 6ee6cccf..8400cac0 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -61,7 +61,6 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" - using hardware_interface::return_type; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -71,65 +70,64 @@ namespace kuka_rsi_hw_interface class KukaRSIHardwareInterface : public hardware_interface::SystemInterface { public: + RCLCPP_SHARED_PTR_DEFINITIONS(KukaRSIHardwareInterface); -RCLCPP_SHARED_PTR_DEFINITIONS(KukaRSIHardwareInterface); - -KUKA_RSI_HW_INTERFACE_PUBLIC -CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; -KUKA_RSI_HW_INTERFACE_PUBLIC -CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; -KUKA_RSI_HW_INTERFACE_PUBLIC -std::vector export_state_interfaces() override; + KUKA_RSI_HW_INTERFACE_PUBLIC + std::vector export_state_interfaces() override; -KUKA_RSI_HW_INTERFACE_PUBLIC -std::vector export_command_interfaces() override; + KUKA_RSI_HW_INTERFACE_PUBLIC + std::vector export_command_interfaces() override; -KUKA_RSI_HW_INTERFACE_PUBLIC -CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; // return_type start() override; -KUKA_RSI_HW_INTERFACE_PUBLIC -CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; // return_type stop() override; -KUKA_RSI_HW_INTERFACE_PUBLIC -return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + KUKA_RSI_HW_INTERFACE_PUBLIC + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; -KUKA_RSI_HW_INTERFACE_PUBLIC -return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + KUKA_RSI_HW_INTERFACE_PUBLIC + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; -bool isActive() const; + bool isActive() const; private: -bool is_active_ = false; -const uint8_t n_dof_ = 6; -std::string rsi_ip_address_ = ""; -int rsi_port_ = 0; + bool is_active_ = false; + const uint8_t n_dof_ = 6; + std::string rsi_ip_address_ = ""; + int rsi_port_ = 0; -std::vector initial_joint_pos_ = std::vector(n_dof_, 0.0); -std::vector joint_pos_correction_deg_ = std::vector(n_dof_, 0.0); + std::vector initial_joint_pos_ = std::vector(n_dof_, 0.0); + std::vector joint_pos_correction_deg_ = std::vector(n_dof_, 0.0); // Dummy parameters -double hw_start_sec_, hw_stop_sec_, hw_slowdown_; + double hw_start_sec_, hw_stop_sec_, hw_slowdown_; // Store the command for the simulated robot -std::vector hw_commands_, hw_states_; - -uint64_t ipoc_ = 0; -RSIState rsi_state_; -RSICommand rsi_command_; -std::unique_ptr server_; -std::string in_buffer_; -std::string out_buffer_; -std::mutex m_; - -double loop_hz_; -std::chrono::steady_clock::time_point time_now_, time_last_; -std::chrono::duration control_period_, elapsed_time_; - -static constexpr double R2D = 180 / M_PI; -static constexpr double D2R = M_PI / 180; + std::vector hw_commands_, hw_states_; + + uint64_t ipoc_ = 0; + RSIState rsi_state_; + RSICommand rsi_command_; + std::unique_ptr server_; + std::string in_buffer_; + std::string out_buffer_; + std::mutex m_; + + double loop_hz_; + std::chrono::steady_clock::time_point time_now_, time_last_; + std::chrono::duration control_period_, elapsed_time_; + + static constexpr double R2D = 180 / M_PI; + static constexpr double D2R = M_PI / 180; }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp index b77d99a5..8bb8f568 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp @@ -33,38 +33,38 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class RobotControlNode : public kroshu_ros2_core::ROS2BaseLCNode { public: -RobotControlNode(const std::string & node_name, const rclcpp::NodeOptions & options); + RobotControlNode(const std::string & node_name, const rclcpp::NodeOptions & options); private: -CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; -CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; -CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; -CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; -std::unique_ptr kuka_rsi_hw_interface_; + std::unique_ptr kuka_rsi_hw_interface_; -void commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg); -bool onRSIIPAddressChange(const std::string & rsi_ip_address); -bool onRSIPortAddressChange(int rsi_port); -bool onNDOFChange(uint8_t n_dof); + void commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg); + bool onRSIIPAddressChange(const std::string & rsi_ip_address); + bool onRSIPortAddressChange(int rsi_port); + bool onNDOFChange(uint8_t n_dof); -void ControlLoop(); + void ControlLoop(); -std::thread control_thread_; + std::thread control_thread_; -std::string rsi_ip_address_ = ""; -int rsi_port_ = 0; -uint8_t n_dof_ = DEFAULT_N_DOF; -std::vector controller_joint_names_; + std::string rsi_ip_address_ = ""; + int rsi_port_ = 0; + uint8_t n_dof_ = DEFAULT_N_DOF; + std::vector controller_joint_names_; -rclcpp_lifecycle::LifecyclePublisher::SharedPtr - joint_state_publisher_; -rclcpp::Subscription::SharedPtr joint_command_subscription_; -rclcpp::CallbackGroup::SharedPtr cbg_; -sensor_msgs::msg::JointState::SharedPtr joint_command_msg_; -sensor_msgs::msg::JointState joint_state_msg_; -std::mutex m_; -std::condition_variable cv_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + joint_state_publisher_; + rclcpp::Subscription::SharedPtr joint_command_subscription_; + rclcpp::CallbackGroup::SharedPtr cbg_; + sensor_msgs::msg::JointState::SharedPtr joint_command_msg_; + sensor_msgs::msg::JointState joint_state_msg_; + std::mutex m_; + std::condition_variable cv_; }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index c5b73276..069ef7fa 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -64,247 +64,268 @@ namespace kuka_rsi_hw_interface CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_init()"); - - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; - } - - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - - for (const hardware_interface::ComponentInfo &joint : info_.joints) { - - if(joint.command_interfaces.size() != 1) { - RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting exactly 1 command interface"); - return CallbackReturn::ERROR; - } - - if(joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting only POSITION command interface"); - return CallbackReturn::ERROR; - } - - if(joint.state_interfaces.size() != 1) { - RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting exactly 1 state interface"); - return CallbackReturn::ERROR; - } - - if(joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "expecting only POSITION state interface"); - return CallbackReturn::ERROR; - } - - } - - //RSI - in_buffer_.resize(1024); //udp_server.h --> #define BUFSIZE 1024 - out_buffer_.resize(1024); - - initial_joint_pos_.resize(n_dof_, 0.0); - joint_pos_correction_deg_.resize(n_dof_, 0.0), - ipoc_ = 0; - - rsi_ip_address_ = info_.hardware_parameters["rsi_ip_address"]; - rsi_port_ = std::stoi(info_.hardware_parameters["rsi_port"]); - - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), - "robot location: %s:%d", rsi_ip_address_.c_str(), rsi_port_); - - //done - // return return_type::OK; - //lifecycle_state_ = rclcpp_lifecycle::State::CONFIGURED; - return CallbackReturn::SUCCESS; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_init()"); + + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) { + + if (joint.command_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "expecting exactly 1 command interface"); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "expecting only POSITION command interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "expecting exactly 1 state interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "expecting only POSITION state interface"); + return CallbackReturn::ERROR; + } + + } + + //RSI + in_buffer_.resize(1024); //udp_server.h --> #define BUFSIZE 1024 + out_buffer_.resize(1024); + + initial_joint_pos_.resize(n_dof_, 0.0); + joint_pos_correction_deg_.resize(n_dof_, 0.0), + ipoc_ = 0; + + rsi_ip_address_ = info_.hardware_parameters["rsi_ip_address"]; + rsi_port_ = std::stoi(info_.hardware_parameters["rsi_port"]); + + RCLCPP_INFO( + rclcpp::get_logger("KukaRSIHardwareInterface"), + "robot location: %s:%d", rsi_ip_address_.c_str(), rsi_port_); + + //done + // return return_type::OK; + //lifecycle_state_ = rclcpp_lifecycle::State::CONFIGURED; + return CallbackReturn::SUCCESS; } -CallbackReturn KukaRSIHardwareInterface::on_configure(const rclcpp_lifecycle::State & previous_state) +CallbackReturn KukaRSIHardwareInterface::on_configure( + const rclcpp_lifecycle::State & previous_state) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_configure()"); + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_configure()"); - //just in case - not 100% sure this is the right thing to do . . . - for (size_t i = 0; i < hw_states_.size(); ++i) { - hw_states_[i] = std::numeric_limits::quiet_NaN(); - hw_commands_[i] = std::numeric_limits::quiet_NaN(); - initial_joint_pos_[i] = 0.0; - joint_pos_correction_deg_[i] = 0.0; - } + //just in case - not 100% sure this is the right thing to do . . . + for (size_t i = 0; i < hw_states_.size(); ++i) { + hw_states_[i] = std::numeric_limits::quiet_NaN(); + hw_commands_[i] = std::numeric_limits::quiet_NaN(); + initial_joint_pos_[i] = 0.0; + joint_pos_correction_deg_[i] = 0.0; + } - return CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } std::vector KukaRSIHardwareInterface::export_state_interfaces() { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "export_state_interfaces()"); - - std::vector state_interfaces; - for(size_t i=0; i state_interfaces; + for (size_t i = 0; i < info_.joints.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, + hardware_interface::HW_IF_POSITION, + &hw_states_[i])); + } + return state_interfaces; } -std::vector KukaRSIHardwareInterface::export_command_interfaces() +std::vector KukaRSIHardwareInterface:: +export_command_interfaces() { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "export_command_interfaces()"); - - std::vector command_interfaces; - for(size_t i=0; i command_interfaces; + for (size_t i = 0; i < info_.joints.size(); i++) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, + hardware_interface::HW_IF_POSITION, + &hw_commands_[i])); + } + return command_interfaces; } CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::State & previous_state) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_activate()"); + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_activate()"); - // Wait for connection from robot - server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); + // Wait for connection from robot + server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Connecting to robot . . ."); + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Connecting to robot . . ."); - int bytes = server_->recv(in_buffer_); + int bytes = server_->recv(in_buffer_); - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "got some bytes"); + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "got some bytes"); - // Drop empty frame with RSI <= 2.3 - if(bytes < 100) { - bytes = server_->recv(in_buffer_); - } + // Drop empty frame with RSI <= 2.3 + if (bytes < 100) { + bytes = server_->recv(in_buffer_); + } - if(bytes < 100) { - RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "not enough data received"); - return CallbackReturn::ERROR; - } + if (bytes < 100) { + RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "not enough data received"); + return CallbackReturn::ERROR; + } - rsi_state_ = RSIState(in_buffer_); + rsi_state_ = RSIState(in_buffer_); - for (size_t i = 0; i < n_dof_; ++i) { - hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; - hw_commands_[i] = hw_states_[i]; - initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; - // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; - } - ipoc_ = rsi_state_.ipoc; + for (size_t i = 0; i < n_dof_; ++i) { + hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + hw_commands_[i] = hw_states_[i]; + initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; + // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + } + ipoc_ = rsi_state_.ipoc; - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; - server_->send(out_buffer_); - server_->set_timeout(1000); // Set receive timeout to 1 second + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; + server_->send(out_buffer_); + server_->set_timeout(1000); // Set receive timeout to 1 second - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System Sucessfully started!"); - is_active_ = true; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System Sucessfully started!"); + is_active_ = true; - // status_ = hardware_interface::status::STARTED; - // return return_type::OK; - return CallbackReturn::SUCCESS; + // status_ = hardware_interface::status::STARTED; + // return return_type::OK; + return CallbackReturn::SUCCESS; } -CallbackReturn KukaRSIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State & previous_state) +CallbackReturn KukaRSIHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & previous_state) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_deactivate()"); - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; - server_->send(out_buffer_); - server_.reset(); - is_active_ = false; - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System sucessfully stopped!"); - return CallbackReturn::SUCCESS; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_deactivate()"); + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; + server_->send(out_buffer_); + server_.reset(); + is_active_ = false; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System sucessfully stopped!"); + return CallbackReturn::SUCCESS; } // void KukaRSIHardwareInterface::start(std::vector & joint_state_msg_position) // { -// std::lock_guard lock(m_); -// // Wait for connection from robot -// server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); -// // TODO(Marton): use any logger -// std::cout << "Waiting for robot connection\n"; -// int bytes = server_->recv(in_buffer_); - -// // Drop empty frame with RSI <= 2.3 -// if (bytes < 100) { -// bytes = server_->recv(in_buffer_); -// } - -// rsi_state_ = RSIState(in_buffer_); -// for (std::size_t i = 0; i < n_dof_; ++i) { -// joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; -// initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; -// } -// ipoc_ = rsi_state_.ipoc; -// out_buffer_ = RSICommand(std::vector(n_dof_, 0), ipoc_).xml_doc; -// server_->send(out_buffer_); -// // Set receive timeout to 1 second -// server_->set_timeout(1000); -// // TODO(Marton): use any logger -// std::cout << "Got connection from robot\n"; -// is_active_ = true; +// std::lock_guard lock(m_); +// // Wait for connection from robot +// server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); +// // TODO(Marton): use any logger +// std::cout << "Waiting for robot connection\n"; +// int bytes = server_->recv(in_buffer_); + +// // Drop empty frame with RSI <= 2.3 +// if (bytes < 100) { +// bytes = server_->recv(in_buffer_); +// } + +// rsi_state_ = RSIState(in_buffer_); +// for (std::size_t i = 0; i < n_dof_; ++i) { +// joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; +// initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; +// } +// ipoc_ = rsi_state_.ipoc; +// out_buffer_ = RSICommand(std::vector(n_dof_, 0), ipoc_).xml_doc; +// server_->send(out_buffer_); +// // Set receive timeout to 1 second +// server_->set_timeout(1000); +// // TODO(Marton): use any logger +// std::cout << "Got connection from robot\n"; +// is_active_ = true; // } -return_type KukaRSIHardwareInterface::read(const rclcpp::Time & time, const rclcpp::Duration & period) +return_type KukaRSIHardwareInterface::read( + const rclcpp::Time & time, + const rclcpp::Duration & period) { - std::lock_guard lock(m_); - if (!is_active_) { - return return_type::ERROR; - } - - if (server_->recv(in_buffer_) == 0) { - return return_type::ERROR; - } - rsi_state_ = RSIState(in_buffer_); - - for (std::size_t i = 0; i < n_dof_; ++i) { - hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Got state %.5f for joint %ld!", hw_states_[i], i); - // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; - } - ipoc_ = rsi_state_.ipoc; - return return_type::OK; + std::lock_guard lock(m_); + if (!is_active_) { + return return_type::ERROR; + } + + if (server_->recv(in_buffer_) == 0) { + return return_type::ERROR; + } + rsi_state_ = RSIState(in_buffer_); + + for (std::size_t i = 0; i < n_dof_; ++i) { + hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + RCLCPP_INFO( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "Got state %.5f for joint %ld!", hw_states_[i], i); + // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + } + ipoc_ = rsi_state_.ipoc; + return return_type::OK; } -return_type KukaRSIHardwareInterface::write(const rclcpp::Time & time, const rclcpp::Duration & period) +return_type KukaRSIHardwareInterface::write( + const rclcpp::Time & time, + const rclcpp::Duration & period) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "write()"); - - std::lock_guard lock(m_); - if (!is_active_) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Controller deactivated"); - return return_type::ERROR; - } - - for (size_t i = 0; i < n_dof_; i++) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Got command %.5f for joint %ld!",hw_commands_[i], i); - joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * KukaRSIHardwareInterface::R2D; - } - - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; - server_->send(out_buffer_); - return return_type::OK; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "write()"); + + std::lock_guard lock(m_); + if (!is_active_) { + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Controller deactivated"); + return return_type::ERROR; + } + + for (size_t i = 0; i < n_dof_; i++) { + RCLCPP_INFO( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "Got command %.5f for joint %ld!", hw_commands_[i], i); + joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * + KukaRSIHardwareInterface::R2D; + } + + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; + server_->send(out_buffer_); + return return_type::OK; } // void KukaRSIHardwareInterface::stop() // { -// std::lock_guard lock(m_); -// out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; -// server_->send(out_buffer_); -// server_.reset(); -// is_active_ = false; -// std::cout << "Connection to robot terminated\n"; +// std::lock_guard lock(m_); +// out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; +// server_->send(out_buffer_); +// server_.reset(); +// is_active_ = false; +// std::cout << "Connection to robot terminated\n"; // } bool KukaRSIHardwareInterface::isActive() const { - return is_active_; + return is_active_; } } // namespace namespace kuka_rsi_hw_interface @@ -312,7 +333,6 @@ bool KukaRSIHardwareInterface::isActive() const #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - kuka_rsi_hw_interface::KukaRSIHardwareInterface, - hardware_interface::SystemInterface - ) - + kuka_rsi_hw_interface::KukaRSIHardwareInterface, + hardware_interface::SystemInterface +) diff --git a/kuka_rsi_hw_interface/src/robot_control_node.cpp b/kuka_rsi_hw_interface/src/robot_control_node.cpp index fbf39c9a..6cb997f2 100644 --- a/kuka_rsi_hw_interface/src/robot_control_node.cpp +++ b/kuka_rsi_hw_interface/src/robot_control_node.cpp @@ -26,161 +26,161 @@ namespace kuka_rsi_hw_interface { RobotControlNode::RobotControlNode( - const std::string & node_name, - const rclcpp::NodeOptions & options) - : kroshu_ros2_core::ROS2BaseLCNode(node_name, options) + const std::string & node_name, + const rclcpp::NodeOptions & options) +: kroshu_ros2_core::ROS2BaseLCNode(node_name, options) { - auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); - qos.best_effort(); - auto callback = - [this](sensor_msgs::msg::JointState::SharedPtr msg) - { - this->commandReceivedCallback(msg); - }; - - joint_command_msg_ = std::make_shared(); - - registerParameter( - "rsi_ip_address_", "127.0.0.1", - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](const std::string & rsi_ip_address) - { - return this->onRSIIPAddressChange(rsi_ip_address); - }); - - registerParameter( - "rsi_port_", 59152, - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](int rsi_port) - { - return this->onRSIPortAddressChange(rsi_port); - }); - - registerParameter( - "n_dof_", 6, - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](uint8_t n_dof) - { - return this->onNDOFChange(n_dof); - }); - - joint_command_subscription_ = this->create_subscription( - "rsi_joint_command", qos, callback); - - joint_state_publisher_ = - this->create_publisher("rsi_joint_state", 1); - joint_command_msg_ = std::make_shared(); - controller_joint_names_ = std::vector( - {"joint_a1", "joint_a2", "joint_a3", - "joint_a4", "joint_a5", "joint_a6"}); + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); + qos.best_effort(); + auto callback = + [this](sensor_msgs::msg::JointState::SharedPtr msg) + { + this->commandReceivedCallback(msg); + }; + + joint_command_msg_ = std::make_shared(); + + registerParameter( + "rsi_ip_address_", "127.0.0.1", + kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, + [this](const std::string & rsi_ip_address) + { + return this->onRSIIPAddressChange(rsi_ip_address); + }); + + registerParameter( + "rsi_port_", 59152, + kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, + [this](int rsi_port) + { + return this->onRSIPortAddressChange(rsi_port); + }); + + registerParameter( + "n_dof_", 6, + kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, + [this](uint8_t n_dof) + { + return this->onNDOFChange(n_dof); + }); + + joint_command_subscription_ = this->create_subscription( + "rsi_joint_command", qos, callback); + + joint_state_publisher_ = + this->create_publisher("rsi_joint_state", 1); + joint_command_msg_ = std::make_shared(); + controller_joint_names_ = std::vector( + {"joint_a1", "joint_a2", "joint_a3", + "joint_a4", "joint_a5", "joint_a6"}); } -CallbackReturn RobotControlNode::on_configure(const rclcpp_lifecycle::State& previous_state) +CallbackReturn RobotControlNode::on_configure(const rclcpp_lifecycle::State & previous_state) { - hardware_interface::HardwareInfo info; - info.name = "KukaRSIHardwareInterface"; - info.type = "System Interface"; - info.hardware_parameters["rsi_ip_address_"] = rsi_ip_address_; - info.hardware_parameters["rsi_port_"] = std::to_string(rsi_port_); - info.hardware_parameters["n_dof_"] = std::to_string(n_dof_); - - kuka_rsi_hw_interface_ = std::make_unique(); - kuka_rsi_hw_interface_->on_init(info); - return SUCCESS; + hardware_interface::HardwareInfo info; + info.name = "KukaRSIHardwareInterface"; + info.type = "System Interface"; + info.hardware_parameters["rsi_ip_address_"] = rsi_ip_address_; + info.hardware_parameters["rsi_port_"] = std::to_string(rsi_port_); + info.hardware_parameters["n_dof_"] = std::to_string(n_dof_); + + kuka_rsi_hw_interface_ = std::make_unique(); + kuka_rsi_hw_interface_->on_init(info); + return SUCCESS; } -CallbackReturn RobotControlNode::on_activate(const rclcpp_lifecycle::State& previous_state) +CallbackReturn RobotControlNode::on_activate(const rclcpp_lifecycle::State & previous_state) { - kuka_rsi_hw_interface_->on_activate(previous_state); - joint_command_msg_->position = joint_state_msg_.position; - joint_state_msg_.name = controller_joint_names_; - joint_state_msg_.header.frame_id = "base"; + kuka_rsi_hw_interface_->on_activate(previous_state); + joint_command_msg_->position = joint_state_msg_.position; + joint_state_msg_.name = controller_joint_names_; + joint_state_msg_.header.frame_id = "base"; - joint_state_publisher_->on_activate(); - joint_state_publisher_->publish(joint_state_msg_); + joint_state_publisher_->on_activate(); + joint_state_publisher_->publish(joint_state_msg_); - control_thread_ = std::thread(&RobotControlNode::ControlLoop, this); - return SUCCESS; + control_thread_ = std::thread(&RobotControlNode::ControlLoop, this); + return SUCCESS; } -CallbackReturn RobotControlNode::on_deactivate(const rclcpp_lifecycle::State& previous_state) +CallbackReturn RobotControlNode::on_deactivate(const rclcpp_lifecycle::State & previous_state) { - kuka_rsi_hw_interface_->on_deactivate(previous_state); - joint_state_publisher_->on_deactivate(); - if (control_thread_.joinable()) { - control_thread_.join(); - } - return SUCCESS; + kuka_rsi_hw_interface_->on_deactivate(previous_state); + joint_state_publisher_->on_deactivate(); + if (control_thread_.joinable()) { + control_thread_.join(); + } + return SUCCESS; } -CallbackReturn RobotControlNode::on_cleanup(const rclcpp_lifecycle::State& previous_state) +CallbackReturn RobotControlNode::on_cleanup(const rclcpp_lifecycle::State & previous_state) { - kuka_rsi_hw_interface_ = nullptr; - return SUCCESS; + kuka_rsi_hw_interface_ = nullptr; + return SUCCESS; } void RobotControlNode::ControlLoop() { - while (kuka_rsi_hw_interface_->isActive()) { - std::unique_lock lock(m_); - if (kuka_rsi_hw_interface_->read(this->now(), rclcpp::Duration(0, 0)) == return_type::ERROR) { - RCLCPP_ERROR(get_logger(), "Failed to read state from robot. Shutting down!"); - rclcpp::shutdown(); - return; - } - joint_state_msg_.header.stamp = this->now(); - joint_state_publisher_->publish(joint_state_msg_); - - cv_.wait(lock); - kuka_rsi_hw_interface_->write(this->now(), rclcpp::Duration(0, 0)); - } + while (kuka_rsi_hw_interface_->isActive()) { + std::unique_lock lock(m_); + if (kuka_rsi_hw_interface_->read(this->now(), rclcpp::Duration(0, 0)) == return_type::ERROR) { + RCLCPP_ERROR(get_logger(), "Failed to read state from robot. Shutting down!"); + rclcpp::shutdown(); + return; + } + joint_state_msg_.header.stamp = this->now(); + joint_state_publisher_->publish(joint_state_msg_); + + cv_.wait(lock); + kuka_rsi_hw_interface_->write(this->now(), rclcpp::Duration(0, 0)); + } } void RobotControlNode::commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg) { - std::lock_guard lock(m_); - joint_command_msg_ = msg; + std::lock_guard lock(m_); + joint_command_msg_ = msg; - cv_.notify_one(); + cv_.notify_one(); } bool RobotControlNode::onRSIIPAddressChange(const std::string & rsi_ip_address) { - rsi_ip_address_ = rsi_ip_address; - return true; + rsi_ip_address_ = rsi_ip_address; + return true; } bool RobotControlNode::onRSIPortAddressChange(int rsi_port) { - rsi_port_ = rsi_port; - return true; + rsi_port_ = rsi_port; + return true; } bool RobotControlNode::onNDOFChange(uint8_t n_dof) { - n_dof_ = n_dof; - joint_command_msg_->position.resize(n_dof_); - joint_command_msg_->effort.resize(n_dof_); - joint_command_msg_->velocity.resize(n_dof_); - joint_state_msg_.position.resize(n_dof_); - joint_state_msg_.effort.resize(n_dof_); - joint_state_msg_.velocity.resize(n_dof_); - return true; + n_dof_ = n_dof; + joint_command_msg_->position.resize(n_dof_); + joint_command_msg_->effort.resize(n_dof_); + joint_command_msg_->velocity.resize(n_dof_); + joint_state_msg_.position.resize(n_dof_); + joint_state_msg_.effort.resize(n_dof_); + joint_state_msg_.velocity.resize(n_dof_); + return true; } } // namespace kuka_rsi_hw_interface int main(int argc, char * argv[]) { - setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared( - "robot_control_node", - rclcpp::NodeOptions()); - executor.add_node(node->get_node_base_interface()); - executor.spin(); - rclcpp::shutdown(); - return 0; + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared( + "robot_control_node", + rclcpp::NodeOptions()); + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + return 0; } From 3805f9ac28125ba0c11f88bae482df66a965ba4b Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 10:59:44 +0200 Subject: [PATCH 10/97] add custom controller_manager, remove unused node --- kuka_rsi_hw_interface/CMakeLists.txt | 9 +- .../config/ros2_controller_config.yaml | 2 +- .../kuka_hardware_interface.hpp | 2 +- .../robot_control_node.hpp | 71 ------- .../src/kuka_hardware_interface.cpp | 91 ++------- .../src/robot_control_node.cpp | 186 ------------------ .../src/rsi_control_node.cpp | 51 +++++ kuka_rsi_hw_interface/test/startup.launch.py | 4 +- 8 files changed, 76 insertions(+), 340 deletions(-) delete mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp delete mode 100644 kuka_rsi_hw_interface/src/robot_control_node.cpp create mode 100644 kuka_rsi_hw_interface/src/rsi_control_node.cpp diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index f9f694c0..0f75cf9b 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -42,14 +42,13 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNC ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface) target_link_libraries(${PROJECT_NAME} tinyxml) -add_executable(robot_control_node - src/robot_control_node.cpp) -ament_target_dependencies(robot_control_node rclcpp kroshu_ros2_core sensor_msgs) -target_link_libraries(robot_control_node ${PROJECT_NAME}) +add_executable(rsi_control_node + src/rsi_control_node.cpp) +ament_target_dependencies(rsi_control_node rclcpp rclcpp_lifecycle controller_manager) pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml) -install(TARGETS ${PROJECT_NAME} robot_control_node +install(TARGETS ${PROJECT_NAME} rsi_control_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/kuka_rsi_hw_interface/config/ros2_controller_config.yaml b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml index aacf2b03..074a01ac 100644 --- a/kuka_rsi_hw_interface/config/ros2_controller_config.yaml +++ b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 250 # Hz joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 8400cac0..4b7a20f3 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -70,7 +70,7 @@ namespace kuka_rsi_hw_interface class KukaRSIHardwareInterface : public hardware_interface::SystemInterface { public: - RCLCPP_SHARED_PTR_DEFINITIONS(KukaRSIHardwareInterface); + RCLCPP_SHARED_PTR_DEFINITIONS(KukaRSIHardwareInterface) KUKA_RSI_HW_INTERFACE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp deleted file mode 100644 index 8bb8f568..00000000 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2022 Aron Svastits -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_NODE_HPP_ -#define KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_NODE_HPP_ - -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "kuka_rsi_hw_interface/kuka_hardware_interface.hpp" -#include "kroshu_ros2_core/ROS2BaseLCNode.hpp" -#include "sensor_msgs/msg/joint_state.hpp" - -#define DEFAULT_N_DOF 6 - -namespace kuka_rsi_hw_interface -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class RobotControlNode : public kroshu_ros2_core::ROS2BaseLCNode -{ -public: - RobotControlNode(const std::string & node_name, const rclcpp::NodeOptions & options); - -private: - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - - std::unique_ptr kuka_rsi_hw_interface_; - - void commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg); - bool onRSIIPAddressChange(const std::string & rsi_ip_address); - bool onRSIPortAddressChange(int rsi_port); - bool onNDOFChange(uint8_t n_dof); - - void ControlLoop(); - - std::thread control_thread_; - - std::string rsi_ip_address_ = ""; - int rsi_port_ = 0; - uint8_t n_dof_ = DEFAULT_N_DOF; - std::vector controller_joint_names_; - - rclcpp_lifecycle::LifecyclePublisher::SharedPtr - joint_state_publisher_; - rclcpp::Subscription::SharedPtr joint_command_subscription_; - rclcpp::CallbackGroup::SharedPtr cbg_; - sensor_msgs::msg::JointState::SharedPtr joint_command_msg_; - sensor_msgs::msg::JointState joint_state_msg_; - std::mutex m_; - std::condition_variable cv_; -}; - -} // namespace kuka_rsi_hw_interface -#endif // KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_NODE_HPP_ diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 069ef7fa..a7091d25 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -50,18 +50,6 @@ namespace kuka_rsi_hw_interface { - -// KukaRSIHardwareInterface::KukaRSIHardwareInterface( -// const std::string & rsi_ip_address, int rsi_port, uint8_t n_dof) -// : rsi_ip_address_(rsi_ip_address), -// rsi_port_(rsi_port), -// n_dof_(n_dof) -// { -// in_buffer_.resize(1024); -// out_buffer_.resize(1024); -// } - - CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_init()"); @@ -106,7 +94,7 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw } //RSI - in_buffer_.resize(1024); //udp_server.h --> #define BUFSIZE 1024 + in_buffer_.resize(1024); // udp_server.h --> #define BUFSIZE 1024 out_buffer_.resize(1024); initial_joint_pos_.resize(n_dof_, 0.0); @@ -120,15 +108,12 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw rclcpp::get_logger("KukaRSIHardwareInterface"), "robot location: %s:%d", rsi_ip_address_.c_str(), rsi_port_); - //done - // return return_type::OK; - //lifecycle_state_ = rclcpp_lifecycle::State::CONFIGURED; return CallbackReturn::SUCCESS; } CallbackReturn KukaRSIHardwareInterface::on_configure( - const rclcpp_lifecycle::State & previous_state) + const rclcpp_lifecycle::State &) { RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_configure()"); @@ -139,7 +124,6 @@ CallbackReturn KukaRSIHardwareInterface::on_configure( initial_joint_pos_[i] = 0.0; joint_pos_correction_deg_[i] = 0.0; } - return CallbackReturn::SUCCESS; } @@ -175,7 +159,7 @@ export_command_interfaces() return command_interfaces; } -CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::State & previous_state) +CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_activate()"); @@ -204,7 +188,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; hw_commands_[i] = hw_states_[i]; initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; - // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; } ipoc_ = rsi_state_.ipoc; @@ -215,13 +199,11 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System Sucessfully started!"); is_active_ = true; - // status_ = hardware_interface::status::STARTED; - // return return_type::OK; return CallbackReturn::SUCCESS; } CallbackReturn KukaRSIHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & previous_state) + const rclcpp_lifecycle::State &) { RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_deactivate()"); out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; @@ -232,43 +214,15 @@ CallbackReturn KukaRSIHardwareInterface::on_deactivate( return CallbackReturn::SUCCESS; } - -// void KukaRSIHardwareInterface::start(std::vector & joint_state_msg_position) -// { -// std::lock_guard lock(m_); -// // Wait for connection from robot -// server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); -// // TODO(Marton): use any logger -// std::cout << "Waiting for robot connection\n"; -// int bytes = server_->recv(in_buffer_); - -// // Drop empty frame with RSI <= 2.3 -// if (bytes < 100) { -// bytes = server_->recv(in_buffer_); -// } - -// rsi_state_ = RSIState(in_buffer_); -// for (std::size_t i = 0; i < n_dof_; ++i) { -// joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; -// initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; -// } -// ipoc_ = rsi_state_.ipoc; -// out_buffer_ = RSICommand(std::vector(n_dof_, 0), ipoc_).xml_doc; -// server_->send(out_buffer_); -// // Set receive timeout to 1 second -// server_->set_timeout(1000); -// // TODO(Marton): use any logger -// std::cout << "Got connection from robot\n"; -// is_active_ = true; -// } - return_type KukaRSIHardwareInterface::read( - const rclcpp::Time & time, - const rclcpp::Duration & period) + const rclcpp::Time &, + const rclcpp::Duration &) { - std::lock_guard lock(m_); if (!is_active_) { - return return_type::ERROR; + RCLCPP_DEBUG( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "Hardware interface is not active"); + return return_type::OK; } if (server_->recv(in_buffer_) == 0) { @@ -288,15 +242,14 @@ return_type KukaRSIHardwareInterface::read( } return_type KukaRSIHardwareInterface::write( - const rclcpp::Time & time, - const rclcpp::Duration & period) + const rclcpp::Time &, + const rclcpp::Duration &) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "write()"); - - std::lock_guard lock(m_); if (!is_active_) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Controller deactivated"); - return return_type::ERROR; + RCLCPP_DEBUG( + rclcpp::get_logger("KukaRSIHardwareInterface"), + "Hardware interface is not active"); + return return_type::OK; } for (size_t i = 0; i < n_dof_; i++) { @@ -313,16 +266,6 @@ return_type KukaRSIHardwareInterface::write( } -// void KukaRSIHardwareInterface::stop() -// { -// std::lock_guard lock(m_); -// out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; -// server_->send(out_buffer_); -// server_.reset(); -// is_active_ = false; -// std::cout << "Connection to robot terminated\n"; -// } - bool KukaRSIHardwareInterface::isActive() const { return is_active_; diff --git a/kuka_rsi_hw_interface/src/robot_control_node.cpp b/kuka_rsi_hw_interface/src/robot_control_node.cpp deleted file mode 100644 index 6cb997f2..00000000 --- a/kuka_rsi_hw_interface/src/robot_control_node.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// Copyright 2022 Aron Svastits -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "kuka_rsi_hw_interface/robot_control_node.hpp" -#include "kroshu_ros2_core/ROS2BaseLCNode.hpp" - -namespace kuka_rsi_hw_interface -{ -RobotControlNode::RobotControlNode( - const std::string & node_name, - const rclcpp::NodeOptions & options) -: kroshu_ros2_core::ROS2BaseLCNode(node_name, options) -{ - auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); - qos.best_effort(); - auto callback = - [this](sensor_msgs::msg::JointState::SharedPtr msg) - { - this->commandReceivedCallback(msg); - }; - - joint_command_msg_ = std::make_shared(); - - registerParameter( - "rsi_ip_address_", "127.0.0.1", - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](const std::string & rsi_ip_address) - { - return this->onRSIIPAddressChange(rsi_ip_address); - }); - - registerParameter( - "rsi_port_", 59152, - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](int rsi_port) - { - return this->onRSIPortAddressChange(rsi_port); - }); - - registerParameter( - "n_dof_", 6, - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](uint8_t n_dof) - { - return this->onNDOFChange(n_dof); - }); - - joint_command_subscription_ = this->create_subscription( - "rsi_joint_command", qos, callback); - - joint_state_publisher_ = - this->create_publisher("rsi_joint_state", 1); - joint_command_msg_ = std::make_shared(); - controller_joint_names_ = std::vector( - {"joint_a1", "joint_a2", "joint_a3", - "joint_a4", "joint_a5", "joint_a6"}); -} - -CallbackReturn RobotControlNode::on_configure(const rclcpp_lifecycle::State & previous_state) -{ - hardware_interface::HardwareInfo info; - info.name = "KukaRSIHardwareInterface"; - info.type = "System Interface"; - info.hardware_parameters["rsi_ip_address_"] = rsi_ip_address_; - info.hardware_parameters["rsi_port_"] = std::to_string(rsi_port_); - info.hardware_parameters["n_dof_"] = std::to_string(n_dof_); - - kuka_rsi_hw_interface_ = std::make_unique(); - kuka_rsi_hw_interface_->on_init(info); - return SUCCESS; -} - -CallbackReturn RobotControlNode::on_activate(const rclcpp_lifecycle::State & previous_state) -{ - kuka_rsi_hw_interface_->on_activate(previous_state); - joint_command_msg_->position = joint_state_msg_.position; - joint_state_msg_.name = controller_joint_names_; - joint_state_msg_.header.frame_id = "base"; - - joint_state_publisher_->on_activate(); - joint_state_publisher_->publish(joint_state_msg_); - - control_thread_ = std::thread(&RobotControlNode::ControlLoop, this); - return SUCCESS; -} - -CallbackReturn RobotControlNode::on_deactivate(const rclcpp_lifecycle::State & previous_state) -{ - kuka_rsi_hw_interface_->on_deactivate(previous_state); - joint_state_publisher_->on_deactivate(); - if (control_thread_.joinable()) { - control_thread_.join(); - } - return SUCCESS; -} - -CallbackReturn RobotControlNode::on_cleanup(const rclcpp_lifecycle::State & previous_state) -{ - kuka_rsi_hw_interface_ = nullptr; - return SUCCESS; -} - -void RobotControlNode::ControlLoop() -{ - while (kuka_rsi_hw_interface_->isActive()) { - std::unique_lock lock(m_); - if (kuka_rsi_hw_interface_->read(this->now(), rclcpp::Duration(0, 0)) == return_type::ERROR) { - RCLCPP_ERROR(get_logger(), "Failed to read state from robot. Shutting down!"); - rclcpp::shutdown(); - return; - } - joint_state_msg_.header.stamp = this->now(); - joint_state_publisher_->publish(joint_state_msg_); - - cv_.wait(lock); - kuka_rsi_hw_interface_->write(this->now(), rclcpp::Duration(0, 0)); - } -} - -void RobotControlNode::commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg) -{ - std::lock_guard lock(m_); - joint_command_msg_ = msg; - - cv_.notify_one(); -} - -bool RobotControlNode::onRSIIPAddressChange(const std::string & rsi_ip_address) -{ - rsi_ip_address_ = rsi_ip_address; - return true; -} - -bool RobotControlNode::onRSIPortAddressChange(int rsi_port) -{ - rsi_port_ = rsi_port; - return true; -} - -bool RobotControlNode::onNDOFChange(uint8_t n_dof) -{ - n_dof_ = n_dof; - joint_command_msg_->position.resize(n_dof_); - joint_command_msg_->effort.resize(n_dof_); - joint_command_msg_->velocity.resize(n_dof_); - joint_state_msg_.position.resize(n_dof_); - joint_state_msg_.effort.resize(n_dof_); - joint_state_msg_.velocity.resize(n_dof_); - return true; -} - -} // namespace kuka_rsi_hw_interface - -int main(int argc, char * argv[]) -{ - setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared( - "robot_control_node", - rclcpp::NodeOptions()); - executor.add_node(node->get_node_base_interface()); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/kuka_rsi_hw_interface/src/rsi_control_node.cpp b/kuka_rsi_hw_interface/src/rsi_control_node.cpp new file mode 100644 index 00000000..01532512 --- /dev/null +++ b/kuka_rsi_hw_interface/src/rsi_control_node.cpp @@ -0,0 +1,51 @@ +// Copyright 2022 Áron Svastits +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto executor = std::make_shared(); + auto controller_manager = std::make_shared( + executor, + "controller_manager"); + + std::thread control_loop([controller_manager]() { + const rclcpp::Duration dt = + rclcpp::Duration::from_seconds(1.0 / controller_manager->get_update_rate()); + + while (rclcpp::ok()) { + controller_manager->read(controller_manager->now(), dt); + controller_manager->update(controller_manager->now(), dt); + controller_manager->write(controller_manager->now(), dt); + } + }); + + executor->add_node(controller_manager); + + executor->spin(); + control_loop.join(); + + // shutdown + rclcpp::shutdown(); + + return 0; +} diff --git a/kuka_rsi_hw_interface/test/startup.launch.py b/kuka_rsi_hw_interface/test/startup.launch.py index eba51d5f..6a132226 100644 --- a/kuka_rsi_hw_interface/test/startup.launch.py +++ b/kuka_rsi_hw_interface/test/startup.launch.py @@ -33,8 +33,8 @@ def generate_launch_description(): return LaunchDescription([ Node( - package='controller_manager', - executable='ros2_control_node', + package='kuka_rsi_hw_interface', + executable='rsi_control_node', parameters=[robot_description, controller_config] ), # Node( From 8e266ee82582cdc0275e0d3ae3e4716d05d2cce1 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 11:07:32 +0200 Subject: [PATCH 11/97] lint --- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index a7091d25..70149556 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -62,7 +62,6 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { - if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( rclcpp::get_logger( @@ -90,10 +89,9 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw "KukaRSIHardwareInterface"), "expecting only POSITION state interface"); return CallbackReturn::ERROR; } - } - //RSI + // RSI in_buffer_.resize(1024); // udp_server.h --> #define BUFSIZE 1024 out_buffer_.resize(1024); @@ -117,7 +115,7 @@ CallbackReturn KukaRSIHardwareInterface::on_configure( { RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_configure()"); - //just in case - not 100% sure this is the right thing to do . . . + // just in case - not 100% sure this is the right thing to do . . . for (size_t i = 0; i < hw_states_.size(); ++i) { hw_states_[i] = std::numeric_limits::quiet_NaN(); hw_commands_[i] = std::numeric_limits::quiet_NaN(); @@ -263,7 +261,6 @@ return_type KukaRSIHardwareInterface::write( out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; server_->send(out_buffer_); return return_type::OK; - } bool KukaRSIHardwareInterface::isActive() const From ce96d769c917c828c70aace118b8d857b512a8b4 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 15:10:01 +0200 Subject: [PATCH 12/97] logging --- .../include/kuka_rsi_hw_interface/udp_server.h | 10 ++++++---- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 2 -- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 37947bd5..2dc4701c 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -57,6 +57,8 @@ #include #include +#include "rclcpp/rclcpp.hpp" + #define BUFSIZE 1024 class UDPServer @@ -66,7 +68,7 @@ class UDPServer : local_host_(host), local_port_(port), timeout_( false) { - printf("%s: %i\n", local_host_.c_str(), local_port_); + RCLCPP_INFO(rclcpp::get_logger("UDPServer"), "%s: %i", local_host_.c_str(), local_port_); sockfd_ = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd_ < 0) { throw std::runtime_error("Error opening socket: " + std::string(strerror(errno))); @@ -107,7 +109,7 @@ class UDPServer sockfd_, buffer.c_str(), buffer.size(), 0, (struct sockaddr *) &clientaddr_, clientlen_); if (bytes < 0) { - std::cout << "ERROR in sendto" << std::endl; + RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in send"); } return bytes; @@ -135,7 +137,7 @@ class UDPServer bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); if (bytes < 0) { - std::cout << "ERROR in recvfrom" << std::endl; + RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } } else { return 0; @@ -145,7 +147,7 @@ class UDPServer memset(buffer_, 0, BUFSIZE); bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); if (bytes < 0) { - std::cout << "ERROR in recvfrom" << std::endl; + RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } } diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 70149556..c5be966a 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -186,7 +186,6 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; hw_commands_[i] = hw_states_[i]; initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; - // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; } ipoc_ = rsi_state_.ipoc; @@ -233,7 +232,6 @@ return_type KukaRSIHardwareInterface::read( RCLCPP_INFO( rclcpp::get_logger( "KukaRSIHardwareInterface"), "Got state %.5f for joint %ld!", hw_states_[i], i); - // joint_state_msg_position[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; } ipoc_ = rsi_state_.ipoc; return return_type::OK; From bf2ec094a34bdd0e135cfbe0f5112fbe3c891c98 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 17:03:41 +0200 Subject: [PATCH 13/97] start controller as active --- kuka_rsi_hw_interface/test/startup.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/test/startup.launch.py b/kuka_rsi_hw_interface/test/startup.launch.py index 6a132226..45d21459 100644 --- a/kuka_rsi_hw_interface/test/startup.launch.py +++ b/kuka_rsi_hw_interface/test/startup.launch.py @@ -52,7 +52,7 @@ def generate_launch_description(): package="controller_manager", executable="spawner", arguments=["joint_trajectory_controller", "-c", controller_manager_node, "-p", - joint_traj_controller_config, "--inactive"] + joint_traj_controller_config] ), # Node( # package="rviz2", From 67938cd7c3094049096754b22e56472894fd6bae Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 17:34:45 +0200 Subject: [PATCH 14/97] remove unnecessary logs --- .../src/kuka_hardware_interface.cpp | 30 +++---------------- 1 file changed, 4 insertions(+), 26 deletions(-) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index c5be966a..912f78c2 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -52,8 +52,6 @@ namespace kuka_rsi_hw_interface { CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_init()"); - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -113,8 +111,6 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw CallbackReturn KukaRSIHardwareInterface::on_configure( const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_configure()"); - // just in case - not 100% sure this is the right thing to do . . . for (size_t i = 0; i < hw_states_.size(); ++i) { hw_states_[i] = std::numeric_limits::quiet_NaN(); @@ -128,8 +124,6 @@ CallbackReturn KukaRSIHardwareInterface::on_configure( std::vector KukaRSIHardwareInterface::export_state_interfaces() { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "export_state_interfaces()"); - std::vector state_interfaces; for (size_t i = 0; i < info_.joints.size(); i++) { state_interfaces.emplace_back( @@ -144,8 +138,6 @@ std::vector KukaRSIHardwareInterface::export std::vector KukaRSIHardwareInterface:: export_command_interfaces() { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "export_command_interfaces()"); - std::vector command_interfaces; for (size_t i = 0; i < info_.joints.size(); i++) { command_interfaces.emplace_back( @@ -159,8 +151,6 @@ export_command_interfaces() CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_activate()"); - // Wait for connection from robot server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); @@ -168,18 +158,13 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta int bytes = server_->recv(in_buffer_); - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "got some bytes"); + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Got data from robot"); // Drop empty frame with RSI <= 2.3 if (bytes < 100) { bytes = server_->recv(in_buffer_); } - if (bytes < 100) { - RCLCPP_FATAL(rclcpp::get_logger("KukaRSIHardwareInterface"), "not enough data received"); - return CallbackReturn::ERROR; - } - rsi_state_ = RSIState(in_buffer_); for (size_t i = 0; i < n_dof_; ++i) { @@ -191,9 +176,9 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; server_->send(out_buffer_); - server_->set_timeout(1000); // Set receive timeout to 1 second + server_->set_timeout(1000); // Set receive timeout to 1 second - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System Sucessfully started!"); + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System Successfully started!"); is_active_ = true; return CallbackReturn::SUCCESS; @@ -202,12 +187,11 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta CallbackReturn KukaRSIHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "on_deactivate()"); out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; server_->send(out_buffer_); server_.reset(); is_active_ = false; - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System sucessfully stopped!"); + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System successfully stopped!"); return CallbackReturn::SUCCESS; } @@ -229,9 +213,6 @@ return_type KukaRSIHardwareInterface::read( for (std::size_t i = 0; i < n_dof_; ++i) { hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; - RCLCPP_INFO( - rclcpp::get_logger( - "KukaRSIHardwareInterface"), "Got state %.5f for joint %ld!", hw_states_[i], i); } ipoc_ = rsi_state_.ipoc; return return_type::OK; @@ -249,9 +230,6 @@ return_type KukaRSIHardwareInterface::write( } for (size_t i = 0; i < n_dof_; i++) { - RCLCPP_INFO( - rclcpp::get_logger( - "KukaRSIHardwareInterface"), "Got command %.5f for joint %ld!", hw_commands_[i], i); joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * KukaRSIHardwareInterface::R2D; } From 12e41f3b25b87ee66bf954786ec233feddff8006 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 17:41:46 +0200 Subject: [PATCH 15/97] lint, move launch file to launch folder --- kuka_rsi_hw_interface/CMakeLists.txt | 2 +- .../kuka_rsi_hw_interface/udp_server.h | 4 +- .../{test => launch}/startup.launch.py | 26 ++++----- .../test/test_hardware_interface.launch.py | 57 ------------------- kuka_rsi_hw_interface/test/test_params.yaml | 3 - .../test/test_params_sim.yaml | 3 - 6 files changed, 16 insertions(+), 79 deletions(-) rename kuka_rsi_hw_interface/{test => launch}/startup.launch.py (80%) delete mode 100644 kuka_rsi_hw_interface/test/test_hardware_interface.launch.py delete mode 100644 kuka_rsi_hw_interface/test/test_params.yaml delete mode 100644 kuka_rsi_hw_interface/test/test_params_sim.yaml diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 0f75cf9b..aea4d5b1 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -84,7 +84,7 @@ ament_export_dependencies( rclcpp_lifecycle ) -install(DIRECTORY config test +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 2dc4701c..6ebdb4e7 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -68,7 +68,7 @@ class UDPServer : local_host_(host), local_port_(port), timeout_( false) { - RCLCPP_INFO(rclcpp::get_logger("UDPServer"), "%s: %i", local_host_.c_str(), local_port_); + RCLCPP_INFO(rclcpp::get_logger("UDPServer"), "%s: %i", local_host_.c_str(), local_port_); sockfd_ = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd_ < 0) { throw std::runtime_error("Error opening socket: " + std::string(strerror(errno))); @@ -147,7 +147,7 @@ class UDPServer memset(buffer_, 0, BUFSIZE); bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); if (bytes < 0) { - RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); + RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } } diff --git a/kuka_rsi_hw_interface/test/startup.launch.py b/kuka_rsi_hw_interface/launch/startup.launch.py similarity index 80% rename from kuka_rsi_hw_interface/test/startup.launch.py rename to kuka_rsi_hw_interface/launch/startup.launch.py index 45d21459..3039faea 100644 --- a/kuka_rsi_hw_interface/test/startup.launch.py +++ b/kuka_rsi_hw_interface/launch/startup.launch.py @@ -37,12 +37,12 @@ def generate_launch_description(): executable='rsi_control_node', parameters=[robot_description, controller_config] ), - # Node( - # package='robot_state_publisher', - # executable='robot_state_publisher', - # output='both', - # parameters=[robot_description] - # ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[robot_description] + ), Node( package="controller_manager", executable="spawner", @@ -54,12 +54,12 @@ def generate_launch_description(): arguments=["joint_trajectory_controller", "-c", controller_manager_node, "-p", joint_traj_controller_config] ), - # Node( - # package="rviz2", - # executable="rviz2", - # name="rviz2", - # output="log", - # arguments=["-d", rviz_config_file], - # ) + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) ]) diff --git a/kuka_rsi_hw_interface/test/test_hardware_interface.launch.py b/kuka_rsi_hw_interface/test/test_hardware_interface.launch.py deleted file mode 100644 index ef4e154d..00000000 --- a/kuka_rsi_hw_interface/test/test_hardware_interface.launch.py +++ /dev/null @@ -1,57 +0,0 @@ -# from launch import LaunchDescription -# from launch.substitutions import EnvironmentVariable -# import os -# import launch_ros.actions -# import pathlib - -# test_params_sim_file_name = 'test_params_sim.yaml' -# test_params_file_name = 'test_params.yaml' -# hardware_controllers_file_name = 'hardware_controllers.yaml' -# controller_joint_names_file_name = 'controller_joint_names.yaml' - - -# def generate_launch_description(): -# test_params_sim_file_path = Path(get_package_share_directory('the_package'), 'config', test_params_sim_file_name) -# test_params_file_path = Path(get_package_share_directory('the_package'), 'config', test_params_file_name) - -# kuka_rsi_hw_interface_node = launch_ros.actions.Node( -# package='kuka_rsi_hw_interface', -# node_executable='kuka_hardware_interface', -# output='screen', -# parameters=[ -# test_params_sim_file_path -# ], -# ) - -# return LaunchDescription([ -# kuka_rsi_hw_interface_node, -# ]) - -# -# -# - -# -# - -# -# - -# -# -# -# - -# -# -# -# - -# -# - -# diff --git a/kuka_rsi_hw_interface/test/test_params.yaml b/kuka_rsi_hw_interface/test/test_params.yaml deleted file mode 100644 index 462b6158..00000000 --- a/kuka_rsi_hw_interface/test/test_params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -rsi: - listen_address: "192.168.1.67" - listen_port: 59152 diff --git a/kuka_rsi_hw_interface/test/test_params_sim.yaml b/kuka_rsi_hw_interface/test/test_params_sim.yaml deleted file mode 100644 index de2580a3..00000000 --- a/kuka_rsi_hw_interface/test/test_params_sim.yaml +++ /dev/null @@ -1,3 +0,0 @@ -rsi: - listen_address: "127.0.0.1" - listen_port: 59152 From cd099191ed4a8d3a6fd3aefd8c2e722ef4f937a6 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 1 Aug 2022 17:49:27 +0200 Subject: [PATCH 16/97] uncrustify --- .../robot_control/src/robot_control/joint_controller_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_sunrise_control/robot_control/src/robot_control/joint_controller_base.cpp b/kuka_sunrise_control/robot_control/src/robot_control/joint_controller_base.cpp index 20f7ad10..3ae8fea9 100644 --- a/kuka_sunrise_control/robot_control/src/robot_control/joint_controller_base.cpp +++ b/kuka_sunrise_control/robot_control/src/robot_control/joint_controller_base.cpp @@ -233,7 +233,7 @@ bool JointControllerBase::onVelocityFactorsChangeRequest( void JointControllerBase::updateMaxPositionDifference() { - auto calc_pos_diff = [ & loop_period_ms_ = loop_period_ms_](double v) { + auto calc_pos_diff = [&loop_period_ms_ = loop_period_ms_](double v) { return v * loop_period_ms_ / JointControllerBase::ms_in_sec_; }; std::transform( From eb11aeb4d9366dfd48282d6d5ec229a26d1d524e Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 5 Aug 2022 10:01:57 +0200 Subject: [PATCH 17/97] initialize with 0-s --- kuka_rsi_hw_interface/launch/startup.launch.py | 2 +- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/kuka_rsi_hw_interface/launch/startup.launch.py b/kuka_rsi_hw_interface/launch/startup.launch.py index 3039faea..ee8a3101 100644 --- a/kuka_rsi_hw_interface/launch/startup.launch.py +++ b/kuka_rsi_hw_interface/launch/startup.launch.py @@ -59,7 +59,7 @@ def generate_launch_description(): executable="rviz2", name="rviz2", output="log", - arguments=["-d", rviz_config_file], + arguments=["-d", rviz_config_file, "--ros-args", "--log-level", "error"], ) ]) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 912f78c2..f1397385 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -113,8 +113,8 @@ CallbackReturn KukaRSIHardwareInterface::on_configure( { // just in case - not 100% sure this is the right thing to do . . . for (size_t i = 0; i < hw_states_.size(); ++i) { - hw_states_[i] = std::numeric_limits::quiet_NaN(); - hw_commands_[i] = std::numeric_limits::quiet_NaN(); + hw_states_[i] = 0; + hw_commands_[i] = 0; initial_joint_pos_[i] = 0.0; joint_pos_correction_deg_[i] = 0.0; } From e1931b65b2722ae28d26b85f3e62570c8201f5dd Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 5 Aug 2022 10:05:22 +0200 Subject: [PATCH 18/97] lint cmake --- kuka_sunrise_driver/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/kuka_sunrise_driver/CMakeLists.txt b/kuka_sunrise_driver/CMakeLists.txt index 8d934390..426cb272 100644 --- a/kuka_sunrise_driver/CMakeLists.txt +++ b/kuka_sunrise_driver/CMakeLists.txt @@ -101,7 +101,8 @@ add_executable(lifecycle_client_node ament_target_dependencies(lifecycle_client_node rclcpp rclcpp_lifecycle std_msgs) -install(TARGETS robot_manager robot_control_client robot_manager_node robot_control_node position_controller_node torque_controller_node lifecycle_client_node +install(TARGETS robot_manager robot_control_client robot_manager_node robot_control_node position_controller_node torque_controller_node + lifecycle_client_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch From eb19f2f972b8f1a17b528f59f33ea5901446d2ec Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 5 Aug 2022 11:21:49 +0200 Subject: [PATCH 19/97] fix humble exclude --- kuka_sunrise_driver/CMakeLists.txt | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/kuka_sunrise_driver/CMakeLists.txt b/kuka_sunrise_driver/CMakeLists.txt index 426cb272..6bf806f8 100644 --- a/kuka_sunrise_driver/CMakeLists.txt +++ b/kuka_sunrise_driver/CMakeLists.txt @@ -142,27 +142,13 @@ if(BUILD_TESTING) \"--single-version-externally-managed\" WORKING_DIRECTORY \"${PROJECT_SOURCE_DIR}\")" ) - file(GLOB fri_headers - LIST_DIRECTORIES FALSE - RELATIVE "${PROJECT_SOURCE_DIR}/include/fri_client" - include/fri_client/*) - - ament_copyright(--exclude ${fri_headers}) -# friClientApplication.h -# friClientIf.h -# friConnectionIf.h -# friException.h -# friLBRClient.h -# friLBRCommand.h -# friLBRState.h -# friTransformationClient.h -# friUdpConnection.h) + ament_copyright(--exclude include/fri_client/*) ament_cppcheck(--language=c++) ament_pep257() ament_flake8() ament_cpplint() ament_lint_cmake() - ament_uncrustify(--exclude ${fri_headers}) + ament_uncrustify(--exclude include/fri_client/*) ament_xmllint() endif() From 7e86221869a4c702e5c0c003bcfd9f9c42a51d22 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 5 Aug 2022 12:40:10 +0200 Subject: [PATCH 20/97] fix sonar code smells --- .../kuka_hardware_interface.hpp | 15 ++++++++----- .../src/kuka_hardware_interface.cpp | 22 ++++++------------- 2 files changed, 16 insertions(+), 21 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 4b7a20f3..9a93b335 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -59,6 +59,11 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "pluginlib/class_list_macros.hpp" using hardware_interface::return_type; @@ -86,11 +91,9 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface KUKA_RSI_HW_INTERFACE_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; -// return_type start() override; KUKA_RSI_HW_INTERFACE_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; -// return_type stop() override; KUKA_RSI_HW_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -109,8 +112,6 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface std::vector initial_joint_pos_ = std::vector(n_dof_, 0.0); std::vector joint_pos_correction_deg_ = std::vector(n_dof_, 0.0); -// Dummy parameters - double hw_start_sec_, hw_stop_sec_, hw_slowdown_; // Store the command for the simulated robot std::vector hw_commands_, hw_states_; @@ -123,8 +124,10 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface std::mutex m_; double loop_hz_; - std::chrono::steady_clock::time_point time_now_, time_last_; - std::chrono::duration control_period_, elapsed_time_; + std::chrono::steady_clock::time_point time_now_; + std::chrono::steady_clock::time_point time_last_; + std::chrono::duration control_period_; + std::chrono::duration elapsed_time_; static constexpr double R2D = 180 / M_PI; static constexpr double D2R = M_PI / 180; diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index f1397385..8edb0210 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -44,9 +44,6 @@ #include #include "kuka_rsi_hw_interface/kuka_hardware_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/state.hpp" namespace kuka_rsi_hw_interface { @@ -94,7 +91,7 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw out_buffer_.resize(1024); initial_joint_pos_.resize(n_dof_, 0.0); - joint_pos_correction_deg_.resize(n_dof_, 0.0), + joint_pos_correction_deg_.resize(n_dof_, 0.0); ipoc_ = 0; rsi_ip_address_ = info_.hardware_parameters["rsi_ip_address"]; @@ -111,7 +108,6 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw CallbackReturn KukaRSIHardwareInterface::on_configure( const rclcpp_lifecycle::State &) { - // just in case - not 100% sure this is the right thing to do . . . for (size_t i = 0; i < hw_states_.size(); ++i) { hw_states_[i] = 0; hw_commands_[i] = 0; @@ -127,10 +123,9 @@ std::vector KukaRSIHardwareInterface::export std::vector state_interfaces; for (size_t i = 0; i < info_.joints.size(); i++) { state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[i].name, - hardware_interface::HW_IF_POSITION, - &hw_states_[i])); + info_.joints[i].name, + hardware_interface::HW_IF_POSITION, + &hw_states_[i]); } return state_interfaces; } @@ -141,10 +136,9 @@ export_command_interfaces() std::vector command_interfaces; for (size_t i = 0; i < info_.joints.size(); i++) { command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.joints[i].name, - hardware_interface::HW_IF_POSITION, - &hw_commands_[i])); + info_.joints[i].name, + hardware_interface::HW_IF_POSITION, + &hw_commands_[i]); } return command_interfaces; } @@ -246,8 +240,6 @@ bool KukaRSIHardwareInterface::isActive() const } // namespace namespace kuka_rsi_hw_interface -#include "pluginlib/class_list_macros.hpp" - PLUGINLIB_EXPORT_CLASS( kuka_rsi_hw_interface::KukaRSIHardwareInterface, hardware_interface::SystemInterface From 43a27007025a2d928c1c71550bf7c1508be74ca5 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 5 Aug 2022 12:49:28 +0200 Subject: [PATCH 21/97] clean up unnecessary class members --- .../kuka_hardware_interface.hpp | 17 +++++------------ .../src/kuka_hardware_interface.cpp | 14 +++++++------- 2 files changed, 12 insertions(+), 19 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 9a93b335..c9e1492e 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -105,15 +105,15 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface private: bool is_active_ = false; - const uint8_t n_dof_ = 6; std::string rsi_ip_address_ = ""; int rsi_port_ = 0; - std::vector initial_joint_pos_ = std::vector(n_dof_, 0.0); - std::vector joint_pos_correction_deg_ = std::vector(n_dof_, 0.0); + std::vector hw_commands_; + std::vector hw_states_; -// Store the command for the simulated robot - std::vector hw_commands_, hw_states_; + // RSI related joint positions + std::vector initial_joint_pos_; + std::vector joint_pos_correction_deg_; uint64_t ipoc_ = 0; RSIState rsi_state_; @@ -121,13 +121,6 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface std::unique_ptr server_; std::string in_buffer_; std::string out_buffer_; - std::mutex m_; - - double loop_hz_; - std::chrono::steady_clock::time_point time_now_; - std::chrono::steady_clock::time_point time_last_; - std::chrono::duration control_period_; - std::chrono::duration elapsed_time_; static constexpr double R2D = 180 / M_PI; static constexpr double D2R = M_PI / 180; diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 8edb0210..75b8b68a 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -53,8 +53,8 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw return CallbackReturn::ERROR; } - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_states_.resize(info_.joints.size()); + hw_commands_.resize(info_.joints.size()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { if (joint.command_interfaces.size() != 1) { @@ -90,8 +90,8 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw in_buffer_.resize(1024); // udp_server.h --> #define BUFSIZE 1024 out_buffer_.resize(1024); - initial_joint_pos_.resize(n_dof_, 0.0); - joint_pos_correction_deg_.resize(n_dof_, 0.0); + initial_joint_pos_.resize(info_.joints.size(), 0.0); + joint_pos_correction_deg_.resize(info_.joints.size(), 0.0); ipoc_ = 0; rsi_ip_address_ = info_.hardware_parameters["rsi_ip_address"]; @@ -161,7 +161,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta rsi_state_ = RSIState(in_buffer_); - for (size_t i = 0; i < n_dof_; ++i) { + for (size_t i = 0; i < info_.joints.size(); ++i) { hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; hw_commands_[i] = hw_states_[i]; initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; @@ -205,7 +205,7 @@ return_type KukaRSIHardwareInterface::read( } rsi_state_ = RSIState(in_buffer_); - for (std::size_t i = 0; i < n_dof_; ++i) { + for (std::size_t i = 0; i < info_.joints.size(); ++i) { hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; } ipoc_ = rsi_state_.ipoc; @@ -223,7 +223,7 @@ return_type KukaRSIHardwareInterface::write( return return_type::OK; } - for (size_t i = 0; i < n_dof_; i++) { + for (size_t i = 0; i < info_.joints.size(); i++) { joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * KukaRSIHardwareInterface::R2D; } From 849e7236956945289618064bb65acbe5d26a8b64 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 5 Aug 2022 13:05:20 +0200 Subject: [PATCH 22/97] move configuration to init --- .../kuka_hardware_interface.hpp | 3 --- .../src/kuka_hardware_interface.cpp | 26 +++---------------- 2 files changed, 4 insertions(+), 25 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index c9e1492e..785d27e3 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -80,9 +80,6 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface KUKA_RSI_HW_INTERFACE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - KUKA_RSI_HW_INTERFACE_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - KUKA_RSI_HW_INTERFACE_PUBLIC std::vector export_state_interfaces() override; diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 75b8b68a..2093a105 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -53,8 +53,8 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw return CallbackReturn::ERROR; } - hw_states_.resize(info_.joints.size()); - hw_commands_.resize(info_.joints.size()); + hw_states_.resize(info_.joints.size(), 0.0); + hw_commands_.resize(info_.joints.size(), 0.0); for (const hardware_interface::ComponentInfo & joint : info_.joints) { if (joint.command_interfaces.size() != 1) { @@ -104,20 +104,6 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw return CallbackReturn::SUCCESS; } - -CallbackReturn KukaRSIHardwareInterface::on_configure( - const rclcpp_lifecycle::State &) -{ - for (size_t i = 0; i < hw_states_.size(); ++i) { - hw_states_[i] = 0; - hw_commands_[i] = 0; - initial_joint_pos_[i] = 0.0; - joint_pos_correction_deg_[i] = 0.0; - } - return CallbackReturn::SUCCESS; -} - - std::vector KukaRSIHardwareInterface::export_state_interfaces() { std::vector state_interfaces; @@ -194,9 +180,7 @@ return_type KukaRSIHardwareInterface::read( const rclcpp::Duration &) { if (!is_active_) { - RCLCPP_DEBUG( - rclcpp::get_logger( - "KukaRSIHardwareInterface"), "Hardware interface is not active"); + RCLCPP_DEBUG(rclcpp::get_logger("KukaRSIHardwareInterface"), "Hardware interface not active"); return return_type::OK; } @@ -217,9 +201,7 @@ return_type KukaRSIHardwareInterface::write( const rclcpp::Duration &) { if (!is_active_) { - RCLCPP_DEBUG( - rclcpp::get_logger("KukaRSIHardwareInterface"), - "Hardware interface is not active"); + RCLCPP_DEBUG(rclcpp::get_logger("KukaRSIHardwareInterface"), "Hardware interface not active"); return return_type::OK; } From be2e70bd65820062818cc51bc93d52ba9dfb8a78 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 5 Aug 2022 13:06:06 +0200 Subject: [PATCH 23/97] uncrustify --- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 2093a105..5f793d40 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -180,7 +180,7 @@ return_type KukaRSIHardwareInterface::read( const rclcpp::Duration &) { if (!is_active_) { - RCLCPP_DEBUG(rclcpp::get_logger("KukaRSIHardwareInterface"), "Hardware interface not active"); + RCLCPP_DEBUG(rclcpp::get_logger("KukaRSIHardwareInterface"), "Hardware interface not active"); return return_type::OK; } @@ -201,7 +201,7 @@ return_type KukaRSIHardwareInterface::write( const rclcpp::Duration &) { if (!is_active_) { - RCLCPP_DEBUG(rclcpp::get_logger("KukaRSIHardwareInterface"), "Hardware interface not active"); + RCLCPP_DEBUG(rclcpp::get_logger("KukaRSIHardwareInterface"), "Hardware interface not active"); return return_type::OK; } From 7622fa92f2e4a07f9e09f07e354e7789a854c438 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 8 Aug 2022 13:40:00 +0200 Subject: [PATCH 24/97] disable writing to hwif before successful read --- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 5f793d40..9d01896d 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -168,6 +168,7 @@ CallbackReturn KukaRSIHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; + ipoc_ = 0; // necessary to disable write-first scenarios after reactivation server_->send(out_buffer_); server_.reset(); is_active_ = false; @@ -185,6 +186,8 @@ return_type KukaRSIHardwareInterface::read( } if (server_->recv(in_buffer_) == 0) { + RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "No data received from robot"); + this->on_deactivate(this->get_state()); return return_type::ERROR; } rsi_state_ = RSIState(in_buffer_); @@ -200,7 +203,9 @@ return_type KukaRSIHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration &) { - if (!is_active_) { + // It is possible, that write is called immediately after activation + // In this case write in that tick should be skipped to be able to read state at first + if (!is_active_ || ipoc_ == 0) { RCLCPP_DEBUG(rclcpp::get_logger("KukaRSIHardwareInterface"), "Hardware interface not active"); return return_type::OK; } From 9885c3f3425f3f6b5578fd4fe0edd64dd4f370a7 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 8 Aug 2022 14:04:52 +0200 Subject: [PATCH 25/97] uncrustify --- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 9d01896d..d2fb9560 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -186,7 +186,7 @@ return_type KukaRSIHardwareInterface::read( } if (server_->recv(in_buffer_) == 0) { - RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "No data received from robot"); + RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "No data received from robot"); this->on_deactivate(this->get_state()); return return_type::ERROR; } From ec09377061bf341efd3368cca5739321f51e9924 Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Thu, 12 Jan 2023 12:31:49 +0100 Subject: [PATCH 26/97] fix merge --- kuka_sunrise_driver/CMakeLists.txt | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/kuka_sunrise_driver/CMakeLists.txt b/kuka_sunrise_driver/CMakeLists.txt index 9e56a2bd..462683f6 100644 --- a/kuka_sunrise_driver/CMakeLists.txt +++ b/kuka_sunrise_driver/CMakeLists.txt @@ -138,21 +138,8 @@ if(BUILD_TESTING) \"--single-version-externally-managed\" WORKING_DIRECTORY \"${PROJECT_SOURCE_DIR}\")" ) - file(GLOB fri_headers - LIST_DIRECTORIES FALSE - RELATIVE "${PROJECT_SOURCE_DIR}/include/fri_client" - include/fri_client/*) - - ament_copyright(--exclude ${fri_headers}) -# friClientApplication.h -# friClientIf.h -# friConnectionIf.h -# friException.h -# friLBRClient.h -# friLBRCommand.h -# friLBRState.h -# friTransformationClient.h -# friUdpConnection.h) + + ament_copyright(--exclude include/fri_client/*) ament_cppcheck(--language=c++) ament_pep257() ament_flake8() From 0bd8a73e8f94d375cee6fa71e7da40b1ea9c34ba Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 19 Jun 2023 09:22:14 +0200 Subject: [PATCH 27/97] add manager node --- kuka_rsi_hw_interface/CMakeLists.txt | 7 +- .../joint_trajectory_controller_config.yaml | 2 +- .../config/ros2_controller_config.yaml | 2 +- .../robot_manager_node.hpp | 69 +++++++ .../launch/startup.launch.py | 9 +- .../src/robot_manager_node.cpp | 191 ++++++++++++++++++ 6 files changed, 275 insertions(+), 5 deletions(-) create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_manager_node.hpp create mode 100644 kuka_rsi_hw_interface/src/robot_manager_node.cpp diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index aea4d5b1..fae08f51 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -46,9 +46,14 @@ add_executable(rsi_control_node src/rsi_control_node.cpp) ament_target_dependencies(rsi_control_node rclcpp rclcpp_lifecycle controller_manager) +add_executable(robot_manager_node + src/robot_manager_node.cpp) +ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager controller_manager_msgs) +target_link_libraries(robot_manager_node kroshu_ros2_core::communication_helpers) + pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml) -install(TARGETS ${PROJECT_NAME} rsi_control_node +install(TARGETS ${PROJECT_NAME} rsi_control_node robot_manager_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml b/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml index fcb94336..3de96e7b 100644 --- a/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml +++ b/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml @@ -1,4 +1,4 @@ -/joint_trajectory_controller: +joint_trajectory_controller: ros__parameters: joints: - joint_a1 diff --git a/kuka_rsi_hw_interface/config/ros2_controller_config.yaml b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml index 074a01ac..2eabba85 100644 --- a/kuka_rsi_hw_interface/config/ros2_controller_config.yaml +++ b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml @@ -8,4 +8,4 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - configure_components_on_start: ["KukaRSIHardwareInterface"] + configure_components_on_start: [""] diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_manager_node.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_manager_node.hpp new file mode 100644 index 00000000..9942b4b5 --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_manager_node.hpp @@ -0,0 +1,69 @@ +// Copyright 2023 Svastits Áron +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef KUKA_RSI_HW_INTERFACE__ROBOT_MANAGER_NODE_HPP_ +#define KUKA_RSI_HW_INTERFACE__ROBOT_MANAGER_NODE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/client.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "controller_manager_msgs/srv/set_hardware_component_state.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "communication_helpers/service_tools.hpp" +#include "kroshu_ros2_core/ROS2BaseLCNode.hpp" + + +namespace kuka_rsi +{ +class RobotManagerNode : public kroshu_ros2_core::ROS2BaseLCNode +{ +public: + RobotManagerNode(); + ~RobotManagerNode(); + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) override; + +private: + rclcpp::Client::SharedPtr + change_hardware_state_client_; + rclcpp::Client::SharedPtr + change_controller_state_client_; + rclcpp::CallbackGroup::SharedPtr cbg_; + + std::shared_ptr> is_configured_pub_; + std_msgs::msg::Bool is_configured_msg_; +}; + +} // namespace kuka_rox + + +#endif // KUKA_RSI_HW_INTERFACE__ROBOT_MANAGER_NODE_HPP_ diff --git a/kuka_rsi_hw_interface/launch/startup.launch.py b/kuka_rsi_hw_interface/launch/startup.launch.py index ee8a3101..4d7d3913 100644 --- a/kuka_rsi_hw_interface/launch/startup.launch.py +++ b/kuka_rsi_hw_interface/launch/startup.launch.py @@ -37,6 +37,11 @@ def generate_launch_description(): executable='rsi_control_node', parameters=[robot_description, controller_config] ), + Node( + package='kuka_rsi_hw_interface', + namespace='', + executable='robot_manager_node' + ), Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -46,13 +51,13 @@ def generate_launch_description(): Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "-c", controller_manager_node], + arguments=["joint_state_broadcaster", "-c", controller_manager_node, "--inactive"], ), Node( package="controller_manager", executable="spawner", arguments=["joint_trajectory_controller", "-c", controller_manager_node, "-p", - joint_traj_controller_config] + joint_traj_controller_config, "--inactive"] ), Node( package="rviz2", diff --git a/kuka_rsi_hw_interface/src/robot_manager_node.cpp b/kuka_rsi_hw_interface/src/robot_manager_node.cpp new file mode 100644 index 00000000..3869d7a5 --- /dev/null +++ b/kuka_rsi_hw_interface/src/robot_manager_node.cpp @@ -0,0 +1,191 @@ +// Copyright 2023 Svastits Áron +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "kuka_rsi_hw_interface/robot_manager_node.hpp" +#include "kroshu_ros2_core/ControlMode.hpp" + + +using namespace controller_manager_msgs::srv; // NOLINT +using namespace lifecycle_msgs::msg; // NOLINT + +namespace kuka_rsi +{ +RobotManagerNode::RobotManagerNode() +: kroshu_ros2_core::ROS2BaseLCNode("robot_manager") +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + qos.reliable(); + cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + change_hardware_state_client_ = + this->create_client( + "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_ + ); + change_controller_state_client_ = + this->create_client( + "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_ + ); + + auto is_configured_qos = rclcpp::QoS(rclcpp::KeepLast(1)); + is_configured_qos.best_effort(); + + is_configured_pub_ = this->create_publisher( + "robot_manager/is_configured", + is_configured_qos); +} +RobotManagerNode::~RobotManagerNode() +{ + +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) +{ + // Configure hardware interface + auto hw_request = + std::make_shared(); + hw_request->name = "KukaRSIHardwareInterface"; + hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE; + auto hw_response = + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 2000); + if (!hw_response || !hw_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); + return FAILURE; + } + RCLCPP_INFO(get_logger(), "Successfully configured hardware interface"); + + is_configured_pub_->on_activate(); + is_configured_msg_.data = true; + is_configured_pub_->publish(is_configured_msg_); + return SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) +{ + // Clean up hardware interface + auto hw_request = + std::make_shared(); + hw_request->name = "KukaRSIHardwareInterface"; + hw_request->target_state.id = State::PRIMARY_STATE_UNCONFIGURED; + auto hw_response = + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 2000); + if (!hw_response || !hw_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface"); + return FAILURE; + } + + if (is_configured_pub_->is_activated()) { + is_configured_msg_.data = false; + is_configured_pub_->publish(is_configured_msg_); + is_configured_pub_->on_deactivate(); + } + // TODO(Svastits): add else branch, and throw exception(?) + return SUCCESS; +} + +// TODO(Svastits): rollback in case of failures +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) +{ + // Activate hardware interface + auto hw_request = + std::make_shared(); + hw_request->name = "KukaRSIHardwareInterface"; + hw_request->target_state.id = State::PRIMARY_STATE_ACTIVE; + auto hw_response = + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 2000); + if (!hw_response || !hw_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); + return FAILURE; + } + + + // Activate RT controller(s) + auto controller_request = + std::make_shared(); + controller_request->strictness = SwitchController::Request::STRICT; + controller_request->activate_controllers = + {"joint_state_broadcaster", "joint_trajectory_controller"}; + + auto controller_response = + kroshu_ros2_core::sendRequest( + change_controller_state_client_, controller_request, 0, 2000 + ); + if (!controller_response || !controller_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not activate controller"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + return FAILURE; + } + + RCLCPP_INFO(get_logger(), "Successfully activated controllers"); + return SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) +{ + // Deactivate hardware interface + auto hw_request = + std::make_shared(); + hw_request->name = "KukaRSIHardwareInterface"; + hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE; + auto hw_response = + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 3000); // was not stable with 2000 ms timeout + if (!hw_response || !hw_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); + return ERROR; + } + + RCLCPP_INFO(get_logger(), "Deactivated hardware interface"); + + + // Stop RT controllers + auto controller_request = + std::make_shared(); + // With best effort strictness, deactivation succeeds if specific controller is not active + controller_request->strictness = + SwitchController::Request::BEST_EFFORT; + controller_request->deactivate_controllers = + {"joint_state_broadcaster", "joint_trajectory_controller"}; + auto controller_response = + kroshu_ros2_core::sendRequest( + change_controller_state_client_, controller_request, 0, 2000 + ); + if (!controller_response || !controller_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not stop controllers"); + return ERROR; + } + + RCLCPP_INFO(get_logger(), "Successfully stopped controllers"); + return SUCCESS; +} +} // namespace kuka_rsi + +int main(int argc, char * argv[]) +{ + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + return 0; +} From d5f69162c1498d9edaf236bb25526a8f2f099fff Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 19 Jun 2023 10:34:11 +0200 Subject: [PATCH 28/97] fix deactivation --- .../kuka_hardware_interface.hpp | 1 + .../include/kuka_rsi_hw_interface/rsi_command.h | 2 +- .../src/kuka_hardware_interface.cpp | 15 +++++++-------- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 785d27e3..98373487 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -101,6 +101,7 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface bool isActive() const; private: + bool stop_flag_ = false; bool is_active_ = false; std::string rsi_ip_address_ = ""; int rsi_port_ = 0; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h index b4d19e3d..208ea66f 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h @@ -64,7 +64,7 @@ class RSICommand el->SetAttribute("A4", std::to_string(joint_position_correction[3])); el->SetAttribute("A5", std::to_string(joint_position_correction[4])); el->SetAttribute("A6", std::to_string(joint_position_correction[5])); - if (!stop) {root->LinkEndChild(el);} + root->LinkEndChild(el); el = new TiXmlElement("Stop"); el->LinkEndChild(new TiXmlText(std::to_string(static_cast(stop)))); diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index d2fb9560..78963925 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -131,6 +131,7 @@ export_command_interfaces() CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { + stop_flag_ = false; // Wait for connection from robot server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); @@ -154,7 +155,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta } ipoc_ = rsi_state_.ipoc; - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); server_->set_timeout(1000); // Set receive timeout to 1 second @@ -167,12 +168,8 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta CallbackReturn KukaRSIHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; - ipoc_ = 0; // necessary to disable write-first scenarios after reactivation - server_->send(out_buffer_); - server_.reset(); - is_active_ = false; - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System successfully stopped!"); + stop_flag_ = true; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Stop flag was set!"); return CallbackReturn::SUCCESS; } @@ -210,12 +207,14 @@ return_type KukaRSIHardwareInterface::write( return return_type::OK; } + if (stop_flag_) is_active_ = false; + for (size_t i = 0; i < info_.joints.size(); i++) { joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * KukaRSIHardwareInterface::R2D; } - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); return return_type::OK; } From cc634a4f6861e7c45013b2c18c754ff090e74407 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Thu, 22 Jun 2023 14:19:11 +0200 Subject: [PATCH 29/97] update rsi context files --- kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi | 112 - .../krl/KR_C4/ros_rsi.rsi.diagram | 198 -- .../krl/KR_C4/ros_rsi.rsi.xml | 51 - .../krl/{KR_C4 => }/README_KRC4.md | 0 .../krl/{KR_C4 => }/README_KRC5.md | 0 kuka_rsi_hw_interface/krl/ros_rsi.rsix | 2437 +++++++++++++++++ .../krl/{KR_C4 => }/ros_rsi.src | 0 .../krl/{KR_C4 => }/ros_rsi_ethernet.xml | 7 +- 8 files changed, 2441 insertions(+), 364 deletions(-) delete mode 100644 kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi delete mode 100644 kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram delete mode 100644 kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml rename kuka_rsi_hw_interface/krl/{KR_C4 => }/README_KRC4.md (100%) rename kuka_rsi_hw_interface/krl/{KR_C4 => }/README_KRC5.md (100%) create mode 100755 kuka_rsi_hw_interface/krl/ros_rsi.rsix rename kuka_rsi_hw_interface/krl/{KR_C4 => }/ros_rsi.src (100%) rename kuka_rsi_hw_interface/krl/{KR_C4 => }/ros_rsi_ethernet.xml (88%) mode change 100644 => 100755 diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi deleted file mode 100644 index 25bcf984..00000000 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi +++ /dev/null @@ -1,112 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram deleted file mode 100644 index 6386e420..00000000 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram +++ /dev/null @@ -1,198 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml deleted file mode 100644 index 30f5715c..00000000 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/KR_C4/README_KRC4.md b/kuka_rsi_hw_interface/krl/README_KRC4.md similarity index 100% rename from kuka_rsi_hw_interface/krl/KR_C4/README_KRC4.md rename to kuka_rsi_hw_interface/krl/README_KRC4.md diff --git a/kuka_rsi_hw_interface/krl/KR_C4/README_KRC5.md b/kuka_rsi_hw_interface/krl/README_KRC5.md similarity index 100% rename from kuka_rsi_hw_interface/krl/KR_C4/README_KRC5.md rename to kuka_rsi_hw_interface/krl/README_KRC5.md diff --git a/kuka_rsi_hw_interface/krl/ros_rsi.rsix b/kuka_rsi_hw_interface/krl/ros_rsi.rsix new file mode 100755 index 00000000..4e95c079 --- /dev/null +++ b/kuka_rsi_hw_interface/krl/ros_rsi.rsix @@ -0,0 +1,2437 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Tooltip_COMMENT + + + + + + Tooltip_FunctionBlockInput + + + + + + Tooltip_FunctionBlockInOrder + + + + + Tooltip_FunctionBlockOutput + + + + + + Tooltip_FunctionBlockOutOrder + + + + + + + Tooltip_ANIN + + + Tooltip_ANIN_Out1 + + + + + Tooltip_ANIN_Index + + + + + Tooltip_DIGIN + + + + + + + Remarks_DIGIN_Index + Tooltip_DIGIN_Index + + + + Tooltip_DIGIN_DataType + + Bit + Byte + UByte + Int16 + UInt16 + Int32 + UInt32 + Float + + + + + + Tooltip_ANOUT + + + Tooltip_ANOUT_Out1 + + + + + Tooltip_ANOUT_Index + + + + + Tooltip_DIGOUT + + + + + + + Remarks_DIGOUT_Index + Tooltip_DIGOUT_Index + + + + Tooltip_DIGOUT_DataType + + Bit + Byte + UByte + Int16 + UInt16 + Int32 + UInt32 + Float + + + + + + + Tooltip_ETHERNET + Summary_ETHERNET + + + + + + + + + + Tooltip_ETHERNET_ConfigFile + + + Tooltip_ETHERNET_Timeout + + + Tooltip_ETHERNET_Flag + + + + Remarks_ETHERNET_Precision + Tooltip_ETHERNET_Precision + + + + + + Tooltip_INPUT + + + Tooltip_INPUT_Port + + + + + Tooltip_INPUT_Index + + + Tooltip_INPUT_DataType + + Bit + Byte + UByte + Int16 + UInt16 + Int32 + UInt32 + Float + + + + + + Tooltip_MAP2ANOUT + + + Tooltip_MAP2ANOUT_In1 + + + + + Tooltip_MAP2ANOUT_Index + + + + + Tooltip_MAP2DIGOUT + + + + + + + Remarks_MAP2DIGOUT_Index + Tooltip_MAP2DIGOUT_Index + + + + Tooltip_MAP2DIGOUT_DataType + + Bit + Byte + Int16 + + + + + + Tooltip_MAP2SEN_PINT + + + Tooltip_MAP2SEN_PINT_In1 + + + + + + Remark_SenPint_SenPrea_Index + Tooltip_MAP2SEN_PINT_Index + + + + + + Tooltip_MAP2SEN_PREA + + + Tooltip_MAP2SEN_PREA_In1 + + + + + + Remark_SenPint_SenPrea_Index + Tooltip_MAP2SEN_PREA_Index + + + + + + Tooltip_MONITOR + + + + + + + Tooltip_MONITOR_Refresh + + + Tooltip_MONITOR_Timeout + + + Tooltip_MONITOR_ReqTimeZero + + + Tooltip_MONITOR_IP + + + + Remarks_MONITOR_Channel + Tooltip_MONITOR_Channel + + + + + + Tooltip_OUTPUT + + + Tooltip_OUTPUT_Port + + + + + Tooltip_OUTPUT_Index + + + Tooltip_OUTPUT_DataType + + Bit + Byte + UByte + Int16 + UInt16 + Int32 + UInt32 + Float + + + + + + Tooltip_RESETDIGOUT + + + + + + Tooltip_RESETDIGOUT_Index + + + + + Tooltip_SEN_PINT + + + Tooltip_SEN_PINT_Out1 + + + + + + Remark_SenPint_SenPrea_Index + Tooltip_SEN_PINT_Index + + + + + + Tooltip_SEN_PREA + + + Tooltip_SEN_PREA_Out1 + + + + + + Remark_SenPint_SenPrea_Index + Tooltip_SEN_PREA_Index + + + + + + Tooltip_SETDIGOUT + + + + + + Tooltip_SETDIGOUT_Index + + + + + + + + Tooltip_AND + Summary_AND + + + + + + + + + Tooltip_AND_Out1 + + + + + Tooltip_BAND + + + + + + + + + + Tooltip_BAND_DefVal + + + + + Tooltip_BCOMPL + + + + + + + + + Tooltip_BOR + + + + + + + + + + Tooltip_BOR_DefVal + + + + + Tooltip_NOT + + + + + + + + + Tooltip_OR + + + + + + + + + + + Tooltip_SIGNALSWITCH + + + Tooltip_SIGNALSWITCH_Ctrl + + + + + + + + + + + + Remarks_XOR + Tooltip_XOR + + + + + + + + + + + + + Remarks_RS + Tooltip_RS + + + + Tooltip_RS_Set + + + Tooltip_RS_Reset + + + + + + Remarks_RS_Out1 + Tooltip_RS_Out1 + rs_remarks.png + + + + + + + Remarks_SR + Tooltip_SR + + + + Tooltip_SR_Set + + + Tooltip_SR_Reset + + + + + + Remarks_SR_Out1 + Tooltip_SR_Out1 + sr_remarks.png + + + + + + + + Tooltip_ABS + + + Tooltip_ABS_In1 + + + + + Tooltip_ABS_Out1 + + + + + Tooltip_ACOS + + + Tooltip_ACOS_In1 + + + + + Tooltip_ACOS_Out1 + + + + + Tooltip_ASIN + + + Tooltip_ASIN_In1 + + + + + Tooltip_ASIN_Out1 + + + + + Tooltip_ATAN + + + + + + Tooltip_ATAN_Out1 + + + + + Tooltip_ATAN2 + + + + + + + Tooltip_ATAN2_Out1 + + + + + Tooltip_CEIL + + + + + + + + + Tooltip_COS + + + Tooltip_COS_In1 + + + + + Tooltip_COS_Out1 + + + + + Tooltip_Division + + + Tooltip_Division_Dividend + + + Tooltip_Division_Divisor + + + + + Tooltip_Division_Quotient + + + + + Tooltip_EQUAL + + + + + + + + + + Tooltip_EQUAL_CompVal + + + Tooltip_EQUAL_Tolerance + + + + + Tooltip_EXP + + + Tooltip_EXP_In1 + + + + + Tooltip_EXP_Out1 + + + + + Tooltip_FLOOR + + + + + + + + + Tooltip_GREATER + + + + + + + Tooltip_GREATER_Out1 + + + + + Tooltip_GREATER_CompVal + + + + Remarks_GREATER_Hysteresis + Tooltip_GREATER_Hysteresis + + + + + + Tooltip_LESS + + + + + + + + + + Tooltip_LESS_CompVal + + + + Remarks_LESS_Hysteresis + Tooltip_LESS_Hysteresis + + + + + + Tooltip_LIMIT + + + + + + + + + Tooltip_LIMIT_LowerLimit + + + Tooltip_LIMIT_UpperLimit + + + + + Tooltip_LOG + + + + + + Tooltip_LOG_Out1 + + + + + Tooltip_MINMAX + + + + + + + + Tooltip_MINMAX_Min + + + Tooltip_MINMAX_Max + + + + + Tooltip_MULTI + + + + + + + Tooltip_MULTI_Out1 + + + + + Tooltip_NORM + + + + + + + + + Remarks_NORM_Out1 + Tooltip_NORM_Out1 + + + + + + Tooltip_POW + + + Tooltip_POW_In1 + + + + + Tooltip_POW_Out1 + + + + + Tooltip_POW_Exp + + + + + Tooltip_ROUND + + + + + + Tooltip_ROUND_Out1 + + + + + Tooltip_ROUND_nDig + + + + + Tooltip_SIN + + + Tooltip_SIN_In1 + + + + + Tooltip_SIN_Out1 + + + + + Tooltip_SUB + + + Tooltip_SUB_Minu + + + Tooltip_SUB_Sub1 + + + + + Tooltip_SUB_Diff + + + + + Tooltip_SUB_Value + + + + + Tooltip_SUM + + + + + + + Tooltip_SUM_Sum + + + + + Tooltip_SUM_Value + + + + + Tooltip_TAN + + + Tooltip_TAN_In1 + + + + + Tooltip_TAN_Out1 + + + + + + Remarks_TRAFO_ROBFRAME + Tooltip_TRAFO_ROBFRAME + + + + Tooltip_TRAFO_ROBFRAME_In1 + + + Tooltip_TRAFO_ROBFRAME_In2 + + + Tooltip_TRAFO_ROBFRAME_In3 + + + + + Tooltip_TRAFO_ROBFRAME_Out1 + + + Tooltip_TRAFO_ROBFRAME_Out2 + + + Tooltip_TRAFO_ROBFRAME_Out3 + + + + + Tooltip_TRAFO_ROBFRAME_Type + + RotTrans + OnlyRot + EulerAngles + + + + Tooltip_TRAFO_ROBFRAME_Source + + World + Base + RobRoot + Tool + TTS + + + + Tooltip_TRAFO_ROBFRAME_Target + + World + Base + RobRoot + Tool + TTS + + + + + + Tooltip_TRAFO_USERFRAME + + + + + + + + + + + + + Tooltip_TRAFO_USERFRAME_Type + + RotTrans + OnlyRot + EulerAngles + + + + Tooltip_TRAFO_USERFRAME_FraX + + + Tooltip_TRAFO_USERFRAME_FraY + + + Tooltip_TRAFO_USERFRAME_FraZ + + + Tooltip_TRAFO_USERFRAME_FraA + + + Tooltip_TRAFO_USERFRAME_FraB + + + Tooltip_TRAFO_USERFRAME_FraC + + + + + + + + Remarks_AXISCORR + Tooltip_AXISCORR + + + + Tooltip_AXISCORR_CorrA1 + + + Tooltip_AXISCORR_CorrA2 + + + Tooltip_AXISCORR_CorrA3 + + + Tooltip_AXISCORR_CorrA4 + + + Tooltip_AXISCORR_CorrA5 + + + Tooltip_AXISCORR_CorrA6 + + + + + + Remarks_AXISCORR_Stat + Tooltip_AXISCORR_Stat + + + + Tooltip_AXISCORR_A1 + + + Tooltip_AXISCORR_A2 + + + Tooltip_AXISCORR_A3 + + + Tooltip_AXISCORR_A4 + + + Tooltip_AXISCORR_A5 + + + Tooltip_AXISCORR_A6 + + + + + Tooltip_AXISCORR_LowerLimA1 + + + Tooltip_AXISCORR_LowerLimA2 + + + Tooltip_AXISCORR_LowerLimA3 + + + Tooltip_AXISCORR_LowerLimA4 + + + Tooltip_AXISCORR_LowerLimA5 + + + Tooltip_AXISCORR_LowerLimA6 + + + Tooltip_AXISCORR_UpperLimA1 + + + Tooltip_AXISCORR_UpperLimA2 + + + Tooltip_AXISCORR_UpperLimA3 + + + Tooltip_AXISCORR_UpperLimA4 + + + Tooltip_AXISCORR_UpperLimA5 + + + Tooltip_AXISCORR_UpperLimA6 + + + + + + Remarks_AXISCORREXT + Tooltip_AXISCORREXT + + + + Tooltip_AXISCORREXT_CorrE1 + + + Tooltip_AXISCORREXT_CorrE2 + + + Tooltip_AXISCORREXT_CorrE3 + + + Tooltip_AXISCORREXT_CorrE4 + + + Tooltip_AXISCORREXT_CorrE5 + + + Tooltip_AXISCORREXT_CorrE6 + + + + + + Remarks_AXISCORREXT_Stat + Tooltip_AXISCORREXT_Stat + + + + Tooltip_AXISCORREXT_E1 + + + Tooltip_AXISCORREXT_E2 + + + Tooltip_AXISCORREXT_E3 + + + Tooltip_AXISCORREXT_E4 + + + Tooltip_AXISCORREXT_E5 + + + Tooltip_AXISCORREXT_E6 + + + + + Tooltip_AXISCORREXT_LowerLimE1 + + + Tooltip_AXISCORREXT_LowerLimE2 + + + Tooltip_AXISCORREXT_LowerLimE3 + + + Tooltip_AXISCORREXT_LowerLimE4 + + + Tooltip_AXISCORREXT_LowerLimE5 + + + Tooltip_AXISCORREXT_LowerLimE6 + + + Tooltip_AXISCORREXT_UpperLimE1 + + + Tooltip_AXISCORREXT_UpperLimE2 + + + Tooltip_AXISCORREXT_UpperLimE3 + + + Tooltip_AXISCORREXT_UpperLimE4 + + + Tooltip_AXISCORREXT_UpperLimE5 + + + Tooltip_AXISCORREXT_UpperLimE6 + + + + + Tooltip_AXISCORRMON + + + Tooltip_AXISCORRMON_A1 + + + Tooltip_AXISCORRMON_A2 + + + Tooltip_AXISCORRMON_A3 + + + Tooltip_AXISCORRMON_A4 + + + Tooltip_AXISCORRMON_A5 + + + Tooltip_AXISCORRMON_A6 + + + Tooltip_AXISCORRMON_E1 + + + Tooltip_AXISCORRMON_E2 + + + Tooltip_AXISCORRMON_E3 + + + Tooltip_AXISCORRMON_E4 + + + Tooltip_AXISCORRMON_E5 + + + Tooltip_AXISCORRMON_E6 + + + + + Tooltip_AXISCORRMON_MaxA1 + + + Tooltip_AXISCORRMON_MaxA2 + + + Tooltip_AXISCORRMON_MaxA3 + + + Tooltip_AXISCORRMON_MaxA4 + + + Tooltip_AXISCORRMON_MaxA5 + + + Tooltip_AXISCORRMON_MaxA6 + + + Tooltip_AXISCORRMON_MaxE1 + + + Tooltip_AXISCORRMON_MaxE2 + + + Tooltip_AXISCORRMON_MaxE3 + + + Tooltip_AXISCORRMON_MaxE4 + + + Tooltip_AXISCORRMON_MaxE5 + + + Tooltip_AXISCORRMON_MaxE6 + + + + + Tooltip_MAP2OV_PRO + + + Tooltip_MAP2OV_PRO_In1 + + + + + + Remarks_POSCORR + Tooltip_POSCORR + + + + Tooltip_POSCORR_CorrX + + + Tooltip_POSCORR_CorrY + + + Tooltip_POSCORR_CorrZ + + + Tooltip_POSCORR_CorrA + + + Tooltip_POSCORR_CorrB + + + Tooltip_POSCORR_CorrC + + + + + + Remarks_POSCORR_Stat + Tooltip_POSCORR_Stat + + + + Tooltip_POSCORR_X + + + Tooltip_POSCORR_Y + + + Tooltip_POSCORR_Z + + + Tooltip_POSCORR_A + + + Tooltip_POSCORR_B + + + Tooltip_POSCORR_C + + + + + Tooltip_POSCORR_LowerLimX + + + Tooltip_POSCORR_LowerLimY + + + Tooltip_POSCORR_LowerLimZ + + + Tooltip_POSCORR_UpperLimX + + + Tooltip_POSCORR_UpperLimY + + + Tooltip_POSCORR_UpperLimZ + + + Tooltip_POSCORR_MaxRotAngle + + + Tooltip_POSCORR_LastCorrStat + + + Tooltip_POSCORR_LastCorrX + + + Tooltip_POSCORR_LastCorrY + + + Tooltip_POSCORR_LastCorrZ + + + Tooltip_POSCORR_LastCorrA + + + Tooltip_POSCORR_LastCorrB + + + Tooltip_POSCORR_LastCorrC + + + Tooltip_POSCORR_RefCorrSys + + World + Base + RobRoot + Tool + TTS + + + + + + Tooltip_POSCORRMON + + + Tooltip_POSCORRMON_X + + + Tooltip_POSCORRMON_Y + + + Tooltip_POSCORRMON_Z + + + Tooltip_POSCORRMON_A + + + Tooltip_POSCORRMON_B + + + Tooltip_POSCORRMON_C + + + + + + Remark_ParamNoChangeDuringRuntime + Tooltip_POSCORRMON_MaxTrans + + + + + Remark_ParamNoChangeDuringRuntime + Tooltip_POSCORRMON_MaxRotAngle + + + + + + + Remarks_STOP + Tooltip_STOP + + + + + + + Tooltip_STOP_Mode + + InfoMessage + PathNormal + Velocity + PathFast + ExitMoveCorr + + + + + Remarks_STOP_Channel + Tooltip_STOP_Channel + + + + + + + + Tooltip_AXISACT + + + Tooltip_AXISACT_A1 + + + Tooltip_AXISACT_A2 + + + Tooltip_AXISACT_A3 + + + Tooltip_AXISACT_A4 + + + Tooltip_AXISACT_A5 + + + Tooltip_AXISACT_A6 + + + + + + Remarks_AXISACT_Type + Tooltip_POSACT_AXISACT_Type + axisact_remarks.png + + + Measured + IPO + IPO_FLT + CF + + + + + + Tooltip_AXISACTEXT + + + Tooltip_AXISACTEXT_E1 + + + Tooltip_AXISACTEXT_E2 + + + Tooltip_AXISACTEXT_E3 + + + Tooltip_AXISACTEXT_E4 + + + Tooltip_AXISACTEXT_E5 + + + Tooltip_AXISACTEXT_E6 + + + + + + Remarks_AXISACT_Type + Tooltip_POSACT_AXISACT_Type + axisact_remarks.png + + + Measured + IPO + IPO_FLT + CF + + + + + + Tooltip_GEARTORQUE + + + Tooltip_GEARTORQUE_A1 + + + Tooltip_GEARTORQUE_A2 + + + Tooltip_GEARTORQUE_A3 + + + Tooltip_GEARTORQUE_A4 + + + Tooltip_GEARTORQUE_A5 + + + Tooltip_GEARTORQUE_A6 + + + + + Tooltip_GEARTORQUE_TorqueSource + + Measured + Setpoint + + + + Tooltip_GEARTORQUE_LocationOnJoint + + Gear + Drive + + + + + + Tooltip_GEARTORQUEEXT + + + Tooltip_GEARTORQUEEXT_E1 + + + Tooltip_GEARTORQUEEXT_E2 + + + Tooltip_GEARTORQUEEXT_E3 + + + Tooltip_GEARTORQUEEXT_E4 + + + Tooltip_GEARTORQUEEXT_E5 + + + Tooltip_GEARTORQUEEXT_E6 + + + + + Tooltip_GEARTORQUE_TorqueSource + + Measured + Setpoint + + + + Tooltip_GEARTORQUE_LocationOnJoint + + Gear + Drive + + + + + + Tooltip_MOTORCURRENT + + + Tooltip_MOTORCURRENT_A1 + + + Tooltip_MOTORCURRENT_A2 + + + Tooltip_MOTORCURRENT_A3 + + + Tooltip_MOTORCURRENT_A4 + + + Tooltip_MOTORCURRENT_A5 + + + Tooltip_MOTORCURRENT_A6 + + + + + Tooltip_MOTORCURRENTEXT + + + Tooltip_MOTORCURRENTEXT_E1 + + + Tooltip_MOTORCURRENTEXT_E2 + + + Tooltip_MOTORCURRENTEXT_E3 + + + Tooltip_MOTORCURRENTEXT_E4 + + + Tooltip_MOTORCURRENTEXT_E5 + + + Tooltip_MOTORCURRENTEXT_E6 + + + + + Tooltip_OV_PRO + + + Tooltip_OV_PRO_Out1 + + + + + Tooltip_POSACT + + + Tooltip_POSACT_X + + + Tooltip_POSACT_Y + + + Tooltip_POSACT_Z + + + Tooltip_POSACT_A + + + Tooltip_POSACT_B + + + Tooltip_POSACT_C + + + Tooltip_POSACT_S + + + Tooltip_POSACT_T + + + + + + Remarks_POSACT_Type + Tooltip_POSACT_AXISACT_Type + posact_remarks.png + + + Measured + IPO + IPO_FLT + CF + + + + + + Tooltip_STATUS + + + Tooltip_STATUS_Stat + + + + + + Remarks_STATUS_Type + Tooltip_STATUS_Type + + + IPO_State + ProState_S + ProState_R + Pro_Mode_S + Pro_Mode_R + Mode_Op + IPO_Mode + IPO_Mode_C + Sensor + + + + + + + + Tooltip_D + + + + + + + + + Tooltip_D_KD + + + + + Tooltip_DELAY + + + + + + Tooltip_DELAY_Out1 + + + + + Tooltip_DELAY_DelayTime + + + + + Tooltip_GENCTRL + + + + + + + + + Tooltip_GENCTRL_Reset + + + Tooltip_GENCTRL_B0 + + + Tooltip_GENCTRL_A1 + + + Tooltip_GENCTRL_B1 + + + Tooltip_GENCTRL_A2 + + + Tooltip_GENCTRL_B2 + + + Tooltip_GENCTRL_A3 + + + Tooltip_GENCTRL_B3 + + + Tooltip_GENCTRL_A4 + + + Tooltip_GENCTRL_B4 + + + Tooltip_GENCTRL_A5 + + + Tooltip_GENCTRL_B5 + + + Tooltip_GENCTRL_A6 + + + Tooltip_GENCTRL_B6 + + + Tooltip_GENCTRL_A7 + + + Tooltip_GENCTRL_B7 + + + Tooltip_GENCTRL_A8 + + + Tooltip_GENCTRL_B8 + + + + + Tooltip_I + + + Tooltip_I_In1 + + + + + Tooltip_I_Out1 + + + + + Tooltip_I_Reset + + + Tooltip_I_TI + + + Tooltip_I_Type + + Always + OnCorrActive + + + + Tooltip_I_LowerLimit + + + Tooltip_I_UpperLimit + + + + + Tooltip_IIRFILTER + + + Tooltip_IIRFILTER_In1 + + + + + Tooltip_IIRFILTER_Out1 + + + + + Tooltip_IIRFILTER_Reset + + + Tooltip_IIRFILTER_Type + + Lowpass + Highpass + + + + Tooltip_IIRFILTER_Name + + Bessel + Butterworth + Tschebyscheff_0_5db + Tschebyscheff_1db + Tschebyscheff_2db + Tschebyscheff_3db + + + + Tooltip_IIRFILTER_Order + + O2 + O4 + O6 + O8 + O10 + + + + Tooltip_IIRFILTER_Cuttoff + + + + + Tooltip_P + + + + + + + + + Tooltip_P_KR + + + + + Tooltip_PD + + + + + + + + + Tooltip_PD_KR + + + Tooltip_PD_TV + + + + + Tooltip_PID + + + + + + + + + Tooltip_PID_Reset + + + Tooltip_PID_KR + + + + Remarks_PID_TN + Tooltip_PID_TN + + + + Tooltip_PID_TV + + + + Remarks_PID_LimLow + Tooltip_PID_LimLow + + + + + Remarks_PID_LimUpp + Tooltip_PID_LimUpp + + + + + + Tooltip_PT1 + + + + + + + + + Tooltip_PT1_KR + + + Tooltip_PT1_T1 + + + + + Tooltip_PT2 + + + + + + + + + Tooltip_PT2_KR + + + Tooltip_PT2_T1 + + + Tooltip_PT2_T2 + + + + + + Remarks_SOURCE + Tooltip_SOURCE_2 + Tooltip_SOURCE + Source.png + + + + + + + + Remarks_SOURCE_Type + Tooltip_SOURCE_Type + + + Const + Sin + Cos + Square + Sawtooth + + + + Tooltip_SOURCE_Offset + + + + Remarks_SOURCE_Amplitude + Tooltip_SOURCE_Amplitude + + + + + Remarks_SOURCE_Period + Tooltip_SOURCE_Period + + + + + + Tooltip_TIMER + + + Tooltip_TIMER_Ctrl + + + Tooltip_TIMER_InReset + + + + + Tooltip_TIMER_Out1 + + + + + Tooltip_TIMER_Reset + + + Tooltip_TIMER_Time + + + + + Tooltip_Constant + + + + + + Tooltip_Constant_Value + + + + + + Remarks_SINE + Summary_SINE + Sine.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_SINE_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + Remarks_COSINE + Summary_COSINE + Cosine.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_COSINE_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + Remarks_SAWTOOTH + Summary_SAWTOOTH + Sawtooth.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_SAWTOOTH_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + Remarks_TRIANGLE + Summary_TRIANGLE + Triangle.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_TRIANGLE_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + Remarks_RECTANGLE + Summary_RECTANGLE + Rectangle.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_RECTANGLE_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src b/kuka_rsi_hw_interface/krl/ros_rsi.src similarity index 100% rename from kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src rename to kuka_rsi_hw_interface/krl/ros_rsi.src diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml b/kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml old mode 100644 new mode 100755 similarity index 88% rename from kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml rename to kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml index 806e89a9..b13c27f0 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml +++ b/kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml @@ -1,7 +1,7 @@ - ip.address.of.rospc - 49152 + 172.32.20.20 + 59152 ImFree FALSE @@ -29,6 +29,7 @@ + - + \ No newline at end of file From f003b8e6bda8d05fade55a4c72e5adcf6cee5737 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 23 Jun 2023 14:15:42 +0200 Subject: [PATCH 30/97] UDP without memory allocation --- .../kuka_rsi_hw_interface/kuka_hardware_interface.hpp | 4 ++-- .../include/kuka_rsi_hw_interface/udp_server.h | 10 +++++----- kuka_rsi_hw_interface/krl/ros_rsi.src | 2 +- kuka_rsi_hw_interface/launch/startup.launch.py | 3 ++- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 4 +--- 5 files changed, 11 insertions(+), 12 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 98373487..2b0d12ca 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -117,8 +117,8 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface RSIState rsi_state_; RSICommand rsi_command_; std::unique_ptr server_; - std::string in_buffer_; - std::string out_buffer_; + char in_buffer_[BUFSIZE]; // udp_server.h --> #define BUFSIZE 1024 + char out_buffer_[BUFSIZE]; static constexpr double R2D = 180 / M_PI; static constexpr double D2R = M_PI / 180; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 6ebdb4e7..10dae2af 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -102,12 +102,12 @@ class UDPServer } } - ssize_t send(std::string & buffer) + ssize_t send(char * buffer) { ssize_t bytes = 0; bytes = sendto( - sockfd_, buffer.c_str(), - buffer.size(), 0, (struct sockaddr *) &clientaddr_, clientlen_); + sockfd_, buffer, + strlen(buffer), 0, (struct sockaddr *) &clientaddr_, clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in send"); } @@ -115,7 +115,7 @@ class UDPServer return bytes; } - ssize_t recv(std::string & buffer) + ssize_t recv(char * buffer) { ssize_t bytes = 0; @@ -151,7 +151,7 @@ class UDPServer } } - buffer = std::string(buffer_); + strncpy(buffer, buffer_, BUFSIZE); return bytes; } diff --git a/kuka_rsi_hw_interface/krl/ros_rsi.src b/kuka_rsi_hw_interface/krl/ros_rsi.src index 9df9f6d6..8ef8c23a 100644 --- a/kuka_rsi_hw_interface/krl/ros_rsi.src +++ b/kuka_rsi_hw_interface/krl/ros_rsi.src @@ -69,7 +69,7 @@ DECL INT CONTID ; ContainerID PTP {A1 0, A2 -90, A3 90, A4 0, A5 90, A6 0} ; Create RSI Context -ret = RSI_CREATE("ros_rsi.rsi",CONTID,TRUE) +ret = RSI_CREATE("ros_rsi",CONTID,TRUE) IF (ret <> RSIOK) THEN HALT ENDIF diff --git a/kuka_rsi_hw_interface/launch/startup.launch.py b/kuka_rsi_hw_interface/launch/startup.launch.py index 4d7d3913..9df53a81 100644 --- a/kuka_rsi_hw_interface/launch/startup.launch.py +++ b/kuka_rsi_hw_interface/launch/startup.launch.py @@ -51,7 +51,8 @@ def generate_launch_description(): Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "-c", controller_manager_node, "--inactive"], + arguments=["joint_state_broadcaster", "-c", + controller_manager_node, "--inactive"], ), Node( package="controller_manager", diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 78963925..e19fcba3 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -87,8 +87,6 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw } // RSI - in_buffer_.resize(1024); // udp_server.h --> #define BUFSIZE 1024 - out_buffer_.resize(1024); initial_joint_pos_.resize(info_.joints.size(), 0.0); joint_pos_correction_deg_.resize(info_.joints.size(), 0.0); @@ -207,7 +205,7 @@ return_type KukaRSIHardwareInterface::write( return return_type::OK; } - if (stop_flag_) is_active_ = false; + if (stop_flag_) {is_active_ = false;} for (size_t i = 0; i < info_.joints.size(); i++) { joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * From 3220ed33ca4da4a1426241129c345aec7b0ab316 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Mon, 26 Jun 2023 09:33:28 +0200 Subject: [PATCH 31/97] todos --- .../include/kuka_rsi_hw_interface/rsi_state.h | 2 +- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h index e66ea478..f859dd67 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h @@ -88,7 +88,7 @@ class RSIState RIst_el->Attribute("A", &cart_position[3]); RIst_el->Attribute("B", &cart_position[4]); RIst_el->Attribute("C", &cart_position[5]); - // Extract cartesian actual position + // Extract cartesian setpoint position TiXmlElement * RSol_el = rob->FirstChildElement("RSol"); RSol_el->Attribute("X", &initial_cart_position[0]); RSol_el->Attribute("Y", &initial_cart_position[1]); diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index e19fcba3..eb71b6f3 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -144,6 +144,8 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta bytes = server_->recv(in_buffer_); } + //TODO (Komáromi): Decode received msg struct and create a struct + //TODO (Komáromi): Receive msg rsi_state_ = RSIState(in_buffer_); for (size_t i = 0; i < info_.joints.size(); ++i) { @@ -153,6 +155,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta } ipoc_ = rsi_state_.ipoc; + //TODO (Komáromi): construct and send msg out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); server_->set_timeout(1000); // Set receive timeout to 1 second From 0bb44e6d9d5d6268cdcbe74b8d9cfed55bdf4879 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 29 Jun 2023 18:12:22 +0200 Subject: [PATCH 32/97] xml_element defined --- .../kuka_hardware_interface.hpp | 4 +- .../rsi_command_handler.hpp | 68 ++++++++++++++++++ .../kuka_rsi_hw_interface/udp_server.h | 16 +++-- .../kuka_rsi_hw_interface/xml_element.hpp | 69 +++++++++++++++++++ .../src/rsi_command_handler.cpp | 55 +++++++++++++++ 5 files changed, 203 insertions(+), 9 deletions(-) create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp create mode 100644 kuka_rsi_hw_interface/src/rsi_command_handler.cpp diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 2b0d12ca..ec6fb03e 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -117,8 +117,8 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface RSIState rsi_state_; RSICommand rsi_command_; std::unique_ptr server_; - char in_buffer_[BUFSIZE]; // udp_server.h --> #define BUFSIZE 1024 - char out_buffer_[BUFSIZE]; + char in_buffer_[BUFFER_SIZE]; // udp_server.h --> constexpr size_t BUFFER_SIZE = 1024; + char out_buffer_[BUFFER_SIZE]; static constexpr double R2D = 180 / M_PI; static constexpr double D2R = M_PI / 180; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp new file mode 100644 index 00000000..8f3c5168 --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -0,0 +1,68 @@ +/********************************************************************* +* 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: Komaromi Sandor + */ + +#ifndef KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_ +#define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_ + + +#include "xml_element.hpp" + +namespace kuka_rsi_hw_interface +{ + +class rsi_command_handler +{ +private: + XML::xml_element command_data_structure_; + XML::xml_element state_data_structure_; + + // void updateDataPlace(); + // void updateData(); + +public: + rsi_command_handler(); + ~rsi_command_handler() = default; + + void Decode(char * buffer, size_t buffer_size); + void Encode(char * buffer); +}; +} // namespace kuka_rsi_hw_interface + + +#endif // KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 10dae2af..cbd255dd 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -59,7 +59,7 @@ #include "rclcpp/rclcpp.hpp" -#define BUFSIZE 1024 +constexpr size_t BUFFER_SIZE = 1024; class UDPServer { @@ -133,9 +133,9 @@ class UDPServer } if (FD_ISSET(sockfd_, &read_fds)) { - memset(buffer_, 0, BUFSIZE); + memset(buffer_, 0, BUFFER_SIZE); bytes = - recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); + recvfrom(sockfd_, buffer_, BUFFER_SIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } @@ -144,14 +144,16 @@ class UDPServer } } else { - memset(buffer_, 0, BUFSIZE); - bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); + memset(buffer_, 0, BUFFER_SIZE); + bytes = recvfrom( + sockfd_, buffer_, BUFFER_SIZE, 0, (struct sockaddr *) &clientaddr_, + &clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } } - strncpy(buffer, buffer_, BUFSIZE); + strncpy(buffer, buffer_, BUFFER_SIZE); return bytes; } @@ -166,7 +168,7 @@ class UDPServer socklen_t clientlen_; struct sockaddr_in serveraddr_; struct sockaddr_in clientaddr_; - char buffer_[BUFSIZE]; + char buffer_[BUFFER_SIZE]; int optval; }; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp new file mode 100644 index 00000000..b0796065 --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp @@ -0,0 +1,69 @@ +/********************************************************************* +* 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: Komaromi Sandor + */ + +#ifndef XML__XML_ELEMENT_H_ +#define XML__XML_ELEMENT_H_ + +#include +#include +#include +#include + +#define XML_STRING_TYPE std::pair + +namespace XML +{ + +class xml_element +{ +public: + std::string name_; + // Extend feature with string type param + std::map> params_; + std::variant data_; + std::vector childs_; + + xml_element(std::string name) + : name_(name) {} + xml_element() = default; + ~xml_element() = default; +}; +} + +#endif // XML__XML_ELEMENT_H_ diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp new file mode 100644 index 00000000..cb16d2cd --- /dev/null +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -0,0 +1,55 @@ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rsi_command_handler.hpp" + +namespace kuka_rsi_hw_interface +{ +rsi_command_handler::rsi_command_handler() +{ + // Later thi should be defined by the rsi xml + command_data_structure_.name_ = "Rob"; + command_data_structure_.params_["TYPE"] = std::make_pair(nullptr, 0); + // how to get string: std::get(command_data_structure_.params_["TYPE"]).first + XML::xml_element Out("Out"); + Out.params_.emplace("01"); + Out.params_.emplace("02"); + Out.params_.emplace("03"); + Out.params_.emplace("04"); + Out.params_.emplace("05"); + XML::xml_element Override("Overrride"); + command_data_structure_.childs_.emplace_back(Out); + command_data_structure_.childs_.emplace_back(Override); +} + +void rsi_command_handler::Decode(char * buffer, size_t buffer_size) +{ + char * string_start_ptr = buffer; + size_t string_length = 0; + size_t i = 0; + + // fast forward for the first open bracket + for (; i < buffer_size || buffer[i] != '<'; i++) { + } + + // Check one node + // Get string + string_start_ptr = &buffer[i + 1]; + for (; buffer[i] != ' '; i++, string_length++) { + } + +} +} From 6ee7f28031142f2ad8c8fa4ccb54e7b904c3adf0 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 30 Jun 2023 11:54:35 +0200 Subject: [PATCH 33/97] update ip log --- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index eb71b6f3..81170b34 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -97,7 +97,7 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw RCLCPP_INFO( rclcpp::get_logger("KukaRSIHardwareInterface"), - "robot location: %s:%d", rsi_ip_address_.c_str(), rsi_port_); + "IP of client machine: %s:%d", rsi_ip_address_.c_str(), rsi_port_); return CallbackReturn::SUCCESS; } From 5719717fa3a1e9557ae62675804d0506883f1ba3 Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 30 Jun 2023 16:43:27 +0200 Subject: [PATCH 34/97] update rsi start instructions --- doc/wiki/Home.md | 2 +- kuka_rsi_hw_interface/krl/README_KRC4.md | 78 ++++++------------------ kuka_rsi_hw_interface/krl/README_KRC5.md | 67 +++++--------------- 3 files changed, 33 insertions(+), 114 deletions(-) diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index c5391f48..e3f834af 100644 --- a/doc/wiki/Home.md +++ b/doc/wiki/Home.md @@ -64,7 +64,7 @@ The following features of the FRI are exposed to ROS2: ## KUKA KSS driver (RSI) -Another project in the repo centers on the development of a ROS2 driver for KSS robots through Robot Sensor Interface (RSI). It is in an experimental state, with only joint angle states and commands available. The structure of the driver is similiar to that of the Sunrise driver and the same interfaces are opened, so the same joint controller can be used to move robots. Howewer this controller should be improved, as it allows big torques that stop the robot for machine protection reasons. +Another project in the repo centers on the development of a ROS2 driver for KSS robots through Robot Sensor Interface (RSI). It is in an experimental state, with only joint angle states and commands available. The guide to set up this driver can be found in kuka_rsi_hw_interface\krl for both KCR4 and KRC5 controllers. ## iiQKA driver (ECI) diff --git a/kuka_rsi_hw_interface/krl/README_KRC4.md b/kuka_rsi_hw_interface/krl/README_KRC4.md index 0aba235f..7782986d 100644 --- a/kuka_rsi_hw_interface/krl/README_KRC4.md +++ b/kuka_rsi_hw_interface/krl/README_KRC4.md @@ -1,6 +1,6 @@ # Configuring RSI on the controller -This guide highlights the steps needed in order to successfully configure the **RSI interface** on the controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS. +This guide highlights the steps needed in order to successfully configure the **RSI interface** on the controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS2. ## 1. Controller network configuration @@ -32,20 +32,9 @@ The files included in this folder specifies the data transferred via RSI. Some o ##### ros_rsi_ethernet.xml 1. Edit the `IP_NUMBER` tag so that it corresponds to the IP address (192.168.1.xx) previously added for your PC. -2. Keep the `PORT` tag as it is (49152) or change it if you want to use another port. +2. Keep the `PORT` tag as it is (59152) or change it if you want to use another port. -Note that the `rsi/listen_address` and `rsi/listen_port` parameters of the `kuka_rsi_hw_interface` must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. - -##### ros_rsi.rsi.xml -This file may be edited with application specific joint limits in degrees. -* Edit the parameters within the RSIObject `AXISCORR` to specify joint limits such as **LowerLimA1**, **UpperLimA1** etc. Note that these limits are in reference to the start position of the robot. -* Edit the parameters within `AXISCORRMON` to specify the overall correction limitation. If this limit is exceeded in either of the joint directions, RSI is stopped. The values of **MaxA1**, **MaxA2** etc. may be large to allow free movement within the specified joint limits in `AXISCORR`. - -Notice the RSIObject of type `ETHERNET`. Within this object is a parameter called **Timeout**. This parameter is set to **100** by default. The RSI interface operates at `250 Hz` (4ms cycle). The **kuka_rsi_hardware_interface** does not have a period configured and is completely driven by the controller's output. Every controller RSI output has a IPOC timestamp which increments for every cycle. The **kuka_rsi_hardware_interface** will answer as quickly as possible. The answer includes the last IPOC received. If the connected **PC with ROS** did not manage to answer within the RSI cycle of **4ms**, the IPOC timestamp of RSI has incremented. The IPOC included in the answer is not matched and the controller will increment a counter. When this counter hits the **Timeout** parameter (**100**), the RSI connection will shut down. If this parameter is lowered, the demand for real-time computation will increase. - -If you have problems with the connection to RSI shutting down now and then while moving the robot it is suggested to: -* Compile and install a [RT-Preempt](https://rt.wiki.kernel.org/index.php/RT_PREEMPT_HOWTO) kernel for your PC. -* Give **kuka_rsi_hardware_interface** on your PC real-time priority when the RSI connection is established. +Note that the `rsi_ip_address` and `rsi_port` tags of the kuka_kr6_support/urdf/kr6r700sixx_macro_ros2_control.xacro (inside kuka_simulators) must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. ##### ros_rsi.src This should only be edited if the start position specified within the file is not desirable for your application. @@ -59,57 +48,24 @@ The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All 4. Copy the `ros_rsi.src` file to `KRC:\R1\Program`. 5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface`. -## 3. Configure the kuka_rsi_hw_interface -The **kuka_rsi_hardware_interface** needs to be configured in order to successfully communicate with RSI. Inside `/kuka_rsi_hw_interface/test` and `/kuka_rsi_hw_interface/config` in this repository is a set of `*.yaml` files. These configuration files may be loaded into a launch-file used to start the **kuka_rsi_hardware_interface** with correct parameters, such as: - -* **Joint names** corresponding to the robot description (URDF or .xacro). -* **IP address** and **port** corresponding to the RSI setup specified in **ros_rsi_ethernet.xml**. - -We recommend that you copy the configuration files, edit the copies for your needs and use these files to create your own launch file. A template will be provided at a later time. However, at this time, have a look at `test_hardware_interface.launch`, `test_params.yaml`, `controller_joint_names.yaml` and `hardware_controllers.yaml` to achieve a working test-launch. - -In order to successfully launch the **kuka_rsi_hardware_interface** a parameter `robot_description` needs to be present on the ROS parameter server. This parameter can be set manually or by adding this line inside the launch file (replace support package and .xacro to match your application): - -``` - -``` - -Make sure that the line is added before the `kuka_hardware_interface` itself is loaded. - -## 4. Testing +## 3. Testing At this point you are ready to test the RSI interface. Before the test, make sure that: -* You have specified the `rsi/listen_address` and `rsi/listen_port` of the **kuka_rsi_hardware_interface** to correspond with the KRL files on the controller. -* You have a launch-file loading the network parameters, robot description, kuka_hardware_interface, hardware controller and controller joint names. - -The next steps describe how to launch the test file: - -* In a new terminal: - -``` -$ roscore -``` - -* Open a new terminal and roslaunch the previously configured launch-file: - -``` -$ roslaunch kuka_rsi_hw_interface test_hardware_interface.launch sim:=false -``` - -The **kuka_rsi_hardware_interface** is now waiting for the robot controller to connect. Follow the next steps to connect RSI: +* You have specified the `rsi_ip_address` and `rsi_port` tags in the urdf (kuka_kr6_support/urdf/kr6r700sixx_macro_ros2_control.xacro) to correspond with the KRL files on the controller. -1. On the teach pad, enter mode **T1** for testing purposes. -2. Navigate to `KRC:\R1\Program` and select `ros_rsi.src`. -3. Press and hold an enabling switch and the run/play-button. The robot will first move to the start position. - * A message like **Programmed path reached (BCO)** will be shown at the teach pad. -4. Press and hold again. The teach pad will post a warning **!!! Attention - Sensor correction goes active !!!**. -5. Confirm the warning and press and hold the buttons again. This time the terminal where **kuka_rsi_hardware_interface** is running should output **Got connection from robot**. The RSI connection is now up and running. -6. In a new terminal: +The next steps describe how to start external control using RSI: -``` -$ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller -``` +* Start the driver: ```ros2 launch kuka_rsi_hw_interface startup.launch.py``` -Choose **controller manager ns** and **controller** and you should be able to move each robot joint. +* In a new terminal: ```ros2 lifecycle set robot_manager configure``` -* Note that T1-mode limits the robot movement velocity and is intended for testing purposes. +* Start the `KRC:\R1\Program\ros_rsi.src` program on the controller and move to the step before RSI_MOVECORR() is run + * in T1, a warning (!!! Attention - Sensor correction goes active !!!) should be visible after reaching RSI_MOVECORR(), which is also okay +* Activate driver and controllers: ```ros2 lifecycle set robot_manager activate``` + * The hardware interface is now waiting for the robot controller to connect, the timeout for this is currently 2 seconds +* Start step RSI_MOVECORR() withing the given timeout + * in T1 this can be done with confirming the previously described warning + * This time the terminal where the driver is running should output **Got connection from robot**. The RSI connection is now up and running. +After this, starting an rqt joint trajectory controller (```ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller```) should enable moving the robot with sliders +- This sends the trajectory in batches, which can result in a little jerky movement, so that is not a bug of the driver diff --git a/kuka_rsi_hw_interface/krl/README_KRC5.md b/kuka_rsi_hw_interface/krl/README_KRC5.md index d92b78f0..10fd05fd 100644 --- a/kuka_rsi_hw_interface/krl/README_KRC5.md +++ b/kuka_rsi_hw_interface/krl/README_KRC5.md @@ -39,18 +39,7 @@ The files included in this folder specifies the data transferred via RSI. Some o 1. Edit the `IP_NUMBER` tag so that it corresponds to the IP address (192.168.1.xx) previously added for your PC. 2. Keep the `PORT` tag as it is (59152) or change it if you want to use another port. -Note that the `rsi/listen_address` and `rsi/listen_port` parameters of the `kuka_rsi_hw_interface` must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. - -##### ros_rsi.rsi.xml -This file may be edited with application specific joint limits in degrees. -* Edit the parameters within the RSIObject `AXISCORR` to specify joint limits such as **LowerLimA1**, **UpperLimA1** etc. Note that these limits are in reference to the start position of the robot. -* Edit the parameters within `AXISCORRMON` to specify the overall correction limitation. If this limit is exceeded in either of the joint directions, RSI is stopped. The values of **MaxA1**, **MaxA2** etc. may be large to allow free movement within the specified joint limits in `AXISCORR`. - -Notice the RSIObject of type `ETHERNET`. Within this object is a parameter called **Timeout**. This parameter is set to **100** by default. The RSI interface operates at `250 Hz` (4ms cycle). The **kuka_rsi_hardware_interface** does not have a period configured and is completely driven by the controller's output. Every controller RSI output has a IPOC timestamp which increments for every cycle. The **kuka_rsi_hardware_interface** will answer as quickly as possible. The answer includes the last IPOC received. If the connected **PC with ROS** did not manage to answer within the RSI cycle of **4ms**, the IPOC timestamp of RSI has incremented. The IPOC included in the answer is not matched and the controller will increment a counter. When this counter hits the **Timeout** parameter (**100**), the RSI connection will shut down. If this parameter is lowered, the demand for real-time computation will increase. - -If you have problems with the connection to RSI shutting down now and then while moving the robot it is suggested to: -* Compile and install a [RT-Preempt](https://rt.wiki.kernel.org/index.php/RT_PREEMPT_HOWTO) kernel for your PC. -* Give **kuka_rsi_hardware_interface** on your PC real-time priority when the RSI connection is established. +Note that the `rsi_ip_address` and `rsi_port` tags of the kuka_kr6_support/urdf/kr6r700sixx_macro_ros2_control.xacro (inside kuka_simulators) must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. ##### ros_rsi.src This should only be edited if the start position specified within the file is not desirable for your application. @@ -72,50 +61,24 @@ Method 2: 5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface` in the WorkVisual 6. Deploy the project, and follow the orders -## 3. Configure the kuka_rsi_hw_interface -The **kuka_rsi_hardware_interface** needs to be configured in order to successfully communicate with RSI. Inside `/kuka_rsi_hw_interface/test` and `/kuka_rsi_hw_interface/config` in this repository is a set of `*.yaml` files. These configuration files may be loaded into a launch-file used to start the **kuka_rsi_hardware_interface** with correct parameters, such as: - -* **Joint names** corresponding to the robot description (URDF or .xacro). -* **IP address** and **port** corresponding to the RSI setup specified in **ros_rsi_ethernet.xml**. - -We recommend that you copy the configuration files, edit the copies for your needs and use these files to create your own launch file. A template will be provided at a later time. However, at this time, have a look at `test_hardware_interface.launch`, `test_params.yaml`, `controller_joint_names.yaml` and `hardware_controllers.yaml` to achieve a working test-launch. - -In order to successfully launch the **kuka_rsi_hardware_interface** a parameter `robot_description` needs to be present on the ROS parameter server. This parameter can be set manually or by adding this line inside the launch file (replace support package and .xacro to match your application): - -``` - -``` - -Make sure that the line is added before the `kuka_hardware_interface` itself is loaded. - -## 4. Testing +## 3. Testing At this point you are ready to test the RSI interface. Before the test, make sure that: -* You have specified the `rsi/listen_address` and `rsi/listen_port` of the **kuka_rsi_hardware_interface** to correspond with the KRL files on the controller. -* You have a launch-file loading the network parameters, robot description, kuka_hardware_interface, hardware controller and controller joint names. -The next steps describe how to launch the test file: -* Open a new terminal and roslaunch the previously configured launch-file: - -``` - -$ roslaunch kuka_rsi_hw_interface test_hardware_interface.launch sim:=false - -``` +* You have specified the `rsi_ip_address` and `rsi_port` tags in the urdf (kuka_kr6_support/urdf/kr6r700sixx_macro_ros2_control.xacro) to correspond with the KRL files on the controller. -The **kuka_rsi_hardware_interface** is now waiting for the robot controller to connect. Follow the next steps to connect RSI: -1. On the teach pad, enter mode **T1** for testing purposes. -2. Navigate to `KRC:\R1\Program` and select `ros_rsi.src`. -3. Press and hold an enabling switch and the run/play-button. The robot will first move to the start position. - * A message like **Programmed path reached (BCO)** will be shown at the teach pad. -4. Press and hold again. The teach pad will post a warning **!!! Attention - Sensor correction goes active !!!**. -5. Confirm the warning and press and hold the buttons again. This time the terminal where **kuka_rsi_hardware_interface** is running should output **Got connection from robot**. The RSI connection is now up and running. -6. In a new terminal: +The next steps describe how to start external control using RSI: -``` +* Start the driver: ```ros2 launch kuka_rsi_hw_interface startup.launch.py``` -$ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller +* In a new terminal: ```ros2 lifecycle set robot_manager configure``` -``` +* Start the `KRC:\R1\Program\ros_rsi.src` program on the controller and move to the step before RSI_MOVECORR() is run + * in T1, a warning (!!! Attention - Sensor correction goes active !!!) should be visible after reaching RSI_MOVECORR(), which is also okay +* Activate driver and controllers: ```ros2 lifecycle set robot_manager activate``` + * The hardware interface is now waiting for the robot controller to connect, the timeout for this is currently 2 seconds +* Start step RSI_MOVECORR() withing the given timeout + * in T1 this can be done with confirming the previously described warning + * This time the terminal where the driver is running should output **Got connection from robot**. The RSI connection is now up and running. -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. \ No newline at end of file +After this, starting an rqt joint trajectory controller (```ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller```) should enable moving the robot with sliders +- This sends the trajectory in batches, which can result in a little jerky movement, so that is not a bug of the driver From dd947cd30eac631e3a18266ff770d36d7ee66657 Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 30 Jun 2023 16:47:37 +0200 Subject: [PATCH 35/97] update readme --- kuka_rsi_hw_interface/krl/README_KRC4.md | 1 + kuka_rsi_hw_interface/krl/README_KRC5.md | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/kuka_rsi_hw_interface/krl/README_KRC4.md b/kuka_rsi_hw_interface/krl/README_KRC4.md index 7782986d..484c462c 100644 --- a/kuka_rsi_hw_interface/krl/README_KRC4.md +++ b/kuka_rsi_hw_interface/krl/README_KRC4.md @@ -52,6 +52,7 @@ The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All At this point you are ready to test the RSI interface. Before the test, make sure that: * You have specified the `rsi_ip_address` and `rsi_port` tags in the urdf (kuka_kr6_support/urdf/kr6r700sixx_macro_ros2_control.xacro) to correspond with the KRL files on the controller. +* This IP address is available on the client machine (see Network configuration) The next steps describe how to start external control using RSI: diff --git a/kuka_rsi_hw_interface/krl/README_KRC5.md b/kuka_rsi_hw_interface/krl/README_KRC5.md index 10fd05fd..cdc1228c 100644 --- a/kuka_rsi_hw_interface/krl/README_KRC5.md +++ b/kuka_rsi_hw_interface/krl/README_KRC5.md @@ -31,7 +31,7 @@ Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows * Select the IPV4 properties and their the **Advanced** settings. * In the new window, you can **Add** new IP addresses. -## 2. KRL Files +## 3. KRL Files The files included in this folder specifies the data transferred via RSI. Some of the files needs to be modified to work for your specific configuration. @@ -61,10 +61,11 @@ Method 2: 5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface` in the WorkVisual 6. Deploy the project, and follow the orders -## 3. Testing +## 4. Testing At this point you are ready to test the RSI interface. Before the test, make sure that: * You have specified the `rsi_ip_address` and `rsi_port` tags in the urdf (kuka_kr6_support/urdf/kr6r700sixx_macro_ros2_control.xacro) to correspond with the KRL files on the controller. +* This IP address is available on the client machine (see Network configuration) The next steps describe how to start external control using RSI: From 5c89a0cf98b59b3e37a91d80e2230e86d1af5d1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 30 Jun 2023 19:17:12 +0200 Subject: [PATCH 36/97] xml_element design update --- .../rsi_command_handler.hpp | 2 +- .../kuka_rsi_hw_interface/xml_element.hpp | 82 +++++++++++++++++-- .../src/rsi_command_handler.cpp | 65 ++++++++++++--- 3 files changed, 132 insertions(+), 17 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 8f3c5168..532b6e5e 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -59,7 +59,7 @@ class rsi_command_handler rsi_command_handler(); ~rsi_command_handler() = default; - void Decode(char * buffer, size_t buffer_size); + bool Decode(char * buffer, size_t buffer_size); void Encode(char * buffer); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp index b0796065..653d109a 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp @@ -40,29 +40,99 @@ #ifndef XML__XML_ELEMENT_H_ #define XML__XML_ELEMENT_H_ -#include #include #include #include - -#define XML_STRING_TYPE std::pair +#include +#include namespace XML { +enum class XMLType : size_t +{ + BOOL = 0, + LONG = 1, + DOUBLE = 2, + STRING = 3 +}; + +struct xml_string +{ + char * data_ptr_; + size_t length_; + xml_string(char * data_ptr = nullptr, size_t length = 0) + : data_ptr_(data_ptr), length_(length) {} +}; + +template +struct xml_param +{ + typedef T ParamType; + std::variant param_; +}; + + +struct xml_string_comperator +{ + bool operator()(const std::string & a, const xml_string & b) const + { + memcmp(a.c_str, b.data_ptr_, b.length_) == 0; + + (a.length() == b.length_ && + memcmp(a.c_str, b.data_ptr_, b.length_) == 0) ? return true : return false; + } +}; + class xml_element { public: + std::map, xml_string_comperator> params_; std::string name_; - // Extend feature with string type param - std::map> params_; - std::variant data_; std::vector childs_; xml_element(std::string name) : name_(name) {} xml_element() = default; ~xml_element() = default; + + // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used + template bool GetParam(std::string key, T & param) + { + auto param_it = params_.find(key); + if (param_it != params_.end()) { + param = std::get(param_it->second); + return true; + } else { + return false; + } + } + + // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used + template bool SetParam(std::string key, const T & param) + { + auto param_it = params_.find(key); + if (param_it != params_.end()) { + param_it->second = param; + return true; + } else { + return false + } + } + int GetLongParam(std::string key) + { + decltype(params_.find(key)->second)::ParamType; + params_.find(key)->second.param_ + } + int GetDoubleParam(std::string key) + { + return strtod(params_.find(key)->second.data_, NULL, 0); + } + xml_string GetStringParam(std::string key) + { + return strtod(params_.find(key)->second.data_, NULL, 0); + } + }; } diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index cb16d2cd..3af5e419 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -22,34 +22,79 @@ rsi_command_handler::rsi_command_handler() { // Later thi should be defined by the rsi xml command_data_structure_.name_ = "Rob"; - command_data_structure_.params_["TYPE"] = std::make_pair(nullptr, 0); - // how to get string: std::get(command_data_structure_.params_["TYPE"]).first + command_data_structure_.params_["TYPE"] = XML::xml_param(XML::XMLType::STRING); + // how to get string: std::get(command_data_structure_.params_["TYPE"]).first XML::xml_element Out("Out"); - Out.params_.emplace("01"); - Out.params_.emplace("02"); - Out.params_.emplace("03"); - Out.params_.emplace("04"); - Out.params_.emplace("05"); + Out.params_["01"] = XML::xml_param(XML::XMLType::BOOL); + Out.params_["02"] = XML::xml_param(XML::XMLType::BOOL); + Out.params_["03"] = XML::xml_param(XML::XMLType::BOOL); + Out.params_["04"] = XML::xml_param(XML::XMLType::BOOL); + Out.params_["05"] = XML::xml_param(XML::XMLType::BOOL); XML::xml_element Override("Overrride"); + Override.params_["Override"] = XML::xml_param(XML::XMLType::LONG); command_data_structure_.childs_.emplace_back(Out); command_data_structure_.childs_.emplace_back(Override); } -void rsi_command_handler::Decode(char * buffer, size_t buffer_size) +bool rsi_command_handler::Decode(char * buffer, size_t buffer_size) { char * string_start_ptr = buffer; size_t string_length = 0; size_t i = 0; + size_t node_depth = 0; // fast forward for the first open bracket for (; i < buffer_size || buffer[i] != '<'; i++) { } + bool isBaseLessNode = false; // Check one node // Get string - string_start_ptr = &buffer[i + 1]; - for (; buffer[i] != ' '; i++, string_length++) { + i++; + string_start_ptr = &buffer[i]; + for (; i < buffer_size || buffer[i] != ' '; i++, string_length++) { } + // Compare string with char* to check node name + + // Check params + bool isNoMoreParam = false; + while (!isNoMoreParam || i < buffer_size) { + // Serching for params + // goes to the next atribute + for (; i < buffer_size || buffer[i] == ' '; i++) { + } + + // checks for the node hadder end characters + if (buffer[i] == '/' && buffer[i + 1] == '>') { + isBaseLessNode = true; + isNoMoreParam = true; + } else if (buffer[i] == '>') { + isNoMoreParam = true; + } else { + string_start_ptr = &buffer[i]; + string_length = 0; + for (; i < buffer_size || buffer[i] != '='; i++, string_length++) { + } + // check the field is present if it is present save the data else go to next param + // move to param start bracket + for (; i < buffer_size || buffer[i] != '"' || buffer[i] != '\''; i++) { + } + + i++; // move to params first character + string_start_ptr = &buffer[i]; + string_length = 0; + for (; i < buffer_size || buffer[i] != '"' || buffer[i] != '\''; i++, string_length++) { + } + // save param + + } + + + } + if (buffer[i - 1] == '/') { + /* code */ + } + } } From 07d3f11a4a673077aec5d730a5a14f1aa2f7caa2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Wed, 5 Jul 2023 17:27:29 +0200 Subject: [PATCH 37/97] param caster function --- .../kuka_rsi_hw_interface/xml_element.hpp | 108 ++++++++++-------- 1 file changed, 63 insertions(+), 45 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp index 653d109a..291d34b7 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp @@ -46,7 +46,7 @@ #include #include -namespace XML +namespace xml { enum class XMLType : size_t @@ -57,80 +57,98 @@ enum class XMLType : size_t STRING = 3 }; -struct xml_string +struct XMLString { char * data_ptr_; size_t length_; - xml_string(char * data_ptr = nullptr, size_t length = 0) + XMLString(char * data_ptr = nullptr, size_t length = 0) : data_ptr_(data_ptr), length_(length) {} }; -template -struct xml_param +struct XMLParam { - typedef T ParamType; - std::variant param_; + XMLType param_type_; + std::variant param_; + XMLParam(XMLType type) + : param_type_(type) {} }; +// struct XMLStringComparator +// { +// bool operator()(const char * a, const char * b) const +// { +// return a.length() == b.length_ && memcmp(a.data(), b.data_ptr_, b.length_) > 0; +// } +// }; -struct xml_string_comperator -{ - bool operator()(const std::string & a, const xml_string & b) const - { - memcmp(a.c_str, b.data_ptr_, b.length_) == 0; - - (a.length() == b.length_ && - memcmp(a.c_str, b.data_ptr_, b.length_) == 0) ? return true : return false; - } -}; - -class xml_element +class XMLElement { public: - std::map, xml_string_comperator> params_; + std::map params_; std::string name_; - std::vector childs_; + std::vector childs_; - xml_element(std::string name) + XMLElement(std::string name) : name_(name) {} - xml_element() = default; - ~xml_element() = default; + XMLElement() = default; + ~XMLElement() = default; // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used - template bool GetParam(std::string key, T & param) + template + bool GetParam(std::string key, T & param) { auto param_it = params_.find(key); if (param_it != params_.end()) { - param = std::get(param_it->second); - return true; - } else { - return false; + if (std::is_same() || std::is_same() || + std::is_same() || std::is_same) + { + param = std::get(param_it->second.param_); + return true; + } } + return false; } // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used - template bool SetParam(std::string key, const T & param) + template + bool SetParam(std::string key, const T & param) { auto param_it = params_.find(key); if (param_it != params_.end()) { - param_it->second = param; - return true; - } else { - return false + if (std::is_same() || std::is_same() || + std::is_same() || std::is_same) + { + param_it->second.param_ = param; + return true; + } } + return false; } - int GetLongParam(std::string key) - { - decltype(params_.find(key)->second)::ParamType; - params_.find(key)->second.param_ - } - int GetDoubleParam(std::string key) - { - return strtod(params_.find(key)->second.data_, NULL, 0); - } - xml_string GetStringParam(std::string key) + + void CastParam(XMLString key, const char * str_start_ptr, char ** end_ptr) { - return strtod(params_.find(key)->second.data_, NULL, 0); + char key_c_str[key.length_]; + strncpy(key_c_str, key.data_ptr_, key.length_); + + auto param_it = params_.find(key_c_str); + if (param_it != params_.end()) { + switch (param_it->second.param_type_) { + case XMLType::BOOL: + case XMLType::LONG: + param_it->second.param_ = strtol(str_start_ptr, end_ptr, 0); + break; + case XMLType::DOUBLE: + param_it->second.param_ = strtod(str_start_ptr, end_ptr); + break; + case XMLType::STRING: + // TODO (Komaromi): write a cast func for casting XMLString + //param_it->second.param_ = XMLString... + break; + default: + break; + } + } + } }; From 83c0f7d3b80f23bb03e6227975b27c53d8b009e3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 11 Jul 2023 15:03:30 +0200 Subject: [PATCH 38/97] xml_element into a cpp and hpp file --- .../kuka_rsi_hw_interface/xml_element.hpp | 90 +++++-------------- kuka_rsi_hw_interface/src/xml_element.cpp | 79 ++++++++++++++++ 2 files changed, 101 insertions(+), 68 deletions(-) create mode 100644 kuka_rsi_hw_interface/src/xml_element.cpp diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp index 291d34b7..220bb3c2 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp @@ -1,50 +1,23 @@ -/********************************************************************* -* 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: Komaromi Sandor - */ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef XML__XML_ELEMENT_H_ #define XML__XML_ELEMENT_H_ #include -#include #include #include -#include namespace xml { @@ -83,9 +56,12 @@ struct XMLParam class XMLElement { +private: + static XMLString castXMLString(char ** str_ptr); + public: - std::map params_; std::string name_; + std::map params_; std::vector childs_; XMLElement(std::string name) @@ -93,6 +69,11 @@ class XMLElement XMLElement() = default; ~XMLElement() = default; + bool CastParam(XMLString key, char ** str_ptr); + + bool IsParamNameValid(XMLString & key, char ** str_ptr); + bool IsNameValid(XMLString & key, char ** str_ptr); + // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used template bool GetParam(std::string key, T & param) @@ -124,33 +105,6 @@ class XMLElement } return false; } - - void CastParam(XMLString key, const char * str_start_ptr, char ** end_ptr) - { - char key_c_str[key.length_]; - strncpy(key_c_str, key.data_ptr_, key.length_); - - auto param_it = params_.find(key_c_str); - if (param_it != params_.end()) { - switch (param_it->second.param_type_) { - case XMLType::BOOL: - case XMLType::LONG: - param_it->second.param_ = strtol(str_start_ptr, end_ptr, 0); - break; - case XMLType::DOUBLE: - param_it->second.param_ = strtod(str_start_ptr, end_ptr); - break; - case XMLType::STRING: - // TODO (Komaromi): write a cast func for casting XMLString - //param_it->second.param_ = XMLString... - break; - default: - break; - } - } - - } - }; } diff --git a/kuka_rsi_hw_interface/src/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_element.cpp new file mode 100644 index 00000000..4c543159 --- /dev/null +++ b/kuka_rsi_hw_interface/src/xml_element.cpp @@ -0,0 +1,79 @@ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "xml_element.hpp" + +namespace xml +{ +bool XMLElement::CastParam(XMLString key, char ** str_ptr) +{ + char key_c_str[key.length_ + 1] = {0}; + strncpy(key_c_str, key.data_ptr_, key.length_); + + auto param_it = params_.find(key_c_str); + auto ret_val = param_it != params_.end(); + if (ret_val) { + switch (param_it->second.param_type_) { + case XMLType::BOOL: + case XMLType::LONG: + param_it->second.param_ = strtol(*str_ptr, str_ptr, 0); + break; + case XMLType::DOUBLE: + param_it->second.param_ = strtod(*str_ptr, str_ptr); + break; + case XMLType::STRING: + param_it->second.param_ = castXMLString(str_ptr); + break; + default: + return false; + } + } + return ret_val; +} + +bool XMLElement::IsParamNameValid(XMLString & key, char ** str_ptr) +{ + key = castXMLString(str_ptr); + char key_c_str[key.length_ + 1] = {0}; + strncpy(key_c_str, key.data_ptr_, key.length_); + return params_.find(key_c_str) != params_.end(); +} + + +bool XMLElement::IsNameValid(XMLString & key, char ** str_ptr) +{ + key = castXMLString(str_ptr); + if (name_.length() != key.length_) { + return false; + } + return strncmp(name_.c_str(), key.data_ptr_, key.length_); +} + +XMLString XMLElement::castXMLString(char ** str_ptr) +{ + char * start_ptr = *str_ptr; + for (; + **str_ptr != '\0' && **str_ptr != '"' && **str_ptr != ' ' && **str_ptr != '=' && + **str_ptr != '>' && **str_ptr != '/'; *str_ptr++) + { + } + return XMLString(start_ptr, (size_t)(*str_ptr - start_ptr)); +} +} From bdb4e5212cea97b07fa81729f61cb2db064e83d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 11 Jul 2023 17:41:02 +0200 Subject: [PATCH 39/97] decode function with indexing --- .../rsi_command_handler.hpp | 13 +- .../kuka_rsi_hw_interface/xml_element.hpp | 12 +- .../src/rsi_command_handler.cpp | 299 ++++++++++++++---- kuka_rsi_hw_interface/src/xml_element.cpp | 64 +++- 4 files changed, 313 insertions(+), 75 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 532b6e5e..76e1921f 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -46,18 +46,21 @@ namespace kuka_rsi_hw_interface { -class rsi_command_handler +class RSICommandHandler { private: - XML::xml_element command_data_structure_; - XML::xml_element state_data_structure_; + xml::XMLElement command_data_structure_; + xml::XMLElement state_data_structure_; // void updateDataPlace(); // void updateData(); + static void detectNode( + xml::XMLElement & element, char * buffer, int & buffer_idx, + const size_t buffer_size); public: - rsi_command_handler(); - ~rsi_command_handler() = default; + RSICommandHandler(); + ~RSICommandHandler() = default; bool Decode(char * buffer, size_t buffer_size); void Encode(char * buffer); diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp index 220bb3c2..60c23e20 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp @@ -18,6 +18,7 @@ #include #include #include +#include namespace xml { @@ -36,6 +37,9 @@ struct XMLString size_t length_; XMLString(char * data_ptr = nullptr, size_t length = 0) : data_ptr_(data_ptr), length_(length) {} + bool operator==(const XMLString & rhs); + bool operator==(const std::string & rhs); + bool operator==(const char * rhs); }; struct XMLParam @@ -57,7 +61,7 @@ struct XMLParam class XMLElement { private: - static XMLString castXMLString(char ** str_ptr); + static XMLString castXMLString(char * str_ptr, int & idx); public: std::string name_; @@ -69,10 +73,10 @@ class XMLElement XMLElement() = default; ~XMLElement() = default; - bool CastParam(XMLString key, char ** str_ptr); + bool CastParam(XMLString key, char * str_ptr, int & idx); - bool IsParamNameValid(XMLString & key, char ** str_ptr); - bool IsNameValid(XMLString & key, char ** str_ptr); + bool IsParamNameValid(XMLString & key, char * str_ptr, int & idx); + bool IsNameValid(XMLString & key, char * str_ptr, int & idx); // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used template diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 3af5e419..11ad8d73 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -18,83 +18,276 @@ namespace kuka_rsi_hw_interface { -rsi_command_handler::rsi_command_handler() +RSICommandHandler::RSICommandHandler() { - // Later thi should be defined by the rsi xml - command_data_structure_.name_ = "Rob"; - command_data_structure_.params_["TYPE"] = XML::xml_param(XML::XMLType::STRING); + // Later this should be defined by the rsi xml + state_data_structure_.name_ = "Rob"; + state_data_structure_.params_["TYPE"] = xml::XMLParam(xml::XMLType::STRING); // how to get string: std::get(command_data_structure_.params_["TYPE"]).first - XML::xml_element Out("Out"); - Out.params_["01"] = XML::xml_param(XML::XMLType::BOOL); - Out.params_["02"] = XML::xml_param(XML::XMLType::BOOL); - Out.params_["03"] = XML::xml_param(XML::XMLType::BOOL); - Out.params_["04"] = XML::xml_param(XML::XMLType::BOOL); - Out.params_["05"] = XML::xml_param(XML::XMLType::BOOL); - XML::xml_element Override("Overrride"); - Override.params_["Override"] = XML::xml_param(XML::XMLType::LONG); - command_data_structure_.childs_.emplace_back(Out); - command_data_structure_.childs_.emplace_back(Override); + xml::XMLElement Out("Out"); + Out.params_["01"] = xml::XMLParam(xml::XMLType::BOOL); + Out.params_["02"] = xml::XMLParam(xml::XMLType::BOOL); + Out.params_["03"] = xml::XMLParam(xml::XMLType::BOOL); + Out.params_["04"] = xml::XMLParam(xml::XMLType::BOOL); + Out.params_["05"] = xml::XMLParam(xml::XMLType::BOOL); + xml::XMLElement Override("Overrride"); + Override.params_["Override"] = xml::XMLParam(xml::XMLType::LONG); + state_data_structure_.childs_.emplace_back(Out); + state_data_structure_.childs_.emplace_back(Override); } -bool rsi_command_handler::Decode(char * buffer, size_t buffer_size) +bool RSICommandHandler::Decode(char * buffer, size_t buffer_size) { - char * string_start_ptr = buffer; - size_t string_length = 0; - size_t i = 0; - size_t node_depth = 0; + int buffer_idx = 0; + detectNode(state_data_structure_, buffer, buffer_idx, buffer_size); + // xml::XMLString decoded_str(buffer, 0); + // char * buffer_it = buffer; + // size_t node_depth = 0; + + // // fast forward for the first open bracket + // for (; *buffer_it != '\0' && *buffer_it != '<'; buffer_it++) { + // } + + // bool isBaseLessNode = false; + // // Check one node + // // Get string + // buffer_it++; + // if (!state_data_structure_.IsNameValid(decoded_str, buffer_it)) { + // // Do something when failed + // } + // string_start_ptr = &buffer[i]; + // for (; i < buffer_size || buffer[i] != ' '; i++, string_length++) { + // } + // // Compare string with char* to check node name + + // // Check params + // bool isNoMoreParam = false; + // while (!isNoMoreParam || i < buffer_size) { + // // Serching for params + // // goes to the next atribute + // for (; i < buffer_size || buffer[i] == ' '; i++) { + // } + + // // checks for the node hadder end characters + // if (buffer[i] == '/' && buffer[i + 1] == '>') { + // isBaseLessNode = true; + // isNoMoreParam = true; + // } else if (buffer[i] == '>') { + // isNoMoreParam = true; + // } else { + // string_start_ptr = &buffer[i]; + // string_length = 0; + // for (; i < buffer_size || buffer[i] != '='; i++, string_length++) { + // } + // // check the field is present if it is present save the data else go to next param + // // move to param start bracket + // for (; i < buffer_size || buffer[i] != '"' || buffer[i] != '\''; i++) { + // } + + // i++; // move to params first character + // string_start_ptr = &buffer[i]; + // string_length = 0; + // for (; i < buffer_size || buffer[i] != '"' || buffer[i] != '\''; i++, string_length++) { + // } + // // save param + + // } + + + // } + // if (buffer[i - 1] == '/') { + // /* code */ + // } +} + +void RSICommandHandler::detectNode( + xml::XMLElement & element, char * buffer, int & buffer_idx, const size_t buffer_size) +{ + xml::XMLString node_name(buffer, 0); // fast forward for the first open bracket - for (; i < buffer_size || buffer[i] != '<'; i++) { + for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '<'; buffer_idx++) { } - - bool isBaseLessNode = false; - // Check one node - // Get string - i++; - string_start_ptr = &buffer[i]; - for (; i < buffer_size || buffer[i] != ' '; i++, string_length++) { + if (buffer_idx < buffer_size == '\0') { + // TODO (Komaromi): Do something when end of string + } + buffer_idx++; + // Validate nodes name + if (!element.IsNameValid(node_name, buffer, buffer_idx)) { + // TODO (Komaromi): Do something when not valid } - // Compare string with char* to check node name - // Check params + // Validate nodes params + size_t numOfParam = 0; + bool isBaseLessNode = false; bool isNoMoreParam = false; - while (!isNoMoreParam || i < buffer_size) { - // Serching for params - // goes to the next atribute - for (; i < buffer_size || buffer[i] == ' '; i++) { + xml::XMLString node_param; + while (!isNoMoreParam && buffer_idx < buffer_size) { + // fast forward to the next non-space character + for (; *(buffer + buffer_idx) == ' '; buffer_idx++) { } - - // checks for the node hadder end characters - if (buffer[i] == '/' && buffer[i + 1] == '>') { + if (buffer_idx >= buffer_size) { + // TODO (Komaromi): Do something when end of string + } + // Check for the nodes hadders end characters + if (*(buffer + buffer_idx) == '/' && *(buffer + buffer_idx + 1) == '>') { isBaseLessNode = true; isNoMoreParam = true; - } else if (buffer[i] == '>') { + } else if (*(buffer + buffer_idx) == '>') { isNoMoreParam = true; } else { - string_start_ptr = &buffer[i]; - string_length = 0; - for (; i < buffer_size || buffer[i] != '='; i++, string_length++) { + // If not the end of the nodes hadder decode param + if (!element.IsParamNameValid(node_param, buffer, buffer_idx)) { + // TODO (Komaromi): Do something when not valid } - // check the field is present if it is present save the data else go to next param - // move to param start bracket - for (; i < buffer_size || buffer[i] != '"' || buffer[i] != '\''; i++) { + for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '"'; buffer_idx++) { } - - i++; // move to params first character - string_start_ptr = &buffer[i]; - string_length = 0; - for (; i < buffer_size || buffer[i] != '"' || buffer[i] != '\''; i++, string_length++) { + if (buffer_idx >= buffer_size) { + // TODO (Komaromi): Do something when end of string } - // save param - + buffer_idx++; + if (!element.CastParam(node_param, buffer, buffer_idx)) { + // TODO (Komaromi): Do something when not valid + } + buffer_idx++; + numOfParam++; } + } + for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '>'; buffer_idx++) { + } + if (buffer_idx >= buffer_size) { + // TODO (Komaromi): Do something when end of string + } + buffer_idx++; - + if (isBaseLessNode && numOfParam != element.params_.size()) { + // TODO (Komaromi): Do something when not valid } - if (buffer[i - 1] == '/') { - /* code */ + // fast forward if + for (; *(buffer + buffer_idx) == ' '; buffer_idx++) { } + if (buffer_idx >= buffer_size) { + // TODO (Komaromi): Do something when end of string + } + if (!isBaseLessNode) { + if (*(buffer + buffer_idx) != '<') { + // Node base is data + if (!element.CastParam(node_name, buffer, buffer_idx)) { + // TODO (Komaromi): Do something when not valid + } + } else { + // node base has childs + for (auto && child : element.childs_) { + detectNode(child, buffer, buffer_idx, buffer_size); + } + } + for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '<'; buffer_idx++) {// maybe use strstr() + } + if (buffer_idx >= buffer_size) { + // TODO (Komaromi): Do something when end of string + } + if (*(buffer + buffer_idx + 1) != '/') { + // TODO (Komaromi): Do something when wrong chield number + } + buffer_idx += 2; + if (!element.IsNameValid(node_name, buffer, buffer_idx)) { + // TODO (Komaromi): Do something when not valid + } + } + + +//########### Copy ##################### + + + // xml::XMLString node_name(buffer, 0); + // // fast forward for the first open bracket + // for (; **buffer_it != '\0' && **buffer_it != '<'; *buffer_it++) { + // } + // if (**buffer_it == '\0') { + // // TODO (Komaromi): Do something when end of string + // } + // *buffer_it++; + // // Validate nodes name + // if (!element.IsNameValid(node_name, buffer_it)) { + // // TODO (Komaromi): Do something when not valid + // } + // // Validate nodes params + // size_t numOfParam = 0; + // bool isBaseLessNode = false; + // bool isNoMoreParam = false; + // xml::XMLString node_param; + // while (!isNoMoreParam && **buffer_it != '\0') { + // // fast forward to the next non-space character + // for (; **buffer_it == ' '; *buffer_it++) { + // } + // if (**buffer_it == '\0') { + // // TODO (Komaromi): Do something when end of string + // } + // // Check for the nodes hadders end characters + // if (**buffer_it == '/' && *(*buffer_it + 1) == '>') { + // isBaseLessNode = true; + // isNoMoreParam = true; + // } else if (**buffer_it == '>') { + // isNoMoreParam = true; + // } else { + // // If not the end of the nodes hadder decode param + // if (!element.IsParamNameValid(node_param, buffer_it)) { + // // TODO (Komaromi): Do something when not valid + // } + // for (; **buffer_it != '\0' && **buffer_it != '"'; *buffer_it++) { + // } + // if (**buffer_it == '\0') { + // // TODO (Komaromi): Do something when end of string + // } + // *buffer_it++; + // if (!element.CastParam(node_param, buffer_it)) { + // // TODO (Komaromi): Do something when not valid + // } + // *buffer_it++; + // numOfParam++; + // } + // } + // for (; **buffer_it != '\0' && **buffer_it != '>'; *buffer_it++) { + // } + // if (**buffer_it == '\0') { + // // TODO (Komaromi): Do something when end of string + // } + // *buffer_it++; + // if (isBaseLessNode && numOfParam != element.params_.size()) { + // // TODO (Komaromi): Do something when not valid + // } + // // fast forward if + // for (; **buffer_it == ' '; *buffer_it++) { + // } + // if (**buffer_it == '\0') { + // // TODO (Komaromi): Do something when end of string + // } + // if (!isBaseLessNode) { + // if (**buffer_it != '<') { + // // Node base is data + // if (!element.CastParam(node_name, buffer_it)) { + // // TODO (Komaromi): Do something when not valid + // } + // } else { + // // node base has childs + // for (auto && child : element.childs_) { + // detectNode(child, buffer_it); + // } + // } + // for (; **buffer_it != '\0' && **buffer_it != '<'; *buffer_it++) {// maybe use strstr() + // } + // if (**buffer_it == '\0') { + // // TODO (Komaromi): Do something when end of string + // } + // if (*(*buffer_it + 1) != '/') { + // // TODO (Komaromi): Do something when wrong chield number + // } + // *buffer_it += 2; + // if (!element.IsNameValid(node_name, buffer_it)) { + // // TODO (Komaromi): Do something when not valid + // } + // } } } diff --git a/kuka_rsi_hw_interface/src/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_element.cpp index 4c543159..331e49ec 100644 --- a/kuka_rsi_hw_interface/src/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_element.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -22,9 +23,10 @@ namespace xml { -bool XMLElement::CastParam(XMLString key, char ** str_ptr) +bool XMLElement::CastParam(XMLString key, char * str_ptr, int & idx) { char key_c_str[key.length_ + 1] = {0}; + char ** end_ptr; strncpy(key_c_str, key.data_ptr_, key.length_); auto param_it = params_.find(key_c_str); @@ -33,13 +35,15 @@ bool XMLElement::CastParam(XMLString key, char ** str_ptr) switch (param_it->second.param_type_) { case XMLType::BOOL: case XMLType::LONG: - param_it->second.param_ = strtol(*str_ptr, str_ptr, 0); + param_it->second.param_ = strtol(str_ptr + idx, end_ptr, 0); + idx = (size_t)(str_ptr + idx - *end_ptr); break; case XMLType::DOUBLE: - param_it->second.param_ = strtod(*str_ptr, str_ptr); + param_it->second.param_ = strtod(str_ptr + idx, end_ptr); + idx = (size_t)(str_ptr + idx - *end_ptr); break; case XMLType::STRING: - param_it->second.param_ = castXMLString(str_ptr); + param_it->second.param_ = castXMLString(str_ptr, idx); break; default: return false; @@ -48,32 +52,66 @@ bool XMLElement::CastParam(XMLString key, char ** str_ptr) return ret_val; } -bool XMLElement::IsParamNameValid(XMLString & key, char ** str_ptr) +bool XMLElement::IsParamNameValid(XMLString & key, char * str_ptr, int & idx) { - key = castXMLString(str_ptr); + key = castXMLString(str_ptr, idx); char key_c_str[key.length_ + 1] = {0}; strncpy(key_c_str, key.data_ptr_, key.length_); return params_.find(key_c_str) != params_.end(); } -bool XMLElement::IsNameValid(XMLString & key, char ** str_ptr) +bool XMLElement::IsNameValid(XMLString & key, char * str_ptr, int & idx) { - key = castXMLString(str_ptr); + key = castXMLString(str_ptr, idx); if (name_.length() != key.length_) { return false; } return strncmp(name_.c_str(), key.data_ptr_, key.length_); } -XMLString XMLElement::castXMLString(char ** str_ptr) +XMLString XMLElement::castXMLString(char * str_ptr, int & idx) { - char * start_ptr = *str_ptr; + int start_idx = idx; for (; - **str_ptr != '\0' && **str_ptr != '"' && **str_ptr != ' ' && **str_ptr != '=' && - **str_ptr != '>' && **str_ptr != '/'; *str_ptr++) + *(str_ptr + idx) != '\0' && *(str_ptr + idx) != '"' && *(str_ptr + idx) != ' ' && + *(str_ptr + idx) != '=' && *(str_ptr + idx) != '>' && *(str_ptr + idx) != '/'; idx++) { } - return XMLString(start_ptr, (size_t)(*str_ptr - start_ptr)); + return XMLString(str_ptr + idx, (size_t)(idx - start_idx)); + +// ###### Copy ######### + +// char * start_ptr = *str_ptr; +// for (; +// **str_ptr != '\0' && **str_ptr != '"' && **str_ptr != ' ' && **str_ptr != '=' && +// **str_ptr != '>' && **str_ptr != '/'; *str_ptr++) +// { +// } +// return XMLString(start_ptr, (size_t)(*str_ptr - start_ptr)); +} + +bool XMLString::operator==(const XMLString & rhs) +{ + if (length_ != rhs.length_) { + return false; + } + return strncmp(data_ptr_, rhs.data_ptr_, length_); +} + +bool XMLString::operator==(const std::string & rhs) +{ + if (length_ != rhs.length()) { + return false; + } + return strncmp(rhs.c_str(), data_ptr_, length_); +} + +bool XMLString::operator==(const char * rhs) +{ + if (length_ != strlen(rhs)) { + return false; + } + return strncmp(rhs, data_ptr_, length_); } } From 306dcac006dc55b4aa8945221516ce9a03d2ff13 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Wed, 12 Jul 2023 13:44:57 +0200 Subject: [PATCH 40/97] decode algorithm without error handling --- .../rsi_command_handler.hpp | 4 +- .../kuka_rsi_hw_interface/udp_server.h | 2 +- .../kuka_rsi_hw_interface/xml_element.hpp | 12 ++-- .../src/rsi_command_handler.cpp | 67 ++----------------- kuka_rsi_hw_interface/src/xml_element.cpp | 8 +-- 5 files changed, 17 insertions(+), 76 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 76e1921f..361aae30 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -55,14 +55,14 @@ class RSICommandHandler // void updateDataPlace(); // void updateData(); static void detectNode( - xml::XMLElement & element, char * buffer, int & buffer_idx, + xml::XMLElement & element, const char * const buffer, int & buffer_idx, const size_t buffer_size); public: RSICommandHandler(); ~RSICommandHandler() = default; - bool Decode(char * buffer, size_t buffer_size); + bool Decode(const char * const buffer, size_t buffer_size); void Encode(char * buffer); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index cbd255dd..4a2c7fe7 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -153,7 +153,7 @@ class UDPServer } } - strncpy(buffer, buffer_, BUFFER_SIZE); + strncpy(buffer, buffer_, bytes); return bytes; } diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp index 60c23e20..91080a0f 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp @@ -33,9 +33,9 @@ enum class XMLType : size_t struct XMLString { - char * data_ptr_; + const char * data_ptr_; size_t length_; - XMLString(char * data_ptr = nullptr, size_t length = 0) + XMLString(const char * data_ptr = nullptr, size_t length = 0) : data_ptr_(data_ptr), length_(length) {} bool operator==(const XMLString & rhs); bool operator==(const std::string & rhs); @@ -61,7 +61,7 @@ struct XMLParam class XMLElement { private: - static XMLString castXMLString(char * str_ptr, int & idx); + static XMLString castXMLString(const char * const str_ptr, int & idx); public: std::string name_; @@ -73,10 +73,10 @@ class XMLElement XMLElement() = default; ~XMLElement() = default; - bool CastParam(XMLString key, char * str_ptr, int & idx); + bool CastParam(XMLString key, const char * const str_ptr, int & idx); - bool IsParamNameValid(XMLString & key, char * str_ptr, int & idx); - bool IsNameValid(XMLString & key, char * str_ptr, int & idx); + bool IsParamNameValid(XMLString & key, const char * const str_ptr, int & idx); + bool IsNameValid(XMLString & key, const char * const str_ptr, int & idx); // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used template diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 11ad8d73..0ad7e891 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -36,80 +36,21 @@ RSICommandHandler::RSICommandHandler() state_data_structure_.childs_.emplace_back(Override); } -bool RSICommandHandler::Decode(char * buffer, size_t buffer_size) +bool RSICommandHandler::Decode(const char * const buffer, const size_t buffer_size) { int buffer_idx = 0; detectNode(state_data_structure_, buffer, buffer_idx, buffer_size); - - // xml::XMLString decoded_str(buffer, 0); - // char * buffer_it = buffer; - // size_t node_depth = 0; - - // // fast forward for the first open bracket - // for (; *buffer_it != '\0' && *buffer_it != '<'; buffer_it++) { - // } - - // bool isBaseLessNode = false; - // // Check one node - // // Get string - // buffer_it++; - // if (!state_data_structure_.IsNameValid(decoded_str, buffer_it)) { - // // Do something when failed - // } - // string_start_ptr = &buffer[i]; - // for (; i < buffer_size || buffer[i] != ' '; i++, string_length++) { - // } - // // Compare string with char* to check node name - - // // Check params - // bool isNoMoreParam = false; - // while (!isNoMoreParam || i < buffer_size) { - // // Serching for params - // // goes to the next atribute - // for (; i < buffer_size || buffer[i] == ' '; i++) { - // } - - // // checks for the node hadder end characters - // if (buffer[i] == '/' && buffer[i + 1] == '>') { - // isBaseLessNode = true; - // isNoMoreParam = true; - // } else if (buffer[i] == '>') { - // isNoMoreParam = true; - // } else { - // string_start_ptr = &buffer[i]; - // string_length = 0; - // for (; i < buffer_size || buffer[i] != '='; i++, string_length++) { - // } - // // check the field is present if it is present save the data else go to next param - // // move to param start bracket - // for (; i < buffer_size || buffer[i] != '"' || buffer[i] != '\''; i++) { - // } - - // i++; // move to params first character - // string_start_ptr = &buffer[i]; - // string_length = 0; - // for (; i < buffer_size || buffer[i] != '"' || buffer[i] != '\''; i++, string_length++) { - // } - // // save param - - // } - - - // } - // if (buffer[i - 1] == '/') { - // /* code */ - // } } void RSICommandHandler::detectNode( - xml::XMLElement & element, char * buffer, int & buffer_idx, const size_t buffer_size) + xml::XMLElement & element, const char * const buffer, int & buffer_idx, const size_t buffer_size) { xml::XMLString node_name(buffer, 0); // fast forward for the first open bracket for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '<'; buffer_idx++) { } - if (buffer_idx < buffer_size == '\0') { - // TODO (Komaromi): Do something when end of string + if (buffer_idx < buffer_size) { + // TODO (Komaromi): Do something when not valid } buffer_idx++; // Validate nodes name diff --git a/kuka_rsi_hw_interface/src/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_element.cpp index 331e49ec..a874f791 100644 --- a/kuka_rsi_hw_interface/src/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_element.cpp @@ -23,7 +23,7 @@ namespace xml { -bool XMLElement::CastParam(XMLString key, char * str_ptr, int & idx) +bool XMLElement::CastParam(XMLString key, const char * const str_ptr, int & idx) { char key_c_str[key.length_ + 1] = {0}; char ** end_ptr; @@ -52,7 +52,7 @@ bool XMLElement::CastParam(XMLString key, char * str_ptr, int & idx) return ret_val; } -bool XMLElement::IsParamNameValid(XMLString & key, char * str_ptr, int & idx) +bool XMLElement::IsParamNameValid(XMLString & key, const char * const str_ptr, int & idx) { key = castXMLString(str_ptr, idx); char key_c_str[key.length_ + 1] = {0}; @@ -61,7 +61,7 @@ bool XMLElement::IsParamNameValid(XMLString & key, char * str_ptr, int & idx) } -bool XMLElement::IsNameValid(XMLString & key, char * str_ptr, int & idx) +bool XMLElement::IsNameValid(XMLString & key, const char * const str_ptr, int & idx) { key = castXMLString(str_ptr, idx); if (name_.length() != key.length_) { @@ -70,7 +70,7 @@ bool XMLElement::IsNameValid(XMLString & key, char * str_ptr, int & idx) return strncmp(name_.c_str(), key.data_ptr_, key.length_); } -XMLString XMLElement::castXMLString(char * str_ptr, int & idx) +XMLString XMLElement::castXMLString(const char * const str_ptr, int & idx) { int start_idx = idx; for (; From 94b781a55f4ca6e218bd7a85b6c46de98c4ff432 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Wed, 19 Jul 2023 18:51:56 +0200 Subject: [PATCH 41/97] working xml decoder function few changes still needed --- kuka_rsi_hw_interface/src/xml_element.cpp | 117 ------------ xml_handler/CMakeLists.txt | 32 ++++ xml_handler/decodeTest.cpp | 22 +++ .../rsi_command_handler.cpp | 171 +++++++++++++----- .../rsi_command_handler.hpp | 10 +- xml_handler/xml_element.cpp | 158 ++++++++++++++++ .../xml_element.hpp | 19 +- 7 files changed, 353 insertions(+), 176 deletions(-) delete mode 100644 kuka_rsi_hw_interface/src/xml_element.cpp create mode 100644 xml_handler/CMakeLists.txt create mode 100644 xml_handler/decodeTest.cpp rename {kuka_rsi_hw_interface/src => xml_handler}/rsi_command_handler.cpp (50%) rename {kuka_rsi_hw_interface/include/kuka_rsi_hw_interface => xml_handler}/rsi_command_handler.hpp (94%) create mode 100644 xml_handler/xml_element.cpp rename {kuka_rsi_hw_interface/include/kuka_rsi_hw_interface => xml_handler}/xml_element.hpp (83%) diff --git a/kuka_rsi_hw_interface/src/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_element.cpp deleted file mode 100644 index a874f791..00000000 --- a/kuka_rsi_hw_interface/src/xml_element.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2023 Komáromi Sándor -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include - -#include "xml_element.hpp" - -namespace xml -{ -bool XMLElement::CastParam(XMLString key, const char * const str_ptr, int & idx) -{ - char key_c_str[key.length_ + 1] = {0}; - char ** end_ptr; - strncpy(key_c_str, key.data_ptr_, key.length_); - - auto param_it = params_.find(key_c_str); - auto ret_val = param_it != params_.end(); - if (ret_val) { - switch (param_it->second.param_type_) { - case XMLType::BOOL: - case XMLType::LONG: - param_it->second.param_ = strtol(str_ptr + idx, end_ptr, 0); - idx = (size_t)(str_ptr + idx - *end_ptr); - break; - case XMLType::DOUBLE: - param_it->second.param_ = strtod(str_ptr + idx, end_ptr); - idx = (size_t)(str_ptr + idx - *end_ptr); - break; - case XMLType::STRING: - param_it->second.param_ = castXMLString(str_ptr, idx); - break; - default: - return false; - } - } - return ret_val; -} - -bool XMLElement::IsParamNameValid(XMLString & key, const char * const str_ptr, int & idx) -{ - key = castXMLString(str_ptr, idx); - char key_c_str[key.length_ + 1] = {0}; - strncpy(key_c_str, key.data_ptr_, key.length_); - return params_.find(key_c_str) != params_.end(); -} - - -bool XMLElement::IsNameValid(XMLString & key, const char * const str_ptr, int & idx) -{ - key = castXMLString(str_ptr, idx); - if (name_.length() != key.length_) { - return false; - } - return strncmp(name_.c_str(), key.data_ptr_, key.length_); -} - -XMLString XMLElement::castXMLString(const char * const str_ptr, int & idx) -{ - int start_idx = idx; - for (; - *(str_ptr + idx) != '\0' && *(str_ptr + idx) != '"' && *(str_ptr + idx) != ' ' && - *(str_ptr + idx) != '=' && *(str_ptr + idx) != '>' && *(str_ptr + idx) != '/'; idx++) - { - } - return XMLString(str_ptr + idx, (size_t)(idx - start_idx)); - -// ###### Copy ######### - -// char * start_ptr = *str_ptr; -// for (; -// **str_ptr != '\0' && **str_ptr != '"' && **str_ptr != ' ' && **str_ptr != '=' && -// **str_ptr != '>' && **str_ptr != '/'; *str_ptr++) -// { -// } -// return XMLString(start_ptr, (size_t)(*str_ptr - start_ptr)); -} - -bool XMLString::operator==(const XMLString & rhs) -{ - if (length_ != rhs.length_) { - return false; - } - return strncmp(data_ptr_, rhs.data_ptr_, length_); -} - -bool XMLString::operator==(const std::string & rhs) -{ - if (length_ != rhs.length()) { - return false; - } - return strncmp(rhs.c_str(), data_ptr_, length_); -} - -bool XMLString::operator==(const char * rhs) -{ - if (length_ != strlen(rhs)) { - return false; - } - return strncmp(rhs, data_ptr_, length_); -} -} diff --git a/xml_handler/CMakeLists.txt b/xml_handler/CMakeLists.txt new file mode 100644 index 00000000..5ddb7362 --- /dev/null +++ b/xml_handler/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(xml_handler) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_library(${PROJECT_NAME} SHARED + rsi_command_handler.cpp + xml_element.cpp +) + +add_executable(decode_test + decodeTest.cpp) + +target_link_libraries(decode_test PUBLIC xml_handler) + +install(TARGETS ${PROJECT_NAME} decode_test + DESTINATION lib/${PROJECT_NAME}) + +# # EXPORTS +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME}) \ No newline at end of file diff --git a/xml_handler/decodeTest.cpp b/xml_handler/decodeTest.cpp new file mode 100644 index 00000000..60974a70 --- /dev/null +++ b/xml_handler/decodeTest.cpp @@ -0,0 +1,22 @@ +#include "rsi_command_handler.hpp" + +#include + +int main(int argc, char const * argv[]) +{ + kuka_rsi_hw_interface::RSICommandHandler handler; + char xml_state[1024] = + { + "55"}; + if (!handler.Decode(xml_state, 1024)) { + std::cout << "decode failed" << std::endl; + return -1; + } + + + std::cout << "decode successful" << std::endl; + + handler.state_data_structure_.operator<<(std::cout) << std::endl; + + return 0; +} diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/xml_handler/rsi_command_handler.cpp similarity index 50% rename from kuka_rsi_hw_interface/src/rsi_command_handler.cpp rename to xml_handler/rsi_command_handler.cpp index 0ad7e891..4a91266c 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/xml_handler/rsi_command_handler.cpp @@ -13,6 +13,9 @@ // limitations under the License. #include +#include +#include +#include #include "rsi_command_handler.hpp" @@ -30,109 +33,159 @@ RSICommandHandler::RSICommandHandler() Out.params_["03"] = xml::XMLParam(xml::XMLType::BOOL); Out.params_["04"] = xml::XMLParam(xml::XMLType::BOOL); Out.params_["05"] = xml::XMLParam(xml::XMLType::BOOL); - xml::XMLElement Override("Overrride"); + xml::XMLElement Override("Override"); Override.params_["Override"] = xml::XMLParam(xml::XMLType::LONG); state_data_structure_.childs_.emplace_back(Out); state_data_structure_.childs_.emplace_back(Override); } -bool RSICommandHandler::Decode(const char * const buffer, const size_t buffer_size) +bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) { - int buffer_idx = 0; - detectNode(state_data_structure_, buffer, buffer_idx, buffer_size); + std::cout << "Decode started" << std::endl; + auto buffer_it = buffer; + try { + detectNode(state_data_structure_, buffer, buffer_it, buffer_size); + } catch (const std::exception & e) { + std::cout << "### ERROR ###" << std::endl; + std::cerr << e.what() << '\n'; + return false; + } + std::cout << "Decode finished" << std::endl; + return true; } void RSICommandHandler::detectNode( - xml::XMLElement & element, const char * const buffer, int & buffer_idx, const size_t buffer_size) + xml::XMLElement & element, char * const buffer, char * & buffer_it, + const size_t buffer_size) { xml::XMLString node_name(buffer, 0); // fast forward for the first open bracket - for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '<'; buffer_idx++) { - } - if (buffer_idx < buffer_size) { - // TODO (Komaromi): Do something when not valid + for (; *buffer_it != '<'; buffer_it++) { + if ((int)(buffer_it - buffer) >= (int)buffer_size) { + throw std::range_error("Out of the buffers range"); + } } - buffer_idx++; + buffer_it++; // Validate nodes name - if (!element.IsNameValid(node_name, buffer, buffer_idx)) { - // TODO (Komaromi): Do something when not valid + if (!element.IsNameValid(node_name, buffer_it)) { + char err_msg[150] = ""; + strcat(err_msg, "The detected name ("); + if (node_name.length_ > 0) { + strcat(err_msg, node_name.c_str()); + } + strcat(err_msg, ") does not match the elements name "); + strcat(err_msg, element.name_.c_str()); + throw std::logic_error(err_msg); } + std::cout << "Name found: " << node_name.c_str() << std::endl; // Validate nodes params size_t numOfParam = 0; bool isBaseLessNode = false; bool isNoMoreParam = false; xml::XMLString node_param; - while (!isNoMoreParam && buffer_idx < buffer_size) { + while (!isNoMoreParam && buffer_it - buffer < buffer_size) { // fast forward to the next non-space character - for (; *(buffer + buffer_idx) == ' '; buffer_idx++) { + for (; *buffer_it == ' '; buffer_it++) { } - if (buffer_idx >= buffer_size) { - // TODO (Komaromi): Do something when end of string + if (buffer_it - buffer >= buffer_size) { + throw std::range_error("Out of the buffers range"); } // Check for the nodes hadders end characters - if (*(buffer + buffer_idx) == '/' && *(buffer + buffer_idx + 1) == '>') { + if (*buffer_it == '/' && *(buffer_it + 1) == '>') { isBaseLessNode = true; isNoMoreParam = true; - } else if (*(buffer + buffer_idx) == '>') { + } else if (*buffer_it == '>') { isNoMoreParam = true; } else { - // If not the end of the nodes hadder decode param - if (!element.IsParamNameValid(node_param, buffer, buffer_idx)) { - // TODO (Komaromi): Do something when not valid + if (!element.IsParamNameValid(node_param, buffer_it)) { + char err_msg[100] = ""; + strcat(err_msg, "The detected parameter ("); + strcat(err_msg, node_param.c_str()); + strcat(err_msg, ") dose not match any of the "); + strcat(err_msg, element.name_.c_str()); + strcat(err_msg, "elements parameters"); + throw std::logic_error(err_msg); } - for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '"'; buffer_idx++) { + for (; buffer_it - buffer < buffer_size && *buffer_it != '"'; buffer_it++) { } - if (buffer_idx >= buffer_size) { - // TODO (Komaromi): Do something when end of string + if (buffer_it - buffer >= buffer_size) { + throw std::range_error("Out of the buffers range"); } - buffer_idx++; - if (!element.CastParam(node_param, buffer, buffer_idx)) { - // TODO (Komaromi): Do something when not valid + buffer_it++; + if (!element.CastParam(node_param, buffer_it)) { + char err_msg[100] = ""; + strcat(err_msg, "The not cast the "); + strcat(err_msg, node_param.c_str()); + strcat(err_msg, " param into the "); + strcat(err_msg, element.name_.c_str()); + strcat(err_msg, " elements parameter list"); + throw std::logic_error(err_msg); } - buffer_idx++; + buffer_it++; numOfParam++; } } - for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '>'; buffer_idx++) { + for (; buffer_it - buffer < buffer_size && *buffer_it != '>'; buffer_it++) { } - if (buffer_idx >= buffer_size) { - // TODO (Komaromi): Do something when end of string + if (buffer_it - buffer >= buffer_size) { + throw std::range_error("Out of the buffers range"); } - buffer_idx++; + buffer_it++; if (isBaseLessNode && numOfParam != element.params_.size()) { - // TODO (Komaromi): Do something when not valid + char err_msg[100] = ""; + strcat(err_msg, "The number of parameters found "); + strcat(err_msg, std::to_string(numOfParam).c_str()); + strcat(err_msg, " param into the "); + strcat(err_msg, element.name_.c_str()); + strcat(err_msg, " elements parameter list"); + throw std::logic_error(err_msg); } // fast forward if - for (; *(buffer + buffer_idx) == ' '; buffer_idx++) { + for (; *buffer_it == ' '; buffer_it++) { } - if (buffer_idx >= buffer_size) { - // TODO (Komaromi): Do something when end of string + if (buffer_it - buffer >= buffer_size) { + throw std::range_error("Out of the buffers range"); } if (!isBaseLessNode) { - if (*(buffer + buffer_idx) != '<') { + if (*buffer_it != '<') { // Node base is data - if (!element.CastParam(node_name, buffer, buffer_idx)) { - // TODO (Komaromi): Do something when not valid + if (!element.CastParam(node_name, buffer_it)) { + char err_msg[100] = ""; + strcat(err_msg, "The not cast the "); + strcat(err_msg, node_param.c_str()); + strcat(err_msg, " param into the "); + strcat(err_msg, element.name_.c_str()); + strcat(err_msg, " elements parameter list"); + throw std::logic_error(err_msg); } } else { // node base has childs for (auto && child : element.childs_) { - detectNode(child, buffer, buffer_idx, buffer_size); + detectNode(child, buffer, buffer_it, buffer_size); } } - for (; buffer_idx < buffer_size && *(buffer + buffer_idx) != '<'; buffer_idx++) {// maybe use strstr() + for (; buffer_it - buffer < buffer_size && *buffer_it != '<'; buffer_it++) {// maybe use strstr() } - if (buffer_idx >= buffer_size) { - // TODO (Komaromi): Do something when end of string + if (buffer_it - buffer >= buffer_size) { + throw std::range_error("Out of the buffers range"); } - if (*(buffer + buffer_idx + 1) != '/') { - // TODO (Komaromi): Do something when wrong chield number + if (*(buffer_it + 1) != '/') { + char err_msg[100] = ""; + strcat(err_msg, "Start of an end Node, where there should be none. Error came in the "); + strcat(err_msg, element.name_.c_str()); + strcat(err_msg, " node."); + throw std::logic_error(err_msg); } - buffer_idx += 2; - if (!element.IsNameValid(node_name, buffer, buffer_idx)) { - // TODO (Komaromi): Do something when not valid + buffer_it += 2; + if (!element.IsNameValid(node_name, buffer_it)) { + char err_msg[100] = ""; + strcat(err_msg, "The detected name ("); + strcat(err_msg, node_name.c_str()); + strcat(err_msg, ") does not match the elements name "); + strcat(err_msg, element.name_.c_str()); + throw std::logic_error(err_msg); } } @@ -231,4 +284,26 @@ void RSICommandHandler::detectNode( // } // } } +// int main() +// { +// RSICommandHandler commandHandler(); +// char xml_state[1024] = +// { +// ""}; +// commandHandler.Decode(); +// } } +// // Later this should be defined by the rsi xml +// state_data_structure_.name_ = "Rob"; +// state_data_structure_.params_["TYPE"] = xml::XMLParam(xml::XMLType::STRING); +// // how to get string: std::get(command_data_structure_.params_["TYPE"]).first +// xml::XMLElement Out("Out"); +// Out.params_["01"] = xml::XMLParam(xml::XMLType::BOOL); +// Out.params_["02"] = xml::XMLParam(xml::XMLType::BOOL); +// Out.params_["03"] = xml::XMLParam(xml::XMLType::BOOL); +// Out.params_["04"] = xml::XMLParam(xml::XMLType::BOOL); +// Out.params_["05"] = xml::XMLParam(xml::XMLType::BOOL); +// xml::XMLElement Override("Overrride"); +// Override.params_["Override"] = xml::XMLParam(xml::XMLType::LONG); +// state_data_structure_.childs_.emplace_back(Out); +// state_data_structure_.childs_.emplace_back(Override); diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/xml_handler/rsi_command_handler.hpp similarity index 94% rename from kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp rename to xml_handler/rsi_command_handler.hpp index 361aae30..69599887 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/xml_handler/rsi_command_handler.hpp @@ -49,20 +49,20 @@ namespace kuka_rsi_hw_interface class RSICommandHandler { private: - xml::XMLElement command_data_structure_; - xml::XMLElement state_data_structure_; - // void updateDataPlace(); // void updateData(); static void detectNode( - xml::XMLElement & element, const char * const buffer, int & buffer_idx, + xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size); public: + xml::XMLElement command_data_structure_; + xml::XMLElement state_data_structure_; + RSICommandHandler(); ~RSICommandHandler() = default; - bool Decode(const char * const buffer, size_t buffer_size); + bool Decode(char * const buffer, const size_t buffer_size); void Encode(char * buffer); }; } // namespace kuka_rsi_hw_interface diff --git a/xml_handler/xml_element.cpp b/xml_handler/xml_element.cpp new file mode 100644 index 00000000..02a018f8 --- /dev/null +++ b/xml_handler/xml_element.cpp @@ -0,0 +1,158 @@ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "xml_element.hpp" + +namespace xml +{ +bool XMLElement::CastParam(XMLString key, char * & str_ptr) +{ + char key_c_str[key.length_ + 1] = {0}; + strncpy(key_c_str, key.data_ptr_, key.length_); + + auto param_it = params_.find(key_c_str); + auto ret_val = param_it != params_.end(); + if (ret_val) { + switch (param_it->second.param_type_) { + case XMLType::BOOL: + param_it->second.param_ = (bool)strtol(str_ptr, &str_ptr, 0); + break; + case XMLType::LONG: + param_it->second.param_ = strtol(str_ptr, &str_ptr, 0); + break; + case XMLType::DOUBLE: + param_it->second.param_ = strtod(str_ptr, &str_ptr); + break; + case XMLType::STRING: + param_it->second.param_ = castXMLString(str_ptr); + break; + default: + return false; + } + } + return ret_val; +} + +bool XMLElement::IsParamNameValid(XMLString & key, char * & str_ptr) +{ + key = castXMLString(str_ptr); + char key_c_str[key.length_ + 1] = {0}; + strncpy(key_c_str, key.data_ptr_, key.length_); + return params_.find(key_c_str) != params_.end(); +} + + +bool XMLElement::IsNameValid(XMLString & key, char * & str_ptr) +{ + key = castXMLString(str_ptr); + if (name_.length() != key.length_) { + std::cout << "Registered names length is: " << name_.length() << ", Keys length is: " << + key.length_ << std::endl; + return false; + } + return strncmp(name_.c_str(), key.data_ptr_, key.length_) == 0; +} + +XMLString XMLElement::castXMLString(char * & str_ptr) +{ + auto start_ref = str_ptr; + for (; + *str_ptr != '\0' && *str_ptr != '"' && *str_ptr != ' ' && + *str_ptr != '=' && *str_ptr != '>' && *str_ptr != '/' && *str_ptr != '<'; str_ptr++) + { + std::cout << *str_ptr; + } + std::cout << std::endl; + auto data = XMLString( + start_ref, (size_t)(str_ptr - start_ref)); + std::cout << "Cast length: " << data.length_ << std::endl; + std::cout << "Cast Data: " << data.c_str() << std::endl; + std::cout << "Cast all Data: " << data.data_ptr_ << std::endl; + return data; +} + +std::ostream & XMLElement::operator<<(std::ostream & out) +{ + out << "Element name: " << this->name_ << "\n"; + for (auto && param : params_) { + out << " - " << param.first << ": "; + switch (param.second.param_type_) { + case XMLType::BOOL: + out << std::get<0>(param.second.param_) << "\n"; + break; + case XMLType::LONG: + out << std::get<1>(param.second.param_) << "\n"; + break; + case XMLType::DOUBLE: + out << std::get<2>(param.second.param_) << "\n"; + break; + case XMLType::STRING: + { + out << std::get<3>(param.second.param_).c_str() << "\n"; + } + break; + + default: + break; + } + } + for (auto && child : childs_) { + child.operator<<(out) << "\n"; + } + return out; +} + +const char * XMLString::c_str() +{ + if (length_ < 49) { + strncpy(data_c_str_, data_ptr_, length_); + data_c_str_[length_] = '\0'; + return data_c_str_; + } + throw std::range_error("C string copy out of range"); +} + +bool XMLString::operator==(const XMLString & rhs) +{ + if (length_ != rhs.length_) { + return false; + } + return strncmp(data_ptr_, rhs.data_ptr_, length_); +} + +bool XMLString::operator==(const std::string & rhs) +{ + if (length_ != rhs.length()) { + return false; + } + return strncmp(rhs.c_str(), data_ptr_, length_); +} + +bool XMLString::operator==(const char * rhs) +{ + if (length_ != strlen(rhs)) { + return false; + } + return strncmp(rhs, data_ptr_, length_); +} +} diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp b/xml_handler/xml_element.hpp similarity index 83% rename from kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp rename to xml_handler/xml_element.hpp index 91080a0f..3064cc06 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/xml_element.hpp +++ b/xml_handler/xml_element.hpp @@ -19,6 +19,9 @@ #include #include #include +#include +#include + namespace xml { @@ -34,9 +37,11 @@ enum class XMLType : size_t struct XMLString { const char * data_ptr_; + char data_c_str_[50] = ""; size_t length_; XMLString(const char * data_ptr = nullptr, size_t length = 0) : data_ptr_(data_ptr), length_(length) {} + const char * c_str(); bool operator==(const XMLString & rhs); bool operator==(const std::string & rhs); bool operator==(const char * rhs); @@ -46,6 +51,7 @@ struct XMLParam { XMLType param_type_; std::variant param_; + XMLParam() = default; XMLParam(XMLType type) : param_type_(type) {} }; @@ -61,7 +67,7 @@ struct XMLParam class XMLElement { private: - static XMLString castXMLString(const char * const str_ptr, int & idx); + static XMLString castXMLString(char * & str_ptr); public: std::string name_; @@ -73,10 +79,11 @@ class XMLElement XMLElement() = default; ~XMLElement() = default; - bool CastParam(XMLString key, const char * const str_ptr, int & idx); + bool CastParam(XMLString key, char * & str_ptr); + bool IsParamNameValid(XMLString & key, char * & str_ptr); + bool IsNameValid(XMLString & key, char * & str_ptr); - bool IsParamNameValid(XMLString & key, const char * const str_ptr, int & idx); - bool IsNameValid(XMLString & key, const char * const str_ptr, int & idx); + std::ostream & operator<<(std::ostream & out); // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used template @@ -85,7 +92,7 @@ class XMLElement auto param_it = params_.find(key); if (param_it != params_.end()) { if (std::is_same() || std::is_same() || - std::is_same() || std::is_same) + std::is_same() || std::is_same()) { param = std::get(param_it->second.param_); return true; @@ -101,7 +108,7 @@ class XMLElement auto param_it = params_.find(key); if (param_it != params_.end()) { if (std::is_same() || std::is_same() || - std::is_same() || std::is_same) + std::is_same() || std::is_same()) { param_it->second.param_ = param; return true; From 7e9fda52e29f05dbd28386003651431d21a546bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 21 Jul 2023 16:08:58 +0200 Subject: [PATCH 42/97] map find with xmlString without allocation --- xml_handler/decodeTest.cpp | 25 ++- xml_handler/rsi_command_handler.cpp | 237 ++++++++-------------------- xml_handler/rsi_command_handler.hpp | 28 +++- xml_handler/xml_element.cpp | 84 ++++++---- xml_handler/xml_element.hpp | 43 +++-- 5 files changed, 189 insertions(+), 228 deletions(-) diff --git a/xml_handler/decodeTest.cpp b/xml_handler/decodeTest.cpp index 60974a70..cb52532d 100644 --- a/xml_handler/decodeTest.cpp +++ b/xml_handler/decodeTest.cpp @@ -16,7 +16,30 @@ int main(int argc, char const * argv[]) std::cout << "decode successful" << std::endl; - handler.state_data_structure_.operator<<(std::cout) << std::endl; + handler.GetState().operator<<(std::cout) << std::endl; + bool param; + handler.GetState().GetElement("Out").GetParam("01", param); + + std::cout << "encode data fill" << std::endl; + std::string msg = "Hello World!"; + xml::XMLString xmlStr(msg.c_str(), msg.length() + 1); + handler.SetCommandParam("Sen", "Type", xmlStr); + double x = 5.2; + double y = 6; + double z = 3.2; + double a = 12; + double b = 7; + double c = 4.35; + long DiO = 12012141212323123; + handler.SetCommandParam("RKorr", "X", x); + handler.SetCommandParam("RKorr", "Y", y); + handler.SetCommandParam("RKorr", "Z", z); + handler.SetCommandParam("RKorr", "A", a); + handler.SetCommandParam("RKorr", "B", b); + handler.SetCommandParam("RKorr", "C", c); + handler.SetCommandParam("DiO", "DiO", DiO); + + handler.GetCommand().operator<<(std::cout) << std::endl; return 0; } diff --git a/xml_handler/rsi_command_handler.cpp b/xml_handler/rsi_command_handler.cpp index 4a91266c..06f2e2ce 100644 --- a/xml_handler/rsi_command_handler.cpp +++ b/xml_handler/rsi_command_handler.cpp @@ -22,9 +22,9 @@ namespace kuka_rsi_hw_interface { RSICommandHandler::RSICommandHandler() +: state_data_structure_("Rob"), command_data_structure_("Sen"), err_buf_("") { // Later this should be defined by the rsi xml - state_data_structure_.name_ = "Rob"; state_data_structure_.params_["TYPE"] = xml::XMLParam(xml::XMLType::STRING); // how to get string: std::get(command_data_structure_.params_["TYPE"]).first xml::XMLElement Out("Out"); @@ -37,6 +37,19 @@ RSICommandHandler::RSICommandHandler() Override.params_["Override"] = xml::XMLParam(xml::XMLType::LONG); state_data_structure_.childs_.emplace_back(Out); state_data_structure_.childs_.emplace_back(Override); + + command_data_structure_.params_["Type"] = xml::XMLParam(xml::XMLType::STRING); + xml::XMLElement RKorr("RKorr"); + RKorr.params_["X"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.params_["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.params_["Z"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.params_["A"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.params_["B"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.params_["C"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement DiO("DiO"); + DiO.params_["DiO"] = xml::XMLParam(xml::XMLType::LONG); + command_data_structure_.childs_.emplace_back(RKorr); + command_data_structure_.childs_.emplace_back(DiO); } bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) @@ -68,27 +81,25 @@ void RSICommandHandler::detectNode( buffer_it++; // Validate nodes name if (!element.IsNameValid(node_name, buffer_it)) { - char err_msg[150] = ""; - strcat(err_msg, "The detected name ("); - if (node_name.length_ > 0) { - strcat(err_msg, node_name.c_str()); - } - strcat(err_msg, ") does not match the elements name "); - strcat(err_msg, element.name_.c_str()); - throw std::logic_error(err_msg); + auto i = std::sprintf(err_buf_, "The detected name ("); + i = std::snprintf(err_buf_ + i, node_name.length_, "%s", node_name.data_ptr_); + i = std::sprintf(err_buf_, ") does not match the elements name %s.", element.name_.c_str()); + throw std::logic_error(err_buf_); } - std::cout << "Name found: " << node_name.c_str() << std::endl; + // char str[node_name.length_ + 1]; + // std::snprintf(str, node_name.length_ + 1, "%s", node_name.data_ptr_); + // std::cout << "Name found: " << str << std::endl; // Validate nodes params size_t numOfParam = 0; bool isBaseLessNode = false; bool isNoMoreParam = false; xml::XMLString node_param; - while (!isNoMoreParam && buffer_it - buffer < buffer_size) { + while (!isNoMoreParam && (size_t)(buffer_it - buffer) < buffer_size) { // fast forward to the next non-space character for (; *buffer_it == ' '; buffer_it++) { } - if (buffer_it - buffer >= buffer_size) { + if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } // Check for the nodes hadders end characters @@ -99,66 +110,61 @@ void RSICommandHandler::detectNode( isNoMoreParam = true; } else { if (!element.IsParamNameValid(node_param, buffer_it)) { - char err_msg[100] = ""; - strcat(err_msg, "The detected parameter ("); - strcat(err_msg, node_param.c_str()); - strcat(err_msg, ") dose not match any of the "); - strcat(err_msg, element.name_.c_str()); - strcat(err_msg, "elements parameters"); - throw std::logic_error(err_msg); + auto i = std::sprintf(err_buf_, "The detected parameter ("); + i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_name.data_ptr_); + std::sprintf( + err_buf_ + i, ") dose not match with any of the %s elements parameters.", + element.name_.c_str()); + throw std::logic_error(err_buf_); } - for (; buffer_it - buffer < buffer_size && *buffer_it != '"'; buffer_it++) { + for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '"'; buffer_it++) { } - if (buffer_it - buffer >= buffer_size) { + if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it++; if (!element.CastParam(node_param, buffer_it)) { - char err_msg[100] = ""; - strcat(err_msg, "The not cast the "); - strcat(err_msg, node_param.c_str()); - strcat(err_msg, " param into the "); - strcat(err_msg, element.name_.c_str()); - strcat(err_msg, " elements parameter list"); - throw std::logic_error(err_msg); + auto i = std::sprintf(err_buf_, "The not cast the "); + i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_param.data_ptr_); + std::sprintf( + err_buf_ + i, " param into the %s elements parameter list.", + element.name_.c_str()); + throw std::logic_error(err_buf_); } buffer_it++; numOfParam++; } } - for (; buffer_it - buffer < buffer_size && *buffer_it != '>'; buffer_it++) { + for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '>'; buffer_it++) { } - if (buffer_it - buffer >= buffer_size) { + if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it++; if (isBaseLessNode && numOfParam != element.params_.size()) { - char err_msg[100] = ""; - strcat(err_msg, "The number of parameters found "); - strcat(err_msg, std::to_string(numOfParam).c_str()); - strcat(err_msg, " param into the "); - strcat(err_msg, element.name_.c_str()); - strcat(err_msg, " elements parameter list"); - throw std::logic_error(err_msg); + std::sprintf( + err_buf_, + "%lu parameter found it does not match with %s elements parameter list size.", numOfParam, + element.name_.c_str()); + throw std::logic_error(err_buf_); } // fast forward if for (; *buffer_it == ' '; buffer_it++) { } - if (buffer_it - buffer >= buffer_size) { + if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } if (!isBaseLessNode) { if (*buffer_it != '<') { // Node base is data if (!element.CastParam(node_name, buffer_it)) { - char err_msg[100] = ""; - strcat(err_msg, "The not cast the "); - strcat(err_msg, node_param.c_str()); - strcat(err_msg, " param into the "); - strcat(err_msg, element.name_.c_str()); - strcat(err_msg, " elements parameter list"); - throw std::logic_error(err_msg); + auto i = std::sprintf(err_buf_, "Could not cast the "); + i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_param.data_ptr_); + std::sprintf( + err_buf_ + i, " parameter into the %s elements parameter list", + element.name_.c_str()); + throw std::logic_error(err_buf_); } } else { // node base has childs @@ -166,144 +172,25 @@ void RSICommandHandler::detectNode( detectNode(child, buffer, buffer_it, buffer_size); } } - for (; buffer_it - buffer < buffer_size && *buffer_it != '<'; buffer_it++) {// maybe use strstr() + for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '<'; buffer_it++) { } - if (buffer_it - buffer >= buffer_size) { + if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } if (*(buffer_it + 1) != '/') { - char err_msg[100] = ""; - strcat(err_msg, "Start of an end Node, where there should be none. Error came in the "); - strcat(err_msg, element.name_.c_str()); - strcat(err_msg, " node."); - throw std::logic_error(err_msg); + std::sprintf( + err_buf_, + "Start of an end Node, where there should be none. Error came in the %s node", + element.name_.c_str()); + throw std::logic_error(err_buf_); } buffer_it += 2; if (!element.IsNameValid(node_name, buffer_it)) { - char err_msg[100] = ""; - strcat(err_msg, "The detected name ("); - strcat(err_msg, node_name.c_str()); - strcat(err_msg, ") does not match the elements name "); - strcat(err_msg, element.name_.c_str()); - throw std::logic_error(err_msg); + auto i = std::sprintf(err_buf_, "The detected name ("); + i = std::snprintf(err_buf_ + i, node_name.length_, "%s", node_name.data_ptr_); + std::sprintf(err_buf_ + i, ") does not match the elements name: %s", element.name_.c_str()); + throw std::logic_error(err_buf_); } } - - -//########### Copy ##################### - - - // xml::XMLString node_name(buffer, 0); - // // fast forward for the first open bracket - // for (; **buffer_it != '\0' && **buffer_it != '<'; *buffer_it++) { - // } - // if (**buffer_it == '\0') { - // // TODO (Komaromi): Do something when end of string - // } - // *buffer_it++; - // // Validate nodes name - // if (!element.IsNameValid(node_name, buffer_it)) { - // // TODO (Komaromi): Do something when not valid - // } - - // // Validate nodes params - // size_t numOfParam = 0; - // bool isBaseLessNode = false; - // bool isNoMoreParam = false; - // xml::XMLString node_param; - // while (!isNoMoreParam && **buffer_it != '\0') { - // // fast forward to the next non-space character - // for (; **buffer_it == ' '; *buffer_it++) { - // } - // if (**buffer_it == '\0') { - // // TODO (Komaromi): Do something when end of string - // } - // // Check for the nodes hadders end characters - // if (**buffer_it == '/' && *(*buffer_it + 1) == '>') { - // isBaseLessNode = true; - // isNoMoreParam = true; - // } else if (**buffer_it == '>') { - // isNoMoreParam = true; - // } else { - // // If not the end of the nodes hadder decode param - // if (!element.IsParamNameValid(node_param, buffer_it)) { - // // TODO (Komaromi): Do something when not valid - // } - // for (; **buffer_it != '\0' && **buffer_it != '"'; *buffer_it++) { - // } - // if (**buffer_it == '\0') { - // // TODO (Komaromi): Do something when end of string - // } - // *buffer_it++; - // if (!element.CastParam(node_param, buffer_it)) { - // // TODO (Komaromi): Do something when not valid - // } - // *buffer_it++; - // numOfParam++; - // } - // } - // for (; **buffer_it != '\0' && **buffer_it != '>'; *buffer_it++) { - // } - // if (**buffer_it == '\0') { - // // TODO (Komaromi): Do something when end of string - // } - // *buffer_it++; - - // if (isBaseLessNode && numOfParam != element.params_.size()) { - // // TODO (Komaromi): Do something when not valid - // } - // // fast forward if - // for (; **buffer_it == ' '; *buffer_it++) { - // } - // if (**buffer_it == '\0') { - // // TODO (Komaromi): Do something when end of string - // } - // if (!isBaseLessNode) { - // if (**buffer_it != '<') { - // // Node base is data - // if (!element.CastParam(node_name, buffer_it)) { - // // TODO (Komaromi): Do something when not valid - // } - // } else { - // // node base has childs - // for (auto && child : element.childs_) { - // detectNode(child, buffer_it); - // } - // } - // for (; **buffer_it != '\0' && **buffer_it != '<'; *buffer_it++) {// maybe use strstr() - // } - // if (**buffer_it == '\0') { - // // TODO (Komaromi): Do something when end of string - // } - // if (*(*buffer_it + 1) != '/') { - // // TODO (Komaromi): Do something when wrong chield number - // } - // *buffer_it += 2; - // if (!element.IsNameValid(node_name, buffer_it)) { - // // TODO (Komaromi): Do something when not valid - // } - // } } -// int main() -// { -// RSICommandHandler commandHandler(); -// char xml_state[1024] = -// { -// ""}; -// commandHandler.Decode(); -// } } -// // Later this should be defined by the rsi xml -// state_data_structure_.name_ = "Rob"; -// state_data_structure_.params_["TYPE"] = xml::XMLParam(xml::XMLType::STRING); -// // how to get string: std::get(command_data_structure_.params_["TYPE"]).first -// xml::XMLElement Out("Out"); -// Out.params_["01"] = xml::XMLParam(xml::XMLType::BOOL); -// Out.params_["02"] = xml::XMLParam(xml::XMLType::BOOL); -// Out.params_["03"] = xml::XMLParam(xml::XMLType::BOOL); -// Out.params_["04"] = xml::XMLParam(xml::XMLType::BOOL); -// Out.params_["05"] = xml::XMLParam(xml::XMLType::BOOL); -// xml::XMLElement Override("Overrride"); -// Override.params_["Override"] = xml::XMLParam(xml::XMLType::LONG); -// state_data_structure_.childs_.emplace_back(Out); -// state_data_structure_.childs_.emplace_back(Override); diff --git a/xml_handler/rsi_command_handler.hpp b/xml_handler/rsi_command_handler.hpp index 69599887..f5095d10 100644 --- a/xml_handler/rsi_command_handler.hpp +++ b/xml_handler/rsi_command_handler.hpp @@ -49,21 +49,35 @@ namespace kuka_rsi_hw_interface class RSICommandHandler { private: - // void updateDataPlace(); - // void updateData(); - static void detectNode( + xml::XMLElement command_data_structure_; + xml::XMLElement state_data_structure_; + char err_buf_[1024]; + + void detectNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size); public: - xml::XMLElement command_data_structure_; - xml::XMLElement state_data_structure_; - RSICommandHandler(); ~RSICommandHandler() = default; + + inline const xml::XMLElement & GetState() const {return state_data_structure_;} + inline const xml::XMLElement & GetCommand() const {return command_data_structure_;} + + template + bool SetCommandParam(const std::string & elementName, const std::string & paramName, T & param) + { + try { + return command_data_structure_.GetElement(elementName).SetParam(paramName, param); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + return false; + } + } + bool Decode(char * const buffer, const size_t buffer_size); - void Encode(char * buffer); + bool Encode(char * & buffer, const size_t buffer_size); }; } // namespace kuka_rsi_hw_interface diff --git a/xml_handler/xml_element.cpp b/xml_handler/xml_element.cpp index 02a018f8..dd6e27c2 100644 --- a/xml_handler/xml_element.cpp +++ b/xml_handler/xml_element.cpp @@ -25,12 +25,9 @@ namespace xml { -bool XMLElement::CastParam(XMLString key, char * & str_ptr) +bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) { - char key_c_str[key.length_ + 1] = {0}; - strncpy(key_c_str, key.data_ptr_, key.length_); - - auto param_it = params_.find(key_c_str); + auto param_it = params_.find(key); auto ret_val = param_it != params_.end(); if (ret_val) { switch (param_it->second.param_type_) { @@ -56,9 +53,7 @@ bool XMLElement::CastParam(XMLString key, char * & str_ptr) bool XMLElement::IsParamNameValid(XMLString & key, char * & str_ptr) { key = castXMLString(str_ptr); - char key_c_str[key.length_ + 1] = {0}; - strncpy(key_c_str, key.data_ptr_, key.length_); - return params_.find(key_c_str) != params_.end(); + return params_.find(key) != params_.end(); } @@ -73,25 +68,38 @@ bool XMLElement::IsNameValid(XMLString & key, char * & str_ptr) return strncmp(name_.c_str(), key.data_ptr_, key.length_) == 0; } -XMLString XMLElement::castXMLString(char * & str_ptr) +XMLElement & XMLElement::GetElement(const std::string & elementName) { - auto start_ref = str_ptr; - for (; - *str_ptr != '\0' && *str_ptr != '"' && *str_ptr != ' ' && - *str_ptr != '=' && *str_ptr != '>' && *str_ptr != '/' && *str_ptr != '<'; str_ptr++) - { - std::cout << *str_ptr; + if (elementName == name_) { + return *this; } - std::cout << std::endl; - auto data = XMLString( - start_ref, (size_t)(str_ptr - start_ref)); - std::cout << "Cast length: " << data.length_ << std::endl; - std::cout << "Cast Data: " << data.c_str() << std::endl; - std::cout << "Cast all Data: " << data.data_ptr_ << std::endl; - return data; + for (auto && child : childs_) { + if (elementName == child.name_) { + return child.GetElement(elementName); + } + } + char err_buf[1000]; + sprintf(err_buf, "%s element not found", elementName.c_str()); + throw std::logic_error(err_buf); +} + +const XMLElement & XMLElement::GetElement(const std::string & elementName) const +{ + if (elementName == name_) { + return *this; + } + for (auto && child : childs_) { + if (elementName == child.name_) { + return child.GetElement(elementName); + } + } + char err_buf[1000]; + sprintf(err_buf, "%s element not found", elementName.c_str()); + throw std::logic_error(err_buf); } -std::ostream & XMLElement::operator<<(std::ostream & out) + +std::ostream & XMLElement::operator<<(std::ostream & out) const { out << "Element name: " << this->name_ << "\n"; for (auto && param : params_) { @@ -108,7 +116,11 @@ std::ostream & XMLElement::operator<<(std::ostream & out) break; case XMLType::STRING: { - out << std::get<3>(param.second.param_).c_str() << "\n"; + char str[std::get<3>(param.second.param_).length_ + 1]; + std::snprintf( + str, std::get<3>(param.second.param_).length_ + 1, "%s", + std::get<3>(param.second.param_).data_ptr_); + out << str << "\n"; } break; @@ -122,14 +134,24 @@ std::ostream & XMLElement::operator<<(std::ostream & out) return out; } -const char * XMLString::c_str() +XMLString XMLElement::castXMLString(char * & str_ptr) { - if (length_ < 49) { - strncpy(data_c_str_, data_ptr_, length_); - data_c_str_[length_] = '\0'; - return data_c_str_; + auto start_ref = str_ptr; + for (; + *str_ptr != '\0' && *str_ptr != '"' && *str_ptr != ' ' && + *str_ptr != '=' && *str_ptr != '>' && *str_ptr != '/' && *str_ptr != '<'; str_ptr++) + { + // std::cout << *str_ptr; } - throw std::range_error("C string copy out of range"); + // std::cout << std::endl; + auto data = XMLString( + start_ref, (size_t)(str_ptr - start_ref)); + // char str[data.length_ + 1]; + // snprintf(str, data.length_ + 1, "%s", data.data_ptr_); + // std::cout << "Cast length: " << data.length_ << std::endl; + // std::cout << "Cast Data: " << str << std::endl; + // std::cout << "Cast all Data: " << data.data_ptr_ << std::endl; + return data; } bool XMLString::operator==(const XMLString & rhs) @@ -148,7 +170,7 @@ bool XMLString::operator==(const std::string & rhs) return strncmp(rhs.c_str(), data_ptr_, length_); } -bool XMLString::operator==(const char * rhs) +bool XMLString::operator==(const char * & rhs) { if (length_ != strlen(rhs)) { return false; diff --git a/xml_handler/xml_element.hpp b/xml_handler/xml_element.hpp index 3064cc06..fa1f0bbc 100644 --- a/xml_handler/xml_element.hpp +++ b/xml_handler/xml_element.hpp @@ -19,8 +19,10 @@ #include #include #include +#include #include #include +#include namespace xml @@ -34,17 +36,16 @@ enum class XMLType : size_t STRING = 3 }; -struct XMLString +class XMLString { +public: const char * data_ptr_; - char data_c_str_[50] = ""; size_t length_; XMLString(const char * data_ptr = nullptr, size_t length = 0) : data_ptr_(data_ptr), length_(length) {} - const char * c_str(); bool operator==(const XMLString & rhs); bool operator==(const std::string & rhs); - bool operator==(const char * rhs); + bool operator==(const char * & rhs); }; struct XMLParam @@ -56,6 +57,17 @@ struct XMLParam : param_type_(type) {} }; + +inline bool operator<(const XMLString & a, const std::string & b) +{ + return a.length_ == b.length() && strncmp(a.data_ptr_, b.c_str(), a.length_) < 0; +} + +inline bool operator<(const std::string & b, const XMLString & a) +{ + return a.length_ == b.length() && strncmp(b.c_str(), a.data_ptr_, a.length_) < 0; +} + // struct XMLStringComparator // { // bool operator()(const char * a, const char * b) const @@ -71,28 +83,31 @@ class XMLElement public: std::string name_; - std::map params_; + std::map> params_; std::vector childs_; - XMLElement(std::string name) + XMLElement(const std::string & name) : name_(name) {} XMLElement() = default; ~XMLElement() = default; - bool CastParam(XMLString key, char * & str_ptr); + bool CastParam(const XMLString & key, char * & str_ptr); bool IsParamNameValid(XMLString & key, char * & str_ptr); bool IsNameValid(XMLString & key, char * & str_ptr); - std::ostream & operator<<(std::ostream & out); + XMLElement & GetElement(const std::string & elementName); + const XMLElement & GetElement(const std::string & elementName) const; + + std::ostream & operator<<(std::ostream & out) const; // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used template - bool GetParam(std::string key, T & param) + bool GetParam(const std::string & key, T & param) const { auto param_it = params_.find(key); if (param_it != params_.end()) { - if (std::is_same() || std::is_same() || - std::is_same() || std::is_same()) + if constexpr (std::is_same::value || std::is_same::value || + std::is_same::value || std::is_same::value) { param = std::get(param_it->second.param_); return true; @@ -103,12 +118,12 @@ class XMLElement // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used template - bool SetParam(std::string key, const T & param) + bool SetParam(const std::string & key, const T & param) { auto param_it = params_.find(key); if (param_it != params_.end()) { - if (std::is_same() || std::is_same() || - std::is_same() || std::is_same()) + if constexpr (std::is_same::value || std::is_same::value || + std::is_same::value || std::is_same::value) { param_it->second.param_ = param; return true; From 15a81db7d62d9f5c6aa2fa6553ae2093ea4a91bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 25 Jul 2023 18:42:08 +0200 Subject: [PATCH 43/97] Encoder layout --- xml_handler/decodeTest.cpp | 4 +-- xml_handler/rsi_command_handler.cpp | 41 ++++++++++++++++++++++++++--- xml_handler/rsi_command_handler.hpp | 6 ++++- xml_handler/xml_element.hpp | 2 ++ 4 files changed, 47 insertions(+), 6 deletions(-) diff --git a/xml_handler/decodeTest.cpp b/xml_handler/decodeTest.cpp index cb52532d..ccef7578 100644 --- a/xml_handler/decodeTest.cpp +++ b/xml_handler/decodeTest.cpp @@ -21,8 +21,8 @@ int main(int argc, char const * argv[]) handler.GetState().GetElement("Out").GetParam("01", param); std::cout << "encode data fill" << std::endl; - std::string msg = "Hello World!"; - xml::XMLString xmlStr(msg.c_str(), msg.length() + 1); + std::string msg = "KROSHU"; + xml::XMLString xmlStr(msg); handler.SetCommandParam("Sen", "Type", xmlStr); double x = 5.2; double y = 6; diff --git a/xml_handler/rsi_command_handler.cpp b/xml_handler/rsi_command_handler.cpp index 06f2e2ce..2fc74d4f 100644 --- a/xml_handler/rsi_command_handler.cpp +++ b/xml_handler/rsi_command_handler.cpp @@ -57,7 +57,7 @@ bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) std::cout << "Decode started" << std::endl; auto buffer_it = buffer; try { - detectNode(state_data_structure_, buffer, buffer_it, buffer_size); + decodeNode(state_data_structure_, buffer, buffer_it, buffer_size); } catch (const std::exception & e) { std::cout << "### ERROR ###" << std::endl; std::cerr << e.what() << '\n'; @@ -67,7 +67,22 @@ bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) return true; } -void RSICommandHandler::detectNode( +bool RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) +{ + std::cout << "Decode started" << std::endl; + auto buffer_it = buffer; + try { + encodeNode(command_data_structure_, buffer, buffer_it, buffer_size); + } catch (const std::exception & e) { + std::cout << "### ERROR ###" << std::endl; + std::cerr << e.what() << '\n'; + return false; + } + std::cout << "Decode finished" << std::endl; + return true; +} + +void RSICommandHandler::decodeNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size) { @@ -169,7 +184,7 @@ void RSICommandHandler::detectNode( } else { // node base has childs for (auto && child : element.childs_) { - detectNode(child, buffer, buffer_it, buffer_size); + decodeNode(child, buffer, buffer_it, buffer_size); } } for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '<'; buffer_it++) { @@ -193,4 +208,24 @@ void RSICommandHandler::detectNode( } } } + +void RSICommandHandler::encodeNode( + xml::XMLElement & element, char * const buffer, char * & buffer_it, + const size_t buffer_size) +{ + buffer_it += sprintf(buffer_it, "<%s ", element.name_.c_str()); + for (auto && param : element.params_) { + if (element.name_ != param.first) { + buffer_it += sprintf(buffer_it, "%s=\"%s\" ", param.first.c_str(), param.second.param_); + } + // check for child if there is none and there is no param with the same key then name finish param and retrun + // if has that type of param and has child return error + // if has child call func for child + // if there is one param, then inset it + // create param ender + + } + + +} } diff --git a/xml_handler/rsi_command_handler.hpp b/xml_handler/rsi_command_handler.hpp index f5095d10..66d7c627 100644 --- a/xml_handler/rsi_command_handler.hpp +++ b/xml_handler/rsi_command_handler.hpp @@ -53,7 +53,11 @@ class RSICommandHandler xml::XMLElement state_data_structure_; char err_buf_[1024]; - void detectNode( + void decodeNode( + xml::XMLElement & element, char * const buffer, char * & buffer_it, + const size_t buffer_size); + + void encodeNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size); diff --git a/xml_handler/xml_element.hpp b/xml_handler/xml_element.hpp index fa1f0bbc..ef1c708f 100644 --- a/xml_handler/xml_element.hpp +++ b/xml_handler/xml_element.hpp @@ -43,6 +43,8 @@ class XMLString size_t length_; XMLString(const char * data_ptr = nullptr, size_t length = 0) : data_ptr_(data_ptr), length_(length) {} + XMLString(const std::string & str) + : data_ptr_(str.c_str()), length_(str.length()) {} bool operator==(const XMLString & rhs); bool operator==(const std::string & rhs); bool operator==(const char * & rhs); From a29f3897bc36c81cf8b63e9aca1c4d6d1e95baca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Mon, 31 Jul 2023 16:28:04 +0200 Subject: [PATCH 44/97] Encode algorithm --- xml_handler/rsi_command_handler.cpp | 56 ++++++++++++++++++++++++++--- 1 file changed, 51 insertions(+), 5 deletions(-) diff --git a/xml_handler/rsi_command_handler.cpp b/xml_handler/rsi_command_handler.cpp index 2fc74d4f..840c6a2d 100644 --- a/xml_handler/rsi_command_handler.cpp +++ b/xml_handler/rsi_command_handler.cpp @@ -213,19 +213,65 @@ void RSICommandHandler::encodeNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size) { - buffer_it += sprintf(buffer_it, "<%s ", element.name_.c_str()); + auto idx = sprintf(buffer_it, "<%s ", element.name_.c_str()); + if ((size_t)buffer_it + idx >= buffer_size) { + throw std::range_error("Out of the buffers range"); + } + buffer_it += idx; + bool isBaseLessNode = false; for (auto && param : element.params_) { - if (element.name_ != param.first) { - buffer_it += sprintf(buffer_it, "%s=\"%s\" ", param.first.c_str(), param.second.param_); + if (element.childs_.size() <= 0 && element.name_ == param.first) { + isBaseLessNode = true; + } else { + idx = sprintf(buffer_it, "%s=\"%s\" ", param.first.c_str(), param.second.param_); + if ((size_t)buffer_it + idx >= buffer_size) { + throw std::range_error("Out of the buffers range"); + } + buffer_it += idx; } // check for child if there is none and there is no param with the same key then name finish param and retrun // if has that type of param and has child return error // if has child call func for child // if there is one param, then inset it // create param ender - } - + if (element.childs_.size() <= 0 && isBaseLessNode == false) { + std::sprintf(err_buf_, "The %s node has no chiled and has no base data"); + throw std::logic_error(err_buf_); + } + if (isBaseLessNode) { + idx = sprintf(buffer_it, "/>"); + if ((size_t)buffer_it + idx >= buffer_size) { + throw std::range_error("Out of the buffers range"); + } + buffer_it += idx; + } else { + buffer_it--; + idx = sprintf(buffer_it, ">"); + if ((size_t)buffer_it + idx >= buffer_size) { + throw std::range_error("Out of the buffers range"); + } + buffer_it += idx; + if (element.childs_.size() > 0) { + // Add childs + for (auto && child : element.childs_) { + encodeNode(child, buffer, buffer_it, buffer_size); + } + } else { + // Add data + idx = sprintf(buffer_it, "%u", element.params_.find(element.name_)->second.param_); + if ((size_t)buffer_it + idx >= buffer_size) { + throw std::range_error("Out of the buffers range"); + } + buffer_it += idx; + } + // Add end bracket + idx = sprintf(buffer_it, "", element.name_); + if ((size_t)buffer_it + idx >= buffer_size) { + throw std::range_error("Out of the buffers range"); + } + buffer_it += idx; + } } } From c6b3a697a47d12b1d97459d750d7603aee7f6610 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 1 Aug 2023 18:34:34 +0200 Subject: [PATCH 45/97] Encode under test --- xml_handler/CMakeLists.txt | 2 + xml_handler/decodeTest.cpp | 7 +++ xml_handler/rsi_command_handler.cpp | 95 +++++++++++++++-------------- xml_handler/xml_element.cpp | 24 -------- xml_handler/xml_element.hpp | 64 ++++++------------- xml_handler/xml_param.cpp | 53 ++++++++++++++++ xml_handler/xml_param.hpp | 56 +++++++++++++++++ xml_handler/xml_string.cpp | 44 +++++++++++++ xml_handler/xml_string.hpp | 37 +++++++++++ 9 files changed, 267 insertions(+), 115 deletions(-) create mode 100644 xml_handler/xml_param.cpp create mode 100644 xml_handler/xml_param.hpp create mode 100644 xml_handler/xml_string.cpp create mode 100644 xml_handler/xml_string.hpp diff --git a/xml_handler/CMakeLists.txt b/xml_handler/CMakeLists.txt index 5ddb7362..c20c5f9e 100644 --- a/xml_handler/CMakeLists.txt +++ b/xml_handler/CMakeLists.txt @@ -17,6 +17,8 @@ endif() add_library(${PROJECT_NAME} SHARED rsi_command_handler.cpp xml_element.cpp + xml_string.cpp + xml_param.cpp ) add_executable(decode_test diff --git a/xml_handler/decodeTest.cpp b/xml_handler/decodeTest.cpp index ccef7578..4657658a 100644 --- a/xml_handler/decodeTest.cpp +++ b/xml_handler/decodeTest.cpp @@ -19,6 +19,7 @@ int main(int argc, char const * argv[]) handler.GetState().operator<<(std::cout) << std::endl; bool param; handler.GetState().GetElement("Out").GetParam("01", param); + auto str = handler.GetState().GetParam("TYPE"); std::cout << "encode data fill" << std::endl; std::string msg = "KROSHU"; @@ -41,5 +42,11 @@ int main(int argc, char const * argv[]) handler.GetCommand().operator<<(std::cout) << std::endl; + char xml_command[1024] = {0}; + if (!handler.Encode(xml_command, 1024)) { + std::cout << "decode failed" << std::endl; + return -1; + } + return 0; } diff --git a/xml_handler/rsi_command_handler.cpp b/xml_handler/rsi_command_handler.cpp index 840c6a2d..cb27ab77 100644 --- a/xml_handler/rsi_command_handler.cpp +++ b/xml_handler/rsi_command_handler.cpp @@ -25,31 +25,31 @@ RSICommandHandler::RSICommandHandler() : state_data_structure_("Rob"), command_data_structure_("Sen"), err_buf_("") { // Later this should be defined by the rsi xml - state_data_structure_.params_["TYPE"] = xml::XMLParam(xml::XMLType::STRING); + state_data_structure_.GetParams()["TYPE"] = xml::XMLParam(xml::XMLType::STRING); // how to get string: std::get(command_data_structure_.params_["TYPE"]).first xml::XMLElement Out("Out"); - Out.params_["01"] = xml::XMLParam(xml::XMLType::BOOL); - Out.params_["02"] = xml::XMLParam(xml::XMLType::BOOL); - Out.params_["03"] = xml::XMLParam(xml::XMLType::BOOL); - Out.params_["04"] = xml::XMLParam(xml::XMLType::BOOL); - Out.params_["05"] = xml::XMLParam(xml::XMLType::BOOL); + Out.GetParams()["01"] = xml::XMLParam(xml::XMLType::BOOL); + Out.GetParams()["02"] = xml::XMLParam(xml::XMLType::BOOL); + Out.GetParams()["03"] = xml::XMLParam(xml::XMLType::BOOL); + Out.GetParams()["04"] = xml::XMLParam(xml::XMLType::BOOL); + Out.GetParams()["05"] = xml::XMLParam(xml::XMLType::BOOL); xml::XMLElement Override("Override"); - Override.params_["Override"] = xml::XMLParam(xml::XMLType::LONG); - state_data_structure_.childs_.emplace_back(Out); - state_data_structure_.childs_.emplace_back(Override); + Override.GetParams()["Override"] = xml::XMLParam(xml::XMLType::LONG); + state_data_structure_.GetChilds().emplace_back(Out); + state_data_structure_.GetChilds().emplace_back(Override); - command_data_structure_.params_["Type"] = xml::XMLParam(xml::XMLType::STRING); + command_data_structure_.GetParams()["Type"] = xml::XMLParam(xml::XMLType::STRING); xml::XMLElement RKorr("RKorr"); - RKorr.params_["X"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.params_["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.params_["Z"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.params_["A"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.params_["B"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.params_["C"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.GetParams()["X"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.GetParams()["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.GetParams()["Z"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.GetParams()["A"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.GetParams()["B"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.GetParams()["C"] = xml::XMLParam(xml::XMLType::DOUBLE); xml::XMLElement DiO("DiO"); - DiO.params_["DiO"] = xml::XMLParam(xml::XMLType::LONG); - command_data_structure_.childs_.emplace_back(RKorr); - command_data_structure_.childs_.emplace_back(DiO); + DiO.GetParams()["DiO"] = xml::XMLParam(xml::XMLType::LONG); + command_data_structure_.GetChilds().emplace_back(RKorr); + command_data_structure_.GetChilds().emplace_back(DiO); } bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) @@ -98,7 +98,7 @@ void RSICommandHandler::decodeNode( if (!element.IsNameValid(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected name ("); i = std::snprintf(err_buf_ + i, node_name.length_, "%s", node_name.data_ptr_); - i = std::sprintf(err_buf_, ") does not match the elements name %s.", element.name_.c_str()); + i = std::sprintf(err_buf_, ") does not match the elements name %s.", element.GetName().c_str()); throw std::logic_error(err_buf_); } // char str[node_name.length_ + 1]; @@ -129,7 +129,7 @@ void RSICommandHandler::decodeNode( i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_name.data_ptr_); std::sprintf( err_buf_ + i, ") dose not match with any of the %s elements parameters.", - element.name_.c_str()); + element.GetName().c_str()); throw std::logic_error(err_buf_); } for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '"'; buffer_it++) { @@ -143,7 +143,7 @@ void RSICommandHandler::decodeNode( i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_param.data_ptr_); std::sprintf( err_buf_ + i, " param into the %s elements parameter list.", - element.name_.c_str()); + element.GetName().c_str()); throw std::logic_error(err_buf_); } buffer_it++; @@ -157,11 +157,11 @@ void RSICommandHandler::decodeNode( } buffer_it++; - if (isBaseLessNode && numOfParam != element.params_.size()) { + if (isBaseLessNode && numOfParam != element.GetParams().size()) { std::sprintf( err_buf_, "%lu parameter found it does not match with %s elements parameter list size.", numOfParam, - element.name_.c_str()); + element.GetName().c_str()); throw std::logic_error(err_buf_); } // fast forward if @@ -178,12 +178,12 @@ void RSICommandHandler::decodeNode( i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_param.data_ptr_); std::sprintf( err_buf_ + i, " parameter into the %s elements parameter list", - element.name_.c_str()); + element.GetName().c_str()); throw std::logic_error(err_buf_); } } else { // node base has childs - for (auto && child : element.childs_) { + for (auto && child : element.GetChilds()) { decodeNode(child, buffer, buffer_it, buffer_size); } } @@ -196,14 +196,16 @@ void RSICommandHandler::decodeNode( std::sprintf( err_buf_, "Start of an end Node, where there should be none. Error came in the %s node", - element.name_.c_str()); + element.GetName().c_str()); throw std::logic_error(err_buf_); } buffer_it += 2; if (!element.IsNameValid(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected name ("); i = std::snprintf(err_buf_ + i, node_name.length_, "%s", node_name.data_ptr_); - std::sprintf(err_buf_ + i, ") does not match the elements name: %s", element.name_.c_str()); + std::sprintf( + err_buf_ + i, ") does not match the elements name: %s", + element.GetName().c_str()); throw std::logic_error(err_buf_); } } @@ -213,30 +215,33 @@ void RSICommandHandler::encodeNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size) { - auto idx = sprintf(buffer_it, "<%s ", element.name_.c_str()); + auto idx = sprintf(buffer_it, "<%s ", element.GetName().c_str()); if ((size_t)buffer_it + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; bool isBaseLessNode = false; - for (auto && param : element.params_) { - if (element.childs_.size() <= 0 && element.name_ == param.first) { + for (auto && param : element.GetParams()) { + if (element.GetParams().size() <= 0 && element.GetName() == param.first) { isBaseLessNode = true; } else { - idx = sprintf(buffer_it, "%s=\"%s\" ", param.first.c_str(), param.second.param_); + idx = sprintf(buffer_it, "%s=\"", param.first.c_str()); + if ((size_t)buffer_it + idx >= buffer_size) { + throw std::range_error("Out of the buffers range"); + } + buffer_it += idx; + param.second.ParamSprint(buffer_it, buffer_size); + idx = sprintf(buffer_it, "\" "); if ((size_t)buffer_it + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; } - // check for child if there is none and there is no param with the same key then name finish param and retrun - // if has that type of param and has child return error - // if has child call func for child - // if there is one param, then inset it - // create param ender } - if (element.childs_.size() <= 0 && isBaseLessNode == false) { - std::sprintf(err_buf_, "The %s node has no chiled and has no base data"); + if (element.GetChilds().size() <= 0 && isBaseLessNode == false) { + std::sprintf( + err_buf_, "The %s node has no chiled and has no base data", + element.GetName().c_str()); throw std::logic_error(err_buf_); } if (isBaseLessNode) { @@ -252,21 +257,17 @@ void RSICommandHandler::encodeNode( throw std::range_error("Out of the buffers range"); } buffer_it += idx; - if (element.childs_.size() > 0) { + if (element.GetChilds().size() > 0) { // Add childs - for (auto && child : element.childs_) { + for (auto && child : element.GetChilds()) { encodeNode(child, buffer, buffer_it, buffer_size); } } else { // Add data - idx = sprintf(buffer_it, "%u", element.params_.find(element.name_)->second.param_); - if ((size_t)buffer_it + idx >= buffer_size) { - throw std::range_error("Out of the buffers range"); - } - buffer_it += idx; + element.GetParams().find(element.GetName())->second.ParamSprint(buffer_it, buffer_size); } // Add end bracket - idx = sprintf(buffer_it, "", element.name_); + idx = sprintf(buffer_it, "", element.GetName().c_str()); if ((size_t)buffer_it + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } diff --git a/xml_handler/xml_element.cpp b/xml_handler/xml_element.cpp index dd6e27c2..fc6e3ed3 100644 --- a/xml_handler/xml_element.cpp +++ b/xml_handler/xml_element.cpp @@ -153,28 +153,4 @@ XMLString XMLElement::castXMLString(char * & str_ptr) // std::cout << "Cast all Data: " << data.data_ptr_ << std::endl; return data; } - -bool XMLString::operator==(const XMLString & rhs) -{ - if (length_ != rhs.length_) { - return false; - } - return strncmp(data_ptr_, rhs.data_ptr_, length_); -} - -bool XMLString::operator==(const std::string & rhs) -{ - if (length_ != rhs.length()) { - return false; - } - return strncmp(rhs.c_str(), data_ptr_, length_); -} - -bool XMLString::operator==(const char * & rhs) -{ - if (length_ != strlen(rhs)) { - return false; - } - return strncmp(rhs, data_ptr_, length_); -} } diff --git a/xml_handler/xml_element.hpp b/xml_handler/xml_element.hpp index ef1c708f..827fa628 100644 --- a/xml_handler/xml_element.hpp +++ b/xml_handler/xml_element.hpp @@ -17,49 +17,16 @@ #include #include -#include #include #include #include #include #include +#include "xml_param.hpp" namespace xml { - -enum class XMLType : size_t -{ - BOOL = 0, - LONG = 1, - DOUBLE = 2, - STRING = 3 -}; - -class XMLString -{ -public: - const char * data_ptr_; - size_t length_; - XMLString(const char * data_ptr = nullptr, size_t length = 0) - : data_ptr_(data_ptr), length_(length) {} - XMLString(const std::string & str) - : data_ptr_(str.c_str()), length_(str.length()) {} - bool operator==(const XMLString & rhs); - bool operator==(const std::string & rhs); - bool operator==(const char * & rhs); -}; - -struct XMLParam -{ - XMLType param_type_; - std::variant param_; - XMLParam() = default; - XMLParam(XMLType type) - : param_type_(type) {} -}; - - inline bool operator<(const XMLString & a, const std::string & b) { return a.length_ == b.length() && strncmp(a.data_ptr_, b.c_str(), a.length_) < 0; @@ -70,29 +37,28 @@ inline bool operator<(const std::string & b, const XMLString & a) return a.length_ == b.length() && strncmp(b.c_str(), a.data_ptr_, a.length_) < 0; } -// struct XMLStringComparator -// { -// bool operator()(const char * a, const char * b) const -// { -// return a.length() == b.length_ && memcmp(a.data(), b.data_ptr_, b.length_) > 0; -// } -// }; - class XMLElement { private: static XMLString castXMLString(char * & str_ptr); -public: - std::string name_; std::map> params_; std::vector childs_; + std::string name_; +public: XMLElement(const std::string & name) : name_(name) {} XMLElement() = default; ~XMLElement() = default; + inline std::string GetName() const {return name_;} + inline void SetName(const std::string & name) {name_ = name;} + inline std::vector GetChilds() const {return childs_;} + inline std::vector & GetChilds() {return childs_;} + inline std::map> & GetParams() {return params_;} + + bool CastParam(const XMLString & key, char * & str_ptr); bool IsParamNameValid(XMLString & key, char * & str_ptr); bool IsNameValid(XMLString & key, char * & str_ptr); @@ -118,6 +84,16 @@ class XMLElement return false; } + template + T GetParam(const std::string & key) const + { + auto param_it = params_.find(key); + if (param_it != params_.end()) { + return param_it->second.GetParam(); + } + throw std::range_error("Could not find key in parameter list"); + } + // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used template bool SetParam(const std::string & key, const T & param) diff --git a/xml_handler/xml_param.cpp b/xml_handler/xml_param.cpp new file mode 100644 index 00000000..388d0aab --- /dev/null +++ b/xml_handler/xml_param.cpp @@ -0,0 +1,53 @@ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "xml_param.hpp" + +namespace xml +{ +size_t XMLParam::ParamSprint(char * & buffer_it, const size_t buffer_size) +{ + int idx = 0; + switch (param_type_) { + case XMLType::BOOL: + idx = sprintf(buffer_it, "%u", std::get<0>(param_)); + break; + + case XMLType::LONG: + idx = sprintf(buffer_it, "%lu", std::get<1>(param_)); + break; + + case XMLType::DOUBLE: + idx = sprintf(buffer_it, "%f", std::get<2>(param_)); + break; + + case XMLType::STRING: + idx = std::snprintf( + buffer_it, std::get<3>(param_).length_, "%s", + std::get<3>(param_).data_ptr_); + break; + default: + throw std::logic_error("Parameter type not supported"); + break; + } + if ((size_t)buffer_it + idx >= buffer_size) { + throw std::range_error("Out of the buffers range"); + } + buffer_it += idx; + return idx; +} +} diff --git a/xml_handler/xml_param.hpp b/xml_handler/xml_param.hpp new file mode 100644 index 00000000..1fda3751 --- /dev/null +++ b/xml_handler/xml_param.hpp @@ -0,0 +1,56 @@ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef XML__XML_PARAM_H_ +#define XML__XML_PARAM_H_ + +#include + +#include "xml_string.hpp" + +namespace xml +{ +enum class XMLType : size_t +{ + BOOL = 0, + LONG = 1, + DOUBLE = 2, + STRING = 3 +}; + + +class XMLParam +{ +public: + XMLType param_type_; + std::variant param_; + XMLParam() = default; + XMLParam(XMLType type) + : param_type_(type) {} + size_t ParamSprint(char * & buffer_it, const size_t buffer_size); + + template + T GetParam() const + { + if constexpr (std::is_same::value || std::is_same::value || + std::is_same::value || std::is_same::value) + { + return std::get(param_); + } + throw std::logic_error("Parameter type not supported"); + } +}; + +} +#endif // XML__XML_PARAM_H_ diff --git a/xml_handler/xml_string.cpp b/xml_handler/xml_string.cpp new file mode 100644 index 00000000..01095aa1 --- /dev/null +++ b/xml_handler/xml_string.cpp @@ -0,0 +1,44 @@ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "xml_string.hpp" + +namespace xml +{ +bool XMLString::operator==(const XMLString & rhs) +{ + if (length_ != rhs.length_) { + return false; + } + return strncmp(data_ptr_, rhs.data_ptr_, length_); +} + +bool XMLString::operator==(const std::string & rhs) +{ + if (length_ != rhs.length()) { + return false; + } + return strncmp(rhs.c_str(), data_ptr_, length_); +} + +bool XMLString::operator==(const char * & rhs) +{ + if (length_ != strlen(rhs)) { + return false; + } + return strncmp(rhs, data_ptr_, length_); +} +} diff --git a/xml_handler/xml_string.hpp b/xml_handler/xml_string.hpp new file mode 100644 index 00000000..a7a4e5d6 --- /dev/null +++ b/xml_handler/xml_string.hpp @@ -0,0 +1,37 @@ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef XML__XML_STRING_H_ +#define XML__XML_STRING_H_ + +#include + +namespace xml +{ +class XMLString +{ +public: + const char * data_ptr_; + size_t length_; + XMLString(const char * data_ptr = nullptr, size_t length = 0) + : data_ptr_(data_ptr), length_(length) {} + XMLString(const std::string & str) + : data_ptr_(str.c_str()), length_(str.length()) {} + bool operator==(const XMLString & rhs); + bool operator==(const std::string & rhs); + bool operator==(const char * & rhs); +}; +} // xml + +#endif // XML__XML_STRING_H_ From 90de396e1fcafe30a0a93a84129b3cabd4f93ec1 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 2 Aug 2023 08:33:03 +0200 Subject: [PATCH 46/97] remove tabs --- kuka_rsi_hw_interface/krl/ros_rsi.src | 4 +- .../krl/ros_rsi_ethernet.xml | 2 +- kuka_rsi_hw_interface/package.xml | 2 +- kuka_sunrise_driver/package.xml | 60 +++++++++---------- 4 files changed, 34 insertions(+), 34 deletions(-) diff --git a/kuka_rsi_hw_interface/krl/ros_rsi.src b/kuka_rsi_hw_interface/krl/ros_rsi.src index 45885d34..42ed9f86 100644 --- a/kuka_rsi_hw_interface/krl/ros_rsi.src +++ b/kuka_rsi_hw_interface/krl/ros_rsi.src @@ -50,8 +50,8 @@ DEF RSI_AxisCorr( ) ; ============================================= ; Declaration of KRL variables -DECL INT ret ; Return value for RSI commands -DECL INT CONTID ; ContainerID +DECL INT ret ; Return value for RSI commands +DECL INT CONTID ; ContainerID ;FOLD INI ;FOLD BASISTECH INI diff --git a/kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml b/kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml index aadb5a8a..a898f18a 100644 --- a/kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml +++ b/kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml @@ -29,7 +29,7 @@ - + \ No newline at end of file diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml index 74a30497..abf33063 100644 --- a/kuka_rsi_hw_interface/package.xml +++ b/kuka_rsi_hw_interface/package.xml @@ -19,7 +19,7 @@ tinyxml_vendor hardware_interface pluginlib - controller_manager_msgs + controller_manager_msgs ament_cmake_copyright ament_cmake_cppcheck diff --git a/kuka_sunrise_driver/package.xml b/kuka_sunrise_driver/package.xml index 4d351380..4dd4424b 100644 --- a/kuka_sunrise_driver/package.xml +++ b/kuka_sunrise_driver/package.xml @@ -1,39 +1,39 @@ - kuka_sunrise - 0.0.0 - ROS2 KUKA sunrise interface - Zoltan Resi - Gergely Kovacs - Apache 2.0 + kuka_sunrise + 0.0.0 + ROS2 KUKA sunrise interface + Zoltan Resi + Gergely Kovacs + Apache 2.0 - ament_cmake - ament_cmake_python - rosidl_default_generators + ament_cmake + ament_cmake_python + rosidl_default_generators - fri - rclcpp - rclcpp_lifecycle - std_msgs - std_srvs - sensor_msgs - kuka_driver_interfaces - kroshu_ros2_core - hardware_interface - controller_manager_msgs + fri + rclcpp + rclcpp_lifecycle + std_msgs + std_srvs + sensor_msgs + kuka_driver_interfaces + kroshu_ros2_core + hardware_interface + controller_manager_msgs - rosidl_default_runtime + rosidl_default_runtime - ament_cmake_copyright - ament_cmake_cppcheck - ament_cmake_pep257 - ament_cmake_flake8 - ament_cmake_cpplint - ament_cmake_lint_cmake - ament_cmake_xmllint + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_pep257 + ament_cmake_flake8 + ament_cmake_cpplint + ament_cmake_lint_cmake + ament_cmake_xmllint - - ament_cmake - + + ament_cmake + From ad4cc508fd643a56308dd6f579fcab405664a2ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Wed, 2 Aug 2023 13:14:01 +0200 Subject: [PATCH 47/97] Working encoding --- xml_handler/decodeTest.cpp | 5 +++- xml_handler/rsi_command_handler.cpp | 40 +++++++++++++++++------------ xml_handler/xml_param.cpp | 6 ++--- xml_handler/xml_param.hpp | 2 +- 4 files changed, 32 insertions(+), 21 deletions(-) diff --git a/xml_handler/decodeTest.cpp b/xml_handler/decodeTest.cpp index 4657658a..c39156a7 100644 --- a/xml_handler/decodeTest.cpp +++ b/xml_handler/decodeTest.cpp @@ -43,10 +43,13 @@ int main(int argc, char const * argv[]) handler.GetCommand().operator<<(std::cout) << std::endl; char xml_command[1024] = {0}; - if (!handler.Encode(xml_command, 1024)) { + char * xml_command_it = xml_command; + if (!handler.Encode(xml_command_it, 1024)) { std::cout << "decode failed" << std::endl; return -1; } + std::cout << "decode successful" << std::endl; + std::cout << xml_command << std::endl; return 0; } diff --git a/xml_handler/rsi_command_handler.cpp b/xml_handler/rsi_command_handler.cpp index cb27ab77..3c5f3e28 100644 --- a/xml_handler/rsi_command_handler.cpp +++ b/xml_handler/rsi_command_handler.cpp @@ -216,44 +216,50 @@ void RSICommandHandler::encodeNode( const size_t buffer_size) { auto idx = sprintf(buffer_it, "<%s ", element.GetName().c_str()); - if ((size_t)buffer_it + idx >= buffer_size) { + if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; bool isBaseLessNode = false; + if (element.GetChilds().size() <= 0) { + isBaseLessNode = true; + } + std::cout << "Added params: " << std::endl; for (auto && param : element.GetParams()) { - if (element.GetParams().size() <= 0 && element.GetName() == param.first) { - isBaseLessNode = true; + if (element.GetName() == param.first) { + isBaseLessNode = false; } else { idx = sprintf(buffer_it, "%s=\"", param.first.c_str()); - if ((size_t)buffer_it + idx >= buffer_size) { + if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; - param.second.ParamSprint(buffer_it, buffer_size); + param.second.ParamSprint(buffer_it, buffer, buffer_size); + std::cout << param.first << ", "; idx = sprintf(buffer_it, "\" "); - if ((size_t)buffer_it + idx >= buffer_size) { + if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; } } - if (element.GetChilds().size() <= 0 && isBaseLessNode == false) { - std::sprintf( - err_buf_, "The %s node has no chiled and has no base data", - element.GetName().c_str()); - throw std::logic_error(err_buf_); - } + std::cout << std::endl; + // if (element.GetChilds().size() <= 0 && isBaseLessNode == false) { + // std::sprintf( + // err_buf_, "The %s node has no chiled and has no base data", + // element.GetName().c_str()); + // throw std::logic_error(err_buf_); + // } if (isBaseLessNode) { idx = sprintf(buffer_it, "/>"); - if ((size_t)buffer_it + idx >= buffer_size) { + if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; } else { buffer_it--; idx = sprintf(buffer_it, ">"); - if ((size_t)buffer_it + idx >= buffer_size) { + if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; @@ -264,11 +270,13 @@ void RSICommandHandler::encodeNode( } } else { // Add data - element.GetParams().find(element.GetName())->second.ParamSprint(buffer_it, buffer_size); + element.GetParams().find(element.GetName())->second.ParamSprint( + buffer_it, buffer, + buffer_size); } // Add end bracket idx = sprintf(buffer_it, "", element.GetName().c_str()); - if ((size_t)buffer_it + idx >= buffer_size) { + if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; diff --git a/xml_handler/xml_param.cpp b/xml_handler/xml_param.cpp index 388d0aab..f3edc39d 100644 --- a/xml_handler/xml_param.cpp +++ b/xml_handler/xml_param.cpp @@ -19,7 +19,7 @@ namespace xml { -size_t XMLParam::ParamSprint(char * & buffer_it, const size_t buffer_size) +size_t XMLParam::ParamSprint(char * & buffer_it, char * const buffer, const size_t buffer_size) { int idx = 0; switch (param_type_) { @@ -37,14 +37,14 @@ size_t XMLParam::ParamSprint(char * & buffer_it, const size_t buffer_size) case XMLType::STRING: idx = std::snprintf( - buffer_it, std::get<3>(param_).length_, "%s", + buffer_it, std::get<3>(param_).length_ + 1, "%s", std::get<3>(param_).data_ptr_); break; default: throw std::logic_error("Parameter type not supported"); break; } - if ((size_t)buffer_it + idx >= buffer_size) { + if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; diff --git a/xml_handler/xml_param.hpp b/xml_handler/xml_param.hpp index 1fda3751..f5ea8e09 100644 --- a/xml_handler/xml_param.hpp +++ b/xml_handler/xml_param.hpp @@ -38,7 +38,7 @@ class XMLParam XMLParam() = default; XMLParam(XMLType type) : param_type_(type) {} - size_t ParamSprint(char * & buffer_it, const size_t buffer_size); + size_t ParamSprint(char * & buffer_it, char * const buffer, const size_t buffer_size); template T GetParam() const From 9800d9ba64f9969f9b65427333f10d711049579f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Wed, 2 Aug 2023 15:23:32 +0200 Subject: [PATCH 48/97] xml_handler moved into kss driver --- kuka_rsi_hw_interface/CMakeLists.txt | 18 ++++-- .../rsi_command_handler.hpp | 2 +- .../include/xml_handler}/xml_element.hpp | 0 .../include/xml_handler}/xml_param.hpp | 0 .../include/xml_handler}/xml_string.hpp | 0 .../src}/rsi_command_handler.cpp | 0 .../src/xml_handler}/xml_element.cpp | 2 +- .../src/xml_handler}/xml_param.cpp | 2 +- .../src/xml_handler}/xml_string.cpp | 2 +- xml_handler/CMakeLists.txt | 34 ------------ xml_handler/decodeTest.cpp | 55 ------------------- 11 files changed, 17 insertions(+), 98 deletions(-) rename {xml_handler => kuka_rsi_hw_interface/include/kuka_rsi_hw_interface}/rsi_command_handler.hpp (98%) rename {xml_handler => kuka_rsi_hw_interface/include/xml_handler}/xml_element.hpp (100%) rename {xml_handler => kuka_rsi_hw_interface/include/xml_handler}/xml_param.hpp (100%) rename {xml_handler => kuka_rsi_hw_interface/include/xml_handler}/xml_string.hpp (100%) rename {xml_handler => kuka_rsi_hw_interface/src}/rsi_command_handler.cpp (100%) rename {xml_handler => kuka_rsi_hw_interface/src/xml_handler}/xml_element.cpp (99%) rename {xml_handler => kuka_rsi_hw_interface/src/xml_handler}/xml_param.cpp (97%) rename {xml_handler => kuka_rsi_hw_interface/src/xml_handler}/xml_string.cpp (96%) delete mode 100644 xml_handler/CMakeLists.txt delete mode 100644 xml_handler/decodeTest.cpp diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 71edd5bf..b417a21c 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -8,7 +8,7 @@ endif() # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -29,21 +29,29 @@ find_package(TinyXML REQUIRED) include_directories(include ${TinyXML2_INCLUDE_DIRS}) +add_library(xml_handler SHARED + src/xml_handler/xml_element.cpp + src/xml_handler/xml_param.cpp + src/xml_handler/xml_string.cpp +) + add_library(${PROJECT_NAME} SHARED src/kuka_hardware_interface.cpp + src/rsi_command_handler.cpp ) +target_link_libraries(${PROJECT_NAME} xml_handler) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") + # prevent pluginlib from using boost target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface) target_link_libraries(${PROJECT_NAME} tinyxml) - - add_executable(robot_manager_node src/robot_manager_node.cpp) ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager_msgs) @@ -51,7 +59,7 @@ target_link_libraries(robot_manager_node kroshu_ros2_core::communication_helpers pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml) -install(TARGETS ${PROJECT_NAME} robot_manager_node +install(TARGETS ${PROJECT_NAME} robot_manager_node xml_handler DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) @@ -73,7 +81,7 @@ if(BUILD_TESTING) ament_xmllint(--exclude ros_rsi.rsi.xml) endif() -## EXPORTS +# # EXPORTS ament_export_include_directories( include ) diff --git a/xml_handler/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp similarity index 98% rename from xml_handler/rsi_command_handler.hpp rename to kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 66d7c627..733345d9 100644 --- a/xml_handler/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -41,7 +41,7 @@ #define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_ -#include "xml_element.hpp" +#include "xml_handler/xml_element.hpp" namespace kuka_rsi_hw_interface { diff --git a/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp similarity index 100% rename from xml_handler/xml_element.hpp rename to kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp diff --git a/xml_handler/xml_param.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp similarity index 100% rename from xml_handler/xml_param.hpp rename to kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp diff --git a/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp similarity index 100% rename from xml_handler/xml_string.hpp rename to kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp diff --git a/xml_handler/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp similarity index 100% rename from xml_handler/rsi_command_handler.cpp rename to kuka_rsi_hw_interface/src/rsi_command_handler.cpp diff --git a/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp similarity index 99% rename from xml_handler/xml_element.cpp rename to kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index fc6e3ed3..b3e4af5d 100644 --- a/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -21,7 +21,7 @@ #include #include -#include "xml_element.hpp" +#include "xml_handler/xml_element.hpp" namespace xml { diff --git a/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp similarity index 97% rename from xml_handler/xml_param.cpp rename to kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index f3edc39d..b22e6e98 100644 --- a/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -15,7 +15,7 @@ #include #include -#include "xml_param.hpp" +#include "xml_handler/xml_param.hpp" namespace xml { diff --git a/xml_handler/xml_string.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp similarity index 96% rename from xml_handler/xml_string.cpp rename to kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp index 01095aa1..ca518104 100644 --- a/xml_handler/xml_string.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp @@ -14,7 +14,7 @@ #include -#include "xml_string.hpp" +#include "xml_handler/xml_string.hpp" namespace xml { diff --git a/xml_handler/CMakeLists.txt b/xml_handler/CMakeLists.txt deleted file mode 100644 index c20c5f9e..00000000 --- a/xml_handler/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(xml_handler) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -add_library(${PROJECT_NAME} SHARED - rsi_command_handler.cpp - xml_element.cpp - xml_string.cpp - xml_param.cpp -) - -add_executable(decode_test - decodeTest.cpp) - -target_link_libraries(decode_test PUBLIC xml_handler) - -install(TARGETS ${PROJECT_NAME} decode_test - DESTINATION lib/${PROJECT_NAME}) - -# # EXPORTS -install(DIRECTORY config launch - DESTINATION share/${PROJECT_NAME}) \ No newline at end of file diff --git a/xml_handler/decodeTest.cpp b/xml_handler/decodeTest.cpp deleted file mode 100644 index c39156a7..00000000 --- a/xml_handler/decodeTest.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "rsi_command_handler.hpp" - -#include - -int main(int argc, char const * argv[]) -{ - kuka_rsi_hw_interface::RSICommandHandler handler; - char xml_state[1024] = - { - "55"}; - if (!handler.Decode(xml_state, 1024)) { - std::cout << "decode failed" << std::endl; - return -1; - } - - - std::cout << "decode successful" << std::endl; - - handler.GetState().operator<<(std::cout) << std::endl; - bool param; - handler.GetState().GetElement("Out").GetParam("01", param); - auto str = handler.GetState().GetParam("TYPE"); - - std::cout << "encode data fill" << std::endl; - std::string msg = "KROSHU"; - xml::XMLString xmlStr(msg); - handler.SetCommandParam("Sen", "Type", xmlStr); - double x = 5.2; - double y = 6; - double z = 3.2; - double a = 12; - double b = 7; - double c = 4.35; - long DiO = 12012141212323123; - handler.SetCommandParam("RKorr", "X", x); - handler.SetCommandParam("RKorr", "Y", y); - handler.SetCommandParam("RKorr", "Z", z); - handler.SetCommandParam("RKorr", "A", a); - handler.SetCommandParam("RKorr", "B", b); - handler.SetCommandParam("RKorr", "C", c); - handler.SetCommandParam("DiO", "DiO", DiO); - - handler.GetCommand().operator<<(std::cout) << std::endl; - - char xml_command[1024] = {0}; - char * xml_command_it = xml_command; - if (!handler.Encode(xml_command_it, 1024)) { - std::cout << "decode failed" << std::endl; - return -1; - } - std::cout << "decode successful" << std::endl; - std::cout << xml_command << std::endl; - - return 0; -} From 61fcf4451dd94100f2fc2b0b567cfbc8545e3c76 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Wed, 2 Aug 2023 16:55:50 +0200 Subject: [PATCH 49/97] Corrected map find functions --- .../rsi_command_handler.hpp | 2 +- .../include/xml_handler/xml_element.hpp | 5 +- .../include/xml_handler/xml_string.hpp | 2 + .../src/rsi_command_handler.cpp | 5 +- .../src/xml_handler/xml_element.cpp | 11 +++- .../src/xml_handler/xml_string.cpp | 9 +++ xml_handler/decodeTest.cpp | 55 +++++++++++++++++++ 7 files changed, 83 insertions(+), 6 deletions(-) create mode 100644 xml_handler/decodeTest.cpp diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 733345d9..1388f6bc 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -51,7 +51,7 @@ class RSICommandHandler private: xml::XMLElement command_data_structure_; xml::XMLElement state_data_structure_; - char err_buf_[1024]; + char err_buf_[1500]; void decodeNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 827fa628..f486a6f7 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -29,12 +29,13 @@ namespace xml { inline bool operator<(const XMLString & a, const std::string & b) { - return a.length_ == b.length() && strncmp(a.data_ptr_, b.c_str(), a.length_) < 0; + return strncmp(a.data_ptr_, b.c_str(), a.length_) < 0; } inline bool operator<(const std::string & b, const XMLString & a) { - return a.length_ == b.length() && strncmp(b.c_str(), a.data_ptr_, a.length_) < 0; + // return a < b; + return strncmp(b.c_str(), a.data_ptr_, a.length_) < 0; } class XMLElement diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp index a7a4e5d6..260b0ab6 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -31,6 +31,8 @@ class XMLString bool operator==(const XMLString & rhs); bool operator==(const std::string & rhs); bool operator==(const char * & rhs); + friend std::ostream & operator<<(std::ostream & out, XMLString & xml_str); + }; } // xml diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 3c5f3e28..9fc3e0e9 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -126,9 +126,10 @@ void RSICommandHandler::decodeNode( } else { if (!element.IsParamNameValid(node_param, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected parameter ("); - i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_name.data_ptr_); + std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); + i += node_param.length_; std::sprintf( - err_buf_ + i, ") dose not match with any of the %s elements parameters.", + err_buf_ + i, ") does not match with any of the %s elements parameters.", element.GetName().c_str()); throw std::logic_error(err_buf_); } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index b3e4af5d..6633b0f0 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -53,13 +53,22 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) bool XMLElement::IsParamNameValid(XMLString & key, char * & str_ptr) { key = castXMLString(str_ptr); - return params_.find(key) != params_.end(); + std::cout << "detected param: " << key << std::endl; + auto param_it = params_.find(key); + if (param_it != params_.end()) { + std::cout << "stored param: " << param_it->first << std::endl; + return true; + } + return false; + // return params_.find(key) != params_.end(); } bool XMLElement::IsNameValid(XMLString & key, char * & str_ptr) { key = castXMLString(str_ptr); + std::cout << "detected name: " << key << std::endl; + std::cout << "stored name: " << name_ << std::endl; if (name_.length() != key.length_) { std::cout << "Registered names length is: " << name_.length() << ", Keys length is: " << key.length_ << std::endl; diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp index ca518104..e7c7198c 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "xml_handler/xml_string.hpp" @@ -41,4 +42,12 @@ bool XMLString::operator==(const char * & rhs) } return strncmp(rhs, data_ptr_, length_); } + +std::ostream & operator<<(std::ostream & out, XMLString & xml_str) +{ + for (size_t i = 0; i < xml_str.length_; i++) { + out << xml_str.data_ptr_[i]; + } + return out; +} } diff --git a/xml_handler/decodeTest.cpp b/xml_handler/decodeTest.cpp new file mode 100644 index 00000000..c39156a7 --- /dev/null +++ b/xml_handler/decodeTest.cpp @@ -0,0 +1,55 @@ +#include "rsi_command_handler.hpp" + +#include + +int main(int argc, char const * argv[]) +{ + kuka_rsi_hw_interface::RSICommandHandler handler; + char xml_state[1024] = + { + "55"}; + if (!handler.Decode(xml_state, 1024)) { + std::cout << "decode failed" << std::endl; + return -1; + } + + + std::cout << "decode successful" << std::endl; + + handler.GetState().operator<<(std::cout) << std::endl; + bool param; + handler.GetState().GetElement("Out").GetParam("01", param); + auto str = handler.GetState().GetParam("TYPE"); + + std::cout << "encode data fill" << std::endl; + std::string msg = "KROSHU"; + xml::XMLString xmlStr(msg); + handler.SetCommandParam("Sen", "Type", xmlStr); + double x = 5.2; + double y = 6; + double z = 3.2; + double a = 12; + double b = 7; + double c = 4.35; + long DiO = 12012141212323123; + handler.SetCommandParam("RKorr", "X", x); + handler.SetCommandParam("RKorr", "Y", y); + handler.SetCommandParam("RKorr", "Z", z); + handler.SetCommandParam("RKorr", "A", a); + handler.SetCommandParam("RKorr", "B", b); + handler.SetCommandParam("RKorr", "C", c); + handler.SetCommandParam("DiO", "DiO", DiO); + + handler.GetCommand().operator<<(std::cout) << std::endl; + + char xml_command[1024] = {0}; + char * xml_command_it = xml_command; + if (!handler.Encode(xml_command_it, 1024)) { + std::cout << "decode failed" << std::endl; + return -1; + } + std::cout << "decode successful" << std::endl; + std::cout << xml_command << std::endl; + + return 0; +} From 1e0c0bf502e83347601ad60fd02779876f52eeae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 3 Aug 2023 19:31:01 +0200 Subject: [PATCH 50/97] corrected decode function --- .../include/xml_handler/xml_element.hpp | 1 - .../include/xml_handler/xml_param.hpp | 1 + .../include/xml_handler/xml_string.hpp | 1 - .../src/rsi_command_handler.cpp | 88 +++++++++++++------ .../src/xml_handler/xml_element.cpp | 46 ++++++---- .../src/xml_handler/xml_param.cpp | 26 ++++++ xml_handler/decodeTest.cpp | 55 ------------ 7 files changed, 121 insertions(+), 97 deletions(-) delete mode 100644 xml_handler/decodeTest.cpp diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index f486a6f7..9af89f92 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -34,7 +34,6 @@ inline bool operator<(const XMLString & a, const std::string & b) inline bool operator<(const std::string & b, const XMLString & a) { - // return a < b; return strncmp(b.c_str(), a.data_ptr_, a.length_) < 0; } diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp index f5ea8e09..af68a9ae 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp @@ -39,6 +39,7 @@ class XMLParam XMLParam(XMLType type) : param_type_(type) {} size_t ParamSprint(char * & buffer_it, char * const buffer, const size_t buffer_size); + friend std::ostream & operator<<(std::ostream & out, class XMLParam & param); template T GetParam() const diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp index 260b0ab6..f54954b6 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -32,7 +32,6 @@ class XMLString bool operator==(const std::string & rhs); bool operator==(const char * & rhs); friend std::ostream & operator<<(std::ostream & out, XMLString & xml_str); - }; } // xml diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 9fc3e0e9..a54e0974 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -97,13 +97,13 @@ void RSICommandHandler::decodeNode( // Validate nodes name if (!element.IsNameValid(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected name ("); - i = std::snprintf(err_buf_ + i, node_name.length_, "%s", node_name.data_ptr_); - i = std::sprintf(err_buf_, ") does not match the elements name %s.", element.GetName().c_str()); + std::snprintf(err_buf_ + i, node_name.length_ + 1, "%s", node_name.data_ptr_); + i += node_name.length_; + std::sprintf( + err_buf_ + i, ") does not match the elements name %s.", + element.GetName().c_str()); throw std::logic_error(err_buf_); } - // char str[node_name.length_ + 1]; - // std::snprintf(str, node_name.length_ + 1, "%s", node_name.data_ptr_); - // std::cout << "Name found: " << str << std::endl; // Validate nodes params size_t numOfParam = 0; @@ -133,24 +133,44 @@ void RSICommandHandler::decodeNode( element.GetName().c_str()); throw std::logic_error(err_buf_); } - for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '"'; buffer_it++) { + if (*buffer_it != '=') { + auto i = sprintf(err_buf_, "In \""); + std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); + i += node_param.length_; + std::sprintf(err_buf_ + i, "\" param syntax error found"); + throw std::logic_error(err_buf_); } - if ((size_t)(buffer_it - buffer) >= buffer_size) { - throw std::range_error("Out of the buffers range"); + buffer_it++; + if (*buffer_it != '"') { + auto i = sprintf(err_buf_, "In \""); + std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); + i += node_param.length_; + std::sprintf(err_buf_ + i, "\" param syntax error found"); + throw std::logic_error(err_buf_); } buffer_it++; if (!element.CastParam(node_param, buffer_it)) { - auto i = std::sprintf(err_buf_, "The not cast the "); - i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_param.data_ptr_); + auto i = std::sprintf(err_buf_, "Could not cast the "); + std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); + i += node_param.length_; std::sprintf( err_buf_ + i, " param into the %s elements parameter list.", element.GetName().c_str()); throw std::logic_error(err_buf_); } + if (*buffer_it != '"') { + auto i = sprintf(err_buf_, "In \""); + std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); + i += node_param.length_; + std::sprintf(err_buf_ + i, "\" param syntax error found"); + throw std::logic_error(err_buf_); + } buffer_it++; numOfParam++; } } + + // Fast forward to close bracket for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '>'; buffer_it++) { } if ((size_t)(buffer_it - buffer) >= buffer_size) { @@ -158,10 +178,19 @@ void RSICommandHandler::decodeNode( } buffer_it++; + // Could not find all parameter, or found too much if (isBaseLessNode && numOfParam != element.GetParams().size()) { std::sprintf( err_buf_, - "%lu parameter found it does not match with %s elements parameter list size.", numOfParam, + "%lu parameter found. It does not match with %s elements parameter list size.", numOfParam, + element.GetName().c_str()); + throw std::logic_error(err_buf_); + } + + // Node should have childs, but did not found any + if (isBaseLessNode && element.GetChilds().size() > 0) { + std::sprintf( + err_buf_, "%s node should have chields but did not found any", element.GetName().c_str()); throw std::logic_error(err_buf_); } @@ -171,17 +200,26 @@ void RSICommandHandler::decodeNode( if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } + if (!isBaseLessNode) { if (*buffer_it != '<') { // Node base is data if (!element.CastParam(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "Could not cast the "); - i = std::snprintf(err_buf_ + i, node_param.length_, "%s", node_param.data_ptr_); + std::snprintf(err_buf_ + i, node_name.length_ + 1, "%s", node_name.data_ptr_); + i += node_name.length_; std::sprintf( err_buf_ + i, " parameter into the %s elements parameter list", element.GetName().c_str()); throw std::logic_error(err_buf_); } + if (*buffer_it != '<') { + auto i = sprintf(err_buf_, "In \""); + std::snprintf(err_buf_ + i, node_name.length_ + 1, "%s", node_name.data_ptr_); + i += node_name.length_; + std::sprintf(err_buf_ + i, "\" param syntax error found"); + throw std::logic_error(err_buf_); + } } else { // node base has childs for (auto && child : element.GetChilds()) { @@ -193,22 +231,31 @@ void RSICommandHandler::decodeNode( if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } - if (*(buffer_it + 1) != '/') { + buffer_it++; + if (*(buffer_it) != '/') { std::sprintf( err_buf_, - "Start of an end Node, where there should be none. Error came in the %s node", + "Syntax error, while node end in the %s node", element.GetName().c_str()); throw std::logic_error(err_buf_); } - buffer_it += 2; + buffer_it++; if (!element.IsNameValid(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected name ("); - i = std::snprintf(err_buf_ + i, node_name.length_, "%s", node_name.data_ptr_); + std::snprintf(err_buf_ + i, node_name.length_ + 1, "%s", node_name.data_ptr_); + i += node_name.length_; std::sprintf( err_buf_ + i, ") does not match the elements name: %s", element.GetName().c_str()); throw std::logic_error(err_buf_); } + if (*buffer_it != '>') { + std::sprintf( + err_buf_, + "Syntax error, while node end in the %s node", + element.GetName().c_str()); + throw std::logic_error(err_buf_); + } } } @@ -225,7 +272,6 @@ void RSICommandHandler::encodeNode( if (element.GetChilds().size() <= 0) { isBaseLessNode = true; } - std::cout << "Added params: " << std::endl; for (auto && param : element.GetParams()) { if (element.GetName() == param.first) { isBaseLessNode = false; @@ -236,7 +282,6 @@ void RSICommandHandler::encodeNode( } buffer_it += idx; param.second.ParamSprint(buffer_it, buffer, buffer_size); - std::cout << param.first << ", "; idx = sprintf(buffer_it, "\" "); if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { throw std::range_error("Out of the buffers range"); @@ -244,13 +289,6 @@ void RSICommandHandler::encodeNode( buffer_it += idx; } } - std::cout << std::endl; - // if (element.GetChilds().size() <= 0 && isBaseLessNode == false) { - // std::sprintf( - // err_buf_, "The %s node has no chiled and has no base data", - // element.GetName().c_str()); - // throw std::logic_error(err_buf_); - // } if (isBaseLessNode) { idx = sprintf(buffer_it, "/>"); if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 6633b0f0..eb1fa306 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -30,16 +30,41 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) auto param_it = params_.find(key); auto ret_val = param_it != params_.end(); if (ret_val) { + errno = 0; switch (param_it->second.param_type_) { case XMLType::BOOL: - param_it->second.param_ = (bool)strtol(str_ptr, &str_ptr, 0); - break; + { + char * end; + auto res = (bool)strtol(str_ptr, &end, 0); + if (res == 0 && (errno != 0 || end == str_ptr)) { + return false; + } + str_ptr = end; + param_it->second.param_ = res; + break; + } case XMLType::LONG: - param_it->second.param_ = strtol(str_ptr, &str_ptr, 0); - break; + { + char * end; + auto res = strtol(str_ptr, &end, 0); + if (res == 0 && (errno != 0 || end == str_ptr)) { + return false; + } + str_ptr = end; + param_it->second.param_ = res; + break; + } case XMLType::DOUBLE: - param_it->second.param_ = strtod(str_ptr, &str_ptr); - break; + { + char * end; + auto res = strtod(str_ptr, &end); + if (res == 0 && (errno != 0 || end == str_ptr)) { + return false; + } + str_ptr = end; + param_it->second.param_ = res; + break; + } case XMLType::STRING: param_it->second.param_ = castXMLString(str_ptr); break; @@ -70,8 +95,6 @@ bool XMLElement::IsNameValid(XMLString & key, char * & str_ptr) std::cout << "detected name: " << key << std::endl; std::cout << "stored name: " << name_ << std::endl; if (name_.length() != key.length_) { - std::cout << "Registered names length is: " << name_.length() << ", Keys length is: " << - key.length_ << std::endl; return false; } return strncmp(name_.c_str(), key.data_ptr_, key.length_) == 0; @@ -150,16 +173,9 @@ XMLString XMLElement::castXMLString(char * & str_ptr) *str_ptr != '\0' && *str_ptr != '"' && *str_ptr != ' ' && *str_ptr != '=' && *str_ptr != '>' && *str_ptr != '/' && *str_ptr != '<'; str_ptr++) { - // std::cout << *str_ptr; } - // std::cout << std::endl; auto data = XMLString( start_ref, (size_t)(str_ptr - start_ref)); - // char str[data.length_ + 1]; - // snprintf(str, data.length_ + 1, "%s", data.data_ptr_); - // std::cout << "Cast length: " << data.length_ << std::endl; - // std::cout << "Cast Data: " << str << std::endl; - // std::cout << "Cast all Data: " << data.data_ptr_ << std::endl; return data; } } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index b22e6e98..8fca2d3b 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "xml_handler/xml_param.hpp" @@ -50,4 +51,29 @@ size_t XMLParam::ParamSprint(char * & buffer_it, char * const buffer, const size buffer_it += idx; return idx; } + +std::ostream & operator<<(std::ostream & out, class XMLParam & param) +{ + switch (param.param_type_) { + case XMLType::BOOL: + out << std::get<0>(param.param_); + break; + + case XMLType::LONG: + out << std::get<1>(param.param_); + break; + + case XMLType::DOUBLE: + out << std::get<2>(param.param_); + break; + + case XMLType::STRING: + out << std::get<3>(param.param_); + break; + default: + throw std::logic_error("Parameter type not supported"); + break; + } + return out; +} } diff --git a/xml_handler/decodeTest.cpp b/xml_handler/decodeTest.cpp deleted file mode 100644 index c39156a7..00000000 --- a/xml_handler/decodeTest.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "rsi_command_handler.hpp" - -#include - -int main(int argc, char const * argv[]) -{ - kuka_rsi_hw_interface::RSICommandHandler handler; - char xml_state[1024] = - { - "55"}; - if (!handler.Decode(xml_state, 1024)) { - std::cout << "decode failed" << std::endl; - return -1; - } - - - std::cout << "decode successful" << std::endl; - - handler.GetState().operator<<(std::cout) << std::endl; - bool param; - handler.GetState().GetElement("Out").GetParam("01", param); - auto str = handler.GetState().GetParam("TYPE"); - - std::cout << "encode data fill" << std::endl; - std::string msg = "KROSHU"; - xml::XMLString xmlStr(msg); - handler.SetCommandParam("Sen", "Type", xmlStr); - double x = 5.2; - double y = 6; - double z = 3.2; - double a = 12; - double b = 7; - double c = 4.35; - long DiO = 12012141212323123; - handler.SetCommandParam("RKorr", "X", x); - handler.SetCommandParam("RKorr", "Y", y); - handler.SetCommandParam("RKorr", "Z", z); - handler.SetCommandParam("RKorr", "A", a); - handler.SetCommandParam("RKorr", "B", b); - handler.SetCommandParam("RKorr", "C", c); - handler.SetCommandParam("DiO", "DiO", DiO); - - handler.GetCommand().operator<<(std::cout) << std::endl; - - char xml_command[1024] = {0}; - char * xml_command_it = xml_command; - if (!handler.Encode(xml_command_it, 1024)) { - std::cout << "decode failed" << std::endl; - return -1; - } - std::cout << "decode successful" << std::endl; - std::cout << xml_command << std::endl; - - return 0; -} From d86ad0e035ceeac75938866bd48e75da909d6308 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 4 Aug 2023 13:30:21 +0200 Subject: [PATCH 51/97] remove unnecessary files (merge error) --- .../config/controller_joint_names.yaml | 1 - .../config/hardware_controllers.yaml | 18 ------- .../src/rsi_control_node.cpp | 51 ------------------- 3 files changed, 70 deletions(-) delete mode 100644 kuka_rsi_hw_interface/config/controller_joint_names.yaml delete mode 100644 kuka_rsi_hw_interface/config/hardware_controllers.yaml delete mode 100644 kuka_rsi_hw_interface/src/rsi_control_node.cpp diff --git a/kuka_rsi_hw_interface/config/controller_joint_names.yaml b/kuka_rsi_hw_interface/config/controller_joint_names.yaml deleted file mode 100644 index 5f8f0343..00000000 --- a/kuka_rsi_hw_interface/config/controller_joint_names.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"] diff --git a/kuka_rsi_hw_interface/config/hardware_controllers.yaml b/kuka_rsi_hw_interface/config/hardware_controllers.yaml deleted file mode 100644 index 991bb3c9..00000000 --- a/kuka_rsi_hw_interface/config/hardware_controllers.yaml +++ /dev/null @@ -1,18 +0,0 @@ -#Publish all joint states -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - -# Joint trajectory controller -position_trajectory_controller: - type: "position_controllers/JointTrajectoryController" - joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 - - state_publish_rate: 50 # Defaults to 50 - action_monitor_rate: 20 # Defaults to 20 diff --git a/kuka_rsi_hw_interface/src/rsi_control_node.cpp b/kuka_rsi_hw_interface/src/rsi_control_node.cpp deleted file mode 100644 index 01532512..00000000 --- a/kuka_rsi_hw_interface/src/rsi_control_node.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 Áron Svastits -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - auto executor = std::make_shared(); - auto controller_manager = std::make_shared( - executor, - "controller_manager"); - - std::thread control_loop([controller_manager]() { - const rclcpp::Duration dt = - rclcpp::Duration::from_seconds(1.0 / controller_manager->get_update_rate()); - - while (rclcpp::ok()) { - controller_manager->read(controller_manager->now(), dt); - controller_manager->update(controller_manager->now(), dt); - controller_manager->write(controller_manager->now(), dt); - } - }); - - executor->add_node(controller_manager); - - executor->spin(); - control_loop.join(); - - // shutdown - rclcpp::shutdown(); - - return 0; -} From 6e00b0eba99c50eb348966a1d8cd20c4bc3a7be1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Mon, 7 Aug 2023 10:52:00 +0200 Subject: [PATCH 52/97] Small pr changes --- .../rsi_command_handler.hpp | 26 ++++----- .../include/xml_handler/xml_element.hpp | 11 ++-- .../src/rsi_command_handler.cpp | 54 +++++++++---------- .../src/xml_handler/xml_element.cpp | 4 +- 4 files changed, 47 insertions(+), 48 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 1388f6bc..2b0b2d96 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -48,19 +48,6 @@ namespace kuka_rsi_hw_interface class RSICommandHandler { -private: - xml::XMLElement command_data_structure_; - xml::XMLElement state_data_structure_; - char err_buf_[1500]; - - void decodeNode( - xml::XMLElement & element, char * const buffer, char * & buffer_it, - const size_t buffer_size); - - void encodeNode( - xml::XMLElement & element, char * const buffer, char * & buffer_it, - const size_t buffer_size); - public: RSICommandHandler(); ~RSICommandHandler() = default; @@ -82,6 +69,19 @@ class RSICommandHandler bool Decode(char * const buffer, const size_t buffer_size); bool Encode(char * & buffer, const size_t buffer_size); + +private: + xml::XMLElement command_data_structure_; + xml::XMLElement state_data_structure_; + char err_buf_[1500]; + + void decodeNode( + xml::XMLElement & element, char * const buffer, char * & buffer_it, + const size_t buffer_size); + + void encodeNode( + xml::XMLElement & element, char * const buffer, char * & buffer_it, + const size_t buffer_size); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 9af89f92..8aa3df8f 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -54,18 +54,17 @@ class XMLElement inline std::string GetName() const {return name_;} inline void SetName(const std::string & name) {name_ = name;} - inline std::vector GetChilds() const {return childs_;} - inline std::vector & GetChilds() {return childs_;} - inline std::map> & GetParams() {return params_;} + inline const std::vector & GetChilds() const {return childs_;} + inline std::vector & Childs() {return childs_;} + inline std::map> & Params() {return params_;} + XMLElement & Element(const std::string & elementName); + const XMLElement & GetElement(const std::string & elementName) const; bool CastParam(const XMLString & key, char * & str_ptr); bool IsParamNameValid(XMLString & key, char * & str_ptr); bool IsNameValid(XMLString & key, char * & str_ptr); - XMLElement & GetElement(const std::string & elementName); - const XMLElement & GetElement(const std::string & elementName) const; - std::ostream & operator<<(std::ostream & out) const; // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index a54e0974..e782d872 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -17,7 +17,7 @@ #include #include -#include "rsi_command_handler.hpp" +#include "kuka_rsi_hw_interface/rsi_command_handler.hpp" namespace kuka_rsi_hw_interface { @@ -25,31 +25,31 @@ RSICommandHandler::RSICommandHandler() : state_data_structure_("Rob"), command_data_structure_("Sen"), err_buf_("") { // Later this should be defined by the rsi xml - state_data_structure_.GetParams()["TYPE"] = xml::XMLParam(xml::XMLType::STRING); + state_data_structure_.Params()["TYPE"] = xml::XMLParam(xml::XMLType::STRING); // how to get string: std::get(command_data_structure_.params_["TYPE"]).first xml::XMLElement Out("Out"); - Out.GetParams()["01"] = xml::XMLParam(xml::XMLType::BOOL); - Out.GetParams()["02"] = xml::XMLParam(xml::XMLType::BOOL); - Out.GetParams()["03"] = xml::XMLParam(xml::XMLType::BOOL); - Out.GetParams()["04"] = xml::XMLParam(xml::XMLType::BOOL); - Out.GetParams()["05"] = xml::XMLParam(xml::XMLType::BOOL); + Out.Params()["01"] = xml::XMLParam(xml::XMLType::BOOL); + Out.Params()["02"] = xml::XMLParam(xml::XMLType::BOOL); + Out.Params()["03"] = xml::XMLParam(xml::XMLType::BOOL); + Out.Params()["04"] = xml::XMLParam(xml::XMLType::BOOL); + Out.Params()["05"] = xml::XMLParam(xml::XMLType::BOOL); xml::XMLElement Override("Override"); - Override.GetParams()["Override"] = xml::XMLParam(xml::XMLType::LONG); - state_data_structure_.GetChilds().emplace_back(Out); - state_data_structure_.GetChilds().emplace_back(Override); + Override.Params()["Override"] = xml::XMLParam(xml::XMLType::LONG); + state_data_structure_.Childs().emplace_back(Out); + state_data_structure_.Childs().emplace_back(Override); - command_data_structure_.GetParams()["Type"] = xml::XMLParam(xml::XMLType::STRING); + command_data_structure_.Params()["Type"] = xml::XMLParam(xml::XMLType::STRING); xml::XMLElement RKorr("RKorr"); - RKorr.GetParams()["X"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.GetParams()["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.GetParams()["Z"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.GetParams()["A"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.GetParams()["B"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.GetParams()["C"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.Params()["X"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.Params()["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.Params()["Z"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.Params()["A"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.Params()["B"] = xml::XMLParam(xml::XMLType::DOUBLE); + RKorr.Params()["C"] = xml::XMLParam(xml::XMLType::DOUBLE); xml::XMLElement DiO("DiO"); - DiO.GetParams()["DiO"] = xml::XMLParam(xml::XMLType::LONG); - command_data_structure_.GetChilds().emplace_back(RKorr); - command_data_structure_.GetChilds().emplace_back(DiO); + DiO.Params()["DiO"] = xml::XMLParam(xml::XMLType::LONG); + command_data_structure_.Childs().emplace_back(RKorr); + command_data_structure_.Childs().emplace_back(DiO); } bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) @@ -69,7 +69,7 @@ bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) bool RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) { - std::cout << "Decode started" << std::endl; + std::cout << "Encode started" << std::endl; auto buffer_it = buffer; try { encodeNode(command_data_structure_, buffer, buffer_it, buffer_size); @@ -78,7 +78,7 @@ bool RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) std::cerr << e.what() << '\n'; return false; } - std::cout << "Decode finished" << std::endl; + std::cout << "Encode finished" << std::endl; return true; } @@ -179,7 +179,7 @@ void RSICommandHandler::decodeNode( buffer_it++; // Could not find all parameter, or found too much - if (isBaseLessNode && numOfParam != element.GetParams().size()) { + if (isBaseLessNode && numOfParam != element.Params().size()) { std::sprintf( err_buf_, "%lu parameter found. It does not match with %s elements parameter list size.", numOfParam, @@ -222,7 +222,7 @@ void RSICommandHandler::decodeNode( } } else { // node base has childs - for (auto && child : element.GetChilds()) { + for (auto && child : element.Childs()) { decodeNode(child, buffer, buffer_it, buffer_size); } } @@ -272,7 +272,7 @@ void RSICommandHandler::encodeNode( if (element.GetChilds().size() <= 0) { isBaseLessNode = true; } - for (auto && param : element.GetParams()) { + for (auto && param : element.Params()) { if (element.GetName() == param.first) { isBaseLessNode = false; } else { @@ -304,12 +304,12 @@ void RSICommandHandler::encodeNode( buffer_it += idx; if (element.GetChilds().size() > 0) { // Add childs - for (auto && child : element.GetChilds()) { + for (auto && child : element.Childs()) { encodeNode(child, buffer, buffer_it, buffer_size); } } else { // Add data - element.GetParams().find(element.GetName())->second.ParamSprint( + element.Params().find(element.GetName())->second.ParamSprint( buffer_it, buffer, buffer_size); } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index eb1fa306..1d1e0fcc 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -100,14 +100,14 @@ bool XMLElement::IsNameValid(XMLString & key, char * & str_ptr) return strncmp(name_.c_str(), key.data_ptr_, key.length_) == 0; } -XMLElement & XMLElement::GetElement(const std::string & elementName) +XMLElement & XMLElement::Element(const std::string & elementName) { if (elementName == name_) { return *this; } for (auto && child : childs_) { if (elementName == child.name_) { - return child.GetElement(elementName); + return child.Element(elementName); } } char err_buf[1000]; From 76ca597e722bb9daf5119e580c7364ddcff501f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Mon, 14 Aug 2023 16:34:07 +0200 Subject: [PATCH 53/97] Integrate command handler into rsi hardware interface --- .../kuka_hardware_interface.hpp | 12 +-- .../rsi_command_handler.hpp | 6 +- .../kuka_rsi_hw_interface/udp_server.h | 14 ++-- .../include/xml_handler/xml_string.hpp | 6 +- .../src/kuka_hardware_interface.cpp | 83 ++++++++++++++++--- .../src/rsi_command_handler.cpp | 77 ++++++++++++----- .../src/xml_handler/xml_element.cpp | 6 +- 7 files changed, 151 insertions(+), 53 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 7b409e86..df752391 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -42,8 +42,9 @@ #define KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ #include -#include -#include +#include +// #include +// #include #include #include @@ -112,11 +113,10 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface std::vector joint_pos_correction_deg_; uint64_t ipoc_ = 0; - RSIState rsi_state_; - RSICommand rsi_command_; + RSICommandHandler command_handler_; std::unique_ptr server_; - std::string in_buffer_; - std::string out_buffer_; + char in_buffer_[UDP_BUFFER_SIZE] = {0}; + char out_buffer_[UDP_BUFFER_SIZE] = {0}; static constexpr double R2D = 180 / M_PI; static constexpr double D2R = M_PI / 180; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 2b0b2d96..672069e0 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -57,10 +57,12 @@ class RSICommandHandler inline const xml::XMLElement & GetCommand() const {return command_data_structure_;} template - bool SetCommandParam(const std::string & elementName, const std::string & paramName, T & param) + bool SetCommandParam( + const std::string & elementName, const std::string & paramName, + const T & param) { try { - return command_data_structure_.GetElement(elementName).SetParam(paramName, param); + return command_data_structure_.Element(elementName).SetParam(paramName, param); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; return false; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 4a2c7fe7..22b3fad1 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -59,7 +59,7 @@ #include "rclcpp/rclcpp.hpp" -constexpr size_t BUFFER_SIZE = 1024; +constexpr size_t UDP_BUFFER_SIZE = 1024; class UDPServer { @@ -133,9 +133,11 @@ class UDPServer } if (FD_ISSET(sockfd_, &read_fds)) { - memset(buffer_, 0, BUFFER_SIZE); + memset(buffer_, 0, UDP_BUFFER_SIZE); bytes = - recvfrom(sockfd_, buffer_, BUFFER_SIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); + recvfrom( + sockfd_, buffer_, UDP_BUFFER_SIZE, 0, (struct sockaddr *) &clientaddr_, + &clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } @@ -144,9 +146,9 @@ class UDPServer } } else { - memset(buffer_, 0, BUFFER_SIZE); + memset(buffer_, 0, UDP_BUFFER_SIZE); bytes = recvfrom( - sockfd_, buffer_, BUFFER_SIZE, 0, (struct sockaddr *) &clientaddr_, + sockfd_, buffer_, UDP_BUFFER_SIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); @@ -168,7 +170,7 @@ class UDPServer socklen_t clientlen_; struct sockaddr_in serveraddr_; struct sockaddr_in clientaddr_; - char buffer_[BUFFER_SIZE]; + char buffer_[UDP_BUFFER_SIZE]; int optval; }; diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp index f54954b6..f92f14b3 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -24,8 +24,12 @@ class XMLString public: const char * data_ptr_; size_t length_; - XMLString(const char * data_ptr = nullptr, size_t length = 0) + XMLString(const char * data_ptr, size_t length) : data_ptr_(data_ptr), length_(length) {} + XMLString() + : data_ptr_(nullptr), length_(0) {} + XMLString(const char * data_ptr) + : data_ptr_(data_ptr), length_(strlen(data_ptr)) {} XMLString(const std::string & str) : data_ptr_(str.c_str()), length_(str.length()) {} bool operator==(const XMLString & rhs); diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index c5caae3e..11129edc 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -53,6 +53,8 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw return CallbackReturn::ERROR; } + command_handler_ = RSICommandHandler(); + hw_states_.resize(info_.joints.size(), 0.0); hw_commands_.resize(info_.joints.size(), 0.0); @@ -152,17 +154,50 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta //TODO (Komáromi): Decode received msg struct and create a struct //TODO (Komáromi): Receive msg - rsi_state_ = RSIState(in_buffer_); + command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE); + + // Position data + hw_states_[0] = command_handler_.GetState().GetElement("AIPos").GetParam("A1") * + KukaRSIHardwareInterface::D2R; + hw_states_[1] = command_handler_.GetState().GetElement("AIPos").GetParam("A2") * + KukaRSIHardwareInterface::D2R; + hw_states_[2] = command_handler_.GetState().GetElement("AIPos").GetParam("A3") * + KukaRSIHardwareInterface::D2R; + hw_states_[3] = command_handler_.GetState().GetElement("AIPos").GetParam("A4") * + KukaRSIHardwareInterface::D2R; + hw_states_[4] = command_handler_.GetState().GetElement("AIPos").GetParam("A5") * + KukaRSIHardwareInterface::D2R; + hw_states_[5] = command_handler_.GetState().GetElement("AIPos").GetParam("A6") * + KukaRSIHardwareInterface::D2R; + + // Initial position data + initial_joint_pos_[0] = command_handler_.GetState().GetElement("ASPos").GetParam("A1") * + KukaRSIHardwareInterface::D2R; + initial_joint_pos_[1] = command_handler_.GetState().GetElement("ASPos").GetParam("A2") * + KukaRSIHardwareInterface::D2R; + initial_joint_pos_[2] = command_handler_.GetState().GetElement("ASPos").GetParam("A3") * + KukaRSIHardwareInterface::D2R; + initial_joint_pos_[3] = command_handler_.GetState().GetElement("ASPos").GetParam("A4") * + KukaRSIHardwareInterface::D2R; + initial_joint_pos_[4] = command_handler_.GetState().GetElement("ASPos").GetParam("A5") * + KukaRSIHardwareInterface::D2R; + initial_joint_pos_[5] = command_handler_.GetState().GetElement("ASPos").GetParam("A6") * + KukaRSIHardwareInterface::D2R; + + // Ipoc data + ipoc_ = command_handler_.GetState().GetElement("IPOC").GetParam("IPOC"); for (size_t i = 0; i < info_.joints.size(); ++i) { - hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; hw_commands_[i] = hw_states_[i]; - initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; + // initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; } - ipoc_ = rsi_state_.ipoc; + // ipoc_ = rsi_state_.ipoc; //TODO (Komáromi): construct and send msg - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; + auto out_buffer_it = out_buffer_; + command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE); + // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); server_->set_timeout(1000); // Set receive timeout to 1 second @@ -194,12 +229,25 @@ return_type KukaRSIHardwareInterface::read( this->on_deactivate(this->get_state()); return return_type::ERROR; } - rsi_state_ = RSIState(in_buffer_); - - for (std::size_t i = 0; i < info_.joints.size(); ++i) { - hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; - } - ipoc_ = rsi_state_.ipoc; + command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE); + + // for (std::size_t i = 0; i < info_.joints.size(); ++i) { + // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + // } + hw_states_[0] = command_handler_.GetState().GetElement("AIPos").GetParam("A1") * + KukaRSIHardwareInterface::D2R; + hw_states_[1] = command_handler_.GetState().GetElement("AIPos").GetParam("A2") * + KukaRSIHardwareInterface::D2R; + hw_states_[2] = command_handler_.GetState().GetElement("AIPos").GetParam("A3") * + KukaRSIHardwareInterface::D2R; + hw_states_[3] = command_handler_.GetState().GetElement("AIPos").GetParam("A4") * + KukaRSIHardwareInterface::D2R; + hw_states_[4] = command_handler_.GetState().GetElement("AIPos").GetParam("A5") * + KukaRSIHardwareInterface::D2R; + hw_states_[5] = command_handler_.GetState().GetElement("AIPos").GetParam("A6") * + KukaRSIHardwareInterface::D2R; + // ipoc_ = rsi_state_.ipoc; + ipoc_ = command_handler_.GetState().GetElement("IPOC").GetParam("IPOC"); return return_type::OK; } @@ -222,7 +270,18 @@ return_type KukaRSIHardwareInterface::write( KukaRSIHardwareInterface::R2D; } - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; + command_handler_.SetCommandParam("AK", "A1", joint_pos_correction_deg_[0]); + command_handler_.SetCommandParam("AK", "A2", joint_pos_correction_deg_[1]); + command_handler_.SetCommandParam("AK", "A3", joint_pos_correction_deg_[2]); + command_handler_.SetCommandParam("AK", "A4", joint_pos_correction_deg_[3]); + command_handler_.SetCommandParam("AK", "A5", joint_pos_correction_deg_[4]); + command_handler_.SetCommandParam("AK", "A6", joint_pos_correction_deg_[5]); + command_handler_.SetCommandParam("Stop", "Stop", stop_flag_); + command_handler_.SetCommandParam("IPOC", "IPOC", static_cast(ipoc_)); + + // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; + auto out_buffer_it = out_buffer_; + command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE); server_->send(out_buffer_); return return_type::OK; } diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index e782d872..6a6d587c 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -22,34 +22,65 @@ namespace kuka_rsi_hw_interface { RSICommandHandler::RSICommandHandler() -: state_data_structure_("Rob"), command_data_structure_("Sen"), err_buf_("") +: command_data_structure_("Sen"), state_data_structure_("Rob"), err_buf_("") { + // State structure // Later this should be defined by the rsi xml state_data_structure_.Params()["TYPE"] = xml::XMLParam(xml::XMLType::STRING); // how to get string: std::get(command_data_structure_.params_["TYPE"]).first - xml::XMLElement Out("Out"); - Out.Params()["01"] = xml::XMLParam(xml::XMLType::BOOL); - Out.Params()["02"] = xml::XMLParam(xml::XMLType::BOOL); - Out.Params()["03"] = xml::XMLParam(xml::XMLType::BOOL); - Out.Params()["04"] = xml::XMLParam(xml::XMLType::BOOL); - Out.Params()["05"] = xml::XMLParam(xml::XMLType::BOOL); - xml::XMLElement Override("Override"); - Override.Params()["Override"] = xml::XMLParam(xml::XMLType::LONG); - state_data_structure_.Childs().emplace_back(Out); - state_data_structure_.Childs().emplace_back(Override); + xml::XMLElement AIPos_el("AIPos"); + AIPos_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A3"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A4"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A5"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A6"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement ASPos_el("ASPos"); + ASPos_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A3"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A4"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A5"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A6"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement RIst_el("RIst"); + RIst_el.Params()["X"] = xml::XMLParam(xml::XMLType::DOUBLE); + RIst_el.Params()["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); + RIst_el.Params()["Z"] = xml::XMLParam(xml::XMLType::DOUBLE); + RIst_el.Params()["A"] = xml::XMLParam(xml::XMLType::DOUBLE); + RIst_el.Params()["B"] = xml::XMLParam(xml::XMLType::DOUBLE); + RIst_el.Params()["C"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement RSol_el("RSol"); + RSol_el.Params()["X"] = xml::XMLParam(xml::XMLType::DOUBLE); + RSol_el.Params()["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); + RSol_el.Params()["Z"] = xml::XMLParam(xml::XMLType::DOUBLE); + RSol_el.Params()["A"] = xml::XMLParam(xml::XMLType::DOUBLE); + RSol_el.Params()["B"] = xml::XMLParam(xml::XMLType::DOUBLE); + RSol_el.Params()["C"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement Ipoc_state_el("IPOC"); + Ipoc_state_el.Params()["IPOC"] = xml::XMLParam(xml::XMLType::LONG); + state_data_structure_.Childs().emplace_back(AIPos_el); + state_data_structure_.Childs().emplace_back(ASPos_el); + state_data_structure_.Childs().emplace_back(RIst_el); + state_data_structure_.Childs().emplace_back(RSol_el); + state_data_structure_.Childs().emplace_back(Ipoc_state_el); + // Command structure command_data_structure_.Params()["Type"] = xml::XMLParam(xml::XMLType::STRING); - xml::XMLElement RKorr("RKorr"); - RKorr.Params()["X"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.Params()["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.Params()["Z"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.Params()["A"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.Params()["B"] = xml::XMLParam(xml::XMLType::DOUBLE); - RKorr.Params()["C"] = xml::XMLParam(xml::XMLType::DOUBLE); - xml::XMLElement DiO("DiO"); - DiO.Params()["DiO"] = xml::XMLParam(xml::XMLType::LONG); - command_data_structure_.Childs().emplace_back(RKorr); - command_data_structure_.Childs().emplace_back(DiO); + command_data_structure_.SetParam("Type", xml::XMLString("KROSHU")); + xml::XMLElement ak_el("AK"); + ak_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); + ak_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); + ak_el.Params()["A3"] = xml::XMLParam(xml::XMLType::DOUBLE); + ak_el.Params()["A4"] = xml::XMLParam(xml::XMLType::DOUBLE); + ak_el.Params()["A5"] = xml::XMLParam(xml::XMLType::DOUBLE); + ak_el.Params()["A6"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement Stop_el("Stop"); + Stop_el.Params()["Stop"] = xml::XMLParam(xml::XMLType::BOOL); + xml::XMLElement Ipoc_command_el("IPOC"); + Ipoc_command_el.Params()["IPOC"] = xml::XMLParam(xml::XMLType::LONG); + command_data_structure_.Childs().emplace_back(ak_el); + command_data_structure_.Childs().emplace_back(Stop_el); + state_data_structure_.Childs().emplace_back(Ipoc_command_el); } bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) @@ -89,7 +120,7 @@ void RSICommandHandler::decodeNode( xml::XMLString node_name(buffer, 0); // fast forward for the first open bracket for (; *buffer_it != '<'; buffer_it++) { - if ((int)(buffer_it - buffer) >= (int)buffer_size) { + if ((int)(buffer_it - buffer) >= (int)buffer_size - 1) { throw std::range_error("Out of the buffers range"); } } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 1d1e0fcc..3ab39f02 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -34,7 +34,7 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) switch (param_it->second.param_type_) { case XMLType::BOOL: { - char * end; + char * end = str_ptr; auto res = (bool)strtol(str_ptr, &end, 0); if (res == 0 && (errno != 0 || end == str_ptr)) { return false; @@ -45,7 +45,7 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) } case XMLType::LONG: { - char * end; + char * end = str_ptr; auto res = strtol(str_ptr, &end, 0); if (res == 0 && (errno != 0 || end == str_ptr)) { return false; @@ -56,7 +56,7 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) } case XMLType::DOUBLE: { - char * end; + char * end = str_ptr; auto res = strtod(str_ptr, &end); if (res == 0 && (errno != 0 || end == str_ptr)) { return false; From df98db4fc3b1559b39ff64ab5065f5a3c57dfed5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Mon, 14 Aug 2023 16:34:57 +0200 Subject: [PATCH 54/97] Fix Element access bug now looking into the whole trees length --- .../src/xml_handler/xml_element.cpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 3ab39f02..c8479fae 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -100,34 +100,34 @@ bool XMLElement::IsNameValid(XMLString & key, char * & str_ptr) return strncmp(name_.c_str(), key.data_ptr_, key.length_) == 0; } -XMLElement & XMLElement::Element(const std::string & elementName) +XMLElement & XMLElement::Element(const std::string & elementName, int depth) { if (elementName == name_) { return *this; } for (auto && child : childs_) { - if (elementName == child.name_) { - return child.Element(elementName); - } + child.Element(elementName, depth + 1); + } + if (depth == 0) { + char err_buf[1000]; + sprintf(err_buf, "%s element not found", elementName.c_str()); + throw std::logic_error(err_buf); } - char err_buf[1000]; - sprintf(err_buf, "%s element not found", elementName.c_str()); - throw std::logic_error(err_buf); } -const XMLElement & XMLElement::GetElement(const std::string & elementName) const +const XMLElement & XMLElement::GetElement(const std::string & elementName, int depth) const { if (elementName == name_) { return *this; } for (auto && child : childs_) { - if (elementName == child.name_) { - return child.GetElement(elementName); - } + child.GetElement(elementName); + } + if (depth == 0) { + char err_buf[1000]; + sprintf(err_buf, "%s element not found", elementName.c_str()); + throw std::logic_error(err_buf); } - char err_buf[1000]; - sprintf(err_buf, "%s element not found", elementName.c_str()); - throw std::logic_error(err_buf); } From 91c31a6cce70ba7844aa7b0b3081cb29d78b4f14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Mon, 14 Aug 2023 16:36:31 +0200 Subject: [PATCH 55/97] fix operator< functions now they are consistent --- .../include/xml_handler/xml_element.hpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 8aa3df8f..343d9b44 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -29,12 +29,24 @@ namespace xml { inline bool operator<(const XMLString & a, const std::string & b) { - return strncmp(a.data_ptr_, b.c_str(), a.length_) < 0; + if (a.length_ < b.length()) { + return true; + } else if (a.length_ > b.length()) { + return false; + } else { + return strncmp(a.data_ptr_, b.c_str(), a.length_) < 0; + } } inline bool operator<(const std::string & b, const XMLString & a) { - return strncmp(b.c_str(), a.data_ptr_, a.length_) < 0; + if (b.length() < a.length_) { + return true; + } else if (b.length() > a.length_) { + return false; + } else { + return strncmp(b.c_str(), a.data_ptr_, a.length_) < 0; + } } class XMLElement @@ -58,8 +70,8 @@ class XMLElement inline std::vector & Childs() {return childs_;} inline std::map> & Params() {return params_;} - XMLElement & Element(const std::string & elementName); - const XMLElement & GetElement(const std::string & elementName) const; + XMLElement & Element(const std::string & elementName, int depth = 0); + const XMLElement & GetElement(const std::string & elementName, int depth = 0) const; bool CastParam(const XMLString & key, char * & str_ptr); bool IsParamNameValid(XMLString & key, char * & str_ptr); From af69f9b04c9a7ea83311f18ba5d047a6e3a78382 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Mon, 14 Aug 2023 18:01:59 +0200 Subject: [PATCH 56/97] fix error handling in encode --- .../rsi_command_handler.hpp | 5 ++- .../include/xml_handler/xml_param.hpp | 2 +- .../src/kuka_hardware_interface.cpp | 16 ++++++--- .../src/rsi_command_handler.cpp | 35 +++++++++---------- .../src/xml_handler/xml_param.cpp | 4 +-- 5 files changed, 34 insertions(+), 28 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 672069e0..cb469b43 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -70,7 +70,7 @@ class RSICommandHandler } bool Decode(char * const buffer, const size_t buffer_size); - bool Encode(char * & buffer, const size_t buffer_size); + int Encode(char * & buffer); private: xml::XMLElement command_data_structure_; @@ -82,8 +82,7 @@ class RSICommandHandler const size_t buffer_size); void encodeNode( - xml::XMLElement & element, char * const buffer, char * & buffer_it, - const size_t buffer_size); + xml::XMLElement & element, char * & buffer_it); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp index af68a9ae..b805e8da 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp @@ -38,7 +38,7 @@ class XMLParam XMLParam() = default; XMLParam(XMLType type) : param_type_(type) {} - size_t ParamSprint(char * & buffer_it, char * const buffer, const size_t buffer_size); + size_t PrintParam(char * & buffer_it); friend std::ostream & operator<<(std::ostream & out, class XMLParam & param); template diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 11129edc..9fbb291e 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -154,7 +154,9 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta //TODO (Komáromi): Decode received msg struct and create a struct //TODO (Komáromi): Receive msg - command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE); + if (!command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE)) { + return CallbackReturn::ERROR; + } // Position data hw_states_[0] = command_handler_.GetState().GetElement("AIPos").GetParam("A1") * @@ -196,7 +198,9 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta //TODO (Komáromi): construct and send msg auto out_buffer_it = out_buffer_; - command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE); + if (command_handler_.Encode(out_buffer_it) < 0) { + return CallbackReturn::ERROR; + } // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); server_->set_timeout(1000); // Set receive timeout to 1 second @@ -229,7 +233,9 @@ return_type KukaRSIHardwareInterface::read( this->on_deactivate(this->get_state()); return return_type::ERROR; } - command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE); + if (!command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE)) { + return return_type::ERROR; + } // for (std::size_t i = 0; i < info_.joints.size(); ++i) { // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; @@ -281,7 +287,9 @@ return_type KukaRSIHardwareInterface::write( // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; auto out_buffer_it = out_buffer_; - command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE); + if (command_handler_.Encode(out_buffer_it) < 0) { + return return_type::ERROR; + } server_->send(out_buffer_); return return_type::OK; } diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 6a6d587c..188178be 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -88,7 +88,7 @@ bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) std::cout << "Decode started" << std::endl; auto buffer_it = buffer; try { - decodeNode(state_data_structure_, buffer, buffer_it, buffer_size); + decodeNode(this->state_data_structure_, buffer, buffer_it, buffer_size); } catch (const std::exception & e) { std::cout << "### ERROR ###" << std::endl; std::cerr << e.what() << '\n'; @@ -98,19 +98,20 @@ bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) return true; } -bool RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) +int RSICommandHandler::Encode(char * & buffer) { std::cout << "Encode started" << std::endl; auto buffer_it = buffer; try { - encodeNode(command_data_structure_, buffer, buffer_it, buffer_size); + encodeNode(this->command_data_structure_, buffer_it); } catch (const std::exception & e) { std::cout << "### ERROR ###" << std::endl; std::cerr << e.what() << '\n'; - return false; + return -1; } std::cout << "Encode finished" << std::endl; - return true; + // +1 is for the \0 character + return (size_t)(buffer_it - buffer) + 1; } void RSICommandHandler::decodeNode( @@ -291,11 +292,10 @@ void RSICommandHandler::decodeNode( } void RSICommandHandler::encodeNode( - xml::XMLElement & element, char * const buffer, char * & buffer_it, - const size_t buffer_size) + xml::XMLElement & element, char * & buffer_it) { auto idx = sprintf(buffer_it, "<%s ", element.GetName().c_str()); - if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { + if (idx < 0) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; @@ -308,13 +308,13 @@ void RSICommandHandler::encodeNode( isBaseLessNode = false; } else { idx = sprintf(buffer_it, "%s=\"", param.first.c_str()); - if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { + if (idx < 0) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; - param.second.ParamSprint(buffer_it, buffer, buffer_size); + param.second.PrintParam(buffer_it); idx = sprintf(buffer_it, "\" "); - if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { + if (idx < 0) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; @@ -322,31 +322,30 @@ void RSICommandHandler::encodeNode( } if (isBaseLessNode) { idx = sprintf(buffer_it, "/>"); - if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { + if (idx < 0) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; } else { buffer_it--; idx = sprintf(buffer_it, ">"); - if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { + if (idx < 0) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; if (element.GetChilds().size() > 0) { // Add childs for (auto && child : element.Childs()) { - encodeNode(child, buffer, buffer_it, buffer_size); + encodeNode(child, buffer_it); } } else { // Add data - element.Params().find(element.GetName())->second.ParamSprint( - buffer_it, buffer, - buffer_size); + element.Params().find(element.GetName())->second.PrintParam( + buffer_it); } // Add end bracket idx = sprintf(buffer_it, "", element.GetName().c_str()); - if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { + if (idx < 0) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index 8fca2d3b..02bc48b0 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -20,7 +20,7 @@ namespace xml { -size_t XMLParam::ParamSprint(char * & buffer_it, char * const buffer, const size_t buffer_size) +size_t XMLParam::PrintParam(char * & buffer_it) { int idx = 0; switch (param_type_) { @@ -45,7 +45,7 @@ size_t XMLParam::ParamSprint(char * & buffer_it, char * const buffer, const size throw std::logic_error("Parameter type not supported"); break; } - if ((size_t)(buffer_it - buffer) + idx >= buffer_size) { + if (idx < 0) { throw std::range_error("Out of the buffers range"); } buffer_it += idx; From ac4d6cb0aa158bef0a6cffea30661446f852cdfd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 15 Aug 2023 16:57:14 +0200 Subject: [PATCH 57/97] fixed error handling in command handler --- .../rsi_command_handler.hpp | 7 +- .../include/xml_handler/xml_param.hpp | 4 +- .../include/xml_handler/xml_string.hpp | 3 + .../src/kuka_hardware_interface.cpp | 15 +- .../src/rsi_command_handler.cpp | 162 ++++++++++-------- .../src/xml_handler/xml_param.cpp | 39 ++--- .../src/xml_handler/xml_string.cpp | 10 ++ 7 files changed, 129 insertions(+), 111 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index cb469b43..62c84266 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -45,6 +45,7 @@ namespace kuka_rsi_hw_interface { +constexpr int err_buff_size_ = 1500; class RSICommandHandler { @@ -70,19 +71,19 @@ class RSICommandHandler } bool Decode(char * const buffer, const size_t buffer_size); - int Encode(char * & buffer); + int Encode(char * & buffer, const size_t buffer_size); private: xml::XMLElement command_data_structure_; xml::XMLElement state_data_structure_; - char err_buf_[1500]; + char err_buf_[err_buff_size_]; void decodeNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size); void encodeNode( - xml::XMLElement & element, char * & buffer_it); + xml::XMLElement & element, char * & buffer_it, int & size_left); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp index b805e8da..c9c824f4 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp @@ -21,7 +21,7 @@ namespace xml { -enum class XMLType : size_t +enum XMLType { BOOL = 0, LONG = 1, @@ -38,7 +38,7 @@ class XMLParam XMLParam() = default; XMLParam(XMLType type) : param_type_(type) {} - size_t PrintParam(char * & buffer_it); + int PrintParam(char * & buffer_it, int & size_left); friend std::ostream & operator<<(std::ostream & out, class XMLParam & param); template diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp index f92f14b3..9a62213f 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -32,6 +32,9 @@ class XMLString : data_ptr_(data_ptr), length_(strlen(data_ptr)) {} XMLString(const std::string & str) : data_ptr_(str.c_str()), length_(str.length()) {} + + int PrintString(char * & buffer_it, const int & size_left); + bool operator==(const XMLString & rhs); bool operator==(const std::string & rhs); bool operator==(const char * & rhs); diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 9fbb291e..b9e7709d 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -94,8 +94,8 @@ CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::Hardw joint_pos_correction_deg_.resize(info_.joints.size(), 0.0); ipoc_ = 0; - rsi_ip_address_ = info_.hardware_parameters["rsi_ip_address"]; - rsi_port_ = std::stoi(info_.hardware_parameters["rsi_port"]); + rsi_ip_address_ = info_.hardware_parameters["client_ip"]; + rsi_port_ = std::stoi(info_.hardware_parameters["client_port"]); RCLCPP_INFO( rclcpp::get_logger("KukaRSIHardwareInterface"), @@ -152,10 +152,11 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta bytes = server_->recv(in_buffer_); } - //TODO (Komáromi): Decode received msg struct and create a struct - //TODO (Komáromi): Receive msg + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "%s", in_buffer_); + return CallbackReturn::FAILURE; + if (!command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE)) { - return CallbackReturn::ERROR; + return CallbackReturn::FAILURE; } // Position data @@ -199,7 +200,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta //TODO (Komáromi): construct and send msg auto out_buffer_it = out_buffer_; if (command_handler_.Encode(out_buffer_it) < 0) { - return CallbackReturn::ERROR; + return CallbackReturn::FAILURE; } // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); @@ -234,6 +235,7 @@ return_type KukaRSIHardwareInterface::read( return return_type::ERROR; } if (!command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE)) { + this->on_deactivate(this->get_state()); return return_type::ERROR; } @@ -288,6 +290,7 @@ return_type KukaRSIHardwareInterface::write( // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; auto out_buffer_it = out_buffer_; if (command_handler_.Encode(out_buffer_it) < 0) { + this->on_deactivate(this->get_state()); return return_type::ERROR; } server_->send(out_buffer_); diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 188178be..caaee0c4 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -18,6 +18,7 @@ #include #include "kuka_rsi_hw_interface/rsi_command_handler.hpp" +#include "rclcpp/rclcpp.hpp" namespace kuka_rsi_hw_interface { @@ -28,20 +29,6 @@ RSICommandHandler::RSICommandHandler() // Later this should be defined by the rsi xml state_data_structure_.Params()["TYPE"] = xml::XMLParam(xml::XMLType::STRING); // how to get string: std::get(command_data_structure_.params_["TYPE"]).first - xml::XMLElement AIPos_el("AIPos"); - AIPos_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); - AIPos_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); - AIPos_el.Params()["A3"] = xml::XMLParam(xml::XMLType::DOUBLE); - AIPos_el.Params()["A4"] = xml::XMLParam(xml::XMLType::DOUBLE); - AIPos_el.Params()["A5"] = xml::XMLParam(xml::XMLType::DOUBLE); - AIPos_el.Params()["A6"] = xml::XMLParam(xml::XMLType::DOUBLE); - xml::XMLElement ASPos_el("ASPos"); - ASPos_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); - ASPos_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); - ASPos_el.Params()["A3"] = xml::XMLParam(xml::XMLType::DOUBLE); - ASPos_el.Params()["A4"] = xml::XMLParam(xml::XMLType::DOUBLE); - ASPos_el.Params()["A5"] = xml::XMLParam(xml::XMLType::DOUBLE); - ASPos_el.Params()["A6"] = xml::XMLParam(xml::XMLType::DOUBLE); xml::XMLElement RIst_el("RIst"); RIst_el.Params()["X"] = xml::XMLParam(xml::XMLType::DOUBLE); RIst_el.Params()["Y"] = xml::XMLParam(xml::XMLType::DOUBLE); @@ -56,17 +43,34 @@ RSICommandHandler::RSICommandHandler() RSol_el.Params()["A"] = xml::XMLParam(xml::XMLType::DOUBLE); RSol_el.Params()["B"] = xml::XMLParam(xml::XMLType::DOUBLE); RSol_el.Params()["C"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement AIPos_el("AIPos"); + AIPos_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A3"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A4"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A5"] = xml::XMLParam(xml::XMLType::DOUBLE); + AIPos_el.Params()["A6"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement ASPos_el("ASPos"); + ASPos_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A3"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A4"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A5"] = xml::XMLParam(xml::XMLType::DOUBLE); + ASPos_el.Params()["A6"] = xml::XMLParam(xml::XMLType::DOUBLE); + xml::XMLElement Delay_el("Delay"); + Delay_el.Params()["D"] = xml::XMLParam(xml::XMLType::LONG); xml::XMLElement Ipoc_state_el("IPOC"); Ipoc_state_el.Params()["IPOC"] = xml::XMLParam(xml::XMLType::LONG); - state_data_structure_.Childs().emplace_back(AIPos_el); - state_data_structure_.Childs().emplace_back(ASPos_el); state_data_structure_.Childs().emplace_back(RIst_el); state_data_structure_.Childs().emplace_back(RSol_el); + state_data_structure_.Childs().emplace_back(AIPos_el); + state_data_structure_.Childs().emplace_back(ASPos_el); + state_data_structure_.Childs().emplace_back(Delay_el); state_data_structure_.Childs().emplace_back(Ipoc_state_el); // Command structure command_data_structure_.Params()["Type"] = xml::XMLParam(xml::XMLType::STRING); - command_data_structure_.SetParam("Type", xml::XMLString("KROSHU")); + command_data_structure_.SetParam("Type", xml::XMLString("ImFree")); xml::XMLElement ak_el("AK"); ak_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); ak_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); @@ -85,33 +89,31 @@ RSICommandHandler::RSICommandHandler() bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) { - std::cout << "Decode started" << std::endl; auto buffer_it = buffer; try { decodeNode(this->state_data_structure_, buffer, buffer_it, buffer_size); + return true; } catch (const std::exception & e) { - std::cout << "### ERROR ###" << std::endl; - std::cerr << e.what() << '\n'; + RCLCPP_ERROR(rclcpp::get_logger("CommandHandler"), "%s", e.what()); return false; } - std::cout << "Decode finished" << std::endl; - return true; } -int RSICommandHandler::Encode(char * & buffer) +int RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) { - std::cout << "Encode started" << std::endl; auto buffer_it = buffer; + int size_left = buffer_size; try { - encodeNode(this->command_data_structure_, buffer_it); + encodeNode(this->command_data_structure_, buffer_it, size_left); + if (buffer_size - size_left != (size_t)(buffer_it - buffer)) { + throw std::range_error("Range error occured"); + } + // +1 is for the \0 character + return buffer_size - size_left + 1; } catch (const std::exception & e) { - std::cout << "### ERROR ###" << std::endl; - std::cerr << e.what() << '\n'; + RCLCPP_ERROR(rclcpp::get_logger("CommandHandler"), "%s", e.what()); return -1; } - std::cout << "Encode finished" << std::endl; - // +1 is for the \0 character - return (size_t)(buffer_it - buffer) + 1; } void RSICommandHandler::decodeNode( @@ -129,8 +131,8 @@ void RSICommandHandler::decodeNode( // Validate nodes name if (!element.IsNameValid(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected name ("); - std::snprintf(err_buf_ + i, node_name.length_ + 1, "%s", node_name.data_ptr_); - i += node_name.length_; + auto err_buff_it = err_buf_ + i; + i += node_name.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf( err_buf_ + i, ") does not match the elements name %s.", element.GetName().c_str()); @@ -158,8 +160,8 @@ void RSICommandHandler::decodeNode( } else { if (!element.IsParamNameValid(node_param, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected parameter ("); - std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); - i += node_param.length_; + auto err_buff_it = err_buf_ + i; + i += node_param.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf( err_buf_ + i, ") does not match with any of the %s elements parameters.", element.GetName().c_str()); @@ -167,24 +169,24 @@ void RSICommandHandler::decodeNode( } if (*buffer_it != '=') { auto i = sprintf(err_buf_, "In \""); - std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); - i += node_param.length_; + auto err_buff_it = err_buf_ + i; + i += node_param.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf(err_buf_ + i, "\" param syntax error found"); throw std::logic_error(err_buf_); } buffer_it++; if (*buffer_it != '"') { auto i = sprintf(err_buf_, "In \""); - std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); - i += node_param.length_; + auto err_buff_it = err_buf_ + i; + i += node_param.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf(err_buf_ + i, "\" param syntax error found"); throw std::logic_error(err_buf_); } buffer_it++; if (!element.CastParam(node_param, buffer_it)) { auto i = std::sprintf(err_buf_, "Could not cast the "); - std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); - i += node_param.length_; + auto err_buff_it = err_buf_ + i; + i += node_param.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf( err_buf_ + i, " param into the %s elements parameter list.", element.GetName().c_str()); @@ -192,8 +194,8 @@ void RSICommandHandler::decodeNode( } if (*buffer_it != '"') { auto i = sprintf(err_buf_, "In \""); - std::snprintf(err_buf_ + i, node_param.length_ + 1, "%s", node_param.data_ptr_); - i += node_param.length_; + auto err_buff_it = err_buf_ + i; + i += node_param.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf(err_buf_ + i, "\" param syntax error found"); throw std::logic_error(err_buf_); } @@ -238,8 +240,8 @@ void RSICommandHandler::decodeNode( // Node base is data if (!element.CastParam(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "Could not cast the "); - std::snprintf(err_buf_ + i, node_name.length_ + 1, "%s", node_name.data_ptr_); - i += node_name.length_; + auto err_buff_it = err_buf_ + i; + i += node_name.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf( err_buf_ + i, " parameter into the %s elements parameter list", element.GetName().c_str()); @@ -247,8 +249,8 @@ void RSICommandHandler::decodeNode( } if (*buffer_it != '<') { auto i = sprintf(err_buf_, "In \""); - std::snprintf(err_buf_ + i, node_name.length_ + 1, "%s", node_name.data_ptr_); - i += node_name.length_; + auto err_buff_it = err_buf_ + i; + i += node_name.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf(err_buf_ + i, "\" param syntax error found"); throw std::logic_error(err_buf_); } @@ -274,8 +276,8 @@ void RSICommandHandler::decodeNode( buffer_it++; if (!element.IsNameValid(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected name ("); - std::snprintf(err_buf_ + i, node_name.length_ + 1, "%s", node_name.data_ptr_); - i += node_name.length_; + auto err_buff_it = err_buf_ + i; + i += node_name.PrintString(err_buff_it, err_buff_size_ - i); std::sprintf( err_buf_ + i, ") does not match the elements name: %s", element.GetName().c_str()); @@ -292,63 +294,81 @@ void RSICommandHandler::decodeNode( } void RSICommandHandler::encodeNode( - xml::XMLElement & element, char * & buffer_it) + xml::XMLElement & element, char * & buffer_it, int & size_left) { - auto idx = sprintf(buffer_it, "<%s ", element.GetName().c_str()); - if (idx < 0) { + auto idx = snprintf(buffer_it, size_left, "<%s ", element.GetName().c_str()); + if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); + } else { + buffer_it += idx; + size_left -= idx; } - buffer_it += idx; bool isBaseLessNode = false; if (element.GetChilds().size() <= 0) { isBaseLessNode = true; } + auto param_idx = element.Params().size() - 1; for (auto && param : element.Params()) { if (element.GetName() == param.first) { isBaseLessNode = false; } else { - idx = sprintf(buffer_it, "%s=\"", param.first.c_str()); - if (idx < 0) { + idx = snprintf(buffer_it, size_left, "%s=\"", param.first.c_str()); + if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); + } else { + buffer_it += idx; + size_left -= idx; } - buffer_it += idx; - param.second.PrintParam(buffer_it); - idx = sprintf(buffer_it, "\" "); - if (idx < 0) { + param.second.PrintParam(buffer_it, size_left); + + if (param_idx == 0) { + idx = snprintf(buffer_it, size_left, "\""); + } else { + idx = snprintf(buffer_it, size_left, "\" "); + } + if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); + } else { + buffer_it += idx; + size_left -= idx; } - buffer_it += idx; + param_idx++; } } if (isBaseLessNode) { - idx = sprintf(buffer_it, "/>"); - if (idx < 0) { + idx = snprintf(buffer_it, size_left, " />"); + if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); + } else { + buffer_it += idx; + size_left -= idx; } - buffer_it += idx; } else { - buffer_it--; - idx = sprintf(buffer_it, ">"); - if (idx < 0) { + idx = snprintf(buffer_it, size_left, ">"); + if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); + } else { + buffer_it += idx; + size_left -= idx; } - buffer_it += idx; if (element.GetChilds().size() > 0) { // Add childs for (auto && child : element.Childs()) { - encodeNode(child, buffer_it); + encodeNode(child, buffer_it, size_left); } } else { // Add data element.Params().find(element.GetName())->second.PrintParam( - buffer_it); + buffer_it, size_left); } // Add end bracket - idx = sprintf(buffer_it, "", element.GetName().c_str()); - if (idx < 0) { + idx = snprintf(buffer_it, size_left, "", element.GetName().c_str()); + if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); + } else { + buffer_it += idx; + size_left -= idx; } - buffer_it += idx; } } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index 02bc48b0..a0a94950 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -20,60 +20,41 @@ namespace xml { -size_t XMLParam::PrintParam(char * & buffer_it) +int XMLParam::PrintParam(char * & buffer_it, int & size_left) { int idx = 0; switch (param_type_) { case XMLType::BOOL: - idx = sprintf(buffer_it, "%u", std::get<0>(param_)); + idx = snprintf(buffer_it, size_left, "%u", std::get(param_)); break; case XMLType::LONG: - idx = sprintf(buffer_it, "%lu", std::get<1>(param_)); + idx = snprintf(buffer_it, size_left, "%lu", std::get(param_)); break; case XMLType::DOUBLE: - idx = sprintf(buffer_it, "%f", std::get<2>(param_)); + idx = snprintf(buffer_it, size_left, "%f", std::get(param_)); break; case XMLType::STRING: - idx = std::snprintf( - buffer_it, std::get<3>(param_).length_ + 1, "%s", - std::get<3>(param_).data_ptr_); + idx = std::get(param_).PrintString(buffer_it, size_left); break; default: throw std::logic_error("Parameter type not supported"); break; } - if (idx < 0) { + if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); + } else { + buffer_it += idx; + size_left -= idx; } - buffer_it += idx; return idx; } std::ostream & operator<<(std::ostream & out, class XMLParam & param) { - switch (param.param_type_) { - case XMLType::BOOL: - out << std::get<0>(param.param_); - break; - - case XMLType::LONG: - out << std::get<1>(param.param_); - break; - - case XMLType::DOUBLE: - out << std::get<2>(param.param_); - break; - - case XMLType::STRING: - out << std::get<3>(param.param_); - break; - default: - throw std::logic_error("Parameter type not supported"); - break; - } + std::visit([&](auto && arg) {out << arg;}, param.param_); return out; } } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp index e7c7198c..14b6deb5 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp @@ -19,6 +19,16 @@ namespace xml { +int XMLString::PrintString(char * & buffer_it, const int & size_left) +{ + if (length_ + 1 > size_left) { + return -1; + } else { + snprintf(buffer_it, length_ + 1, "%s", data_ptr_); + return length_; + } +} + bool XMLString::operator==(const XMLString & rhs) { if (length_ != rhs.length_) { From fba8d4f7aad9fb48677e0f82e511d4dd5069f15e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Wed, 16 Aug 2023 15:45:35 +0200 Subject: [PATCH 58/97] working control tested with simulator --- .../rsi_command_handler.hpp | 6 +- .../include/xml_handler/xml_element.hpp | 9 ++- .../src/kuka_hardware_interface.cpp | 59 +++++++++++-------- .../src/rsi_command_handler.cpp | 33 ++++++----- .../src/xml_handler/xml_element.cpp | 26 ++++---- 5 files changed, 77 insertions(+), 56 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 62c84266..d47f2c5e 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -45,7 +45,7 @@ namespace kuka_rsi_hw_interface { -constexpr int err_buff_size_ = 1500; +constexpr int ERROR_BUFFER_SIZE = 1500; class RSICommandHandler { @@ -63,7 +63,7 @@ class RSICommandHandler const T & param) { try { - return command_data_structure_.Element(elementName).SetParam(paramName, param); + return command_data_structure_.Element(elementName)->SetParam(paramName, param); } catch (const std::exception & e) { std::cerr << e.what() << '\n'; return false; @@ -76,7 +76,7 @@ class RSICommandHandler private: xml::XMLElement command_data_structure_; xml::XMLElement state_data_structure_; - char err_buf_[err_buff_size_]; + char err_buf_[ERROR_BUFFER_SIZE]; void decodeNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 343d9b44..a2b80e83 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -53,6 +53,10 @@ class XMLElement { private: static XMLString castXMLString(char * & str_ptr); + XMLElement * element(const std::string & elementName, int depth = 0); + const XMLElement * const getElement( + const std::string & elementName, + int depth = 0) const; std::map> params_; std::vector childs_; @@ -70,8 +74,9 @@ class XMLElement inline std::vector & Childs() {return childs_;} inline std::map> & Params() {return params_;} - XMLElement & Element(const std::string & elementName, int depth = 0); - const XMLElement & GetElement(const std::string & elementName, int depth = 0) const; + inline XMLElement * Element(const std::string & elementName) {return element(elementName);} + inline const XMLElement * const GetElement( + const std::string & elementName) const {return getElement(elementName);} bool CastParam(const XMLString & key, char * & str_ptr); bool IsParamNameValid(XMLString & key, char * & str_ptr); diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index b9e7709d..fbeea962 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -152,43 +152,41 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta bytes = server_->recv(in_buffer_); } - RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "%s", in_buffer_); - return CallbackReturn::FAILURE; - if (!command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE)) { + RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "Decode failed"); return CallbackReturn::FAILURE; } // Position data - hw_states_[0] = command_handler_.GetState().GetElement("AIPos").GetParam("A1") * + hw_states_[0] = command_handler_.GetState().GetElement("AIPos")->GetParam("A1") * KukaRSIHardwareInterface::D2R; - hw_states_[1] = command_handler_.GetState().GetElement("AIPos").GetParam("A2") * + hw_states_[1] = command_handler_.GetState().GetElement("AIPos")->GetParam("A2") * KukaRSIHardwareInterface::D2R; - hw_states_[2] = command_handler_.GetState().GetElement("AIPos").GetParam("A3") * + hw_states_[2] = command_handler_.GetState().GetElement("AIPos")->GetParam("A3") * KukaRSIHardwareInterface::D2R; - hw_states_[3] = command_handler_.GetState().GetElement("AIPos").GetParam("A4") * + hw_states_[3] = command_handler_.GetState().GetElement("AIPos")->GetParam("A4") * KukaRSIHardwareInterface::D2R; - hw_states_[4] = command_handler_.GetState().GetElement("AIPos").GetParam("A5") * + hw_states_[4] = command_handler_.GetState().GetElement("AIPos")->GetParam("A5") * KukaRSIHardwareInterface::D2R; - hw_states_[5] = command_handler_.GetState().GetElement("AIPos").GetParam("A6") * + hw_states_[5] = command_handler_.GetState().GetElement("AIPos")->GetParam("A6") * KukaRSIHardwareInterface::D2R; // Initial position data - initial_joint_pos_[0] = command_handler_.GetState().GetElement("ASPos").GetParam("A1") * + initial_joint_pos_[0] = command_handler_.GetState().GetElement("ASPos")->GetParam("A1") * KukaRSIHardwareInterface::D2R; - initial_joint_pos_[1] = command_handler_.GetState().GetElement("ASPos").GetParam("A2") * + initial_joint_pos_[1] = command_handler_.GetState().GetElement("ASPos")->GetParam("A2") * KukaRSIHardwareInterface::D2R; - initial_joint_pos_[2] = command_handler_.GetState().GetElement("ASPos").GetParam("A3") * + initial_joint_pos_[2] = command_handler_.GetState().GetElement("ASPos")->GetParam("A3") * KukaRSIHardwareInterface::D2R; - initial_joint_pos_[3] = command_handler_.GetState().GetElement("ASPos").GetParam("A4") * + initial_joint_pos_[3] = command_handler_.GetState().GetElement("ASPos")->GetParam("A4") * KukaRSIHardwareInterface::D2R; - initial_joint_pos_[4] = command_handler_.GetState().GetElement("ASPos").GetParam("A5") * + initial_joint_pos_[4] = command_handler_.GetState().GetElement("ASPos")->GetParam("A5") * KukaRSIHardwareInterface::D2R; - initial_joint_pos_[5] = command_handler_.GetState().GetElement("ASPos").GetParam("A6") * + initial_joint_pos_[5] = command_handler_.GetState().GetElement("ASPos")->GetParam("A6") * KukaRSIHardwareInterface::D2R; // Ipoc data - ipoc_ = command_handler_.GetState().GetElement("IPOC").GetParam("IPOC"); + ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); for (size_t i = 0; i < info_.joints.size(); ++i) { // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; @@ -197,9 +195,20 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta } // ipoc_ = rsi_state_.ipoc; + // Initial command pos data + command_handler_.SetCommandParam("AK", "A1", joint_pos_correction_deg_[0]); + command_handler_.SetCommandParam("AK", "A2", joint_pos_correction_deg_[1]); + command_handler_.SetCommandParam("AK", "A3", joint_pos_correction_deg_[2]); + command_handler_.SetCommandParam("AK", "A4", joint_pos_correction_deg_[3]); + command_handler_.SetCommandParam("AK", "A5", joint_pos_correction_deg_[4]); + command_handler_.SetCommandParam("AK", "A6", joint_pos_correction_deg_[5]); + command_handler_.SetCommandParam("Stop", "Stop", stop_flag_); + command_handler_.SetCommandParam("IPOC", "IPOC", static_cast(ipoc_)); + //TODO (Komáromi): construct and send msg auto out_buffer_it = out_buffer_; - if (command_handler_.Encode(out_buffer_it) < 0) { + if (command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE) < 0) { + RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "Encode Failed"); return CallbackReturn::FAILURE; } // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; @@ -242,20 +251,20 @@ return_type KukaRSIHardwareInterface::read( // for (std::size_t i = 0; i < info_.joints.size(); ++i) { // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; // } - hw_states_[0] = command_handler_.GetState().GetElement("AIPos").GetParam("A1") * + hw_states_[0] = command_handler_.GetState().GetElement("AIPos")->GetParam("A1") * KukaRSIHardwareInterface::D2R; - hw_states_[1] = command_handler_.GetState().GetElement("AIPos").GetParam("A2") * + hw_states_[1] = command_handler_.GetState().GetElement("AIPos")->GetParam("A2") * KukaRSIHardwareInterface::D2R; - hw_states_[2] = command_handler_.GetState().GetElement("AIPos").GetParam("A3") * + hw_states_[2] = command_handler_.GetState().GetElement("AIPos")->GetParam("A3") * KukaRSIHardwareInterface::D2R; - hw_states_[3] = command_handler_.GetState().GetElement("AIPos").GetParam("A4") * + hw_states_[3] = command_handler_.GetState().GetElement("AIPos")->GetParam("A4") * KukaRSIHardwareInterface::D2R; - hw_states_[4] = command_handler_.GetState().GetElement("AIPos").GetParam("A5") * + hw_states_[4] = command_handler_.GetState().GetElement("AIPos")->GetParam("A5") * KukaRSIHardwareInterface::D2R; - hw_states_[5] = command_handler_.GetState().GetElement("AIPos").GetParam("A6") * + hw_states_[5] = command_handler_.GetState().GetElement("AIPos")->GetParam("A6") * KukaRSIHardwareInterface::D2R; // ipoc_ = rsi_state_.ipoc; - ipoc_ = command_handler_.GetState().GetElement("IPOC").GetParam("IPOC"); + ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); return return_type::OK; } @@ -289,7 +298,7 @@ return_type KukaRSIHardwareInterface::write( // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; auto out_buffer_it = out_buffer_; - if (command_handler_.Encode(out_buffer_it) < 0) { + if (command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE) < 0) { this->on_deactivate(this->get_state()); return return_type::ERROR; } diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index caaee0c4..f307f244 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -84,7 +84,7 @@ RSICommandHandler::RSICommandHandler() Ipoc_command_el.Params()["IPOC"] = xml::XMLParam(xml::XMLType::LONG); command_data_structure_.Childs().emplace_back(ak_el); command_data_structure_.Childs().emplace_back(Stop_el); - state_data_structure_.Childs().emplace_back(Ipoc_command_el); + command_data_structure_.Childs().emplace_back(Ipoc_command_el); } bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) @@ -132,7 +132,7 @@ void RSICommandHandler::decodeNode( if (!element.IsNameValid(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected name ("); auto err_buff_it = err_buf_ + i; - i += node_name.PrintString(err_buff_it, err_buff_size_ - i); + i += node_name.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf( err_buf_ + i, ") does not match the elements name %s.", element.GetName().c_str()); @@ -161,7 +161,7 @@ void RSICommandHandler::decodeNode( if (!element.IsParamNameValid(node_param, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected parameter ("); auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, err_buff_size_ - i); + i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf( err_buf_ + i, ") does not match with any of the %s elements parameters.", element.GetName().c_str()); @@ -170,7 +170,7 @@ void RSICommandHandler::decodeNode( if (*buffer_it != '=') { auto i = sprintf(err_buf_, "In \""); auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, err_buff_size_ - i); + i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf(err_buf_ + i, "\" param syntax error found"); throw std::logic_error(err_buf_); } @@ -178,7 +178,7 @@ void RSICommandHandler::decodeNode( if (*buffer_it != '"') { auto i = sprintf(err_buf_, "In \""); auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, err_buff_size_ - i); + i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf(err_buf_ + i, "\" param syntax error found"); throw std::logic_error(err_buf_); } @@ -186,7 +186,7 @@ void RSICommandHandler::decodeNode( if (!element.CastParam(node_param, buffer_it)) { auto i = std::sprintf(err_buf_, "Could not cast the "); auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, err_buff_size_ - i); + i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf( err_buf_ + i, " param into the %s elements parameter list.", element.GetName().c_str()); @@ -195,7 +195,7 @@ void RSICommandHandler::decodeNode( if (*buffer_it != '"') { auto i = sprintf(err_buf_, "In \""); auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, err_buff_size_ - i); + i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf(err_buf_ + i, "\" param syntax error found"); throw std::logic_error(err_buf_); } @@ -241,7 +241,7 @@ void RSICommandHandler::decodeNode( if (!element.CastParam(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "Could not cast the "); auto err_buff_it = err_buf_ + i; - i += node_name.PrintString(err_buff_it, err_buff_size_ - i); + i += node_name.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf( err_buf_ + i, " parameter into the %s elements parameter list", element.GetName().c_str()); @@ -250,7 +250,7 @@ void RSICommandHandler::decodeNode( if (*buffer_it != '<') { auto i = sprintf(err_buf_, "In \""); auto err_buff_it = err_buf_ + i; - i += node_name.PrintString(err_buff_it, err_buff_size_ - i); + i += node_name.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf(err_buf_ + i, "\" param syntax error found"); throw std::logic_error(err_buf_); } @@ -260,10 +260,12 @@ void RSICommandHandler::decodeNode( decodeNode(child, buffer, buffer_it, buffer_size); } } - for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '<'; buffer_it++) { - } - if ((size_t)(buffer_it - buffer) >= buffer_size) { - throw std::range_error("Out of the buffers range"); + if (*(buffer_it) != '<') { + std::sprintf( + err_buf_, + "Syntax error, while node end in the %s node", + element.GetName().c_str()); + throw std::logic_error(err_buf_); } buffer_it++; if (*(buffer_it) != '/') { @@ -277,7 +279,7 @@ void RSICommandHandler::decodeNode( if (!element.IsNameValid(node_name, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected name ("); auto err_buff_it = err_buf_ + i; - i += node_name.PrintString(err_buff_it, err_buff_size_ - i); + i += node_name.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); std::sprintf( err_buf_ + i, ") does not match the elements name: %s", element.GetName().c_str()); @@ -290,6 +292,7 @@ void RSICommandHandler::decodeNode( element.GetName().c_str()); throw std::logic_error(err_buf_); } + buffer_it++; } } @@ -320,7 +323,6 @@ void RSICommandHandler::encodeNode( size_left -= idx; } param.second.PrintParam(buffer_it, size_left); - if (param_idx == 0) { idx = snprintf(buffer_it, size_left, "\""); } else { @@ -370,6 +372,5 @@ void RSICommandHandler::encodeNode( size_left -= idx; } } - } } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index c8479fae..69d2d219 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -78,10 +78,8 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) bool XMLElement::IsParamNameValid(XMLString & key, char * & str_ptr) { key = castXMLString(str_ptr); - std::cout << "detected param: " << key << std::endl; auto param_it = params_.find(key); if (param_it != params_.end()) { - std::cout << "stored param: " << param_it->first << std::endl; return true; } return false; @@ -92,42 +90,50 @@ bool XMLElement::IsParamNameValid(XMLString & key, char * & str_ptr) bool XMLElement::IsNameValid(XMLString & key, char * & str_ptr) { key = castXMLString(str_ptr); - std::cout << "detected name: " << key << std::endl; - std::cout << "stored name: " << name_ << std::endl; if (name_.length() != key.length_) { return false; } return strncmp(name_.c_str(), key.data_ptr_, key.length_) == 0; } -XMLElement & XMLElement::Element(const std::string & elementName, int depth) +XMLElement * XMLElement::element(const std::string & elementName, int depth) { if (elementName == name_) { - return *this; + return this; } for (auto && child : childs_) { - child.Element(elementName, depth + 1); + auto ret_val = child.element(elementName, depth + 1); + if (ret_val != nullptr) { + return ret_val; + } } if (depth == 0) { char err_buf[1000]; sprintf(err_buf, "%s element not found", elementName.c_str()); throw std::logic_error(err_buf); } + return nullptr; } -const XMLElement & XMLElement::GetElement(const std::string & elementName, int depth) const +const XMLElement * const XMLElement::getElement( + const std::string & elementName, + int depth) const { if (elementName == name_) { - return *this; + return this; } for (auto && child : childs_) { - child.GetElement(elementName); + auto ret_val = child.getElement(elementName, depth + 1); + if (ret_val != nullptr) { + return ret_val; + } } if (depth == 0) { char err_buf[1000]; sprintf(err_buf, "%s element not found", elementName.c_str()); throw std::logic_error(err_buf); } + return nullptr; } From aa6a3ffd18fabba60a735affc5d0ccf2d9b1b879 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Wed, 16 Aug 2023 18:45:04 +0200 Subject: [PATCH 59/97] Small PR changes --- .../include/xml_handler/xml_element.hpp | 10 ++--- .../include/xml_handler/xml_param.hpp | 6 +-- .../src/rsi_command_handler.cpp | 8 ++-- .../src/xml_handler/xml_element.cpp | 44 ++----------------- .../src/xml_handler/xml_param.cpp | 10 ++--- 5 files changed, 20 insertions(+), 58 deletions(-) diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index a2b80e83..4c6827d3 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -82,8 +82,6 @@ class XMLElement bool IsParamNameValid(XMLString & key, char * & str_ptr); bool IsNameValid(XMLString & key, char * & str_ptr); - std::ostream & operator<<(std::ostream & out) const; - // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used template bool GetParam(const std::string & key, T & param) const @@ -93,19 +91,19 @@ class XMLElement if constexpr (std::is_same::value || std::is_same::value || std::is_same::value || std::is_same::value) { - param = std::get(param_it->second.param_); + param = param_it->second.GetParamValue(); return true; } } return false; } - + template T GetParam(const std::string & key) const { auto param_it = params_.find(key); if (param_it != params_.end()) { - return param_it->second.GetParam(); + return param_it->second.GetParamValue(); } throw std::range_error("Could not find key in parameter list"); } @@ -119,7 +117,7 @@ class XMLElement if constexpr (std::is_same::value || std::is_same::value || std::is_same::value || std::is_same::value) { - param_it->second.param_ = param; + param_it->second.param_value_ = param; return true; } } diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp index c9c824f4..f4bac444 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp @@ -34,7 +34,7 @@ class XMLParam { public: XMLType param_type_; - std::variant param_; + std::variant param_value_; XMLParam() = default; XMLParam(XMLType type) : param_type_(type) {} @@ -42,12 +42,12 @@ class XMLParam friend std::ostream & operator<<(std::ostream & out, class XMLParam & param); template - T GetParam() const + T GetParamValue() const { if constexpr (std::is_same::value || std::is_same::value || std::is_same::value || std::is_same::value) { - return std::get(param_); + return std::get(param_value_); } throw std::logic_error("Parameter type not supported"); } diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index f307f244..9cdd7d70 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -70,7 +70,7 @@ RSICommandHandler::RSICommandHandler() // Command structure command_data_structure_.Params()["Type"] = xml::XMLParam(xml::XMLType::STRING); - command_data_structure_.SetParam("Type", xml::XMLString("ImFree")); + command_data_structure_.SetParam("Type", xml::XMLString("KROSHU")); xml::XMLElement ak_el("AK"); ak_el.Params()["A1"] = xml::XMLParam(xml::XMLType::DOUBLE); ak_el.Params()["A2"] = xml::XMLParam(xml::XMLType::DOUBLE); @@ -263,7 +263,7 @@ void RSICommandHandler::decodeNode( if (*(buffer_it) != '<') { std::sprintf( err_buf_, - "Syntax error, while node end in the %s node", + "Syntax error at the end of %s node", element.GetName().c_str()); throw std::logic_error(err_buf_); } @@ -271,7 +271,7 @@ void RSICommandHandler::decodeNode( if (*(buffer_it) != '/') { std::sprintf( err_buf_, - "Syntax error, while node end in the %s node", + "Syntax error at the end of %s node", element.GetName().c_str()); throw std::logic_error(err_buf_); } @@ -288,7 +288,7 @@ void RSICommandHandler::decodeNode( if (*buffer_it != '>') { std::sprintf( err_buf_, - "Syntax error, while node end in the %s node", + "Syntax error at the end of %s node", element.GetName().c_str()); throw std::logic_error(err_buf_); } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 69d2d219..9f1e30a5 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -40,7 +40,7 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) return false; } str_ptr = end; - param_it->second.param_ = res; + param_it->second.param_value_ = res; break; } case XMLType::LONG: @@ -51,7 +51,7 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) return false; } str_ptr = end; - param_it->second.param_ = res; + param_it->second.param_value_ = res; break; } case XMLType::DOUBLE: @@ -62,11 +62,11 @@ bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) return false; } str_ptr = end; - param_it->second.param_ = res; + param_it->second.param_value_ = res; break; } case XMLType::STRING: - param_it->second.param_ = castXMLString(str_ptr); + param_it->second.param_value_ = castXMLString(str_ptr); break; default: return false; @@ -136,42 +136,6 @@ const XMLElement * const XMLElement::getElement( return nullptr; } - -std::ostream & XMLElement::operator<<(std::ostream & out) const -{ - out << "Element name: " << this->name_ << "\n"; - for (auto && param : params_) { - out << " - " << param.first << ": "; - switch (param.second.param_type_) { - case XMLType::BOOL: - out << std::get<0>(param.second.param_) << "\n"; - break; - case XMLType::LONG: - out << std::get<1>(param.second.param_) << "\n"; - break; - case XMLType::DOUBLE: - out << std::get<2>(param.second.param_) << "\n"; - break; - case XMLType::STRING: - { - char str[std::get<3>(param.second.param_).length_ + 1]; - std::snprintf( - str, std::get<3>(param.second.param_).length_ + 1, "%s", - std::get<3>(param.second.param_).data_ptr_); - out << str << "\n"; - } - break; - - default: - break; - } - } - for (auto && child : childs_) { - child.operator<<(out) << "\n"; - } - return out; -} - XMLString XMLElement::castXMLString(char * & str_ptr) { auto start_ref = str_ptr; diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index a0a94950..99cfe5ac 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -25,19 +25,19 @@ int XMLParam::PrintParam(char * & buffer_it, int & size_left) int idx = 0; switch (param_type_) { case XMLType::BOOL: - idx = snprintf(buffer_it, size_left, "%u", std::get(param_)); + idx = snprintf(buffer_it, size_left, "%u", GetParamValue()); break; case XMLType::LONG: - idx = snprintf(buffer_it, size_left, "%lu", std::get(param_)); + idx = snprintf(buffer_it, size_left, "%lu", GetParamValue()); break; case XMLType::DOUBLE: - idx = snprintf(buffer_it, size_left, "%f", std::get(param_)); + idx = snprintf(buffer_it, size_left, "%f", GetParamValue()); break; case XMLType::STRING: - idx = std::get(param_).PrintString(buffer_it, size_left); + idx = GetParamValue().PrintString(buffer_it, size_left); break; default: throw std::logic_error("Parameter type not supported"); @@ -54,7 +54,7 @@ int XMLParam::PrintParam(char * & buffer_it, int & size_left) std::ostream & operator<<(std::ostream & out, class XMLParam & param) { - std::visit([&](auto && arg) {out << arg;}, param.param_); + std::visit([&](auto && arg) {out << arg;}, param.param_value_); return out; } } From fc68638cf5b7f61aea9c7bd241adcd0cbc8137f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 17 Aug 2023 13:26:53 +0200 Subject: [PATCH 60/97] Update algorithm to be more stable plus added small pr changes --- .../src/rsi_command_handler.cpp | 35 ++++++++----------- 1 file changed, 14 insertions(+), 21 deletions(-) diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 9cdd7d70..57818f60 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -121,11 +121,12 @@ void RSICommandHandler::decodeNode( const size_t buffer_size) { xml::XMLString node_name(buffer, 0); - // fast forward for the first open bracket - for (; *buffer_it != '<'; buffer_it++) { - if ((int)(buffer_it - buffer) >= (int)buffer_size - 1) { - throw std::range_error("Out of the buffers range"); - } + if (*(buffer_it) != '<') { + std::sprintf( + err_buf_, + "Syntax error at the start of %s node", + element.GetName().c_str()); + throw std::logic_error(err_buf_); } buffer_it++; // Validate nodes name @@ -151,12 +152,14 @@ void RSICommandHandler::decodeNode( if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } - // Check for the nodes hadders end characters + // Check for the nodes headers end characters if (*buffer_it == '/' && *(buffer_it + 1) == '>') { isBaseLessNode = true; isNoMoreParam = true; + buffer_it += 2; } else if (*buffer_it == '>') { isNoMoreParam = true; + buffer_it++; } else { if (!element.IsParamNameValid(node_param, buffer_it)) { auto i = std::sprintf(err_buf_, "The detected parameter ("); @@ -203,14 +206,9 @@ void RSICommandHandler::decodeNode( numOfParam++; } } - - // Fast forward to close bracket - for (; (size_t)(buffer_it - buffer) < buffer_size && *buffer_it != '>'; buffer_it++) { - } if ((size_t)(buffer_it - buffer) >= buffer_size) { throw std::range_error("Out of the buffers range"); } - buffer_it++; // Could not find all parameter, or found too much if (isBaseLessNode && numOfParam != element.Params().size()) { @@ -299,7 +297,7 @@ void RSICommandHandler::decodeNode( void RSICommandHandler::encodeNode( xml::XMLElement & element, char * & buffer_it, int & size_left) { - auto idx = snprintf(buffer_it, size_left, "<%s ", element.GetName().c_str()); + auto idx = snprintf(buffer_it, size_left, "<%s", element.GetName().c_str()); if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); } else { @@ -307,15 +305,14 @@ void RSICommandHandler::encodeNode( size_left -= idx; } bool isBaseLessNode = false; - if (element.GetChilds().size() <= 0) { + if (element.GetChilds().size() == 0) { isBaseLessNode = true; } - auto param_idx = element.Params().size() - 1; for (auto && param : element.Params()) { if (element.GetName() == param.first) { isBaseLessNode = false; } else { - idx = snprintf(buffer_it, size_left, "%s=\"", param.first.c_str()); + idx = snprintf(buffer_it, size_left, " %s=\"", param.first.c_str()); if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); } else { @@ -323,18 +320,14 @@ void RSICommandHandler::encodeNode( size_left -= idx; } param.second.PrintParam(buffer_it, size_left); - if (param_idx == 0) { - idx = snprintf(buffer_it, size_left, "\""); - } else { - idx = snprintf(buffer_it, size_left, "\" "); - } + + idx = snprintf(buffer_it, size_left, "\""); if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); } else { buffer_it += idx; size_left -= idx; } - param_idx++; } } if (isBaseLessNode) { From 95a3f7cafeb9bb9910c84a669498c2651336ee19 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 17 Aug 2023 15:32:38 +0200 Subject: [PATCH 61/97] Updating Error handling and added comments --- .../rsi_command_handler.hpp | 2 - .../src/rsi_command_handler.cpp | 224 +++++++++--------- 2 files changed, 113 insertions(+), 113 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index d47f2c5e..3bfbd361 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -45,7 +45,6 @@ namespace kuka_rsi_hw_interface { -constexpr int ERROR_BUFFER_SIZE = 1500; class RSICommandHandler { @@ -76,7 +75,6 @@ class RSICommandHandler private: xml::XMLElement command_data_structure_; xml::XMLElement state_data_structure_; - char err_buf_[ERROR_BUFFER_SIZE]; void decodeNode( xml::XMLElement & element, char * const buffer, char * & buffer_it, diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 57818f60..e16b2e0a 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -23,7 +23,7 @@ namespace kuka_rsi_hw_interface { RSICommandHandler::RSICommandHandler() -: command_data_structure_("Sen"), state_data_structure_("Rob"), err_buf_("") +: command_data_structure_("Sen"), state_data_structure_("Rob") { // State structure // Later this should be defined by the rsi xml @@ -122,27 +122,23 @@ void RSICommandHandler::decodeNode( { xml::XMLString node_name(buffer, 0); if (*(buffer_it) != '<') { - std::sprintf( - err_buf_, - "Syntax error at the start of %s node", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "Syntax error at the start of " << element.GetName() << " node."; + throw std::logic_error(err_ss.str()); } buffer_it++; // Validate nodes name if (!element.IsNameValid(node_name, buffer_it)) { - auto i = std::sprintf(err_buf_, "The detected name ("); - auto err_buff_it = err_buf_ + i; - i += node_name.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf( - err_buf_ + i, ") does not match the elements name %s.", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "The detected start node name \"" << node_name << + "\" does not match the elements name " << + element.GetName() << "."; + throw std::logic_error(err_ss.str()); } // Validate nodes params - size_t numOfParam = 0; - bool isBaseLessNode = false; + size_t num_of_param = 0; + bool is_param_only_node = false; bool isNoMoreParam = false; xml::XMLString node_param; while (!isNoMoreParam && (size_t)(buffer_it - buffer) < buffer_size) { @@ -150,145 +146,132 @@ void RSICommandHandler::decodeNode( for (; *buffer_it == ' '; buffer_it++) { } if ((size_t)(buffer_it - buffer) >= buffer_size) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } - // Check for the nodes headers end characters + // Check for the start nodes end characters if (*buffer_it == '/' && *(buffer_it + 1) == '>') { - isBaseLessNode = true; + is_param_only_node = true; isNoMoreParam = true; buffer_it += 2; } else if (*buffer_it == '>') { isNoMoreParam = true; buffer_it++; } else { + // Validates params name if (!element.IsParamNameValid(node_param, buffer_it)) { - auto i = std::sprintf(err_buf_, "The detected parameter ("); - auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf( - err_buf_ + i, ") does not match with any of the %s elements parameters.", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "The detected parameter \"" << node_param << + "\" does not match with any of the " << element.GetName() << " elements parameters."; + throw std::logic_error(err_ss.str()); } + // Checks xml syntax if (*buffer_it != '=') { - auto i = sprintf(err_buf_, "In \""); - auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf(err_buf_ + i, "\" param syntax error found"); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "In \"" << node_param << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); } buffer_it++; if (*buffer_it != '"') { - auto i = sprintf(err_buf_, "In \""); - auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf(err_buf_ + i, "\" param syntax error found"); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "In \"" << node_param << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); } buffer_it++; + // Casts param data if (!element.CastParam(node_param, buffer_it)) { - auto i = std::sprintf(err_buf_, "Could not cast the "); - auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf( - err_buf_ + i, " param into the %s elements parameter list.", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "Could not cast the \"" << node_param << "\" param into the " << + element.GetName() << " elements parameter list."; + throw std::logic_error(err_ss.str()); } + // Checks xml syntax if (*buffer_it != '"') { - auto i = sprintf(err_buf_, "In \""); - auto err_buff_it = err_buf_ + i; - i += node_param.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf(err_buf_ + i, "\" param syntax error found"); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "In \"" << node_param << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); } buffer_it++; - numOfParam++; + num_of_param++; } } if ((size_t)(buffer_it - buffer) >= buffer_size) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } // Could not find all parameter, or found too much - if (isBaseLessNode && numOfParam != element.Params().size()) { - std::sprintf( - err_buf_, - "%lu parameter found. It does not match with %s elements parameter list size.", numOfParam, - element.GetName().c_str()); - throw std::logic_error(err_buf_); + // Checks if it is a param only node, + // because the normal leaf nodes data is stored within the elemets parameters. + // So in case of a normal leaf node the num_of_param and the Params().size() does not match. + if (is_param_only_node && num_of_param != element.Params().size()) { + std::stringstream err_ss; + err_ss << num_of_param << " parameter found. It does not match with " << element.GetName() << + "elements parameter list size."; + throw std::logic_error(err_ss.str()); } // Node should have childs, but did not found any - if (isBaseLessNode && element.GetChilds().size() > 0) { - std::sprintf( - err_buf_, "%s node should have chields but did not found any", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + if (is_param_only_node && element.GetChilds().size() > 0) { + std::stringstream err_ss; + err_ss << element.GetName() << " node should have chields but did not found any."; + throw std::logic_error(err_ss.str()); } - // fast forward if + // Fast forward until the next node or the end of the node for (; *buffer_it == ' '; buffer_it++) { } if ((size_t)(buffer_it - buffer) >= buffer_size) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } - if (!isBaseLessNode) { + // If a node not an only param node, therefore has data or childs and need an end node + if (!is_param_only_node) { if (*buffer_it != '<') { - // Node base is data + // Node is a leaf node, checking its data if (!element.CastParam(node_name, buffer_it)) { - auto i = std::sprintf(err_buf_, "Could not cast the "); - auto err_buff_it = err_buf_ + i; - i += node_name.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf( - err_buf_ + i, " parameter into the %s elements parameter list", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "Could not cast the \"" << node_name << "\" parameter into the " << + element.GetName() << " elements parameter list."; + throw std::logic_error(err_ss.str()); } if (*buffer_it != '<') { - auto i = sprintf(err_buf_, "In \""); - auto err_buff_it = err_buf_ + i; - i += node_name.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf(err_buf_ + i, "\" param syntax error found"); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "In \"" << node_param << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); } } else { - // node base has childs + // Node has childs for (auto && child : element.Childs()) { decodeNode(child, buffer, buffer_it, buffer_size); } } + // Checking the end node for syntax and node name if (*(buffer_it) != '<') { - std::sprintf( - err_buf_, - "Syntax error at the end of %s node", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "Syntax error at the end of " << element.GetName() << " node."; + throw std::logic_error(err_ss.str()); } buffer_it++; if (*(buffer_it) != '/') { - std::sprintf( - err_buf_, - "Syntax error at the end of %s node", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "Syntax error at the end of " << element.GetName() << " node."; + throw std::logic_error(err_ss.str()); } buffer_it++; if (!element.IsNameValid(node_name, buffer_it)) { - auto i = std::sprintf(err_buf_, "The detected name ("); - auto err_buff_it = err_buf_ + i; - i += node_name.PrintString(err_buff_it, ERROR_BUFFER_SIZE - i); - std::sprintf( - err_buf_ + i, ") does not match the elements name: %s", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "The detected name \"" << node_name << "\" does not match the elements name: " << + element.GetName() << "."; + throw std::logic_error(err_ss.str()); } if (*buffer_it != '>') { - std::sprintf( - err_buf_, - "Syntax error at the end of %s node", - element.GetName().c_str()); - throw std::logic_error(err_buf_); + std::stringstream err_ss; + err_ss << "Syntax error at the end of " << element.GetName() << " node."; + throw std::logic_error(err_ss.str()); } buffer_it++; } @@ -297,43 +280,58 @@ void RSICommandHandler::decodeNode( void RSICommandHandler::encodeNode( xml::XMLElement & element, char * & buffer_it, int & size_left) { + // Start the node with its name auto idx = snprintf(buffer_it, size_left, "<%s", element.GetName().c_str()); if (idx < 0 || idx > size_left) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } else { buffer_it += idx; size_left -= idx; } - bool isBaseLessNode = false; + bool is_param_only_node = false; + // If the node do not have childs it is probably a param only node if (element.GetChilds().size() == 0) { - isBaseLessNode = true; + is_param_only_node = true; } for (auto && param : element.Params()) { + // For a parameter and its node having the same name is not supported, + // because it means that it is not a param only, just a leaf if (element.GetName() == param.first) { - isBaseLessNode = false; + is_param_only_node = false; } else { + // Creating parameters + // Add param name idx = snprintf(buffer_it, size_left, " %s=\"", param.first.c_str()); if (idx < 0 || idx > size_left) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } else { buffer_it += idx; size_left -= idx; } + // Add param data param.second.PrintParam(buffer_it, size_left); - idx = snprintf(buffer_it, size_left, "\""); if (idx < 0 || idx > size_left) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } else { buffer_it += idx; size_left -= idx; } } } - if (isBaseLessNode) { + // Add start node end bracket + if (is_param_only_node) { idx = snprintf(buffer_it, size_left, " />"); if (idx < 0 || idx > size_left) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } else { buffer_it += idx; size_left -= idx; @@ -341,7 +339,9 @@ void RSICommandHandler::encodeNode( } else { idx = snprintf(buffer_it, size_left, ">"); if (idx < 0 || idx > size_left) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } else { buffer_it += idx; size_left -= idx; @@ -356,10 +356,12 @@ void RSICommandHandler::encodeNode( element.Params().find(element.GetName())->second.PrintParam( buffer_it, size_left); } - // Add end bracket + // Add node end idx = snprintf(buffer_it, size_left, "", element.GetName().c_str()); if (idx < 0 || idx > size_left) { - throw std::range_error("Out of the buffers range"); + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); } else { buffer_it += idx; size_left -= idx; From 09106d2fe761ce012e8c443cee738130b96ba26c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 17 Aug 2023 15:50:07 +0200 Subject: [PATCH 62/97] CI changes --- kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp | 8 ++++---- kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp | 6 +++--- kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp | 2 +- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 8 ++++---- kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp | 2 +- kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp | 4 ++-- kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp | 2 +- 7 files changed, 16 insertions(+), 16 deletions(-) diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 4c6827d3..907ae420 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -88,7 +88,7 @@ class XMLElement { auto param_it = params_.find(key); if (param_it != params_.end()) { - if constexpr (std::is_same::value || std::is_same::value || + if constexpr (std::is_same::value || std::is_same::value || std::is_same::value || std::is_same::value) { param = param_it->second.GetParamValue(); @@ -97,7 +97,7 @@ class XMLElement } return false; } - + template T GetParam(const std::string & key) const { @@ -114,7 +114,7 @@ class XMLElement { auto param_it = params_.find(key); if (param_it != params_.end()) { - if constexpr (std::is_same::value || std::is_same::value || + if constexpr (std::is_same::value || std::is_same::value || std::is_same::value || std::is_same::value) { param_it->second.param_value_ = param; @@ -124,6 +124,6 @@ class XMLElement return false; } }; -} +} // namespace xml #endif // XML__XML_ELEMENT_H_ diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp index f4bac444..9834e413 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp @@ -34,7 +34,7 @@ class XMLParam { public: XMLType param_type_; - std::variant param_value_; + std::variant param_value_; XMLParam() = default; XMLParam(XMLType type) : param_type_(type) {} @@ -44,7 +44,7 @@ class XMLParam template T GetParamValue() const { - if constexpr (std::is_same::value || std::is_same::value || + if constexpr (std::is_same::value || std::is_same::value || std::is_same::value || std::is_same::value) { return std::get(param_value_); @@ -53,5 +53,5 @@ class XMLParam } }; -} +} // namespace xml #endif // XML__XML_PARAM_H_ diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp index 9a62213f..a0acf832 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -40,6 +40,6 @@ class XMLString bool operator==(const char * & rhs); friend std::ostream & operator<<(std::ostream & out, XMLString & xml_str); }; -} // xml +} // namespace xml #endif // XML__XML_STRING_H_ diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index fbeea962..7f443d09 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -186,7 +186,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta KukaRSIHardwareInterface::D2R; // Ipoc data - ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); + ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); for (size_t i = 0; i < info_.joints.size(); ++i) { // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; @@ -203,7 +203,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta command_handler_.SetCommandParam("AK", "A5", joint_pos_correction_deg_[4]); command_handler_.SetCommandParam("AK", "A6", joint_pos_correction_deg_[5]); command_handler_.SetCommandParam("Stop", "Stop", stop_flag_); - command_handler_.SetCommandParam("IPOC", "IPOC", static_cast(ipoc_)); + command_handler_.SetCommandParam("IPOC", "IPOC", static_cast(ipoc_)); //TODO (Komáromi): construct and send msg auto out_buffer_it = out_buffer_; @@ -264,7 +264,7 @@ return_type KukaRSIHardwareInterface::read( hw_states_[5] = command_handler_.GetState().GetElement("AIPos")->GetParam("A6") * KukaRSIHardwareInterface::D2R; // ipoc_ = rsi_state_.ipoc; - ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); + ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); return return_type::OK; } @@ -294,7 +294,7 @@ return_type KukaRSIHardwareInterface::write( command_handler_.SetCommandParam("AK", "A5", joint_pos_correction_deg_[4]); command_handler_.SetCommandParam("AK", "A6", joint_pos_correction_deg_[5]); command_handler_.SetCommandParam("Stop", "Stop", stop_flag_); - command_handler_.SetCommandParam("IPOC", "IPOC", static_cast(ipoc_)); + command_handler_.SetCommandParam("IPOC", "IPOC", static_cast(ipoc_)); // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; auto out_buffer_it = out_buffer_; diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 9f1e30a5..f5e92967 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -148,4 +148,4 @@ XMLString XMLElement::castXMLString(char * & str_ptr) start_ref, (size_t)(str_ptr - start_ref)); return data; } -} +} // namespace xml diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index 99cfe5ac..72f141a9 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -29,7 +29,7 @@ int XMLParam::PrintParam(char * & buffer_it, int & size_left) break; case XMLType::LONG: - idx = snprintf(buffer_it, size_left, "%lu", GetParamValue()); + idx = snprintf(buffer_it, size_left, "%lu", GetParamValue()); break; case XMLType::DOUBLE: @@ -57,4 +57,4 @@ std::ostream & operator<<(std::ostream & out, class XMLParam & param) std::visit([&](auto && arg) {out << arg;}, param.param_value_); return out; } -} +} // namespace xml diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp index 14b6deb5..4312ed5f 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp @@ -60,4 +60,4 @@ std::ostream & operator<<(std::ostream & out, XMLString & xml_str) } return out; } -} +} // namespace xml From 92fb416f82cafab8ec7dbec7f4f384f299fe84c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 17 Aug 2023 19:39:47 +0200 Subject: [PATCH 63/97] XMLElement class function comments --- .../include/xml_handler/xml_element.hpp | 165 +++++++++++++----- .../src/rsi_command_handler.cpp | 4 +- .../src/xml_handler/xml_element.cpp | 4 +- 3 files changed, 130 insertions(+), 43 deletions(-) diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 907ae420..53e81db2 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -49,55 +49,57 @@ inline bool operator<(const std::string & b, const XMLString & a) } } +/** + * @brief The XMLElement class is a tree based representation of a XML structure. + * It has name, parameters and childs. Its name is a unic key, this identify the XMLElement object. + * The paramters contains every data and attribute together. + * The Childs contains the XMLElements every child, they are XMLElement type. + */ class XMLElement { -private: - static XMLString castXMLString(char * & str_ptr); - XMLElement * element(const std::string & elementName, int depth = 0); - const XMLElement * const getElement( - const std::string & elementName, - int depth = 0) const; - - std::map> params_; - std::vector childs_; - std::string name_; - public: + /** + * @brief Construct a new XMLElement object + * + * @param name: Name of the XMLElement object + */ XMLElement(const std::string & name) : name_(name) {} - XMLElement() = default; ~XMLElement() = default; - inline std::string GetName() const {return name_;} - inline void SetName(const std::string & name) {name_ = name;} + /** + * @brief Get the Name of the XMLElement object + * + * @return const std::string & - The name of the XMLElement object + */ + inline const std::string & GetName() const {return name_;} + + /** + * @brief Get the Childs of the XMLElement object + * + * @return const std::vector & - A constant vector of the XMLElements childs + */ inline const std::vector & GetChilds() const {return childs_;} - inline std::vector & Childs() {return childs_;} - inline std::map> & Params() {return params_;} - inline XMLElement * Element(const std::string & elementName) {return element(elementName);} - inline const XMLElement * const GetElement( + /** + * @brief Returns an XMLElement object which names was given as key + * + * @param elementName Name of the XMLElement object that the functon is looking for + * in the XMLElement tree + * @return const XMLElement * - The found XMLElment object. + * Return nullptr if the given key not found. + */ + inline const XMLElement * GetElement( const std::string & elementName) const {return getElement(elementName);} - bool CastParam(const XMLString & key, char * & str_ptr); - bool IsParamNameValid(XMLString & key, char * & str_ptr); - bool IsNameValid(XMLString & key, char * & str_ptr); - - // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used - template - bool GetParam(const std::string & key, T & param) const - { - auto param_it = params_.find(key); - if (param_it != params_.end()) { - if constexpr (std::is_same::value || std::is_same::value || - std::is_same::value || std::is_same::value) - { - param = param_it->second.GetParamValue(); - return true; - } - } - return false; - } - + /** + * @brief Returns one parameter of the XMLElement object according to the given key. + * + * @tparam T Can only be bool, int64_t, double and XMLString. + * @param key The parameters name that the function is looking for. + * @return T - The parameter it is found properly. + * @exception Range_error if key not found in the parameter list. + */ template T GetParam(const std::string & key) const { @@ -108,7 +110,22 @@ class XMLElement throw std::range_error("Could not find key in parameter list"); } - // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used + /** + * @brief Set the Name of the XMLElement object + * + * @param name: Name of the XMLElement object + */ + inline void SetName(const std::string & name) {name_ = name;} + + /** + * @brief Set one of the parameters of the XMLElement object acording to the given key and parameter. + * + * @tparam T Can only be bool, int64_t, double and XMLString. + * @param key The parameters name witch the fnction overrides. + * @param param The data witch will be inserted. + * @return true if the key is fount in the parameter list and the parameter was proper type, + * @return false otherwise + */ template bool SetParam(const std::string & key, const T & param) { @@ -123,6 +140,76 @@ class XMLElement } return false; } + + /** + * @brief Return every child in a std::vector that the XMLElement object has + * + * @return A reference to a vector of the XMLElement objects childs + */ + inline std::vector & Childs() {return childs_;} + + /** + * @brief Returns every parameter in a std::map that the XMLElement object has + * + * @return A reference to the XMLElement objects parameter map + */ + inline std::map> & Params() {return params_;} + + /** + * @brief Returns an XMLElement object which names was given as key + * + * @param elementName Name of the XMLElement object that the functon is looking for + * in the XMLElement tree + * @return XMLElement* - The found XMLElment object. + * Return nullptr if the given key not found. + */ + inline XMLElement * Element(const std::string & elementName) {return element(elementName);} + + /** + * @brief Casts data into one of the XMLElement objects parameter with the given key. + * The function casts until there is valid for the for the parameters type. + * + * @param key The XMLElement objects parameters name that the function casts into. + * @param str_ptr Pointer reference to a string where the function casts from. + * When the function returns the pointer points to the character after the casted data. + * @return true if the cast was succesfull and the parameter found in the parameter list, + * @return false otherwise + */ + bool CastParamData(const XMLString & key, char * & str_ptr); + + /** + * @brief Checks one fo the XMLElement objects parameters name with the given string + * + * @param key The casted parameter name from the given string. + * @param str_ptr Pointer reference to a string where the function checks from. + * When the function returns the pointer points to the character after the checked data. + * @return true if the name check was successfull, + * @return false otherwise. + */ + bool IsParamNameValid(XMLString & key, char * & str_ptr); + + /** + * @brief Checks the XMLElement objects name on the given key, + * with the given string + * + * @param key the casted name of the XMLElement object from the given string. + * @param str_ptr Pointer reference to a string where the function checks from. + * When the function returns the pointer points to the character after the checked data. + * @return true if the name check was successfull, + * @return false otherwise. + */ + bool IsNameValid(XMLString & key, char * & str_ptr); + +private: + static XMLString castXMLString(char * & str_ptr); + XMLElement * element(const std::string & elementName, int depth = 0); + const XMLElement * getElement( + const std::string & elementName, + int depth = 0) const; + + std::map> params_; + std::vector childs_; + std::string name_; }; } // namespace xml diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index e16b2e0a..9675baae 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -180,7 +180,7 @@ void RSICommandHandler::decodeNode( } buffer_it++; // Casts param data - if (!element.CastParam(node_param, buffer_it)) { + if (!element.CastParamData(node_param, buffer_it)) { std::stringstream err_ss; err_ss << "Could not cast the \"" << node_param << "\" param into the " << element.GetName() << " elements parameter list."; @@ -232,7 +232,7 @@ void RSICommandHandler::decodeNode( if (!is_param_only_node) { if (*buffer_it != '<') { // Node is a leaf node, checking its data - if (!element.CastParam(node_name, buffer_it)) { + if (!element.CastParamData(node_name, buffer_it)) { std::stringstream err_ss; err_ss << "Could not cast the \"" << node_name << "\" parameter into the " << element.GetName() << " elements parameter list."; diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index f5e92967..7b661680 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -25,7 +25,7 @@ namespace xml { -bool XMLElement::CastParam(const XMLString & key, char * & str_ptr) +bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) { auto param_it = params_.find(key); auto ret_val = param_it != params_.end(); @@ -115,7 +115,7 @@ XMLElement * XMLElement::element(const std::string & elementName, int depth) return nullptr; } -const XMLElement * const XMLElement::getElement( +const XMLElement * XMLElement::getElement( const std::string & elementName, int depth) const { From 23374a344040c4273e3a83ef77795f52c0b344bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 18 Aug 2023 10:29:40 +0200 Subject: [PATCH 64/97] CI corrections --- .../kuka_hardware_interface.hpp | 9 +++---- .../rsi_command_handler.hpp | 8 ++++--- .../include/xml_handler/xml_element.hpp | 8 +++---- .../include/xml_handler/xml_param.hpp | 8 +++---- .../include/xml_handler/xml_string.hpp | 17 ++++++------- .../src/kuka_hardware_interface.cpp | 3 +-- .../src/rsi_command_handler.cpp | 10 ++++---- .../src/xml_handler/xml_element.cpp | 19 +++++++-------- .../src/xml_handler/xml_string.cpp | 24 ------------------- 9 files changed, 38 insertions(+), 68 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index df752391..979853de 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -41,11 +41,6 @@ #ifndef KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ #define KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_ -#include -#include -// #include -// #include - #include #include #include @@ -53,8 +48,10 @@ #include #include -#include "rclcpp/macros.hpp" +#include +#include +#include "rclcpp/macros.hpp" #include "kuka_rsi_hw_interface/visibility_control.h" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 3bfbd361..18602073 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -37,12 +37,14 @@ * Author: Komaromi Sandor */ -#ifndef KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_ -#define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_ +#ifndef KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_HPP_ +#define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_HPP_ #include "xml_handler/xml_element.hpp" +#include + namespace kuka_rsi_hw_interface { @@ -86,4 +88,4 @@ class RSICommandHandler } // namespace kuka_rsi_hw_interface -#endif // KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_ +#endif // KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_HPP_ diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 53e81db2..74ca4cd4 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef XML__XML_ELEMENT_H_ -#define XML__XML_ELEMENT_H_ +#ifndef XML_HANDLER__XML_ELEMENT_HPP_ +#define XML_HANDLER__XML_ELEMENT_HPP_ #include #include @@ -63,7 +63,7 @@ class XMLElement * * @param name: Name of the XMLElement object */ - XMLElement(const std::string & name) + explicit XMLElement(const std::string & name) : name_(name) {} ~XMLElement() = default; @@ -213,4 +213,4 @@ class XMLElement }; } // namespace xml -#endif // XML__XML_ELEMENT_H_ +#endif // XML_HANDLER__XML_ELEMENT_HPP_ diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp index 9834e413..83b9951a 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef XML__XML_PARAM_H_ -#define XML__XML_PARAM_H_ +#ifndef XML_HANDLER__XML_PARAM_HPP_ +#define XML_HANDLER__XML_PARAM_HPP_ #include @@ -36,7 +36,7 @@ class XMLParam XMLType param_type_; std::variant param_value_; XMLParam() = default; - XMLParam(XMLType type) + explicit XMLParam(XMLType type) : param_type_(type) {} int PrintParam(char * & buffer_it, int & size_left); friend std::ostream & operator<<(std::ostream & out, class XMLParam & param); @@ -54,4 +54,4 @@ class XMLParam }; } // namespace xml -#endif // XML__XML_PARAM_H_ +#endif // XML_HANDLER__XML_PARAM_HPP_ diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp index a0acf832..005afe9f 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef XML__XML_STRING_H_ -#define XML__XML_STRING_H_ +#ifndef XML_HANDLER__XML_STRING_HPP_ +#define XML_HANDLER__XML_STRING_HPP_ #include @@ -24,22 +24,19 @@ class XMLString public: const char * data_ptr_; size_t length_; - XMLString(const char * data_ptr, size_t length) - : data_ptr_(data_ptr), length_(length) {} XMLString() : data_ptr_(nullptr), length_(0) {} - XMLString(const char * data_ptr) + XMLString(const char * data_ptr, size_t length) + : data_ptr_(data_ptr), length_(length) {} + explicit XMLString(const char * data_ptr) : data_ptr_(data_ptr), length_(strlen(data_ptr)) {} - XMLString(const std::string & str) + explicit XMLString(const std::string & str) : data_ptr_(str.c_str()), length_(str.length()) {} int PrintString(char * & buffer_it, const int & size_left); - bool operator==(const XMLString & rhs); - bool operator==(const std::string & rhs); - bool operator==(const char * & rhs); friend std::ostream & operator<<(std::ostream & out, XMLString & xml_str); }; } // namespace xml -#endif // XML__XML_STRING_H_ +#endif // XML_HANDLER__XML_STRING_HPP_ diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 7f443d09..6850ddf1 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -205,7 +205,6 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta command_handler_.SetCommandParam("Stop", "Stop", stop_flag_); command_handler_.SetCommandParam("IPOC", "IPOC", static_cast(ipoc_)); - //TODO (Komáromi): construct and send msg auto out_buffer_it = out_buffer_; if (command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE) < 0) { RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "Encode Failed"); @@ -305,7 +304,7 @@ return_type KukaRSIHardwareInterface::write( server_->send(out_buffer_); return return_type::OK; } -} // namespace namespace kuka_rsi_hw_interface +} // namespace kuka_rsi_hw_interface PLUGINLIB_EXPORT_CLASS( kuka_rsi_hw_interface::KukaRSIHardwareInterface, diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 9675baae..0290598b 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -105,7 +105,7 @@ int RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) int size_left = buffer_size; try { encodeNode(this->command_data_structure_, buffer_it, size_left); - if (buffer_size - size_left != (size_t)(buffer_it - buffer)) { + if (static_cast(buffer_size) - size_left != static_cast(buffer_it - buffer)) { throw std::range_error("Range error occured"); } // +1 is for the \0 character @@ -141,11 +141,11 @@ void RSICommandHandler::decodeNode( bool is_param_only_node = false; bool isNoMoreParam = false; xml::XMLString node_param; - while (!isNoMoreParam && (size_t)(buffer_it - buffer) < buffer_size) { + while (!isNoMoreParam && static_cast(buffer_it - buffer) < buffer_size) { // fast forward to the next non-space character for (; *buffer_it == ' '; buffer_it++) { } - if ((size_t)(buffer_it - buffer) >= buffer_size) { + if (static_cast(buffer_it - buffer) >= buffer_size) { std::stringstream err_ss; err_ss << "Out of range error in " << element.GetName() << " node."; throw std::range_error(err_ss.str()); @@ -196,7 +196,7 @@ void RSICommandHandler::decodeNode( num_of_param++; } } - if ((size_t)(buffer_it - buffer) >= buffer_size) { + if (static_cast(buffer_it - buffer) >= buffer_size) { std::stringstream err_ss; err_ss << "Out of range error in " << element.GetName() << " node."; throw std::range_error(err_ss.str()); @@ -222,7 +222,7 @@ void RSICommandHandler::decodeNode( // Fast forward until the next node or the end of the node for (; *buffer_it == ' '; buffer_it++) { } - if ((size_t)(buffer_it - buffer) >= buffer_size) { + if (static_cast(buffer_it - buffer) >= buffer_size) { std::stringstream err_ss; err_ss << "Out of range error in " << element.GetName() << " node."; throw std::range_error(err_ss.str()); diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 7b661680..91ba9d63 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -18,8 +18,7 @@ #include #include #include -#include -#include +#include #include "xml_handler/xml_element.hpp" @@ -35,7 +34,7 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) case XMLType::BOOL: { char * end = str_ptr; - auto res = (bool)strtol(str_ptr, &end, 0); + auto res = static_cast(strtol(str_ptr, &end, 0)); if (res == 0 && (errno != 0 || end == str_ptr)) { return false; } @@ -108,9 +107,9 @@ XMLElement * XMLElement::element(const std::string & elementName, int depth) } } if (depth == 0) { - char err_buf[1000]; - sprintf(err_buf, "%s element not found", elementName.c_str()); - throw std::logic_error(err_buf); + std::stringstream err_ss; + err_ss << elementName << " element not found."; + throw std::logic_error(err_ss.str()); } return nullptr; } @@ -129,9 +128,9 @@ const XMLElement * XMLElement::getElement( } } if (depth == 0) { - char err_buf[1000]; - sprintf(err_buf, "%s element not found", elementName.c_str()); - throw std::logic_error(err_buf); + std::stringstream err_ss; + err_ss << elementName << " element not found."; + throw std::logic_error(err_ss.str()); } return nullptr; } @@ -145,7 +144,7 @@ XMLString XMLElement::castXMLString(char * & str_ptr) { } auto data = XMLString( - start_ref, (size_t)(str_ptr - start_ref)); + start_ref, static_cast(str_ptr - start_ref)); return data; } } // namespace xml diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp index 4312ed5f..55864e21 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp @@ -29,30 +29,6 @@ int XMLString::PrintString(char * & buffer_it, const int & size_left) } } -bool XMLString::operator==(const XMLString & rhs) -{ - if (length_ != rhs.length_) { - return false; - } - return strncmp(data_ptr_, rhs.data_ptr_, length_); -} - -bool XMLString::operator==(const std::string & rhs) -{ - if (length_ != rhs.length()) { - return false; - } - return strncmp(rhs.c_str(), data_ptr_, length_); -} - -bool XMLString::operator==(const char * & rhs) -{ - if (length_ != strlen(rhs)) { - return false; - } - return strncmp(rhs, data_ptr_, length_); -} - std::ostream & operator<<(std::ostream & out, XMLString & xml_str) { for (size_t i = 0; i < xml_str.length_; i++) { From 12c4773b916d09ab7db384c501690ebd9ec39d69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 18 Aug 2023 11:08:27 +0200 Subject: [PATCH 65/97] CI correction #2 --- .../kuka_rsi_hw_interface/kuka_hardware_interface.hpp | 8 ++++---- .../include/kuka_rsi_hw_interface/rsi_command_handler.hpp | 3 +-- kuka_rsi_hw_interface/src/rsi_command_handler.cpp | 2 +- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 979853de..c891310a 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -48,16 +48,16 @@ #include #include -#include -#include - -#include "rclcpp/macros.hpp" +#include "kuka_rsi_hw_interface/udp_server.h" #include "kuka_rsi_hw_interface/visibility_control.h" +#include "kuka_rsi_hw_interface/rsi_command_handler.hpp" + #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/macros.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 18602073..90ca0937 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -40,11 +40,10 @@ #ifndef KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_HPP_ #define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_HPP_ +#include #include "xml_handler/xml_element.hpp" -#include - namespace kuka_rsi_hw_interface { diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 0290598b..db17ed8c 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -368,4 +368,4 @@ void RSICommandHandler::encodeNode( } } } -} +} // namespace kuka_rsi_hw_interface From 57bcdcb95791fc7e92dd98b1a52d53fa825a3fc5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 18 Aug 2023 11:27:47 +0200 Subject: [PATCH 66/97] using ros angles --- kuka_rsi_hw_interface/CMakeLists.txt | 3 +- .../kuka_hardware_interface.hpp | 4 +- kuka_rsi_hw_interface/package.xml | 1 + .../src/kuka_hardware_interface.cpp | 76 +++++++++---------- 4 files changed, 43 insertions(+), 41 deletions(-) diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index b417a21c..37c367b0 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(kroshu_ros2_core REQUIRED) find_package(hardware_interface REQUIRED) find_package(controller_manager_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(angles REQUIRED) find_package(tinyxml_vendor REQUIRED) find_package(TinyXML REQUIRED) @@ -49,7 +50,7 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUIL # prevent pluginlib from using boost target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface) +ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface angles) target_link_libraries(${PROJECT_NAME} tinyxml) add_executable(robot_manager_node diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index c891310a..1be64e8a 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -115,8 +115,8 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface char in_buffer_[UDP_BUFFER_SIZE] = {0}; char out_buffer_[UDP_BUFFER_SIZE] = {0}; - static constexpr double R2D = 180 / M_PI; - static constexpr double D2R = M_PI / 180; + // static constexpr double R2D = 180 / M_PI; + // static constexpr double D2R = M_PI / 180; }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml index abf33063..bf1e66e9 100644 --- a/kuka_rsi_hw_interface/package.xml +++ b/kuka_rsi_hw_interface/package.xml @@ -20,6 +20,7 @@ hardware_interface pluginlib controller_manager_msgs + angles ament_cmake_copyright ament_cmake_cppcheck diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 6850ddf1..01b1bf79 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -44,6 +44,7 @@ #include #include "kuka_rsi_hw_interface/kuka_hardware_interface.hpp" +#include "angles/angles.h" namespace kuka_rsi_hw_interface { @@ -158,32 +159,32 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta } // Position data - hw_states_[0] = command_handler_.GetState().GetElement("AIPos")->GetParam("A1") * - KukaRSIHardwareInterface::D2R; - hw_states_[1] = command_handler_.GetState().GetElement("AIPos")->GetParam("A2") * - KukaRSIHardwareInterface::D2R; - hw_states_[2] = command_handler_.GetState().GetElement("AIPos")->GetParam("A3") * - KukaRSIHardwareInterface::D2R; - hw_states_[3] = command_handler_.GetState().GetElement("AIPos")->GetParam("A4") * - KukaRSIHardwareInterface::D2R; - hw_states_[4] = command_handler_.GetState().GetElement("AIPos")->GetParam("A5") * - KukaRSIHardwareInterface::D2R; - hw_states_[5] = command_handler_.GetState().GetElement("AIPos")->GetParam("A6") * - KukaRSIHardwareInterface::D2R; + hw_states_[0] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A1")); + hw_states_[1] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A2")); + hw_states_[2] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A3")); + hw_states_[3] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A4")); + hw_states_[4] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A5")); + hw_states_[5] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A6")); // Initial position data - initial_joint_pos_[0] = command_handler_.GetState().GetElement("ASPos")->GetParam("A1") * - KukaRSIHardwareInterface::D2R; - initial_joint_pos_[1] = command_handler_.GetState().GetElement("ASPos")->GetParam("A2") * - KukaRSIHardwareInterface::D2R; - initial_joint_pos_[2] = command_handler_.GetState().GetElement("ASPos")->GetParam("A3") * - KukaRSIHardwareInterface::D2R; - initial_joint_pos_[3] = command_handler_.GetState().GetElement("ASPos")->GetParam("A4") * - KukaRSIHardwareInterface::D2R; - initial_joint_pos_[4] = command_handler_.GetState().GetElement("ASPos")->GetParam("A5") * - KukaRSIHardwareInterface::D2R; - initial_joint_pos_[5] = command_handler_.GetState().GetElement("ASPos")->GetParam("A6") * - KukaRSIHardwareInterface::D2R; + initial_joint_pos_[0] = angles::from_degrees( + command_handler_.GetState().GetElement("ASPos")->GetParam("A1")); + initial_joint_pos_[1] = angles::from_degrees( + command_handler_.GetState().GetElement("ASPos")->GetParam("A2")); + initial_joint_pos_[2] = angles::from_degrees( + command_handler_.GetState().GetElement("ASPos")->GetParam("A3")); + initial_joint_pos_[3] = angles::from_degrees( + command_handler_.GetState().GetElement("ASPos")->GetParam("A4")); + initial_joint_pos_[4] = angles::from_degrees( + command_handler_.GetState().GetElement("ASPos")->GetParam("A5")); + initial_joint_pos_[5] = angles::from_degrees( + command_handler_.GetState().GetElement("ASPos")->GetParam("A6")); // Ipoc data ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); @@ -250,18 +251,18 @@ return_type KukaRSIHardwareInterface::read( // for (std::size_t i = 0; i < info_.joints.size(); ++i) { // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; // } - hw_states_[0] = command_handler_.GetState().GetElement("AIPos")->GetParam("A1") * - KukaRSIHardwareInterface::D2R; - hw_states_[1] = command_handler_.GetState().GetElement("AIPos")->GetParam("A2") * - KukaRSIHardwareInterface::D2R; - hw_states_[2] = command_handler_.GetState().GetElement("AIPos")->GetParam("A3") * - KukaRSIHardwareInterface::D2R; - hw_states_[3] = command_handler_.GetState().GetElement("AIPos")->GetParam("A4") * - KukaRSIHardwareInterface::D2R; - hw_states_[4] = command_handler_.GetState().GetElement("AIPos")->GetParam("A5") * - KukaRSIHardwareInterface::D2R; - hw_states_[5] = command_handler_.GetState().GetElement("AIPos")->GetParam("A6") * - KukaRSIHardwareInterface::D2R; + hw_states_[0] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A1")); + hw_states_[1] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A2")); + hw_states_[2] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A3")); + hw_states_[3] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A4")); + hw_states_[4] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A5")); + hw_states_[5] = angles::from_degrees( + command_handler_.GetState().GetElement("AIPos")->GetParam("A6")); // ipoc_ = rsi_state_.ipoc; ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); return return_type::OK; @@ -282,8 +283,7 @@ return_type KukaRSIHardwareInterface::write( if (stop_flag_) {is_active_ = false;} for (size_t i = 0; i < info_.joints.size(); i++) { - joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * - KukaRSIHardwareInterface::R2D; + joint_pos_correction_deg_[i] = angles::to_degrees(hw_commands_[i] - initial_joint_pos_[i]); } command_handler_.SetCommandParam("AK", "A1", joint_pos_correction_deg_[0]); From 97163e92ecf03d951e4338db3198700a5b1dc0bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 18 Aug 2023 12:37:27 +0200 Subject: [PATCH 67/97] sonarCloud codesmells --- .../kuka_rsi_hw_interface/rsi_command_handler.hpp | 4 ++-- .../include/kuka_rsi_hw_interface/udp_server.h | 5 +++-- .../include/xml_handler/xml_string.hpp | 14 ++++++++++---- kuka_rsi_hw_interface/src/rsi_command_handler.cpp | 12 ++++++------ .../src/xml_handler/xml_string.cpp | 2 +- 5 files changed, 22 insertions(+), 15 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 90ca0937..cf220b98 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -77,11 +77,11 @@ class RSICommandHandler xml::XMLElement command_data_structure_; xml::XMLElement state_data_structure_; - void decodeNode( + void decodeNodes( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size); - void encodeNode( + void encodeNodes( xml::XMLElement & element, char * & buffer_it, int & size_left); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 22b3fad1..50296f74 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -107,7 +107,7 @@ class UDPServer ssize_t bytes = 0; bytes = sendto( sockfd_, buffer, - strlen(buffer), 0, (struct sockaddr *) &clientaddr_, clientlen_); + strnlen(buffer, UDP_BUFFER_SIZE), 0, (struct sockaddr *) &clientaddr_, clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in send"); } @@ -140,6 +140,7 @@ class UDPServer &clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); + return bytes; } } else { return 0; @@ -152,9 +153,9 @@ class UDPServer &clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); + return bytes; } } - strncpy(buffer, buffer_, bytes); return bytes; diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp index 005afe9f..95e2cca7 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -28,10 +28,16 @@ class XMLString : data_ptr_(nullptr), length_(0) {} XMLString(const char * data_ptr, size_t length) : data_ptr_(data_ptr), length_(length) {} - explicit XMLString(const char * data_ptr) - : data_ptr_(data_ptr), length_(strlen(data_ptr)) {} - explicit XMLString(const std::string & str) - : data_ptr_(str.c_str()), length_(str.length()) {} + explicit inline XMLString(const char * data_ptr) + : data_ptr_(data_ptr) + { + length_ = strnlen(data_ptr, 256); + if (length_ == 256) { + throw std::range_error("XMLString data length out of range. Range: 0 - 255"); + } + } + // explicit XMLString(const std::string & str) + // : data_ptr_(str.c_str()), length_(str.length()) {} int PrintString(char * & buffer_it, const int & size_left); diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index db17ed8c..51238b5c 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -91,7 +91,7 @@ bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) { auto buffer_it = buffer; try { - decodeNode(this->state_data_structure_, buffer, buffer_it, buffer_size); + decodeNodes(this->state_data_structure_, buffer, buffer_it, buffer_size); return true; } catch (const std::exception & e) { RCLCPP_ERROR(rclcpp::get_logger("CommandHandler"), "%s", e.what()); @@ -104,7 +104,7 @@ int RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) auto buffer_it = buffer; int size_left = buffer_size; try { - encodeNode(this->command_data_structure_, buffer_it, size_left); + encodeNodes(this->command_data_structure_, buffer_it, size_left); if (static_cast(buffer_size) - size_left != static_cast(buffer_it - buffer)) { throw std::range_error("Range error occured"); } @@ -116,7 +116,7 @@ int RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) } } -void RSICommandHandler::decodeNode( +void RSICommandHandler::decodeNodes( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size) { @@ -246,7 +246,7 @@ void RSICommandHandler::decodeNode( } else { // Node has childs for (auto && child : element.Childs()) { - decodeNode(child, buffer, buffer_it, buffer_size); + decodeNodes(child, buffer, buffer_it, buffer_size); } } // Checking the end node for syntax and node name @@ -277,7 +277,7 @@ void RSICommandHandler::decodeNode( } } -void RSICommandHandler::encodeNode( +void RSICommandHandler::encodeNodes( xml::XMLElement & element, char * & buffer_it, int & size_left) { // Start the node with its name @@ -349,7 +349,7 @@ void RSICommandHandler::encodeNode( if (element.GetChilds().size() > 0) { // Add childs for (auto && child : element.Childs()) { - encodeNode(child, buffer_it, size_left); + encodeNodes(child, buffer_it, size_left); } } else { // Add data diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp index 55864e21..a9688e81 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp @@ -21,7 +21,7 @@ namespace xml { int XMLString::PrintString(char * & buffer_it, const int & size_left) { - if (length_ + 1 > size_left) { + if (static_cast(length_) + 1 > size_left) { return -1; } else { snprintf(buffer_it, length_ + 1, "%s", data_ptr_); From 2011cc3e26c2899074ac8f9f717ab049ea128ab6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 18 Aug 2023 13:44:36 +0200 Subject: [PATCH 68/97] try to make strncpy safe --- kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h | 1 + 1 file changed, 1 insertion(+) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 50296f74..76834159 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -157,6 +157,7 @@ class UDPServer } } strncpy(buffer, buffer_, bytes); + buffer[bytes] = '\0'; return bytes; } From cd30168a6478ebbf5d22b650169a90261df71240 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 18 Aug 2023 14:11:08 +0200 Subject: [PATCH 69/97] try to make strncpy safe #2 --- .../include/kuka_rsi_hw_interface/udp_server.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 76834159..f6542047 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -156,8 +156,7 @@ class UDPServer return bytes; } } - strncpy(buffer, buffer_, bytes); - buffer[bytes] = '\0'; + strcpy(buffer, buffer_); return bytes; } From 3af369d10496927a8708995d6008190d8db6523f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 18 Aug 2023 14:26:13 +0200 Subject: [PATCH 70/97] try to make strncpy safe #3 --- .../include/kuka_rsi_hw_interface/udp_server.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index f6542047..44971eaf 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -156,7 +156,7 @@ class UDPServer return bytes; } } - strcpy(buffer, buffer_); + memcpy(buffer, buffer_, bytes); return bytes; } From 98507f273fc101f73d2ca14774a46a169c69e1a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 18 Aug 2023 14:52:07 +0200 Subject: [PATCH 71/97] sonarCloud corrections --- kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index 72f141a9..d777fa9c 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -54,7 +54,7 @@ int XMLParam::PrintParam(char * & buffer_it, int & size_left) std::ostream & operator<<(std::ostream & out, class XMLParam & param) { - std::visit([&](auto && arg) {out << arg;}, param.param_value_); + std::visit([&out](auto & arg) {out << arg;}, param.param_value_); return out; } } // namespace xml From 46e41b474f3602180fabd788f6ac189ca1539509 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 21 Aug 2023 10:53:35 +0200 Subject: [PATCH 72/97] comment typo corrections --- .../include/xml_handler/xml_element.hpp | 63 +++++++++---------- .../src/rsi_command_handler.cpp | 44 ++++++------- 2 files changed, 54 insertions(+), 53 deletions(-) diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 74ca4cd4..f796262e 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -51,9 +51,9 @@ inline bool operator<(const std::string & b, const XMLString & a) /** * @brief The XMLElement class is a tree based representation of a XML structure. - * It has name, parameters and childs. Its name is a unic key, this identify the XMLElement object. - * The paramters contains every data and attribute together. - * The Childs contains the XMLElements every child, they are XMLElement type. + * It has name, parameters and childs. It's name is a unique key, which identifies the XMLElement object. + * The parameters contain the data and attributes together. + * The childs contains the XMLElement's every child (of type XMLElement). */ class XMLElement { @@ -68,7 +68,7 @@ class XMLElement ~XMLElement() = default; /** - * @brief Get the Name of the XMLElement object + * @brief Get the name of the XMLElement object * * @return const std::string & - The name of the XMLElement object */ @@ -82,22 +82,22 @@ class XMLElement inline const std::vector & GetChilds() const {return childs_;} /** - * @brief Returns an XMLElement object which names was given as key + * @brief Returns the XMLElement object with the given name. * * @param elementName Name of the XMLElement object that the functon is looking for * in the XMLElement tree * @return const XMLElement * - The found XMLElment object. - * Return nullptr if the given key not found. + * Return nullptr if the given key was not found. */ inline const XMLElement * GetElement( const std::string & elementName) const {return getElement(elementName);} /** - * @brief Returns one parameter of the XMLElement object according to the given key. + * @brief Returns the value of the parameter of the XMLElement object with the given key. * * @tparam T Can only be bool, int64_t, double and XMLString. - * @param key The parameters name that the function is looking for. - * @return T - The parameter it is found properly. + * @param key The parameter's name that the function is looking for. + * @return T - The parameter value if it is found * @exception Range_error if key not found in the parameter list. */ template @@ -118,12 +118,12 @@ class XMLElement inline void SetName(const std::string & name) {name_ = name;} /** - * @brief Set one of the parameters of the XMLElement object acording to the given key and parameter. - * + * @brief Set the value of the parameter with the given name + * * @tparam T Can only be bool, int64_t, double and XMLString. - * @param key The parameters name witch the fnction overrides. + * @param key The name of the parameter to be changed. * @param param The data witch will be inserted. - * @return true if the key is fount in the parameter list and the parameter was proper type, + * @return true if the key is found in the parameter list and the parameter has proper type, * @return false otherwise */ template @@ -144,57 +144,56 @@ class XMLElement /** * @brief Return every child in a std::vector that the XMLElement object has * - * @return A reference to a vector of the XMLElement objects childs + * @return A reference to a vector of the XMLElement object's childs */ inline std::vector & Childs() {return childs_;} /** * @brief Returns every parameter in a std::map that the XMLElement object has * - * @return A reference to the XMLElement objects parameter map + * @return A reference to the XMLElement object's parameter map */ inline std::map> & Params() {return params_;} /** - * @brief Returns an XMLElement object which names was given as key + * @brief Returns the XMLElement object with the given name * * @param elementName Name of the XMLElement object that the functon is looking for * in the XMLElement tree * @return XMLElement* - The found XMLElment object. - * Return nullptr if the given key not found. + * Return nullptr if the given key was not found. */ inline XMLElement * Element(const std::string & elementName) {return element(elementName);} /** - * @brief Casts data into one of the XMLElement objects parameter with the given key. - * The function casts until there is valid for the for the parameters type. + * @brief Converts the given string to the specified parameter of the element + * and updates the parameter map * - * @param key The XMLElement objects parameters name that the function casts into. - * @param str_ptr Pointer reference to a string where the function casts from. - * When the function returns the pointer points to the character after the casted data. - * @return true if the cast was succesfull and the parameter found in the parameter list, + * @param [out] key The name of the parameter that the function casts into. + * @param str_ptr Pointer reference to a string the function casts from. + * When the function returns, the pointer points to the character after the cast data. + * @return true if the cast was succesful and the parameter was found in the parameter list, * @return false otherwise */ bool CastParamData(const XMLString & key, char * & str_ptr); /** - * @brief Checks one fo the XMLElement objects parameters name with the given string + * @brief Checks if the given string matches one of parameter names of the XMLElement * - * @param key The casted parameter name from the given string. - * @param str_ptr Pointer reference to a string where the function checks from. - * When the function returns the pointer points to the character after the checked data. + * @param [out] key name of the parameter as an XMLString. + * @param str_ptr Pointer reference to the string the function checks. + * When the function returns, the pointer points to the character after the checked data. * @return true if the name check was successfull, * @return false otherwise. */ bool IsParamNameValid(XMLString & key, char * & str_ptr); /** - * @brief Checks the XMLElement objects name on the given key, - * with the given string + * @brief Checks if the given string matches the XMLElement's name * - * @param key the casted name of the XMLElement object from the given string. - * @param str_ptr Pointer reference to a string where the function checks from. - * When the function returns the pointer points to the character after the checked data. + * @param [out] key name of the XMLElement object as an XMLString + * @param str_ptr Pointer reference to the string the function checks. + * When the function returns, the pointer points to the character after the checked data. * @return true if the name check was successfull, * @return false otherwise. */ diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 51238b5c..84d85afa 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -26,7 +26,7 @@ RSICommandHandler::RSICommandHandler() : command_data_structure_("Sen"), state_data_structure_("Rob") { // State structure - // Later this should be defined by the rsi xml + // TODO(Komaromi): Later this should be defined by the rsi xml state_data_structure_.Params()["TYPE"] = xml::XMLParam(xml::XMLType::STRING); // how to get string: std::get(command_data_structure_.params_["TYPE"]).first xml::XMLElement RIst_el("RIst"); @@ -127,7 +127,7 @@ void RSICommandHandler::decodeNodes( throw std::logic_error(err_ss.str()); } buffer_it++; - // Validate nodes name + // Validate node name if (!element.IsNameValid(node_name, buffer_it)) { std::stringstream err_ss; err_ss << "The detected start node name \"" << node_name << @@ -136,13 +136,13 @@ void RSICommandHandler::decodeNodes( throw std::logic_error(err_ss.str()); } - // Validate nodes params + // Validate node parameters size_t num_of_param = 0; bool is_param_only_node = false; bool isNoMoreParam = false; xml::XMLString node_param; while (!isNoMoreParam && static_cast(buffer_it - buffer) < buffer_size) { - // fast forward to the next non-space character + // Go to the next non-space character for (; *buffer_it == ' '; buffer_it++) { } if (static_cast(buffer_it - buffer) >= buffer_size) { @@ -150,7 +150,7 @@ void RSICommandHandler::decodeNodes( err_ss << "Out of range error in " << element.GetName() << " node."; throw std::range_error(err_ss.str()); } - // Check for the start nodes end characters + // Check for the start node's closing tag if (*buffer_it == '/' && *(buffer_it + 1) == '>') { is_param_only_node = true; isNoMoreParam = true; @@ -159,14 +159,14 @@ void RSICommandHandler::decodeNodes( isNoMoreParam = true; buffer_it++; } else { - // Validates params name + // Validate parameter name if (!element.IsParamNameValid(node_param, buffer_it)) { std::stringstream err_ss; err_ss << "The detected parameter \"" << node_param << - "\" does not match with any of the " << element.GetName() << " elements parameters."; + "\" does not match any of the " << element.GetName() << " elements parameters."; throw std::logic_error(err_ss.str()); } - // Checks xml syntax + // Check xml syntax (parameter name must be followed by '=') if (*buffer_it != '=') { std::stringstream err_ss; err_ss << "In \"" << node_param << "\" param syntax error found."; @@ -179,14 +179,14 @@ void RSICommandHandler::decodeNodes( throw std::logic_error(err_ss.str()); } buffer_it++; - // Casts param data + // Cast parameter data if (!element.CastParamData(node_param, buffer_it)) { std::stringstream err_ss; err_ss << "Could not cast the \"" << node_param << "\" param into the " << element.GetName() << " elements parameter list."; throw std::logic_error(err_ss.str()); } - // Checks xml syntax + // Check xml syntax (Parameter value must be inside quotes) if (*buffer_it != '"') { std::stringstream err_ss; err_ss << "In \"" << node_param << "\" param syntax error found."; @@ -202,10 +202,10 @@ void RSICommandHandler::decodeNodes( throw std::range_error(err_ss.str()); } - // Could not find all parameter, or found too much - // Checks if it is a param only node, - // because the normal leaf nodes data is stored within the elemets parameters. - // So in case of a normal leaf node the num_of_param and the Params().size() does not match. + // Could not find all parameters, or found too much + // Checks if it is a parameter-only node, because the normal leaf node's data is stored within + // the element's parameters. So in case of a normal leaf node the num_of_param and the + // Params().size() would not match even for a valid xml if (is_param_only_node && num_of_param != element.Params().size()) { std::stringstream err_ss; err_ss << num_of_param << " parameter found. It does not match with " << element.GetName() << @@ -219,7 +219,7 @@ void RSICommandHandler::decodeNodes( err_ss << element.GetName() << " node should have chields but did not found any."; throw std::logic_error(err_ss.str()); } - // Fast forward until the next node or the end of the node + // Go to the next node or the end of the node for (; *buffer_it == ' '; buffer_it++) { } if (static_cast(buffer_it - buffer) >= buffer_size) { @@ -228,7 +228,7 @@ void RSICommandHandler::decodeNodes( throw std::range_error(err_ss.str()); } - // If a node not an only param node, therefore has data or childs and need an end node + // If a node is not an only-parameter node, than it has data or childs and needs an end node if (!is_param_only_node) { if (*buffer_it != '<') { // Node is a leaf node, checking its data @@ -249,7 +249,7 @@ void RSICommandHandler::decodeNodes( decodeNodes(child, buffer, buffer_it, buffer_size); } } - // Checking the end node for syntax and node name + // Check for the end tag () if (*(buffer_it) != '<') { std::stringstream err_ss; err_ss << "Syntax error at the end of " << element.GetName() << " node."; @@ -291,13 +291,15 @@ void RSICommandHandler::encodeNodes( size_left -= idx; } bool is_param_only_node = false; - // If the node do not have childs it is probably a param only node + // If the node does not have childs it is a parameter-only node if (element.GetChilds().size() == 0) { is_param_only_node = true; } for (auto && param : element.Params()) { - // For a parameter and its node having the same name is not supported, - // because it means that it is not a param only, just a leaf + // In the current implementation it is not supported, that a parameter has the same name as + // it's base node + // Therefore if the element's name is equal to one of it's parameters, it must be a + // parameter-only node if (element.GetName() == param.first) { is_param_only_node = false; } else { @@ -356,7 +358,7 @@ void RSICommandHandler::encodeNodes( element.Params().find(element.GetName())->second.PrintParam( buffer_it, size_left); } - // Add node end + // Add node end tag idx = snprintf(buffer_it, size_left, "", element.GetName().c_str()); if (idx < 0 || idx > size_left) { std::stringstream err_ss; From 0442e63945b986e254235b256b661ca28b4ac709 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 21 Aug 2023 10:55:13 +0200 Subject: [PATCH 73/97] fix description --- kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index f796262e..37707439 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -169,8 +169,8 @@ class XMLElement * @brief Converts the given string to the specified parameter of the element * and updates the parameter map * - * @param [out] key The name of the parameter that the function casts into. - * @param str_ptr Pointer reference to a string the function casts from. + * @param key The name of the parameter that the function casts into. + * @param str_ptr Pointer reference to the string the function casts from. * When the function returns, the pointer points to the character after the cast data. * @return true if the cast was succesful and the parameter was found in the parameter list, * @return false otherwise From 25ae06e550f5b70ead9e7e1f94a4722767555064 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 21 Aug 2023 11:45:12 +0200 Subject: [PATCH 74/97] ci --- kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp | 4 ++-- kuka_rsi_hw_interface/src/rsi_command_handler.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 37707439..05a4ee10 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -119,7 +119,7 @@ class XMLElement /** * @brief Set the value of the parameter with the given name - * + * * @tparam T Can only be bool, int64_t, double and XMLString. * @param key The name of the parameter to be changed. * @param param The data witch will be inserted. @@ -166,7 +166,7 @@ class XMLElement inline XMLElement * Element(const std::string & elementName) {return element(elementName);} /** - * @brief Converts the given string to the specified parameter of the element + * @brief Converts the given string to the specified parameter of the element * and updates the parameter map * * @param key The name of the parameter that the function casts into. diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 84d85afa..c3eebf8a 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -296,9 +296,9 @@ void RSICommandHandler::encodeNodes( is_param_only_node = true; } for (auto && param : element.Params()) { - // In the current implementation it is not supported, that a parameter has the same name as + // In the current implementation it is not supported, that a parameter has the same name as // it's base node - // Therefore if the element's name is equal to one of it's parameters, it must be a + // Therefore if the element's name is equal to one of it's parameters, it must be a // parameter-only node if (element.GetName() == param.first) { is_param_only_node = false; From c8413414cc7c4d95a887bf9906ed4241a99546ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 22 Aug 2023 19:11:38 +0200 Subject: [PATCH 75/97] refactor encode to lower complexity --- .../rsi_command_handler.hpp | 4 + .../src/rsi_command_handler.cpp | 74 +++++++------------ 2 files changed, 29 insertions(+), 49 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index cf220b98..22ac1a2a 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -83,6 +83,10 @@ class RSICommandHandler void encodeNodes( xml::XMLElement & element, char * & buffer_it, int & size_left); + + void update_iterators( + char * & buffer_it, int & buf_size_left, const xml::XMLElement & element, + const int & buf_idx) }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index c3eebf8a..ddb031b4 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -282,14 +282,8 @@ void RSICommandHandler::encodeNodes( { // Start the node with its name auto idx = snprintf(buffer_it, size_left, "<%s", element.GetName().c_str()); - if (idx < 0 || idx > size_left) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } else { - buffer_it += idx; - size_left -= idx; - } + update_iterators(buffer_it, size_left, element, idx); + bool is_param_only_node = false; // If the node does not have childs it is a parameter-only node if (element.GetChilds().size() == 0) { @@ -298,7 +292,7 @@ void RSICommandHandler::encodeNodes( for (auto && param : element.Params()) { // In the current implementation it is not supported, that a parameter has the same name as // it's base node - // Therefore if the element's name is equal to one of it's parameters, it must be a + // Therefore if the element's name is equal to one of it's parameters, it can't be a // parameter-only node if (element.GetName() == param.first) { is_param_only_node = false; @@ -306,48 +300,23 @@ void RSICommandHandler::encodeNodes( // Creating parameters // Add param name idx = snprintf(buffer_it, size_left, " %s=\"", param.first.c_str()); - if (idx < 0 || idx > size_left) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } else { - buffer_it += idx; - size_left -= idx; - } + update_iterators(buffer_it, size_left, element, idx); + // Add param data param.second.PrintParam(buffer_it, size_left); idx = snprintf(buffer_it, size_left, "\""); - if (idx < 0 || idx > size_left) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } else { - buffer_it += idx; - size_left -= idx; - } + update_iterators(buffer_it, size_left, element, idx); } } // Add start node end bracket if (is_param_only_node) { idx = snprintf(buffer_it, size_left, " />"); - if (idx < 0 || idx > size_left) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } else { - buffer_it += idx; - size_left -= idx; - } + + update_iterators(buffer_it, size_left, element, idx); } else { idx = snprintf(buffer_it, size_left, ">"); - if (idx < 0 || idx > size_left) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } else { - buffer_it += idx; - size_left -= idx; - } + + update_iterators(buffer_it, size_left, element, idx); if (element.GetChilds().size() > 0) { // Add childs for (auto && child : element.Childs()) { @@ -360,14 +329,21 @@ void RSICommandHandler::encodeNodes( } // Add node end tag idx = snprintf(buffer_it, size_left, "", element.GetName().c_str()); - if (idx < 0 || idx > size_left) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } else { - buffer_it += idx; - size_left -= idx; - } + update_iterators(buffer_it, size_left, element, idx); + } +} + +void update_iterators( + char * & buffer_it, int & buf_size_left, const xml::XMLElement & element, + const int & buf_idx) +{ + if (buf_idx < 0 || buf_idx > buf_size_left) { + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); + } else { + buffer_it += buf_idx; + buf_size_left -= buf_idx; } } } // namespace kuka_rsi_hw_interface From 37fd426738a982f7731d0717f8dea76f74644241 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 22 Aug 2023 19:32:22 +0200 Subject: [PATCH 76/97] refactor decode to lower complexity --- .../rsi_command_handler.hpp | 10 +- .../src/rsi_command_handler.cpp | 222 +++++++++--------- 2 files changed, 121 insertions(+), 111 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 22ac1a2a..8899b701 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -77,16 +77,24 @@ class RSICommandHandler xml::XMLElement command_data_structure_; xml::XMLElement state_data_structure_; + // node decoder functions + void decodeNodes( xml::XMLElement & element, char * const buffer, char * & buffer_it, const size_t buffer_size); + void decodeParam(xml::XMLElement & element, char * & buffer_it); + + void decodeLeafNodeParamData(xml::XMLElement & element, char * & buffer_it); + + // node encoder functins + void encodeNodes( xml::XMLElement & element, char * & buffer_it, int & size_left); void update_iterators( char * & buffer_it, int & buf_size_left, const xml::XMLElement & element, - const int & buf_idx) + const int & buf_idx); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index ddb031b4..9f18e65e 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -140,7 +140,6 @@ void RSICommandHandler::decodeNodes( size_t num_of_param = 0; bool is_param_only_node = false; bool isNoMoreParam = false; - xml::XMLString node_param; while (!isNoMoreParam && static_cast(buffer_it - buffer) < buffer_size) { // Go to the next non-space character for (; *buffer_it == ' '; buffer_it++) { @@ -159,40 +158,8 @@ void RSICommandHandler::decodeNodes( isNoMoreParam = true; buffer_it++; } else { - // Validate parameter name - if (!element.IsParamNameValid(node_param, buffer_it)) { - std::stringstream err_ss; - err_ss << "The detected parameter \"" << node_param << - "\" does not match any of the " << element.GetName() << " elements parameters."; - throw std::logic_error(err_ss.str()); - } - // Check xml syntax (parameter name must be followed by '=') - if (*buffer_it != '=') { - std::stringstream err_ss; - err_ss << "In \"" << node_param << "\" param syntax error found."; - throw std::logic_error(err_ss.str()); - } - buffer_it++; - if (*buffer_it != '"') { - std::stringstream err_ss; - err_ss << "In \"" << node_param << "\" param syntax error found."; - throw std::logic_error(err_ss.str()); - } - buffer_it++; - // Cast parameter data - if (!element.CastParamData(node_param, buffer_it)) { - std::stringstream err_ss; - err_ss << "Could not cast the \"" << node_param << "\" param into the " << - element.GetName() << " elements parameter list."; - throw std::logic_error(err_ss.str()); - } - // Check xml syntax (Parameter value must be inside quotes) - if (*buffer_it != '"') { - std::stringstream err_ss; - err_ss << "In \"" << node_param << "\" param syntax error found."; - throw std::logic_error(err_ss.str()); - } - buffer_it++; + // Decode parameter + decodeParam(element, buffer_it); num_of_param++; } } @@ -232,36 +199,20 @@ void RSICommandHandler::decodeNodes( if (!is_param_only_node) { if (*buffer_it != '<') { // Node is a leaf node, checking its data - if (!element.CastParamData(node_name, buffer_it)) { - std::stringstream err_ss; - err_ss << "Could not cast the \"" << node_name << "\" parameter into the " << - element.GetName() << " elements parameter list."; - throw std::logic_error(err_ss.str()); - } - if (*buffer_it != '<') { - std::stringstream err_ss; - err_ss << "In \"" << node_param << "\" param syntax error found."; - throw std::logic_error(err_ss.str()); - } + decodeLeafNodeParamData(element, buffer_it); } else { - // Node has childs + // Node has childs, not a leaf node for (auto && child : element.Childs()) { decodeNodes(child, buffer, buffer_it, buffer_size); } } // Check for the end tag () - if (*(buffer_it) != '<') { - std::stringstream err_ss; - err_ss << "Syntax error at the end of " << element.GetName() << " node."; - throw std::logic_error(err_ss.str()); - } - buffer_it++; - if (*(buffer_it) != '/') { + if (*(buffer_it) != '<' && *(buffer_it + 1) != '/') { std::stringstream err_ss; err_ss << "Syntax error at the end of " << element.GetName() << " node."; throw std::logic_error(err_ss.str()); } - buffer_it++; + buffer_it += 2; if (!element.IsNameValid(node_name, buffer_it)) { std::stringstream err_ss; err_ss << "The detected name \"" << node_name << "\" does not match the elements name: " << @@ -277,73 +228,124 @@ void RSICommandHandler::decodeNodes( } } -void RSICommandHandler::encodeNodes( - xml::XMLElement & element, char * & buffer_it, int & size_left) +void RSICommandHandler::decodeParam(xml::XMLElement & element, char * & buffer_it) { - // Start the node with its name - auto idx = snprintf(buffer_it, size_left, "<%s", element.GetName().c_str()); - update_iterators(buffer_it, size_left, element, idx); - - bool is_param_only_node = false; - // If the node does not have childs it is a parameter-only node - if (element.GetChilds().size() == 0) { - is_param_only_node = true; + xml::XMLString node_param; + // Validate parameter name + if (!element.IsParamNameValid(node_param, buffer_it)) { + std::stringstream err_ss; + err_ss << "The detected parameter \"" << node_param << + "\" does not match any of the " << element.GetName() << " elements parameters."; + throw std::logic_error(err_ss.str()); } - for (auto && param : element.Params()) { - // In the current implementation it is not supported, that a parameter has the same name as - // it's base node - // Therefore if the element's name is equal to one of it's parameters, it can't be a - // parameter-only node - if (element.GetName() == param.first) { - is_param_only_node = false; - } else { - // Creating parameters - // Add param name - idx = snprintf(buffer_it, size_left, " %s=\"", param.first.c_str()); - update_iterators(buffer_it, size_left, element, idx); + // Check xml syntax (parameter name must be followed by '=') + if (*(buffer_it) != '=' && *(buffer_it + 1) != '"') { + std::stringstream err_ss; + err_ss << "In \"" << node_param << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); + } + buffer_it += 2; + // Cast parameter data + if (!element.CastParamData(node_param, buffer_it)) { + std::stringstream err_ss; + err_ss << "Could not cast the \"" << node_param << "\" param into the " << + element.GetName() << " elements parameter list."; + throw std::logic_error(err_ss.str()); + } + // Check xml syntax (Parameter value must be inside quotes) + if (*(buffer_it) != '"') { + std::stringstream err_ss; + err_ss << "In \"" << node_param << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); + } + buffer_it++; +} - // Add param data - param.second.PrintParam(buffer_it, size_left); - idx = snprintf(buffer_it, size_left, "\""); - update_iterators(buffer_it, size_left, element, idx); +void decodeLeafNodeParamData(xml::XMLElement & element, char * & buffer_it) +{ + xml::XMLString node_param; + if (*buffer_it != '<') { + // Node is a leaf node, checking its data + if (!element.CastParamData(node_name, buffer_it)) { + std::stringstream err_ss; + err_ss << "Could not cast the \"" << node_name << "\" parameter into the " << + element.GetName() << " elements parameter list."; + throw std::logic_error(err_ss.str()); + } + if (*buffer_it != '<') { + std::stringstream err_ss; + err_ss << "In \"" << node_param << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); } } - // Add start node end bracket - if (is_param_only_node) { - idx = snprintf(buffer_it, size_left, " />"); + void RSICommandHandler::encodeNodes( + xml::XMLElement & element, char * & buffer_it, int & size_left) + { + // Start the node with its name + auto idx = snprintf(buffer_it, size_left, "<%s", element.GetName().c_str()); update_iterators(buffer_it, size_left, element, idx); - } else { - idx = snprintf(buffer_it, size_left, ">"); - update_iterators(buffer_it, size_left, element, idx); - if (element.GetChilds().size() > 0) { - // Add childs - for (auto && child : element.Childs()) { - encodeNodes(child, buffer_it, size_left); + bool is_param_only_node = false; + // If the node does not have childs it is a parameter-only node + if (element.GetChilds().size() == 0) { + is_param_only_node = true; + } + for (auto && param : element.Params()) { + // In the current implementation it is not supported, that a parameter has the same name as + // it's base node + // Therefore if the element's name is equal to one of it's parameters, it can't be a + // parameter-only node + if (element.GetName() == param.first) { + is_param_only_node = false; + } else { + // Creating parameters + // Add param name + idx = snprintf(buffer_it, size_left, " %s=\"", param.first.c_str()); + update_iterators(buffer_it, size_left, element, idx); + + // Add param data + param.second.PrintParam(buffer_it, size_left); + idx = snprintf(buffer_it, size_left, "\""); + update_iterators(buffer_it, size_left, element, idx); } + } + // Add start node end bracket + if (is_param_only_node) { + idx = snprintf(buffer_it, size_left, " />"); + + update_iterators(buffer_it, size_left, element, idx); } else { - // Add data - element.Params().find(element.GetName())->second.PrintParam( - buffer_it, size_left); + idx = snprintf(buffer_it, size_left, ">"); + + update_iterators(buffer_it, size_left, element, idx); + if (element.GetChilds().size() > 0) { + // Add childs + for (auto && child : element.Childs()) { + encodeNodes(child, buffer_it, size_left); + } + } else { + // Add data + element.Params().find(element.GetName())->second.PrintParam( + buffer_it, size_left); + } + // Add node end tag + idx = snprintf(buffer_it, size_left, "", element.GetName().c_str()); + update_iterators(buffer_it, size_left, element, idx); } - // Add node end tag - idx = snprintf(buffer_it, size_left, "", element.GetName().c_str()); - update_iterators(buffer_it, size_left, element, idx); } -} -void update_iterators( - char * & buffer_it, int & buf_size_left, const xml::XMLElement & element, - const int & buf_idx) -{ - if (buf_idx < 0 || buf_idx > buf_size_left) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } else { - buffer_it += buf_idx; - buf_size_left -= buf_idx; + void RSICommandHandler::update_iterators( + char * & buffer_it, int & buf_size_left, const xml::XMLElement & element, + const int & buf_idx) + { + if (buf_idx < 0 || buf_idx > buf_size_left) { + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); + } else { + buffer_it += buf_idx; + buf_size_left -= buf_idx; + } } -} } // namespace kuka_rsi_hw_interface From 4871cbdd5906b3b05dc4cdcf91bb5921d91f3574 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 22 Aug 2023 19:44:44 +0200 Subject: [PATCH 77/97] update decode refactor --- .../rsi_command_handler.hpp | 51 ++---- .../include/xml_handler/xml_string.hpp | 2 - .../src/kuka_hardware_interface.cpp | 13 +- .../src/rsi_command_handler.cpp | 145 +++++++++--------- 4 files changed, 86 insertions(+), 125 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 8899b701..013f4fb3 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -1,41 +1,16 @@ -/********************************************************************* -* 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: Komaromi Sandor - */ +// Copyright 2023 Komáromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_HPP_ #define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_HPP_ diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp index 95e2cca7..cc113ac1 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -36,8 +36,6 @@ class XMLString throw std::range_error("XMLString data length out of range. Range: 0 - 255"); } } - // explicit XMLString(const std::string & str) - // : data_ptr_(str.c_str()), length_(str.length()) {} int PrintString(char * & buffer_it, const int & size_left); diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 01b1bf79..6b90bcb4 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -190,9 +190,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); for (size_t i = 0; i < info_.joints.size(); ++i) { - // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; hw_commands_[i] = hw_states_[i]; - // initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; } // ipoc_ = rsi_state_.ipoc; @@ -211,7 +209,6 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "Encode Failed"); return CallbackReturn::FAILURE; } - // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); server_->set_timeout(1000); // Set receive timeout to 1 second @@ -247,10 +244,7 @@ return_type KukaRSIHardwareInterface::read( this->on_deactivate(this->get_state()); return return_type::ERROR; } - - // for (std::size_t i = 0; i < info_.joints.size(); ++i) { - // hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; - // } + // Update state params hw_states_[0] = angles::from_degrees( command_handler_.GetState().GetElement("AIPos")->GetParam("A1")); hw_states_[1] = angles::from_degrees( @@ -263,7 +257,8 @@ return_type KukaRSIHardwareInterface::read( command_handler_.GetState().GetElement("AIPos")->GetParam("A5")); hw_states_[5] = angles::from_degrees( command_handler_.GetState().GetElement("AIPos")->GetParam("A6")); - // ipoc_ = rsi_state_.ipoc; + + // Update ipoc ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); return return_type::OK; } @@ -286,6 +281,7 @@ return_type KukaRSIHardwareInterface::write( joint_pos_correction_deg_[i] = angles::to_degrees(hw_commands_[i] - initial_joint_pos_[i]); } + // Update Command params command_handler_.SetCommandParam("AK", "A1", joint_pos_correction_deg_[0]); command_handler_.SetCommandParam("AK", "A2", joint_pos_correction_deg_[1]); command_handler_.SetCommandParam("AK", "A3", joint_pos_correction_deg_[2]); @@ -295,7 +291,6 @@ return_type KukaRSIHardwareInterface::write( command_handler_.SetCommandParam("Stop", "Stop", stop_flag_); command_handler_.SetCommandParam("IPOC", "IPOC", static_cast(ipoc_)); - // out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; auto out_buffer_it = out_buffer_; if (command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE) < 0) { this->on_deactivate(this->get_state()); diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 9f18e65e..8d314e92 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -199,7 +199,18 @@ void RSICommandHandler::decodeNodes( if (!is_param_only_node) { if (*buffer_it != '<') { // Node is a leaf node, checking its data - decodeLeafNodeParamData(element, buffer_it); + if (!element.CastParamData(node_name, buffer_it)) { + std::stringstream err_ss; + err_ss << "Could not cast the \"" << node_name << "\" parameter into the " << + element.GetName() << " elements parameter list."; + throw std::logic_error(err_ss.str()); + } + if (*buffer_it != '<') { + std::stringstream err_ss; + err_ss << "In \"" << node_name << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); + } + } else { // Node has childs, not a leaf node for (auto && child : element.Childs()) { @@ -261,91 +272,73 @@ void RSICommandHandler::decodeParam(xml::XMLElement & element, char * & buffer_i buffer_it++; } -void decodeLeafNodeParamData(xml::XMLElement & element, char * & buffer_it) +void RSICommandHandler::encodeNodes( + xml::XMLElement & element, char * & buffer_it, int & size_left) { - xml::XMLString node_param; - if (*buffer_it != '<') { - // Node is a leaf node, checking its data - if (!element.CastParamData(node_name, buffer_it)) { - std::stringstream err_ss; - err_ss << "Could not cast the \"" << node_name << "\" parameter into the " << - element.GetName() << " elements parameter list."; - throw std::logic_error(err_ss.str()); - } - if (*buffer_it != '<') { - std::stringstream err_ss; - err_ss << "In \"" << node_param << "\" param syntax error found."; - throw std::logic_error(err_ss.str()); + // Start the node with its name + auto idx = snprintf(buffer_it, size_left, "<%s", element.GetName().c_str()); + update_iterators(buffer_it, size_left, element, idx); + + bool is_param_only_node = false; + // If the node does not have childs it is a parameter-only node + if (element.GetChilds().size() == 0) { + is_param_only_node = true; + } + for (auto && param : element.Params()) { + // In the current implementation it is not supported, that a parameter has the same name as + // it's base node + // Therefore if the element's name is equal to one of it's parameters, it can't be a + // parameter-only node + if (element.GetName() == param.first) { + is_param_only_node = false; + } else { + // Creating parameters + // Add param name + idx = snprintf(buffer_it, size_left, " %s=\"", param.first.c_str()); + update_iterators(buffer_it, size_left, element, idx); + + // Add param data + param.second.PrintParam(buffer_it, size_left); + idx = snprintf(buffer_it, size_left, "\""); + update_iterators(buffer_it, size_left, element, idx); } } + // Add start node end bracket + if (is_param_only_node) { + idx = snprintf(buffer_it, size_left, " />"); - void RSICommandHandler::encodeNodes( - xml::XMLElement & element, char * & buffer_it, int & size_left) - { - // Start the node with its name - auto idx = snprintf(buffer_it, size_left, "<%s", element.GetName().c_str()); update_iterators(buffer_it, size_left, element, idx); + } else { + idx = snprintf(buffer_it, size_left, ">"); - bool is_param_only_node = false; - // If the node does not have childs it is a parameter-only node - if (element.GetChilds().size() == 0) { - is_param_only_node = true; - } - for (auto && param : element.Params()) { - // In the current implementation it is not supported, that a parameter has the same name as - // it's base node - // Therefore if the element's name is equal to one of it's parameters, it can't be a - // parameter-only node - if (element.GetName() == param.first) { - is_param_only_node = false; - } else { - // Creating parameters - // Add param name - idx = snprintf(buffer_it, size_left, " %s=\"", param.first.c_str()); - update_iterators(buffer_it, size_left, element, idx); - - // Add param data - param.second.PrintParam(buffer_it, size_left); - idx = snprintf(buffer_it, size_left, "\""); - update_iterators(buffer_it, size_left, element, idx); + update_iterators(buffer_it, size_left, element, idx); + if (element.GetChilds().size() > 0) { + // Add childs + for (auto && child : element.Childs()) { + encodeNodes(child, buffer_it, size_left); } - } - // Add start node end bracket - if (is_param_only_node) { - idx = snprintf(buffer_it, size_left, " />"); - - update_iterators(buffer_it, size_left, element, idx); } else { - idx = snprintf(buffer_it, size_left, ">"); - - update_iterators(buffer_it, size_left, element, idx); - if (element.GetChilds().size() > 0) { - // Add childs - for (auto && child : element.Childs()) { - encodeNodes(child, buffer_it, size_left); - } - } else { - // Add data - element.Params().find(element.GetName())->second.PrintParam( - buffer_it, size_left); - } - // Add node end tag - idx = snprintf(buffer_it, size_left, "", element.GetName().c_str()); - update_iterators(buffer_it, size_left, element, idx); + // Add data + element.Params().find(element.GetName())->second.PrintParam( + buffer_it, size_left); } + // Add node end tag + idx = snprintf(buffer_it, size_left, "", element.GetName().c_str()); + update_iterators(buffer_it, size_left, element, idx); } +} - void RSICommandHandler::update_iterators( - char * & buffer_it, int & buf_size_left, const xml::XMLElement & element, - const int & buf_idx) - { - if (buf_idx < 0 || buf_idx > buf_size_left) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } else { - buffer_it += buf_idx; - buf_size_left -= buf_idx; - } +void RSICommandHandler::update_iterators( + char * & buffer_it, int & buf_size_left, const xml::XMLElement & element, + const int & buf_idx) +{ + if (buf_idx < 0 || buf_idx > buf_size_left) { + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); + } else { + buffer_it += buf_idx; + buf_size_left -= buf_idx; } +} } // namespace kuka_rsi_hw_interface From 7bb798d70f9e434543514ae88aa34f4a4ff53431 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 22 Aug 2023 20:06:59 +0200 Subject: [PATCH 78/97] refactor decode to lower complexity #2 --- .../kuka_hardware_interface.hpp | 3 - .../rsi_command_handler.hpp | 6 +- .../src/rsi_command_handler.cpp | 71 +++++++++++-------- .../src/xml_handler/xml_element.cpp | 1 - 4 files changed, 47 insertions(+), 34 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index 1be64e8a..53d83152 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -114,9 +114,6 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface std::unique_ptr server_; char in_buffer_[UDP_BUFFER_SIZE] = {0}; char out_buffer_[UDP_BUFFER_SIZE] = {0}; - - // static constexpr double R2D = 180 / M_PI; - // static constexpr double D2R = M_PI / 180; }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 013f4fb3..473993bc 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -60,7 +60,11 @@ class RSICommandHandler void decodeParam(xml::XMLElement & element, char * & buffer_it); - void decodeLeafNodeParamData(xml::XMLElement & element, char * & buffer_it); + void decodeLeafNodeParamData( + xml::XMLElement & element, char * & buffer_it, + xml::XMLString & param_name); + + void decodeNodeEnd(xml::XMLElement & element, char * & buffer_it); // node encoder functins diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 8d314e92..8a979cb6 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -199,17 +199,7 @@ void RSICommandHandler::decodeNodes( if (!is_param_only_node) { if (*buffer_it != '<') { // Node is a leaf node, checking its data - if (!element.CastParamData(node_name, buffer_it)) { - std::stringstream err_ss; - err_ss << "Could not cast the \"" << node_name << "\" parameter into the " << - element.GetName() << " elements parameter list."; - throw std::logic_error(err_ss.str()); - } - if (*buffer_it != '<') { - std::stringstream err_ss; - err_ss << "In \"" << node_name << "\" param syntax error found."; - throw std::logic_error(err_ss.str()); - } + decodeLeafNodeParamData(element, buffer_it, node_name); } else { // Node has childs, not a leaf node @@ -218,24 +208,7 @@ void RSICommandHandler::decodeNodes( } } // Check for the end tag () - if (*(buffer_it) != '<' && *(buffer_it + 1) != '/') { - std::stringstream err_ss; - err_ss << "Syntax error at the end of " << element.GetName() << " node."; - throw std::logic_error(err_ss.str()); - } - buffer_it += 2; - if (!element.IsNameValid(node_name, buffer_it)) { - std::stringstream err_ss; - err_ss << "The detected name \"" << node_name << "\" does not match the elements name: " << - element.GetName() << "."; - throw std::logic_error(err_ss.str()); - } - if (*buffer_it != '>') { - std::stringstream err_ss; - err_ss << "Syntax error at the end of " << element.GetName() << " node."; - throw std::logic_error(err_ss.str()); - } - buffer_it++; + decodeNodeEnd(element, buffer_it); } } @@ -272,6 +245,46 @@ void RSICommandHandler::decodeParam(xml::XMLElement & element, char * & buffer_i buffer_it++; } +void RSICommandHandler::decodeLeafNodeParamData( + xml::XMLElement & element, char * & buffer_it, + xml::XMLString & param_name) +{ + if (!element.CastParamData(param_name, buffer_it)) { + std::stringstream err_ss; + err_ss << "Could not cast the \"" << param_name << "\" parameter into the " << + element.GetName() << " elements parameter list."; + throw std::logic_error(err_ss.str()); + } + if (*buffer_it != '<') { + std::stringstream err_ss; + err_ss << "In \"" << param_name << "\" param syntax error found."; + throw std::logic_error(err_ss.str()); + } +} + +void RSICommandHandler::decodeNodeEnd(xml::XMLElement & element, char * & buffer_it) +{ + xml::XMLString node_name; + if (*(buffer_it) != '<' && *(buffer_it + 1) != '/') { + std::stringstream err_ss; + err_ss << "Syntax error at the end of " << element.GetName() << " node."; + throw std::logic_error(err_ss.str()); + } + buffer_it += 2; + if (!element.IsNameValid(node_name, buffer_it)) { + std::stringstream err_ss; + err_ss << "The detected name \"" << node_name << "\" does not match the elements name: " << + element.GetName() << "."; + throw std::logic_error(err_ss.str()); + } + if (*buffer_it != '>') { + std::stringstream err_ss; + err_ss << "Syntax error at the end of " << element.GetName() << " node."; + throw std::logic_error(err_ss.str()); + } + buffer_it++; +} + void RSICommandHandler::encodeNodes( xml::XMLElement & element, char * & buffer_it, int & size_left) { diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 91ba9d63..581260b4 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -82,7 +82,6 @@ bool XMLElement::IsParamNameValid(XMLString & key, char * & str_ptr) return true; } return false; - // return params_.find(key) != params_.end(); } From 24aa60070baefd06c16420a8d667817fb1a3449b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 22 Aug 2023 20:24:07 +0200 Subject: [PATCH 79/97] refactor decode to lower complexity #3 --- .../src/rsi_command_handler.cpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 8a979cb6..955801c5 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -139,8 +139,8 @@ void RSICommandHandler::decodeNodes( // Validate node parameters size_t num_of_param = 0; bool is_param_only_node = false; - bool isNoMoreParam = false; - while (!isNoMoreParam && static_cast(buffer_it - buffer) < buffer_size) { + bool is_no_more_param = false; + while (!is_no_more_param && static_cast(buffer_it - buffer) < buffer_size) { // Go to the next non-space character for (; *buffer_it == ' '; buffer_it++) { } @@ -152,10 +152,10 @@ void RSICommandHandler::decodeNodes( // Check for the start node's closing tag if (*buffer_it == '/' && *(buffer_it + 1) == '>') { is_param_only_node = true; - isNoMoreParam = true; + is_no_more_param = true; buffer_it += 2; } else if (*buffer_it == '>') { - isNoMoreParam = true; + is_no_more_param = true; buffer_it++; } else { // Decode parameter @@ -196,17 +196,17 @@ void RSICommandHandler::decodeNodes( } // If a node is not an only-parameter node, than it has data or childs and needs an end node - if (!is_param_only_node) { - if (*buffer_it != '<') { - // Node is a leaf node, checking its data - decodeLeafNodeParamData(element, buffer_it, node_name); - - } else { - // Node has childs, not a leaf node - for (auto && child : element.Childs()) { - decodeNodes(child, buffer, buffer_it, buffer_size); - } + if (!is_param_only_node && *buffer_it != '<') { + // Node is a leaf node, checking its data + decodeLeafNodeParamData(element, buffer_it, node_name); + } + if (!is_param_only_node && *buffer_it == '<') { + // Node has childs, not a leaf node + for (auto && child : element.Childs()) { + decodeNodes(child, buffer, buffer_it, buffer_size); } + } + if (!is_no_more_param) { // Check for the end tag () decodeNodeEnd(element, buffer_it); } From 8ec36ffd2cab68e8884c4cbaeddeb6e7a495b17b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 22 Aug 2023 20:45:48 +0200 Subject: [PATCH 80/97] refactor decode to lower complexity #4 --- .../rsi_command_handler.hpp | 4 ++ .../src/rsi_command_handler.cpp | 50 ++++++++++--------- 2 files changed, 30 insertions(+), 24 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index 473993bc..eb723573 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -66,6 +66,10 @@ class RSICommandHandler void decodeNodeEnd(xml::XMLElement & element, char * & buffer_it); + void decodeSkipSpaces( + char * & buffer_it, const xml::XMLElement & element, char * const buffer, + const size_t & buffer_size); + // node encoder functins void encodeNodes( diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 955801c5..5964df2b 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -142,13 +142,8 @@ void RSICommandHandler::decodeNodes( bool is_no_more_param = false; while (!is_no_more_param && static_cast(buffer_it - buffer) < buffer_size) { // Go to the next non-space character - for (; *buffer_it == ' '; buffer_it++) { - } - if (static_cast(buffer_it - buffer) >= buffer_size) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } + decodeSkipSpaces(buffer_it, element, buffer, buffer_size); + // Check for the start node's closing tag if (*buffer_it == '/' && *(buffer_it + 1) == '>') { is_param_only_node = true; @@ -187,26 +182,20 @@ void RSICommandHandler::decodeNodes( throw std::logic_error(err_ss.str()); } // Go to the next node or the end of the node - for (; *buffer_it == ' '; buffer_it++) { - } - if (static_cast(buffer_it - buffer) >= buffer_size) { - std::stringstream err_ss; - err_ss << "Out of range error in " << element.GetName() << " node."; - throw std::range_error(err_ss.str()); - } + decodeSkipSpaces(buffer_it, element, buffer, buffer_size); // If a node is not an only-parameter node, than it has data or childs and needs an end node - if (!is_param_only_node && *buffer_it != '<') { - // Node is a leaf node, checking its data - decodeLeafNodeParamData(element, buffer_it, node_name); - } - if (!is_param_only_node && *buffer_it == '<') { - // Node has childs, not a leaf node - for (auto && child : element.Childs()) { - decodeNodes(child, buffer, buffer_it, buffer_size); + if (!is_param_only_node) { + if (*buffer_it != '<') { + // Node is a leaf node, checking its data + decodeLeafNodeParamData(element, buffer_it, node_name); + + } else { + // Node has childs, not a leaf node + for (auto && child : element.Childs()) { + decodeNodes(child, buffer, buffer_it, buffer_size); + } } - } - if (!is_no_more_param) { // Check for the end tag () decodeNodeEnd(element, buffer_it); } @@ -285,6 +274,19 @@ void RSICommandHandler::decodeNodeEnd(xml::XMLElement & element, char * & buffer buffer_it++; } +void RSICommandHandler::decodeSkipSpaces( + char * & buffer_it, const xml::XMLElement & element, char * const buffer, + const size_t & buffer_size) +{ + for (; *buffer_it == ' '; buffer_it++) { + } + if (static_cast(buffer_it - buffer) >= buffer_size) { + std::stringstream err_ss; + err_ss << "Out of range error in " << element.GetName() << " node."; + throw std::range_error(err_ss.str()); + } +} + void RSICommandHandler::encodeNodes( xml::XMLElement & element, char * & buffer_it, int & size_left) { From 856db58871268fecfcf41bbd2166664243d35189 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 22 Aug 2023 20:50:26 +0200 Subject: [PATCH 81/97] Remove old xml parser --- kuka_rsi_hw_interface/CMakeLists.txt | 3 - .../kuka_rsi_hw_interface/rsi_command.h | 87 -------------- .../include/kuka_rsi_hw_interface/rsi_state.h | 112 ------------------ 3 files changed, 202 deletions(-) delete mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h delete mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 37c367b0..6b763e58 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -25,8 +25,6 @@ find_package(controller_manager_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(angles REQUIRED) -find_package(tinyxml_vendor REQUIRED) -find_package(TinyXML REQUIRED) include_directories(include ${TinyXML2_INCLUDE_DIRS}) @@ -51,7 +49,6 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUIL target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface angles) -target_link_libraries(${PROJECT_NAME} tinyxml) add_executable(robot_manager_node src/robot_manager_node.cpp) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h deleted file mode 100644 index 4be14a34..00000000 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h +++ /dev/null @@ -1,87 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Lars Tingelstad - */ - -#ifndef KUKA_RSI_HW_INTERFACE__RSI_COMMAND_H_ -#define KUKA_RSI_HW_INTERFACE__RSI_COMMAND_H_ - -#include -#include -#include - -namespace kuka_rsi_hw_interface -{ -class RSICommand -{ -public: - RSICommand() - { - } - RSICommand(std::vector joint_position_correction, uint64_t ipoc, bool stop = false) - { - TiXmlDocument doc; - TiXmlElement * root = new TiXmlElement("Sen"); - root->SetAttribute("Type", "KROSHU"); - TiXmlElement * el = new TiXmlElement("AK"); - // Add string attribute - el->SetAttribute("A1", std::to_string(joint_position_correction[0])); - el->SetAttribute("A2", std::to_string(joint_position_correction[1])); - el->SetAttribute("A3", std::to_string(joint_position_correction[2])); - el->SetAttribute("A4", std::to_string(joint_position_correction[3])); - el->SetAttribute("A5", std::to_string(joint_position_correction[4])); - el->SetAttribute("A6", std::to_string(joint_position_correction[5])); - root->LinkEndChild(el); - - el = new TiXmlElement("Stop"); - el->LinkEndChild(new TiXmlText(std::to_string(static_cast(stop)))); - root->LinkEndChild(el); - - el = new TiXmlElement("IPOC"); - el->LinkEndChild(new TiXmlText(std::to_string(ipoc))); - root->LinkEndChild(el); - doc.LinkEndChild(root); - TiXmlPrinter printer; - printer.SetStreamPrinting(); - doc.Accept(&printer); - - xml_doc = printer.Str(); - } - std::string xml_doc; -}; -} // namespace kuka_rsi_hw_interface - -#endif // KUKA_RSI_HW_INTERFACE__RSI_COMMAND_H_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h deleted file mode 100644 index f859dd67..00000000 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h +++ /dev/null @@ -1,112 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Lars Tingelstad -*/ - -#ifndef KUKA_RSI_HW_INTERFACE__RSI_STATE_H_ -#define KUKA_RSI_HW_INTERFACE__RSI_STATE_H_ - -#include -#include -#include - -namespace kuka_rsi_hw_interface -{ -class RSIState -{ -private: - std::string xml_doc_; - -public: - RSIState() - { - xml_doc_.resize(1024); - } - - explicit RSIState(std::string xml_doc) - : xml_doc_(xml_doc) - { - // Parse message from robot - TiXmlDocument bufferdoc; - bufferdoc.Parse(xml_doc_.c_str()); - // Get the Rob node: - TiXmlElement * rob = bufferdoc.FirstChildElement("Rob"); - // Extract axis specific actual position - TiXmlElement * AIPos_el = rob->FirstChildElement("AIPos"); - AIPos_el->Attribute("A1", &positions[0]); - AIPos_el->Attribute("A2", &positions[1]); - AIPos_el->Attribute("A3", &positions[2]); - AIPos_el->Attribute("A4", &positions[3]); - AIPos_el->Attribute("A5", &positions[4]); - AIPos_el->Attribute("A6", &positions[5]); - // Extract axis specific setpoint position - TiXmlElement * ASPos_el = rob->FirstChildElement("ASPos"); - ASPos_el->Attribute("A1", &initial_positions[0]); - ASPos_el->Attribute("A2", &initial_positions[1]); - ASPos_el->Attribute("A3", &initial_positions[2]); - ASPos_el->Attribute("A4", &initial_positions[3]); - ASPos_el->Attribute("A5", &initial_positions[4]); - ASPos_el->Attribute("A6", &initial_positions[5]); - // Extract cartesian actual position - TiXmlElement * RIst_el = rob->FirstChildElement("RIst"); - RIst_el->Attribute("X", &cart_position[0]); - RIst_el->Attribute("Y", &cart_position[1]); - RIst_el->Attribute("Z", &cart_position[2]); - RIst_el->Attribute("A", &cart_position[3]); - RIst_el->Attribute("B", &cart_position[4]); - RIst_el->Attribute("C", &cart_position[5]); - // Extract cartesian setpoint position - TiXmlElement * RSol_el = rob->FirstChildElement("RSol"); - RSol_el->Attribute("X", &initial_cart_position[0]); - RSol_el->Attribute("Y", &initial_cart_position[1]); - RSol_el->Attribute("Z", &initial_cart_position[2]); - RSol_el->Attribute("A", &initial_cart_position[3]); - RSol_el->Attribute("B", &initial_cart_position[4]); - RSol_el->Attribute("C", &initial_cart_position[5]); - // Get the IPOC timestamp - TiXmlElement * ipoc_el = rob->FirstChildElement("IPOC"); - ipoc = std::stoull(ipoc_el->FirstChild()->Value()); - } - - std::vector positions = std::vector(6, 0.0); - std::vector initial_positions = std::vector(6, 0.0); - std::vector cart_position = std::vector(6, 0.0); - std::vector initial_cart_position = std::vector(6, 0.0); - uint64_t ipoc = 0; -}; -} // namespace kuka_rsi_hw_interface - -#endif // KUKA_RSI_HW_INTERFACE__RSI_STATE_H_ From d68f057eb606fddd314b553bb241b697814af536 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 22 Aug 2023 21:01:56 +0200 Subject: [PATCH 82/97] remove redundant --- kuka_rsi_hw_interface/src/rsi_command_handler.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 5964df2b..9b7e67c6 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -121,7 +121,7 @@ void RSICommandHandler::decodeNodes( const size_t buffer_size) { xml::XMLString node_name(buffer, 0); - if (*(buffer_it) != '<') { + if (*buffer_it != '<') { std::stringstream err_ss; err_ss << "Syntax error at the start of " << element.GetName() << " node."; throw std::logic_error(err_ss.str()); @@ -212,7 +212,7 @@ void RSICommandHandler::decodeParam(xml::XMLElement & element, char * & buffer_i throw std::logic_error(err_ss.str()); } // Check xml syntax (parameter name must be followed by '=') - if (*(buffer_it) != '=' && *(buffer_it + 1) != '"') { + if (*buffer_it != '=' && *(buffer_it + 1) != '"') { std::stringstream err_ss; err_ss << "In \"" << node_param << "\" param syntax error found."; throw std::logic_error(err_ss.str()); @@ -226,7 +226,7 @@ void RSICommandHandler::decodeParam(xml::XMLElement & element, char * & buffer_i throw std::logic_error(err_ss.str()); } // Check xml syntax (Parameter value must be inside quotes) - if (*(buffer_it) != '"') { + if (*buffer_it != '"') { std::stringstream err_ss; err_ss << "In \"" << node_param << "\" param syntax error found."; throw std::logic_error(err_ss.str()); @@ -254,7 +254,7 @@ void RSICommandHandler::decodeLeafNodeParamData( void RSICommandHandler::decodeNodeEnd(xml::XMLElement & element, char * & buffer_it) { xml::XMLString node_name; - if (*(buffer_it) != '<' && *(buffer_it + 1) != '/') { + if (*buffer_it != '<' && *(buffer_it + 1) != '/') { std::stringstream err_ss; err_ss << "Syntax error at the end of " << element.GetName() << " node."; throw std::logic_error(err_ss.str()); From 74322030a827abc0c75fdb6ec5b260d7de31679b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kom=C3=A1romi=20S=C3=A1ndor?= <48948212+altex111@users.noreply.github.com> Date: Thu, 24 Aug 2023 09:47:42 +0200 Subject: [PATCH 83/97] correcting comments --- kuka_rsi_hw_interface/CMakeLists.txt | 3 --- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 2 -- 2 files changed, 5 deletions(-) diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 6b763e58..decc14da 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -25,9 +25,6 @@ find_package(controller_manager_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(angles REQUIRED) - -include_directories(include ${TinyXML2_INCLUDE_DIRS}) - add_library(xml_handler SHARED src/xml_handler/xml_element.cpp src/xml_handler/xml_param.cpp diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 6b90bcb4..f31e3fdd 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -192,7 +192,6 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta for (size_t i = 0; i < info_.joints.size(); ++i) { hw_commands_[i] = hw_states_[i]; } - // ipoc_ = rsi_state_.ipoc; // Initial command pos data command_handler_.SetCommandParam("AK", "A1", joint_pos_correction_deg_[0]); @@ -241,7 +240,6 @@ return_type KukaRSIHardwareInterface::read( return return_type::ERROR; } if (!command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE)) { - this->on_deactivate(this->get_state()); return return_type::ERROR; } // Update state params From 2706ee1b7724aaecc4421fb29872b24d95f54404 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 24 Aug 2023 10:42:25 +0200 Subject: [PATCH 84/97] fix include --- kuka_rsi_hw_interface/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index decc14da..31a1887f 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -25,6 +25,8 @@ find_package(controller_manager_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(angles REQUIRED) +include_directories(include) + add_library(xml_handler SHARED src/xml_handler/xml_element.cpp src/xml_handler/xml_param.cpp From c7d7b221bcea2f73fb40d4ffca861603f57f155b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 24 Aug 2023 16:30:11 +0200 Subject: [PATCH 85/97] set locale when it is needed --- .../include/xml_handler/xml_element.hpp | 2 ++ .../include/xml_handler/xml_param.hpp | 3 +++ .../src/xml_handler/xml_element.cpp | 19 +++++++++++++++++++ .../src/xml_handler/xml_param.cpp | 9 +++++++++ 4 files changed, 33 insertions(+) diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 05a4ee10..0b98e9f5 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -209,6 +209,8 @@ class XMLElement std::map> params_; std::vector childs_; std::string name_; + + char prev_locale_[100] = {0}; }; } // namespace xml diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp index 83b9951a..459f6247 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp @@ -51,6 +51,9 @@ class XMLParam } throw std::logic_error("Parameter type not supported"); } + +private: + char prev_locale_[100] = {0}; }; } // namespace xml diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 581260b4..8fc9141e 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "xml_handler/xml_element.hpp" @@ -30,12 +31,20 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) auto ret_val = param_it != params_.end(); if (ret_val) { errno = 0; + char * prev_locale_ptr = prev_locale_; + prev_locale_ptr = std::setlocale(LC_NUMERIC, nullptr); + if (!std::setlocale(LC_NUMERIC, "C")) { + throw std::logic_error("Locale set failed"); + } switch (param_it->second.param_type_) { case XMLType::BOOL: { char * end = str_ptr; auto res = static_cast(strtol(str_ptr, &end, 0)); if (res == 0 && (errno != 0 || end == str_ptr)) { + if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { + throw std::logic_error("Locale set failed"); + } return false; } str_ptr = end; @@ -47,6 +56,9 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) char * end = str_ptr; auto res = strtol(str_ptr, &end, 0); if (res == 0 && (errno != 0 || end == str_ptr)) { + if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { + throw std::logic_error("Locale set failed"); + } return false; } str_ptr = end; @@ -58,6 +70,9 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) char * end = str_ptr; auto res = strtod(str_ptr, &end); if (res == 0 && (errno != 0 || end == str_ptr)) { + if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { + throw std::logic_error("Locale set failed"); + } return false; } str_ptr = end; @@ -68,8 +83,12 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) param_it->second.param_value_ = castXMLString(str_ptr); break; default: + std::setlocale(LC_NUMERIC, prev_locale_ptr); return false; } + if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { + throw std::logic_error("Locale set failed"); + } } return ret_val; } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index d777fa9c..38fdbaff 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -23,6 +23,11 @@ namespace xml int XMLParam::PrintParam(char * & buffer_it, int & size_left) { int idx = 0; + char * prev_locale_ptr = prev_locale_; + prev_locale_ptr = std::setlocale(LC_NUMERIC, nullptr); + if (!std::setlocale(LC_NUMERIC, "C")) { + throw std::logic_error("Locale set failed"); + } switch (param_type_) { case XMLType::BOOL: idx = snprintf(buffer_it, size_left, "%u", GetParamValue()); @@ -39,10 +44,14 @@ int XMLParam::PrintParam(char * & buffer_it, int & size_left) case XMLType::STRING: idx = GetParamValue().PrintString(buffer_it, size_left); break; + default: throw std::logic_error("Parameter type not supported"); break; } + if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { + throw std::logic_error("Locale set failed"); + } if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); } else { From cd58358aee603b71c98dd5c6d606d95ac9efc65f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 24 Aug 2023 16:51:37 +0200 Subject: [PATCH 86/97] remove deactivate --- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index f31e3fdd..47d955b6 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -291,7 +291,6 @@ return_type KukaRSIHardwareInterface::write( auto out_buffer_it = out_buffer_; if (command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE) < 0) { - this->on_deactivate(this->get_state()); return return_type::ERROR; } server_->send(out_buffer_); From 081857803ba88ad4c63d02695240fa18c8d31a7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kom=C3=A1romi=20S=C3=A1ndor?= <48948212+altex111@users.noreply.github.com> Date: Fri, 25 Aug 2023 14:10:37 +0200 Subject: [PATCH 87/97] Update locale set and reset --- .../rsi_command_handler.hpp | 4 ++++ .../include/xml_handler/xml_element.hpp | 2 -- .../src/kuka_hardware_interface.cpp | 3 ++- .../src/rsi_command_handler.cpp | 14 ++++++++++++++ .../src/xml_handler/xml_element.cpp | 19 ------------------- .../src/xml_handler/xml_param.cpp | 8 -------- 6 files changed, 20 insertions(+), 30 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp index eb723573..fcec4008 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -47,11 +47,15 @@ class RSICommandHandler bool Decode(char * const buffer, const size_t buffer_size); int Encode(char * & buffer, const size_t buffer_size); + bool SetLocale(); + bool ResetLocale(); private: xml::XMLElement command_data_structure_; xml::XMLElement state_data_structure_; + std::string prev_locale_; + // node decoder functions void decodeNodes( diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp index 0b98e9f5..05a4ee10 100644 --- a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -209,8 +209,6 @@ class XMLElement std::map> params_; std::vector childs_; std::string name_; - - char prev_locale_[100] = {0}; }; } // namespace xml diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 47d955b6..210ad326 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -152,7 +152,7 @@ CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta if (bytes < 100) { bytes = server_->recv(in_buffer_); } - + command_handler_.SetLocale(); if (!command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE)) { RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "Decode failed"); return CallbackReturn::FAILURE; @@ -222,6 +222,7 @@ CallbackReturn KukaRSIHardwareInterface::on_deactivate( { stop_flag_ = true; RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Stop flag was set!"); + command_handler_.ResetLocale(); return CallbackReturn::SUCCESS; } diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 9b7e67c6..37002670 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "kuka_rsi_hw_interface/rsi_command_handler.hpp" #include "rclcpp/rclcpp.hpp" @@ -25,6 +26,8 @@ namespace kuka_rsi_hw_interface RSICommandHandler::RSICommandHandler() : command_data_structure_("Sen"), state_data_structure_("Rob") { + // save previous locale to restore later; + prev_locale_ = std::setlocale(LC_NUMERIC, nullptr); // State structure // TODO(Komaromi): Later this should be defined by the rsi xml state_data_structure_.Params()["TYPE"] = xml::XMLParam(xml::XMLType::STRING); @@ -86,6 +89,15 @@ RSICommandHandler::RSICommandHandler() command_data_structure_.Childs().emplace_back(Stop_el); command_data_structure_.Childs().emplace_back(Ipoc_command_el); } +bool RSICommandHandler::SetLocale() +{ + return std::setlocale(LC_NUMERIC, "C"); +} + +bool RSICommandHandle::ResetLocale() +{ + return std::setlocale(LC_NUMERIC, prev_locale_) +} bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) { @@ -95,6 +107,7 @@ bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) return true; } catch (const std::exception & e) { RCLCPP_ERROR(rclcpp::get_logger("CommandHandler"), "%s", e.what()); + ResetLocale(); return false; } } @@ -112,6 +125,7 @@ int RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) return buffer_size - size_left + 1; } catch (const std::exception & e) { RCLCPP_ERROR(rclcpp::get_logger("CommandHandler"), "%s", e.what()); + ResetLocale(); return -1; } } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp index 8fc9141e..581260b4 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include "xml_handler/xml_element.hpp" @@ -31,20 +30,12 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) auto ret_val = param_it != params_.end(); if (ret_val) { errno = 0; - char * prev_locale_ptr = prev_locale_; - prev_locale_ptr = std::setlocale(LC_NUMERIC, nullptr); - if (!std::setlocale(LC_NUMERIC, "C")) { - throw std::logic_error("Locale set failed"); - } switch (param_it->second.param_type_) { case XMLType::BOOL: { char * end = str_ptr; auto res = static_cast(strtol(str_ptr, &end, 0)); if (res == 0 && (errno != 0 || end == str_ptr)) { - if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { - throw std::logic_error("Locale set failed"); - } return false; } str_ptr = end; @@ -56,9 +47,6 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) char * end = str_ptr; auto res = strtol(str_ptr, &end, 0); if (res == 0 && (errno != 0 || end == str_ptr)) { - if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { - throw std::logic_error("Locale set failed"); - } return false; } str_ptr = end; @@ -70,9 +58,6 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) char * end = str_ptr; auto res = strtod(str_ptr, &end); if (res == 0 && (errno != 0 || end == str_ptr)) { - if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { - throw std::logic_error("Locale set failed"); - } return false; } str_ptr = end; @@ -83,12 +68,8 @@ bool XMLElement::CastParamData(const XMLString & key, char * & str_ptr) param_it->second.param_value_ = castXMLString(str_ptr); break; default: - std::setlocale(LC_NUMERIC, prev_locale_ptr); return false; } - if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { - throw std::logic_error("Locale set failed"); - } } return ret_val; } diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp index 38fdbaff..59023102 100644 --- a/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -23,11 +23,6 @@ namespace xml int XMLParam::PrintParam(char * & buffer_it, int & size_left) { int idx = 0; - char * prev_locale_ptr = prev_locale_; - prev_locale_ptr = std::setlocale(LC_NUMERIC, nullptr); - if (!std::setlocale(LC_NUMERIC, "C")) { - throw std::logic_error("Locale set failed"); - } switch (param_type_) { case XMLType::BOOL: idx = snprintf(buffer_it, size_left, "%u", GetParamValue()); @@ -49,9 +44,6 @@ int XMLParam::PrintParam(char * & buffer_it, int & size_left) throw std::logic_error("Parameter type not supported"); break; } - if (!std::setlocale(LC_NUMERIC, prev_locale_ptr)) { - throw std::logic_error("Locale set failed"); - } if (idx < 0 || idx > size_left) { throw std::range_error("Out of the buffers range"); } else { From 0a752dd070129a3ee940cb5f8661c35ef37d056b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kom=C3=A1romi=20S=C3=A1ndor?= <48948212+altex111@users.noreply.github.com> Date: Fri, 25 Aug 2023 14:21:41 +0200 Subject: [PATCH 88/97] fix --- kuka_rsi_hw_interface/src/rsi_command_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index 37002670..a7b1f296 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -96,7 +96,7 @@ bool RSICommandHandler::SetLocale() bool RSICommandHandle::ResetLocale() { - return std::setlocale(LC_NUMERIC, prev_locale_) + return std::setlocale(LC_NUMERIC, prev_locale_); } bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) From a4eff552c9812c898bde883d74d1dcfb242ad106 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 25 Aug 2023 15:38:46 +0200 Subject: [PATCH 89/97] fix #2 --- kuka_rsi_hw_interface/src/rsi_command_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp index a7b1f296..d4b03afc 100644 --- a/kuka_rsi_hw_interface/src/rsi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -94,9 +94,9 @@ bool RSICommandHandler::SetLocale() return std::setlocale(LC_NUMERIC, "C"); } -bool RSICommandHandle::ResetLocale() +bool RSICommandHandler::ResetLocale() { - return std::setlocale(LC_NUMERIC, prev_locale_); + return std::setlocale(LC_NUMERIC, prev_locale_.c_str()); } bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) From 32e89a79aa7ce02c37b25f85ac89283cb5afa29a Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Tue, 5 Sep 2023 11:53:15 +0200 Subject: [PATCH 90/97] add unit tests --- kuka_rsi_hw_interface/CMakeLists.txt | 3 ++ kuka_rsi_hw_interface/test/CMakeLists.txt | 27 ++++++++++++ .../test/include/decode_test.h | 25 +++++++++++ .../test/include/encode_test.h | 25 +++++++++++ .../test/src/decode_test.cpp | 28 +++++++++++++ .../test/src/encode_test.cpp | 41 +++++++++++++++++++ .../test/src/ut_test_runner.cpp | 21 ++++++++++ 7 files changed, 170 insertions(+) create mode 100644 kuka_rsi_hw_interface/test/CMakeLists.txt create mode 100644 kuka_rsi_hw_interface/test/include/decode_test.h create mode 100644 kuka_rsi_hw_interface/test/include/encode_test.h create mode 100644 kuka_rsi_hw_interface/test/src/decode_test.cpp create mode 100644 kuka_rsi_hw_interface/test/src/encode_test.cpp create mode 100644 kuka_rsi_hw_interface/test/src/ut_test_runner.cpp diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 31a1887f..e16cfb3a 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -76,6 +76,9 @@ if(BUILD_TESTING) ament_lint_cmake() ament_uncrustify(--language=C++) ament_xmllint(--exclude ros_rsi.rsi.xml) + + enable_testing() + add_subdirectory(test) endif() # # EXPORTS diff --git a/kuka_rsi_hw_interface/test/CMakeLists.txt b/kuka_rsi_hw_interface/test/CMakeLists.txt new file mode 100644 index 00000000..7e2079a7 --- /dev/null +++ b/kuka_rsi_hw_interface/test/CMakeLists.txt @@ -0,0 +1,27 @@ +find_package(Threads REQUIRED) + +set(sources + src/ut_test_runner.cpp + src/decode_test.cpp + src/encode_test.cpp +) + +add_executable(${PROJECT_NAME}-test ${sources}) +target_compile_features(${PROJECT_NAME}-test PUBLIC cxx_std_17 PRIVATE cxx_std_17) + +target_link_libraries(${PROJECT_NAME}-test + ${SYSTEMD_LIBRARIES} + gtest + gtest_main + xml_handler + ${PROJECT_NAME} + ${CMAKE_THREAD_LIBS_INIT} # it should be last as lpthread should be last in linking invocation... +) + +target_include_directories(${PROJECT_NAME}-test PRIVATE + $ + $ + $ +) + +add_test(NAME ${PROJECT_NAME}-test COMMAND ${PROJECT_NAME}-test) \ No newline at end of file diff --git a/kuka_rsi_hw_interface/test/include/decode_test.h b/kuka_rsi_hw_interface/test/include/decode_test.h new file mode 100644 index 00000000..8e8dc43a --- /dev/null +++ b/kuka_rsi_hw_interface/test/include/decode_test.h @@ -0,0 +1,25 @@ +// Copyright 2023 Áron Svastits +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +class DecodeTest : public ::testing::Test +{ +public: + DecodeTest() + { + } +}; diff --git a/kuka_rsi_hw_interface/test/include/encode_test.h b/kuka_rsi_hw_interface/test/include/encode_test.h new file mode 100644 index 00000000..c7b56aeb --- /dev/null +++ b/kuka_rsi_hw_interface/test/include/encode_test.h @@ -0,0 +1,25 @@ +// Copyright 2023 Áron Svastits +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +class EncodeTest : public ::testing::Test +{ +public: + EncodeTest() + { + } +}; diff --git a/kuka_rsi_hw_interface/test/src/decode_test.cpp b/kuka_rsi_hw_interface/test/src/decode_test.cpp new file mode 100644 index 00000000..ce7c952e --- /dev/null +++ b/kuka_rsi_hw_interface/test/src/decode_test.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 Áron Svastits +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "decode_test.h" // NOLINT + +#include "kuka_rsi_hw_interface/rsi_command_handler.hpp" + +TEST_F(DecodeTest, DecodeSuccessful) { + kuka_rsi_hw_interface::RSICommandHandler rsi_command_handler; + + char input_string[] = + "0"; + ASSERT_TRUE(rsi_command_handler.Decode(input_string, 1024)); +} diff --git a/kuka_rsi_hw_interface/test/src/encode_test.cpp b/kuka_rsi_hw_interface/test/src/encode_test.cpp new file mode 100644 index 00000000..665595aa --- /dev/null +++ b/kuka_rsi_hw_interface/test/src/encode_test.cpp @@ -0,0 +1,41 @@ +// Copyright 2023 Áron Svastits +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "encode_test.h" // NOLINT + +#include "kuka_rsi_hw_interface/rsi_command_handler.hpp" + +TEST_F(EncodeTest, EncodeSuccessful) { + constexpr size_t UDP_BUFFER_SIZE = 1024; + kuka_rsi_hw_interface::RSICommandHandler rsi_command_handler; + char out_buffer_[UDP_BUFFER_SIZE] = {0}; + + + std::vector cmd_pos {0, 1, 2, 3, 4, 5}; + bool stop_flag = false; + int64_t ipoc = 100; + + // Initial command pos data + rsi_command_handler.SetCommandParam("AK", "A1", cmd_pos[0]); + rsi_command_handler.SetCommandParam("AK", "A2", cmd_pos[1]); + rsi_command_handler.SetCommandParam("AK", "A3", cmd_pos[2]); + rsi_command_handler.SetCommandParam("AK", "A4", cmd_pos[3]); + rsi_command_handler.SetCommandParam("AK", "A5", cmd_pos[4]); + rsi_command_handler.SetCommandParam("AK", "A6", cmd_pos[5]); + rsi_command_handler.SetCommandParam("Stop", "Stop", stop_flag); + rsi_command_handler.SetCommandParam("IPOC", "IPOC", static_cast(ipoc)); + + auto out_buffer_it = out_buffer_; + ASSERT_TRUE(rsi_command_handler.Encode(out_buffer_it, 1024)); +} diff --git a/kuka_rsi_hw_interface/test/src/ut_test_runner.cpp b/kuka_rsi_hw_interface/test/src/ut_test_runner.cpp new file mode 100644 index 00000000..728bf991 --- /dev/null +++ b/kuka_rsi_hw_interface/test/src/ut_test_runner.cpp @@ -0,0 +1,21 @@ +// Copyright 2023 Áron Svastits +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 1d6a3cc6e69d25ddba6903c85e54c0adf233106e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 7 Sep 2023 15:52:10 +0200 Subject: [PATCH 91/97] Test cases with pugixml --- kuka_rsi_hw_interface/CMakeLists.txt | 4 +- .../pugi_command_handler.hpp | 67 +++++++++ .../src/pugi_command_handler.cpp | 52 +++++++ .../test/include/decode_test.h | 8 + .../test/include/encode_test.h | 8 + .../test/src/decode_test.cpp | 37 +++++ .../test/src/encode_test.cpp | 137 +++++++++++++++++- 7 files changed, 304 insertions(+), 9 deletions(-) create mode 100644 kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp create mode 100644 kuka_rsi_hw_interface/src/pugi_command_handler.cpp diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index e16cfb3a..b254a78d 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(hardware_interface REQUIRED) find_package(controller_manager_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(angles REQUIRED) +find_package(pugixml REQUIRED) include_directories(include) @@ -36,9 +37,10 @@ add_library(xml_handler SHARED add_library(${PROJECT_NAME} SHARED src/kuka_hardware_interface.cpp src/rsi_command_handler.cpp + src/pugi_command_handler.cpp ) -target_link_libraries(${PROJECT_NAME} xml_handler) +target_link_libraries(${PROJECT_NAME} xml_handler pugixml) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp new file mode 100644 index 00000000..0fc6e1ca --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp @@ -0,0 +1,67 @@ +// Copyright 2023 Komaromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef KUKA_RSI_HW_INTERFACE__PUGI_COMMAND_HANDLER_HPP_ +#define KUKA_RSI_HW_INTERFACE__PUGI_COMMAND_HANDLER_HPP_ + +#include +#include + +namespace kuka_rsi_hw_interface +{ +struct xml_memory_writer : pugi::xml_writer +{ + char * buffer_; + size_t capacity_; + size_t result_; + + xml_memory_writer() + : buffer_(0), capacity_(0), result_(0) {} + xml_memory_writer(char * buffer, size_t capacity) + : buffer_(buffer), capacity_(capacity), result_(0) {} + + size_t written_size() const + { + return result_ < capacity_ ? result_ : capacity_; + } + + virtual void write(const void * data, size_t size) + { + if (result_ < capacity_) { + size_t chunk = (capacity_ - result_ < size) ? capacity_ - result_ : size; + memcpy(buffer_ + result_, data, chunk); + } + result_ += size; + } +}; + +class PugiCommandHandler +{ +private: + char * state_buffer; + +public: + pugi::xml_document state_doc; + pugi::xml_document command_doc; + + PugiCommandHandler(const size_t buffer_size); + ~PugiCommandHandler(); + + bool Decode(const char * buffer, const size_t buffer_size); + char * Encode(char * buffer, const size_t buffer_size); +}; +} // namespace kuka_rsi_hw_interface + + +#endif // KUKA_RSI_HW_INTERFACE__PUGI_COMMAND_HANDLER_HPP_ diff --git a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp new file mode 100644 index 00000000..ea994beb --- /dev/null +++ b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp @@ -0,0 +1,52 @@ +// Copyright 2023 Komaromi Sándor +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include "kuka_rsi_hw_interface/pugi_command_handler.hpp" + +namespace kuka_rsi_hw_interface +{ +PugiCommandHandler::PugiCommandHandler(const size_t buffer_size) +{ + state_buffer = new char[buffer_size]; +} + +PugiCommandHandler::~PugiCommandHandler() +{ + delete[] state_buffer; +} + +bool PugiCommandHandler::Decode(const char * buffer, const size_t buffer_size) +{ + std::memcpy(state_buffer, buffer, buffer_size); + return state_doc.load_buffer_inplace(state_buffer, buffer_size); +} + +char * PugiCommandHandler::Encode(char * buffer, const size_t buffer_size) +{ + if (buffer_size == 0) { + return nullptr; + } + + // leave one character for null terminator + xml_memory_writer writer(buffer, buffer_size - 1); + + command_doc.save(writer, "\t", pugi::format_raw | pugi::format_no_declaration); + + // null terminate + buffer[writer.written_size()] = 0; + + return buffer; +} +} // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/test/include/decode_test.h b/kuka_rsi_hw_interface/test/include/decode_test.h index 8e8dc43a..4f7ab5c7 100644 --- a/kuka_rsi_hw_interface/test/include/decode_test.h +++ b/kuka_rsi_hw_interface/test/include/decode_test.h @@ -23,3 +23,11 @@ class DecodeTest : public ::testing::Test { } }; + +class PugiDecodeTest : public ::testing::Test +{ +public: + PugiDecodeTest() + { + } +}; diff --git a/kuka_rsi_hw_interface/test/include/encode_test.h b/kuka_rsi_hw_interface/test/include/encode_test.h index c7b56aeb..e1cdbdcf 100644 --- a/kuka_rsi_hw_interface/test/include/encode_test.h +++ b/kuka_rsi_hw_interface/test/include/encode_test.h @@ -23,3 +23,11 @@ class EncodeTest : public ::testing::Test { } }; + +class PugiEncodeTest : public ::testing::Test +{ +public: + PugiEncodeTest() + { + } +}; diff --git a/kuka_rsi_hw_interface/test/src/decode_test.cpp b/kuka_rsi_hw_interface/test/src/decode_test.cpp index ce7c952e..a707b4d2 100644 --- a/kuka_rsi_hw_interface/test/src/decode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/decode_test.cpp @@ -15,6 +15,7 @@ #include "decode_test.h" // NOLINT #include "kuka_rsi_hw_interface/rsi_command_handler.hpp" +#include "kuka_rsi_hw_interface/pugi_command_handler.hpp" TEST_F(DecodeTest, DecodeSuccessful) { kuka_rsi_hw_interface::RSICommandHandler rsi_command_handler; @@ -26,3 +27,39 @@ TEST_F(DecodeTest, DecodeSuccessful) { " A3=\"90.0\" A4=\"0.0\" A5=\"90.0\" A6=\"0.0\" />0"; ASSERT_TRUE(rsi_command_handler.Decode(input_string, 1024)); } +TEST_F(DecodeTest, DecodeParamValid) { + kuka_rsi_hw_interface::RSICommandHandler rsi_command_handler; + + char input_string[] = + "0"; + ASSERT_TRUE(rsi_command_handler.Decode(input_string, 1024)); + ASSERT_TRUE(rsi_command_handler.GetState().GetElement("AIPos")->GetParam("A5") == 90.0); +} + +TEST_F(PugiDecodeTest, DecodeSuccessful) { + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); + + char input_string[] = + "0"; + ASSERT_TRUE(pugi_command_handler.Decode(input_string, 1024)); +} +TEST_F(PugiDecodeTest, DecodeParamValid) { + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); + + char input_string[] = + "0"; + ASSERT_TRUE(pugi_command_handler.Decode(input_string, 1024)); + ASSERT_TRUE( + pugi_command_handler.state_doc.child("Rob").child("AIPos").attribute( + "A5").as_double() == 90.0); + ASSERT_TRUE(pugi_command_handler.state_doc.child("Rob").child("IPOC").text().as_llong() == 0); +} diff --git a/kuka_rsi_hw_interface/test/src/encode_test.cpp b/kuka_rsi_hw_interface/test/src/encode_test.cpp index 665595aa..401cd184 100644 --- a/kuka_rsi_hw_interface/test/src/encode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/encode_test.cpp @@ -15,11 +15,13 @@ #include "encode_test.h" // NOLINT #include "kuka_rsi_hw_interface/rsi_command_handler.hpp" +#include "kuka_rsi_hw_interface/pugi_command_handler.hpp" TEST_F(EncodeTest, EncodeSuccessful) { constexpr size_t UDP_BUFFER_SIZE = 1024; kuka_rsi_hw_interface::RSICommandHandler rsi_command_handler; char out_buffer_[UDP_BUFFER_SIZE] = {0}; + bool ret_val = true; std::vector cmd_pos {0, 1, 2, 3, 4, 5}; @@ -27,15 +29,134 @@ TEST_F(EncodeTest, EncodeSuccessful) { int64_t ipoc = 100; // Initial command pos data - rsi_command_handler.SetCommandParam("AK", "A1", cmd_pos[0]); - rsi_command_handler.SetCommandParam("AK", "A2", cmd_pos[1]); - rsi_command_handler.SetCommandParam("AK", "A3", cmd_pos[2]); - rsi_command_handler.SetCommandParam("AK", "A4", cmd_pos[3]); - rsi_command_handler.SetCommandParam("AK", "A5", cmd_pos[4]); - rsi_command_handler.SetCommandParam("AK", "A6", cmd_pos[5]); - rsi_command_handler.SetCommandParam("Stop", "Stop", stop_flag); - rsi_command_handler.SetCommandParam("IPOC", "IPOC", static_cast(ipoc)); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A1", cmd_pos[0]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A2", cmd_pos[1]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A3", cmd_pos[2]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A4", cmd_pos[3]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A5", cmd_pos[4]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A6", cmd_pos[5]); + ret_val &= rsi_command_handler.SetCommandParam("Stop", "Stop", stop_flag); + ret_val &= + rsi_command_handler.SetCommandParam("IPOC", "IPOC", static_cast(ipoc)); auto out_buffer_it = out_buffer_; + ASSERT_TRUE(ret_val); ASSERT_TRUE(rsi_command_handler.Encode(out_buffer_it, 1024)); } +TEST_F(EncodeTest, EncodedWell) { + constexpr size_t UDP_BUFFER_SIZE = 1024; + kuka_rsi_hw_interface::RSICommandHandler rsi_command_handler; + char out_buffer_[UDP_BUFFER_SIZE] = {0}; + bool ret_val = true; + + + std::vector cmd_pos {0, 1, 2, 3, 4, 5}; + bool stop_flag = false; + int64_t ipoc = 100; + + // Initial command pos data + ret_val &= rsi_command_handler.SetCommandParam("AK", "A1", cmd_pos[0]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A2", cmd_pos[1]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A3", cmd_pos[2]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A4", cmd_pos[3]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A5", cmd_pos[4]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A6", cmd_pos[5]); + ret_val &= rsi_command_handler.SetCommandParam("Stop", "Stop", stop_flag); + ret_val &= + rsi_command_handler.SetCommandParam("IPOC", "IPOC", static_cast(ipoc)); + + auto out_buffer_it = out_buffer_; + ASSERT_TRUE(ret_val); + ASSERT_TRUE(rsi_command_handler.Encode(out_buffer_it, 1024)); + ASSERT_TRUE( + strcmp( + out_buffer_, + "0100") == 0 + ); +} + +TEST_F(PugiEncodeTest, EncodeSuccessful) { + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); + char out_buffer_[1024] = {0}; + bool ret_val = true; + + std::vector cmd_pos {0, 1, 2, 3, 4, 5}; + bool stop_flag = false; + int64_t ipoc = 100; + + // Add structure + ret_val &= + pugi_command_handler.command_doc.append_child("Sen").append_attribute("Type").set_value( + "KROSHU"); + pugi::xml_node Sen = pugi_command_handler.command_doc.child("Sen"); + Sen.append_child("AK"); + pugi::xml_node AK = Sen.child("AK"); + AK.append_attribute("A1"); + AK.append_attribute("A2"); + AK.append_attribute("A3"); + AK.append_attribute("A4"); + AK.append_attribute("A5"); + AK.append_attribute("A6"); + Sen.append_child("Stop"); + Sen.append_child("IPOC"); + + // Fill Data + // This Should be real time + auto cmd_pos_it = cmd_pos.begin(); + for (auto it = AK.attributes_begin(); it != AK.attributes_end() && cmd_pos_it != cmd_pos.end(); + ++it) + { + ret_val &= it->set_value(&cmd_pos_it); + ++cmd_pos_it; + } + ret_val &= Sen.child("Stop").text().set(stop_flag); + ret_val &= Sen.child("IPOC").text().set(ipoc); + + ASSERT_TRUE(ret_val); + ASSERT_TRUE((pugi_command_handler.Encode(out_buffer_, 1024) != nullptr)); +} +TEST_F(PugiEncodeTest, EncodedWell) { + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); + char out_buffer_[1024] = {0}; + bool ret_val = true; + + std::vector cmd_pos {0, 1, 2, 3, 4, 5}; + bool stop_flag = false; + int64_t ipoc = 100; + + // Add structure + ret_val &= + pugi_command_handler.command_doc.append_child("Sen").append_attribute("Type").set_value( + "KROSHU"); + pugi::xml_node Sen = pugi_command_handler.command_doc.child("Sen"); + Sen.append_child("AK"); + pugi::xml_node AK = Sen.child("AK"); + AK.append_attribute("A1"); + AK.append_attribute("A2"); + AK.append_attribute("A3"); + AK.append_attribute("A4"); + AK.append_attribute("A5"); + AK.append_attribute("A6"); + Sen.append_child("Stop"); + Sen.append_child("IPOC"); + + // Fill Data + // This Should be real time + auto cmd_pos_it = cmd_pos.begin(); + for (auto it = AK.attributes_begin(); it != AK.attributes_end() && cmd_pos_it != cmd_pos.end(); + ++it) + { + ret_val &= it->set_value(&cmd_pos_it); + ++cmd_pos_it; + } + ret_val &= Sen.child("Stop").text().set(stop_flag); + ret_val &= Sen.child("IPOC").text().set(ipoc); + + ASSERT_TRUE(ret_val); + ASSERT_TRUE((pugi_command_handler.Encode(out_buffer_, 1024) != nullptr)); + ASSERT_TRUE( + strcmp( + out_buffer_, + "0100") == 0 + ); +} From a32ddccf2b21c7553df17ff8043184c5b470568a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 8 Sep 2023 15:51:16 +0200 Subject: [PATCH 92/97] further test cases --- kuka_rsi_hw_interface/test/CMakeLists.txt | 20 +++++-- .../test/src/decode_test.cpp | 56 +++++++++++++++++++ .../test/src/encode_test.cpp | 18 +++--- 3 files changed, 79 insertions(+), 15 deletions(-) diff --git a/kuka_rsi_hw_interface/test/CMakeLists.txt b/kuka_rsi_hw_interface/test/CMakeLists.txt index 7e2079a7..a685c3fa 100644 --- a/kuka_rsi_hw_interface/test/CMakeLists.txt +++ b/kuka_rsi_hw_interface/test/CMakeLists.txt @@ -6,16 +6,24 @@ set(sources src/encode_test.cpp ) +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +set(CMAKE_CXX_FLAGS "-Wall -Wextra") +set(CMAKE_CXX_FLAGS_DEBUG "-g") +set(CMAKE_CXX_FLAGS_RELEASE "-O3") + add_executable(${PROJECT_NAME}-test ${sources}) target_compile_features(${PROJECT_NAME}-test PUBLIC cxx_std_17 PRIVATE cxx_std_17) target_link_libraries(${PROJECT_NAME}-test - ${SYSTEMD_LIBRARIES} - gtest - gtest_main - xml_handler - ${PROJECT_NAME} - ${CMAKE_THREAD_LIBS_INIT} # it should be last as lpthread should be last in linking invocation... + ${SYSTEMD_LIBRARIES} + gtest + gtest_main + xml_handler + ${PROJECT_NAME} + ${CMAKE_THREAD_LIBS_INIT} # it should be last as lpthread should be last in linking invocation... ) target_include_directories(${PROJECT_NAME}-test PRIVATE diff --git a/kuka_rsi_hw_interface/test/src/decode_test.cpp b/kuka_rsi_hw_interface/test/src/decode_test.cpp index a707b4d2..ecd308b3 100644 --- a/kuka_rsi_hw_interface/test/src/decode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/decode_test.cpp @@ -14,6 +14,9 @@ #include "decode_test.h" // NOLINT +#include +#include + #include "kuka_rsi_hw_interface/rsi_command_handler.hpp" #include "kuka_rsi_hw_interface/pugi_command_handler.hpp" @@ -38,6 +41,33 @@ TEST_F(DecodeTest, DecodeParamValid) { ASSERT_TRUE(rsi_command_handler.Decode(input_string, 1024)); ASSERT_TRUE(rsi_command_handler.GetState().GetElement("AIPos")->GetParam("A5") == 90.0); } +TEST_F(DecodeTest, DecodeTimeMeasurement) { + kuka_rsi_hw_interface::RSICommandHandler rsi_command_handler; + + char input_string[] = + "0"; + bool ret_val = true; + std::ofstream timer_data; + + timer_data.open("DecodeTest_DecodeTimeMeasurement"); + + struct sched_param param; + param.sched_priority = 98; + ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1); + + for (size_t i = 0; i < 1000; i++) { + auto start = std::chrono::high_resolution_clock::now(); + ret_val &= rsi_command_handler.Decode(input_string, 1024); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); + timer_data << duration.count() << std::endl; + } + ASSERT_TRUE(ret_val); +} + TEST_F(PugiDecodeTest, DecodeSuccessful) { kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); @@ -63,3 +93,29 @@ TEST_F(PugiDecodeTest, DecodeParamValid) { "A5").as_double() == 90.0); ASSERT_TRUE(pugi_command_handler.state_doc.child("Rob").child("IPOC").text().as_llong() == 0); } +TEST_F(PugiDecodeTest, DecodeTimeMeasurement) { + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); + + char input_string[] = + "0"; + bool ret_val = true; + std::ofstream timer_data; + + timer_data.open("DecodeTest_DecodeTimeMeasurement"); + + struct sched_param param; + param.sched_priority = 98; + ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1); + + for (size_t i = 0; i < 1000; i++) { + auto start = std::chrono::high_resolution_clock::now(); + ret_val &= pugi_command_handler.Decode(input_string, 1024); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); + timer_data << duration.count() << std::endl; + } + ASSERT_TRUE(ret_val); +} diff --git a/kuka_rsi_hw_interface/test/src/encode_test.cpp b/kuka_rsi_hw_interface/test/src/encode_test.cpp index 401cd184..3bc083e5 100644 --- a/kuka_rsi_hw_interface/test/src/encode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/encode_test.cpp @@ -14,6 +14,8 @@ #include "encode_test.h" // NOLINT +#include + #include "kuka_rsi_hw_interface/rsi_command_handler.hpp" #include "kuka_rsi_hw_interface/pugi_command_handler.hpp" @@ -71,7 +73,7 @@ TEST_F(EncodeTest, EncodedWell) { ASSERT_TRUE( strcmp( out_buffer_, - "0100") == 0 + "0100") == 0 ); } @@ -142,14 +144,12 @@ TEST_F(PugiEncodeTest, EncodedWell) { // Fill Data // This Should be real time - auto cmd_pos_it = cmd_pos.begin(); - for (auto it = AK.attributes_begin(); it != AK.attributes_end() && cmd_pos_it != cmd_pos.end(); - ++it) - { - ret_val &= it->set_value(&cmd_pos_it); - ++cmd_pos_it; + size_t i = 0; + for (auto it = AK.attributes_begin(); it != AK.attributes_end(); ++it) { + ret_val &= it->set_value(cmd_pos[i]); + ++i; } - ret_val &= Sen.child("Stop").text().set(stop_flag); + ret_val &= Sen.child("Stop").text().set(static_cast(stop_flag)); ret_val &= Sen.child("IPOC").text().set(ipoc); ASSERT_TRUE(ret_val); @@ -157,6 +157,6 @@ TEST_F(PugiEncodeTest, EncodedWell) { ASSERT_TRUE( strcmp( out_buffer_, - "0100") == 0 + "0100") == 0 ); } From 6e4e2e626de4d1426708e127b09c802cc86e1432 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Fri, 8 Sep 2023 18:53:37 +0200 Subject: [PATCH 93/97] working on custom mem alloc --- .../pugi_command_handler.hpp | 16 +++++++++++++--- .../src/pugi_command_handler.cpp | 19 +++++++++++++++---- 2 files changed, 28 insertions(+), 7 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp index 0fc6e1ca..dc0c6515 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp @@ -17,6 +17,9 @@ #include #include +#include +#include +#include namespace kuka_rsi_hw_interface { @@ -46,11 +49,9 @@ struct xml_memory_writer : pugi::xml_writer } }; + class PugiCommandHandler { -private: - char * state_buffer; - public: pugi::xml_document state_doc; pugi::xml_document command_doc; @@ -60,6 +61,15 @@ class PugiCommandHandler bool Decode(const char * buffer, const size_t buffer_size); char * Encode(char * buffer, const size_t buffer_size); + +private: + static std::pmr::synchronized_pool_resource memory_pull_; + static std::pmr::map memory_pulled_sizes_; + + char * state_buffer_; + + static void * custom_allocate(size_t size); + static void custom_deallocate(void * ptr); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp index ea994beb..975f8769 100644 --- a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp @@ -18,19 +18,21 @@ namespace kuka_rsi_hw_interface { PugiCommandHandler::PugiCommandHandler(const size_t buffer_size) +: memory_pull_() { - state_buffer = new char[buffer_size]; + pugi::set_memory_management_functions(custom_allocate, custom_deallocate); + state_buffer_ = new char[buffer_size]; } PugiCommandHandler::~PugiCommandHandler() { - delete[] state_buffer; + delete[] state_buffer_; } bool PugiCommandHandler::Decode(const char * buffer, const size_t buffer_size) { - std::memcpy(state_buffer, buffer, buffer_size); - return state_doc.load_buffer_inplace(state_buffer, buffer_size); + std::memcpy(state_buffer_, buffer, buffer_size); + return state_doc.load_buffer_inplace(state_buffer_, buffer_size); } char * PugiCommandHandler::Encode(char * buffer, const size_t buffer_size) @@ -49,4 +51,13 @@ char * PugiCommandHandler::Encode(char * buffer, const size_t buffer_size) return buffer; } + +void * PugiCommandHandler::custom_allocate(size_t size) +{ + return memory_pull_.allocate(size); +} +void PugiCommandHandler::custom_deallocate(void * ptr) +{ + return memory_pull_.deallocate(ptr, sizeof(ptr)); +} } // namespace kuka_rsi_hw_interface From aa66dc8e6e21cad8e8dc994f341fea890ad81afe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 14 Sep 2023 13:51:24 +0200 Subject: [PATCH 94/97] more test cases --- .../pugi_command_handler.hpp | 24 +++-- .../src/pugi_command_handler.cpp | 41 +++++-- .../test/src/decode_test.cpp | 44 ++++++-- .../test/src/encode_test.cpp | 100 +++++++++++++++++- 4 files changed, 181 insertions(+), 28 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp index dc0c6515..2a015c31 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp @@ -23,6 +23,10 @@ namespace kuka_rsi_hw_interface { + +void * custom_allocate(size_t size); +void custom_deallocate(void * ptr); + struct xml_memory_writer : pugi::xml_writer { char * buffer_; @@ -49,6 +53,20 @@ struct xml_memory_writer : pugi::xml_writer } }; +// struct MemoryManager +// { +// MemoryManager() = default; + +// static std::pmr::monotonic_buffer_resource monotonic_; +// static std::pmr::synchronized_pool_resource memory_pool_; +// static std::pmr::map memory_pool_sizes_; + +// // std::pmr::set_default_resource(std::pmr::null_memory_resource()); +// // std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); +// // std::pmr::synchronized_pool_resource memory_pull_(&monotonic_); + +// // std::pmr::map memory_pool_sizes_{ & memory_pull_}; +// }; class PugiCommandHandler { @@ -63,13 +81,7 @@ class PugiCommandHandler char * Encode(char * buffer, const size_t buffer_size); private: - static std::pmr::synchronized_pool_resource memory_pull_; - static std::pmr::map memory_pulled_sizes_; - char * state_buffer_; - - static void * custom_allocate(size_t size); - static void custom_deallocate(void * ptr); }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp index 975f8769..027c8cec 100644 --- a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp @@ -18,9 +18,7 @@ namespace kuka_rsi_hw_interface { PugiCommandHandler::PugiCommandHandler(const size_t buffer_size) -: memory_pull_() { - pugi::set_memory_management_functions(custom_allocate, custom_deallocate); state_buffer_ = new char[buffer_size]; } @@ -52,12 +50,35 @@ char * PugiCommandHandler::Encode(char * buffer, const size_t buffer_size) return buffer; } -void * PugiCommandHandler::custom_allocate(size_t size) -{ - return memory_pull_.allocate(size); -} -void PugiCommandHandler::custom_deallocate(void * ptr) -{ - return memory_pull_.deallocate(ptr, sizeof(ptr)); -} +// void * custom_allocate(size_t size) +// { +// auto ptr = MemoryManager::memory_pool_.allocate(size); +// MemoryManager::memory_pool_sizes_.emplace(std::make_pair(ptr, size)); +// return ptr; +// } +// void custom_deallocate(void * ptr) +// { +// auto memory_pool_it = MemoryManager::memory_pool_sizes_.find(ptr); +// if (memory_pool_it != MemoryManager::memory_pool_sizes_.end()) { +// // pointer not found +// } +// MemoryManager::memory_pool_.deallocate(ptr, memory_pool_it->second); +// } + +// void MemoryManager::memory_manager_init(size_t memory_size) +// { +// std::pmr::set_default_resource(std::pmr::null_memory_resource()); +// std::pmr::monotonic_buffer_resource monotonic_(memory_size, std::pmr::null_memory_resource()); +// std::pmr::synchronized_pool_resource memory_pull_(&monotonic_); + +// std::pmr::map memory_pool_sizes_{&memory_pull_}; +// } +// MemoryManager::MemoryManager() +// { +// std::pmr::set_default_resource(std::pmr::null_memory_resource()); +// std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); +// std::pmr::synchronized_pool_resource memory_pull_(&monotonic_); + +// std::pmr::map memory_pool_sizes_{&memory_pull_}; +// } } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/test/src/decode_test.cpp b/kuka_rsi_hw_interface/test/src/decode_test.cpp index ecd308b3..71793113 100644 --- a/kuka_rsi_hw_interface/test/src/decode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/decode_test.cpp @@ -52,18 +52,18 @@ TEST_F(DecodeTest, DecodeTimeMeasurement) { bool ret_val = true; std::ofstream timer_data; - timer_data.open("DecodeTest_DecodeTimeMeasurement"); + timer_data.open("DecodeTest_DecodeTimeMeasurement.txt"); struct sched_param param; param.sched_priority = 98; - ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1); + ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) != -1); - for (size_t i = 0; i < 1000; i++) { + for (size_t i = 0; i < 4000; i++) { auto start = std::chrono::high_resolution_clock::now(); ret_val &= rsi_command_handler.Decode(input_string, 1024); auto stop = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop - start); - timer_data << duration.count() << std::endl; + auto duration = std::chrono::duration_cast(stop - start); + timer_data << i << " " << duration.count() << std::endl; } ASSERT_TRUE(ret_val); } @@ -94,6 +94,30 @@ TEST_F(PugiDecodeTest, DecodeParamValid) { ASSERT_TRUE(pugi_command_handler.state_doc.child("Rob").child("IPOC").text().as_llong() == 0); } TEST_F(PugiDecodeTest, DecodeTimeMeasurement) { + // Memory manager + // std::pmr::set_default_resource(std::pmr::null_memory_resource()); + // std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); + // std::pmr::synchronized_pool_resource kuka_rsi_hw_interface::MemoryManager::memory_pull_(&monotonic_); + + // std::pmr::map memory_pool_sizes_{&memory_pull_}; + + // pugi::set_memory_management_functions( + // kuka_rsi_hw_interface::custom_allocate, + // kuka_rsi_hw_interface::custom_deallocate); + // pugi::set_memory_management_functions( + // [&](size_t size) { + // auto ptr = memory_pool.allocate(size); + // memory_pool_sizes.emplace(std::make_pair(ptr, size)); + // return ptr; + // }, + // [&](void * ptr) { + // auto memory_pool_it = memory_pool_sizes.find(ptr); + // if (memory_pool_it != memory_pool_sizes.end()) { + // // pointer not found + // } + // memory_pool.deallocate(ptr, memory_pool_it->second); + // }); + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); char input_string[] = @@ -104,18 +128,18 @@ TEST_F(PugiDecodeTest, DecodeTimeMeasurement) { bool ret_val = true; std::ofstream timer_data; - timer_data.open("DecodeTest_DecodeTimeMeasurement"); + timer_data.open("PugiDecodeTest_DecodeTimeMeasurement.txt"); struct sched_param param; param.sched_priority = 98; - ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1); + ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) != -1); - for (size_t i = 0; i < 1000; i++) { + for (size_t i = 0; i < 4000; i++) { auto start = std::chrono::high_resolution_clock::now(); ret_val &= pugi_command_handler.Decode(input_string, 1024); auto stop = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop - start); - timer_data << duration.count() << std::endl; + auto duration = std::chrono::duration_cast(stop - start); + timer_data << i << " " << duration.count() << std::endl; } ASSERT_TRUE(ret_val); } diff --git a/kuka_rsi_hw_interface/test/src/encode_test.cpp b/kuka_rsi_hw_interface/test/src/encode_test.cpp index 3bc083e5..fcb7a3be 100644 --- a/kuka_rsi_hw_interface/test/src/encode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/encode_test.cpp @@ -15,6 +15,7 @@ #include "encode_test.h" // NOLINT #include +#include #include "kuka_rsi_hw_interface/rsi_command_handler.hpp" #include "kuka_rsi_hw_interface/pugi_command_handler.hpp" @@ -43,7 +44,7 @@ TEST_F(EncodeTest, EncodeSuccessful) { auto out_buffer_it = out_buffer_; ASSERT_TRUE(ret_val); - ASSERT_TRUE(rsi_command_handler.Encode(out_buffer_it, 1024)); + ASSERT_TRUE(rsi_command_handler.Encode(out_buffer_it, 1024) != -1); } TEST_F(EncodeTest, EncodedWell) { constexpr size_t UDP_BUFFER_SIZE = 1024; @@ -69,7 +70,7 @@ TEST_F(EncodeTest, EncodedWell) { auto out_buffer_it = out_buffer_; ASSERT_TRUE(ret_val); - ASSERT_TRUE(rsi_command_handler.Encode(out_buffer_it, 1024)); + ASSERT_TRUE(rsi_command_handler.Encode(out_buffer_it, 1024) != -1); ASSERT_TRUE( strcmp( out_buffer_, @@ -77,6 +78,48 @@ TEST_F(EncodeTest, EncodedWell) { ); } +TEST_F(EncodeTest, EncodeTimeMeasurement) { + constexpr size_t UDP_BUFFER_SIZE = 1024; + kuka_rsi_hw_interface::RSICommandHandler rsi_command_handler; + char out_buffer_[UDP_BUFFER_SIZE] = {0}; + bool ret_val = true; + + struct sched_param param; + param.sched_priority = 98; + ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) != -1); + + std::ofstream timer_data; + + timer_data.open("EncodeTest_EncodeTimeMeasurement.txt"); + + std::vector cmd_pos {0, 1, 2, 3, 4, 5}; + bool stop_flag = false; + int64_t ipoc = 100; + + // Initial command pos data + ret_val &= rsi_command_handler.SetCommandParam("AK", "A1", cmd_pos[0]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A2", cmd_pos[1]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A3", cmd_pos[2]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A4", cmd_pos[3]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A5", cmd_pos[4]); + ret_val &= rsi_command_handler.SetCommandParam("AK", "A6", cmd_pos[5]); + ret_val &= rsi_command_handler.SetCommandParam("Stop", "Stop", stop_flag); + ret_val &= + rsi_command_handler.SetCommandParam("IPOC", "IPOC", static_cast(ipoc)); + + auto out_buffer_it = out_buffer_; + + for (size_t i = 0; i < 4000; i++) { + auto start = std::chrono::high_resolution_clock::now(); + ret_val &= (rsi_command_handler.Encode(out_buffer_it, 1024) != -1); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); + timer_data << i << " " << duration.count() << std::endl; + } + + ASSERT_TRUE(ret_val); +} + TEST_F(PugiEncodeTest, EncodeSuccessful) { kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); char out_buffer_[1024] = {0}; @@ -160,3 +203,56 @@ TEST_F(PugiEncodeTest, EncodedWell) { "0100") == 0 ); } +TEST_F(PugiEncodeTest, EncodeTimeMeasurement) { + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); + char out_buffer_[1024] = {0}; + bool ret_val = true; + + struct sched_param param; + param.sched_priority = 98; + ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) != -1); + + std::ofstream timer_data; + + timer_data.open("PugiEncodeTest_EncodeTimeMeasurement.txt"); + + std::vector cmd_pos {0, 1, 2, 3, 4, 5}; + bool stop_flag = false; + int64_t ipoc = 100; + + // Add structure + ret_val &= + pugi_command_handler.command_doc.append_child("Sen").append_attribute("Type").set_value( + "KROSHU"); + pugi::xml_node Sen = pugi_command_handler.command_doc.child("Sen"); + Sen.append_child("AK"); + pugi::xml_node AK = Sen.child("AK"); + AK.append_attribute("A1"); + AK.append_attribute("A2"); + AK.append_attribute("A3"); + AK.append_attribute("A4"); + AK.append_attribute("A5"); + AK.append_attribute("A6"); + Sen.append_child("Stop"); + Sen.append_child("IPOC"); + + // Fill Data + // This Should be real time + size_t i = 0; + for (auto it = AK.attributes_begin(); it != AK.attributes_end(); ++it) { + ret_val &= it->set_value(cmd_pos[i]); + ++i; + } + ret_val &= Sen.child("Stop").text().set(static_cast(stop_flag)); + ret_val &= Sen.child("IPOC").text().set(ipoc); + + for (size_t i = 0; i < 4000; i++) { + auto start = std::chrono::high_resolution_clock::now(); + ret_val &= (pugi_command_handler.Encode(out_buffer_, 1024) != nullptr); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); + timer_data << i << " " << duration.count() << std::endl; + } + + ASSERT_TRUE(ret_val); +} From daaa42c2aef1772a526ed6315af21ac3f35e9b48 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kom=C3=A1romi=20S=C3=A1ndor?= <48948212+altex111@users.noreply.github.com> Date: Tue, 19 Sep 2023 12:03:20 +0200 Subject: [PATCH 95/97] pugi without heap allocation --- .../pugi_command_handler.hpp | 14 ++++ .../src/pugi_command_handler.cpp | 66 ++++++++++--------- .../test/src/decode_test.cpp | 9 ++- 3 files changed, 55 insertions(+), 34 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp index 2a015c31..8c74619c 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp @@ -68,6 +68,20 @@ struct xml_memory_writer : pugi::xml_writer // // std::pmr::map memory_pool_sizes_{ & memory_pull_}; // }; +MemoryManager * memory_manager_handler; + +class MemoryManager +{ +public: + MemoryManager(); + void * Allocate(size_t size); + void Deallocate(void * ptr); +private: + std::pmr::monotonic_buffer_resource monotonic_; + std::pmr::synchronized_pool_resource memory_pool_; + std::pmr::map memory_pool_sizes_; +} + class PugiCommandHandler { public: diff --git a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp index 027c8cec..a2cdf008 100644 --- a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp @@ -50,35 +50,39 @@ char * PugiCommandHandler::Encode(char * buffer, const size_t buffer_size) return buffer; } -// void * custom_allocate(size_t size) -// { -// auto ptr = MemoryManager::memory_pool_.allocate(size); -// MemoryManager::memory_pool_sizes_.emplace(std::make_pair(ptr, size)); -// return ptr; -// } -// void custom_deallocate(void * ptr) -// { -// auto memory_pool_it = MemoryManager::memory_pool_sizes_.find(ptr); -// if (memory_pool_it != MemoryManager::memory_pool_sizes_.end()) { -// // pointer not found -// } -// MemoryManager::memory_pool_.deallocate(ptr, memory_pool_it->second); -// } - -// void MemoryManager::memory_manager_init(size_t memory_size) -// { -// std::pmr::set_default_resource(std::pmr::null_memory_resource()); -// std::pmr::monotonic_buffer_resource monotonic_(memory_size, std::pmr::null_memory_resource()); -// std::pmr::synchronized_pool_resource memory_pull_(&monotonic_); - -// std::pmr::map memory_pool_sizes_{&memory_pull_}; -// } -// MemoryManager::MemoryManager() -// { -// std::pmr::set_default_resource(std::pmr::null_memory_resource()); -// std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); -// std::pmr::synchronized_pool_resource memory_pull_(&monotonic_); - -// std::pmr::map memory_pool_sizes_{&memory_pull_}; -// } + +void * custom_allocate(size_t size) +{ + return memory_manager_handler->Allocate(size); +} +void custom_deallocate(void * ptr) +{ + memory_manager_handler->Deallocate(ptr); +} + +MemoryManager::MemoryManager() +{ + std::pmr::set_default_resource(std::pmr::null_memory_resource()); + std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); + std::pmr::synchronized_pool_resource memory_pull_(&monotonic_); + + std::pmr::map memory_pool_sizes_{&memory_pull_}; +} + +void * MemoryManager::Allocate(size_t size) +{ + auto ptr = memory_pool_.allocate(size); + memory_pool_sizes_.emplace(std::make_pair(ptr, size)); + return ptr; +} + +void MemoryManager::Deallocate(void * ptr) + { + auto memory_pool_it = memory_pool_sizes_.find(ptr); + if (memory_pool_it != memory_pool_sizes_.end()) { + // pointer not found + } + memory_pool_.deallocate(ptr, memory_pool_it->second); + } + } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/test/src/decode_test.cpp b/kuka_rsi_hw_interface/test/src/decode_test.cpp index 71793113..5c266c11 100644 --- a/kuka_rsi_hw_interface/test/src/decode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/decode_test.cpp @@ -95,15 +95,18 @@ TEST_F(PugiDecodeTest, DecodeParamValid) { } TEST_F(PugiDecodeTest, DecodeTimeMeasurement) { // Memory manager + MemoryManager memory_manager; + memory_manager_handler = &memory_manager; + // std::pmr::set_default_resource(std::pmr::null_memory_resource()); // std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); // std::pmr::synchronized_pool_resource kuka_rsi_hw_interface::MemoryManager::memory_pull_(&monotonic_); // std::pmr::map memory_pool_sizes_{&memory_pull_}; - // pugi::set_memory_management_functions( - // kuka_rsi_hw_interface::custom_allocate, - // kuka_rsi_hw_interface::custom_deallocate); + pugi::set_memory_management_functions( + kuka_rsi_hw_interface::custom_allocate, + kuka_rsi_hw_interface::custom_deallocate); // pugi::set_memory_management_functions( // [&](size_t size) { // auto ptr = memory_pool.allocate(size); From 8da9c35f4cfddd6147a8a36150dbb85f2380da75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Thu, 21 Sep 2023 15:56:58 +0200 Subject: [PATCH 96/97] working pugi with pmr memory pool --- .../pugi_command_handler.hpp | 55 ++++++++---- .../src/pugi_command_handler.cpp | 32 ++++--- .../test/src/decode_test.cpp | 45 +++++----- .../test/src/encode_test.cpp | 90 +++++++++++++++++++ 4 files changed, 172 insertions(+), 50 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp index 8c74619c..e700675e 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include @@ -53,34 +55,53 @@ struct xml_memory_writer : pugi::xml_writer } }; -// struct MemoryManager -// { -// MemoryManager() = default; +class print_resource : public std::pmr::memory_resource +{ +public: + print_resource(std::string name, std::pmr::memory_resource * upstream) + : name_(name), upstream_(upstream) + { + assert(upstream); + } -// static std::pmr::monotonic_buffer_resource monotonic_; -// static std::pmr::synchronized_pool_resource memory_pool_; -// static std::pmr::map memory_pool_sizes_; +private: + std::string name_; + std::pmr::memory_resource * upstream_; -// // std::pmr::set_default_resource(std::pmr::null_memory_resource()); -// // std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); -// // std::pmr::synchronized_pool_resource memory_pull_(&monotonic_); + void * do_allocate(std::size_t bytes, std::size_t alignment) override + { + std::cout << "[" << name_ << " (alloc)] Size: " << bytes << " Alignment: " << alignment << + " ..." << std::endl; + auto result = upstream_->allocate(bytes, alignment); + std::cout << "[" << name_ << " (alloc)] ... Address: " << result << std::endl; + return result; + } -// // std::pmr::map memory_pool_sizes_{ & memory_pull_}; -// }; + void do_deallocate(void * ptr, std::size_t bytes, std::size_t alignment) override + { + std::cout << "[" << name_ << " (dealloc)] Address: " << ptr << "Size: " << bytes << + " Alignment: " << alignment << std::endl; + upstream_->deallocate(ptr, bytes, alignment); + } -MemoryManager * memory_manager_handler; + bool do_is_equal(const std::pmr::memory_resource & other) const noexcept override + { + return this == &other; + } +}; class MemoryManager { public: - MemoryManager(); + MemoryManager(std::pmr::memory_resource * upstream); void * Allocate(size_t size); void Deallocate(void * ptr); + private: - std::pmr::monotonic_buffer_resource monotonic_; - std::pmr::synchronized_pool_resource memory_pool_; + //std::pmr::polymorphic_allocator allocator_; + std::pmr::memory_resource * memory_pool_; std::pmr::map memory_pool_sizes_; -} +}; class PugiCommandHandler { @@ -97,6 +118,8 @@ class PugiCommandHandler private: char * state_buffer_; }; + +extern MemoryManager * memory_manager_handler; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp index a2cdf008..cff2c890 100644 --- a/kuka_rsi_hw_interface/src/pugi_command_handler.cpp +++ b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp @@ -17,6 +17,8 @@ namespace kuka_rsi_hw_interface { +MemoryManager * memory_manager_handler; + PugiCommandHandler::PugiCommandHandler(const size_t buffer_size) { state_buffer_ = new char[buffer_size]; @@ -53,36 +55,38 @@ char * PugiCommandHandler::Encode(char * buffer, const size_t buffer_size) void * custom_allocate(size_t size) { - return memory_manager_handler->Allocate(size); + return kuka_rsi_hw_interface::memory_manager_handler->Allocate(size); } void custom_deallocate(void * ptr) { - memory_manager_handler->Deallocate(ptr); + kuka_rsi_hw_interface::memory_manager_handler->Deallocate(ptr); } -MemoryManager::MemoryManager() +MemoryManager::MemoryManager(std::pmr::memory_resource * upstream) +: memory_pool_(upstream), memory_pool_sizes_(upstream) { - std::pmr::set_default_resource(std::pmr::null_memory_resource()); - std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); - std::pmr::synchronized_pool_resource memory_pull_(&monotonic_); - - std::pmr::map memory_pool_sizes_{&memory_pull_}; + // std::cout << "Memory Manager init" << std::endl; + // // std::pmr::set_default_resource(&memory_pool_); + // + // std::pmr::map memory_pool_sizes_{&save_size}; } void * MemoryManager::Allocate(size_t size) { - auto ptr = memory_pool_.allocate(size); + auto ptr = static_cast(memory_pool_->allocate(size)); memory_pool_sizes_.emplace(std::make_pair(ptr, size)); - return ptr; + return ptr; } void MemoryManager::Deallocate(void * ptr) - { +{ auto memory_pool_it = memory_pool_sizes_.find(ptr); - if (memory_pool_it != memory_pool_sizes_.end()) { + if (memory_pool_it == memory_pool_sizes_.end()) { // pointer not found } - memory_pool_.deallocate(ptr, memory_pool_it->second); - } + memory_pool_->deallocate(static_cast(ptr),memory_pool_it->second); + + memory_pool_sizes_.erase(memory_pool_it); +} } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/test/src/decode_test.cpp b/kuka_rsi_hw_interface/test/src/decode_test.cpp index 5c266c11..48080640 100644 --- a/kuka_rsi_hw_interface/test/src/decode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/decode_test.cpp @@ -95,31 +95,34 @@ TEST_F(PugiDecodeTest, DecodeParamValid) { } TEST_F(PugiDecodeTest, DecodeTimeMeasurement) { // Memory manager - MemoryManager memory_manager; - memory_manager_handler = &memory_manager; + // Set default resource + kuka_rsi_hw_interface::print_resource default_resource("Rogue PMR Allocation!", + std::pmr::null_memory_resource()); + std::pmr::set_default_resource(&default_resource); - // std::pmr::set_default_resource(std::pmr::null_memory_resource()); - // std::pmr::monotonic_buffer_resource monotonic_(50000, std::pmr::null_memory_resource()); - // std::pmr::synchronized_pool_resource kuka_rsi_hw_interface::MemoryManager::memory_pull_(&monotonic_); + // create memory resource + kuka_rsi_hw_interface::print_resource out_of_memory("Out of Memory", + std::pmr::null_memory_resource()); - // std::pmr::map memory_pool_sizes_{&memory_pull_}; + std::array buffer{}; + std::pmr::monotonic_buffer_resource underlying_bytes{buffer.data(), buffer.size(), + &out_of_memory}; + + //kuka_rsi_hw_interface::print_resource monotonic("Monotonic Array", &underlying_bytes); + + // Create pool resource + std::pmr::pool_options pool_options; + pool_options.largest_required_pool_block = 35000; + std::pmr::synchronized_pool_resource sync_pool{pool_options, &underlying_bytes}; + //kuka_rsi_hw_interface::print_resource memory_pool("Pool", &sync_pool); + + // Create memory handler + kuka_rsi_hw_interface::MemoryManager memory_manager(&sync_pool); + kuka_rsi_hw_interface::memory_manager_handler = &memory_manager; pugi::set_memory_management_functions( kuka_rsi_hw_interface::custom_allocate, kuka_rsi_hw_interface::custom_deallocate); - // pugi::set_memory_management_functions( - // [&](size_t size) { - // auto ptr = memory_pool.allocate(size); - // memory_pool_sizes.emplace(std::make_pair(ptr, size)); - // return ptr; - // }, - // [&](void * ptr) { - // auto memory_pool_it = memory_pool_sizes.find(ptr); - // if (memory_pool_it != memory_pool_sizes.end()) { - // // pointer not found - // } - // memory_pool.deallocate(ptr, memory_pool_it->second); - // }); kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); @@ -137,7 +140,9 @@ TEST_F(PugiDecodeTest, DecodeTimeMeasurement) { param.sched_priority = 98; ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) != -1); - for (size_t i = 0; i < 4000; i++) { + + for (size_t i = 0; i < 1000; i++) { + //std::cout << "[Debug] Decode started" << std::endl; auto start = std::chrono::high_resolution_clock::now(); ret_val &= pugi_command_handler.Decode(input_string, 1024); auto stop = std::chrono::high_resolution_clock::now(); diff --git a/kuka_rsi_hw_interface/test/src/encode_test.cpp b/kuka_rsi_hw_interface/test/src/encode_test.cpp index fcb7a3be..d058b1ea 100644 --- a/kuka_rsi_hw_interface/test/src/encode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/encode_test.cpp @@ -121,6 +121,36 @@ TEST_F(EncodeTest, EncodeTimeMeasurement) { } TEST_F(PugiEncodeTest, EncodeSuccessful) { + // Memory manager + // Set default resource + kuka_rsi_hw_interface::print_resource default_resource("Rogue PMR Allocation!", + std::pmr::null_memory_resource()); + std::pmr::set_default_resource(&default_resource); + + // create memory resource + kuka_rsi_hw_interface::print_resource out_of_memory("Out of Memory", + std::pmr::null_memory_resource()); + + std::array buffer{}; + std::pmr::monotonic_buffer_resource underlying_bytes{buffer.data(), buffer.size(), + &out_of_memory}; + + //kuka_rsi_hw_interface::print_resource monotonic("Monotonic Array", &underlying_bytes); + + // Create pool resource + std::pmr::pool_options pool_options; + pool_options.largest_required_pool_block = 35000; + std::pmr::synchronized_pool_resource sync_pool{pool_options, &underlying_bytes}; + //kuka_rsi_hw_interface::print_resource memory_pool("Pool", &sync_pool); + + // Create memory handler + kuka_rsi_hw_interface::MemoryManager memory_manager(&sync_pool); + kuka_rsi_hw_interface::memory_manager_handler = &memory_manager; + + pugi::set_memory_management_functions( + kuka_rsi_hw_interface::custom_allocate, + kuka_rsi_hw_interface::custom_deallocate); + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); char out_buffer_[1024] = {0}; bool ret_val = true; @@ -161,6 +191,36 @@ TEST_F(PugiEncodeTest, EncodeSuccessful) { ASSERT_TRUE((pugi_command_handler.Encode(out_buffer_, 1024) != nullptr)); } TEST_F(PugiEncodeTest, EncodedWell) { + // Memory manager + // Set default resource + kuka_rsi_hw_interface::print_resource default_resource("Rogue PMR Allocation!", + std::pmr::null_memory_resource()); + std::pmr::set_default_resource(&default_resource); + + // create memory resource + kuka_rsi_hw_interface::print_resource out_of_memory("Out of Memory", + std::pmr::null_memory_resource()); + + std::array buffer{}; + std::pmr::monotonic_buffer_resource underlying_bytes{buffer.data(), buffer.size(), + &out_of_memory}; + + //kuka_rsi_hw_interface::print_resource monotonic("Monotonic Array", &underlying_bytes); + + // Create pool resource + std::pmr::pool_options pool_options; + pool_options.largest_required_pool_block = 35000; + std::pmr::synchronized_pool_resource sync_pool{pool_options, &underlying_bytes}; + //kuka_rsi_hw_interface::print_resource memory_pool("Pool", &sync_pool); + + // Create memory handler + kuka_rsi_hw_interface::MemoryManager memory_manager(&sync_pool); + kuka_rsi_hw_interface::memory_manager_handler = &memory_manager; + + pugi::set_memory_management_functions( + kuka_rsi_hw_interface::custom_allocate, + kuka_rsi_hw_interface::custom_deallocate); + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); char out_buffer_[1024] = {0}; bool ret_val = true; @@ -204,6 +264,36 @@ TEST_F(PugiEncodeTest, EncodedWell) { ); } TEST_F(PugiEncodeTest, EncodeTimeMeasurement) { + // Memory manager + // Set default resource + kuka_rsi_hw_interface::print_resource default_resource("Rogue PMR Allocation!", + std::pmr::null_memory_resource()); + std::pmr::set_default_resource(&default_resource); + + // create memory resource + kuka_rsi_hw_interface::print_resource out_of_memory("Out of Memory", + std::pmr::null_memory_resource()); + + std::array buffer{}; + std::pmr::monotonic_buffer_resource underlying_bytes{buffer.data(), buffer.size(), + &out_of_memory}; + + //kuka_rsi_hw_interface::print_resource monotonic("Monotonic Array", &underlying_bytes); + + // Create pool resource + std::pmr::pool_options pool_options; + pool_options.largest_required_pool_block = 35000; + std::pmr::synchronized_pool_resource sync_pool{pool_options, &underlying_bytes}; + //kuka_rsi_hw_interface::print_resource memory_pool("Pool", &sync_pool); + + // Create memory handler + kuka_rsi_hw_interface::MemoryManager memory_manager(&sync_pool); + kuka_rsi_hw_interface::memory_manager_handler = &memory_manager; + + pugi::set_memory_management_functions( + kuka_rsi_hw_interface::custom_allocate, + kuka_rsi_hw_interface::custom_deallocate); + kuka_rsi_hw_interface::PugiCommandHandler pugi_command_handler(1024); char out_buffer_[1024] = {0}; bool ret_val = true; From cb5bce24ecfabe67073c8c9988aaf8460babaa5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Komaromi=20S=C3=A1ndor?= Date: Tue, 26 Sep 2023 09:31:35 +0200 Subject: [PATCH 97/97] test duration correction --- kuka_rsi_hw_interface/test/src/decode_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_rsi_hw_interface/test/src/decode_test.cpp b/kuka_rsi_hw_interface/test/src/decode_test.cpp index 48080640..ce606ca7 100644 --- a/kuka_rsi_hw_interface/test/src/decode_test.cpp +++ b/kuka_rsi_hw_interface/test/src/decode_test.cpp @@ -141,7 +141,7 @@ TEST_F(PugiDecodeTest, DecodeTimeMeasurement) { ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) != -1); - for (size_t i = 0; i < 1000; i++) { + for (size_t i = 0; i < 4000; i++) { //std::cout << "[Debug] Decode started" << std::endl; auto start = std::chrono::high_resolution_clock::now(); ret_val &= pugi_command_handler.Decode(input_string, 1024);