teleop bootstrap to tailwind migration, performance fixes, waypoints module refactor

CMakeLists.txt

@@ -59,6 +59,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rtcm_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(action_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(urdf REQUIRED)
@@ -151,6 +152,9 @@ message(STATUS "Found message files: ${MROVER_MESSAGE_FILE_PATHS}")
 file(GLOB_RECURSE MROVER_SERVICE_FILE_PATHS RELATIVE ${CMAKE_CURRENT_LIST_DIR} CONFIGURE_DEPENDS srv/*.srv)
 message(STATUS "Found service files: ${MROVER_SERVICE_FILE_PATHS}")
 
+file(GLOB_RECURSE MROVER_ACTION_FILE_PATHS RELATIVE ${CMAKE_CURRENT_LIST_DIR} CONFIGURE_DEPENDS action/*.action)
+message(STATUS "Found action files: ${MROVER_ACTION_FILE_PATHS}")
+
 # Starter Project
 message(STATUS "PROJ NAME: ${PROJECT_NAME}")
 
@@ -167,7 +171,8 @@ message(STATUS "Found message files: ${MROVER_MESSAGE_FILE_PATHS}")
 rosidl_generate_interfaces(${PROJECT_NAME}
     ${MROVER_MESSAGE_FILE_PATHS}
     ${MROVER_SERVICE_FILE_PATHS}
-    DEPENDENCIES nav_msgs sensor_msgs geometry_msgs
+    ${MROVER_ACTION_FILE_PATHS}
+    DEPENDENCIES nav_msgs sensor_msgs geometry_msgs action_msgs
 )
 
 # Starter project 

action/KeyAction.action

@@ -0,0 +1,6 @@
+string code
+---
+bool success
+---
+char key
+string state

launch/base.launch.py

@@ -28,6 +28,4 @@ def generate_launch_description():
         parameters=[os.path.join(get_package_share_directory("mrover"), "config", "superstructure.yaml")],
     )
 
-    led_node = Node(package="mrover", executable="led", name="led")
-
-    return LaunchDescription([diff_drive_controller_node, superstructure_node, led_node])
+    return LaunchDescription([diff_drive_controller_node, superstructure_node])

launch/jetson_base.launch.py

@@ -41,8 +41,6 @@ def generate_launch_description():
         ],
     )
 
-    led_node = Node(package="mrover", executable="led", name="led")
-
     pdlb_hw_bridge_node = Node(
         package="mrover",
         executable="pdlb_hw_bridge",
@@ -109,7 +107,6 @@ def generate_launch_description():
             launch_include_can,
             diff_drive_controller_node,
             superstructure_node,
-            led_node,
             drive_hw_bridge_node,
             pdlb_hw_bridge_node,
             mob_left_streamer_node,

launch/teleop_testing.launch.py

@@ -0,0 +1,44 @@
+import os
+from pathlib import Path
+
+from ament_index_python import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    share = get_package_share_directory("mrover")
+
+    launch_simulator = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(share, "launch/simulator.launch.py")),
+        launch_arguments={"rviz": "false"}.items(),
+    )
+
+    launch_navigation = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(share, "launch/navigation.launch.py"))
+    )
+
+    launch_basestation = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(share, "launch/basestation.launch.py")),
+        launch_arguments={"mode": LaunchConfiguration("mode")}.items(),
+    )
+
+    cost_map_node = Node(
+        package="mrover",
+        executable="cost_map",
+        name="cost_map",
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("mode", default_value="dev", description="Basestation mode: dev or prod"),
+            launch_simulator,
+            launch_navigation,
+            launch_basestation,
+            cost_map_node,
+        ]
+    )

teleoperation/basestation_gui/backend/database.py

@@ -1,6 +1,9 @@
+import logging
 import sqlite3
 import os
 
+logger = logging.getLogger(__name__)
+
 BASE_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), '../..')
 
 # Ensure base directory exists
@@ -60,14 +63,14 @@ def init_waypoints_db():
         cursor.execute("SELECT count(*) FROM auton_waypoints")
         if cursor.fetchone()[0] == 0:
             defaults = [
-                ("No Search 1", 0, 0, 0.0, 0.0, 1, 0), # Not deletable
-                ("No Search 2", 1, 0, 0.0, 0.0, 1, 0), # Not deletable
-                ("Post 1", 2, 1, 0.0, 0.0, 1, 0),      # Not deletable
-                ("Post 2", 3, 1, 0.0, 0.0, 1, 0),      # Not deletable
-                ("Post 3", 4, 1, 0.0, 0.0, 1, 0),      # Not deletable
-                ("Mallet", 5, 2, 0.0, 0.0, 1, 0),      # Not deletable
-                ("Water Bottle", 6, 3, 0.0, 0.0, 1, 0),# Not deletable
-                ("Rock Pick", 7, 4, 0.0, 0.0, 1, 0),   # Not deletable
+                ("No Search 1", 0, 0, 0.0, 0.0, 1, 0),
+                ("No Search 2", 1, 0, 0.0, 0.0, 1, 0),
+                ("Post 1", 2, 1, 0.0, 0.0, 1, 0),
+                ("Post 2", 3, 1, 0.0, 0.0, 1, 0),
+                ("Post 3", 4, 1, 0.0, 0.0, 1, 0),
+                ("Mallet", 5, 2, 0.0, 0.0, 1, 0),
+                ("Water Bottle", 6, 3, 0.0, 0.0, 1, 0),
+                ("Rock Pick", 7, 4, 0.0, 0.0, 1, 0),
             ]
             cursor.executemany('''
                 INSERT INTO auton_waypoints (name, tag_id, type, latitude, longitude, enable_costmap, deletable)
@@ -80,9 +83,9 @@ def init_waypoints_db():
 
         conn.commit()
         conn.close()
-        print(f"Waypoints database initialized: {WAYPOINTS_DB}")
+        logger.info(f"Waypoints database initialized: {WAYPOINTS_DB}")
     except Exception as e:
-        print(f"Error initializing waypoints database: {e}")
+        logger.error(f"Error initializing waypoints database: {e}")
         raise
 
 def init_recordings_db():
@@ -118,9 +121,9 @@ def init_recordings_db():
 
         conn.commit()
         conn.close()
-        print(f"Recordings database initialized: {RECORDINGS_DB}")
+        logger.info(f"Recordings database initialized: {RECORDINGS_DB}")
     except Exception as e:
-        print(f"Error initializing recordings database: {e}")
+        logger.error(f"Error initializing recordings database: {e}")
         raise
 
 def get_waypoints_db():
@@ -137,6 +140,11 @@ def get_recordings_db():
 def get_db_connection():
     return get_waypoints_db()
 
-# Initialize on module load
-init_waypoints_db()
-init_recordings_db()
+_initialized = False
+
+def ensure_initialized():
+    global _initialized
+    if not _initialized:
+        init_waypoints_db()
+        init_recordings_db()
+        _initialized = True

teleoperation/basestation_gui/backend/managers/led.py

@@ -25,6 +25,7 @@ def get_led_publisher(self):
         if self.led_pub is None:
             node = get_node()
             self.led_pub = node.create_publisher(LED, "/led", 1)
+            node.create_timer(0.5, self.publish_led)
         return self.led_pub
 
     def set_teleop_enabled(self, enabled: bool):
@@ -37,26 +38,22 @@ def set_nav_state(self, state: str):
 
     def update_led(self):
         if self.teleop_enabled:
-            new_color = "blue"
+            self.current_led_color = "blue"
         elif self.nav_state == "DoneState":
-            new_color = "blinking-green"
+            self.current_led_color = "blinking-green"
         else:
-            new_color = "red"
-
-        if new_color != self.current_led_color:
-            self.current_led_color = new_color
-
-            led_pub = self.get_led_publisher()
-            led_msg = LED()
-
-            if new_color == "red":
-                led_msg.color = LED.RED
-            elif new_color == "blue":
-                led_msg.color = LED.BLUE
-            elif new_color == "blinking-green":
-                led_msg.color = LED.BLINKING_GREEN
-
-            led_pub.publish(led_msg)
+            self.current_led_color = "red"
+        self.publish_led()
+
+    def publish_led(self):
+        led_msg = LED()
+        if self.current_led_color == "red":
+            led_msg.color = LED.RED
+        elif self.current_led_color == "blue":
+            led_msg.color = LED.BLUE
+        elif self.current_led_color == "blinking-green":
+            led_msg.color = LED.BLINKING_GREEN
+        self.get_led_publisher().publish(led_msg)
 
 
 manager = LEDManager()

teleoperation/basestation_gui/backend/managers/recording.py

@@ -1,5 +1,6 @@
+import threading
 from typing import Optional
-from backend.managers.ros import get_node
+from backend.managers.ros import get_node, get_logger
 from backend.database import get_recordings_db
 from sensor_msgs.msg import NavSatFix
 from rclpy.qos import qos_profile_sensor_data
@@ -74,7 +75,7 @@ def recording_callback(self):
             conn.commit()
             self.recording_sequence += 1
         except Exception as e:
-            print(f"Failed to save waypoint: {e}")
+            get_logger().error(f"Failed to save waypoint: {e}")
         finally:
             if conn:
                 conn.close()
@@ -103,7 +104,7 @@ def start_recording(self, name: str, is_drone: bool) -> int:
             self.recording_callback
         )
 
-        print(f"Started recording: {name} (ID: {recording_id}) at {RECORDING_RATE_HZ}Hz")
+        get_logger().info(f"Started recording: {name} (ID: {recording_id}) at {RECORDING_RATE_HZ}Hz")
 
         return recording_id
 
@@ -123,7 +124,7 @@ def stop_recording(self) -> dict:
         self.recording_sequence = 0
         self.is_drone_recording = False
 
-        print(f"Stopped recording ID: {recording_id} with {waypoint_count} waypoints")
+        get_logger().info(f"Stopped recording ID: {recording_id} with {waypoint_count} waypoints")
 
         return {
             "recording_id": recording_id,
@@ -139,11 +140,14 @@ def get_status(self) -> dict:
         }
 
 
-recording_manager = None
+_recording_manager = None
+_recording_manager_lock = threading.Lock()
 
 
 def get_recording_manager() -> RecordingManager:
-    global recording_manager
-    if recording_manager is None:
-        recording_manager = RecordingManager()
-    return recording_manager
+    global _recording_manager
+    if _recording_manager is None:
+        with _recording_manager_lock:
+            if _recording_manager is None:
+                _recording_manager = RecordingManager()
+    return _recording_manager

teleoperation/basestation_gui/backend/managers/ros.py

@@ -1,9 +1,12 @@
+import logging
 import rclpy
 import threading
 import atexit
 from rclpy.node import Node
 from rclpy.executors import SingleThreadedExecutor
 
+logger = logging.getLogger(__name__)
+
 lock = threading.Lock()
 initialized = threading.Event()
 
@@ -28,9 +31,13 @@ def get_logger():
 
 def get_service_client(srv_type, srv_name):
     with service_clients_lock:
-        if srv_name not in service_clients:
-            n = get_node()
-            service_clients[srv_name] = n.create_client(srv_type, srv_name)
+        if srv_name in service_clients:
+            client = service_clients[srv_name]
+            if client.service_is_ready():
+                return client
+            client.destroy()
+        n = get_node()
+        service_clients[srv_name] = n.create_client(srv_type, srv_name)
         return service_clients[srv_name]
 
 
@@ -54,7 +61,7 @@ def ros_spin():
 def init_ros():
     global context, node, ros_thread
 
-    print("Initializing ROS Manager...")
+    logger.info("Initializing ROS Manager...")
     context = rclpy.Context()
     rclpy.init(context=context)
 
@@ -64,13 +71,13 @@ def init_ros():
     ros_thread = threading.Thread(target=ros_spin, daemon=True)
     ros_thread.start()
 
-    print("ROS Manager started in a background thread.")
+    logger.info("ROS Manager started in a background thread.")
     initialized.set()
 
 
 def shutdown_ros():
     global context, node
-    print("Shutting down ROS Manager...")
+    logger.info("Shutting down ROS Manager...")
     if context and rclpy.ok(context=context):
         if node:
             node.destroy_node()