diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d130e7..2650649 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,6 @@ 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) @@ -16,6 +15,7 @@ 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 $ - $) + $ + /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) diff --git a/include/onnxruntime_controller/onnxruntime_controller.hpp b/include/onnxruntime_controller/onnxruntime_controller.hpp index 550d7f9..b285607 100644 --- a/include/onnxruntime_controller/onnxruntime_controller.hpp +++ b/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 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 valid_joint_interfaces = { + "position", "velocity", "effort", "acceleration"}; class ONNXRuntimeController : public controller_interface::ChainableControllerInterface { @@ -61,13 +60,12 @@ class ONNXRuntimeController protected: std::vector - 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 state_indices_; std::vector observation_scales_; + std::vector observation_offsets_; std::vector actions_; Ort::Value actions_tensor_; diff --git a/include/onnxruntime_controller/typed_interface.hpp b/include/onnxruntime_controller/typed_interface.hpp index 088cfbb..4b2d006 100644 --- a/include/onnxruntime_controller/typed_interface.hpp +++ b/include/onnxruntime_controller/typed_interface.hpp @@ -2,10 +2,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include @@ -27,7 +29,7 @@ class TypedInterface { std::shared_ptr 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 library_members_; std::string name_; diff --git a/models/policy.onnx b/models/policy.onnx index 55f3613..e826834 100644 Binary files a/models/policy.onnx and b/models/policy.onnx differ diff --git a/models/policy.onnx.data b/models/policy.onnx.data index d3d3b26..adcfa64 100644 Binary files a/models/policy.onnx.data and b/models/policy.onnx.data differ diff --git a/package.xml b/package.xml index 0c11161..13cac92 100644 --- a/package.xml +++ b/package.xml @@ -10,12 +10,12 @@ ament_cmake generate_parameter_library - onnxruntime_vendor controller_interface hardware_interface rclcpp rclcpp_lifecycle pluginlib + realtime_tools rosidl_typesupport_introspection_cpp ament_lint_auto diff --git a/src/onnxruntime_controller.cpp b/src/onnxruntime_controller.cpp index 2156b77..9235e24 100644 --- a/src/onnxruntime_controller.cpp +++ b/src/onnxruntime_controller.cpp @@ -26,9 +26,6 @@ std::tuple, 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(), @@ -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::quiet_NaN()); return {std::vector(), 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; } diff --git a/src/onnxruntime_controller_parameters.yaml b/src/onnxruntime_controller_parameters.yaml index 4d9ad3e..7bd2a13 100644 --- a/src/onnxruntime_controller_parameters.yaml +++ b/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", diff --git a/src/typed_interface.cpp b/src/typed_interface.cpp index 611fafa..fd359dc 100644 --- a/src/typed_interface.cpp +++ b/src/typed_interface.cpp @@ -12,6 +12,7 @@ #include #include + #include #include @@ -37,7 +38,6 @@ TypedInterface::TypedInterface( std::shared_ptr 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_);