|
@@ -1,11 +1,9 @@
|
|
|
+import math
|
|
|
import threading
|
|
|
-from dataclasses import dataclass
|
|
|
from pathlib import Path
|
|
|
-from typing import List
|
|
|
|
|
|
import rclpy
|
|
|
from geometry_msgs.msg import Pose, Twist
|
|
|
-from pyquaternion import Quaternion
|
|
|
from rclpy.executors import ExternalShutdownException
|
|
|
from rclpy.node import Node
|
|
|
|
|
@@ -13,25 +11,24 @@ from nicegui import app, globals, run, ui
|
|
|
|
|
|
|
|
|
class NiceGuiNode(Node):
|
|
|
+
|
|
|
def __init__(self) -> None:
|
|
|
super().__init__('nicegui')
|
|
|
self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 1)
|
|
|
- self.subscription = self.create_subscription(Pose, 'pose', self.on_pose, 1)
|
|
|
+ self.subscription = self.create_subscription(Pose, 'pose', self.handle_pose, 1)
|
|
|
|
|
|
with globals.index_client:
|
|
|
with ui.row().classes('items-stretch'):
|
|
|
with ui.card().classes('w-44 text-center items-center'):
|
|
|
ui.label('Control').classes('text-2xl')
|
|
|
- ui.joystick(
|
|
|
- color='blue', size=50,
|
|
|
- on_move=lambda e: self.send_speed(float(e.y), float(e.x)),
|
|
|
- on_end=lambda _: self.send_speed(0.0, 0.0),
|
|
|
- )
|
|
|
+ ui.joystick(color='blue', size=50,
|
|
|
+ on_move=lambda e: self.send_speed(e.y, e.x),
|
|
|
+ on_end=lambda _: self.send_speed(0.0, 0.0))
|
|
|
ui.label('Publish steering commands by dragging your mouse around in the blue field').classes('mt-6')
|
|
|
with ui.card().classes('w-44 text-center items-center'):
|
|
|
ui.label('Data').classes('text-2xl')
|
|
|
ui.label('linear velocity').classes('text-xs mb-[-1.8em]')
|
|
|
- slider_props = 'readonly selection-color="transparent"'
|
|
|
+ slider_props = 'readonly selection-color=transparent'
|
|
|
self.linear = ui.slider(min=-1, max=1, step=0.05, value=0).props(slider_props)
|
|
|
ui.label('angular velocity').classes('text-xs mb-[-1.8em]')
|
|
|
self.angular = ui.slider(min=-1, max=1, step=0.05, value=0).props(slider_props)
|
|
@@ -41,12 +38,10 @@ class NiceGuiNode(Node):
|
|
|
ui.label('Visualization').classes('text-2xl')
|
|
|
with ui.scene(350, 300) as scene:
|
|
|
with scene.group() as self.robot_3d:
|
|
|
- prism = [(-0.5, -0.5), (0.5, -0.5), (0.75, 0), (0.5, 0.5), (-0.5, 0.5)]
|
|
|
- outline = list(map(list, prism))
|
|
|
- self.robot_object = scene.extrusion(outline, 0.4)
|
|
|
- self.robot_object.material('#4488ff', 0.5)
|
|
|
+ prism = [[-0.5, -0.5], [0.5, -0.5], [0.75, 0], [0.5, 0.5], [-0.5, 0.5]]
|
|
|
+ self.robot_object = scene.extrusion(prism, 0.4).material('#4488ff', 0.5)
|
|
|
|
|
|
- def send_speed(self, x, y) -> None:
|
|
|
+ def send_speed(self, x: float, y: float) -> None:
|
|
|
msg = Twist()
|
|
|
msg.linear.x = x
|
|
|
msg.angular.z = -y
|
|
@@ -54,12 +49,10 @@ class NiceGuiNode(Node):
|
|
|
self.angular.value = y
|
|
|
self.cmd_vel_publisher.publish(msg)
|
|
|
|
|
|
- def on_pose(self, msg: Pose) -> None:
|
|
|
+ def handle_pose(self, msg: Pose) -> None:
|
|
|
self.position.text = f'x: {msg.position.x:.2f}, y: {msg.position.y:.2f}'
|
|
|
- self.position.update()
|
|
|
self.robot_3d.move(msg.position.x, msg.position.y)
|
|
|
- quaternion = Quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z)
|
|
|
- self.robot_3d.rotate(0, 0, quaternion.yaw_pitch_roll[0])
|
|
|
+ self.robot_3d.rotate(0, 0, 2 * math.atan2(msg.orientation.z, msg.orientation.w))
|
|
|
|
|
|
|
|
|
def node_spin(node: Node) -> None:
|