showing received position data

examples/ros2/Dockerfile

@@ -7,7 +7,7 @@ RUN apt-get update && apt-get install -y \
 
 SHELL ["/bin/bash", "-c"]
 
-RUN pip3 install nicegui
+RUN pip3 install nicegui pyquaternion
 
 ADD ros2_ws ros2_ws/
 

examples/ros2/ros2_ws/src/gui/gui/node.py

@@ -1,4 +1,5 @@
 import threading
+from dataclasses import dataclass
 from pathlib import Path
 
 import rclpy
@@ -9,49 +10,71 @@ from rclpy.node import Node
 from nicegui import app, run, ui
 
 
+@dataclass(kw_only=True)
+class User():
+    linear: ui.slider = None
+    angular: ui.slider = None
+    position: ui.label = None
+    map: ui.html = None
+
+
 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.users = []
 
         @ui.page('/')
         def index() -> None:
+            user = User()
             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(e.y, e.x),
+                        on_move=lambda e: self.send_speed(float(e.y), float(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')
                 with ui.card().classes('w-44 text-center items-center'):
                     ui.label('Data').classes('text-2xl')
-                    ui.label('linear speed')
-                    ui.label().bind_text_from(self, 'linear', backward=lambda value: f'{value:.3f}')
-                    ui.label('angular speed')
-                    ui.label().bind_text_from(self, 'angular', backward=lambda value: f'{value:.3f}')
-                with ui.card().classes('w-96 items-center'):
+                    ui.label('linear velocity').classes('text-xs mb-[-1.8em]')
+                    slider_props = 'readonly selection-color="transparent"'
+                    user.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]')
+                    user.angular = ui.slider(min=-1, max=1, step=0.05, value=0).props(slider_props)
+                    ui.label('position').classes('text-xs mt-8 mb-[-1.4em]')
+                    user.position = ui.label('x: 0.0, y: 0.0')
+                with ui.card().classes('w-96 h-96 items-center'):
                     ui.label('Visualization').classes('text-2xl')
-                    map = ui.html()
+                    user.map = ui.html()
+            self.users.append(user)
 
     def send_speed(self, x, y) -> None:
         msg = Twist()
-        msg.linear.x = float(x)
-        msg.angular.z = float(-y)
+        msg.linear.x = x
+        msg.angular.z = -y
+        for user in self.users:
+            user.linear.value = x
+            user.angular.value = y
         self.cmd_vel_publisher.publish(msg)
 
     def on_pose(self, msg: Pose) -> None:
-        print(f'pose: {msg}')
+        for user in self.users:
+            user.position.text = f'x: {msg.position.x:.2f}, y: {msg.position.y:.2f}'
+            user.map.content = f'''
+            <svg width="100%" height="100%" viewBox="-20 -20 40 40">
+                <circle cx="{msg.position.x}" cy="{msg.position.y}" r="1" fill="red" />
+            </svg>'''
 
 
 def node_spin(node: Node) -> None:
     try:
-        print('starting node')
+        print(f'starting node', flush=True)
         rclpy.spin(node)
     except ExternalShutdownException:
-        print('stopped node')
+        print(f'stopped node', flush=True)
 
 
 def main() -> None:

examples/ros2/ros2_ws/src/simulator/simulator/node.py

@@ -1,5 +1,8 @@
+import math
+
 import rclpy
 from geometry_msgs.msg import Pose, Twist
+from pyquaternion import Quaternion
 from rclpy.node import Node
 
 
@@ -10,10 +13,32 @@ class Simulator(Node):
         self.pose_publisher_ = self.create_publisher(Pose, 'pose', 1)
         self.subscription = self.create_subscription(Twist, 'cmd_vel', self.on_steering, 1)
         self.pose = Pose()
+        self.linear_velocity = 0.0
+        self.angular_velocity = 0.0
+        self.step_size = 0.1
+        self.timer = self.create_timer(self.step_size, self.update_pose)
 
     def on_steering(self, msg):
-        self.pose.position.x += msg.linear.x
-        self.pose.orientation.z += msg.angular.z
+        self.linear_velocity = msg.linear.x
+        self.angular_velocity = msg.angular.z
+        self.get_logger().info(f'linear: {self.linear_velocity}, angular: {self.angular_velocity}')
+
+    def update_pose(self):
+        quaternion = Quaternion(self.pose.orientation.w, self.pose.orientation.x,
+                                self.pose.orientation.y, self.pose.orientation.z)
+        theta = quaternion.yaw_pitch_roll[0]
+
+        self.pose.position.x += self.linear_velocity * math.cos(theta) * self.step_size
+        self.pose.position.y += self.linear_velocity * math.sin(theta) * self.step_size
+        self.get_logger().info(f'x: {self.pose.position.x}, y: {self.pose.position.y}')
+        theta += self.angular_velocity * self.step_size
+
+        quaternion = Quaternion(axis=[0.0, 0.0, 1.0], radians=theta)
+        self.pose.orientation.x = quaternion.x
+        self.pose.orientation.y = quaternion.y
+        self.pose.orientation.z = quaternion.z
+        self.pose.orientation.w = quaternion.w
+
         self.pose_publisher_.publish(self.pose)