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