Add GUI

crazyflie/CMakeLists.txt

@@ -109,6 +109,7 @@ install(PROGRAMS
  scripts/vel_mux.py
  scripts/cfmult.py
  scripts/simple_mapper_multiranger.py
+ scripts/gui.py
  DESTINATION lib/${PROJECT_NAME}
 )
 

crazyflie/config/crazyflies.yaml

@@ -72,11 +72,13 @@ all:
  # firmware logging for all drones (use robot_types/type_name to set per type, or
  # robots/drone_name to set per drone)
  firmware_logging:
- enabled: false
+ enabled: true
  default_topics:
  # remove to disable default topic
- pose:
- frequency: 10 # Hz
+ # pose:
+ # frequency: 10 # Hz
+ status:
+ frequency: 1 # Hz
  #custom_topics:
  # topic_name1:
  # frequency: 10 # Hz

crazyflie/scripts/gui.py

@@ -0,0 +1,230 @@
+#!/usr/bin/env python3
+
+import threading
+import time
+from pathlib import Path
+from functools import partial
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rcl_interfaces.msg import Log
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+
+# from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+from crazyflie_interfaces.msg import Status
+import rowan
+
+from nicegui import Client, app, events, ui, ui_run
+
+
+class NiceGuiNode(Node):
+
+ def __init__(self) -> None:
+ super().__init__('nicegui')
+
+ # find all crazyflies
+ self.cfnames = []
+ for srv_name, srv_types in self.get_service_names_and_types():
+ if 'crazyflie_interfaces/srv/StartTrajectory' in srv_types:
+ # remove '/' and '/start_trajectory'
+ cfname = srv_name[1:-17]
+ if cfname != 'all':
+ self.cfnames.append(cfname)
+
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+
+ self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 1)
+ self.sub_log = self.create_subscription(Log, 'rosout', self.on_rosout, rclpy.qos.QoSProfile(
+ depth=1000,
+ durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL))
+
+ self.logs = dict()
+ self.supervisor_labels = dict()
+ self.battery_labels = dict()
+ self.radio_labels = dict()
+ self.robotmodels = dict()
+
+ with Client.auto_index_client:
+
+ with ui.row().classes('items-stretch'):
+ with ui.card().classes('w-full h-full'):
+ ui.label('Visualization').classes('text-2xl')
+ with ui.scene(800, 400, on_click=self.on_vis_click) as scene:
+ for name in self.cfnames:
+ robot = scene.stl('/urdf/cf2_assembly.stl').scale(1.0).material('#ff0000').with_name(name)
+ self.robotmodels[name] = robot
+ # augment with some additional fields
+ robot.status_ok = False
+ robot.status_watchdog = time.time()
+ scene.camera.x = 0
+ scene.camera.y = -1
+ scene.camera.z = 2
+ scene.camera.look_at_x = 0
+ scene.camera.look_at_y = 0
+ scene.camera.look_at_z = 0
+
+ with ui.row().classes('w-full h-lvh'):
+ with ui.tabs().classes('w-full') as tabs:
+ self.tabs = []
+ for name in ["all"] + self.cfnames:
+ self.tabs.append(ui.tab(name))
+ with ui.tab_panels(tabs, value=self.tabs[0], on_change=self.on_tab_change).classes('w-full') as self.tabpanels:
+ for name, tab in zip(["all"] + self.cfnames, self.tabs):
+ with ui.tab_panel(tab):
+ self.logs[name] = ui.log().classes('w-full h-96 no-wrap')
+ self.supervisor_labels[name] = ui.label("")
+ self.battery_labels[name] = ui.label("")
+ self.radio_labels[name] = ui.label("")
+
+ for name in self.cfnames:
+ self.create_subscription(Status, name + '/status', partial(self.on_status, name=name), 1)
+
+ # Call on_timer function
+ update_rate = 30 # Hz
+ self.timer = self.create_timer(
+ 1.0/update_rate, 
+ self.on_timer,
+ clock=rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.SYSTEM_TIME))
+
+ def on_rosout(self, msg: Log) -> None:
+ # filter by crazyflie and add to the correct log
+ if msg.name == "crazyflie_server":
+ if msg.msg.startswith("["):
+ idx = msg.msg.find("]")
+ name = msg.msg[1:idx]
+ # if it was an "all" category, add only to CFs
+ if name == 'all':
+ for logname ,log in self.logs.items():
+ if logname != "all":
+ log.push(msg.msg)
+ elif name in self.logs:
+ self.logs[name].push(msg.msg[idx+2:])
+
+ # add all possible messages to the 'all' tab
+ self.logs['all'].push(msg.msg)
+
+ def on_timer(self) -> None:
+ for name, robotmodel in self.robotmodels.items():
+ ros_time = rclpy.time.Time() # get the latest
+ robot_status_ok = robotmodel.status_ok
+ if self.tf_buffer.can_transform("world", name, ros_time):
+ t = self.tf_buffer.lookup_transform(
+ "world",
+ name,
+ ros_time)
+ transform_time = rclpy.time.Time.from_msg(t.header.stamp)
+ transform_age = self.get_clock().now() - transform_time
+ # latest transform is older than a second indicates a problem
+ if transform_age.nanoseconds * 1e-9 > 1:
+ robot_status_ok = False
+ else:
+ pos = t.transform.translation
+ robotmodel.move(pos.x, pos.y, pos.z)
+ robotmodel.rotate(*rowan.to_euler([
+ t.transform.rotation.w,
+ t.transform.rotation.x,
+ t.transform.rotation.y,
+ t.transform.rotation.z], "xyz"))
+ else:
+ # no available transform indicates a problem
+ robot_status_ok = False
+
+ # no status update for a while, indicate a problem
+ if time.time() - robotmodel.status_watchdog > 2.0:
+ robot_status_ok = False
+
+ # any issues detected -> mark red, otherwise green
+ if robot_status_ok:
+ robotmodel.material('#00ff00')
+ else:
+ robotmodel.material('#ff0000')
+
+ def on_vis_click(self, e: events.SceneClickEventArguments):
+ hit = e.hits[0]
+ name = hit.object_name or hit.object_id
+ ui.notify(f'You clicked on the {name}')
+ if name == 'ground':
+ self.tabpanels.value = 'all'
+ else:
+ self.tabpanels.value = name
+
+ def on_status(self, msg, name) -> None:
+ status_ok = True
+ supervisor_text = ""
+ if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_BE_ARMED:
+ supervisor_text += "can be armed; "
+ if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_ARMED:
+ supervisor_text += "is armed; "
+ if msg.supervisor_info & Status.SUPERVISOR_INFO_AUTO_ARM:
+ supervisor_text += "auto-arm; "
+ if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_FLY:
+ supervisor_text += "can fly; "
+ if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_FLYING:
+ supervisor_text += "is flying; "
+ if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_TUMBLED:
+ supervisor_text += "is tumpled; "
+ status_ok = False
+ if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_LOCKED:
+ supervisor_text += "is locked; "
+ status_ok = False
+ self.supervisor_labels[name].set_text(supervisor_text)
+
+ battery_text = f'{msg.battery_voltage:.2f} V'
+ if msg.battery_voltage < 3.8:
+ status_ok = False
+ if msg.pm_state == Status.PM_STATE_BATTERY:
+ battery_text += " (on battery)"
+ elif msg.pm_state == Status.PM_STATE_CHARGING:
+ battery_text += " (charging)"
+ elif msg.pm_state == Status.PM_STATE_CHARGED:
+ battery_text += " (charged)"
+ elif msg.pm_state == Status.PM_STATE_LOW_POWER:
+ battery_text += " (low power)"
+ status_ok = False
+ elif msg.pm_state == Status.PM_STATE_SHUTDOWN:
+ battery_text += " (shutdown)"
+ status_ok = False
+ self.battery_labels[name].set_text(battery_text)
+
+ radio_text = f'{msg.rssi} dBm; Unicast: {msg.num_rx_unicast} / {msg.num_tx_unicast}; Broadcast: {msg.num_rx_broadcast} / {msg.num_tx_broadcast}'
+ self.radio_labels[name].set_text(radio_text)
+
+ # save status here
+ self.robotmodels[name].status_ok = status_ok
+
+ # store the time when we last received any status
+ self.robotmodels[name].status_watchdog = time.time()
+
+ def on_tab_change(self, arg):
+ for name, robotmodel in self.robotmodels.items():
+ if name != arg.value:
+ robotmodel.scale(1)
+ if arg.value in self.robotmodels:
+ self.robotmodels[arg.value].scale(2)
+
+
+def main() -> None:
+ # NOTE: This function is defined as the ROS entry point in setup.py, but it's empty to enable NiceGUI auto-reloading
+ pass
+
+
+def ros_main() -> None:
+ rclpy.init()
+ node = NiceGuiNode()
+ try:
+ rclpy.spin(node)
+ except ExternalShutdownException:
+ pass
+
+
+app.add_static_files("/urdf",
+ str((Path(__file__).parent.parent.parent / "share" / "crazyflie" / "urdf").resolve()),
+ follow_symlink=True)
+app.on_startup(lambda: threading.Thread(target=ros_main).start())
+ui_run.APP_IMPORT_STRING = f'{__name__}:app' # ROS2 uses a non-standard module name, so we need to specify it here
+ui.run(uvicorn_reload_dirs=str(Path(__file__).parent.resolve()), favicon='🤖')