#!/usr/bin/env python3
import logging
import os
import threading
from pathlib import Path
import rclpy
import rclpy.parameter
import rosys
from ament_index_python.packages import get_package_share_directory
from feldfreund_devkit import FeldfreundHardware, FeldfreundSimulation, System, api
from feldfreund_devkit.config import Secrets, config_from_file
from nicegui import app, ui, ui_run
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from rosgraph_msgs.msg import Clock
from devkit_driver.modules import (
BMSHandler,
BumperHandler,
EStopHandler,
ImuHandler,
OdomHandler,
ReconDEMLogger,
RobotBrainHandler,
TwistHandler,
)
[docs]
class DevkitDriver(Node):
[docs]
def __init__(self, system: System):
"""Wire up hardware/simulation handlers, skipping any attribute the system lacks."""
super().__init__('devkit_driver_node')
self.system = system
if isinstance(self.system.feldfreund, FeldfreundHardware):
self.get_logger().info('Running in hardware mode')
self._robot_brain_handler = RobotBrainHandler(self, self.system.feldfreund.robot_brain)
elif isinstance(self.system.feldfreund, FeldfreundSimulation):
self.get_logger().info('Running in simulation mode')
self._clock_publisher = self.create_publisher(Clock, '/clock', 10)
self.set_parameters([rclpy.parameter.Parameter('use_sim_time',
rclpy.parameter.Parameter.Type.BOOL,
True)])
rosys.on_repeat(self._publish_clock, 0.01)
else:
raise TypeError(f'Unknown feldfreund type: {type(self.system.feldfreund)}')
# All other handlers work with both hardware and simulation
self._odom_handler = OdomHandler(self, self.system.odometer)
# Opt-in RTK recon logging for DEM building (see issue #110);
# subscribes to /gnss/fix + /odom directly, no hardware attr needed.
self._recon_dem_logger = ReconDEMLogger(self)
if hasattr(self.system.feldfreund, 'bms'):
self._bms_handler = BMSHandler(self, self.system.feldfreund.bms)
if getattr(self.system.feldfreund, 'bumper', None) is not None:
self._bumper_handler = BumperHandler(self, self.system.feldfreund.bumper, self.system.feldfreund.estop)
self._twist_handler = TwistHandler(self, self.system.feldfreund.wheels)
self._estop_handler = EStopHandler(self, self.system.feldfreund.estop)
# BNO085 IMU — publishes sensor_msgs/Imu on /imu/data when present.
if self.system.feldfreund.imu is not None:
self._imu_handler = ImuHandler(self, self.system.feldfreund.imu)
def _publish_clock(self) -> None:
current_time = rosys.time()
msg = Clock()
msg.clock.sec = int(current_time)
msg.clock.nanosec = int((current_time - int(current_time)) * 1e9)
self._clock_publisher.publish(msg)
[docs]
def destroy_node(self) -> None:
"""Close the recon DEM logger's CSV handle before the usual node teardown."""
# ReconDEMLogger holds an open CSV file handle for the process
# lifetime (see recon_dem_logger.py) — nothing else releases it.
try:
self._recon_dem_logger.close()
finally:
super().destroy_node()
[docs]
def main() -> None:
"""ROS entry point for colcon; execution is deferred to NiceGUI startup."""
[docs]
def on_startup() -> None:
"""Configures the hardware system and launches the ROS spin thread."""
# Priority 1: Standard ROS 2 share directory (post-build)
try:
pkg_share = get_package_share_directory('devkit_bringup')
config_path = Path(pkg_share) / 'config' / 'feldfreund.py'
except Exception:
config_path = Path('/workspace/src/devkit_bringup/config/feldfreund.py')
# Priority 2: Direct workspace source path (for development/hot-reloading)
if not config_path.exists():
config_path = Path('/workspace/src/devkit_bringup/config/feldfreund.py')
# Priority 3: Relative path from script (host-side execution fallback)
if not config_path.exists():
config_path = Path(__file__).parents[3] / 'devkit_bringup/config/feldfreund.py'
logging.basicConfig(level=logging.INFO, format="%(levelname)s: %(message)s")
log = logging.getLogger("devkit_driver.startup")
if not config_path.exists():
log.critical("Configuration file not found at %s", config_path)
os._exit(1)
log.info("Loading hardware configuration from %s", config_path)
simulation_mode = os.environ.get('FELDFREUND_SIMULATION', 'false').lower() in ('true', '1', 'yes')
if simulation_mode:
rosys.enter_simulation()
secrets = Secrets()
config = config_from_file(str(config_path), secrets=secrets)
system = System(config, secrets=secrets)
api.Online()
threading.Thread(target=ros_main, args=(system,), daemon=True).start()
[docs]
def ros_main(system: System) -> None:
"""Primary ROS 2 executor loop."""
rclpy.init()
devkit_driver = DevkitDriver(system)
try:
rclpy.spin(devkit_driver)
except (ExternalShutdownException, KeyboardInterrupt):
pass
finally:
# destroy_node() (closes the ReconDEMLogger CSV handle, see its
# override above) — rclpy.shutdown() alone never calls this.
devkit_driver.destroy_node()
if rclpy.ok():
rclpy.shutdown()
app.on_startup(on_startup)
# NiceGUI-specific configuration for Jazzy compatibility
ui_run.APP_IMPORT_STRING = f'{__name__}:app'
ui.run(
uvicorn_reload_dirs=str(Path(__file__).parent.resolve()),
title='Agroecology Lab Sowbot UI',
favicon='assets/favicon.ico',
dark=True
)