diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 71edd5bf..b254a78d 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") @@ -23,26 +23,33 @@ 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(pugixml REQUIRED) -find_package(tinyxml_vendor REQUIRED) -find_package(TinyXML REQUIRED) +include_directories(include) -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 + src/pugi_command_handler.cpp ) +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. 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) - - +ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface angles) add_executable(robot_manager_node src/robot_manager_node.cpp) @@ -51,7 +58,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) @@ -71,9 +78,12 @@ if(BUILD_TESTING) ament_lint_cmake() ament_uncrustify(--language=C++) ament_xmllint(--exclude ros_rsi.rsi.xml) + + enable_testing() + add_subdirectory(test) endif() -## EXPORTS +# # EXPORTS ament_export_include_directories( include ) 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..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 @@ -41,10 +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 @@ -52,14 +48,16 @@ #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" @@ -112,14 +110,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_; - - static constexpr double R2D = 180 / M_PI; - static constexpr double D2R = M_PI / 180; + char in_buffer_[UDP_BUFFER_SIZE] = {0}; + char out_buffer_[UDP_BUFFER_SIZE] = {0}; }; } // namespace kuka_rsi_hw_interface 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..e700675e --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/pugi_command_handler.hpp @@ -0,0 +1,126 @@ +// 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 +#include +#include +#include +#include +#include + +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_; + 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 print_resource : public std::pmr::memory_resource +{ +public: + print_resource(std::string name, std::pmr::memory_resource * upstream) + : name_(name), upstream_(upstream) + { + assert(upstream); + } + +private: + std::string name_; + std::pmr::memory_resource * upstream_; + + 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; + } + + 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); + } + + bool do_is_equal(const std::pmr::memory_resource & other) const noexcept override + { + return this == &other; + } +}; + +class MemoryManager +{ +public: + MemoryManager(std::pmr::memory_resource * upstream); + void * Allocate(size_t size); + void Deallocate(void * ptr); + +private: + //std::pmr::polymorphic_allocator allocator_; + std::pmr::memory_resource * memory_pool_; + std::pmr::map memory_pool_sizes_; +}; + +class PugiCommandHandler +{ +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); + +private: + char * state_buffer_; +}; + +extern MemoryManager * memory_manager_handler; +} // namespace kuka_rsi_hw_interface + + +#endif // KUKA_RSI_HW_INTERFACE__PUGI_COMMAND_HANDLER_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h deleted file mode 100644 index 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_command_handler.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp new file mode 100644 index 00000000..fcec4008 --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp @@ -0,0 +1,89 @@ +// 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_ + +#include + +#include "xml_handler/xml_element.hpp" + +namespace kuka_rsi_hw_interface +{ + +class RSICommandHandler +{ +public: + 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, + const T & param) + { + try { + return command_data_structure_.Element(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); + 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( + 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, + xml::XMLString & param_name); + + 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( + 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 + + +#endif // KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h deleted file mode 100644 index e66ea478..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 actual position - TiXmlElement * RSol_el = rob->FirstChildElement("RSol"); - RSol_el->Attribute("X", &initial_cart_position[0]); - RSol_el->Attribute("Y", &initial_cart_position[1]); - RSol_el->Attribute("Z", &initial_cart_position[2]); - RSol_el->Attribute("A", &initial_cart_position[3]); - RSol_el->Attribute("B", &initial_cart_position[4]); - RSol_el->Attribute("C", &initial_cart_position[5]); - // Get the IPOC timestamp - TiXmlElement * ipoc_el = rob->FirstChildElement("IPOC"); - ipoc = std::stoull(ipoc_el->FirstChild()->Value()); - } - - 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_ 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..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 @@ -59,7 +59,7 @@ #include "rclcpp/rclcpp.hpp" -#define BUFSIZE 1024 +constexpr size_t UDP_BUFFER_SIZE = 1024; class UDPServer { @@ -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, + strnlen(buffer, UDP_BUFFER_SIZE), 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; @@ -133,25 +133,30 @@ class UDPServer } if (FD_ISSET(sockfd_, &read_fds)) { - memset(buffer_, 0, BUFSIZE); + memset(buffer_, 0, UDP_BUFFER_SIZE); bytes = - recvfrom(sockfd_, buffer_, BUFSIZE, 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"); + return bytes; } } else { return 0; } } else { - memset(buffer_, 0, BUFSIZE); - bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); + memset(buffer_, 0, UDP_BUFFER_SIZE); + bytes = recvfrom( + sockfd_, buffer_, UDP_BUFFER_SIZE, 0, (struct sockaddr *) &clientaddr_, + &clientlen_); if (bytes < 0) { RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); + return bytes; } } - - buffer = std::string(buffer_); + memcpy(buffer, buffer_, bytes); return bytes; } @@ -166,7 +171,7 @@ class UDPServer socklen_t clientlen_; struct sockaddr_in serveraddr_; struct sockaddr_in clientaddr_; - char buffer_[BUFSIZE]; + char buffer_[UDP_BUFFER_SIZE]; int optval; }; diff --git a/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp new file mode 100644 index 00000000..05a4ee10 --- /dev/null +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp @@ -0,0 +1,215 @@ +// 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_HANDLER__XML_ELEMENT_HPP_ +#define XML_HANDLER__XML_ELEMENT_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "xml_param.hpp" + +namespace xml +{ +inline bool operator<(const XMLString & a, const std::string & b) +{ + 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) +{ + 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; + } +} + +/** + * @brief The XMLElement class is a tree based representation of a XML structure. + * 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 +{ +public: + /** + * @brief Construct a new XMLElement object + * + * @param name: Name of the XMLElement object + */ + explicit XMLElement(const std::string & name) + : name_(name) {} + ~XMLElement() = default; + + /** + * @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_;} + + /** + * @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 was not found. + */ + inline const XMLElement * GetElement( + const std::string & elementName) const {return getElement(elementName);} + + /** + * @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 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 + T GetParam(const std::string & key) const + { + auto param_it = params_.find(key); + if (param_it != params_.end()) { + return param_it->second.GetParamValue(); + } + throw std::range_error("Could not find key in parameter list"); + } + + /** + * @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 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. + * @return true if the key is found in the parameter list and the parameter has proper type, + * @return false otherwise + */ + template + bool SetParam(const std::string & key, const T & param) + { + 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_it->second.param_value_ = param; + return true; + } + } + return false; + } + + /** + * @brief Return every child in a std::vector that the XMLElement object has + * + * @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 object's parameter map + */ + inline std::map> & Params() {return params_;} + + /** + * @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 was not found. + */ + inline XMLElement * Element(const std::string & elementName) {return element(elementName);} + + /** + * @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. + * @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 + */ + bool CastParamData(const XMLString & key, char * & str_ptr); + + /** + * @brief Checks if the given string matches one of parameter names of the XMLElement + * + * @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 if the given string matches the XMLElement's name + * + * @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. + */ + 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 + +#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 new file mode 100644 index 00000000..459f6247 --- /dev/null +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_param.hpp @@ -0,0 +1,60 @@ +// 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_HANDLER__XML_PARAM_HPP_ +#define XML_HANDLER__XML_PARAM_HPP_ + +#include + +#include "xml_string.hpp" + +namespace xml +{ +enum XMLType +{ + BOOL = 0, + LONG = 1, + DOUBLE = 2, + STRING = 3 +}; + + +class XMLParam +{ +public: + XMLType param_type_; + std::variant param_value_; + XMLParam() = default; + 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); + + template + 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_value_); + } + throw std::logic_error("Parameter type not supported"); + } + +private: + char prev_locale_[100] = {0}; +}; + +} // namespace xml +#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 new file mode 100644 index 00000000..cc113ac1 --- /dev/null +++ b/kuka_rsi_hw_interface/include/xml_handler/xml_string.hpp @@ -0,0 +1,46 @@ +// 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_HANDLER__XML_STRING_HPP_ +#define XML_HANDLER__XML_STRING_HPP_ + +#include + +namespace xml +{ +class XMLString +{ +public: + const char * data_ptr_; + size_t length_; + XMLString() + : data_ptr_(nullptr), length_(0) {} + XMLString(const char * data_ptr, size_t length) + : data_ptr_(data_ptr), length_(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"); + } + } + + int PrintString(char * & buffer_it, const int & size_left); + + friend std::ostream & operator<<(std::ostream & out, XMLString & xml_str); +}; +} // namespace xml + +#endif // XML_HANDLER__XML_STRING_HPP_ diff --git a/kuka_rsi_hw_interface/krl/README_KRC5.md b/kuka_rsi_hw_interface/krl/README_KRC5.md index b3ff5066..4177baa6 100644 --- a/kuka_rsi_hw_interface/krl/README_KRC5.md +++ b/kuka_rsi_hw_interface/krl/README_KRC5.md @@ -1,87 +1,87 @@ -# Configuring RSI on the controller - -This tutorial was tested with RSI 5.0.2 (on KSS8.7) - -This guide highlights the steps needed in order to successfully configure the **RSI interface** on the KRC5 controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS. - -## 1. Controller network configuration - -Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. - -1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). -2. There should already be an interface checked out as the **Windows interface**. For example: - * **IP**: 192.168.250.20 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: 192.168.250.20 or leave it empty - * **Windows interface checkbox** should be checked. -3. Add a new interface bz pressign the **Advanced** button and **New interface**. -4. Select **Mixed IP address** and keep the default **Receiving task: Target subnet** and **Real-time receiving Task: UDP** -5. Set the IP address to a different subnet then the **KLI interface**. For example: - * **IP**: 192.168.10.20 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: leave it empty - * **Windows interface checkbox** should NOT be checked -6. Save the changes and reboot the robot controller using the settings **Cold start** and **Reload files** - -## 2. Set fix IP in your PC -1. Set one IP in the subnet of the KLI interface, it is required to connect the WorkVisual and transfer project. For example: - * **IP**: 192.168.250.10 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: 192.168.250.20 or leave it empty -2. Add another IP in the subnet of the RSI interface, it is required to send commands via the RSI interface. For example: - * In windows **Network and Sharing center**, select the internet adapter settings *e.g. Ethernet Properties. - * Select the IPV4 properties and their the **Advanced** settings. - * In the new window, you can **Add** new IP addresses. - -## 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. - -##### 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 (59152) or change it if you want to use another port. - -Note that the `client_ip` and `client_port` parameters in config/rsi_config.yaml 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. - -##### Copy files to controller -The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All files are now ready to be copied to the Kuka controller: - -Method 1: -1. Copy the files to a USB-stick. -2. Plug it into the teach pad or controller. -3. Log in as **Expert** or **Administrator**. -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`. - -Method 2: -1. Use the WorkVisual, connect to the KRC -2. Log in as **Expert** or **Administrator**. -4. Copy the `ros_rsi.src` file to `KRC:\R1\Program` in the WorkVisual -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 - -## 4. Testing -At this point you are ready to test the RSI interface. Before the test, make sure that: - -* You have specified the `client_ip` and `client_port` tags in the configuration file (config/rsi_config.yaml) 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: - -* Start the driver: ```ros2 launch kuka_rsi_hw_interface startup.launch.py``` - -* 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. - -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 +# Configuring RSI on the controller + +This tutorial was tested with RSI 5.0.2 (on KSS8.7) + +This guide highlights the steps needed in order to successfully configure the **RSI interface** on the KRC5 controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS. + +## 1. Controller network configuration + +Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. + +1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). +2. There should already be an interface checked out as the **Windows interface**. For example: + * **IP**: 192.168.250.20 + * **Subnet mask**: 255.255.255.0 + * **Default gateway**: 192.168.250.20 or leave it empty + * **Windows interface checkbox** should be checked. +3. Add a new interface bz pressign the **Advanced** button and **New interface**. +4. Select **Mixed IP address** and keep the default **Receiving task: Target subnet** and **Real-time receiving Task: UDP** +5. Set the IP address to a different subnet then the **KLI interface**. For example: + * **IP**: 192.168.10.20 + * **Subnet mask**: 255.255.255.0 + * **Default gateway**: leave it empty + * **Windows interface checkbox** should NOT be checked +6. Save the changes and reboot the robot controller using the settings **Cold start** and **Reload files** + +## 2. Set fix IP in your PC +1. Set one IP in the subnet of the KLI interface, it is required to connect the WorkVisual and transfer project. For example: + * **IP**: 192.168.250.10 + * **Subnet mask**: 255.255.255.0 + * **Default gateway**: 192.168.250.20 or leave it empty +2. Add another IP in the subnet of the RSI interface, it is required to send commands via the RSI interface. For example: + * In windows **Network and Sharing center**, select the internet adapter settings *e.g. Ethernet Properties. + * Select the IPV4 properties and their the **Advanced** settings. + * In the new window, you can **Add** new IP addresses. + +## 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. + +##### 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 (59152) or change it if you want to use another port. + +Note that the `client_ip` and `client_port` parameters in config/rsi_config.yaml 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. + +##### Copy files to controller +The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All files are now ready to be copied to the Kuka controller: + +Method 1: +1. Copy the files to a USB-stick. +2. Plug it into the teach pad or controller. +3. Log in as **Expert** or **Administrator**. +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`. + +Method 2: +1. Use the WorkVisual, connect to the KRC +2. Log in as **Expert** or **Administrator**. +4. Copy the `ros_rsi.src` file to `KRC:\R1\Program` in the WorkVisual +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 + +## 4. Testing +At this point you are ready to test the RSI interface. Before the test, make sure that: + +* You have specified the `client_ip` and `client_port` tags in the configuration file (config/rsi_config.yaml) 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: + +* Start the driver: ```ros2 launch kuka_rsi_hw_interface startup.launch.py``` + +* 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. + +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/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 old mode 100755 new mode 100644 index aadb5a8a..a898f18a --- 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..bf1e66e9 100644 --- a/kuka_rsi_hw_interface/package.xml +++ b/kuka_rsi_hw_interface/package.xml @@ -19,7 +19,8 @@ tinyxml_vendor hardware_interface pluginlib - controller_manager_msgs + 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 1d2eabfc..210ad326 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 { @@ -53,6 +54,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); @@ -87,8 +90,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); @@ -151,17 +152,62 @@ 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; + } - rsi_state_ = RSIState(in_buffer_); + // Position data + 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] = 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"); 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; - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; + // 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_)); + + 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"); + return CallbackReturn::FAILURE; + } server_->send(out_buffer_); server_->set_timeout(1000); // Set receive timeout to 1 second @@ -176,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; } @@ -193,12 +240,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; + if (!command_handler_.Decode(in_buffer_, UDP_BUFFER_SIZE)) { + return return_type::ERROR; } - ipoc_ = rsi_state_.ipoc; + // Update state params + 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")); + + // Update ipoc + ipoc_ = command_handler_.GetState().GetElement("IPOC")->GetParam("IPOC"); return return_type::OK; } @@ -217,15 +277,27 @@ 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]); } - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; + // 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]); + 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_)); + + auto out_buffer_it = out_buffer_; + if (command_handler_.Encode(out_buffer_it, UDP_BUFFER_SIZE) < 0) { + return return_type::ERROR; + } 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/pugi_command_handler.cpp b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp new file mode 100644 index 00000000..cff2c890 --- /dev/null +++ b/kuka_rsi_hw_interface/src/pugi_command_handler.cpp @@ -0,0 +1,92 @@ +// 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 +{ +MemoryManager * memory_manager_handler; + +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; +} + + +void * custom_allocate(size_t size) +{ + return kuka_rsi_hw_interface::memory_manager_handler->Allocate(size); +} +void custom_deallocate(void * ptr) +{ + kuka_rsi_hw_interface::memory_manager_handler->Deallocate(ptr); +} + +MemoryManager::MemoryManager(std::pmr::memory_resource * upstream) +: memory_pool_(upstream), memory_pool_sizes_(upstream) +{ + // 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 = static_cast(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(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/src/rsi_command_handler.cpp b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp new file mode 100644 index 00000000..d4b03afc --- /dev/null +++ b/kuka_rsi_hw_interface/src/rsi_command_handler.cpp @@ -0,0 +1,373 @@ +// 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 "kuka_rsi_hw_interface/rsi_command_handler.hpp" +#include "rclcpp/rclcpp.hpp" + +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); + // how to get string: std::get(command_data_structure_.params_["TYPE"]).first + 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 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(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")); + 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); + command_data_structure_.Childs().emplace_back(Ipoc_command_el); +} +bool RSICommandHandler::SetLocale() +{ + return std::setlocale(LC_NUMERIC, "C"); +} + +bool RSICommandHandler::ResetLocale() +{ + return std::setlocale(LC_NUMERIC, prev_locale_.c_str()); +} + +bool RSICommandHandler::Decode(char * const buffer, const size_t buffer_size) +{ + auto buffer_it = buffer; + try { + 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()); + ResetLocale(); + return false; + } +} + +int RSICommandHandler::Encode(char * & buffer, const size_t buffer_size) +{ + auto buffer_it = buffer; + int size_left = buffer_size; + try { + 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"); + } + // +1 is for the \0 character + return buffer_size - size_left + 1; + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("CommandHandler"), "%s", e.what()); + ResetLocale(); + return -1; + } +} + +void RSICommandHandler::decodeNodes( + xml::XMLElement & element, char * const buffer, char * & buffer_it, + const size_t buffer_size) +{ + xml::XMLString node_name(buffer, 0); + 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()); + } + buffer_it++; + // Validate node name + if (!element.IsNameValid(node_name, buffer_it)) { + 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 node parameters + size_t num_of_param = 0; + bool is_param_only_node = false; + 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 + 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; + is_no_more_param = true; + buffer_it += 2; + } else if (*buffer_it == '>') { + is_no_more_param = true; + buffer_it++; + } else { + // Decode parameter + decodeParam(element, buffer_it); + num_of_param++; + } + } + 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()); + } + + // 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() << + "elements parameter list size."; + throw std::logic_error(err_ss.str()); + } + + // Node should have childs, but did not found any + 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()); + } + // Go to the next node or the end of the node + 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) { + 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); + } + } + // Check for the end tag () + decodeNodeEnd(element, buffer_it); + } +} + +void RSICommandHandler::decodeParam(xml::XMLElement & element, char * & buffer_it) +{ + 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()); + } + // 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++; +} + +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::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) +{ + // 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, " />"); + + 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); + } +} + +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 diff --git a/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp new file mode 100644 index 00000000..581260b4 --- /dev/null +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_element.cpp @@ -0,0 +1,149 @@ +// 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 "xml_handler/xml_element.hpp" + +namespace xml +{ +bool XMLElement::CastParamData(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: + { + char * end = str_ptr; + auto res = static_cast(strtol(str_ptr, &end, 0)); + if (res == 0 && (errno != 0 || end == str_ptr)) { + return false; + } + str_ptr = end; + param_it->second.param_value_ = res; + break; + } + case XMLType::LONG: + { + char * end = str_ptr; + auto res = strtol(str_ptr, &end, 0); + if (res == 0 && (errno != 0 || end == str_ptr)) { + return false; + } + str_ptr = end; + param_it->second.param_value_ = res; + break; + } + case XMLType::DOUBLE: + { + char * end = str_ptr; + auto res = strtod(str_ptr, &end); + if (res == 0 && (errno != 0 || end == str_ptr)) { + return false; + } + str_ptr = end; + param_it->second.param_value_ = res; + break; + } + case XMLType::STRING: + param_it->second.param_value_ = castXMLString(str_ptr); + break; + default: + return false; + } + } + return ret_val; +} + +bool XMLElement::IsParamNameValid(XMLString & key, char * & str_ptr) +{ + key = castXMLString(str_ptr); + auto param_it = params_.find(key); + if (param_it != params_.end()) { + return true; + } + return false; +} + + +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_) == 0; +} + +XMLElement * XMLElement::element(const std::string & elementName, int depth) +{ + if (elementName == name_) { + return this; + } + for (auto && child : childs_) { + auto ret_val = child.element(elementName, depth + 1); + if (ret_val != nullptr) { + return ret_val; + } + } + if (depth == 0) { + std::stringstream err_ss; + err_ss << elementName << " element not found."; + throw std::logic_error(err_ss.str()); + } + return nullptr; +} + +const XMLElement * XMLElement::getElement( + const std::string & elementName, + int depth) const +{ + if (elementName == name_) { + return this; + } + for (auto && child : childs_) { + auto ret_val = child.getElement(elementName, depth + 1); + if (ret_val != nullptr) { + return ret_val; + } + } + if (depth == 0) { + std::stringstream err_ss; + err_ss << elementName << " element not found."; + throw std::logic_error(err_ss.str()); + } + return nullptr; +} + +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++) + { + } + auto data = XMLString( + start_ref, static_cast(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 new file mode 100644 index 00000000..59023102 --- /dev/null +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_param.cpp @@ -0,0 +1,61 @@ +// 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 "xml_handler/xml_param.hpp" + +namespace xml +{ +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", GetParamValue()); + break; + + case XMLType::LONG: + idx = snprintf(buffer_it, size_left, "%lu", GetParamValue()); + break; + + case XMLType::DOUBLE: + idx = snprintf(buffer_it, size_left, "%f", GetParamValue()); + break; + + case XMLType::STRING: + idx = GetParamValue().PrintString(buffer_it, size_left); + break; + + default: + throw std::logic_error("Parameter type not supported"); + break; + } + if (idx < 0 || idx > size_left) { + throw std::range_error("Out of the buffers range"); + } else { + buffer_it += idx; + size_left -= idx; + } + return idx; +} + +std::ostream & operator<<(std::ostream & out, class XMLParam & param) +{ + std::visit([&out](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 new file mode 100644 index 00000000..a9688e81 --- /dev/null +++ b/kuka_rsi_hw_interface/src/xml_handler/xml_string.cpp @@ -0,0 +1,39 @@ +// 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_handler/xml_string.hpp" + +namespace xml +{ +int XMLString::PrintString(char * & buffer_it, const int & size_left) +{ + if (static_cast(length_) + 1 > size_left) { + return -1; + } else { + snprintf(buffer_it, length_ + 1, "%s", data_ptr_); + return 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; +} +} // namespace xml diff --git a/kuka_rsi_hw_interface/test/CMakeLists.txt b/kuka_rsi_hw_interface/test/CMakeLists.txt new file mode 100644 index 00000000..a685c3fa --- /dev/null +++ b/kuka_rsi_hw_interface/test/CMakeLists.txt @@ -0,0 +1,35 @@ +find_package(Threads REQUIRED) + +set(sources + src/ut_test_runner.cpp + src/decode_test.cpp + 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... +) + +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..4f7ab5c7 --- /dev/null +++ b/kuka_rsi_hw_interface/test/include/decode_test.h @@ -0,0 +1,33 @@ +// 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() + { + } +}; + +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 new file mode 100644 index 00000000..e1cdbdcf --- /dev/null +++ b/kuka_rsi_hw_interface/test/include/encode_test.h @@ -0,0 +1,33 @@ +// 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() + { + } +}; + +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 new file mode 100644 index 00000000..ce606ca7 --- /dev/null +++ b/kuka_rsi_hw_interface/test/src/decode_test.cpp @@ -0,0 +1,153 @@ +// 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 +#include + +#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; + + char input_string[] = + "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(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.txt"); + + struct sched_param param; + param.sched_priority = 98; + ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) != -1); + + 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 << i << " " << duration.count() << std::endl; + } + ASSERT_TRUE(ret_val); +} + + +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); +} +TEST_F(PugiDecodeTest, DecodeTimeMeasurement) { + // 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 input_string[] = + "0"; + bool ret_val = true; + std::ofstream timer_data; + + timer_data.open("PugiDecodeTest_DecodeTimeMeasurement.txt"); + + struct sched_param param; + param.sched_priority = 98; + ret_val &= (sched_setscheduler(0, SCHED_FIFO, ¶m) != -1); + + + 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); + 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); +} 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..d058b1ea --- /dev/null +++ b/kuka_rsi_hw_interface/test/src/encode_test.cpp @@ -0,0 +1,348 @@ +// 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 +#include + +#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}; + 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) != -1); +} +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) != -1); + ASSERT_TRUE( + strcmp( + out_buffer_, + "0100") == 0 + ); +} + +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) { + // 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; + + 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) { + // 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; + + 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); + + ASSERT_TRUE(ret_val); + ASSERT_TRUE((pugi_command_handler.Encode(out_buffer_, 1024) != nullptr)); + ASSERT_TRUE( + strcmp( + out_buffer_, + "0100") == 0 + ); +} +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; + + 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); +} 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(); +} 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 +