drawing 3d scene for robot

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

@@ -4,6 +4,7 @@ from pathlib import Path
 
 import rclpy
 from geometry_msgs.msg import Pose, Twist
+from pyquaternion import Quaternion
 from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
 
@@ -15,7 +16,7 @@ class User():
     linear: ui.slider = None
     angular: ui.slider = None
     position: ui.label = None
-    map: ui.html = None
+    robot_3d: ui.scene.group = None
 
 
 class NiceGuiNode(Node):
@@ -36,7 +37,7 @@ class NiceGuiNode(Node):
                         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')
+                    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]')
@@ -44,12 +45,18 @@ class NiceGuiNode(Node):
                     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')
+                    ui.label('position').classes('text-xs mb-[-1.4em]')
+                    user.position = ui.label('---')
                 with ui.card().classes('w-96 h-96 items-center'):
                     ui.label('Visualization').classes('text-2xl')
-                    user.map = ui.html()
+                    with ui.scene(350, 300) as scene:
+                        with scene.group() as user.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)
             self.users.append(user)
+            app.on_disconnect(lambda: self.users.remove(user))
 
     def send_speed(self, x, y) -> None:
         msg = Twist()
@@ -63,18 +70,18 @@ class NiceGuiNode(Node):
     def on_pose(self, msg: Pose) -> None:
         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>'''
+            user.position.update()
+            user.robot_3d.move(msg.position.x, msg.position.y)
+            quaternion = Quaternion(msg.orientation.w, msg.orientation.x,
+                                    msg.orientation.y, msg.orientation.z)
+            user.robot_3d.rotate(0, 0, quaternion.yaw_pitch_roll[0])
 
 
 def node_spin(node: Node) -> None:
     try:
-        print(f'starting node', flush=True)
         rclpy.spin(node)
     except ExternalShutdownException:
-        print(f'stopped node', flush=True)
+        pass
 
 
 def main() -> None:
@@ -89,4 +96,4 @@ def ros_main() -> None:
 
 app.on_startup(lambda: threading.Thread(target=ros_main).start())
 run.APP_IMPORT_STRING = f'{__name__}:app'
-ui.run(uvicorn_reload_dirs=str(Path(__file__).parent.resolve()))
+ui.run(uvicorn_reload_dirs=str(Path(__file__).parent.resolve()), favicon='🤖')

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

@@ -21,7 +21,6 @@ class Simulator(Node):
     def on_steering(self, msg):
         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,
@@ -30,7 +29,6 @@ class Simulator(Node):
 
         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)