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")
@@ -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
 )

kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp

@@ -41,25 +41,23 @@
 #ifndef KUKA_RSI_HW_INTERFACE__KUKA_HARDWARE_INTERFACE_HPP_
 #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 <vector>
 #include <string>
 #include <memory>
 #include <chrono>
 #include <cmath>
 #include <mutex>
 
-#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<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_;
-
-  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
 

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 <pugixml.hpp>
+#include <cstring>
+#include <memory_resource>
+#include <iostream>
+#include <cassert>
+#include <map>
+#include <functional>
+
+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<std::byte> allocator_;
+  std::pmr::memory_resource * memory_pool_;
+  std::pmr::map<void *, size_t> 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_