diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index c8899cfbe..66e1e4ae1 100644 --- a/crazyflie/CMakeLists.txt +++ b/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} ) diff --git a/crazyflie/config/crazyflies.yaml b/crazyflie/config/crazyflies.yaml index e136fcd8b..f102bd78a 100644 --- a/crazyflie/config/crazyflies.yaml +++ b/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 diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index fbbe8fe72..a38dd6808 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -26,7 +26,7 @@ from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType -from crazyflie_interfaces.msg import Hover, LogDataGeneric, FullState +from crazyflie_interfaces.msg import Status, Hover, LogDataGeneric, FullState from motion_capture_tracking_interfaces.msg import NamedPoseArray from std_srvs.srv import Empty @@ -67,24 +67,31 @@ def __init__(self): self._ros_parameters = self._param_to_dict(self._parameters) self.uris = [] - self.cf_dict = {} + # for logging, assign a all -> all mapping + self.cf_dict = { + 'all': 'all' + } self.uri_dict = {} self.type_dict = {} # Assign default topic types, variables and callbacks self.default_log_type = {"pose": PoseStamped, "scan": LaserScan, - "odom": Odometry} + "odom": Odometry, + "status": Status} self.default_log_vars = {"pose": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z', 'stabilizer.roll', 'stabilizer.pitch', 'stabilizer.yaw'], "scan": ['range.front', 'range.left', 'range.back', 'range.right'], "odom": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z', 'stabilizer.yaw', 'stabilizer.roll', 'stabilizer.pitch', 'kalman.statePX', 'kalman.statePY', 'kalman.statePZ', - 'gyro.z', 'gyro.x', 'gyro.y']} + 'gyro.z', 'gyro.x', 'gyro.y'], + "status": ['supervisor.info', 'pm.vbatMV', 'pm.state', + 'radio.rssi']} self.default_log_fnc = {"pose": self._log_pose_data_callback, "scan": self._log_scan_data_callback, - "odom": self._log_odom_data_callback} + "odom": self._log_odom_data_callback, + "status": self._log_status_data_callback} self.world_tf_name = "world" try: @@ -220,6 +227,9 @@ def __init__(self): StartTrajectory, "all/start_trajectory", self._start_trajectory_callback) for uri in self.cf_dict: + if uri == "all": + continue + name = self.cf_dict[uri] pub = self.create_publisher(String, name + '/robot_description', @@ -350,11 +360,12 @@ def _fully_connected(self, link_uri): Called when all parameters have been updated and the full log toc has been received of the Crazyflie """ - self.get_logger().info(f" {link_uri} is fully connected!") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] is fully connected!") self.swarm.fully_connected_crazyflie_cnt += 1 - if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict): + # use len(self.cf_dict) - 1, since cf_dict contains "all" as well + if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict) - 1: self.get_logger().info("All Crazyflies are fully connected!") self._init_parameters() self._init_logging() @@ -363,10 +374,10 @@ def _fully_connected(self, link_uri): return def _disconnected(self, link_uri): - self.get_logger().info(f" {link_uri} is disconnected!") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] is disconnected!") def _connection_failed(self, link_uri, msg): - self.get_logger().info(f"{link_uri} connection Failed") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] connection Failed") self.swarm.close_links() def _init_logging(self): @@ -402,20 +413,20 @@ def _init_logging(self): self._log_error_callback) lg_custom.start() except KeyError as e: - self.get_logger().info(f'{link_uri}: Could not start log configuration,' + self.get_logger().info(f'[{self.cf_dict[link_uri]}] Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: self.get_logger().info( - f'{link_uri}: Could not add log config, bad configuration.') + f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.') - self.get_logger().info(f"{link_uri} setup custom logging") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] setup custom logging") self.create_service( RemoveLogging, self.cf_dict[link_uri] + "/remove_logging", partial(self._remove_logging, uri=link_uri)) self.create_service( AddLogging, self.cf_dict[link_uri] + "/add_logging", partial(self._add_logging, uri=link_uri)) - self.get_logger().info("All Crazyflies loggging are initialized") + self.get_logger().info("All Crazyflies logging are initialized.") def _init_default_logging(self, prefix, link_uri, callback_fnc): """ @@ -434,13 +445,13 @@ def _init_default_logging(self, prefix, link_uri, callback_fnc): self.declare_parameter( self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency) self.get_logger().info( - f"{link_uri} setup logging for {prefix} at freq {frequency}") + f"[{self.cf_dict[link_uri]}] setup logging for {prefix} at freq {frequency}") except KeyError as e: - self.get_logger().info(f'{link_uri}: Could not start log configuration,' + self.get_logger().error(f'[{self.cf_dict[link_uri]}] Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: - self.get_logger().info( - f'{link_uri}: Could not add log config, bad configuration.') + self.get_logger().error( + f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.') def _log_scan_data_callback(self, timestamp, data, logconf, uri): """ @@ -569,6 +580,21 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri): t_base.transform.rotation.w = q[3] self.tfbr.sendTransform(t_base) + def _log_status_data_callback(self, timestamp, data, logconf, uri): + """ + Send out the ROS 2 status topic + """ + + msg = Status() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.world_tf_name + msg.supervisor_info = data.get('supervisor.info') + msg.battery_voltage = data.get('pm.vbatMV') / 1000.0 + msg.pm_state = data.get('pm.state') + msg.rssi = data.get('radio.rssi') + + self.swarm._cfs[uri].logging["status_publisher"].publish(msg) + def _log_custom_data_callback(self, timestamp, data, logconf, uri): """ Once custom log block is retrieved from the Crazyflie, @@ -633,7 +659,7 @@ def _init_parameters(self): # crazyflie with get_value due to threading. cf.param.set_value(name, set_param_value) self.get_logger().info( - f" {link_uri}: {name} is set to {set_param_value}" + f"[{self.cf_dict[link_uri]}] {name} is set to {set_param_value}" ) self.declare_parameter( self.cf_dict[link_uri] + @@ -672,7 +698,7 @@ def _init_parameters(self): # Now all parameters are set set_param_all = True - self.get_logger().info("All Crazyflies parameters are initialized") + self.get_logger().info("All Crazyflies parameters are initialized.") def _parameters_callback(self, params): """ @@ -682,38 +708,39 @@ def _parameters_callback(self, params): for param in params: param_split = param.name.split(".") - if param_split[0] in self.cf_dict.values(): - cf_name = param_split[0] + if param_split[0] == "all": if param_split[1] == "params": name_param = param_split[2] + "." + param_split[3] try: - self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value( - name_param, param.value - ) + for link_uri in self.uris: + cf = self.swarm._cfs[link_uri].cf.param.set_value( + name_param, param.value + ) self.get_logger().info( - f" {self.uri_dict[cf_name]}: {name_param} is set to {param.value}" + f"[{self.cf_dict[link_uri]}] {name_param} is set to {param.value}" ) return SetParametersResult(successful=True) except Exception as e: self.get_logger().info(str(e)) return SetParametersResult(successful=False) - if param_split[1] == "logs": - return SetParametersResult(successful=True) - elif param_split[0] == "all": + elif param_split[0] in self.cf_dict.values(): + cf_name = param_split[0] if param_split[1] == "params": name_param = param_split[2] + "." + param_split[3] try: - for link_uri in self.uris: - cf = self.swarm._cfs[link_uri].cf.param.set_value( - name_param, param.value - ) + self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value( + name_param, param.value + ) self.get_logger().info( - f" {link_uri}: {name_param} is set to {param.value}" + f"[{self.uri_dict[cf_name]}] {name_param} is set to {param.value}" ) return SetParametersResult(successful=True) except Exception as e: self.get_logger().info(str(e)) return SetParametersResult(successful=False) + if param_split[1] == "logs": + return SetParametersResult(successful=True) + return SetParametersResult(successful=False) @@ -732,12 +759,14 @@ def _takeoff_callback(self, request, response, uri="all"): a certain height in high level commander """ + print("call1 ", uri) + duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) self.get_logger().info( - f"takeoff(height={request.height} m," + f"[{self.cf_dict[uri]}] takeoff(height={request.height} m," + f"duration={duration} s," - + f"group_mask={request.group_mask}) {uri}" + + f"group_mask={request.group_mask})" ) if uri == "all": for link_uri in self.uris: @@ -759,7 +788,7 @@ def _land_callback(self, request, response, uri="all"): duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) self.get_logger().info( - f"land(height={request.height} m," + f"[{self.cf_dict[uri]}] land(height={request.height} m," + f"duration={duration} s," + f"group_mask={request.group_mask})" ) @@ -784,8 +813,9 @@ def _go_to_callback(self, request, response, uri="all"): float(request.duration.nanosec / 1e9) self.get_logger().info( - "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" + "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" % ( + self.cf_dict[uri], request.goal.x, request.goal.y, request.goal.z, @@ -820,7 +850,7 @@ def _go_to_callback(self, request, response, uri="all"): def _notify_setpoints_stop_callback(self, request, response, uri="all"): - self.get_logger().info(f"{uri}: Received notify setpoint stop") + self.get_logger().info(f"[{self.cf_dict[uri]}] Received notify setpoint stop") if uri == "all": for link_uri in self.uris: @@ -836,7 +866,8 @@ def _upload_trajectory_callback(self, request, response, uri="all"): offset = request.piece_offset lenght = len(request.pieces) total_duration = 0 - self.get_logger().info("upload_trajectory(id=%d,offset=%d, lenght=%d)" % ( + self.get_logger().info("[%s] upload_trajectory(id=%d,offset=%d, lenght=%d)" % ( + self.cf_dict[uri], id, offset, lenght, @@ -862,7 +893,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"): trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: - self.get_logger().info(f"{link_uri}: Upload failed") + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed") upload_success_all = False else: self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( @@ -876,7 +907,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"): trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: - self.get_logger().info(f"{uri}: Upload failed") + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed") response.success = False return response self.swarm._cfs[uri].cf.high_level_commander.define_trajectory( @@ -892,7 +923,8 @@ def _start_trajectory_callback(self, request, response, uri="all"): rev = request.reversed gm = request.group_mask - self.get_logger().info("start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % ( + self.get_logger().info("[%s] start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % ( + self.cf_dict[uri], id, ts, rel, @@ -983,10 +1015,10 @@ def _remove_logging(self, request, response, uri="all"): self.swarm._cfs[uri].logging[topic_name + "_log_config"].stop() self.destroy_publisher( self.swarm._cfs[uri].logging[topic_name + "_publisher"]) - self.get_logger().info(f"{uri}: Remove {topic_name} logging") + self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging") except rclpy.exceptions.ParameterNotDeclaredException: self.get_logger().info( - f"{uri}: No logblock of {topic_name} has been found ") + f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ") response.success = False return response else: @@ -996,10 +1028,10 @@ def _remove_logging(self, request, response, uri="all"): for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"]: self.destroy_publisher( self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name]) - self.get_logger().info(f"{uri}: Remove {topic_name} logging") + self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging") except rclpy.exceptions.ParameterNotDeclaredException: self.get_logger().info( - f"{uri}: No logblock of {topic_name} has been found ") + f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ") response.success = False return response @@ -1023,10 +1055,10 @@ def _add_logging(self, request, response, uri="all"): "_log_config"].period_in_ms = 1000 / frequency self.swarm._cfs[uri].logging[topic_name + "_log_config"].start() - self.get_logger().info(f"{uri}: Add {topic_name} logging") + self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging") except rclpy.exceptions.ParameterAlreadyDeclaredException: self.get_logger().info( - f"{uri}: The content the logging of {topic_name} has already started ") + f"[{self.cf_dict[uri]}] The content the logging of {topic_name} has already started ") response.success = False return response else: @@ -1054,11 +1086,11 @@ def _add_logging(self, request, response, uri="all"): self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"] = variables self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["frequency"] = frequency - self.get_logger().info(f"{uri}: Add {topic_name} logging") + self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging") except KeyError as e: - self.get_logger().info( - f"{uri}: Failed to add {topic_name} logging") - self.get_logger().info(str(e) + "is not in TOC") + self.get_logger().error( + f"[{self.cf_dict[uri]}] Failed to add {topic_name} logging") + self.get_logger().error(str(e) + "is not in TOC") self.undeclare_parameter( self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") self.undeclare_parameter( @@ -1066,8 +1098,8 @@ def _add_logging(self, request, response, uri="all"): response.success = False return response except rclpy.exceptions.ParameterAlreadyDeclaredException: - self.get_logger().info( - f"{uri}: The content or part of the logging of {topic_name} has already started ") + self.get_logger().error( + f"[{self.cf_dict[uri]}] The content or part of the logging of {topic_name} has already started ") response.success = False return response diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py new file mode 100755 index 000000000..9a7d4b9e5 --- /dev/null +++ b/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='🤖') diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index 4a71e0245..b4e7e65ea 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -39,13 +39,19 @@ using std_srvs::srv::Empty; using motion_capture_tracking_interfaces::msg::NamedPoseArray; using crazyflie_interfaces::msg::FullState; +// Note on logging: we use a single logger with string prefixes +// A better way would be to use named child loggers, but these do not +// report to /rosout in humble, see https://github.com/ros2/rclpy/issues/1131 +// Once we do not support humble anymore, consider switching to child loggers + // Helper class to convert crazyflie_cpp logging messages to ROS logging messages class CrazyflieLogger : public Logger { public: - CrazyflieLogger(rclcpp::Logger logger) + CrazyflieLogger(rclcpp::Logger logger, const std::string& prefix) : Logger() , logger_(logger) + , prefix_(prefix) { } @@ -53,20 +59,21 @@ class CrazyflieLogger : public Logger virtual void info(const std::string &msg) { - RCLCPP_INFO(logger_, "%s", msg.c_str()); + RCLCPP_INFO(logger_, "%s %s", prefix_.c_str(), msg.c_str()); } virtual void warning(const std::string &msg) { - RCLCPP_WARN(logger_, "%s", msg.c_str()); + RCLCPP_WARN(logger_, "%s %s", prefix_.c_str(), msg.c_str()); } virtual void error(const std::string &msg) { - RCLCPP_ERROR(logger_, "%s", msg.c_str()); + RCLCPP_ERROR(logger_, "%s %s", prefix_.c_str(), msg.c_str()); } private: rclcpp::Logger logger_; + std::string prefix_; }; std::set extract_names( @@ -129,8 +136,8 @@ class CrazyflieROS rclcpp::CallbackGroup::SharedPtr callback_group_cf_srv, const CrazyflieBroadcaster* cfbc, bool enable_parameters = true) - : logger_(rclcpp::get_logger(name)) - , cf_logger_(logger_) + : logger_(node->get_logger()) + , cf_logger_(logger_, "[" + name + "]") , cf_( link_uri, cf_logger_, @@ -228,7 +235,7 @@ class CrazyflieROS bool query_all_values_on_connect = node->get_parameter("firmware_params.query_all_values_on_connect").get_parameter_value().get(); int numParams = 0; - RCLCPP_INFO(logger_, "Requesting parameters..."); + RCLCPP_INFO(logger_, "[%s] Requesting parameters...", name_.c_str()); cf_.requestParamToc(/*forceNoCache*/false, /*requestValues*/query_all_values_on_connect); for (auto iter = cf_.paramsBegin(); iter != cf_.paramsEnd(); ++iter) { auto entry = *iter; @@ -285,7 +292,7 @@ class CrazyflieROS } break; default: - RCLCPP_WARN(logger_, "Unknown param type for %s/%s", entry.group.c_str(), entry.name.c_str()); + RCLCPP_WARN(logger_, "[%s] Unknown param type for %s/%s", name_.c_str(), entry.group.c_str(), entry.name.c_str()); break; } // If there is no such parameter in all, add it @@ -301,7 +308,7 @@ class CrazyflieROS } auto end1 = std::chrono::system_clock::now(); std::chrono::duration elapsedSeconds1 = end1 - start; - RCLCPP_INFO(logger_, "reqParamTOC: %f s (%d params)", elapsedSeconds1.count(), numParams); + RCLCPP_INFO(logger_, "[%s] reqParamTOC: %f s (%d params)", name_.c_str(), elapsedSeconds1.count(), numParams); // Set parameters as specified in the configuration files, as in the following order // 1.) check all/firmware_params @@ -351,7 +358,7 @@ class CrazyflieROS // check if any of the default topics are enabled if (i.first.find("default_topics.pose") == 0) { int freq = log_config_map["default_topics.pose.frequency"].get(); - RCLCPP_INFO(logger_, "Logging to /pose at %d Hz", freq); + RCLCPP_INFO(logger_, "[%s] Logging to /pose at %d Hz", name_.c_str(), freq); publisher_pose_ = node->create_publisher(name + "/pose", 10); @@ -368,7 +375,7 @@ class CrazyflieROS } else if (i.first.find("default_topics.scan") == 0) { int freq = log_config_map["default_topics.scan.frequency"].get(); - RCLCPP_INFO(logger_, "Logging to /scan at %d Hz", freq); + RCLCPP_INFO(logger_, "[%s] Logging to /scan at %d Hz", name_.c_str(), freq); publisher_scan_ = node->create_publisher(name + "/scan", 10); @@ -385,7 +392,7 @@ class CrazyflieROS } else if (i.first.find("default_topics.status") == 0) { int freq = log_config_map["default_topics.status.frequency"].get(); - RCLCPP_INFO(logger_, "Logging to /status at %d Hz", freq); + RCLCPP_INFO(logger_, "[%s] Logging to /status at %d Hz", name_.c_str(), freq); publisher_status_ = node->create_publisher(name + "/status", 10); @@ -415,7 +422,7 @@ class CrazyflieROS // older firmware -> use other 16-bit variables if (!status_has_radio_stats_) { - RCLCPP_WARN(logger_, "Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero."); + RCLCPP_WARN(logger_, "[%s] Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero.", name_.c_str()); logvars.push_back({"pm", "vbatMV"}); logvars.push_back({"pm", "vbatMV"}); } @@ -431,7 +438,7 @@ class CrazyflieROS int freq = log_config_map["custom_topics." + topic_name + ".frequency"].get(); auto vars = log_config_map["custom_topics." + topic_name + ".vars"].get>(); - RCLCPP_INFO(logger_, "Logging to %s at %d Hz", topic_name.c_str(), freq); + RCLCPP_INFO(logger_, "[%s] Logging to %s at %d Hz", name_.c_str(), topic_name.c_str(), freq); publishers_generic_.emplace_back(node->create_publisher(name + "/" + topic_name, 10)); @@ -453,7 +460,7 @@ class CrazyflieROS } } - RCLCPP_INFO(logger_, "Requesting memories..."); + RCLCPP_INFO(logger_, "[%s] Requesting memories...", name_.c_str()); cf_.requestMemoryToc(); } @@ -489,7 +496,7 @@ class CrazyflieROS if (p.get_name().find(prefix) != 0) { RCLCPP_ERROR( logger_, - "Incorrect parameter update request for param \"%s\"", p.get_name().c_str()); + "[%s] Incorrect parameter update request for param \"%s\"", name_.c_str(), p.get_name().c_str()); return; } size_t pos = p.get_name().find(".", prefix.size()); @@ -498,7 +505,8 @@ class CrazyflieROS RCLCPP_INFO( logger_, - "Update parameter \"%s.%s\" to %s", + "[%s] Update parameter \"%s.%s\" to %s", + name_.c_str(), group.c_str(), name.c_str(), p.value_to_string().c_str()); @@ -535,7 +543,7 @@ class CrazyflieROS break; } } else { - RCLCPP_ERROR(logger_, "Could not find param %s/%s", group.c_str(), name.c_str()); + RCLCPP_ERROR(logger_, "[%s] Could not find param %s/%s", name_.c_str(), group.c_str(), name.c_str()); } } @@ -594,7 +602,7 @@ class CrazyflieROS if (pos != std::string::npos) { message_buffer_[pos] = 0; - RCLCPP_INFO(logger_, "%s", message_buffer_.c_str()); + RCLCPP_INFO(logger_, "[%s] %s", name_.c_str(), message_buffer_.c_str()); message_buffer_.erase(0, pos + 1); } } @@ -602,14 +610,15 @@ class CrazyflieROS void emergency(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "emergency()"); + RCLCPP_INFO(logger_, "[%s] emergency()", name_.c_str()); cf_.emergencyStop(); } void start_trajectory(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "start_trajectory(id=%d, timescale=%f, reversed=%d, relative=%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] start_trajectory(id=%d, timescale=%f, reversed=%d, relative=%d, group_mask=%d)", + name_.c_str(), request->trajectory_id, request->timescale, request->reversed, @@ -625,7 +634,8 @@ class CrazyflieROS void takeoff(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "takeoff(height=%f m, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] takeoff(height=%f m, duration=%f s, group_mask=%d)", + name_.c_str(), request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -635,7 +645,8 @@ class CrazyflieROS void land(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "land(height=%f m, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] land(height=%f m, duration=%f s, group_mask=%d)", + name_.c_str(), request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -645,7 +656,8 @@ class CrazyflieROS void go_to(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)", + name_.c_str(), request->goal.x, request->goal.y, request->goal.z, request->yaw, rclcpp::Duration(request->duration).seconds(), request->relative, @@ -658,7 +670,8 @@ class CrazyflieROS void upload_trajectory(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "upload_trajectory(id=%d, offset=%d)", + RCLCPP_INFO(logger_, "[%s] upload_trajectory(id=%d, offset=%d)", + name_.c_str(), request->trajectory_id, request->piece_offset); @@ -670,7 +683,7 @@ class CrazyflieROS || request->pieces[i].poly_z.size() != 8 || request->pieces[i].poly_yaw.size() != 8) { - RCLCPP_FATAL(logger_, "Wrong number of pieces!"); + RCLCPP_FATAL(logger_, "[%s] Wrong number of pieces!", name_.c_str()); return; } pieces[i].duration = rclcpp::Duration(request->pieces[i].duration).seconds(); @@ -688,7 +701,8 @@ class CrazyflieROS void notify_setpoints_stop(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[%s] notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)", + name_.c_str(), request->remain_valid_millisecs, request->group_mask); @@ -824,13 +838,13 @@ class CrazyflieROS auto now = std::chrono::steady_clock::now(); std::chrono::duration elapsed = now - last_on_latency_; if (elapsed.count() > 1.0 / warning_freq_) { - RCLCPP_WARN(logger_, "last latency update: %f s", elapsed.count()); + RCLCPP_WARN(logger_, "[%s] last latency update: %f s", name_.c_str(), elapsed.count()); } auto stats = cf_.connectionStatsDelta(); float ack_rate = stats.sent_count / stats.ack_count; if (ack_rate < min_ack_rate_) { - RCLCPP_WARN(logger_, "Ack rate: %.1f %%", ack_rate * 100); + RCLCPP_WARN(logger_, "[%s] Ack rate: %.1f %%", name_.c_str(), ack_rate * 100); } if (publish_stats_) { @@ -853,7 +867,7 @@ class CrazyflieROS void on_latency(uint64_t latency_in_us) { if (latency_in_us / 1000.0 > max_latency_) { - RCLCPP_WARN(logger_, "Latency: %.1f ms", latency_in_us / 1000.0); + RCLCPP_WARN(logger_, "[%s] Latency: %.1f ms", name_.c_str(), latency_in_us / 1000.0); } last_on_latency_ = std::chrono::steady_clock::now(); } @@ -921,7 +935,7 @@ class CrazyflieServer : public rclcpp::Node public: CrazyflieServer() : Node("crazyflie_server") - , logger_(rclcpp::get_logger("all")) + , logger_(get_logger()) { // Create callback groups (each group can run in a separate thread) callback_group_mocap_ = this->create_callback_group( @@ -1036,7 +1050,7 @@ class CrazyflieServer : public rclcpp::Node uint8_t id = parameter_overrides.at("robots." + name + ".id").get(); update_name_to_id_map(name, id); } else { - RCLCPP_INFO(logger_, "Unknown connection type %s", constr.c_str()); + RCLCPP_INFO(logger_, "[all] Unknown connection type %s", constr.c_str()); } } } @@ -1063,7 +1077,7 @@ class CrazyflieServer : public rclcpp::Node void emergency(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "emergency()"); + RCLCPP_INFO(logger_, "[all] emergency()"); for (int i = 0; i < broadcasts_num_repeats_; ++i) { for (auto &bc : broadcaster_) { @@ -1077,7 +1091,7 @@ class CrazyflieServer : public rclcpp::Node void start_trajectory(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "start_trajectory(id=%d, timescale=%f, reversed=%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] start_trajectory(id=%d, timescale=%f, reversed=%d, group_mask=%d)", request->trajectory_id, request->timescale, request->reversed, @@ -1097,7 +1111,7 @@ class CrazyflieServer : public rclcpp::Node void takeoff(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "takeoff(height=%f m, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] takeoff(height=%f m, duration=%f s, group_mask=%d)", request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -1113,7 +1127,7 @@ class CrazyflieServer : public rclcpp::Node void land(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "land(height=%f m, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] land(height=%f m, duration=%f s, group_mask=%d)", request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -1129,7 +1143,7 @@ class CrazyflieServer : public rclcpp::Node void go_to(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, group_mask=%d)", request->goal.x, request->goal.y, request->goal.z, request->yaw, rclcpp::Duration(request->duration).seconds(), request->group_mask); @@ -1147,7 +1161,7 @@ class CrazyflieServer : public rclcpp::Node void notify_setpoints_stop(const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO(logger_, "notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)", + RCLCPP_INFO(logger_, "[all] notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)", request->remain_valid_millisecs, request->group_mask); @@ -1254,7 +1268,7 @@ class CrazyflieServer : public rclcpp::Node RCLCPP_INFO( logger_, - "Update parameter \"%s.%s\" to %s", + "[all] Update parameter \"%s.%s\" to %s", group.c_str(), name.c_str(), p.value_to_string().c_str()); @@ -1324,11 +1338,11 @@ class CrazyflieServer : public rclcpp::Node mean_rate /= (mocap_data_received_timepoints_.size() - 1); if (num_rates_wrong > 0) { - RCLCPP_WARN(logger_, "Motion capture rate off (#: %d, Avg: %.1f)", num_rates_wrong, mean_rate); + RCLCPP_WARN(logger_, "[all] Motion capture rate off (#: %d, Avg: %.1f)", num_rates_wrong, mean_rate); } } else if (mocap_enabled_) { // b) warn if no data was received - RCLCPP_WARN(logger_, "Motion capture did not receive data!"); + RCLCPP_WARN(logger_, "[all] Motion capture did not receive data!"); } mocap_data_received_timepoints_.clear(); @@ -1377,7 +1391,7 @@ class CrazyflieServer : public rclcpp::Node { const auto iter = name_to_id_.find(name); if (iter != name_to_id_.end()) { - RCLCPP_WARN(logger_, "At least two objects with the same id (%d, %s, %s)", id, name.c_str(), iter->first.c_str()); + RCLCPP_WARN(logger_, "[all] At least two objects with the same id (%d, %s, %s)", id, name.c_str(), iter->first.c_str()); } else { name_to_id_.insert(std::make_pair(name, id)); } @@ -1400,6 +1414,8 @@ class CrazyflieServer : public rclcpp::Node std::map> crazyflies_; + + // broadcastUri -> broadcast object std::map> broadcaster_; diff --git a/crazyflie/urdf/cf2_assembly.stl b/crazyflie/urdf/cf2_assembly.stl new file mode 100644 index 000000000..02772d31e Binary files /dev/null and b/crazyflie/urdf/cf2_assembly.stl differ diff --git a/crazyflie_interfaces/msg/Status.msg b/crazyflie_interfaces/msg/Status.msg index bebd42a71..d7c2f2352 100644 --- a/crazyflie_interfaces/msg/Status.msg +++ b/crazyflie_interfaces/msg/Status.msg @@ -1,6 +1,6 @@ # Constants -uint16 SUPERVISOR_INFO_CAN_BE_ARMED = 0 +uint16 SUPERVISOR_INFO_CAN_BE_ARMED = 1 uint16 SUPERVISOR_INFO_IS_ARMED = 2 uint16 SUPERVISOR_INFO_AUTO_ARM = 4 uint16 SUPERVISOR_INFO_CAN_FLY = 8 diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_server.py b/crazyflie_sim/crazyflie_sim/crazyflie_server.py index 13bf4ae2e..93dd698da 100755 --- a/crazyflie_sim/crazyflie_sim/crazyflie_server.py +++ b/crazyflie_sim/crazyflie_sim/crazyflie_server.py @@ -210,7 +210,7 @@ def _param_to_dict(self, param_ros): return tree def _emergency_callback(self, request, response, name='all'): - self.get_logger().info('emergency not yet implemented') + self.get_logger().info(f'[{name}] emergency not yet implemented') return response @@ -219,9 +219,9 @@ def _takeoff_callback(self, request, response, name='all'): duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) self.get_logger().info( - f'takeoff(height={request.height} m,' + f'[{name}] takeoff(height={request.height} m,' + f'duration={duration} s,' - + f'group_mask={request.group_mask}) {name}' + + f'group_mask={request.group_mask})' ) cfs = self.cfs if name == 'all' else {name: self.cfs[name]} for _, cf in cfs.items(): @@ -234,7 +234,7 @@ def _land_callback(self, request, response, name='all'): duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) self.get_logger().info( - f'land(height={request.height} m,' + f'[{name}] land(height={request.height} m,' + f'duration={duration} s,' + f'group_mask={request.group_mask})' ) @@ -250,8 +250,13 @@ def _go_to_callback(self, request, response, name='all'): float(request.duration.nanosec / 1e9) self.get_logger().info( - 'go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)' + """[%s] go_to(position=%f,%f,%f m, + yaw=%f rad, + duration=%f s, + relative=%d, + group_mask=%d)""" % ( + name, request.goal.x, request.goal.y, request.goal.z, @@ -269,11 +274,11 @@ def _go_to_callback(self, request, response, name='all'): return response def _notify_setpoints_stop_callback(self, request, response, name='all'): - self.get_logger().info('Notify setpoint stop not yet implemented') + self.get_logger().info(f'[{name}] Notify setpoint stop not yet implemented') return response def _upload_trajectory_callback(self, request, response, name='all'): - self.get_logger().info('Upload trajectory(id=%d)' % (request.trajectory_id)) + self.get_logger().info('[%s] Upload trajectory(id=%d)' % (name, request.trajectory_id)) cfs = self.cfs if name == 'all' else {name: self.cfs[name]} for _, cf in cfs.items(): @@ -297,8 +302,9 @@ def _upload_trajectory_callback(self, request, response, name='all'): def _start_trajectory_callback(self, request, response, name='all'): self.get_logger().info( - 'start_trajectory(id=%d, timescale=%f, reverse=%d, relative=%d, group_mask=%d)' + '[%s] start_trajectory(id=%d, timescale=%f, reverse=%d, relative=%d, group_mask=%d)' % ( + name, request.trajectory_id, request.timescale, request.reversed, diff --git a/docs2/faq.rst b/docs2/faq.rst index 8270b9343..7422be127 100644 --- a/docs2/faq.rst +++ b/docs2/faq.rst @@ -45,9 +45,9 @@ Crazyswarm2 was forked from Crazyswarm. However, there is also heavy re-design o In Crazyswarm1, a simple visualization of the setpoints for high-level Python scripts is supported. There is no support for simulation of ROS code that does not use the high-level Python scripts and no support for physics-based simulation. In contrast, Crazyswarm2 implements the simulation as an alternative backend. This will support multiple physics/visualization backends (optionally with physics and aerodynamic interaction). -- **Support of Distributed Swarm Monitoring (Planned).** +- **Support of Distributed Swarm Monitoring.** In Crazyswarm1, a common swarm monitoring tool is the chooser.py (to enable/disable CFs, check the battery voltage etc.). However, this tool was not functioning while the swarm is operational. - In contrast, Crazyswarm2 will allow common swarm monitoring tasks without restarting ROS nodes or launching additional tools. + In contrast, Crazyswarm2 allows common swarm monitoring tasks without restarting ROS nodes or launching additional tools. How is Crazyswarm2 different from Bitcraze's cflib? diff --git a/docs2/overview.rst b/docs2/overview.rst index 014cfc93d..cc413fe1f 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -67,6 +67,8 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | - default: odom | No | Yes | No | +---------------------+---------+-----------+---------+ +| - default: status | Yes | Yes | No | ++---------------------+---------+-----------+---------+ | - custom | Yes | Yes | No | +---------------------+---------+-----------+---------+ | - Add/Remove Srv | No | Yes | No | diff --git a/docs2/usage.rst b/docs2/usage.rst index d8278958b..52117f0ae 100644 --- a/docs2/usage.rst +++ b/docs2/usage.rst @@ -155,3 +155,11 @@ You may run the script multiple times or different scripts while leaving the ser [terminal1]$ ros2 launch crazyflie launch.py [terminal2]$ ros2 run crazyflie_examples hello_world + +Swarm Management +---------------- + +The launch file will also start a swarm management tool that is a ROS node and web-based GUI. +In the upper pane is the location of the drone visualized in a 3D window, similar to rviz. +In the lower pane, the status as well as log messages are visible (tabbed per drone). +In the future, we are planning to add support for rebooting and other actions. \ No newline at end of file