From 850a75f9a2cf53c0178b9bbdf1b08a2a821bb026 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 26 Mar 2025 19:45:42 +0900 Subject: [PATCH 1/4] add control tool Signed-off-by: Takagi, Isamu --- .../CMakeLists.txt | 9 ++ .../rqt_autoware_manual_controller/README.md | 1 + .../package.xml | 26 ++++ .../rqt_autoware_manual_controller/plugin.xml | 16 ++ .../python/__init__.py | 33 ++++ .../python/module.py | 63 ++++++++ .../python/parts/__init__,py | 13 ++ .../python/parts/adapi.py | 147 ++++++++++++++++++ .../python/parts/mode_select.py | 43 +++++ .../python/widget.py | 75 +++++++++ .../script/rqt_autoware_manual_controller | 8 + 11 files changed, 434 insertions(+) create mode 100644 system/rqt_autoware_manual_controller/CMakeLists.txt create mode 100644 system/rqt_autoware_manual_controller/README.md create mode 100644 system/rqt_autoware_manual_controller/package.xml create mode 100644 system/rqt_autoware_manual_controller/plugin.xml create mode 100644 system/rqt_autoware_manual_controller/python/__init__.py create mode 100644 system/rqt_autoware_manual_controller/python/module.py create mode 100644 system/rqt_autoware_manual_controller/python/parts/__init__,py create mode 100644 system/rqt_autoware_manual_controller/python/parts/adapi.py create mode 100644 system/rqt_autoware_manual_controller/python/parts/mode_select.py create mode 100644 system/rqt_autoware_manual_controller/python/widget.py create mode 100755 system/rqt_autoware_manual_controller/script/rqt_autoware_manual_controller diff --git a/system/rqt_autoware_manual_controller/CMakeLists.txt b/system/rqt_autoware_manual_controller/CMakeLists.txt new file mode 100644 index 000000000..bf4577977 --- /dev/null +++ b/system/rqt_autoware_manual_controller/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.14) +project(rqt_autoware_manual_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR python) +install(FILES plugin.xml DESTINATION share/${PROJECT_NAME}) +install(PROGRAMS script/rqt_autoware_manual_controller DESTINATION lib/${PROJECT_NAME}) +ament_auto_package(INSTALL_TO_SHARE script) diff --git a/system/rqt_autoware_manual_controller/README.md b/system/rqt_autoware_manual_controller/README.md new file mode 100644 index 000000000..dfcd7928a --- /dev/null +++ b/system/rqt_autoware_manual_controller/README.md @@ -0,0 +1 @@ +# rqt_autoware_manual_controller diff --git a/system/rqt_autoware_manual_controller/package.xml b/system/rqt_autoware_manual_controller/package.xml new file mode 100644 index 000000000..36bf62dbe --- /dev/null +++ b/system/rqt_autoware_manual_controller/package.xml @@ -0,0 +1,26 @@ + + + + rqt_autoware_manual_controller + 0.1.0 + The rqt_autoware_manual_controller package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + python_qt_binding + rclpy + rqt_gui + rqt_gui_py + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/system/rqt_autoware_manual_controller/plugin.xml b/system/rqt_autoware_manual_controller/plugin.xml new file mode 100644 index 000000000..b38568c8d --- /dev/null +++ b/system/rqt_autoware_manual_controller/plugin.xml @@ -0,0 +1,16 @@ + + + + + + + + folder + + + + utilities-system-monitor + + + + diff --git a/system/rqt_autoware_manual_controller/python/__init__.py b/system/rqt_autoware_manual_controller/python/__init__.py new file mode 100644 index 000000000..d819dc19b --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/__init__.py @@ -0,0 +1,33 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from rqt_autoware_manual_controller.parts.adapi import Adapi +from rqt_autoware_manual_controller.widget import ControllerWidget +from rqt_gui_py.plugin import Plugin + + +class ControllerPlugin(Plugin): + def __init__(self, context): + super().__init__(context) + self.widget = ControllerWidget(Adapi(context.node, "local")) + context.add_widget(self.widget) + + def shutdown_plugin(self): + self.widget.shutdown() + + def save_settings(self, plugin_settings, instance_settings): + pass + + def restore_settings(self, plugin_settings, instance_settings): + pass diff --git a/system/rqt_autoware_manual_controller/python/module.py b/system/rqt_autoware_manual_controller/python/module.py new file mode 100644 index 000000000..5229d25d2 --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/module.py @@ -0,0 +1,63 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from rclpy.node import Node +from tier4_system_msgs.msg import DiagGraphStatus +from tier4_system_msgs.msg import DiagGraphStruct + +from .graph import Graph +from .utils import default_qos +from .utils import durable_qos +from .utils import foreach + + +class MonitorModule: + def __init__(self, node: Node): + self.graph = None + self.struct_callbacks = [] + self.status_callbacks = [] + self.node = node + self.sub_struct = self.subscribe_struct() + self.sub_status = self.subscribe_status() + + def append_struct_callback(self, callback): + self.struct_callbacks.append(callback) + + def append_status_callback(self, callback): + self.status_callbacks.append(callback) + + def on_struct(self, msg): + self.graph = Graph(msg) + foreach(self.struct_callbacks, lambda callback: callback(self.graph)) + + def on_status(self, msg): + if self.graph is None: + return + self.graph.update(msg) + foreach(self.status_callbacks, lambda callback: callback(self.graph)) + + def subscribe_struct(self): + return self.node.create_subscription( + DiagGraphStruct, "/diagnostics_graph/struct", self.on_struct, durable_qos(1) + ) + + def subscribe_status(self): + return self.node.create_subscription( + DiagGraphStatus, "/diagnostics_graph/status", self.on_status, default_qos(1) + ) + + def shutdown(self): + self.node.destroy_subscription(self.sub_struct) + self.node.destroy_subscription(self.sub_status) diff --git a/system/rqt_autoware_manual_controller/python/parts/__init__,py b/system/rqt_autoware_manual_controller/python/parts/__init__,py new file mode 100644 index 000000000..8004e46d2 --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/parts/__init__,py @@ -0,0 +1,13 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. diff --git a/system/rqt_autoware_manual_controller/python/parts/adapi.py b/system/rqt_autoware_manual_controller/python/parts/adapi.py new file mode 100644 index 000000000..ead0e25de --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/parts/adapi.py @@ -0,0 +1,147 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from enum import Enum + +from autoware_adapi_v1_msgs.msg import Gear as GearMsg +from autoware_adapi_v1_msgs.msg import GearCommand +from autoware_adapi_v1_msgs.msg import HazardLights as HazardLightsMsg +from autoware_adapi_v1_msgs.msg import HazardLightsCommand +from autoware_adapi_v1_msgs.msg import ManualControlMode +from autoware_adapi_v1_msgs.msg import ManualControlModeStatus +from autoware_adapi_v1_msgs.msg import ManualOperatorStatus +from autoware_adapi_v1_msgs.msg import PedalsCommand +from autoware_adapi_v1_msgs.msg import SteeringCommand +from autoware_adapi_v1_msgs.msg import TurnIndicators as TurnIndicatorsMsg +from autoware_adapi_v1_msgs.msg import TurnIndicatorsCommand +from autoware_adapi_v1_msgs.srv import ListManualControlMode +from autoware_adapi_v1_msgs.srv import SelectManualControlMode +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile + + +class ManualMode(Enum): + Disabled = ManualControlMode.DISABLED + Pedals = ManualControlMode.PEDALS + Acceleration = ManualControlMode.ACCELERATION + Velocity = ManualControlMode.VELOCITY + + +class Gear(Enum): + Unknown = GearMsg.UNKNOWN + Neutral = GearMsg.NEUTRAL + Drive = GearMsg.DRIVE + Reverse = GearMsg.REVERSE + Park = GearMsg.PARK + Low = GearMsg.LOW + + +class TurnIndicators(Enum): + Disable = TurnIndicatorsMsg.DISABLE + Left = TurnIndicatorsMsg.LEFT + Right = TurnIndicatorsMsg.RIGHT + + +class HazardLights(Enum): + Disable = HazardLightsMsg.DISABLE + Enable = HazardLightsMsg.ENABLE + + +class Adapi: + def __init__(self, node: Node, target: str): + # interfaces + durable_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self._node = node + # fmt: off + self._cli_mode_list = node.create_client(ListManualControlMode, f"/api/manual/{target}/control_mode/list") # noqa: E221 + self._cli_mode_select = node.create_client(SelectManualControlMode, f"/api/manual/{target}/control_mode/select") # noqa: E221 + self._sub_mode_status = node.create_subscription(ManualControlModeStatus, f"/api/manual/{target}/control_mode/status", self._on_mode_status, durable_qos) # noqa: E221 + self._pub_pedals = node.create_publisher(PedalsCommand, f"/api/manual/{target}/command/pedals", 1) # noqa: E221 + self._pub_steering = node.create_publisher(SteeringCommand, f"/api/manual/{target}/command/steering", 1) # noqa: E221 + self._pub_gear = node.create_publisher(GearCommand, f"/api/manual/{target}/command/gear", durable_qos) # noqa: E221 + self._pub_turn_indicators = node.create_publisher(TurnIndicatorsCommand, f"/api/manual/{target}/command/turn_indicators", durable_qos) # noqa: E221 + self._pub_hazard_lights = node.create_publisher(HazardLightsCommand, f"/api/manual/{target}/command/hazard_lights", durable_qos) # noqa: E221 + self._pub_heartbeat = node.create_publisher(ManualOperatorStatus, f"/api/manual/{target}/heartbeat", durable_qos) # noqa: E221 + # fmt: on + self._timer = node.create_timer(1.0, self._on_timer) + self._command_timer = node.create_timer(0.1, self._on_command_timer) + + # variables + self._is_mode_listed = False + self._msg_pedals = PedalsCommand() + self._msg_steering = SteeringCommand() + self._msg_gear = GearCommand() + self._msg_turn_indicators = TurnIndicatorsCommand() + self._msg_hazard_lights = HazardLightsCommand() + self._msg_heartbeat = ManualOperatorStatus() + # callbacks + self.on_mode_list = lambda _: None + self.on_mode_status = lambda _: None + # init + self.set_gear(Gear.Drive) + self.set_turn_indicators(TurnIndicators.Disable) + self.set_hazard_lights(HazardLights.Disable) + + def select_mode(self, mode: ManualMode): + self._cli_mode_select.call_async( + SelectManualControlMode.Request(mode=ManualControlMode(mode=mode.value)) + ) + + def _on_mode_status(self, status): + self.on_mode_status(ManualMode(status.mode.mode)) + + def _on_mode_list(self, future): + result = future.result() + self.on_mode_list([ManualMode(mode.mode) for mode in result.modes]) + self._is_mode_listed = True + + def _on_timer(self): + if not self._is_mode_listed and self._cli_mode_list.service_is_ready(): + future = self._cli_mode_list.call_async(ListManualControlMode.Request()) + future.add_done_callback(self._on_mode_list) + + def set_pedals(self, accel: float, brake: float): + self._msg_pedals.accelerator = accel + self._msg_pedals.brake = brake + + def set_steering(self, steering: float): + self._msg_steering.steering_tire_angle = steering + + def set_gear(self, gear: Gear): + self._msg_gear.command.status = gear.value + + def set_turn_indicators(self, turn_indicators: TurnIndicators): + self._msg_turn_indicators.command.status = turn_indicators.value + + def set_hazard_lights(self, hazard_lights: HazardLights): + self._msg_hazard_lights.command.status = hazard_lights.value + + def set_heartbeat(self, ready: bool): + self._msg_heartbeat.ready = ready + + def _on_command_timer(self): + stamp = self._node.get_clock().now().to_msg() + self._msg_pedals.stamp = stamp + self._msg_steering.stamp = stamp + self._msg_gear.stamp = stamp + self._msg_turn_indicators.stamp = stamp + self._msg_hazard_lights.stamp = stamp + self._msg_heartbeat.stamp = stamp + self._pub_pedals.publish(self._msg_pedals) + self._pub_steering.publish(self._msg_steering) + self._pub_gear.publish(self._msg_gear) + self._pub_turn_indicators.publish(self._msg_turn_indicators) + self._pub_hazard_lights.publish(self._msg_hazard_lights) + self._pub_heartbeat.publish(self._msg_heartbeat) diff --git a/system/rqt_autoware_manual_controller/python/parts/mode_select.py b/system/rqt_autoware_manual_controller/python/parts/mode_select.py new file mode 100644 index 000000000..923f2e5c2 --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/parts/mode_select.py @@ -0,0 +1,43 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from python_qt_binding import QtWidgets +from rqt_autoware_manual_controller.parts.adapi import Adapi +from rqt_autoware_manual_controller.parts.adapi import ManualMode + + +class ManualModeSelect(QtWidgets.QVBoxLayout): + def __init__(self, adapi: Adapi): + super().__init__() + adapi.on_mode_list = self.on_mode_list + self.adapi = adapi + self.buttons = {mode: QtWidgets.QPushButton(mode.name) for mode in ManualMode} + for mode, button in self.buttons.items(): + button.setEnabled(mode is ManualMode.Disabled) + button.clicked.connect(lambda clicked, mode=mode: self.adapi.select_mode(mode)) + self.addWidget(button) + + def on_mode_list(self, modes): + for mode in modes: + self.buttons[mode].setEnabled(True) + + +class ManualModeStatus(QtWidgets.QLabel): + def __init__(self, adapi: Adapi): + super().__init__() + self.setText("Unknown") + adapi.on_mode_status = self.on_status + + def on_status(self, mode: ManualMode): + self.setText(mode.name) diff --git a/system/rqt_autoware_manual_controller/python/widget.py b/system/rqt_autoware_manual_controller/python/widget.py new file mode 100644 index 000000000..6e4130558 --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/widget.py @@ -0,0 +1,75 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from python_qt_binding import QtCore +from python_qt_binding import QtWidgets +from rqt_autoware_manual_controller.parts.adapi import Adapi +from rqt_autoware_manual_controller.parts.mode_select import ManualModeSelect +from rqt_autoware_manual_controller.parts.mode_select import ManualModeStatus + + +class ControllerWidget(QtWidgets.QSplitter): + def __init__(self, adapi: Adapi): + super().__init__() + self.adapi = adapi + self.mode_select = ManualModeSelect(self.adapi) + self.mode_status = ManualModeStatus(self.adapi) + + layout = QtWidgets.QGridLayout() + layout.addWidget(QtWidgets.QLabel("Item"), 0, 0) + layout.addWidget(QtWidgets.QLabel("Status"), 0, 1) + layout.addWidget(QtWidgets.QLabel("Command"), 0, 2) + layout.addWidget(QtWidgets.QLabel("Mode"), 1, 0) + layout.addWidget(self.mode_status, 1, 1) + layout.addLayout(self.mode_select, 1, 2) + layout.addWidget(QtWidgets.QLabel("Accel Pedal"), 2, 0) + layout.addWidget(QtWidgets.QLabel("Brake Pedal"), 3, 0) + layout.addWidget(QtWidgets.QLabel("Steering"), 4, 0) + layout.addWidget(QtWidgets.QLabel("Gear"), 5, 0) + layout.addWidget(QtWidgets.QLabel("Turn Indicator"), 6, 0) + layout.addWidget(QtWidgets.QLabel("Hazard Lights"), 7, 0) + layout.setRowStretch(8, 1) + widget = QtWidgets.QWidget() + widget.setLayout(layout) + + self.mouse = MouseCapture(self.adapi) + self.addWidget(self.mouse) + self.addWidget(widget) + + def shutdown(self): + pass + + +class MouseCapture(QtWidgets.QLabel): + def __init__(self, adapi: Adapi): + super().__init__() + self.adapi = adapi + self.setText("+") + self.setAlignment(QtCore.Qt.AlignCenter) + self.setStyleSheet("background-color: gray;") + self.setMouseTracking(False) + + def mouseMoveEvent(self, event): + w = self.size().width() / 2 + h = self.size().height() / 2 + x = (event.pos().x() - w) / w + y = (event.pos().y() - h) / h + x = -max(-1.0, min(1.0, x)) + y = -max(-1.0, min(1.0, y)) + steer = x + accel = max(0.0, +y) + brake = max(0.0, -y) + print("accel", accel, "brake", brake, "steer", steer) + self.adapi.set_pedals(accel, brake) + self.adapi.set_steering(steer) diff --git a/system/rqt_autoware_manual_controller/script/rqt_autoware_manual_controller b/system/rqt_autoware_manual_controller/script/rqt_autoware_manual_controller new file mode 100755 index 000000000..8aaad4d21 --- /dev/null +++ b/system/rqt_autoware_manual_controller/script/rqt_autoware_manual_controller @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 + +import sys + +import rqt_gui.main + +rqt_main = rqt_gui.main.Main() +sys.exit(rqt_main.main(sys.argv, standalone="rqt_autoware_manual_controller.ControllerPlugin")) From 0eebbf7aae55182ea5a903e75845b077d860a27c Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 27 Mar 2025 19:00:17 +0900 Subject: [PATCH 2/4] implement minimum features Signed-off-by: Takagi, Isamu --- .../python/__init__.py | 7 +- .../python/{parts => modules}/__init__,py | 0 .../python/modules/adapi.py | 203 ++++++++++++++++++ .../python/modules/gear.py | 35 +++ .../python/modules/hazard_lights.py | 39 ++++ .../python/modules/heartbeat.py | 30 +++ .../python/{parts => modules}/mode_select.py | 10 +- .../python/modules/mouse_control.py | 44 ++++ .../python/modules/turn_indicators.py | 46 ++++ .../python/parts/adapi.py | 147 ------------- .../python/widget.py | 106 +++++---- 11 files changed, 467 insertions(+), 200 deletions(-) rename system/rqt_autoware_manual_controller/python/{parts => modules}/__init__,py (100%) create mode 100644 system/rqt_autoware_manual_controller/python/modules/adapi.py create mode 100644 system/rqt_autoware_manual_controller/python/modules/gear.py create mode 100644 system/rqt_autoware_manual_controller/python/modules/hazard_lights.py create mode 100644 system/rqt_autoware_manual_controller/python/modules/heartbeat.py rename system/rqt_autoware_manual_controller/python/{parts => modules}/mode_select.py (83%) create mode 100644 system/rqt_autoware_manual_controller/python/modules/mouse_control.py create mode 100644 system/rqt_autoware_manual_controller/python/modules/turn_indicators.py delete mode 100644 system/rqt_autoware_manual_controller/python/parts/adapi.py diff --git a/system/rqt_autoware_manual_controller/python/__init__.py b/system/rqt_autoware_manual_controller/python/__init__.py index d819dc19b..ef3745916 100644 --- a/system/rqt_autoware_manual_controller/python/__init__.py +++ b/system/rqt_autoware_manual_controller/python/__init__.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from rqt_autoware_manual_controller.parts.adapi import Adapi +from rqt_autoware_manual_controller.modules.adapi import Adapi from rqt_autoware_manual_controller.widget import ControllerWidget from rqt_gui_py.plugin import Plugin @@ -27,7 +27,8 @@ def shutdown_plugin(self): self.widget.shutdown() def save_settings(self, plugin_settings, instance_settings): - pass + plugin_settings.set_value("SplitterState", self.widget.saveState()) def restore_settings(self, plugin_settings, instance_settings): - pass + if plugin_settings.contains("SplitterState"): + self.widget.restoreState(plugin_settings.value("SplitterState")) diff --git a/system/rqt_autoware_manual_controller/python/parts/__init__,py b/system/rqt_autoware_manual_controller/python/modules/__init__,py similarity index 100% rename from system/rqt_autoware_manual_controller/python/parts/__init__,py rename to system/rqt_autoware_manual_controller/python/modules/__init__,py diff --git a/system/rqt_autoware_manual_controller/python/modules/adapi.py b/system/rqt_autoware_manual_controller/python/modules/adapi.py new file mode 100644 index 000000000..68c82df14 --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/modules/adapi.py @@ -0,0 +1,203 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from enum import Enum + +from autoware_adapi_v1_msgs.msg import Gear +from autoware_adapi_v1_msgs.msg import GearCommand +from autoware_adapi_v1_msgs.msg import HazardLights +from autoware_adapi_v1_msgs.msg import HazardLightsCommand +from autoware_adapi_v1_msgs.msg import ManualControlMode +from autoware_adapi_v1_msgs.msg import ManualControlModeStatus +from autoware_adapi_v1_msgs.msg import ManualOperatorHeartbeat +from autoware_adapi_v1_msgs.msg import PedalsCommand +from autoware_adapi_v1_msgs.msg import SteeringCommand +from autoware_adapi_v1_msgs.msg import TurnIndicators +from autoware_adapi_v1_msgs.msg import TurnIndicatorsCommand +from autoware_adapi_v1_msgs.msg import VehicleStatus +from autoware_adapi_v1_msgs.srv import ListManualControlMode +from autoware_adapi_v1_msgs.srv import SelectManualControlMode +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class ManualMode(Enum): + Disabled = ManualControlMode.DISABLED + Pedals = ManualControlMode.PEDALS + Acceleration = ManualControlMode.ACCELERATION + Velocity = ManualControlMode.VELOCITY + + +class GearEnum(Enum): + Unknown = Gear.UNKNOWN + Neutral = Gear.NEUTRAL + Drive = Gear.DRIVE + Reverse = Gear.REVERSE + Park = Gear.PARK + Low = Gear.LOW + + +class TurnIndicatorsEnum(Enum): + Unknown = TurnIndicators.UNKNOWN + Disable = TurnIndicators.DISABLE + Left = TurnIndicators.LEFT + Right = TurnIndicators.RIGHT + + +class HazardLightsEnum(Enum): + Unknown = HazardLights.UNKNOWN + Disable = HazardLights.DISABLE + Enable = HazardLights.ENABLE + + +class HeartbeatEnum(Enum): + NotReady = False + Ready = True + + +class Adapi: + def __init__(self, node: Node, target: str): + # interfaces + qos_notification = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + qos_realtime = QoSProfile(depth=1, reliability=QoSReliabilityPolicy.BEST_EFFORT) + self._node = node + # fmt: off + self._cli_mode_list = node.create_client(ListManualControlMode, f"/api/manual/{target}/control_mode/list") # noqa: E221 + self._cli_mode_select = node.create_client(SelectManualControlMode, f"/api/manual/{target}/control_mode/select") # noqa: E221 + self._sub_mode_status = node.create_subscription(ManualControlModeStatus, f"/api/manual/{target}/control_mode/status", self._on_mode_status, qos_notification) # noqa: E221 + self._pub_pedals = node.create_publisher(PedalsCommand, f"/api/manual/{target}/command/pedals", 1) # noqa: E221 + self._pub_steering = node.create_publisher(SteeringCommand, f"/api/manual/{target}/command/steering", 1) # noqa: E221 + self._pub_gear = node.create_publisher(GearCommand, f"/api/manual/{target}/command/gear", qos_notification) # noqa: E221 + self._pub_turn_indicators = node.create_publisher(TurnIndicatorsCommand, f"/api/manual/{target}/command/turn_indicators", qos_notification) # noqa: E221 + self._pub_hazard_lights = node.create_publisher(HazardLightsCommand, f"/api/manual/{target}/command/hazard_lights", qos_notification) # noqa: E221 + self._pub_heartbeat = node.create_publisher(ManualOperatorHeartbeat, f"/api/manual/{target}/operator/heartbeat", qos_realtime) # noqa: E221 + self._sub_vehicle_status = node.create_subscription(VehicleStatus, "/api/vehicle/status", self._on_vehicle_status, qos_realtime) # noqa: E221 + # fmt: on + self._timer = node.create_timer(1.0, self._on_timer) + self._command_timer = node.create_timer(0.1, self._on_command_timer) + + # variables + self._mode_list = None + self._mode_status = None + self._gear_status = None + self._turn_indicators_status = None + self._hazard_lights_status = None + # message + self._msg_pedals = PedalsCommand() + self._msg_steering = SteeringCommand() + self._msg_gear = GearCommand() + self._msg_turn_indicators = TurnIndicatorsCommand() + self._msg_hazard_lights = HazardLightsCommand() + self._msg_heartbeat = ManualOperatorHeartbeat() + # callbacks + self._callback_mode_list = lambda _: None + self._callback_mode_status = lambda _: None + self._callback_gear_status = lambda _: None + self._callback_turn_indicators_status = lambda _: None + self._callback_hazard_lights_status = lambda _: None + # init + self.send_pedals(0.0, 0.5) + self.send_steering(0.0) + self.send_gear(GearEnum.Park) + self.send_turn_indicators(TurnIndicatorsEnum.Disable) + self.send_hazard_lights(HazardLightsEnum.Disable) + self.send_heartbeat(HeartbeatEnum.NotReady) + + def select_mode(self, mode: ManualMode): + self._cli_mode_select.call_async( + SelectManualControlMode.Request(mode=ManualControlMode(mode=mode.value)) + ) + + def set_on_mode_list(self, callback): + self._callback_mode_list = callback + if self._mode_list is not None: + self._callback_mode_list(self._mode_list) + + def set_on_mode_status(self, callback): + self._callback_mode_status = callback + if self._mode_status is not None: + self._callback_mode_status(self._mode_status) + + def set_on_gear_status(self, callback): + self._callback_gear_status = callback + if self._gear_status is not None: + self._callback_gear_status(self._gear_status) + + def set_on_turn_indicators_status(self, callback): + self._callback_turn_indicators_status = callback + if self._turn_indicators_status is not None: + self._callback_turn_indicators_status(self._turn_indicators_status) + + def set_on_hazard_lights_status(self, callback): + self._callback_hazard_lights_status = callback + if self._hazard_lights_status is not None: + self._callback_hazard_lights_status(self._hazard_lights_status) + + def _on_mode_list(self, future): + result = future.result() + self._mode_list = [ManualMode(mode.mode) for mode in result.modes] + self._callback_mode_list(self._mode_list) + + def _on_mode_status(self, status): + self._mode_status = ManualMode(status.mode.mode) + self._callback_mode_status(self._mode_status) + + def _on_timer(self): + if self._mode_list is None and self._cli_mode_list.service_is_ready(): + future = self._cli_mode_list.call_async(ListManualControlMode.Request()) + future.add_done_callback(self._on_mode_list) + + def _on_vehicle_status(self, status: VehicleStatus): + self._gear_status = GearEnum(status.gear.status) + self._turn_indicators_status = TurnIndicatorsEnum(status.turn_indicators.status) + self._hazard_lights_status = HazardLightsEnum(status.hazard_lights.status) + self._callback_gear_status(self._gear_status) + self._callback_turn_indicators_status(self._turn_indicators_status) + self._callback_hazard_lights_status(self._hazard_lights_status) + + def send_pedals(self, throttle: float, brake: float): + self._msg_pedals.throttle = throttle + self._msg_pedals.brake = brake + + def send_steering(self, steering: float): + self._msg_steering.steering_tire_angle = steering + + def send_gear(self, gear: GearEnum): + self._msg_gear.command.status = gear.value + + def send_turn_indicators(self, turn_indicators: TurnIndicatorsEnum): + self._msg_turn_indicators.command.status = turn_indicators.value + + def send_hazard_lights(self, hazard_lights: HazardLightsEnum): + self._msg_hazard_lights.command.status = hazard_lights.value + + def send_heartbeat(self, ready: HeartbeatEnum): + self._msg_heartbeat.ready = ready.value + + def _on_command_timer(self): + stamp = self._node.get_clock().now().to_msg() + self._msg_pedals.stamp = stamp + self._msg_steering.stamp = stamp + self._msg_gear.stamp = stamp + self._msg_turn_indicators.stamp = stamp + self._msg_hazard_lights.stamp = stamp + self._msg_heartbeat.stamp = stamp + self._pub_pedals.publish(self._msg_pedals) + self._pub_steering.publish(self._msg_steering) + self._pub_gear.publish(self._msg_gear) + self._pub_turn_indicators.publish(self._msg_turn_indicators) + self._pub_hazard_lights.publish(self._msg_hazard_lights) + self._pub_heartbeat.publish(self._msg_heartbeat) diff --git a/system/rqt_autoware_manual_controller/python/modules/gear.py b/system/rqt_autoware_manual_controller/python/modules/gear.py new file mode 100644 index 000000000..74b7da732 --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/modules/gear.py @@ -0,0 +1,35 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from python_qt_binding import QtWidgets +from rqt_autoware_manual_controller.modules.adapi import Adapi +from rqt_autoware_manual_controller.modules.adapi import GearEnum + + +class GearControl: + def __init__(self, adapi: Adapi): + self.adapi = adapi + self.adapi.set_on_gear_status(self.on_gear_status) + + self.command = QtWidgets.QVBoxLayout() + self.status = QtWidgets.QLabel() + + gears = [GearEnum.Drive, GearEnum.Reverse, GearEnum.Park, GearEnum.Neutral] + self.buttons = {gear: QtWidgets.QPushButton(gear.name) for gear in gears} + for gear, button in self.buttons.items(): + button.clicked.connect(lambda _, gear=gear: self.adapi.send_gear(gear)) + self.command.addWidget(button) + + def on_gear_status(self, gear: GearEnum): + self.status.setText(gear.name) diff --git a/system/rqt_autoware_manual_controller/python/modules/hazard_lights.py b/system/rqt_autoware_manual_controller/python/modules/hazard_lights.py new file mode 100644 index 000000000..0a4888dc5 --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/modules/hazard_lights.py @@ -0,0 +1,39 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from python_qt_binding import QtWidgets +from rqt_autoware_manual_controller.modules.adapi import Adapi +from rqt_autoware_manual_controller.modules.adapi import HazardLightsEnum + + +class HazardLightsControl: + def __init__(self, adapi: Adapi): + self.adapi = adapi + self.adapi.set_on_hazard_lights_status(self.on_hazard_lights_status) + + self.command = QtWidgets.QVBoxLayout() + self.status = QtWidgets.QLabel() + + hazard_lights = [HazardLightsEnum.Disable, HazardLightsEnum.Enable] + self.buttons = { + hazard_light: QtWidgets.QPushButton(hazard_light.name) for hazard_light in hazard_lights + } + for hazard_light, button in self.buttons.items(): + button.clicked.connect( + lambda _, hazard_light=hazard_light: self.adapi.send_hazard_lights(hazard_light) + ) + self.command.addWidget(button) + + def on_hazard_lights_status(self, hazard_lights: HazardLightsEnum): + self.status.setText(hazard_lights.name) diff --git a/system/rqt_autoware_manual_controller/python/modules/heartbeat.py b/system/rqt_autoware_manual_controller/python/modules/heartbeat.py new file mode 100644 index 000000000..b1fb3091e --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/modules/heartbeat.py @@ -0,0 +1,30 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from python_qt_binding import QtWidgets +from rqt_autoware_manual_controller.modules.adapi import Adapi +from rqt_autoware_manual_controller.modules.adapi import HeartbeatEnum + + +class HeartbeatControl: + def __init__(self, adapi: Adapi): + self.adapi = adapi + self.command = QtWidgets.QVBoxLayout() + self.status = QtWidgets.QLabel("---") + + modes = [HeartbeatEnum.NotReady, HeartbeatEnum.Ready] + self.buttons = {mode: QtWidgets.QPushButton(mode.name) for mode in modes} + for mode, button in self.buttons.items(): + button.clicked.connect(lambda _, mode=mode: self.adapi.send_heartbeat(mode)) + self.command.addWidget(button) diff --git a/system/rqt_autoware_manual_controller/python/parts/mode_select.py b/system/rqt_autoware_manual_controller/python/modules/mode_select.py similarity index 83% rename from system/rqt_autoware_manual_controller/python/parts/mode_select.py rename to system/rqt_autoware_manual_controller/python/modules/mode_select.py index 923f2e5c2..47ab54cc3 100644 --- a/system/rqt_autoware_manual_controller/python/parts/mode_select.py +++ b/system/rqt_autoware_manual_controller/python/modules/mode_select.py @@ -13,14 +13,14 @@ # limitations under the License. from python_qt_binding import QtWidgets -from rqt_autoware_manual_controller.parts.adapi import Adapi -from rqt_autoware_manual_controller.parts.adapi import ManualMode +from rqt_autoware_manual_controller.modules.adapi import Adapi +from rqt_autoware_manual_controller.modules.adapi import ManualMode class ManualModeSelect(QtWidgets.QVBoxLayout): def __init__(self, adapi: Adapi): super().__init__() - adapi.on_mode_list = self.on_mode_list + adapi.set_on_mode_list(self.on_mode_list) self.adapi = adapi self.buttons = {mode: QtWidgets.QPushButton(mode.name) for mode in ManualMode} for mode, button in self.buttons.items(): @@ -37,7 +37,7 @@ class ManualModeStatus(QtWidgets.QLabel): def __init__(self, adapi: Adapi): super().__init__() self.setText("Unknown") - adapi.on_mode_status = self.on_status + adapi.set_on_mode_status(self.on_mode_status) - def on_status(self, mode: ManualMode): + def on_mode_status(self, mode: ManualMode): self.setText(mode.name) diff --git a/system/rqt_autoware_manual_controller/python/modules/mouse_control.py b/system/rqt_autoware_manual_controller/python/modules/mouse_control.py new file mode 100644 index 000000000..4f0ad5fff --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/modules/mouse_control.py @@ -0,0 +1,44 @@ +from python_qt_binding import QtCore +from python_qt_binding import QtWidgets +from rqt_autoware_manual_controller.modules.adapi import Adapi + + +class MouseControl: + def __init__(self, adapi: Adapi): + self.adapi = adapi + self.capture = MouseCapture(self) + self.capture.setText("+") + self.capture.setAlignment(QtCore.Qt.AlignCenter) + self.capture.setStyleSheet("background-color: gray;") + self.capture.setMouseTracking(False) + self.accel = QtWidgets.QLabel() + self.brake = QtWidgets.QLabel() + self.steer = QtWidgets.QLabel() + + def update_pedals(self, accel: float, brake: float): + self.accel.setText(f"{accel:+0.2f}") + self.brake.setText(f"{brake:+0.2f}") + self.adapi.send_pedals(accel, brake) + + def update_steer(self, steer: float): + self.steer.setText(f"{steer:+0.2f}") + self.adapi.send_steering(steer) + + +class MouseCapture(QtWidgets.QLabel): + def __init__(self, control: MouseControl): + super().__init__() + self.control = control + + def mouseMoveEvent(self, event): + w = self.size().width() / 2 + h = self.size().height() / 2 + x = (event.pos().x() - w) / w + y = (event.pos().y() - h) / h + x = -max(-1.0, min(1.0, x)) + y = -max(-1.0, min(1.0, y)) + steer = x + accel = max(0.0, +y) + brake = max(0.0, -y) + self.control.update_pedals(accel, brake) + self.control.update_steer(steer) diff --git a/system/rqt_autoware_manual_controller/python/modules/turn_indicators.py b/system/rqt_autoware_manual_controller/python/modules/turn_indicators.py new file mode 100644 index 000000000..8ca4981dc --- /dev/null +++ b/system/rqt_autoware_manual_controller/python/modules/turn_indicators.py @@ -0,0 +1,46 @@ +# Copyright 2025 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from python_qt_binding import QtWidgets +from rqt_autoware_manual_controller.modules.adapi import Adapi +from rqt_autoware_manual_controller.modules.adapi import TurnIndicatorsEnum + + +class TurnIndicatorsControl: + def __init__(self, adapi: Adapi): + self.adapi = adapi + self.adapi.set_on_turn_indicators_status(self.on_turn_indicators_status) + + self.command = QtWidgets.QVBoxLayout() + self.status = QtWidgets.QLabel() + + turn_indicators = [ + TurnIndicatorsEnum.Disable, + TurnIndicatorsEnum.Left, + TurnIndicatorsEnum.Right, + ] + self.buttons = { + turn_indicator: QtWidgets.QPushButton(turn_indicator.name) + for turn_indicator in turn_indicators + } + for turn_indicator, button in self.buttons.items(): + button.clicked.connect( + lambda _, turn_indicator=turn_indicator: self.adapi.send_turn_indicators( + turn_indicator + ) + ) + self.command.addWidget(button) + + def on_turn_indicators_status(self, turn_indicators: TurnIndicatorsEnum): + self.status.setText(turn_indicators.name) diff --git a/system/rqt_autoware_manual_controller/python/parts/adapi.py b/system/rqt_autoware_manual_controller/python/parts/adapi.py deleted file mode 100644 index ead0e25de..000000000 --- a/system/rqt_autoware_manual_controller/python/parts/adapi.py +++ /dev/null @@ -1,147 +0,0 @@ -# Copyright 2025 The Autoware Contributors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from enum import Enum - -from autoware_adapi_v1_msgs.msg import Gear as GearMsg -from autoware_adapi_v1_msgs.msg import GearCommand -from autoware_adapi_v1_msgs.msg import HazardLights as HazardLightsMsg -from autoware_adapi_v1_msgs.msg import HazardLightsCommand -from autoware_adapi_v1_msgs.msg import ManualControlMode -from autoware_adapi_v1_msgs.msg import ManualControlModeStatus -from autoware_adapi_v1_msgs.msg import ManualOperatorStatus -from autoware_adapi_v1_msgs.msg import PedalsCommand -from autoware_adapi_v1_msgs.msg import SteeringCommand -from autoware_adapi_v1_msgs.msg import TurnIndicators as TurnIndicatorsMsg -from autoware_adapi_v1_msgs.msg import TurnIndicatorsCommand -from autoware_adapi_v1_msgs.srv import ListManualControlMode -from autoware_adapi_v1_msgs.srv import SelectManualControlMode -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile - - -class ManualMode(Enum): - Disabled = ManualControlMode.DISABLED - Pedals = ManualControlMode.PEDALS - Acceleration = ManualControlMode.ACCELERATION - Velocity = ManualControlMode.VELOCITY - - -class Gear(Enum): - Unknown = GearMsg.UNKNOWN - Neutral = GearMsg.NEUTRAL - Drive = GearMsg.DRIVE - Reverse = GearMsg.REVERSE - Park = GearMsg.PARK - Low = GearMsg.LOW - - -class TurnIndicators(Enum): - Disable = TurnIndicatorsMsg.DISABLE - Left = TurnIndicatorsMsg.LEFT - Right = TurnIndicatorsMsg.RIGHT - - -class HazardLights(Enum): - Disable = HazardLightsMsg.DISABLE - Enable = HazardLightsMsg.ENABLE - - -class Adapi: - def __init__(self, node: Node, target: str): - # interfaces - durable_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - self._node = node - # fmt: off - self._cli_mode_list = node.create_client(ListManualControlMode, f"/api/manual/{target}/control_mode/list") # noqa: E221 - self._cli_mode_select = node.create_client(SelectManualControlMode, f"/api/manual/{target}/control_mode/select") # noqa: E221 - self._sub_mode_status = node.create_subscription(ManualControlModeStatus, f"/api/manual/{target}/control_mode/status", self._on_mode_status, durable_qos) # noqa: E221 - self._pub_pedals = node.create_publisher(PedalsCommand, f"/api/manual/{target}/command/pedals", 1) # noqa: E221 - self._pub_steering = node.create_publisher(SteeringCommand, f"/api/manual/{target}/command/steering", 1) # noqa: E221 - self._pub_gear = node.create_publisher(GearCommand, f"/api/manual/{target}/command/gear", durable_qos) # noqa: E221 - self._pub_turn_indicators = node.create_publisher(TurnIndicatorsCommand, f"/api/manual/{target}/command/turn_indicators", durable_qos) # noqa: E221 - self._pub_hazard_lights = node.create_publisher(HazardLightsCommand, f"/api/manual/{target}/command/hazard_lights", durable_qos) # noqa: E221 - self._pub_heartbeat = node.create_publisher(ManualOperatorStatus, f"/api/manual/{target}/heartbeat", durable_qos) # noqa: E221 - # fmt: on - self._timer = node.create_timer(1.0, self._on_timer) - self._command_timer = node.create_timer(0.1, self._on_command_timer) - - # variables - self._is_mode_listed = False - self._msg_pedals = PedalsCommand() - self._msg_steering = SteeringCommand() - self._msg_gear = GearCommand() - self._msg_turn_indicators = TurnIndicatorsCommand() - self._msg_hazard_lights = HazardLightsCommand() - self._msg_heartbeat = ManualOperatorStatus() - # callbacks - self.on_mode_list = lambda _: None - self.on_mode_status = lambda _: None - # init - self.set_gear(Gear.Drive) - self.set_turn_indicators(TurnIndicators.Disable) - self.set_hazard_lights(HazardLights.Disable) - - def select_mode(self, mode: ManualMode): - self._cli_mode_select.call_async( - SelectManualControlMode.Request(mode=ManualControlMode(mode=mode.value)) - ) - - def _on_mode_status(self, status): - self.on_mode_status(ManualMode(status.mode.mode)) - - def _on_mode_list(self, future): - result = future.result() - self.on_mode_list([ManualMode(mode.mode) for mode in result.modes]) - self._is_mode_listed = True - - def _on_timer(self): - if not self._is_mode_listed and self._cli_mode_list.service_is_ready(): - future = self._cli_mode_list.call_async(ListManualControlMode.Request()) - future.add_done_callback(self._on_mode_list) - - def set_pedals(self, accel: float, brake: float): - self._msg_pedals.accelerator = accel - self._msg_pedals.brake = brake - - def set_steering(self, steering: float): - self._msg_steering.steering_tire_angle = steering - - def set_gear(self, gear: Gear): - self._msg_gear.command.status = gear.value - - def set_turn_indicators(self, turn_indicators: TurnIndicators): - self._msg_turn_indicators.command.status = turn_indicators.value - - def set_hazard_lights(self, hazard_lights: HazardLights): - self._msg_hazard_lights.command.status = hazard_lights.value - - def set_heartbeat(self, ready: bool): - self._msg_heartbeat.ready = ready - - def _on_command_timer(self): - stamp = self._node.get_clock().now().to_msg() - self._msg_pedals.stamp = stamp - self._msg_steering.stamp = stamp - self._msg_gear.stamp = stamp - self._msg_turn_indicators.stamp = stamp - self._msg_hazard_lights.stamp = stamp - self._msg_heartbeat.stamp = stamp - self._pub_pedals.publish(self._msg_pedals) - self._pub_steering.publish(self._msg_steering) - self._pub_gear.publish(self._msg_gear) - self._pub_turn_indicators.publish(self._msg_turn_indicators) - self._pub_hazard_lights.publish(self._msg_hazard_lights) - self._pub_heartbeat.publish(self._msg_heartbeat) diff --git a/system/rqt_autoware_manual_controller/python/widget.py b/system/rqt_autoware_manual_controller/python/widget.py index 6e4130558..adfa378d5 100644 --- a/system/rqt_autoware_manual_controller/python/widget.py +++ b/system/rqt_autoware_manual_controller/python/widget.py @@ -12,11 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -from python_qt_binding import QtCore from python_qt_binding import QtWidgets -from rqt_autoware_manual_controller.parts.adapi import Adapi -from rqt_autoware_manual_controller.parts.mode_select import ManualModeSelect -from rqt_autoware_manual_controller.parts.mode_select import ManualModeStatus +from rqt_autoware_manual_controller.modules.adapi import Adapi +from rqt_autoware_manual_controller.modules.gear import GearControl +from rqt_autoware_manual_controller.modules.hazard_lights import HazardLightsControl +from rqt_autoware_manual_controller.modules.heartbeat import HeartbeatControl +from rqt_autoware_manual_controller.modules.mode_select import ManualModeSelect +from rqt_autoware_manual_controller.modules.mode_select import ManualModeStatus +from rqt_autoware_manual_controller.modules.mouse_control import MouseControl +from rqt_autoware_manual_controller.modules.turn_indicators import TurnIndicatorsControl class ControllerWidget(QtWidgets.QSplitter): @@ -25,51 +29,63 @@ def __init__(self, adapi: Adapi): self.adapi = adapi self.mode_select = ManualModeSelect(self.adapi) self.mode_status = ManualModeStatus(self.adapi) - + self.mouse = MouseControl(self.adapi) + self.gear = GearControl(self.adapi) + self.turn_indicators = TurnIndicatorsControl(self.adapi) + self.hazard_lights = HazardLightsControl(self.adapi) + self.heartbeat = HeartbeatControl(self.adapi) + row = 0 layout = QtWidgets.QGridLayout() - layout.addWidget(QtWidgets.QLabel("Item"), 0, 0) - layout.addWidget(QtWidgets.QLabel("Status"), 0, 1) - layout.addWidget(QtWidgets.QLabel("Command"), 0, 2) - layout.addWidget(QtWidgets.QLabel("Mode"), 1, 0) - layout.addWidget(self.mode_status, 1, 1) - layout.addLayout(self.mode_select, 1, 2) - layout.addWidget(QtWidgets.QLabel("Accel Pedal"), 2, 0) - layout.addWidget(QtWidgets.QLabel("Brake Pedal"), 3, 0) - layout.addWidget(QtWidgets.QLabel("Steering"), 4, 0) - layout.addWidget(QtWidgets.QLabel("Gear"), 5, 0) - layout.addWidget(QtWidgets.QLabel("Turn Indicator"), 6, 0) - layout.addWidget(QtWidgets.QLabel("Hazard Lights"), 7, 0) - layout.setRowStretch(8, 1) + layout.addWidget(QtWidgets.QLabel("Item"), row, 0) + layout.addWidget(QtWidgets.QLabel("Status"), row, 1) + layout.addWidget(QtWidgets.QLabel("Command"), row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Mode"), row, 0) + layout.addWidget(self.mode_status, row, 1) + layout.addLayout(self.mode_select, row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Velocity"), row, 0) + layout.addWidget(QtWidgets.QLabel("---"), row, 1) + layout.addWidget(QtWidgets.QLabel("---"), row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Acceleration"), row, 0) + layout.addWidget(QtWidgets.QLabel("---"), row, 1) + layout.addWidget(QtWidgets.QLabel("---"), row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Throttle"), row, 0) + layout.addWidget(QtWidgets.QLabel("---"), row, 1) + layout.addWidget(self.mouse.accel, row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Brake"), row, 0) + layout.addWidget(QtWidgets.QLabel("---"), row, 1) + layout.addWidget(self.mouse.brake, row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Steering"), row, 0) + layout.addWidget(QtWidgets.QLabel("---"), row, 1) + layout.addWidget(self.mouse.steer, row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Gear"), row, 0) + layout.addWidget(self.gear.status, row, 1) + layout.addLayout(self.gear.command, row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Turn Indicator"), row, 0) + layout.addWidget(self.turn_indicators.status, row, 1) + layout.addLayout(self.turn_indicators.command, row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Hazard Lights"), row, 0) + layout.addWidget(self.hazard_lights.status, row, 1) + layout.addLayout(self.hazard_lights.command, row, 2) + row += 1 + layout.addWidget(QtWidgets.QLabel("Heartbeat"), row, 0) + layout.addWidget(self.heartbeat.status, row, 1) + layout.addLayout(self.heartbeat.command, row, 2) + row += 1 + layout.setRowStretch(row, 1) + widget = QtWidgets.QWidget() widget.setLayout(layout) - - self.mouse = MouseCapture(self.adapi) - self.addWidget(self.mouse) + self.addWidget(self.mouse.capture) self.addWidget(widget) def shutdown(self): pass - - -class MouseCapture(QtWidgets.QLabel): - def __init__(self, adapi: Adapi): - super().__init__() - self.adapi = adapi - self.setText("+") - self.setAlignment(QtCore.Qt.AlignCenter) - self.setStyleSheet("background-color: gray;") - self.setMouseTracking(False) - - def mouseMoveEvent(self, event): - w = self.size().width() / 2 - h = self.size().height() / 2 - x = (event.pos().x() - w) / w - y = (event.pos().y() - h) / h - x = -max(-1.0, min(1.0, x)) - y = -max(-1.0, min(1.0, y)) - steer = x - accel = max(0.0, +y) - brake = max(0.0, -y) - print("accel", accel, "brake", brake, "steer", steer) - self.adapi.set_pedals(accel, brake) - self.adapi.set_steering(steer) From 4382ddbd938e849273b01868e350de36fa05a42f Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 27 Mar 2025 19:53:21 +0900 Subject: [PATCH 3/4] fix qos Signed-off-by: Takagi, Isamu --- .../python/modules/adapi.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/system/rqt_autoware_manual_controller/python/modules/adapi.py b/system/rqt_autoware_manual_controller/python/modules/adapi.py index 68c82df14..577ab9ccf 100644 --- a/system/rqt_autoware_manual_controller/python/modules/adapi.py +++ b/system/rqt_autoware_manual_controller/python/modules/adapi.py @@ -78,12 +78,12 @@ def __init__(self, node: Node, target: str): self._cli_mode_list = node.create_client(ListManualControlMode, f"/api/manual/{target}/control_mode/list") # noqa: E221 self._cli_mode_select = node.create_client(SelectManualControlMode, f"/api/manual/{target}/control_mode/select") # noqa: E221 self._sub_mode_status = node.create_subscription(ManualControlModeStatus, f"/api/manual/{target}/control_mode/status", self._on_mode_status, qos_notification) # noqa: E221 - self._pub_pedals = node.create_publisher(PedalsCommand, f"/api/manual/{target}/command/pedals", 1) # noqa: E221 - self._pub_steering = node.create_publisher(SteeringCommand, f"/api/manual/{target}/command/steering", 1) # noqa: E221 - self._pub_gear = node.create_publisher(GearCommand, f"/api/manual/{target}/command/gear", qos_notification) # noqa: E221 + self._pub_pedals = node.create_publisher(PedalsCommand, f"/api/manual/{target}/command/pedals", qos_realtime) # noqa: E221 + self._pub_steering = node.create_publisher(SteeringCommand, f"/api/manual/{target}/command/steering", qos_realtime) # noqa: E221 + self._pub_gear = node.create_publisher(GearCommand, f"/api/manual/{target}/command/gear", qos_notification) # noqa: E221 self._pub_turn_indicators = node.create_publisher(TurnIndicatorsCommand, f"/api/manual/{target}/command/turn_indicators", qos_notification) # noqa: E221 - self._pub_hazard_lights = node.create_publisher(HazardLightsCommand, f"/api/manual/{target}/command/hazard_lights", qos_notification) # noqa: E221 - self._pub_heartbeat = node.create_publisher(ManualOperatorHeartbeat, f"/api/manual/{target}/operator/heartbeat", qos_realtime) # noqa: E221 + self._pub_hazard_lights = node.create_publisher(HazardLightsCommand, f"/api/manual/{target}/command/hazard_lights", qos_notification) # noqa: E221 + self._pub_heartbeat = node.create_publisher(ManualOperatorHeartbeat, f"/api/manual/{target}/operator/heartbeat", qos_realtime) # noqa: E221 self._sub_vehicle_status = node.create_subscription(VehicleStatus, "/api/vehicle/status", self._on_vehicle_status, qos_realtime) # noqa: E221 # fmt: on self._timer = node.create_timer(1.0, self._on_timer) From 6c5d31bfa1e39d2f1879e0b37c7416d0d1208e2f Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 27 Mar 2025 20:09:48 +0900 Subject: [PATCH 4/4] remove unused file Signed-off-by: Takagi, Isamu --- .../python/module.py | 63 ------------------- 1 file changed, 63 deletions(-) delete mode 100644 system/rqt_autoware_manual_controller/python/module.py diff --git a/system/rqt_autoware_manual_controller/python/module.py b/system/rqt_autoware_manual_controller/python/module.py deleted file mode 100644 index 5229d25d2..000000000 --- a/system/rqt_autoware_manual_controller/python/module.py +++ /dev/null @@ -1,63 +0,0 @@ -# Copyright 2025 The Autoware Contributors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -from rclpy.node import Node -from tier4_system_msgs.msg import DiagGraphStatus -from tier4_system_msgs.msg import DiagGraphStruct - -from .graph import Graph -from .utils import default_qos -from .utils import durable_qos -from .utils import foreach - - -class MonitorModule: - def __init__(self, node: Node): - self.graph = None - self.struct_callbacks = [] - self.status_callbacks = [] - self.node = node - self.sub_struct = self.subscribe_struct() - self.sub_status = self.subscribe_status() - - def append_struct_callback(self, callback): - self.struct_callbacks.append(callback) - - def append_status_callback(self, callback): - self.status_callbacks.append(callback) - - def on_struct(self, msg): - self.graph = Graph(msg) - foreach(self.struct_callbacks, lambda callback: callback(self.graph)) - - def on_status(self, msg): - if self.graph is None: - return - self.graph.update(msg) - foreach(self.status_callbacks, lambda callback: callback(self.graph)) - - def subscribe_struct(self): - return self.node.create_subscription( - DiagGraphStruct, "/diagnostics_graph/struct", self.on_struct, durable_qos(1) - ) - - def subscribe_status(self): - return self.node.create_subscription( - DiagGraphStatus, "/diagnostics_graph/status", self.on_status, default_qos(1) - ) - - def shutdown(self): - self.node.destroy_subscription(self.sub_struct) - self.node.destroy_subscription(self.sub_status)