From e5b213dd255e80384fa9de9c6f72480a5ba30d3c Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Sun, 14 Jun 2026 21:44:53 -0700 Subject: [PATCH 1/6] Add steering wheel and vehicle control schemas Signed-off-by: Justin Yue --- 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 ff937c09a76a53b91375ff9c9fec93ab22421d2f Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Sun, 14 Jun 2026 21:45:13 -0700 Subject: [PATCH 2/6] Add DeviceIO steering wheel tracker Signed-off-by: Justin Yue --- .../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 3051b31e42d01e27baff8269cd9013e9006d5471 Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Sun, 14 Jun 2026 21:45:28 -0700 Subject: [PATCH 3/6] Add vehicle control retargeter Signed-off-by: Justin Yue --- .../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 09a4bafc945b5ae824d7514f823075863ced3dd6 Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Sun, 14 Jun 2026 21:45:45 -0700 Subject: [PATCH 4/6] Add Linux steering wheel plugin Signed-off-by: Justin Yue --- 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 From a3099d55b4e259f24c928bc3defcfeece13d4411 Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Thu, 18 Jun 2026 13:35:00 -0700 Subject: [PATCH 5/6] add vehicle teleop example Signed-off-by: Justin Yue --- CMakeLists.txt | 1 + examples/vehicle_teleop/.gitignore | 3 + examples/vehicle_teleop/CMakeLists.txt | 22 + examples/vehicle_teleop/README.md | 94 +++++ .../config/steering_wheel_config.yaml | 15 + examples/vehicle_teleop/python/pyproject.toml | 17 + .../python/vehicle_teleop/__init__.py | 4 + .../python/vehicle_teleop/command_log.py | 153 +++++++ .../isaac_remote_steering_worker.py | 396 ++++++++++++++++++ .../python/vehicle_teleop/kia_panda_worker.py | 177 ++++++++ .../vehicle_teleop/replay_command_mcap.py | 48 +++ .../python/vehicle_teleop/vehicle_command.py | 49 +++ .../scripts/replay_command_mcap.sh | 11 + .../run_isaac_remote_steering_worker.sh | 12 + .../scripts/run_kia_panda_worker.sh | 11 + 15 files changed, 1013 insertions(+) create mode 100644 examples/vehicle_teleop/.gitignore create mode 100644 examples/vehicle_teleop/CMakeLists.txt create mode 100644 examples/vehicle_teleop/README.md create mode 100644 examples/vehicle_teleop/config/steering_wheel_config.yaml create mode 100644 examples/vehicle_teleop/python/pyproject.toml create mode 100644 examples/vehicle_teleop/python/vehicle_teleop/__init__.py create mode 100644 examples/vehicle_teleop/python/vehicle_teleop/command_log.py create mode 100644 examples/vehicle_teleop/python/vehicle_teleop/isaac_remote_steering_worker.py create mode 100644 examples/vehicle_teleop/python/vehicle_teleop/kia_panda_worker.py create mode 100644 examples/vehicle_teleop/python/vehicle_teleop/replay_command_mcap.py create mode 100644 examples/vehicle_teleop/python/vehicle_teleop/vehicle_command.py create mode 100755 examples/vehicle_teleop/scripts/replay_command_mcap.sh create mode 100755 examples/vehicle_teleop/scripts/run_isaac_remote_steering_worker.sh create mode 100755 examples/vehicle_teleop/scripts/run_kia_panda_worker.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d896bd5e..24ca7eb7e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -149,6 +149,7 @@ if(BUILD_EXAMPLES) add_subdirectory(examples/schemaio) add_subdirectory(examples/native_openxr) add_subdirectory(examples/mcap_record_replay) + add_subdirectory(examples/vehicle_teleop) add_subdirectory(examples/haptic_feedback) if(BUILD_VIZ) add_subdirectory(examples/camera_viz/tests) diff --git a/examples/vehicle_teleop/.gitignore b/examples/vehicle_teleop/.gitignore new file mode 100644 index 000000000..ba0bc2022 --- /dev/null +++ b/examples/vehicle_teleop/.gitignore @@ -0,0 +1,3 @@ +logs/ +thirdparty/ +python/.venv/ diff --git a/examples/vehicle_teleop/CMakeLists.txt b/examples/vehicle_teleop/CMakeLists.txt new file mode 100644 index 000000000..eb320e3ef --- /dev/null +++ b/examples/vehicle_teleop/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) +install_python_example(DESTINATION examples/vehicle_teleop/python) + +install(FILES README.md + DESTINATION examples/vehicle_teleop +) + +install(DIRECTORY config + DESTINATION examples/vehicle_teleop +) + +install(PROGRAMS + scripts/replay_command_mcap.sh + scripts/run_isaac_remote_steering_worker.sh + scripts/run_kia_panda_worker.sh + DESTINATION examples/vehicle_teleop/scripts +) diff --git a/examples/vehicle_teleop/README.md b/examples/vehicle_teleop/README.md new file mode 100644 index 000000000..7101235e7 --- /dev/null +++ b/examples/vehicle_teleop/README.md @@ -0,0 +1,94 @@ + + +# Vehicle Teleop Example + +This example sends steering wheel and pedal input through Isaac Teleop, retargets it to a vehicle command, and publishes that command over ZMQ for a Kia Panda worker. + +The remote side uses the native Linux steering wheel plugin and Isaac Teleop OpenXR session. The vehicle side subscribes to the command stream and writes to `PandaRunner`. + +## Setup + +Build and install Isaac Teleop with examples enabled. From the Isaac Teleop repository root: + +```bash +cmake -B build -DBUILD_EXAMPLES=ON +cmake --build build --parallel 2 +cmake --install build +``` + +Clone the vehicle-side dependencies into this example's `thirdparty` directory. These repositories are intentionally not added as git submodules. + +```bash +cd examples/vehicle_teleop +mkdir -p thirdparty +git clone https://github.com/commaai/panda.git thirdparty/panda +git clone git@github.com:UCR-CISL/kia-opendbc.git thirdparty/kia-opendbc +``` + +You can use existing local checkouts instead, as long as the paths match `thirdparty/panda` and `thirdparty/kia-opendbc`. + +## Remote Side + +Start the CloudXR runtime: + +```bash +python3 -m isaacteleop.cloudxr +``` + +In a separate terminal, activate the CloudXR environment printed by that command, then start the Isaac Teleop steering worker: + +```bash +source ~/.cloudxr/run/cloudxr.env +./scripts/run_isaac_remote_steering_worker.sh --verbose +``` + +By default, the worker binds to `tcp://*:5555` on topic `kia_control`. + +Useful options: + +```bash +./scripts/run_isaac_remote_steering_worker.sh --device /dev/input/js0 +./scripts/run_isaac_remote_steering_worker.sh --bind "tcp://*:5555" +./scripts/run_isaac_remote_steering_worker.sh --log-mcap logs/kia_control.mcap +./scripts/run_isaac_remote_steering_worker.sh --plugin-binary /path/to/steering_wheel_plugin +``` + +Steering wheel axis mapping is configured in `config/steering_wheel_config.yaml`. The default mapping is for a Logitech G920/G923-style setup where steering, throttle, brake, and clutch are raw Linux joystick axes. + +## Vehicle Side + +Run the Panda worker on the vehicle machine: + +```bash +./scripts/run_kia_panda_worker.sh --connect "tcp://:5555" +``` + +For local testing without opening PandaRunner, use dry-run mode: + +```bash +./scripts/run_kia_panda_worker.sh --connect "tcp://:5555" --dry-run +``` + +Do not run the live vehicle-side worker unless the vehicle-side hardware and safety process are ready. + +## Replay Logs + +If the remote worker was started with `--log-mcap`, replay the recorded commands with: + +```bash +./scripts/replay_command_mcap.sh logs/kia_control.mcap +./scripts/replay_command_mcap.sh logs/kia_control.mcap --realtime +``` + +The replay command prints the retargeted vehicle command values. It does not write to PandaRunner. + +## Troubleshooting + +If the steering worker cannot find the native plugin, pass `--plugin-binary` explicitly. In a source build, the default location is usually `build/src/plugins/steering_wheel/steering_wheel_plugin`. In an install tree, it is usually `plugins/steering_wheel/steering_wheel_plugin`. + +If the worker fails while creating the OpenXR session, confirm that the CloudXR runtime is running, the CloudXR environment has been sourced in the worker terminal, and the OpenXR client is connected. + +If the Panda worker cannot import `opendbc` or `panda`, confirm the two dependency repositories were cloned under `examples/vehicle_teleop/thirdparty/`. diff --git a/examples/vehicle_teleop/config/steering_wheel_config.yaml b/examples/vehicle_teleop/config/steering_wheel_config.yaml new file mode 100644 index 000000000..7e558ae5f --- /dev/null +++ b/examples/vehicle_teleop/config/steering_wheel_config.yaml @@ -0,0 +1,15 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +g920_racing_wheel: + steering_wheel: 0 + throttle: 2 + brake: 3 + clutch: 1 + handbrake: 4 + reverse: 10 + +sensitivity: + mode: 0 + min: 0.5 + max: 0.5 diff --git a/examples/vehicle_teleop/python/pyproject.toml b/examples/vehicle_teleop/python/pyproject.toml new file mode 100644 index 000000000..d50ce776e --- /dev/null +++ b/examples/vehicle_teleop/python/pyproject.toml @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "vehicle-teleop-example" +version = "0.0.0" +description = "Isaac Teleop vehicle steering and Kia Panda control example" +requires-python = ">=3.10,<3.14" +dependencies = [ + "isaacteleop[cloudxr]", + "mcap>=1.2.0", + "pyyaml", + "pyzmq", +] + +[[tool.uv.index]] +url = "https://pypi.nvidia.com" diff --git a/examples/vehicle_teleop/python/vehicle_teleop/__init__.py b/examples/vehicle_teleop/python/vehicle_teleop/__init__.py new file mode 100644 index 000000000..4acd927e9 --- /dev/null +++ b/examples/vehicle_teleop/python/vehicle_teleop/__init__.py @@ -0,0 +1,4 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Vehicle teleoperation example using Isaac Teleop steering wheel input.""" diff --git a/examples/vehicle_teleop/python/vehicle_teleop/command_log.py b/examples/vehicle_teleop/python/vehicle_teleop/command_log.py new file mode 100644 index 000000000..b25f07796 --- /dev/null +++ b/examples/vehicle_teleop/python/vehicle_teleop/command_log.py @@ -0,0 +1,153 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import json +from dataclasses import dataclass +from pathlib import Path +from typing import BinaryIO, Iterable + +from mcap.reader import make_reader +from mcap.writer import CompressionType, Writer + +from vehicle_teleop.vehicle_command import VehicleControlCommand + + +@dataclass(frozen=True) +class SteeringWheelSample: + steering: float + accel_axis: float + brake_axis: float + timestamp_ns: int + + def to_dict(self) -> dict[str, float | int]: + return { + "steering": self.steering, + "accel_axis": self.accel_axis, + "brake_axis": self.brake_axis, + "timestamp_ns": self.timestamp_ns, + } + + +VEHICLE_COMMAND_TOPIC = "vehicle_control" +VEHICLE_COMMAND_SCHEMA = { + "type": "object", + "properties": { + "sample": { + "type": "object", + "properties": { + "steering": {"type": "number"}, + "accel_axis": {"type": "number"}, + "brake_axis": {"type": "number"}, + "timestamp_ns": {"type": "integer"}, + }, + "required": ["steering", "accel_axis", "brake_axis", "timestamp_ns"], + }, + "command": { + "type": "object", + "properties": { + "sequence": {"type": "integer"}, + "timestamp_ns": {"type": "integer"}, + "steer": {"type": "number"}, + "accel": {"type": "number"}, + "throttle": {"type": "number"}, + "brake": {"type": "number"}, + }, + "required": [ + "sequence", + "timestamp_ns", + "steer", + "accel", + "throttle", + "brake", + ], + }, + }, + "required": ["sample", "command"], +} + + +@dataclass(frozen=True) +class CommandLogRecord: + sample: SteeringWheelSample + command: VehicleControlCommand + + @classmethod + def from_dict(cls, value: dict) -> "CommandLogRecord": + sample = value["sample"] + return cls( + sample=SteeringWheelSample( + steering=float(sample["steering"]), + accel_axis=float(sample["accel_axis"]), + brake_axis=float(sample["brake_axis"]), + timestamp_ns=int(sample["timestamp_ns"]), + ), + command=VehicleControlCommand.from_dict(value["command"]), + ) + + def to_dict(self) -> dict: + return { + "sample": self.sample.to_dict(), + "command": self.command.to_dict(), + } + + def to_json(self) -> bytes: + return json.dumps(self.to_dict(), separators=(",", ":")).encode("utf-8") + + +class McapCommandLogger: + def __init__(self, path: str | Path) -> None: + self._path = Path(path) + self._file: BinaryIO | None = None + self._writer: Writer | None = None + self._channel_id: int | None = None + + def __enter__(self) -> "McapCommandLogger": + self._path.parent.mkdir(parents=True, exist_ok=True) + self._file = self._path.open("wb") + self._writer = Writer(self._file, compression=CompressionType.NONE) + self._writer.start() + schema_id = self._writer.register_schema( + name="vehicle_teleop.VehicleControlCommandRecord", + encoding="jsonschema", + data=json.dumps(VEHICLE_COMMAND_SCHEMA, separators=(",", ":")).encode("utf-8"), + ) + self._channel_id = self._writer.register_channel( + topic=VEHICLE_COMMAND_TOPIC, + message_encoding="json", + schema_id=schema_id, + ) + return self + + def __exit__(self, _exc_type, _exc_value, _traceback) -> None: + if self._writer is not None: + self._writer.finish() + self._writer = None + self._channel_id = None + if self._file is not None: + self._file.close() + self._file = None + + def write(self, *, sample: SteeringWheelSample, command: VehicleControlCommand) -> None: + if self._writer is None or self._channel_id is None: + raise RuntimeError("McapCommandLogger must be opened before writing") + record = CommandLogRecord(sample=sample, command=command) + self._writer.add_message( + channel_id=self._channel_id, + log_time=command.timestamp_ns, + publish_time=command.timestamp_ns, + sequence=command.sequence, + data=record.to_json(), + ) + + +class McapCommandLogReader: + def __init__(self, path: str | Path) -> None: + self._path = Path(path) + + def records(self) -> Iterable[CommandLogRecord]: + with self._path.open("rb") as file: + reader = make_reader(file) + for _schema, _channel, message in reader.iter_messages(topics=[VEHICLE_COMMAND_TOPIC]): + yield CommandLogRecord.from_dict(json.loads(message.data.decode("utf-8"))) diff --git a/examples/vehicle_teleop/python/vehicle_teleop/isaac_remote_steering_worker.py b/examples/vehicle_teleop/python/vehicle_teleop/isaac_remote_steering_worker.py new file mode 100644 index 000000000..914d8d6ea --- /dev/null +++ b/examples/vehicle_teleop/python/vehicle_teleop/isaac_remote_steering_worker.py @@ -0,0 +1,396 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import argparse +import json +import shutil +import signal +import subprocess +import time +from contextlib import nullcontext +from pathlib import Path +from typing import ContextManager + +import isaacteleop.deviceio as deviceio +import isaacteleop.oxr as oxr +import yaml +import zmq +from isaacteleop.retargeters import ( + VehicleControlRetargeter, + VehicleControlRetargeterConfig, +) +from isaacteleop.schema import SteeringWheelOutput + +from vehicle_teleop.command_log import McapCommandLogger, SteeringWheelSample +from vehicle_teleop.vehicle_command import VehicleControlCommand + + +DEFAULT_BIND = "tcp://*:5555" +DEFAULT_TOPIC = "kia_control" +DEFAULT_COLLECTION_ID = "steering_wheel" +DEFAULT_DEVICE_PATH = "/dev/input/js0" +DEFAULT_FIRST_SAMPLE_TIMEOUT_S = 5.0 +DEFAULT_AXIS_MAPPING = { + "steering_axis": 0, + "throttle_axis": 1, + "brake_axis": 2, + "clutch_axis": -1, +} + + +def example_root() -> Path: + return Path(__file__).resolve().parents[2] + + +def default_config_path() -> Path: + return example_root() / "config" / "steering_wheel_config.yaml" + + +def default_plugin_binary() -> Path: + root = example_root() + candidates = [ + root.parents[1] / "build" / "src" / "plugins" / "steering_wheel" / "steering_wheel_plugin", + root.parents[1] / "plugins" / "steering_wheel" / "steering_wheel_plugin", + ] + for candidate in candidates: + if candidate.exists(): + return candidate + return candidates[0] + + +class NativeSteeringWheelPlugin: + def __init__( + self, + *, + binary: Path, + device_path: str, + collection_id: str, + steering_axis: int, + throttle_axis: int, + brake_axis: int, + clutch_axis: int, + ) -> None: + self._command = [ + str(binary), + device_path, + collection_id, + str(steering_axis), + str(throttle_axis), + str(brake_axis), + str(clutch_axis), + ] + self._process: subprocess.Popen | None = None + + @property + def command(self) -> list[str]: + return list(self._command) + + def __enter__(self) -> "NativeSteeringWheelPlugin": + self._process = subprocess.Popen(self._command) + return self + + def __exit__(self, _exc_type, _exc_value, _traceback) -> None: + if self._process is None: + return + if self._process.poll() is None: + self._process.terminate() + try: + self._process.wait(timeout=2.0) + except subprocess.TimeoutExpired: + self._process.kill() + self._process.wait() + self._process = None + + +def load_axis_mapping_from_config(path: str | Path) -> dict[str, int]: + config_path = Path(path) + with config_path.open("r", encoding="utf-8") as file: + data = yaml.safe_load(file) + if not isinstance(data, dict): + raise ValueError(f"Steering wheel config must be a YAML mapping: {config_path}") + wheel = data.get("g920_racing_wheel") + if not isinstance(wheel, dict): + raise KeyError(f"Steering wheel config missing g920_racing_wheel section: {config_path}") + + mapping = dict(DEFAULT_AXIS_MAPPING) + mapping["steering_axis"] = int(wheel["steering_wheel"]) + mapping["throttle_axis"] = int(wheel["throttle"]) + mapping["brake_axis"] = int(wheel["brake"]) + if "clutch" in wheel: + mapping["clutch_axis"] = int(wheel["clutch"]) + return mapping + + +def resolve_axis_mapping(args: argparse.Namespace) -> dict[str, int]: + mapping = dict(DEFAULT_AXIS_MAPPING) + if args.config: + mapping.update(load_axis_mapping_from_config(args.config)) + for arg_name in ("steering_axis", "throttle_axis", "brake_axis", "clutch_axis"): + value = getattr(args, arg_name) + if value is not None: + mapping[arg_name] = int(value) + return mapping + + +class IsaacRemoteSteeringWorker: + def __init__(self, args: argparse.Namespace): + self._bind = args.bind + self._topic = args.topic + self._rate_hz = args.rate_hz + self._collection_id = args.collection_id + self._device_path = args.device + self._plugin_binary = Path(args.plugin_binary) + self._axis_args = resolve_axis_mapping(args) + self._start_plugin = not args.no_start_plugin + self._first_sample_timeout_s = args.first_sample_timeout_s + self._verbose = args.verbose + self._context = zmq.Context() + self._socket = self._context.socket(zmq.PUB) + self._tracker = deviceio.SteeringWheelTracker(self._collection_id) + self._deviceio_session = None + self._retargeter = VehicleControlRetargeter( + VehicleControlRetargeterConfig(steer_scale=-1.0) + ) + self._log_mcap = args.log_mcap + self._logger = None + self._sequence = 0 + self._running = True + + def run(self) -> None: + self._register_signal_handlers() + self._socket.setsockopt(zmq.LINGER, 0) + self._socket.bind(self._bind) + + plugin_ctx = self._plugin_context() + trackers = [self._tracker] + required_extensions = deviceio.DeviceIOSession.get_required_extensions(trackers) + logger_ctx: ContextManager = ( + McapCommandLogger(self._log_mcap) if self._log_mcap else nullcontext() + ) + try: + with plugin_ctx, oxr.OpenXRSession( + "VehicleTeleopSteeringWorker", required_extensions + ) as oxr_session: + handles = oxr_session.get_handles() + with deviceio.DeviceIOSession.run(trackers, handles) as self._deviceio_session: + with logger_ctx as self._logger: + self._capture_steering_neutral() + period_s = 1.0 / self._rate_hz + print( + f"Publishing IsaacTeleop steering commands on {self._bind} " + f"topic={self._topic!r} at {self._rate_hz:.1f} Hz" + ) + print( + f"Reading steering wheel through IsaacTeleop DeviceIO collection " + f"{self._collection_id!r}" + ) + self._run_loop(period_s) + finally: + self._publish_neutral() + self._socket.close() + self._context.term() + print("\nIsaacTeleop vehicle steering worker stopped.") + + def _plugin_context(self) -> ContextManager: + if not self._start_plugin: + return nullcontext() + if not self._plugin_binary.exists(): + raise FileNotFoundError( + f"IsaacTeleop steering wheel plugin not found at {self._plugin_binary}. " + "Build IsaacTeleop first, or pass --plugin-binary/--no-start-plugin." + ) + plugin = NativeSteeringWheelPlugin( + binary=self._plugin_binary, + device_path=self._device_path, + collection_id=self._collection_id, + **self._axis_args, + ) + print("Starting native IsaacTeleop steering wheel plugin:") + print(" ".join(plugin.command)) + return plugin + + def _run_loop(self, period_s: float) -> None: + while self._running: + started = time.monotonic() + self._publish_next_command() + time.sleep(max(0.0, period_s - (time.monotonic() - started))) + + def _publish_next_command(self) -> None: + sample = self._read_isaac_sample() + command = isaac_command_to_wire_command( + self._retargeter.retarget(sample, sequence=self._sequence), + timestamp_ns=time.time_ns(), + ) + self._publish(command) + if self._logger is not None: + self._logger.write( + sample=wire_sample_from_isaac_sample(sample, timestamp_ns=command.timestamp_ns), + command=command, + ) + if self._verbose: + self._print_status_line( + f"seq={command.sequence} accel={command.accel:+.3f} steer={command.steer:+.3f} " + f"throttle={command.throttle:.3f} brake={command.brake:.3f}" + ) + self._sequence += 1 + + @staticmethod + def _print_status_line(text: str) -> None: + columns = shutil.get_terminal_size(fallback=(120, 24)).columns + if columns > 1: + text = text[: columns - 1] + print(f"\r\033[K{text}", end="", flush=True) + + def _read_isaac_sample(self) -> SteeringWheelOutput: + if self._deviceio_session is None: + raise RuntimeError("DeviceIO session is not initialized") + self._deviceio_session.update() + tracked = self._tracker.get_wheel_data(self._deviceio_session) + if tracked.data is None: + raise RuntimeError( + "No steering wheel data available from IsaacTeleop. " + "Check that the native steering_wheel_plugin is running and publishing " + f"collection {self._collection_id!r}." + ) + return tracked.data + + def _wait_for_first_sample(self) -> SteeringWheelOutput: + deadline = time.monotonic() + self._first_sample_timeout_s + last_error: RuntimeError | None = None + while self._running and time.monotonic() < deadline: + try: + return self._read_isaac_sample() + except RuntimeError as exc: + last_error = exc + time.sleep(0.05) + if last_error is not None: + raise last_error + raise RuntimeError("Stopped before receiving steering wheel data") + + def _publish_neutral(self) -> None: + self._publish(VehicleControlCommand.neutral(sequence=self._sequence)) + + def _publish(self, command: VehicleControlCommand) -> None: + payload = json.dumps(command.to_dict(), separators=(",", ":")) + self._socket.send_string(f"{self._topic} {payload}") + + def _capture_steering_neutral(self) -> None: + sample = self._wait_for_first_sample() + self._retargeter.calibrate_neutral(sample) + print(f"Steering neutral offset: {self._retargeter.steering_neutral:+.3f}") + + def stop(self, _signum=None, _frame=None) -> None: + self._running = False + + def _register_signal_handlers(self) -> None: + signal.signal(signal.SIGINT, self.stop) + signal.signal(signal.SIGTERM, self.stop) + + +def isaac_command_to_wire_command(command, *, timestamp_ns: int) -> VehicleControlCommand: + return VehicleControlCommand( + sequence=int(command.sequence), + timestamp_ns=timestamp_ns, + steer=float(command.steer), + accel=float(command.accel), + throttle=float(command.throttle), + brake=float(command.brake), + ) + + +def wire_sample_from_isaac_sample( + sample: SteeringWheelOutput, *, timestamp_ns: int +) -> SteeringWheelSample: + return SteeringWheelSample( + steering=float(sample.steering), + accel_axis=float(sample.throttle), + brake_axis=float(sample.brake), + timestamp_ns=timestamp_ns, + ) + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description=( + "Read steering wheel input through IsaacTeleop and publish Kia " + "vehicle commands over ZMQ." + ) + ) + parser.add_argument("--bind", default=DEFAULT_BIND, help="ZMQ PUB bind address.") + parser.add_argument("--topic", default=DEFAULT_TOPIC, help="ZMQ topic prefix.") + parser.add_argument("--rate-hz", type=float, default=50.0, help="Publish rate.") + parser.add_argument( + "--device", + default=DEFAULT_DEVICE_PATH, + help="Linux joystick device path for the native plugin.", + ) + parser.add_argument( + "--config", + default=str(default_config_path()), + help=( + "Steering wheel YAML config used to resolve axis indexes. Pass an " + "empty string to use plugin defaults." + ), + ) + parser.add_argument( + "--collection-id", + default=DEFAULT_COLLECTION_ID, + help="IsaacTeleop tensor collection id.", + ) + parser.add_argument( + "--plugin-binary", + default=str(default_plugin_binary()), + help="Path to steering_wheel_plugin.", + ) + parser.add_argument( + "--steering-axis", + type=int, + default=None, + help="Override joystick axis index for steering.", + ) + parser.add_argument( + "--throttle-axis", + type=int, + default=None, + help="Override joystick axis index for throttle.", + ) + parser.add_argument( + "--brake-axis", + type=int, + default=None, + help="Override joystick axis index for brake.", + ) + parser.add_argument( + "--clutch-axis", + type=int, + default=None, + help="Override joystick axis index for clutch, or -1 to disable.", + ) + parser.add_argument( + "--no-start-plugin", + action="store_true", + help="Do not launch steering_wheel_plugin; read from an already running publisher.", + ) + parser.add_argument( + "--first-sample-timeout-s", + type=float, + default=DEFAULT_FIRST_SAMPLE_TIMEOUT_S, + help="Seconds to wait for the first IsaacTeleop steering sample before failing.", + ) + parser.add_argument( + "--log-mcap", + default=None, + help="Record raw-axis samples and commands to an MCAP log.", + ) + parser.add_argument("--verbose", action="store_true", help="Print live control values.") + return parser + + +def main() -> None: + IsaacRemoteSteeringWorker(build_parser().parse_args()).run() + + +if __name__ == "__main__": + main() diff --git a/examples/vehicle_teleop/python/vehicle_teleop/kia_panda_worker.py b/examples/vehicle_teleop/python/vehicle_teleop/kia_panda_worker.py new file mode 100644 index 000000000..84f0722cf --- /dev/null +++ b/examples/vehicle_teleop/python/vehicle_teleop/kia_panda_worker.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +from __future__ import annotations + +import argparse +import json +import signal +import time + +import zmq + +from vehicle_teleop.vehicle_command import VehicleControlCommand, clamp + + +DEFAULT_CONNECT = "tcp://127.0.0.1:5555" +DEFAULT_TOPIC = "kia_control" + + +class KiaPandaWorker: + def __init__(self, args: argparse.Namespace): + self._connect = args.connect + self._topic = args.topic + self._command_timeout = args.command_timeout + self._poll_ms = args.poll_ms + self._steer_sign = args.steer_sign + self._dry_run = args.dry_run + self._context = zmq.Context() + self._socket = self._context.socket(zmq.SUB) + self._poller = zmq.Poller() + self._panda_runner = None + self._panda = None + self._car_control = None + self._last_command = VehicleControlCommand.neutral() + self._last_received = 0.0 + self._last_printed = None + self._neutral_sent_after_timeout = False + self._running = True + + def run(self) -> None: + self._register_signal_handlers() + self._open_socket() + self._open_panda() + + print(f"Receiving steering commands from {self._connect} topic={self._topic!r}") + try: + while self._running: + command = self._next_command_to_apply() + if command is not None: + if self._dry_run: + self._print_command(command) + else: + self._apply_command(command) + finally: + if self._panda is not None: + self._apply_command(VehicleControlCommand.neutral()) + if self._panda_runner is not None: + self._panda_runner.__exit__(None, None, None) + self._socket.close() + self._context.term() + print("\nVehicle panda worker stopped.") + + def stop(self, _signum=None, _frame=None) -> None: + self._running = False + + def _register_signal_handlers(self) -> None: + signal.signal(signal.SIGINT, self.stop) + signal.signal(signal.SIGTERM, self.stop) + + def _open_socket(self) -> None: + self._socket.setsockopt(zmq.LINGER, 0) + self._socket.setsockopt(zmq.CONFLATE, 1) + self._socket.setsockopt_string(zmq.SUBSCRIBE, self._topic) + self._socket.connect(self._connect) + self._poller.register(self._socket, zmq.POLLIN) + + def _open_panda(self) -> None: + if self._dry_run: + return + + from opendbc.car.panda_runner import PandaRunner + from opendbc.car.structs import CarControl + + self._car_control = CarControl(enabled=False) + self._panda_runner = PandaRunner() + self._panda = self._panda_runner.__enter__() + + def _poll_once(self) -> bool: + events = dict(self._poller.poll(timeout=self._poll_ms)) + if self._socket not in events: + return False + + message = self._socket.recv_string() + _topic, payload = message.split(" ", 1) + self._last_command = VehicleControlCommand.from_dict(json.loads(payload)) + self._last_received = time.monotonic() + self._neutral_sent_after_timeout = False + return True + + def _next_command_to_apply(self) -> VehicleControlCommand | None: + if self._poll_once(): + return self._last_command + if self._last_received == 0.0: + return None + if time.monotonic() - self._last_received <= self._command_timeout: + return None + if self._neutral_sent_after_timeout: + return None + + self._neutral_sent_after_timeout = True + return VehicleControlCommand( + sequence=self._last_command.sequence, + timestamp_ns=time.time_ns(), + steer=0.0, + accel=0.0, + throttle=0.0, + brake=0.0, + ) + + def _print_command(self, command: VehicleControlCommand) -> None: + printable = (command.sequence, round(command.accel, 3), round(command.steer, 3)) + if printable == self._last_printed: + return + print(f"seq={command.sequence} accel={command.accel:+.3f} steer={command.steer:+.3f}") + self._last_printed = printable + + def _apply_command(self, command: VehicleControlCommand) -> None: + self._car_control.actuators.accel = float(4.0 * clamp(command.accel, -1.0, 1.0)) + self._car_control.actuators.torque = float( + self._steer_sign * clamp(command.steer, -1.0, 1.0) + ) + + self._panda.read() + self._car_control.enabled = True + self._car_control.latActive = True + self._car_control.longActive = True + self._panda.write(self._car_control) + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Receive Kia teleop commands over ZMQ and write them to PandaRunner." + ) + parser.add_argument( + "--connect", default=DEFAULT_CONNECT, help="ZMQ PUB endpoint to connect to." + ) + parser.add_argument("--topic", default=DEFAULT_TOPIC, help="ZMQ topic prefix.") + parser.add_argument( + "--command-timeout", + type=float, + default=0.25, + help="Seconds before stale commands go neutral.", + ) + parser.add_argument( + "--poll-ms", type=int, default=10, help="ZMQ poll interval in milliseconds." + ) + parser.add_argument( + "--steer-sign", + type=float, + choices=(-1.0, 1.0), + default=1.0, + help="Invert steering torque if needed.", + ) + parser.add_argument( + "--dry-run", + action="store_true", + help="Receive and print commands without opening PandaRunner.", + ) + return parser + + +def main() -> None: + KiaPandaWorker(build_parser().parse_args()).run() + + +if __name__ == "__main__": + main() diff --git a/examples/vehicle_teleop/python/vehicle_teleop/replay_command_mcap.py b/examples/vehicle_teleop/python/vehicle_teleop/replay_command_mcap.py new file mode 100644 index 000000000..a57cf3fb8 --- /dev/null +++ b/examples/vehicle_teleop/python/vehicle_teleop/replay_command_mcap.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +from __future__ import annotations + +import argparse +import time + +from vehicle_teleop.command_log import McapCommandLogReader + + +class CommandMcapReplay: + def __init__(self, args: argparse.Namespace) -> None: + self._reader = McapCommandLogReader(args.path) + self._realtime = args.realtime + + def run(self) -> None: + previous_sample_time_ns = None + for record in self._reader.records(): + if self._realtime and previous_sample_time_ns is not None: + delay_ns = max(0, record.sample.timestamp_ns - previous_sample_time_ns) + time.sleep(delay_ns / 1_000_000_000) + previous_sample_time_ns = record.sample.timestamp_ns + command = record.command + print( + f"seq={command.sequence} t={command.timestamp_ns} " + f"accel={command.accel:+.3f} steer={command.steer:+.3f} " + f"throttle={command.throttle:.3f} brake={command.brake:.3f}" + ) + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser(description="Replay an MCAP vehicle command log.") + parser.add_argument("path", help="Path to MCAP command log.") + parser.add_argument( + "--realtime", + action="store_true", + help="Sleep between records using sample timestamps.", + ) + return parser + + +def main() -> None: + CommandMcapReplay(build_parser().parse_args()).run() + + +if __name__ == "__main__": + main() diff --git a/examples/vehicle_teleop/python/vehicle_teleop/vehicle_command.py b/examples/vehicle_teleop/python/vehicle_teleop/vehicle_command.py new file mode 100644 index 000000000..d004043f8 --- /dev/null +++ b/examples/vehicle_teleop/python/vehicle_teleop/vehicle_command.py @@ -0,0 +1,49 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import time +from dataclasses import asdict, dataclass +from typing import Any + + +@dataclass(frozen=True) +class VehicleControlCommand: + sequence: int + timestamp_ns: int + steer: float + accel: float + throttle: float + brake: float + + @classmethod + def neutral( + cls, sequence: int = 0, timestamp_ns: int | None = None + ) -> "VehicleControlCommand": + return cls( + sequence=sequence, + timestamp_ns=time.time_ns() if timestamp_ns is None else timestamp_ns, + steer=0.0, + accel=0.0, + throttle=0.0, + brake=0.0, + ) + + @classmethod + def from_dict(cls, value: dict[str, Any]) -> "VehicleControlCommand": + return cls( + sequence=int(value["sequence"]), + timestamp_ns=int(value["timestamp_ns"]), + steer=float(value["steer"]), + accel=float(value["accel"]), + throttle=float(value.get("throttle", 0.0)), + brake=float(value.get("brake", 0.0)), + ) + + def to_dict(self) -> dict[str, Any]: + return asdict(self) + + +def clamp(value: float, lower: float, upper: float) -> float: + return min(upper, max(lower, float(value))) diff --git a/examples/vehicle_teleop/scripts/replay_command_mcap.sh b/examples/vehicle_teleop/scripts/replay_command_mcap.sh new file mode 100755 index 000000000..b30e09da4 --- /dev/null +++ b/examples/vehicle_teleop/scripts/replay_command_mcap.sh @@ -0,0 +1,11 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +set -euo pipefail + +EXAMPLE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +export PYTHONPATH="${EXAMPLE_ROOT}/python${PYTHONPATH:+:${PYTHONPATH}}" + +exec uv run --project "${EXAMPLE_ROOT}/python" \ + python -m vehicle_teleop.replay_command_mcap \ + "$@" diff --git a/examples/vehicle_teleop/scripts/run_isaac_remote_steering_worker.sh b/examples/vehicle_teleop/scripts/run_isaac_remote_steering_worker.sh new file mode 100755 index 000000000..c2d3eb37f --- /dev/null +++ b/examples/vehicle_teleop/scripts/run_isaac_remote_steering_worker.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +set -euo pipefail + +EXAMPLE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +export PYTHONPATH="${EXAMPLE_ROOT}/python${PYTHONPATH:+:${PYTHONPATH}}" + +exec uv run --project "${EXAMPLE_ROOT}/python" \ + python -m vehicle_teleop.isaac_remote_steering_worker \ + --rate-hz 50 \ + "$@" diff --git a/examples/vehicle_teleop/scripts/run_kia_panda_worker.sh b/examples/vehicle_teleop/scripts/run_kia_panda_worker.sh new file mode 100755 index 000000000..d7a975b81 --- /dev/null +++ b/examples/vehicle_teleop/scripts/run_kia_panda_worker.sh @@ -0,0 +1,11 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +set -euo pipefail + +EXAMPLE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +export PYTHONPATH="${EXAMPLE_ROOT}/python:${EXAMPLE_ROOT}/thirdparty/kia-opendbc:${EXAMPLE_ROOT}/thirdparty/panda${PYTHONPATH:+:${PYTHONPATH}}" + +exec uv run --project "${EXAMPLE_ROOT}/python" \ + python -m vehicle_teleop.kia_panda_worker \ + "$@" From a51280b369325af31e657bba95a75bfbfd2806eb Mon Sep 17 00:00:00 2001 From: Justin Yue Date: Mon, 22 Jun 2026 22:40:28 -0700 Subject: [PATCH 6/6] Verify everything works, add fall-back keyboard runner as well Signed-off-by: Justin Yue --- examples/vehicle_teleop/CMakeLists.txt | 1 + examples/vehicle_teleop/README.md | 30 ++- examples/vehicle_teleop/python/pyproject.toml | 8 + .../python/vehicle_teleop/command_log.py | 16 +- .../isaac_keyboard_control_worker.py | 222 ++++++++++++++++++ .../isaac_remote_steering_worker.py | 36 ++- .../python/vehicle_teleop/kia_panda_worker.py | 5 +- .../vehicle_teleop/replay_command_mcap.py | 1 - .../scripts/replay_command_mcap.sh | 11 +- .../run_isaac_keyboard_control_worker.sh | 19 ++ .../run_isaac_remote_steering_worker.sh | 11 +- .../scripts/run_kia_panda_worker.sh | 13 +- 12 files changed, 346 insertions(+), 27 deletions(-) create mode 100644 examples/vehicle_teleop/python/vehicle_teleop/isaac_keyboard_control_worker.py create mode 100755 examples/vehicle_teleop/scripts/run_isaac_keyboard_control_worker.sh diff --git a/examples/vehicle_teleop/CMakeLists.txt b/examples/vehicle_teleop/CMakeLists.txt index eb320e3ef..e505747ff 100644 --- a/examples/vehicle_teleop/CMakeLists.txt +++ b/examples/vehicle_teleop/CMakeLists.txt @@ -16,6 +16,7 @@ install(DIRECTORY config install(PROGRAMS scripts/replay_command_mcap.sh + scripts/run_isaac_keyboard_control_worker.sh scripts/run_isaac_remote_steering_worker.sh scripts/run_kia_panda_worker.sh DESTINATION examples/vehicle_teleop/scripts diff --git a/examples/vehicle_teleop/README.md b/examples/vehicle_teleop/README.md index 7101235e7..e3787463b 100644 --- a/examples/vehicle_teleop/README.md +++ b/examples/vehicle_teleop/README.md @@ -15,20 +15,34 @@ Build and install Isaac Teleop with examples enabled. From the Isaac Teleop repo ```bash cmake -B build -DBUILD_EXAMPLES=ON -cmake --build build --parallel 2 +cmake --build build --parallel 4 cmake --install build ``` -Clone the vehicle-side dependencies into this example's `thirdparty` directory. These repositories are intentionally not added as git submodules. +Create the example virtual environment from this directory. The scripts use +`.venv/bin/python` directly, so keep the virtual environment at +`examples/vehicle_teleop/.venv`. ```bash cd examples/vehicle_teleop +uv venv --python 3.11 .venv +uv pip install --python .venv/bin/python --find-links ../../build/wheels "isaacteleop[cloudxr]" +source .venv/bin/activate +cd python +uv sync --active --inexact --no-install-project --no-install-package isaacteleop +cd .. +``` + +Clone the vehicle-side dependencies into this example's `thirdparty` directory. These repositories are intentionally not added as git submodules. + +```bash mkdir -p thirdparty git clone https://github.com/commaai/panda.git thirdparty/panda -git clone git@github.com:UCR-CISL/kia-opendbc.git thirdparty/kia-opendbc +git clone https://github.com/commaai/opendbc.git thirdparty/opendbc ``` -You can use existing local checkouts instead, as long as the paths match `thirdparty/panda` and `thirdparty/kia-opendbc`. +You can use existing local checkouts instead, as long as the paths match `thirdparty/panda` and `thirdparty/opendbc`. +In our usage, the `opendbc` and `panda` repos are not a "one-size-fits-all" solutions for some cars. We recommend debugging as needed if the Panda device cannot connect to the car. In the future, we will provide a repository that works for our usage (Kia Niro EV 2022) as a useful reference. ## Remote Side @@ -58,6 +72,14 @@ Useful options: Steering wheel axis mapping is configured in `config/steering_wheel_config.yaml`. The default mapping is for a Logitech G920/G923-style setup where steering, throttle, brake, and clutch are raw Linux joystick axes. +For IsaacTeleop keyboard fallback without a steering wheel, run: + +```bash +./scripts/run_isaac_keyboard_control_worker.sh --verbose +``` + +Keyboard controls are `W`/`S` for gas-brake, `A`/`D` for steering, `R` or `C` for neutral, and `Q` or `Esc` to quit. + ## Vehicle Side Run the Panda worker on the vehicle machine: diff --git a/examples/vehicle_teleop/python/pyproject.toml b/examples/vehicle_teleop/python/pyproject.toml index d50ce776e..d6b6ecd7b 100644 --- a/examples/vehicle_teleop/python/pyproject.toml +++ b/examples/vehicle_teleop/python/pyproject.toml @@ -7,10 +7,18 @@ version = "0.0.0" description = "Isaac Teleop vehicle steering and Kia Panda control example" requires-python = ">=3.10,<3.14" dependencies = [ + "crcmod", "isaacteleop[cloudxr]", + "libusb-package", + "libusb1", "mcap>=1.2.0", + "pycapnp==2.1.0", + "pycryptodome", "pyyaml", "pyzmq", + "scons", + "tqdm", + "websockets>=14", ] [[tool.uv.index]] diff --git a/examples/vehicle_teleop/python/vehicle_teleop/command_log.py b/examples/vehicle_teleop/python/vehicle_teleop/command_log.py index b25f07796..1ae15747d 100644 --- a/examples/vehicle_teleop/python/vehicle_teleop/command_log.py +++ b/examples/vehicle_teleop/python/vehicle_teleop/command_log.py @@ -111,7 +111,9 @@ def __enter__(self) -> "McapCommandLogger": schema_id = self._writer.register_schema( name="vehicle_teleop.VehicleControlCommandRecord", encoding="jsonschema", - data=json.dumps(VEHICLE_COMMAND_SCHEMA, separators=(",", ":")).encode("utf-8"), + data=json.dumps(VEHICLE_COMMAND_SCHEMA, separators=(",", ":")).encode( + "utf-8" + ), ) self._channel_id = self._writer.register_channel( topic=VEHICLE_COMMAND_TOPIC, @@ -129,7 +131,9 @@ def __exit__(self, _exc_type, _exc_value, _traceback) -> None: self._file.close() self._file = None - def write(self, *, sample: SteeringWheelSample, command: VehicleControlCommand) -> None: + def write( + self, *, sample: SteeringWheelSample, command: VehicleControlCommand + ) -> None: if self._writer is None or self._channel_id is None: raise RuntimeError("McapCommandLogger must be opened before writing") record = CommandLogRecord(sample=sample, command=command) @@ -149,5 +153,9 @@ def __init__(self, path: str | Path) -> None: def records(self) -> Iterable[CommandLogRecord]: with self._path.open("rb") as file: reader = make_reader(file) - for _schema, _channel, message in reader.iter_messages(topics=[VEHICLE_COMMAND_TOPIC]): - yield CommandLogRecord.from_dict(json.loads(message.data.decode("utf-8"))) + for _schema, _channel, message in reader.iter_messages( + topics=[VEHICLE_COMMAND_TOPIC] + ): + yield CommandLogRecord.from_dict( + json.loads(message.data.decode("utf-8")) + ) diff --git a/examples/vehicle_teleop/python/vehicle_teleop/isaac_keyboard_control_worker.py b/examples/vehicle_teleop/python/vehicle_teleop/isaac_keyboard_control_worker.py new file mode 100644 index 000000000..160386015 --- /dev/null +++ b/examples/vehicle_teleop/python/vehicle_teleop/isaac_keyboard_control_worker.py @@ -0,0 +1,222 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +from __future__ import annotations + +import argparse +import json +import select +import signal +import sys +import termios +import time +from dataclasses import dataclass + +import zmq +from isaacteleop.retargeters import ( + VehicleControlRetargeter, + VehicleControlRetargeterConfig, +) +from isaacteleop.schema import SteeringWheelOutput + +from vehicle_teleop.vehicle_command import VehicleControlCommand, clamp + + +DEFAULT_BIND = "tcp://*:5555" +DEFAULT_TOPIC = "kia_control" + + +@dataclass +class IsaacKeyboardControlState: + axis_increment: float = 0.05 + gas_brake: float = 0.0 + steer: float = 0.0 + quit_requested: bool = False + + def apply_key(self, key: str) -> bool: + normalized = key.lower() + if normalized == "w": + self.gas_brake = clamp(self.gas_brake + self.axis_increment, -1.0, 1.0) + elif normalized == "s": + self.gas_brake = clamp(self.gas_brake - self.axis_increment, -1.0, 1.0) + elif normalized == "a": + self.steer = clamp(self.steer + self.axis_increment, -1.0, 1.0) + elif normalized == "d": + self.steer = clamp(self.steer - self.axis_increment, -1.0, 1.0) + elif normalized in ("r", "c"): + self.reset() + elif normalized in ("q", "\x1b"): + self.quit_requested = True + else: + return False + return True + + def reset(self) -> None: + self.gas_brake = 0.0 + self.steer = 0.0 + + def to_isaac_sample(self) -> SteeringWheelOutput: + accel = clamp(self.gas_brake, -1.0, 1.0) + return SteeringWheelOutput( + clamp(self.steer, -1.0, 1.0), + pedal_to_inverted_axis(max(0.0, accel)), + pedal_to_inverted_axis(max(0.0, -accel)), + 0.0, + ) + + +def pedal_to_inverted_axis(value: float) -> float: + return 1.0 - 2.0 * clamp(value, 0.0, 1.0) + + +class TerminalKeyReader: + def __init__(self, input_stream=sys.stdin) -> None: + self._input_stream = input_stream + self._fd = input_stream.fileno() + self._old_term: list | None = None + + def __enter__(self) -> "TerminalKeyReader": + if not self._input_stream.isatty(): + raise RuntimeError( + "Keyboard worker must be run from an interactive terminal." + ) + self._old_term = termios.tcgetattr(self._fd) + new_term = self._old_term.copy() + new_term[3] &= ~(termios.ICANON | termios.ECHO) + termios.tcsetattr(self._fd, termios.TCSAFLUSH, new_term) + return self + + def __exit__(self, _exc_type, _exc_value, _traceback) -> None: + if self._old_term is not None: + termios.tcsetattr(self._fd, termios.TCSAFLUSH, self._old_term) + self._old_term = None + + def read_key(self) -> str | None: + ready, _, _ = select.select([self._input_stream], [], [], 0) + if not ready: + return None + return self._input_stream.read(1) + + +class IsaacKeyboardControlWorker: + def __init__(self, args: argparse.Namespace) -> None: + self._bind = args.bind + self._topic = args.topic + self._rate_hz = args.rate_hz + self._verbose = args.verbose + self._state = IsaacKeyboardControlState(axis_increment=args.axis_increment) + self._retargeter = VehicleControlRetargeter( + VehicleControlRetargeterConfig(steer_scale=args.steer_scale) + ) + self._context = zmq.Context() + self._socket = self._context.socket(zmq.PUB) + self._sequence = 0 + self._running = True + + def run(self) -> None: + self._register_signal_handlers() + self._socket.setsockopt(zmq.LINGER, 0) + self._socket.bind(self._bind) + period_s = 1.0 / self._rate_hz + + print( + f"Publishing IsaacTeleop keyboard commands on {self._bind} " + f"topic={self._topic!r} at {self._rate_hz:.1f} Hz" + ) + print("Controls: W/S gas-brake, A/D steer, R reset, C neutral, Q or Esc quit") + try: + with TerminalKeyReader() as reader: + self._run_loop(reader, period_s) + finally: + self._publish_neutral() + self._socket.close() + self._context.term() + print("\nIsaacTeleop keyboard control worker stopped.") + + def stop(self, _signum=None, _frame=None) -> None: + self._running = False + + def _register_signal_handlers(self) -> None: + signal.signal(signal.SIGINT, self.stop) + signal.signal(signal.SIGTERM, self.stop) + + def _run_loop(self, reader: TerminalKeyReader, period_s: float) -> None: + while self._running and not self._state.quit_requested: + started = time.monotonic() + key = reader.read_key() + if key is not None: + self._state.apply_key(key) + self._publish_next_command() + time.sleep(max(0.0, period_s - (time.monotonic() - started))) + + def _publish_next_command(self) -> None: + timestamp_ns = time.time_ns() + isaac_command = self._retargeter.retarget( + self._state.to_isaac_sample(), + sequence=self._sequence, + ) + command = isaac_command_to_wire_command( + isaac_command, timestamp_ns=timestamp_ns + ) + self._publish(command) + if self._verbose: + print( + f"seq={command.sequence} accel={command.accel:+.3f} " + f"steer={command.steer:+.3f} throttle={command.throttle:.3f} " + f"brake={command.brake:.3f}", + end="\r", + flush=True, + ) + self._sequence += 1 + + def _publish_neutral(self) -> None: + self._publish(VehicleControlCommand.neutral(sequence=self._sequence)) + + def _publish(self, command: VehicleControlCommand) -> None: + payload = json.dumps(command.to_dict(), separators=(",", ":")) + self._socket.send_string(f"{self._topic} {payload}") + + +def isaac_command_to_wire_command( + command, *, timestamp_ns: int +) -> VehicleControlCommand: + return VehicleControlCommand( + sequence=int(command.sequence), + timestamp_ns=timestamp_ns, + steer=float(command.steer), + accel=float(command.accel), + throttle=float(command.throttle), + brake=float(command.brake), + ) + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Read WASD keyboard input through IsaacTeleop retargeting and publish Kia teleop commands over ZMQ." + ) + parser.add_argument("--bind", default=DEFAULT_BIND, help="ZMQ PUB bind address.") + parser.add_argument("--topic", default=DEFAULT_TOPIC, help="ZMQ topic prefix.") + parser.add_argument("--rate-hz", type=float, default=50.0, help="Publish rate.") + parser.add_argument( + "--axis-increment", + type=float, + default=0.05, + help="Axis delta applied for each key press.", + ) + parser.add_argument( + "--steer-scale", + type=float, + default=1.0, + help="Scale or invert steering after IsaacTeleop retargeting.", + ) + parser.add_argument( + "--verbose", action="store_true", help="Print live control values." + ) + return parser + + +def main() -> None: + IsaacKeyboardControlWorker(build_parser().parse_args()).run() + + +if __name__ == "__main__": + main() diff --git a/examples/vehicle_teleop/python/vehicle_teleop/isaac_remote_steering_worker.py b/examples/vehicle_teleop/python/vehicle_teleop/isaac_remote_steering_worker.py index 914d8d6ea..63fc9ad2c 100644 --- a/examples/vehicle_teleop/python/vehicle_teleop/isaac_remote_steering_worker.py +++ b/examples/vehicle_teleop/python/vehicle_teleop/isaac_remote_steering_worker.py @@ -51,7 +51,12 @@ def default_config_path() -> Path: def default_plugin_binary() -> Path: root = example_root() candidates = [ - root.parents[1] / "build" / "src" / "plugins" / "steering_wheel" / "steering_wheel_plugin", + root.parents[1] + / "build" + / "src" + / "plugins" + / "steering_wheel" + / "steering_wheel_plugin", root.parents[1] / "plugins" / "steering_wheel" / "steering_wheel_plugin", ] for candidate in candidates: @@ -112,7 +117,9 @@ def load_axis_mapping_from_config(path: str | Path) -> dict[str, int]: raise ValueError(f"Steering wheel config must be a YAML mapping: {config_path}") wheel = data.get("g920_racing_wheel") if not isinstance(wheel, dict): - raise KeyError(f"Steering wheel config missing g920_racing_wheel section: {config_path}") + raise KeyError( + f"Steering wheel config missing g920_racing_wheel section: {config_path}" + ) mapping = dict(DEFAULT_AXIS_MAPPING) mapping["steering_axis"] = int(wheel["steering_wheel"]) @@ -170,11 +177,16 @@ def run(self) -> None: McapCommandLogger(self._log_mcap) if self._log_mcap else nullcontext() ) try: - with plugin_ctx, oxr.OpenXRSession( - "VehicleTeleopSteeringWorker", required_extensions - ) as oxr_session: + with ( + plugin_ctx, + oxr.OpenXRSession( + "VehicleTeleopSteeringWorker", required_extensions + ) as oxr_session, + ): handles = oxr_session.get_handles() - with deviceio.DeviceIOSession.run(trackers, handles) as self._deviceio_session: + with deviceio.DeviceIOSession.run( + trackers, handles + ) as self._deviceio_session: with logger_ctx as self._logger: self._capture_steering_neutral() period_s = 1.0 / self._rate_hz @@ -226,7 +238,9 @@ def _publish_next_command(self) -> None: self._publish(command) if self._logger is not None: self._logger.write( - sample=wire_sample_from_isaac_sample(sample, timestamp_ns=command.timestamp_ns), + sample=wire_sample_from_isaac_sample( + sample, timestamp_ns=command.timestamp_ns + ), command=command, ) if self._verbose: @@ -289,7 +303,9 @@ def _register_signal_handlers(self) -> None: signal.signal(signal.SIGTERM, self.stop) -def isaac_command_to_wire_command(command, *, timestamp_ns: int) -> VehicleControlCommand: +def isaac_command_to_wire_command( + command, *, timestamp_ns: int +) -> VehicleControlCommand: return VehicleControlCommand( sequence=int(command.sequence), timestamp_ns=timestamp_ns, @@ -384,7 +400,9 @@ def build_parser() -> argparse.ArgumentParser: default=None, help="Record raw-axis samples and commands to an MCAP log.", ) - parser.add_argument("--verbose", action="store_true", help="Print live control values.") + parser.add_argument( + "--verbose", action="store_true", help="Print live control values." + ) return parser diff --git a/examples/vehicle_teleop/python/vehicle_teleop/kia_panda_worker.py b/examples/vehicle_teleop/python/vehicle_teleop/kia_panda_worker.py index 84f0722cf..52b5d49e3 100644 --- a/examples/vehicle_teleop/python/vehicle_teleop/kia_panda_worker.py +++ b/examples/vehicle_teleop/python/vehicle_teleop/kia_panda_worker.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 from __future__ import annotations @@ -121,7 +120,9 @@ def _print_command(self, command: VehicleControlCommand) -> None: printable = (command.sequence, round(command.accel, 3), round(command.steer, 3)) if printable == self._last_printed: return - print(f"seq={command.sequence} accel={command.accel:+.3f} steer={command.steer:+.3f}") + print( + f"seq={command.sequence} accel={command.accel:+.3f} steer={command.steer:+.3f}" + ) self._last_printed = printable def _apply_command(self, command: VehicleControlCommand) -> None: diff --git a/examples/vehicle_teleop/python/vehicle_teleop/replay_command_mcap.py b/examples/vehicle_teleop/python/vehicle_teleop/replay_command_mcap.py index a57cf3fb8..5d401e63d 100644 --- a/examples/vehicle_teleop/python/vehicle_teleop/replay_command_mcap.py +++ b/examples/vehicle_teleop/python/vehicle_teleop/replay_command_mcap.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 from __future__ import annotations diff --git a/examples/vehicle_teleop/scripts/replay_command_mcap.sh b/examples/vehicle_teleop/scripts/replay_command_mcap.sh index b30e09da4..8706a85de 100755 --- a/examples/vehicle_teleop/scripts/replay_command_mcap.sh +++ b/examples/vehicle_teleop/scripts/replay_command_mcap.sh @@ -4,8 +4,15 @@ set -euo pipefail EXAMPLE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +PYTHON="${EXAMPLE_ROOT}/.venv/bin/python" + +if [[ ! -x "${PYTHON}" ]]; then + echo "Missing example virtual environment: ${EXAMPLE_ROOT}/.venv" >&2 + echo "Create it with: uv venv --python 3.11 .venv" >&2 + exit 1 +fi + export PYTHONPATH="${EXAMPLE_ROOT}/python${PYTHONPATH:+:${PYTHONPATH}}" -exec uv run --project "${EXAMPLE_ROOT}/python" \ - python -m vehicle_teleop.replay_command_mcap \ +exec "${PYTHON}" -m vehicle_teleop.replay_command_mcap \ "$@" diff --git a/examples/vehicle_teleop/scripts/run_isaac_keyboard_control_worker.sh b/examples/vehicle_teleop/scripts/run_isaac_keyboard_control_worker.sh new file mode 100755 index 000000000..c7751d2d7 --- /dev/null +++ b/examples/vehicle_teleop/scripts/run_isaac_keyboard_control_worker.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +set -euo pipefail + +EXAMPLE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +PYTHON="${EXAMPLE_ROOT}/.venv/bin/python" + +if [[ ! -x "${PYTHON}" ]]; then + echo "Missing example virtual environment: ${EXAMPLE_ROOT}/.venv" >&2 + echo "Create it with: uv venv --python 3.11 .venv" >&2 + exit 1 +fi + +export PYTHONPATH="${EXAMPLE_ROOT}/python${PYTHONPATH:+:${PYTHONPATH}}" + +exec "${PYTHON}" -m vehicle_teleop.isaac_keyboard_control_worker \ + --rate-hz 50 \ + "$@" diff --git a/examples/vehicle_teleop/scripts/run_isaac_remote_steering_worker.sh b/examples/vehicle_teleop/scripts/run_isaac_remote_steering_worker.sh index c2d3eb37f..d4f2ea274 100755 --- a/examples/vehicle_teleop/scripts/run_isaac_remote_steering_worker.sh +++ b/examples/vehicle_teleop/scripts/run_isaac_remote_steering_worker.sh @@ -4,9 +4,16 @@ set -euo pipefail EXAMPLE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +PYTHON="${EXAMPLE_ROOT}/.venv/bin/python" + +if [[ ! -x "${PYTHON}" ]]; then + echo "Missing example virtual environment: ${EXAMPLE_ROOT}/.venv" >&2 + echo "Create it with: uv venv --python 3.11 .venv" >&2 + exit 1 +fi + export PYTHONPATH="${EXAMPLE_ROOT}/python${PYTHONPATH:+:${PYTHONPATH}}" -exec uv run --project "${EXAMPLE_ROOT}/python" \ - python -m vehicle_teleop.isaac_remote_steering_worker \ +exec "${PYTHON}" -m vehicle_teleop.isaac_remote_steering_worker \ --rate-hz 50 \ "$@" diff --git a/examples/vehicle_teleop/scripts/run_kia_panda_worker.sh b/examples/vehicle_teleop/scripts/run_kia_panda_worker.sh index d7a975b81..0f15cc0f4 100755 --- a/examples/vehicle_teleop/scripts/run_kia_panda_worker.sh +++ b/examples/vehicle_teleop/scripts/run_kia_panda_worker.sh @@ -4,8 +4,15 @@ set -euo pipefail EXAMPLE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" -export PYTHONPATH="${EXAMPLE_ROOT}/python:${EXAMPLE_ROOT}/thirdparty/kia-opendbc:${EXAMPLE_ROOT}/thirdparty/panda${PYTHONPATH:+:${PYTHONPATH}}" +PYTHON="${EXAMPLE_ROOT}/.venv/bin/python" -exec uv run --project "${EXAMPLE_ROOT}/python" \ - python -m vehicle_teleop.kia_panda_worker \ +if [[ ! -x "${PYTHON}" ]]; then + echo "Missing example virtual environment: ${EXAMPLE_ROOT}/.venv" >&2 + echo "Create it with: uv venv --python 3.11 .venv" >&2 + exit 1 +fi + +export PYTHONPATH="${EXAMPLE_ROOT}/python:${EXAMPLE_ROOT}/thirdparty/opendbc:${EXAMPLE_ROOT}/thirdparty/panda${PYTHONPATH:+:${PYTHONPATH}}" + +exec "${PYTHON}" -m vehicle_teleop.kia_panda_worker \ "$@"