Humble

CMakeLists.txt

@@ -8,14 +8,14 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(onnxruntime_vendor REQUIRED)
 find_package(controller_interface REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 find_package(rosidl_typesupport_introspection_cpp REQUIRED)
+find_package(realtime_tools REQUIRED)
 
 generate_parameter_library(onnxruntime_controller_parameters
   src/onnxruntime_controller_parameters.yaml
@@ -28,15 +28,21 @@ add_library(${PROJECT_NAME} SHARED
 target_compile_features(onnxruntime_controller PUBLIC cxx_std_17)
 target_include_directories(onnxruntime_controller PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+  /usr/local/include/onnxruntime
+  /usr/local/include/onnxruntime/onnxruntime/core/session
+)
 target_link_libraries(onnxruntime_controller PUBLIC
-                      onnxruntime_controller_parameters
-                      controller_interface::controller_interface
-                      hardware_interface::hardware_interface
-                      pluginlib::pluginlib
-                      rclcpp::rclcpp
-                      rclcpp_lifecycle::rclcpp_lifecycle
-                      ${onnxruntime_vendor_LIBRARIES})
+  onnxruntime_controller_parameters
+  controller_interface::controller_interface
+  hardware_interface::hardware_interface
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  realtime_tools::realtime_tools
+  /usr/local/lib/libonnxruntime.so
+  /usr/local/lib/libonnxruntime_providers_cuda.so
+)
 
 pluginlib_export_plugin_description_file(
   controller_interface onnxruntime_controller_plugins.xml)

include/onnxruntime_controller/onnxruntime_controller.hpp

@@ -32,9 +32,8 @@ namespace onnxruntime_controller {
 /// Constant defining last action interface name
 constexpr char HW_IF_LAST_ACTION[] = "last_action";
 
-const std::array<std::string, 5> valid_joint_interfaces = {
-    hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
-    hardware_interface::HW_IF_EFFORT, hardware_interface::HW_IF_ACCELERATION};
+const std::array<std::string, 4> valid_joint_interfaces = {
+    "position", "velocity", "effort", "acceleration"};
 
 class ONNXRuntimeController
     : public controller_interface::ChainableControllerInterface {
@@ -61,13 +60,12 @@ class ONNXRuntimeController
 
 protected:
   std::vector<hardware_interface::CommandInterface>
-  on_export_reference_interfaces() override;
+  on_export_reference_interfaces();
 
   bool on_set_chained_mode(bool chained) override;
 
   controller_interface::return_type
-  update_reference_from_subscribers(const rclcpp::Time &time,
-                                    const rclcpp::Duration &period) override;
+  update_reference_from_subscribers() override;
 
   controller_interface::return_type
   update_and_write_commands(const rclcpp::Time &time,
@@ -127,6 +125,7 @@ class ONNXRuntimeController
   std::vector<size_t> state_indices_;
 
   std::vector<double> observation_scales_;
+  std::vector<double> observation_offsets_;
 
   std::vector<float> actions_;
   Ort::Value actions_tensor_;

include/onnxruntime_controller/typed_interface.hpp

@@ -2,10 +2,12 @@
 #include <hardware_interface/handle.hpp>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/serialization.hpp>
 #include <realtime_tools/realtime_buffer.hpp>
 #include <rosidl_runtime_c/message_type_support_struct.h>
 #include <rosidl_runtime_cpp/message_initialization.hpp>
 #include <rosidl_typesupport_introspection_cpp/field_types.hpp>
+#include <rosidl_typesupport_introspection_cpp/message_introspection.hpp>
 #include <string>
 #include <vector>
 
@@ -27,7 +29,7 @@ class TypedInterface {
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
 
   rosidl_message_type_support_t ts_members_;
-  const rosidl_typesupport_introspection_cpp::MessageMembers *members_;
+  const rosidl_typesupport_introspection_cpp::MessageMembers_s *members_;
   std::shared_ptr<rcpputils::SharedLibrary> library_members_;
 
   std::string name_;

package.xml

@@ -10,12 +10,12 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>generate_parameter_library</build_depend>
 
-  <depend>onnxruntime_vendor</depend>
   <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>pluginlib</depend>
+  <depend>realtime_tools</depend>
   <depend>rosidl_typesupport_introspection_cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/onnxruntime_controller.cpp

@@ -26,9 +26,6 @@ std::tuple<std::vector<std::string>, controller_interface::CallbackReturn>
 ONNXRuntimeController::process_interface(std::string interface_name,
                                          std::string interface_type,
                                          bool is_reference_interface) {
-  RCLCPP_INFO(get_node()->get_logger(), "Processing interface: %s, type: %s",
-              interface_name.c_str(), interface_type.c_str());
-
   // Invalid configuration
   if (interface_name.empty() || interface_type.empty()) {
     return {std::vector<std::string>(),
@@ -48,7 +45,8 @@ ONNXRuntimeController::process_interface(std::string interface_name,
     for (auto i = 0U; i < num_actions_; ++i) {
       last_actions_indices_.push_back(curr + i);
     }
-    observations_.resize(observations_.size() + num_actions_, 0.0f);
+    observations_.resize(observations_.size() + num_actions_,
+                         std::numeric_limits<float>::quiet_NaN());
     return {std::vector<std::string>(),
             controller_interface::CallbackReturn::SUCCESS};
   }
@@ -152,9 +150,13 @@ controller_interface::CallbackReturn ONNXRuntimeController::on_configure(
           params_.observation_types.size() ||
       (params_.observation_interfaces.size() !=
            params_.observation_scales.size() &&
-       !params_.observation_scales.empty())) {
+       !params_.observation_scales.empty()) ||
+      (params_.observation_interfaces.size() !=
+           params_.observation_offsets.size() &&
+       !params_.observation_offsets.empty())) {
     RCLCPP_ERROR(get_node()->get_logger(),
-                 "Observation interfaces and types must have the same size.");
+                 "Observation interfaces, types, scales, and offsets must have "
+                 "compatible sizes.");
     return controller_interface::CallbackReturn::ERROR;
   }
 
@@ -233,12 +235,6 @@ controller_interface::CallbackReturn ONNXRuntimeController::on_configure(
     reference_interface_names_.insert(reference_interface_names_.end(),
                                       interfaces.begin(), interfaces.end());
   }
-
-  for (auto i = 0U; i < reference_interface_names_.size(); ++i) {
-    RCLCPP_INFO(get_node()->get_logger(), "Reference interface: %s",
-                reference_interface_names_[i].c_str());
-  }
-
   for (auto i = 0U; i < params_.observation_interfaces.size(); ++i) {
     auto ret = validate_interface_name(params_.observation_interfaces[i]);
     if (ret != controller_interface::CallbackReturn::SUCCESS) {
@@ -273,6 +269,9 @@ controller_interface::CallbackReturn ONNXRuntimeController::on_configure(
       observation_scales_.push_back(params_.observation_scales.empty()
                                         ? 1.0
                                         : params_.observation_scales[i]);
+      observation_offsets_.push_back(params_.observation_offsets.empty()
+                                         ? 0.0
+                                         : params_.observation_offsets[i]);
     }
   }
 
@@ -347,8 +346,7 @@ bool ONNXRuntimeController::on_set_chained_mode(bool /*chained*/) {
 }
 
 controller_interface::return_type
-ONNXRuntimeController::update_reference_from_subscribers(
-    const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+ONNXRuntimeController::update_reference_from_subscribers() {
   for (auto &reference : references_) {
     reference->update_from_subscriber();
   }
@@ -365,10 +363,9 @@ ONNXRuntimeController::update_and_write_commands(
 
   for (auto i = 0U; i < state_indices_.size(); ++i) {
     auto index = state_indices_[i];
-    auto state_op = state_interfaces_[i].get_optional();
-    if (state_op.has_value()) {
-      observations_[index] = state_op.value() * observation_scales_[index];
-    }
+    observations_[index] =
+        (state_interfaces_[i].get_value() - observation_offsets_[index]) *
+        observation_scales_[index];
   }
 
   for (auto i = 0U; i < last_actions_indices_.size(); ++i) {
@@ -381,22 +378,24 @@ ONNXRuntimeController::update_and_write_commands(
   session_.Run(Ort::RunOptions{}, input_names_, &observations_tensor_, 1,
                output_names_, &actions_tensor_, 1);
 
-  for (auto i = 0U; i < actions_.size(); ++i) {
-    RCLCPP_INFO(get_node()->get_logger(), "Action %u: %f", i, actions_[i]);
-  }
-
-  bool actions_set = true;
+  // bool actions_set = true;
   for (auto i = 0U; i < actions_.size(); ++i) {
     actions_[i] = std::clamp(actions_[i], -clip_actions_, clip_actions_);
-    actions_set &= command_interfaces_[i].set_value(
-        (actions_[i] * actions_scale_) + action_offsets_[i]);
+    command_interfaces_[i].set_value((actions_[i] * actions_scale_) +
+                                     action_offsets_[i]);
   }
 
-  if (!actions_set) {
-    RCLCPP_DEBUG_EXPRESSION(get_node()->get_logger(), !actions_set,
-                            "Unable to set an actions command interface. :(");
+  for (auto i = 0U; i < command_interfaces_.size(); ++i) {
+    RCLCPP_INFO(get_node()->get_logger(), "Command %u: %f", i,
+                command_interfaces_[i].get_value());
   }
 
+  // if (!actions_set) {
+  //   RCLCPP_DEBUG_EXPRESSION(get_node()->get_logger(), !actions_set,
+  //                           "Unable to set an actions command interface.
+  //                           :(");
+  // }
+
   return controller_interface::return_type::OK;
 }
 

src/onnxruntime_controller_parameters.yaml

@@ -20,6 +20,13 @@ onnxruntime_controller:
       default_value: [],
       read_only: true,
     }
+  observation_offsets:
+    {
+      type: "double_array",
+      description: "The offsets for each observation interface.",
+      default_value: [],
+      read_only: true,
+    }
   actions_interface:
     {
       type: "string",

src/typed_interface.cpp

@@ -12,6 +12,7 @@
 #include <rcpputils/find_library.hpp>
 #include <rcpputils/shared_library.hpp>
 
+
 #include <rosidl_typesupport_introspection_cpp/message_introspection.hpp>
 #include <utility>
 
@@ -37,7 +38,6 @@ TypedInterface::TypedInterface(
     std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, std::string name,
     std::string type)
     : node_(node),
-      ts_members_(::rosidl_get_zero_initialized_message_type_support_handle()),
       members_(nullptr), name_(name) {
   ts_members_ = *get_type_support_handle("rosidl_typesupport_introspection_cpp",
                                          type, library_members_);