From 2468422c9b745e53e1ae6c5b8ef6167493e6f0fc Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Sun, 14 Jun 2026 21:44:53 -0700 Subject: [PATCH 1/4] Add steering wheel and vehicle control schemas --- src/core/python/deviceio_init.py | 4 + src/core/schema/fbs/steering_wheel.fbs | 39 ++++++++ src/core/schema/fbs/vehicle_control.fbs | 30 ++++++ src/core/schema/python/CMakeLists.txt | 2 + src/core/schema/python/schema_init.py | 14 +++ src/core/schema/python/schema_module.cpp | 8 ++ .../schema/python/steering_wheel_bindings.h | 92 +++++++++++++++++++ .../schema/python/vehicle_control_bindings.h | 69 ++++++++++++++ src/core/schema_tests/cpp/CMakeLists.txt | 2 + .../schema_tests/cpp/test_steering_wheel.cpp | 68 ++++++++++++++ .../schema_tests/cpp/test_vehicle_control.cpp | 64 +++++++++++++ .../schema_tests/python/test_vehicle_io.py | 71 ++++++++++++++ 12 files changed, 463 insertions(+) create mode 100644 src/core/schema/fbs/steering_wheel.fbs create mode 100644 src/core/schema/fbs/vehicle_control.fbs create mode 100644 src/core/schema/python/steering_wheel_bindings.h create mode 100644 src/core/schema/python/vehicle_control_bindings.h create mode 100644 src/core/schema_tests/cpp/test_steering_wheel.cpp create mode 100644 src/core/schema_tests/cpp/test_vehicle_control.cpp create mode 100644 src/core/schema_tests/python/test_vehicle_io.py diff --git a/src/core/python/deviceio_init.py b/src/core/python/deviceio_init.py index ea4a5aafe..40cce5f0e 100644 --- a/src/core/python/deviceio_init.py +++ b/src/core/python/deviceio_init.py @@ -17,6 +17,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + SteeringWheelTracker, FullBodyTrackerPico, NUM_JOINTS, JOINT_PALM, @@ -42,6 +43,7 @@ StreamType, FrameMetadataOak, Generic3AxisPedalOutput, + SteeringWheelOutput, ) __all__ = [ @@ -52,6 +54,7 @@ "StreamType", "FrameMetadataOak", "Generic3AxisPedalOutput", + "SteeringWheelOutput", "ITracker", "HandTracker", "HeadTracker", @@ -60,6 +63,7 @@ "MessageChannelTracker", "FrameMetadataTrackerOak", "Generic3AxisPedalTracker", + "SteeringWheelTracker", "FullBodyTrackerPico", "OpenXRSessionHandles", "DeviceIOSession", diff --git a/src/core/schema/fbs/steering_wheel.fbs b/src/core/schema/fbs/steering_wheel.fbs new file mode 100644 index 000000000..74b2165da --- /dev/null +++ b/src/core/schema/fbs/steering_wheel.fbs @@ -0,0 +1,39 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +// Output from a steering wheel and pedal set. +// Axis values are normalized joystick values in [-1, 1], and buttons are +// stored as 0/1 bytes. Retargeters own any device-specific pedal inversion or +// pressed-fraction conversion. +table SteeringWheelOutput { + steering: float (id: 0); + + throttle: float (id: 1); + + brake: float (id: 2); + + clutch: float (id: 3); + + buttons: [ubyte] (id: 4); + + hat_x: int (id: 5); + + hat_y: int (id: 6); +} + +// Tracked wrapper for the in-memory tracker API (data is null when no wheel data available). +table SteeringWheelOutputTracked { + data: SteeringWheelOutput (id: 0); +} + +// MCAP recording wrapper for SteeringWheelOutput. +table SteeringWheelOutputRecord { + data: SteeringWheelOutput (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type SteeringWheelOutputRecord; diff --git a/src/core/schema/fbs/vehicle_control.fbs b/src/core/schema/fbs/vehicle_control.fbs new file mode 100644 index 000000000..b7960d2f2 --- /dev/null +++ b/src/core/schema/fbs/vehicle_control.fbs @@ -0,0 +1,30 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +// Normalized vehicle command produced by retargeting operator input. +// Steering is in [-1, 1]. Accel is signed [-1, 1], where positive values request +// propulsion and negative values request braking. Throttle and brake are split +// non-negative [0, 1] values for vehicle backends that do not consume signed accel. +table VehicleControlCommand { + sequence: uint64 (id: 0); + + steer: float (id: 1); + + accel: float (id: 2); + + throttle: float (id: 3); + + brake: float (id: 4); +} + +// MCAP recording wrapper for VehicleControlCommand. +table VehicleControlCommandRecord { + data: VehicleControlCommand (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type VehicleControlCommandRecord; diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index d948e1417..30da2d578 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -11,6 +11,8 @@ pybind11_add_module(schema_py pedals_bindings.h pose_bindings.h schema_module.cpp + steering_wheel_bindings.h + vehicle_control_bindings.h ) target_link_libraries(schema_py diff --git a/src/core/schema/python/schema_init.py b/src/core/schema/python/schema_init.py index 3f3aeb108..a69e94a75 100644 --- a/src/core/schema/python/schema_init.py +++ b/src/core/schema/python/schema_init.py @@ -35,6 +35,13 @@ Generic3AxisPedalOutput, Generic3AxisPedalOutputTrackedT, Generic3AxisPedalOutputRecord, + # Steering wheel types. + SteeringWheelOutput, + SteeringWheelOutputTrackedT, + SteeringWheelOutputRecord, + # Vehicle control types. + VehicleControlCommand, + VehicleControlCommandRecord, # Message channel types. MessageChannelMessages, MessageChannelMessagesTrackedT, @@ -82,6 +89,13 @@ "Generic3AxisPedalOutput", "Generic3AxisPedalOutputTrackedT", "Generic3AxisPedalOutputRecord", + # Steering wheel types. + "SteeringWheelOutput", + "SteeringWheelOutputTrackedT", + "SteeringWheelOutputRecord", + # Vehicle control types. + "VehicleControlCommand", + "VehicleControlCommandRecord", # Message channel types. "MessageChannelMessages", "MessageChannelMessagesTrackedT", diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index e20dae586..f33c7aa56 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -14,7 +14,9 @@ #include "oak_bindings.h" #include "pedals_bindings.h" #include "pose_bindings.h" +#include "steering_wheel_bindings.h" #include "timestamp_bindings.h" +#include "vehicle_control_bindings.h" namespace py = pybind11; @@ -40,6 +42,12 @@ PYBIND11_MODULE(_schema, m) // Bind pedals types (Generic3AxisPedalOutput table). core::bind_pedals(m); + // Bind steering wheel types (SteeringWheelOutput table). + core::bind_steering_wheel(m); + + // Bind vehicle control types (VehicleControlCommand table). + core::bind_vehicle_control(m); + // Bind message channel types (MessageChannelMessages table). core::bind_message_channel(m); diff --git a/src/core/schema/python/steering_wheel_bindings.h b/src/core/schema/python/steering_wheel_bindings.h new file mode 100644 index 000000000..3b0f8a1a1 --- /dev/null +++ b/src/core/schema/python/steering_wheel_bindings.h @@ -0,0 +1,92 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the SteeringWheel FlatBuffer schema. + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_steering_wheel(py::module& m) +{ + py::class_>(m, "SteeringWheelOutput") + .def(py::init([]() { return std::make_shared(); })) + .def(py::init( + [](float steering, float throttle, float brake, float clutch, std::vector buttons, int hat_x, + int hat_y) + { + auto obj = std::make_shared(); + obj->steering = steering; + obj->throttle = throttle; + obj->brake = brake; + obj->clutch = clutch; + obj->buttons = std::move(buttons); + obj->hat_x = hat_x; + obj->hat_y = hat_y; + return obj; + }), + py::arg("steering"), py::arg("throttle"), py::arg("brake"), py::arg("clutch"), + py::arg("buttons") = std::vector{}, py::arg("hat_x") = 0, py::arg("hat_y") = 0) + .def_readwrite("steering", &SteeringWheelOutputT::steering) + .def_readwrite("throttle", &SteeringWheelOutputT::throttle) + .def_readwrite("brake", &SteeringWheelOutputT::brake) + .def_readwrite("clutch", &SteeringWheelOutputT::clutch) + .def_readwrite("buttons", &SteeringWheelOutputT::buttons) + .def_readwrite("hat_x", &SteeringWheelOutputT::hat_x) + .def_readwrite("hat_y", &SteeringWheelOutputT::hat_y) + .def("__repr__", + [](const SteeringWheelOutputT& output) + { + return "SteeringWheelOutput(steering=" + std::to_string(output.steering) + + ", throttle=" + std::to_string(output.throttle) + ", brake=" + std::to_string(output.brake) + + ", clutch=" + std::to_string(output.clutch) + + ", buttons=" + std::to_string(output.buttons.size()) + + ", hat_x=" + std::to_string(output.hat_x) + ", hat_y=" + std::to_string(output.hat_y) + ")"; + }); + + py::class_>(m, "SteeringWheelOutputRecord") + .def(py::init<>()) + .def(py::init( + [](const SteeringWheelOutputT& data, const DeviceDataTimestamp& timestamp) + { + auto obj = std::make_shared(); + obj->data = std::make_shared(data); + obj->timestamp = std::make_shared(timestamp); + return obj; + }), + py::arg("data"), py::arg("timestamp")) + .def_property_readonly("data", + [](const SteeringWheelOutputRecordT& self) -> std::shared_ptr + { return self.data; }) + .def_readonly("timestamp", &SteeringWheelOutputRecordT::timestamp); + + py::class_>(m, "SteeringWheelOutputTrackedT") + .def(py::init<>()) + .def(py::init( + [](const SteeringWheelOutputT& data) + { + auto obj = std::make_shared(); + obj->data = std::make_shared(data); + return obj; + }), + py::arg("data")) + .def_property_readonly("data", + [](const SteeringWheelOutputTrackedT& self) -> std::shared_ptr + { return self.data; }); +} + +} // namespace core diff --git a/src/core/schema/python/vehicle_control_bindings.h b/src/core/schema/python/vehicle_control_bindings.h new file mode 100644 index 000000000..13cef82ad --- /dev/null +++ b/src/core/schema/python/vehicle_control_bindings.h @@ -0,0 +1,69 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the VehicleControl FlatBuffer schema. + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_vehicle_control(py::module& m) +{ + py::class_>(m, "VehicleControlCommand") + .def(py::init([]() { return std::make_shared(); })) + .def(py::init( + [](uint64_t sequence, float steer, float accel, float throttle, float brake) + { + auto obj = std::make_shared(); + obj->sequence = sequence; + obj->steer = steer; + obj->accel = accel; + obj->throttle = throttle; + obj->brake = brake; + return obj; + }), + py::arg("sequence"), py::arg("steer"), py::arg("accel"), py::arg("throttle"), py::arg("brake")) + .def_readwrite("sequence", &VehicleControlCommandT::sequence) + .def_readwrite("steer", &VehicleControlCommandT::steer) + .def_readwrite("accel", &VehicleControlCommandT::accel) + .def_readwrite("throttle", &VehicleControlCommandT::throttle) + .def_readwrite("brake", &VehicleControlCommandT::brake) + .def("__repr__", + [](const VehicleControlCommandT& command) + { + return "VehicleControlCommand(sequence=" + std::to_string(command.sequence) + + ", steer=" + std::to_string(command.steer) + ", accel=" + std::to_string(command.accel) + + ", throttle=" + std::to_string(command.throttle) + ", brake=" + std::to_string(command.brake) + + ")"; + }); + + py::class_>( + m, "VehicleControlCommandRecord") + .def(py::init<>()) + .def(py::init( + [](const VehicleControlCommandT& data, const DeviceDataTimestamp& timestamp) + { + auto obj = std::make_shared(); + obj->data = std::make_shared(data); + obj->timestamp = std::make_shared(timestamp); + return obj; + }), + py::arg("data"), py::arg("timestamp")) + .def_property_readonly("data", + [](const VehicleControlCommandRecordT& self) -> std::shared_ptr + { return self.data; }) + .def_readonly("timestamp", &VehicleControlCommandRecordT::timestamp); +} + +} // namespace core diff --git a/src/core/schema_tests/cpp/CMakeLists.txt b/src/core/schema_tests/cpp/CMakeLists.txt index f8815306d..4451b45db 100644 --- a/src/core/schema_tests/cpp/CMakeLists.txt +++ b/src/core/schema_tests/cpp/CMakeLists.txt @@ -11,6 +11,8 @@ add_executable(schema_tests test_full_body.cpp test_controller.cpp test_pedals.cpp + test_steering_wheel.cpp + test_vehicle_control.cpp ) target_link_libraries(schema_tests PRIVATE isaacteleop_schema diff --git a/src/core/schema_tests/cpp/test_steering_wheel.cpp b/src/core/schema_tests/cpp/test_steering_wheel.cpp new file mode 100644 index 000000000..69b4ad268 --- /dev/null +++ b/src/core/schema_tests/cpp/test_steering_wheel.cpp @@ -0,0 +1,68 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for the generated SteeringWheel FlatBuffer types. + +#include +#include +#include +#include + +#define VT(field) (field + 2) * 2 + +static_assert(core::SteeringWheelOutput::VT_STEERING == VT(0)); +static_assert(core::SteeringWheelOutput::VT_THROTTLE == VT(1)); +static_assert(core::SteeringWheelOutput::VT_BRAKE == VT(2)); +static_assert(core::SteeringWheelOutput::VT_CLUTCH == VT(3)); +static_assert(core::SteeringWheelOutput::VT_BUTTONS == VT(4)); +static_assert(core::SteeringWheelOutput::VT_HAT_X == VT(5)); +static_assert(core::SteeringWheelOutput::VT_HAT_Y == VT(6)); + +static_assert(core::SteeringWheelOutputRecord::VT_DATA == VT(0)); +static_assert(core::SteeringWheelOutputRecord::VT_TIMESTAMP == VT(1)); + +TEST_CASE("SteeringWheelOutputT default construction", "[steering_wheel][native]") +{ + core::SteeringWheelOutputT output; + + CHECK(output.steering == 0.0f); + CHECK(output.throttle == 0.0f); + CHECK(output.brake == 0.0f); + CHECK(output.clutch == 0.0f); + CHECK(output.buttons.empty()); + CHECK(output.hat_x == 0); + CHECK(output.hat_y == 0); +} + +TEST_CASE("SteeringWheelOutput serialization and deserialization", "[steering_wheel][serialize]") +{ + flatbuffers::FlatBufferBuilder builder; + + core::SteeringWheelOutputT output; + output.steering = -0.25f; + output.throttle = 0.8f; + output.brake = 0.1f; + output.clutch = 0.0f; + output.buttons = { 1, 0, 1 }; + output.hat_x = -1; + output.hat_y = 1; + + auto offset = core::SteeringWheelOutput::Pack(builder, &output); + builder.Finish(offset); + + const auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + CHECK(deserialized->steering() == Catch::Approx(-0.25f)); + CHECK(deserialized->throttle() == Catch::Approx(0.8f)); + CHECK(deserialized->brake() == Catch::Approx(0.1f)); + CHECK(deserialized->buttons()->size() == 3); + CHECK(deserialized->buttons()->Get(0) == 1); + CHECK(deserialized->hat_x() == -1); + CHECK(deserialized->hat_y() == 1); +} + +TEST_CASE("SteeringWheelOutputTrackedT defaults to null data", "[steering_wheel][tracked]") +{ + core::SteeringWheelOutputTrackedT tracked; + + CHECK(tracked.data == nullptr); +} diff --git a/src/core/schema_tests/cpp/test_vehicle_control.cpp b/src/core/schema_tests/cpp/test_vehicle_control.cpp new file mode 100644 index 000000000..581069ef1 --- /dev/null +++ b/src/core/schema_tests/cpp/test_vehicle_control.cpp @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for the generated VehicleControl FlatBuffer types. + +#include +#include +#include +#include + +#define VT(field) (field + 2) * 2 + +static_assert(core::VehicleControlCommand::VT_SEQUENCE == VT(0)); +static_assert(core::VehicleControlCommand::VT_STEER == VT(1)); +static_assert(core::VehicleControlCommand::VT_ACCEL == VT(2)); +static_assert(core::VehicleControlCommand::VT_THROTTLE == VT(3)); +static_assert(core::VehicleControlCommand::VT_BRAKE == VT(4)); + +static_assert(core::VehicleControlCommandRecord::VT_DATA == VT(0)); +static_assert(core::VehicleControlCommandRecord::VT_TIMESTAMP == VT(1)); + +TEST_CASE("VehicleControlCommandT default construction", "[vehicle_control][native]") +{ + core::VehicleControlCommandT command; + + CHECK(command.sequence == 0); + CHECK(command.steer == 0.0f); + CHECK(command.accel == 0.0f); + CHECK(command.throttle == 0.0f); + CHECK(command.brake == 0.0f); +} + +TEST_CASE("VehicleControlCommand serialization and deserialization", "[vehicle_control][serialize]") +{ + flatbuffers::FlatBufferBuilder builder; + + core::VehicleControlCommandT command; + command.sequence = 42; + command.steer = -0.5f; + command.accel = 0.75f; + command.throttle = 0.75f; + command.brake = 0.0f; + + auto offset = core::VehicleControlCommand::Pack(builder, &command); + builder.Finish(offset); + + const auto* deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + CHECK(deserialized->sequence() == 42); + CHECK(deserialized->steer() == Catch::Approx(-0.5f)); + CHECK(deserialized->accel() == Catch::Approx(0.75f)); + CHECK(deserialized->throttle() == Catch::Approx(0.75f)); + CHECK(deserialized->brake() == Catch::Approx(0.0f)); +} + +TEST_CASE("VehicleControlCommandRecord can omit data", "[vehicle_control][record]") +{ + flatbuffers::FlatBufferBuilder builder; + + core::VehicleControlCommandRecordBuilder record_builder(builder); + builder.Finish(record_builder.Finish()); + + const auto* record = flatbuffers::GetRoot(builder.GetBufferPointer()); + CHECK(record->data() == nullptr); +} diff --git a/src/core/schema_tests/python/test_vehicle_io.py b/src/core/schema_tests/python/test_vehicle_io.py new file mode 100644 index 000000000..bad4f0b6c --- /dev/null +++ b/src/core/schema_tests/python/test_vehicle_io.py @@ -0,0 +1,71 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Unit tests for steering wheel and vehicle control schema bindings.""" + +import pytest + +from isaacteleop.schema import ( + DeviceDataTimestamp, + SteeringWheelOutput, + SteeringWheelOutputRecord, + SteeringWheelOutputTrackedT, + VehicleControlCommand, + VehicleControlCommandRecord, +) + + +def test_steering_wheel_output_defaults(): + output = SteeringWheelOutput() + + assert output.steering == 0.0 + assert output.throttle == 0.0 + assert output.brake == 0.0 + assert output.clutch == 0.0 + assert output.buttons == [] + assert output.hat_x == 0 + assert output.hat_y == 0 + + +def test_steering_wheel_output_constructs_with_values(): + output = SteeringWheelOutput(-0.25, 0.8, 0.1, 0.0, [1, 0, 1], -1, 1) + + assert output.steering == pytest.approx(-0.25) + assert output.throttle == pytest.approx(0.8) + assert output.brake == pytest.approx(0.1) + assert output.buttons == [1, 0, 1] + assert output.hat_x == -1 + assert output.hat_y == 1 + + +def test_steering_wheel_wrappers_hold_data(): + output = SteeringWheelOutput(0.0, 1.0, 0.0, 0.0) + timestamp = DeviceDataTimestamp(10, 20, 30) + + record = SteeringWheelOutputRecord(output, timestamp) + tracked = SteeringWheelOutputTrackedT(output) + + assert record.data.throttle == pytest.approx(1.0) + assert tracked.data.throttle == pytest.approx(1.0) + assert record.timestamp.sample_time_local_common_clock == 20 + + +def test_vehicle_control_command_constructs_with_values(): + command = VehicleControlCommand(42, -0.5, 0.75, 0.75, 0.0) + + assert command.sequence == 42 + assert command.steer == pytest.approx(-0.5) + assert command.accel == pytest.approx(0.75) + assert command.throttle == pytest.approx(0.75) + assert command.brake == pytest.approx(0.0) + + +def test_vehicle_control_record_holds_data(): + command = VehicleControlCommand(7, 0.25, -0.5, 0.0, 0.5) + timestamp = DeviceDataTimestamp(10, 20, 30) + + record = VehicleControlCommandRecord(command, timestamp) + + assert record.data.sequence == 7 + assert record.data.brake == pytest.approx(0.5) + assert record.timestamp.sample_time_raw_device_clock == 30 From ef153f98828b54a9542e7b86cf55e6222634258b Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Sun, 14 Jun 2026 21:45:13 -0700 Subject: [PATCH 2/4] Add DeviceIO steering wheel tracker --- .../steering_wheel_tracker_base.hpp | 20 +++++++ src/core/deviceio_trackers/cpp/CMakeLists.txt | 2 + .../steering_wheel_tracker.hpp | 54 +++++++++++++++++ .../cpp/steering_wheel_tracker.cpp | 19 ++++++ .../python/deviceio_trackers_init.py | 2 + .../python/tracker_bindings.cpp | 13 +++++ src/core/live_trackers/cpp/CMakeLists.txt | 2 + .../live_trackers/live_deviceio_factory.hpp | 3 + .../cpp/live_deviceio_factory.cpp | 20 +++++++ .../cpp/live_steering_wheel_tracker_impl.cpp | 58 +++++++++++++++++++ .../cpp/live_steering_wheel_tracker_impl.hpp | 52 +++++++++++++++++ .../mcap/cpp/inc/mcap/recording_traits.hpp | 7 +++ src/core/replay_trackers/cpp/CMakeLists.txt | 2 + .../replay_deviceio_factory.hpp | 3 + .../cpp/replay_deviceio_factory.cpp | 23 +++++++- .../replay_steering_wheel_tracker_impl.cpp | 44 ++++++++++++++ .../replay_steering_wheel_tracker_impl.hpp | 37 ++++++++++++ 17 files changed, 359 insertions(+), 2 deletions(-) create mode 100644 src/core/deviceio_base/cpp/inc/deviceio_base/steering_wheel_tracker_base.hpp create mode 100644 src/core/deviceio_trackers/cpp/inc/deviceio_trackers/steering_wheel_tracker.hpp create mode 100644 src/core/deviceio_trackers/cpp/steering_wheel_tracker.cpp create mode 100644 src/core/live_trackers/cpp/live_steering_wheel_tracker_impl.cpp create mode 100644 src/core/live_trackers/cpp/live_steering_wheel_tracker_impl.hpp create mode 100644 src/core/replay_trackers/cpp/replay_steering_wheel_tracker_impl.cpp create mode 100644 src/core/replay_trackers/cpp/replay_steering_wheel_tracker_impl.hpp diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/steering_wheel_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/steering_wheel_tracker_base.hpp new file mode 100644 index 000000000..3016ddcff --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/steering_wheel_tracker_base.hpp @@ -0,0 +1,20 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +namespace core +{ + +struct SteeringWheelOutputTrackedT; + +// Abstract base interface for SteeringWheelTracker implementations. +class ISteeringWheelTrackerImpl : public ITrackerImpl +{ +public: + virtual const SteeringWheelOutputTrackedT& get_data() const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/CMakeLists.txt b/src/core/deviceio_trackers/cpp/CMakeLists.txt index 48b460d71..a204c52bf 100644 --- a/src/core/deviceio_trackers/cpp/CMakeLists.txt +++ b/src/core/deviceio_trackers/cpp/CMakeLists.txt @@ -10,6 +10,7 @@ add_library(deviceio_trackers STATIC controller_tracker.cpp message_channel_tracker.cpp generic_3axis_pedal_tracker.cpp + steering_wheel_tracker.cpp frame_metadata_tracker_oak.cpp full_body_tracker_pico.cpp inc/deviceio_trackers/head_tracker.hpp @@ -18,6 +19,7 @@ add_library(deviceio_trackers STATIC inc/deviceio_trackers/message_channel_tracker.hpp inc/deviceio_trackers/full_body_tracker_pico.hpp inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp + inc/deviceio_trackers/steering_wheel_tracker.hpp inc/deviceio_trackers/frame_metadata_tracker_oak.hpp ) diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/steering_wheel_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/steering_wheel_tracker.hpp new file mode 100644 index 000000000..fbad907a0 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/steering_wheel_tracker.hpp @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include + +namespace core +{ + +/*! + * @brief Facade for steering wheel state exposed as ``SteeringWheelOutputTrackedT``. + * + * ``SteeringWheelOutput`` uses normalized joystick axis values independent of a specific wheel model. + * Producers should map raw device axes into [-1, 1] before publishing. + */ +class SteeringWheelTracker : public ITracker +{ +public: + //! Default maximum FlatBuffer size for SteeringWheelOutput messages. + static constexpr size_t DEFAULT_MAX_FLATBUFFER_SIZE = 1024; + + explicit SteeringWheelTracker(const std::string& collection_id, + size_t max_flatbuffer_size = DEFAULT_MAX_FLATBUFFER_SIZE); + + std::string_view get_name() const override + { + return TRACKER_NAME; + } + + const SteeringWheelOutputTrackedT& get_data(const ITrackerSession& session) const; + + const std::string& collection_id() const + { + return collection_id_; + } + + size_t max_flatbuffer_size() const + { + return max_flatbuffer_size_; + } + +private: + static constexpr const char* TRACKER_NAME = "SteeringWheelTracker"; + + std::string collection_id_; + size_t max_flatbuffer_size_; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/steering_wheel_tracker.cpp b/src/core/deviceio_trackers/cpp/steering_wheel_tracker.cpp new file mode 100644 index 000000000..b7fbb9637 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/steering_wheel_tracker.cpp @@ -0,0 +1,19 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/steering_wheel_tracker.hpp" + +namespace core +{ + +SteeringWheelTracker::SteeringWheelTracker(const std::string& collection_id, size_t max_flatbuffer_size) + : collection_id_(collection_id), max_flatbuffer_size_(max_flatbuffer_size) +{ +} + +const SteeringWheelOutputTrackedT& SteeringWheelTracker::get_data(const ITrackerSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_data(); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/python/deviceio_trackers_init.py b/src/core/deviceio_trackers/python/deviceio_trackers_init.py index f867e8f54..2b9eb9a32 100644 --- a/src/core/deviceio_trackers/python/deviceio_trackers_init.py +++ b/src/core/deviceio_trackers/python/deviceio_trackers_init.py @@ -12,6 +12,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + SteeringWheelTracker, FullBodyTrackerPico, ITrackerSession, NUM_JOINTS, @@ -28,6 +29,7 @@ "FrameMetadataTrackerOak", "FullBodyTrackerPico", "Generic3AxisPedalTracker", + "SteeringWheelTracker", "HandTracker", "HeadTracker", "ITracker", diff --git a/src/core/deviceio_trackers/python/tracker_bindings.cpp b/src/core/deviceio_trackers/python/tracker_bindings.cpp index 601c7db06..d330359c1 100644 --- a/src/core/deviceio_trackers/python/tracker_bindings.cpp +++ b/src/core/deviceio_trackers/python/tracker_bindings.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -150,6 +151,18 @@ PYBIND11_MODULE(_deviceio_trackers, m) { return self.get_data(session); }, py::arg("session"), "Get the current foot pedal tracked state (data is None when no data available)"); + py::class_>( + m, "SteeringWheelTracker") + .def(py::init(), py::arg("collection_id"), + py::arg("max_flatbuffer_size") = core::SteeringWheelTracker::DEFAULT_MAX_FLATBUFFER_SIZE, + "Construct a SteeringWheelTracker for the given tensor collection ID") + .def( + "get_wheel_data", + [](const core::SteeringWheelTracker& self, + const core::ITrackerSession& session) -> core::SteeringWheelOutputTrackedT + { return self.get_data(session); }, + py::arg("session"), "Get the current steering wheel tracked state (data is None when no data available)"); + py::class_>( m, "FullBodyTrackerPico") .def(py::init<>()) diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index 23d105b7d..b3d01f46b 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -12,6 +12,7 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.cpp live_full_body_tracker_pico_impl.cpp live_generic_3axis_pedal_tracker_impl.cpp + live_steering_wheel_tracker_impl.cpp live_frame_metadata_tracker_oak_impl.cpp inc/live_trackers/schema_tracker_base.hpp inc/live_trackers/schema_tracker.hpp @@ -22,6 +23,7 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.hpp live_full_body_tracker_pico_impl.hpp live_generic_3axis_pedal_tracker_impl.hpp + live_steering_wheel_tracker_impl.hpp live_frame_metadata_tracker_oak_impl.hpp ) diff --git a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp index 7d6b5c4f9..5307d1459 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp @@ -30,6 +30,8 @@ class FullBodyTrackerPico; class IFullBodyTrackerPicoImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; +class SteeringWheelTracker; +class ISteeringWheelTrackerImpl; class HandTracker; class IHandTrackerImpl; class HeadTracker; @@ -62,6 +64,7 @@ class LiveDeviceIOFactory std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); + std::unique_ptr create_steering_wheel_tracker_impl(const SteeringWheelTracker* tracker); std::unique_ptr create_frame_metadata_tracker_oak_impl( const FrameMetadataTrackerOak* tracker); diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index 2c304480c..a84718674 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -10,6 +10,7 @@ #include "live_hand_tracker_impl.hpp" #include "live_head_tracker_impl.hpp" #include "live_message_channel_tracker_impl.hpp" +#include "live_steering_wheel_tracker_impl.hpp" #include #include @@ -18,6 +19,7 @@ #include #include #include +#include #include #include @@ -79,6 +81,12 @@ std::unique_ptr try_create_generic_pedal_impl(LiveDeviceIOFactory& return typed ? factory.create_generic_3axis_pedal_tracker_impl(typed) : nullptr; } +std::unique_ptr try_create_steering_wheel_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_steering_wheel_tracker_impl(typed) : nullptr; +} + std::unique_ptr try_create_oak_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -102,6 +110,7 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { { &try_add_extensions, &try_create_message_channel_impl }, { &try_add_extensions, &try_create_full_body_pico_impl }, { &try_add_extensions, &try_create_generic_pedal_impl }, + { &try_add_extensions, &try_create_steering_wheel_impl }, { &try_add_extensions, &try_create_oak_impl }, }; @@ -244,6 +253,17 @@ std::unique_ptr LiveDeviceIOFactory::create_gener return std::make_unique(handles_, tracker, std::move(channels)); } +std::unique_ptr LiveDeviceIOFactory::create_steering_wheel_tracker_impl( + const SteeringWheelTracker* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveSteeringWheelTrackerImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, tracker, std::move(channels)); +} + std::unique_ptr LiveDeviceIOFactory::create_frame_metadata_tracker_oak_impl( const FrameMetadataTrackerOak* tracker) { diff --git a/src/core/live_trackers/cpp/live_steering_wheel_tracker_impl.cpp b/src/core/live_trackers/cpp/live_steering_wheel_tracker_impl.cpp new file mode 100644 index 000000000..0af3eb111 --- /dev/null +++ b/src/core/live_trackers/cpp/live_steering_wheel_tracker_impl.cpp @@ -0,0 +1,58 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_steering_wheel_tracker_impl.hpp" + +#include +#include + +namespace core +{ + +namespace +{ + +SchemaTrackerConfig make_steering_wheel_tensor_config(const SteeringWheelTracker* tracker) +{ + SchemaTrackerConfig cfg; + cfg.collection_id = tracker->collection_id(); + cfg.max_flatbuffer_size = tracker->max_flatbuffer_size(); + cfg.tensor_identifier = "steering_wheel"; + cfg.localized_name = "SteeringWheelTracker"; + return cfg; +} + +} // namespace + +std::unique_ptr LiveSteeringWheelTrackerImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique( + writer, base_name, SteeringWheelRecordingTraits::schema_name, + std::vector(SteeringWheelRecordingTraits::recording_channels.begin(), + SteeringWheelRecordingTraits::recording_channels.end())); +} + +LiveSteeringWheelTrackerImpl::LiveSteeringWheelTrackerImpl(const OpenXRSessionHandles& handles, + const SteeringWheelTracker* tracker, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + m_schema_reader(handles, + make_steering_wheel_tensor_config(tracker), + mcap_channels_.get(), + /*mcap_channel_index=*/0, + /*mcap_channel_tracked_index=*/1) +{ +} + +void LiveSteeringWheelTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + m_schema_reader.update(m_tracked.data); +} + +const SteeringWheelOutputTrackedT& LiveSteeringWheelTrackerImpl::get_data() const +{ + return m_tracked; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_steering_wheel_tracker_impl.hpp b/src/core/live_trackers/cpp/live_steering_wheel_tracker_impl.hpp new file mode 100644 index 000000000..623442ff1 --- /dev/null +++ b/src/core/live_trackers/cpp/live_steering_wheel_tracker_impl.hpp @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/live_trackers/schema_tracker.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ + +using SteeringWheelMcapChannels = McapTrackerChannels; +using SteeringWheelSchemaTracker = SchemaTracker; + +class LiveSteeringWheelTrackerImpl : public ISteeringWheelTrackerImpl +{ +public: + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name); + + LiveSteeringWheelTrackerImpl(const OpenXRSessionHandles& handles, + const SteeringWheelTracker* tracker, + std::unique_ptr mcap_channels); + + LiveSteeringWheelTrackerImpl(const LiveSteeringWheelTrackerImpl&) = delete; + LiveSteeringWheelTrackerImpl& operator=(const LiveSteeringWheelTrackerImpl&) = delete; + LiveSteeringWheelTrackerImpl(LiveSteeringWheelTrackerImpl&&) = delete; + LiveSteeringWheelTrackerImpl& operator=(LiveSteeringWheelTrackerImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const SteeringWheelOutputTrackedT& get_data() const override; + +private: + std::unique_ptr mcap_channels_; + SteeringWheelSchemaTracker m_schema_reader; + SteeringWheelOutputTrackedT m_tracked; +}; + +} // namespace core diff --git a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp index 8eb960396..9432d081d 100644 --- a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp +++ b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp @@ -52,6 +52,13 @@ struct PedalRecordingTraits static constexpr std::array replay_channels = { "pedals_tracked" }; }; +struct SteeringWheelRecordingTraits +{ + static constexpr std::string_view schema_name = "core.SteeringWheelOutputRecord"; + static constexpr std::array recording_channels = { "steering_wheel", "steering_wheel_tracked" }; + static constexpr std::array replay_channels = { "steering_wheel_tracked" }; +}; + struct OakRecordingTraits { static constexpr std::string_view schema_name = "core.FrameMetadataOakRecord"; diff --git a/src/core/replay_trackers/cpp/CMakeLists.txt b/src/core/replay_trackers/cpp/CMakeLists.txt index 3647af299..eb4dcb799 100644 --- a/src/core/replay_trackers/cpp/CMakeLists.txt +++ b/src/core/replay_trackers/cpp/CMakeLists.txt @@ -10,6 +10,7 @@ add_library(replay_trackers STATIC replay_controller_tracker_impl.cpp replay_full_body_tracker_pico_impl.cpp replay_generic_3axis_pedal_tracker_impl.cpp + replay_steering_wheel_tracker_impl.cpp replay_message_channel_tracker_impl.cpp inc/replay_trackers/replay_deviceio_factory.hpp replay_hand_tracker_impl.hpp @@ -17,6 +18,7 @@ add_library(replay_trackers STATIC replay_controller_tracker_impl.hpp replay_full_body_tracker_pico_impl.hpp replay_generic_3axis_pedal_tracker_impl.hpp + replay_steering_wheel_tracker_impl.hpp replay_message_channel_tracker_impl.hpp ) diff --git a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp index c8272babb..93847b42b 100644 --- a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp +++ b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp @@ -21,6 +21,8 @@ class FullBodyTrackerPico; class IFullBodyTrackerPicoImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; +class SteeringWheelTracker; +class ISteeringWheelTrackerImpl; class HandTracker; class IHandTrackerImpl; class HeadTracker; @@ -50,6 +52,7 @@ class ReplayDeviceIOFactory std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); + std::unique_ptr create_steering_wheel_tracker_impl(const SteeringWheelTracker* tracker); std::unique_ptr create_message_channel_tracker_impl(const MessageChannelTracker* tracker); private: diff --git a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp index a3d6c3b6a..e980f2359 100644 --- a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp +++ b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp @@ -9,6 +9,7 @@ #include "replay_hand_tracker_impl.hpp" #include "replay_head_tracker_impl.hpp" #include "replay_message_channel_tracker_impl.hpp" +#include "replay_steering_wheel_tracker_impl.hpp" #include #include @@ -16,6 +17,7 @@ #include #include #include +#include #include #include @@ -71,6 +73,12 @@ std::unique_ptr try_create_generic_pedal_impl(ReplayDeviceIOFactor return typed ? factory.create_generic_3axis_pedal_tracker_impl(typed) : nullptr; } +std::unique_ptr try_create_steering_wheel_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_steering_wheel_tracker_impl(typed) : nullptr; +} + std::unique_ptr try_create_message_channel_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -80,8 +88,13 @@ std::unique_ptr try_create_message_channel_impl(ReplayDeviceIOFact using TryCreateFn = std::unique_ptr (*)(ReplayDeviceIOFactory&, const ITracker&); inline const TryCreateFn k_tracker_dispatch[] = { - &try_create_head_impl, &try_create_hand_impl, &try_create_controller_impl, - &try_create_full_body_pico_impl, &try_create_generic_pedal_impl, &try_create_message_channel_impl, + &try_create_head_impl, + &try_create_hand_impl, + &try_create_controller_impl, + &try_create_full_body_pico_impl, + &try_create_generic_pedal_impl, + &try_create_steering_wheel_impl, + &try_create_message_channel_impl, }; } // namespace @@ -148,6 +161,12 @@ std::unique_ptr ReplayDeviceIOFactory::create_gen return std::make_unique(open_reader(filename_), get_name(tracker)); } +std::unique_ptr ReplayDeviceIOFactory::create_steering_wheel_tracker_impl( + const SteeringWheelTracker* tracker) +{ + return std::make_unique(open_reader(filename_), get_name(tracker)); +} + std::unique_ptr ReplayDeviceIOFactory::create_message_channel_tracker_impl( const MessageChannelTracker* tracker) { diff --git a/src/core/replay_trackers/cpp/replay_steering_wheel_tracker_impl.cpp b/src/core/replay_trackers/cpp/replay_steering_wheel_tracker_impl.cpp new file mode 100644 index 000000000..77e76b061 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_steering_wheel_tracker_impl.cpp @@ -0,0 +1,44 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "replay_steering_wheel_tracker_impl.hpp" + +#include +#include +#include + +#include + +namespace core +{ + +ReplaySteeringWheelTrackerImpl::ReplaySteeringWheelTrackerImpl(std::unique_ptr reader, + std::string_view base_name) + : mcap_viewers_(std::make_unique( + std::move(reader), + base_name, + std::vector(SteeringWheelRecordingTraits::replay_channels.begin(), + SteeringWheelRecordingTraits::replay_channels.end()))) +{ +} + +const SteeringWheelOutputTrackedT& ReplaySteeringWheelTrackerImpl::get_data() const +{ + return tracked_; +} + +void ReplaySteeringWheelTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + auto record = mcap_viewers_->read(0); + if (record) + { + tracked_.data = std::move(record->data); + } + else + { + std::cerr << "ReplaySteeringWheelTrackerImpl: steering wheel data not found" << std::endl; + tracked_.data.reset(); + } +} + +} // namespace core diff --git a/src/core/replay_trackers/cpp/replay_steering_wheel_tracker_impl.hpp b/src/core/replay_trackers/cpp/replay_steering_wheel_tracker_impl.hpp new file mode 100644 index 000000000..68959f00c --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_steering_wheel_tracker_impl.hpp @@ -0,0 +1,37 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace core +{ + +using SteeringWheelMcapViewers = McapTrackerViewers; + +class ReplaySteeringWheelTrackerImpl : public ISteeringWheelTrackerImpl +{ +public: + ReplaySteeringWheelTrackerImpl(std::unique_ptr reader, std::string_view base_name); + + ReplaySteeringWheelTrackerImpl(const ReplaySteeringWheelTrackerImpl&) = delete; + ReplaySteeringWheelTrackerImpl& operator=(const ReplaySteeringWheelTrackerImpl&) = delete; + ReplaySteeringWheelTrackerImpl(ReplaySteeringWheelTrackerImpl&&) = delete; + ReplaySteeringWheelTrackerImpl& operator=(ReplaySteeringWheelTrackerImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const SteeringWheelOutputTrackedT& get_data() const override; + +private: + SteeringWheelOutputTrackedT tracked_; + std::unique_ptr mcap_viewers_; +}; + +} // namespace core From bb10204c308b59780868c17cf4297657933c797b Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Sun, 14 Jun 2026 21:45:28 -0700 Subject: [PATCH 3/4] Add vehicle control retargeter --- .../python/deviceio_source_nodes/__init__.py | 6 ++ .../deviceio_tensor_types.py | 29 ++++++ .../steering_wheel_source.py | 70 +++++++++++++++ .../python/tensor_types/__init__.py | 4 + .../python/tensor_types/indices.py | 4 + .../python/tensor_types/standard_types.py | 20 +++++ src/retargeters/__init__.py | 16 ++++ src/retargeters/vehicle_control_retargeter.py | 89 +++++++++++++++++++ 8 files changed, 238 insertions(+) create mode 100644 src/core/retargeting_engine/python/deviceio_source_nodes/steering_wheel_source.py create mode 100644 src/retargeters/vehicle_control_retargeter.py diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py index 4945cbe97..1defff4da 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py @@ -9,6 +9,7 @@ from .hands_source import HandsSource from .controllers_source import ControllersSource from .pedals_source import Generic3AxisPedalSource +from .steering_wheel_source import SteeringWheelSource from .full_body_source import FullBodySource from .message_channel_source import MessageChannelSource from .message_channel_sink import MessageChannelSink @@ -23,11 +24,13 @@ HandPoseTrackedType, ControllerSnapshotTrackedType, Generic3AxisPedalOutputTrackedType, + SteeringWheelOutputTrackedType, FullBodyPosePicoTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, DeviceIOGeneric3AxisPedalOutputTracked, + DeviceIOSteeringWheelOutputTracked, DeviceIOFullBodyPosePicoTracked, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, @@ -44,6 +47,7 @@ "HandsSource", "ControllersSource", "Generic3AxisPedalSource", + "SteeringWheelSource", "FullBodySource", "MessageChannelSource", "MessageChannelSink", @@ -55,6 +59,7 @@ "HandPoseTrackedType", "ControllerSnapshotTrackedType", "Generic3AxisPedalOutputTrackedType", + "SteeringWheelOutputTrackedType", "FullBodyPosePicoTrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", @@ -63,6 +68,7 @@ "DeviceIOHandPoseTracked", "DeviceIOControllerSnapshotTracked", "DeviceIOGeneric3AxisPedalOutputTracked", + "DeviceIOSteeringWheelOutputTracked", "DeviceIOFullBodyPosePicoTracked", "DeviceIOMessageChannelMessagesTracked", "MessageChannelMessagesTrackedGroup", diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py b/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py index 77aaa4e4f..ad590cacc 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py @@ -18,6 +18,7 @@ HandPoseTrackedT, ControllerSnapshotTrackedT, Generic3AxisPedalOutputTrackedT, + SteeringWheelOutputTrackedT, FullBodyPosePicoTrackedT, MessageChannelMessagesTrackedT, ) @@ -99,6 +100,26 @@ def validate_value(self, value: Any) -> None: ) +class SteeringWheelOutputTrackedType(TensorType): + """SteeringWheelOutputTrackedT wrapper type from DeviceIO SteeringWheelTracker.""" + + def __init__(self, name: str) -> None: + super().__init__(name) + + def _check_instance_compatibility(self, other: TensorType) -> bool: + if not isinstance(other, SteeringWheelOutputTrackedType): + raise TypeError( + f"Expected SteeringWheelOutputTrackedType, got {type(other).__name__}" + ) + return True + + def validate_value(self, value: Any) -> None: + if not isinstance(value, SteeringWheelOutputTrackedT): + raise TypeError( + f"Expected SteeringWheelOutputTrackedT for '{self.name}', got {type(value).__name__}" + ) + + class FullBodyPosePicoTrackedType(TensorType): """FullBodyPosePicoTrackedT wrapper type from DeviceIO FullBodyTrackerPico.""" @@ -211,6 +232,14 @@ def DeviceIOGeneric3AxisPedalOutputTracked() -> TensorGroupType: ) +def DeviceIOSteeringWheelOutputTracked() -> TensorGroupType: + """Tracked steering wheel data from DeviceIO SteeringWheelTracker.""" + return TensorGroupType( + "deviceio_steering_wheel_output", + [SteeringWheelOutputTrackedType("steering_wheel_tracked")], + ) + + def DeviceIOFullBodyPosePicoTracked() -> TensorGroupType: """Tracked full body pose data from DeviceIO FullBodyTrackerPico. diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/steering_wheel_source.py b/src/core/retargeting_engine/python/deviceio_source_nodes/steering_wheel_source.py new file mode 100644 index 000000000..f3eaa4f93 --- /dev/null +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/steering_wheel_source.py @@ -0,0 +1,70 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Steering wheel source node - DeviceIO to retargeting engine converter.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +from ..interface.retargeter_core_types import RetargeterIO, RetargeterIOType +from ..interface.tensor_group import TensorGroup +from ..interface.tensor_group_type import OptionalType +from ..tensor_types import SteeringWheelInput, SteeringWheelInputIndex +from .deviceio_tensor_types import DeviceIOSteeringWheelOutputTracked +from .interface import IDeviceIOSource + +if TYPE_CHECKING: + from isaacteleop.deviceio import ITracker + from isaacteleop.schema import SteeringWheelOutput, SteeringWheelOutputTrackedT + + +DEFAULT_STEERING_WHEEL_COLLECTION_ID = "steering_wheel" + + +class SteeringWheelSource(IDeviceIOSource): + """Stateless converter: DeviceIO SteeringWheelOutput -> SteeringWheelInput tensors.""" + + def __init__( + self, name: str, collection_id: str = DEFAULT_STEERING_WHEEL_COLLECTION_ID + ) -> None: + import isaacteleop.deviceio as deviceio + + self._steering_wheel_tracker = deviceio.SteeringWheelTracker(collection_id) + self._collection_id = collection_id + super().__init__(name) + + def get_tracker(self) -> "ITracker": + return self._steering_wheel_tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + tracked = self._steering_wheel_tracker.get_wheel_data(deviceio_session) + tg = TensorGroup(DeviceIOSteeringWheelOutputTracked()) + tg[0] = tracked + return {"deviceio_steering_wheel": tg} + + def input_spec(self) -> RetargeterIOType: + return { + "deviceio_steering_wheel": DeviceIOSteeringWheelOutputTracked(), + } + + def output_spec(self) -> RetargeterIOType: + return { + "steering_wheel": OptionalType(SteeringWheelInput()), + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + tracked: "SteeringWheelOutputTrackedT" = inputs["deviceio_steering_wheel"][0] + wheel: SteeringWheelOutput | None = tracked.data + + out = outputs["steering_wheel"] + if wheel is None: + out.set_none() + return + + out[SteeringWheelInputIndex.STEERING] = float(wheel.steering) + out[SteeringWheelInputIndex.THROTTLE] = float(wheel.throttle) + out[SteeringWheelInputIndex.BRAKE] = float(wheel.brake) + out[SteeringWheelInputIndex.CLUTCH] = float(wheel.clutch) + out[SteeringWheelInputIndex.HAT_X] = float(wheel.hat_x) + out[SteeringWheelInputIndex.HAT_Y] = float(wheel.hat_y) diff --git a/src/core/retargeting_engine/python/tensor_types/__init__.py b/src/core/retargeting_engine/python/tensor_types/__init__.py index da106fae6..a2c7e9adc 100644 --- a/src/core/retargeting_engine/python/tensor_types/__init__.py +++ b/src/core/retargeting_engine/python/tensor_types/__init__.py @@ -12,6 +12,7 @@ FullBodyInput, TransformMatrix, Generic3AxisPedalInput, + SteeringWheelInput, NUM_HAND_JOINTS, NUM_BODY_JOINTS_PICO, RobotHandJoints, @@ -29,6 +30,7 @@ HeadPoseIndex, ControllerInputIndex, Generic3AxisPedalInputIndex, + SteeringWheelInputIndex, FullBodyInputIndex, HandJointIndex, BodyJointPicoIndex, @@ -50,6 +52,7 @@ "FullBodyInput", "TransformMatrix", "Generic3AxisPedalInput", + "SteeringWheelInput", "NUM_HAND_JOINTS", "NUM_BODY_JOINTS_PICO", "RobotHandJoints", @@ -65,6 +68,7 @@ "HeadPoseIndex", "ControllerInputIndex", "Generic3AxisPedalInputIndex", + "SteeringWheelInputIndex", "FullBodyInputIndex", "HandJointIndex", "BodyJointPicoIndex", diff --git a/src/core/retargeting_engine/python/tensor_types/indices.py b/src/core/retargeting_engine/python/tensor_types/indices.py index 2fec98b92..dd762b928 100644 --- a/src/core/retargeting_engine/python/tensor_types/indices.py +++ b/src/core/retargeting_engine/python/tensor_types/indices.py @@ -19,6 +19,7 @@ HeadPose, ControllerInput, Generic3AxisPedalInput, + SteeringWheelInput, FullBodyInput, ) @@ -43,6 +44,9 @@ def _create_index_enum(name: str, group_type, prefix: str = "") -> IntEnum: Generic3AxisPedalInputIndex: Any = _create_index_enum( "Generic3AxisPedalInputIndex", Generic3AxisPedalInput(), "pedal_" ) +SteeringWheelInputIndex: Any = _create_index_enum( + "SteeringWheelInputIndex", SteeringWheelInput(), "wheel_" +) FullBodyInputIndex: Any = _create_index_enum( "FullBodyInputIndex", FullBodyInput(), "body_" ) diff --git a/src/core/retargeting_engine/python/tensor_types/standard_types.py b/src/core/retargeting_engine/python/tensor_types/standard_types.py index 74d04ecf2..081355aaa 100644 --- a/src/core/retargeting_engine/python/tensor_types/standard_types.py +++ b/src/core/retargeting_engine/python/tensor_types/standard_types.py @@ -326,3 +326,23 @@ def Generic3AxisPedalInput() -> TensorGroupType: FloatType("pedal_rudder"), ], ) + + +def SteeringWheelInput() -> TensorGroupType: + """ + Standard TensorGroupType for steering wheel and pedal axis data. + + Matches the scalar axis fields in SteeringWheelOutput. Axis values are + normalized joystick values in [-1, 1]. + """ + return TensorGroupType( + "steering_wheel", + [ + FloatType("wheel_steering"), + FloatType("wheel_throttle"), + FloatType("wheel_brake"), + FloatType("wheel_clutch"), + FloatType("wheel_hat_x"), + FloatType("wheel_hat_y"), + ], + ) diff --git a/src/retargeters/__init__.py b/src/retargeters/__init__.py index c485e69f2..666cfec37 100644 --- a/src/retargeters/__init__.py +++ b/src/retargeters/__init__.py @@ -15,6 +15,7 @@ - LocomotionFixedRootCmdRetargeter: Fixed root command (standing still) - LocomotionRootCmdRetargeter: Locomotion from controller inputs - FootPedalRootCmdRetargeter: Root command from 3-axis foot pedal (horizontal/vertical + rudder) + - VehicleControlRetargeter: Vehicle command retargeting from steering wheel input - GripperRetargeter: Pinch-based gripper control - SO101ClutchRetargeter: Clutch-rebased absolute EE pose for the SO-101 5-DOF arm - SO101GripperRetargeter: Proportional (analog) jaw closedness for the SO-101 gripper @@ -97,6 +98,18 @@ "FootPedalRootCmdRetargeterConfig", None, ), + # .vehicle_control_retargeter + "VehicleControlRetargeter": ( + ".vehicle_control_retargeter", + "VehicleControlRetargeter", + None, + ), + "VehicleControlRetargeterConfig": ( + ".vehicle_control_retargeter", + "VehicleControlRetargeterConfig", + None, + ), + "axis_to_pedal": (".vehicle_control_retargeter", "axis_to_pedal", None), # .gripper_retargeter "GripperRetargeter": (".gripper_retargeter", "GripperRetargeter", None), "GripperRetargeterConfig": (".gripper_retargeter", "GripperRetargeterConfig", None), @@ -194,6 +207,9 @@ def __getattr__(name: str): "TriHandMotionControllerConfig", "FootPedalRootCmdRetargeter", "FootPedalRootCmdRetargeterConfig", + "VehicleControlRetargeter", + "VehicleControlRetargeterConfig", + "axis_to_pedal", # Locomotion retargeters "LocomotionFixedRootCmdRetargeter", "LocomotionFixedRootCmdRetargeterConfig", diff --git a/src/retargeters/vehicle_control_retargeter.py b/src/retargeters/vehicle_control_retargeter.py new file mode 100644 index 000000000..5500bf921 --- /dev/null +++ b/src/retargeters/vehicle_control_retargeter.py @@ -0,0 +1,89 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Retarget steering wheel input into normalized vehicle control commands.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Protocol + +from isaacteleop.schema import SteeringWheelOutput, VehicleControlCommand + + +@dataclass(frozen=True) +class VehicleControlRetargeterConfig: + """Configuration for steering wheel to vehicle command retargeting.""" + + steering_deadzone: float = 0.01 + pedal_deadzone: float = 0.01 + steer_scale: float = 1.0 + throttle_scale: float = 1.0 + brake_scale: float = 1.0 + + +class SteeringWheelLike(Protocol): + """Structural type for objects with SteeringWheelOutput-compatible fields.""" + + steering: float + throttle: float + brake: float + + +class VehicleControlRetargeter: + """Maps normalized steering wheel state into ``VehicleControlCommand``.""" + + def __init__( + self, + config: VehicleControlRetargeterConfig | None = None, + *, + steering_neutral: float = 0.0, + ) -> None: + self._config = config or VehicleControlRetargeterConfig() + self._steering_neutral = steering_neutral + + @property + def steering_neutral(self) -> float: + return self._steering_neutral + + def calibrate_neutral(self, sample: SteeringWheelLike) -> None: + self._steering_neutral = sample.steering + + def retarget( + self, sample: SteeringWheelLike | SteeringWheelOutput, *, sequence: int + ) -> VehicleControlCommand: + steer = self._apply_deadzone( + (sample.steering - self._steering_neutral) * self._config.steer_scale, + self._config.steering_deadzone, + ) + throttle = self._apply_deadzone( + axis_to_pedal(sample.throttle) * self._config.throttle_scale, + self._config.pedal_deadzone, + ) + brake = self._apply_deadzone( + axis_to_pedal(sample.brake) * self._config.brake_scale, + self._config.pedal_deadzone, + ) + accel = _clamp(throttle - brake, -1.0, 1.0) + + return VehicleControlCommand( + sequence=sequence, + steer=_clamp(steer, -1.0, 1.0), + accel=accel, + throttle=accel if accel > 0.0 else 0.0, + brake=-accel if accel < 0.0 else 0.0, + ) + + @staticmethod + def _apply_deadzone(value: float, threshold: float) -> float: + return 0.0 if abs(value) <= threshold else value + + +def axis_to_pedal(axis_value: float) -> float: + """Map inverted full-range pedal axes (1 released, -1 pressed) into [0, 1].""" + + return _clamp((-float(axis_value) + 1.0) / 2.0, 0.0, 1.0) + + +def _clamp(value: float, lower: float, upper: float) -> float: + return min(upper, max(lower, float(value))) From 3182c9e4e41653b4edbf2237a1e5ae0fc4ea0a33 Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Sun, 14 Jun 2026 21:45:45 -0700 Subject: [PATCH 4/4] Add Linux steering wheel plugin --- CMakeLists.txt | 1 + src/plugins/steering_wheel/CMakeLists.txt | 23 ++ src/plugins/steering_wheel/README.md | 22 ++ src/plugins/steering_wheel/main.cpp | 72 +++++++ src/plugins/steering_wheel/plugin.yaml | 11 + .../steering_wheel/steering_wheel_plugin.cpp | 199 ++++++++++++++++++ .../steering_wheel/steering_wheel_plugin.hpp | 64 ++++++ 7 files changed, 392 insertions(+) create mode 100644 src/plugins/steering_wheel/CMakeLists.txt create mode 100644 src/plugins/steering_wheel/README.md create mode 100644 src/plugins/steering_wheel/main.cpp create mode 100644 src/plugins/steering_wheel/plugin.yaml create mode 100644 src/plugins/steering_wheel/steering_wheel_plugin.cpp create mode 100644 src/plugins/steering_wheel/steering_wheel_plugin.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 33a061f55..3d896bd5e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -164,6 +164,7 @@ if(BUILD_PLUGINS) add_subdirectory(src/plugins/controller_synthetic_hands) add_subdirectory(src/plugins/generic_3axis_pedal) + add_subdirectory(src/plugins/steering_wheel) add_subdirectory(src/plugins/manus) add_subdirectory(src/plugins/haptikos) if(BUILD_PLUGIN_OAK_CAMERA) diff --git a/src/plugins/steering_wheel/CMakeLists.txt b/src/plugins/steering_wheel/CMakeLists.txt new file mode 100644 index 000000000..832b9cae2 --- /dev/null +++ b/src/plugins/steering_wheel/CMakeLists.txt @@ -0,0 +1,23 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(STATUS "Skipping steering_wheel plugin (Linux only)") + add_custom_target(steering_wheel_plugin + COMMAND ${CMAKE_COMMAND} -E echo "Skipping steering_wheel: Linux only") + return() +endif() + +add_executable(steering_wheel_plugin + main.cpp + steering_wheel_plugin.cpp +) + +target_link_libraries(steering_wheel_plugin PRIVATE + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema +) + +install(TARGETS steering_wheel_plugin RUNTIME DESTINATION plugins/steering_wheel) +install(FILES plugin.yaml README.md DESTINATION plugins/steering_wheel) diff --git a/src/plugins/steering_wheel/README.md b/src/plugins/steering_wheel/README.md new file mode 100644 index 000000000..16f75fb22 --- /dev/null +++ b/src/plugins/steering_wheel/README.md @@ -0,0 +1,22 @@ + + +# Steering Wheel Plugin + +Reads a Linux joystick device from `/dev/input/js*` and pushes `SteeringWheelOutput` via OpenXR. Use with `SteeringWheelTracker` and the same `collection_id`. + +## Usage + +```bash +./steering_wheel_plugin [device_path] [collection_id] [steering_axis] [throttle_axis] [brake_axis] [clutch_axis] +``` + +- **device_path**: Default `/dev/input/js0`. +- **collection_id**: Default `steering_wheel`. +- **axis indexes**: Generic defaults are steering `0`, throttle `1`, brake `2`, clutch disabled with `-1`. Pass explicit axis indexes for wheel-specific layouts. + +Axis values are normalized joystick values in `[-1, 1]`. The plugin does not convert pedal axes into pressed fractions; retargeters own that mapping. + +The RemoteTeleop wrapper can read `config/steering_wheel_config.yaml` and pass the resulting axis indexes to this plugin. On the current G920 setup that maps steering `0`, throttle `2`, brake `3`, and clutch `1`. diff --git a/src/plugins/steering_wheel/main.cpp b/src/plugins/steering_wheel/main.cpp new file mode 100644 index 000000000..7d20683fb --- /dev/null +++ b/src/plugins/steering_wheel/main.cpp @@ -0,0 +1,72 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "steering_wheel_plugin.hpp" + +#include +#include +#include +#include +#include + +using namespace plugins::steering_wheel; + +namespace +{ + +int parse_axis_arg(int argc, char** argv, int index, int default_value) +{ + return (argc > index) ? std::stoi(argv[index]) : default_value; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + if (argc == 0) + { + std::cerr << "Usage: " << argv[0] + << " " + << std::endl; + return 1; + } + + const std::string device_path = (argc > 1) ? argv[1] : "/dev/input/js0"; + const std::string collection_id = (argc > 2) ? argv[2] : "steering_wheel"; + SteeringWheelAxisMapping axis_mapping; + axis_mapping.steering_axis = parse_axis_arg(argc, argv, 3, axis_mapping.steering_axis); + axis_mapping.throttle_axis = parse_axis_arg(argc, argv, 4, axis_mapping.throttle_axis); + axis_mapping.brake_axis = parse_axis_arg(argc, argv, 5, axis_mapping.brake_axis); + axis_mapping.clutch_axis = parse_axis_arg(argc, argv, 6, axis_mapping.clutch_axis); + + std::cout << "Steering Wheel (device: " << device_path << ", collection: " << collection_id + << ", steering_axis: " << axis_mapping.steering_axis << ", throttle_axis: " << axis_mapping.throttle_axis + << ", brake_axis: " << axis_mapping.brake_axis << ", clutch_axis: " << axis_mapping.clutch_axis << ")" + << std::endl; + + SteeringWheelPlugin plugin(device_path, collection_id, axis_mapping); + + const auto frame_duration = std::chrono::nanoseconds(1000000000 / 90); + const auto program_start = std::chrono::steady_clock::now(); + std::size_t frame_count = 0; + + while (true) + { + plugin.update(); + frame_count++; + std::this_thread::sleep_until(program_start + frame_duration * frame_count); + } + + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error" << std::endl; + return 1; +} diff --git a/src/plugins/steering_wheel/plugin.yaml b/src/plugins/steering_wheel/plugin.yaml new file mode 100644 index 000000000..c97960a9c --- /dev/null +++ b/src/plugins/steering_wheel/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: steering_wheel +description: "Steering wheel and pedals via Linux joystick device" +command: "./steering_wheel_plugin" +version: "1.0.0" +devices: + - path: "/steering_wheel/generic" + type: "steering_wheel" + description: "Steering wheel from /dev/input/js*" diff --git a/src/plugins/steering_wheel/steering_wheel_plugin.cpp b/src/plugins/steering_wheel/steering_wheel_plugin.cpp new file mode 100644 index 000000000..f8d8309c1 --- /dev/null +++ b/src/plugins/steering_wheel/steering_wheel_plugin.cpp @@ -0,0 +1,199 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "steering_wheel_plugin.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace steering_wheel +{ + +namespace +{ + +constexpr size_t kJsEventSize = sizeof(js_event); +constexpr double kMaxAxisValue = 32767.0; +constexpr size_t kMaxFlatbufferSize = 1024; + +float normalize_axis(int16_t raw_value) +{ + return static_cast(std::max(-1.0, std::min(1.0, static_cast(raw_value) / kMaxAxisValue))); +} + +} // namespace + +SteeringWheelPlugin::SteeringWheelPlugin(const std::string& device_path, + const std::string& collection_id, + SteeringWheelAxisMapping axis_mapping) + : device_path_(device_path), + axis_mapping_(axis_mapping), + session_( + std::make_shared("SteeringWheelPlugin", core::SchemaPusher::get_required_extensions())), + pusher_(session_->get_handles(), + core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = kMaxFlatbufferSize, + .tensor_identifier = "steering_wheel", + .localized_name = "Steering Wheel", + .app_name = "SteeringWheelPlugin" }) +{ + if (!open_device()) + throw std::runtime_error("SteeringWheelPlugin: Failed to open " + device_path + " (" + strerror(errno) + ")"); +} + +SteeringWheelPlugin::~SteeringWheelPlugin() +{ + close_device(); +} + +void SteeringWheelPlugin::update() +{ + if (device_fd_ < 0) + { + open_device(); + if (device_fd_ < 0) + { + push_current_state(); + return; + } + } + + fd_set read_fds; + struct timeval timeout = { 0, 0 }; + + while (true) + { + FD_ZERO(&read_fds); + FD_SET(device_fd_, &read_fds); + timeout = { 0, 0 }; + + int ret = select(device_fd_ + 1, &read_fds, nullptr, nullptr, &timeout); + if (ret < 0) + { + if (errno == EINTR) + return; + close_device(); + push_current_state(); + return; + } + if (ret == 0 || !FD_ISSET(device_fd_, &read_fds)) + { + break; + } + + js_event event; + ssize_t n = read(device_fd_, &event, kJsEventSize); + if (n != static_cast(kJsEventSize)) + { + if (n < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) + break; + close_device(); + push_current_state(); + return; + } + + const uint8_t event_type = event.type & ~JS_EVENT_INIT; + if (event_type == JS_EVENT_AXIS) + { + if (event.number < axes_.size()) + { + axes_[event.number] = normalize_axis(event.value); + } + } + else if (event_type == JS_EVENT_BUTTON) + { + if (event.number < buttons_.size()) + { + buttons_[event.number] = event.value ? 1 : 0; + } + } + } + + push_current_state(); +} + +bool SteeringWheelPlugin::open_device() +{ + if (device_fd_ >= 0) + return true; + + int fd = open(device_path_.c_str(), O_RDONLY | O_NONBLOCK); + if (fd < 0) + return false; + + device_fd_ = fd; + resize_state_from_device(); + std::cout << "SteeringWheelPlugin: Opened " << device_path_ << std::endl; + return true; +} + +void SteeringWheelPlugin::close_device() +{ + if (device_fd_ < 0) + return; + + close(device_fd_); + device_fd_ = -1; +} + +void SteeringWheelPlugin::push_current_state() +{ + core::SteeringWheelOutputT out; + out.steering = axis_value(axis_mapping_.steering_axis); + out.throttle = axis_value(axis_mapping_.throttle_axis); + out.brake = axis_value(axis_mapping_.brake_axis); + out.clutch = axis_value(axis_mapping_.clutch_axis); + out.buttons = buttons_; + out.hat_x = hat_[0]; + out.hat_y = hat_[1]; + + const auto sample_time_ns = core::os_monotonic_now_ns(); + + flatbuffers::FlatBufferBuilder builder(kMaxFlatbufferSize); + auto offset = core::SteeringWheelOutput::Pack(builder, &out); + builder.Finish(offset); + pusher_.push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time_ns, sample_time_ns); +} + +float SteeringWheelPlugin::axis_value(int axis_index) const +{ + if (axis_index < 0) + return 0.0f; + const auto index = static_cast(axis_index); + return index < axes_.size() ? axes_[index] : 0.0f; +} + +void SteeringWheelPlugin::resize_state_from_device() +{ + unsigned char axis_count = 0; + unsigned char button_count = 0; + if (ioctl(device_fd_, JSIOCGAXES, &axis_count) < 0) + { + axis_count = 0; + } + if (ioctl(device_fd_, JSIOCGBUTTONS, &button_count) < 0) + { + button_count = 0; + } + + axes_.assign(axis_count, 0.0f); + buttons_.assign(button_count, 0); +} + +} // namespace steering_wheel +} // namespace plugins diff --git a/src/plugins/steering_wheel/steering_wheel_plugin.hpp b/src/plugins/steering_wheel/steering_wheel_plugin.hpp new file mode 100644 index 000000000..a16049036 --- /dev/null +++ b/src/plugins/steering_wheel/steering_wheel_plugin.hpp @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include +#include + +namespace core +{ +class OpenXRSession; +} + +namespace plugins +{ +namespace steering_wheel +{ + +struct SteeringWheelAxisMapping +{ + int steering_axis = 0; + int throttle_axis = 1; + int brake_axis = 2; + int clutch_axis = -1; +}; + +/*! + * @brief Reads a Linux joystick (e.g. /dev/input/js0), maps axes/buttons to + * SteeringWheelOutput, and pushes it via OpenXR SchemaPusher. + */ +class SteeringWheelPlugin +{ +public: + SteeringWheelPlugin(const std::string& device_path, + const std::string& collection_id, + SteeringWheelAxisMapping axis_mapping); + ~SteeringWheelPlugin(); + + void update(); + +private: + bool open_device(); + void close_device(); + void push_current_state(); + float axis_value(int axis_index) const; + void resize_state_from_device(); + + std::string device_path_; + int device_fd_ = -1; + SteeringWheelAxisMapping axis_mapping_; + std::vector axes_; + std::vector buttons_; + std::array hat_ = { 0, 0 }; + + std::shared_ptr session_; + core::SchemaPusher pusher_; +}; + +} // namespace steering_wheel +} // namespace plugins