Selaa lähdekoodia

receiving poses in NiceGUI node

Rodja Trappe 1 vuosi sitten
vanhempi
säilyke
e67c9c8f4f

+ 17 - 21
examples/ros2/ros2_ws/src/gui/gui/node.py

@@ -2,21 +2,22 @@ import threading
 from pathlib import Path
 
 import rclpy
-from geometry_msgs.msg import Twist
+from geometry_msgs.msg import Pose, Twist
 from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
 
 from nicegui import app, run, ui
 
 
-class TurtleTwistNode(Node):
+class NiceGuiNode(Node):
     def __init__(self) -> None:
         super().__init__('nicegui')
         self.linear = 0.0
         self.angular = 0.0
-        self.publisher_ = self.create_publisher(Twist, 'turtle1/cmd_vel', 1)
+        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 1)
         timer_period = 0.15  # seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
+        self.subscription = self.create_subscription(Pose, 'pose', self.on_pose, 1)
 
     def timer_callback(self) -> None:
         msg = Twist()
@@ -28,49 +29,44 @@ class TurtleTwistNode(Node):
         self.linear = x
         self.angular = y
 
+    def on_pose(self, msg: Pose) -> None:
+        print(f'pose: {msg}')
 
-active_node: TurtleTwistNode = None
+
+active_node: NiceGuiNode = None
 
 
 def node_spin(node: Node) -> None:
     try:
+        print('starting node')
         rclpy.spin(node)
     except ExternalShutdownException:
-        print('ROS2 was shutdown by NiceGUI.')
+        print('stopped node')
 
 
 @ui.page('/')
-def page() -> None:
-
+def index() -> None:
     with ui.row().classes('items-stretch'):
-        with ui.card():
+        with ui.card().classes('w-44'):
             ui.markdown('''
-                ## Guide
+                ## Control
 
-                This is a simple virtual joystick <br>
-                that sends twist commands to turtlesim.<br><br>
-                To control the turtle, just klick inside the blue <br>
-                field and drag your mouse around.
+                Publish steering commands by dragging your mouse around in the blue field
             ''')
-
-        with ui.card():
-            ui.markdown('### Turtlesim Joystick')
-
-            # NOTE: Joystick will be reworked in the future, so this is a temporary workaround for the size.
-            ui.add_head_html('<style>.my-joystick > div { width: 20em !important; height: 20em !important; }</style>')
             ui.joystick(
                 color='blue',
                 size=50,
                 on_move=lambda e: active_node.update_speeds(e.y, e.x),
                 on_end=lambda _: active_node.update_speeds(0.0, 0.0),
             ).classes('my-joystick')
-
         with ui.card():
             ui.markdown('### Status')
             ui.label('linear speed')
             ui.label().bind_text_from(active_node, 'linear', backward=lambda value: f'{value:.3f}')
             ui.label('angular speed')
             ui.label().bind_text_from(active_node, 'angular', backward=lambda value: f'{value:.3f}')
+        with ui.card().classes('w-96'):
+            map = ui.html()
 
 
 def main() -> None:
@@ -80,7 +76,7 @@ def main() -> None:
 def ros_main() -> None:
     global active_node
     rclpy.init()
-    active_node = TurtleTwistNode()
+    active_node = NiceGuiNode()
     threading.Thread(target=node_spin, args=(active_node,), daemon=True).start()
 
 

+ 6 - 7
examples/ros2/ros2_ws/src/simulator/simulator/node.py

@@ -6,20 +6,19 @@ from rclpy.node import Node
 class Simulator(Node):
 
     def __init__(self):
-        super().__init__('simple_simulator')
-        self.pose_publisher_ = self.create_publisher(Pose, 'pose', 10)
+        super().__init__('simulator')
+        self.pose_publisher_ = self.create_publisher(Pose, 'pose', 1)
         self.subscription = self.create_subscription(
             Twist,
             'cmd_vel',
             self.listener_callback,
             10)
+        self.pose = Pose()
 
     def listener_callback(self, msg):
-        pose = Pose()
-        pose.position.x += msg.linear.x
-        pose.position.y += 0
-        pose.orientation.z += msg.angular.z
-        self.pose_publisher_.publish(pose)
+        self.pose.position.x += msg.linear.x
+        self.pose.orientation.z += msg.angular.z
+        self.pose_publisher_.publish(self.pose)
 
 
 def main(args=None):