ros2_control - rolling
Loading...
Searching...
No Matches
onnx_policy_controller.hpp
1// Copyright 2025 ros2_control Development Team
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14//
15// Author: Julia Jia
16
17#ifndef ONNX_POLICY_CONTROLLER__ONNX_POLICY_CONTROLLER_HPP_
18#define ONNX_POLICY_CONTROLLER__ONNX_POLICY_CONTROLLER_HPP_
19
20#include <onnxruntime_cxx_api.h>
21
22#include <atomic>
23#include <memory>
24#include <string>
25#include <vector>
26
27#include "control_msgs/msg/float64_values.hpp"
28#include "control_msgs/msg/keys.hpp"
29#include "controller_interface/controller_interface.hpp"
30#include "onnx_policy_controller/action_processor.hpp"
31#include "onnx_policy_controller/observation_formatter.hpp"
32#include "rclcpp/service.hpp"
33#include "rclcpp/subscription.hpp"
34#include "rclcpp_lifecycle/state.hpp"
35#include "realtime_tools/realtime_thread_safe_box.hpp"
36#include "ros2_control_demo_example_18/msg/velocity_command_with_head.hpp"
37// auto-generated by generate_parameter_library
38#include "ros2_control_demo_example_18/onnx_policy_controller_parameters.hpp"
39
40namespace onnx_policy_controller
41{
42
44{
45public:
46 controller_interface::CallbackReturn on_init() override;
47
49
51
52 controller_interface::CallbackReturn on_configure(
53 const rclcpp_lifecycle::State & previous_state) override;
54
55 controller_interface::CallbackReturn on_activate(
56 const rclcpp_lifecycle::State & previous_state) override;
57
58 controller_interface::CallbackReturn on_deactivate(
59 const rclcpp_lifecycle::State & previous_state) override;
60
61 controller_interface::return_type update(
62 const rclcpp::Time & time, const rclcpp::Duration & period) override;
63
68 static std::vector<double> velocity_command_to_policy_input(
69 const ros2_control_demo_example_18::msg::VelocityCommandWithHead & msg);
70
74 static std::string format_shape_string(const std::vector<int64_t> & shape);
75
88 static std::vector<int64_t> resolve_input_shape(
89 const std::vector<int64_t> & model_shape, size_t input_size);
90
91private:
92 bool load_model(const std::string & model_path);
93 bool run_model_inference(const std::vector<float> & inputs, std::vector<double> & outputs);
94 void initialize_motor_targets_from_defaults();
95 size_t write_commands_to_hardware(const std::vector<double> & joint_commands);
96 void apply_blend_in(std::vector<double> & joint_commands, double blend_factor);
97 std::vector<bool> apply_rate_limiting(
98 std::vector<double> & joint_commands, double control_period);
99 std::vector<bool> apply_position_limiting(std::vector<double> & joint_commands);
100
101 void validate_model_structure(size_t num_inputs, size_t num_outputs);
102
103 std::shared_ptr<ParamListener> param_listener_;
104 Params params_;
105
106 std::vector<std::string> joint_names_;
107 std::string model_path_;
108 int model_input_size_;
109 int model_output_size_;
110 std::string interfaces_broadcaster_topic_;
111 std::string interfaces_broadcaster_names_topic_;
112 std::string velocity_command_topic_;
113 std::string left_contact_sensor_name_{"left_foot_contact"};
114 std::string right_contact_sensor_name_{"right_foot_contact"};
115
116 std::unique_ptr<Ort::Session> onnx_session_;
117 std::unique_ptr<Ort::Env> onnx_env_;
118 std::unique_ptr<Ort::MemoryInfo> onnx_memory_info_;
119 std::vector<std::string> input_names_;
120 std::vector<std::string> output_names_;
121 std::vector<const char *> input_name_ptrs_;
122 std::vector<const char *> output_name_ptrs_;
123 std::vector<int64_t> input_shape_;
124 std::vector<int64_t> output_shape_;
125
126 // Topic inputs (non-realtime callbacks) -> realtime update loop handoff
127 rclcpp::Subscription<control_msgs::msg::Float64Values>::SharedPtr interface_data_subscriber_;
128 rclcpp::Subscription<control_msgs::msg::Keys>::SharedPtr interfaces_names_subscriber_;
129 rclcpp::Subscription<ros2_control_demo_example_18::msg::VelocityCommandWithHead>::SharedPtr
130 velocity_command_subscriber_;
131
135 rt_velocity_command_;
136 // The boxes start with default-constructed messages and try_get() only reports lock success,
137 // so these flags (set by the callbacks after storing) tell whether real data has arrived.
138 std::atomic<bool> interface_data_received_{false};
139 std::atomic<bool> velocity_command_received_{false};
140 ros2_control_demo_example_18::msg::VelocityCommandWithHead velocity_cmd_;
141
142 // Model/observation/action state
143 bool model_loaded_;
144 std::unique_ptr<ObservationFormatter> observation_formatter_;
145 std::unique_ptr<ActionProcessor> action_processor_;
146 std::vector<double> previous_action_;
147
148 // Command interfaces and command output state
149 std::vector<std::string> command_interface_names_;
150 std::vector<double> default_joint_positions_;
151 bool default_joint_positions_initialized_;
152 double max_motor_velocity_;
153 std::vector<double> joint_position_limits_min_;
154 std::vector<double> joint_position_limits_max_;
155 bool joint_position_limits_enabled_{false};
156 std::vector<double> motor_targets_;
157 std::vector<double> prev_motor_targets_;
158 bool prev_motor_targets_initialized_;
159
160 // Gait/timing configuration and counters
161 double phase_frequency_factor_offset_;
162 double num_steps_in_gait_period_;
163 double training_control_period_;
164 double gyro_deadband_;
165 int stabilization_delay_;
166 int blend_in_steps_;
167 size_t stabilization_steps_;
168 size_t onnx_active_steps_;
169 size_t update_count_;
170};
171
172} // namespace onnx_policy_controller
173
174#endif // ONNX_POLICY_CONTROLLER__ONNX_POLICY_CONTROLLER_HPP_
Definition controller_interface.hpp:27
Definition onnx_policy_controller.hpp:44
static std::vector< int64_t > resolve_input_shape(const std::vector< int64_t > &model_shape, size_t input_size)
Resolve dynamic (-1) dimensions of an ONNX input shape for a flat input buffer.
Definition onnx_policy_controller.cpp:746
static std::string format_shape_string(const std::vector< int64_t > &shape)
Format a tensor shape for logging, printing dynamic (-1) dimensions as "dynamic".
Definition onnx_policy_controller.cpp:791
controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition onnx_policy_controller.cpp:72
controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition onnx_policy_controller.cpp:46
controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition onnx_policy_controller.cpp:63
static std::vector< double > velocity_command_to_policy_input(const ros2_control_demo_example_18::msg::VelocityCommandWithHead &msg)
Convert a velocity command to the 7D command input of the policy.
Definition onnx_policy_controller.cpp:733
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Control step update. Command interfaces are updated based on on reference inputs and current states.
Definition onnx_policy_controller.cpp:283
Definition realtime_thread_safe_box.hpp:68
Configuring what command/state interfaces to claim.
Definition controller_interface_base.hpp:78