Feature/rsi driver

kuka_rsi_hw_interface/CMakeLists.txt

@@ -8,7 +8,7 @@ endif()
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -29,29 +29,37 @@ find_package(TinyXML REQUIRED)
 
 include_directories(include ${TinyXML2_INCLUDE_DIRS})
 
+add_library(xml_handler SHARED
+  src/xml_handler/xml_element.cpp
+  src/xml_handler/xml_param.cpp
+  src/xml_handler/xml_string.cpp
+)
+
 add_library(${PROJECT_NAME} SHARED
   src/kuka_hardware_interface.cpp
+  src/rsi_command_handler.cpp
 )
 
+target_link_libraries(${PROJECT_NAME} xml_handler)
+
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL")
+
 # prevent pluginlib from using boost
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface)
 target_link_libraries(${PROJECT_NAME} tinyxml)
 
-
-
 add_executable(robot_manager_node
   src/robot_manager_node.cpp)
 ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager_msgs)
 target_link_libraries(robot_manager_node kroshu_ros2_core::communication_helpers)
 
 pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml)
 
-install(TARGETS ${PROJECT_NAME} robot_manager_node
+install(TARGETS ${PROJECT_NAME} robot_manager_node xml_handler
   DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
@@ -73,7 +81,7 @@ if(BUILD_TESTING)
   ament_xmllint(--exclude ros_rsi.rsi.xml)
 endif()
 
-## EXPORTS
+# # EXPORTS
 ament_export_include_directories(
   include
 )

kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp

@@ -42,8 +42,9 @@
 #define KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_
 
 #include <kuka_rsi_hw_interface/udp_server.h>
-#include <kuka_rsi_hw_interface/rsi_state.h>
-#include <kuka_rsi_hw_interface/rsi_command.h>
+#include <kuka_rsi_hw_interface/rsi_command_handler.hpp>
+// #include <kuka_rsi_hw_interface/rsi_state.h>
+// #include <kuka_rsi_hw_interface/rsi_command.h>
 
 #include <vector>
 #include <string>
@@ -112,11 +113,10 @@ class KukaRSIHardwareInterface : public hardware_interface::SystemInterface
   std::vector<double> joint_pos_correction_deg_;
 
   uint64_t ipoc_ = 0;
-  RSIState rsi_state_;
-  RSICommand rsi_command_;
+  RSICommandHandler command_handler_;
   std::unique_ptr<UDPServer> server_;
-  std::string in_buffer_;
-  std::string out_buffer_;
+  char in_buffer_[UDP_BUFFER_SIZE] = {0};
+  char out_buffer_[UDP_BUFFER_SIZE] = {0};
 
   static constexpr double R2D = 180 / M_PI;
   static constexpr double D2R = M_PI / 180;

kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command_handler.hpp

@@ -0,0 +1,89 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2014 Norwegian University of Science and Technology
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Norwegian University of Science and
+*     Technology, nor the names of its contributors may be used to
+*     endorse or promote products derived from this software without
+*     specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/*
+ * Author: Komaromi Sandor
+ */
+
+#ifndef  KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_
+#define  KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_
+
+
+#include "xml_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<typename T>
+  bool SetCommandParam(
+    const std::string & elementName, const std::string & paramName,
+    const T & param)
+  {
+    try {
+      return command_data_structure_.Element(elementName)->SetParam<T>(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);
+
+private:
+  xml::XMLElement command_data_structure_;
+  xml::XMLElement state_data_structure_;
+
+  void decodeNode(
+    xml::XMLElement & element, char * const buffer, char * & buffer_it,
+    const size_t buffer_size);
+
+  void encodeNode(
+    xml::XMLElement & element, char * & buffer_it, int & size_left);
+};
+}  // namespace kuka_rsi_hw_interface
+
+
+#endif  //  KUKA_RSI_HW_INTERFACE__RSI_COMMAND_HANDLER_H_

kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h

@@ -88,7 +88,7 @@ class RSIState
     RIst_el->Attribute("A", &cart_position[3]);
     RIst_el->Attribute("B", &cart_position[4]);
     RIst_el->Attribute("C", &cart_position[5]);
-    // Extract cartesian actual position
+    // Extract cartesian setpoint position
     TiXmlElement * RSol_el = rob->FirstChildElement("RSol");
     RSol_el->Attribute("X", &initial_cart_position[0]);
     RSol_el->Attribute("Y", &initial_cart_position[1]);

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,20 +102,20 @@ class UDPServer
     }
   }
 
-  ssize_t send(std::string & buffer)
+  ssize_t send(char * buffer)
   {
     ssize_t bytes = 0;
     bytes = sendto(
-      sockfd_, buffer.c_str(),
-      buffer.size(), 0, (struct sockaddr *) &clientaddr_, clientlen_);
+      sockfd_, buffer,
+      strlen(buffer), 0, (struct sockaddr *) &clientaddr_, clientlen_);
     if (bytes < 0) {
       RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in send");
     }
 
     return bytes;
   }
 
-  ssize_t recv(std::string & buffer)
+  ssize_t recv(char * buffer)
   {
     ssize_t bytes = 0;
 
@@ -133,9 +133,11 @@ 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");
         }
@@ -144,14 +146,16 @@ class UDPServer
       }
 
     } 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");
       }
     }
 
-    buffer = std::string(buffer_);
+    strncpy(buffer, buffer_, bytes);
 
     return bytes;
   }
@@ -166,7 +170,7 @@ class UDPServer
   socklen_t clientlen_;
   struct sockaddr_in serveraddr_;
   struct sockaddr_in clientaddr_;
-  char buffer_[BUFSIZE];
+  char buffer_[UDP_BUFFER_SIZE];
   int optval;
 };
 

kuka_rsi_hw_interface/include/xml_handler/xml_element.hpp

@@ -0,0 +1,129 @@
+// Copyright 2023 Komáromi Sándor
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef  XML__XML_ELEMENT_H_
+#define  XML__XML_ELEMENT_H_
+
+#include <map>
+#include <vector>
+#include <string>
+#include <cstring>
+#include <cstdlib>
+#include <iostream>
+#include <functional>
+
+#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;
+  }
+}
+
+class XMLElement
+{
+private:
+  static XMLString castXMLString(char * & str_ptr);
+  XMLElement * element(const std::string & elementName, int depth = 0);
+  const XMLElement * const getElement(
+    const std::string & elementName,
+    int depth = 0) const;
+
+  std::map<std::string, XMLParam, std::less<>> params_;
+  std::vector<XMLElement> childs_;
+  std::string name_;
+
+public:
+  XMLElement(const std::string & name)
+  : name_(name) {}
+  XMLElement() = default;
+  ~XMLElement() = default;
+
+  inline std::string GetName() const {return name_;}
+  inline void SetName(const std::string & name) {name_ = name;}
+  inline const std::vector<XMLElement> & GetChilds() const {return childs_;}
+  inline std::vector<XMLElement> & Childs() {return childs_;}
+  inline std::map<std::string, XMLParam, std::less<>> & Params() {return params_;}
+
+  inline XMLElement * Element(const std::string & elementName) {return element(elementName);}
+  inline const XMLElement * const GetElement(
+    const std::string & elementName) const {return getElement(elementName);}
+
+  bool CastParam(const XMLString & key, char * & str_ptr);
+  bool IsParamNameValid(XMLString & key, char * & str_ptr);
+  bool IsNameValid(XMLString & key, char * & str_ptr);
+
+  // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used
+  template<typename T>
+  bool GetParam(const std::string & key, T & param) const
+  {
+    auto param_it = params_.find(key);
+    if (param_it != params_.end()) {
+      if constexpr (std::is_same<T, bool>::value || std::is_same<T, long>::value ||
+        std::is_same<T, double>::value || std::is_same<T, XMLString>::value)
+      {
+        param = param_it->second.GetParamValue<T>();
+        return true;
+      }
+    }
+    return false;
+  }
+
+  template<typename T>
+  T GetParam(const std::string & key) const
+  {
+    auto param_it = params_.find(key);
+    if (param_it != params_.end()) {
+      return param_it->second.GetParamValue<T>();
+    }
+    throw std::range_error("Could not find key in parameter list");
+  }
+
+  // TODO (Komaromi): When cpp20 is in use, use requires so only the types we set can be used
+  template<typename T>
+  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<T, bool>::value || std::is_same<T, long>::value ||
+        std::is_same<T, double>::value || std::is_same<T, XMLString>::value)
+      {
+        param_it->second.param_value_ = param;
+        return true;
+      }
+    }
+    return false;
+  }
+};
+}
+
+#endif  // XML__XML_ELEMENT_H_