diff --git a/.github/workflows/systemtests_sim.yml b/.github/workflows/systemtests_sim.yml index e30cd8fc7..75a84d912 100644 --- a/.github/workflows/systemtests_sim.yml +++ b/.github/workflows/systemtests_sim.yml @@ -2,7 +2,9 @@ name: System Tests Simulation on: push: - branches: [ "main", "feature-systemtests-sim-fixed-timestamp" ] + branches: [ main ] + pull_request: + branches: [ main ] # manual trigger workflow_dispatch: diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index 8a9290c11..7676e175d 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -71,6 +71,8 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('backend', default_value='cpp'), DeclareLaunchArgument('debug', default_value='False'), + DeclareLaunchArgument('rviz', default_value='False'), + DeclareLaunchArgument('gui', default_value='True'), Node( package='motion_capture_tracking', executable='motion_capture_tracking_node', @@ -125,6 +127,7 @@ def generate_launch_description(): parameters=server_params ), Node( + condition=LaunchConfigurationEquals('rviz', 'True'), package='rviz2', namespace='', executable='rviz2', @@ -134,4 +137,11 @@ def generate_launch_description(): "use_sim_time": True, }] ), + Node( + condition=LaunchConfigurationEquals('gui', 'True'), + package='crazyflie', + namespace='', + executable='gui.py', + name='gui', + ), ]) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index a38dd6808..90c476313 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -218,14 +218,6 @@ def __init__(self): " or if your script have proper access to a Crazyradio PA") exit() - # Create services for the entire swarm and each individual crazyflie - self.create_service(Empty, "all/emergency", self._emergency_callback) - self.create_service(Takeoff, "all/takeoff", self._takeoff_callback) - self.create_service(Land, "all/land", self._land_callback) - self.create_service(GoTo, "all/go_to", self._go_to_callback) - self.create_service( - StartTrajectory, "all/start_trajectory", self._start_trajectory_callback) - for uri in self.cf_dict: if uri == "all": continue @@ -294,6 +286,16 @@ def __init__(self): self._poses_changed, qos_profile ) + # Create services for the entire swarm and each individual crazyflie + self.create_service(Takeoff, "all/takeoff", self._takeoff_callback) + self.create_service(Land, "all/land", self._land_callback) + self.create_service(GoTo, "all/go_to", self._go_to_callback) + self.create_service( + StartTrajectory, "all/start_trajectory", self._start_trajectory_callback) + + # This is the last service to announce and can be used to check if the server is fully available + self.create_service(Empty, "all/emergency", self._emergency_callback) + def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type): """ Prepare default logblocks as defined in crazyflies.yaml diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py index 9a7d4b9e5..086d6d315 100755 --- a/crazyflie/scripts/gui.py +++ b/crazyflie/scripts/gui.py @@ -6,6 +6,7 @@ from functools import partial import rclpy +from std_srvs.srv import Empty from geometry_msgs.msg import Twist from rcl_interfaces.msg import Log from rclpy.executors import ExternalShutdownException @@ -18,7 +19,7 @@ from crazyflie_interfaces.msg import Status import rowan -from nicegui import Client, app, events, ui, ui_run +from nicegui import Client, app, events, ui, ui_run, Tailwind class NiceGuiNode(Node): @@ -26,6 +27,10 @@ class NiceGuiNode(Node): def __init__(self) -> None: super().__init__('nicegui') + # wait until the crazyflie_server is up and running + self.emergencyService = self.create_client(Empty, 'all/emergency') + self.emergencyService.wait_for_service() + # find all crazyflies self.cfnames = [] for srv_name, srv_types in self.get_service_names_and_types(): @@ -49,6 +54,9 @@ def __init__(self) -> None: self.radio_labels = dict() self.robotmodels = dict() + self.normal_style = Tailwind().text_color('black').font_weight('normal') + self.red_style = Tailwind().text_color('red-600').font_weight('bold') + with Client.auto_index_client: with ui.row().classes('items-stretch'): @@ -60,7 +68,11 @@ def __init__(self) -> None: self.robotmodels[name] = robot # augment with some additional fields robot.status_ok = False + robot.battery_ok = False robot.status_watchdog = time.time() + robot.supervisor_text = "" + robot.battery_text = "" + robot.radio_text = "" scene.camera.x = 0 scene.camera.y = -1 scene.camera.z = 2 @@ -111,7 +123,8 @@ def on_rosout(self, msg: Log) -> None: 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 + robot_status_ok = robotmodel.status_ok and robotmodel.battery_ok + robot_status_text = "" if self.tf_buffer.can_transform("world", name, ros_time): t = self.tf_buffer.lookup_transform( "world", @@ -122,6 +135,7 @@ def on_timer(self) -> None: # latest transform is older than a second indicates a problem if transform_age.nanoseconds * 1e-9 > 1: robot_status_ok = False + robot_status_text += "old transform; " else: pos = t.transform.translation robotmodel.move(pos.x, pos.y, pos.z) @@ -133,10 +147,20 @@ def on_timer(self) -> None: else: # no available transform indicates a problem robot_status_ok = False + robot_status_text += "unavailable transform; " # no status update for a while, indicate a problem if time.time() - robotmodel.status_watchdog > 2.0: robot_status_ok = False + robot_status_text += "no recent status update; " + + self.supervisor_labels[name].set_text(robot_status_text) + self.battery_labels[name].set_text("N.A.") + self.radio_labels[name].set_text("N.A.") + else: + self.supervisor_labels[name].set_text(robot_status_text + robotmodel.supervisor_text) + self.battery_labels[name].set_text(robotmodel.battery_text) + self.radio_labels[name].set_text(robotmodel.radio_text) # any issues detected -> mark red, otherwise green if robot_status_ok: @@ -144,6 +168,11 @@ def on_timer(self) -> None: else: robotmodel.material('#ff0000') + if robotmodel.battery_ok: + self.normal_style.apply(self.battery_labels[name]) + else: + self.red_style.apply(self.battery_labels[name]) + def on_vis_click(self, e: events.SceneClickEventArguments): hit = e.hits[0] name = hit.object_name or hit.object_id @@ -155,6 +184,7 @@ def on_vis_click(self, e: events.SceneClickEventArguments): def on_status(self, msg, name) -> None: status_ok = True + is_flying = False supervisor_text = "" if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_BE_ARMED: supervisor_text += "can be armed; " @@ -166,17 +196,21 @@ def on_status(self, msg, name) -> None: supervisor_text += "can fly; " if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_FLYING: supervisor_text += "is flying; " + is_flying = True if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_TUMBLED: - supervisor_text += "is tumpled; " + supervisor_text += "is tumbled; " 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) + self.robotmodels[name].supervisor_text = supervisor_text battery_text = f'{msg.battery_voltage:.2f} V' - if msg.battery_voltage < 3.8: - status_ok = False + battery_ok = True + # TODO (WH): We could read the voltage limits from the firmware (parameter) or crazyflies.yaml + # In the firmware, anything below 3.2 is warning, anything below 3.0 is critical + if (is_flying and msg.battery_voltage < 3.2) or (not is_flying and msg.battery_voltage < 3.8): + battery_ok = False if msg.pm_state == Status.PM_STATE_BATTERY: battery_text += " (on battery)" elif msg.pm_state == Status.PM_STATE_CHARGING: @@ -185,17 +219,18 @@ def on_status(self, msg, name) -> None: battery_text += " (charged)" elif msg.pm_state == Status.PM_STATE_LOW_POWER: battery_text += " (low power)" - status_ok = False + battery_ok = False elif msg.pm_state == Status.PM_STATE_SHUTDOWN: battery_text += " (shutdown)" - status_ok = False - self.battery_labels[name].set_text(battery_text) - + battery_ok = False + self.robotmodels[name].battery_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) + self.robotmodels[name].radio_text = radio_text # save status here self.robotmodels[name].status_ok = status_ok + self.robotmodels[name].battery_ok = battery_ok # store the time when we last received any status self.robotmodels[name].status_watchdog = time.time() diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index 549438e26..de21f2544 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -985,19 +985,6 @@ class CrazyflieServer : public rclcpp::Node callback_group_cf_srv_ = this->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); - // topics for "all" - subscription_cmd_full_state_ = this->create_subscription("all/cmd_full_state", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieServer::cmd_full_state_changed, this, _1), sub_opt_all_cmd); - - // services for "all" - auto service_qos = rmw_qos_profile_services_default; - - service_emergency_ = this->create_service("all/emergency", std::bind(&CrazyflieServer::emergency, this, _1, _2), service_qos, callback_group_all_srv_); - service_start_trajectory_ = this->create_service("all/start_trajectory", std::bind(&CrazyflieServer::start_trajectory, this, _1, _2), service_qos, callback_group_all_srv_); - service_takeoff_ = this->create_service("all/takeoff", std::bind(&CrazyflieServer::takeoff, this, _1, _2), service_qos, callback_group_all_srv_); - service_land_ = this->create_service("all/land", std::bind(&CrazyflieServer::land, this, _1, _2), service_qos, callback_group_all_srv_); - service_go_to_ = this->create_service("all/go_to", std::bind(&CrazyflieServer::go_to, this, _1, _2), service_qos, callback_group_all_srv_); - service_notify_setpoints_stop_ = this->create_service("all/notify_setpoints_stop", std::bind(&CrazyflieServer::notify_setpoints_stop, this, _1, _2), service_qos, callback_group_all_srv_); - // declare global params this->declare_parameter("all.broadcasts.num_repeats", 15); this->declare_parameter("all.broadcasts.delay_between_repeats_ms", 1); @@ -1100,6 +1087,20 @@ class CrazyflieServer : public rclcpp::Node param_subscriber_ = std::make_shared(this); cb_handle_ = param_subscriber_->add_parameter_event_callback(std::bind(&CrazyflieServer::on_parameter_event, this, _1)); + // topics for "all" + subscription_cmd_full_state_ = this->create_subscription("all/cmd_full_state", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieServer::cmd_full_state_changed, this, _1), sub_opt_all_cmd); + + // services for "all" + auto service_qos = rmw_qos_profile_services_default; + + service_start_trajectory_ = this->create_service("all/start_trajectory", std::bind(&CrazyflieServer::start_trajectory, this, _1, _2), service_qos, callback_group_all_srv_); + service_takeoff_ = this->create_service("all/takeoff", std::bind(&CrazyflieServer::takeoff, this, _1, _2), service_qos, callback_group_all_srv_); + service_land_ = this->create_service("all/land", std::bind(&CrazyflieServer::land, this, _1, _2), service_qos, callback_group_all_srv_); + service_go_to_ = this->create_service("all/go_to", std::bind(&CrazyflieServer::go_to, this, _1, _2), service_qos, callback_group_all_srv_); + service_notify_setpoints_stop_ = this->create_service("all/notify_setpoints_stop", std::bind(&CrazyflieServer::notify_setpoints_stop, this, _1, _2), service_qos, callback_group_all_srv_); + + // This is the last service to announce and can be used to check if the server is fully available + service_emergency_ = this->create_service("all/emergency", std::bind(&CrazyflieServer::emergency, this, _1, _2), service_qos, callback_group_all_srv_); } diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index bbb7cbf2f..30c50dd27 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -717,6 +717,8 @@ class CrazyflieServer(rclpy.node.Node): def __init__(self): """Initialize the server. Waits for all ROS services before returning.""" super().__init__('CrazyflieAPI') + + # wait for server to be fully started self.emergencyService = self.create_client(Empty, 'all/emergency') self.emergencyService.wait_for_service() diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_server.py b/crazyflie_sim/crazyflie_sim/crazyflie_server.py index 93dd698da..00ddfc33b 100755 --- a/crazyflie_sim/crazyflie_sim/crazyflie_server.py +++ b/crazyflie_sim/crazyflie_sim/crazyflie_server.py @@ -91,15 +91,6 @@ def __init__(self): controller_name, self.backend.time) - # Create services for the entire swarm and each individual crazyflie - self.create_service(Empty, 'all/emergency', self._emergency_callback) - self.create_service(Takeoff, 'all/takeoff', self._takeoff_callback) - self.create_service(Land, 'all/land', self._land_callback) - self.create_service(GoTo, 'all/go_to', self._go_to_callback) - self.create_service(StartTrajectory, - 'all/start_trajectory', - self._start_trajectory_callback) - for name, _ in self.cfs.items(): pub = self.create_publisher( String, @@ -166,6 +157,18 @@ def __init__(self): 10 ) + # Create services for the entire swarm and each individual crazyflie + self.create_service(Takeoff, 'all/takeoff', self._takeoff_callback) + self.create_service(Land, 'all/land', self._land_callback) + self.create_service(GoTo, 'all/go_to', self._go_to_callback) + self.create_service(StartTrajectory, + 'all/start_trajectory', + self._start_trajectory_callback) + + # This is the last service to announce. + # Can be used to check if the server is fully available. + self.create_service(Empty, 'all/emergency', self._emergency_callback) + # step as fast as possible max_dt = 0.0 if 'max_dt' not in self._ros_parameters['sim'] \ else self._ros_parameters['sim']['max_dt']