Merge pull request #426 from IMRCLab/issue422

server: fix startup race condition, GUI improvements

.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:
 

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',
+        ),
     ])

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

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,14 +19,18 @@
 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):
 
     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,17 +147,32 @@ 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:
                 robotmodel.material('#00ff00')
             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()

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<crazyflie_interfaces::msg::FullState>("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<Empty>("all/emergency", std::bind(&CrazyflieServer::emergency, this, _1, _2), service_qos, callback_group_all_srv_);
-    service_start_trajectory_ = this->create_service<StartTrajectory>("all/start_trajectory", std::bind(&CrazyflieServer::start_trajectory, this, _1, _2), service_qos, callback_group_all_srv_);
-    service_takeoff_ = this->create_service<Takeoff>("all/takeoff", std::bind(&CrazyflieServer::takeoff, this, _1, _2), service_qos, callback_group_all_srv_);
-    service_land_ = this->create_service<Land>("all/land", std::bind(&CrazyflieServer::land, this, _1, _2), service_qos, callback_group_all_srv_);
-    service_go_to_ = this->create_service<GoTo>("all/go_to", std::bind(&CrazyflieServer::go_to, this, _1, _2), service_qos, callback_group_all_srv_);
-    service_notify_setpoints_stop_ = this->create_service<NotifySetpointsStop>("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<rclcpp::ParameterEventHandler>(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<crazyflie_interfaces::msg::FullState>("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<StartTrajectory>("all/start_trajectory", std::bind(&CrazyflieServer::start_trajectory, this, _1, _2), service_qos, callback_group_all_srv_);
+    service_takeoff_ = this->create_service<Takeoff>("all/takeoff", std::bind(&CrazyflieServer::takeoff, this, _1, _2), service_qos, callback_group_all_srv_);
+    service_land_ = this->create_service<Land>("all/land", std::bind(&CrazyflieServer::land, this, _1, _2), service_qos, callback_group_all_srv_);
+    service_go_to_ = this->create_service<GoTo>("all/go_to", std::bind(&CrazyflieServer::go_to, this, _1, _2), service_qos, callback_group_all_srv_);
+    service_notify_setpoints_stop_ = this->create_service<NotifySetpointsStop>("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<Empty>("all/emergency", std::bind(&CrazyflieServer::emergency, this, _1, _2), service_qos, callback_group_all_srv_);
   }
 
 

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()
 

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']