Seperate parameter handling with connection status

crazyflie/scripts/crazyflie_server.py

@@ -2,7 +2,7 @@
 
 """
 A crazyflie server for communicating with several crazyflies
- based on the official crazyflie python library from 
+ based on the official crazyflie python library from
  Bitcraze AB
 
 
@@ -42,7 +42,7 @@
 from functools import partial
 from math import degrees, radians, pi, isnan
 
-cf_log_to_ros_param = {
+type_cf_param_to_ros_param = {
  "uint8_t": ParameterType.PARAMETER_INTEGER,
  "uint16_t": ParameterType.PARAMETER_INTEGER,
  "uint32_t": ParameterType.PARAMETER_INTEGER,
@@ -54,6 +54,20 @@
  "double": ParameterType.PARAMETER_DOUBLE,
 }
 
+type_cf_param_to_index = {
+ 'uint8_t': 0x08,
+ 'uint16_t': 0x09,
+ 'uint32_t': 0x0A,
+ 'uint64_t': 0x0B,
+ 'int8_t': 0x00,
+ 'int16_t': 0x01,
+ 'int32_t': 0x02,
+ 'int64_t': 0x03,
+ 'FP16': 0x05,
+ 'float': 0x06,
+ 'double': 0x07
+}
+
 
 class CrazyflieServer(Node):
  def __init__(self):
@@ -121,14 +135,19 @@ def __init__(self):
  factory = CachedCfFactory(rw_cache="./cache")
  self.swarm = Swarm(self.uris, factory=factory)
  self.swarm.fully_connected_crazyflie_cnt = 0
+ self.swarm.connected_crazyflie_cnt = 0
+
+ # Check if parameter values needs to be uploaded and put on ROS 2 params
+ self.swarm.query_all_values_on_connect = self._ros_parameters["firmware_params"]["query_all_values_on_connect"]
 
  # Initialize logging, services and parameters for each crazyflie
  for link_uri in self.uris:
 
  # Connect callbacks for different connection states of the crazyflie
  self.swarm._cfs[link_uri].cf.fully_connected.add_callback(
- self._fully_connected
- )
+ self._fully_connected)
+ self.swarm._cfs[link_uri].cf.connected.add_callback(
+ self._connected)
  self.swarm._cfs[link_uri].cf.disconnected.add_callback(
  self._disconnected)
  self.swarm._cfs[link_uri].cf.connection_failed.add_callback(
@@ -210,6 +229,7 @@ def __init__(self):
 
  # Now all crazyflies are initialized, open links!
  try:
+ self.time_open_link = self.get_clock().now().nanoseconds * 1e-9
  self.swarm.open_links()
  except Exception as e:
  # Close node if one of the Crazyflies can not be found
@@ -292,7 +312,7 @@ def __init__(self):
  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)
 
@@ -357,21 +377,43 @@ def _param_to_dict(self, param_ros):
  t = t.setdefault(part, {})
  return tree
 
+ def _connected(self, link_uri):
+ """
+ Called when the toc of the parameters and
+ logs has been received of the Crazyflie
+ """
+ self.get_logger().info(f"[{self.cf_dict[link_uri]}] is connected!")
+ self.swarm.connected_crazyflie_cnt += 1
+
+ if self.swarm.connected_crazyflie_cnt == len(self.cf_dict) - 1:
+ self.time_all_crazyflie_connected = self.get_clock().now().nanoseconds * 1e-9
+ self.get_logger().info(f"All Crazyflies are connected! It took {self.time_all_crazyflie_connected - self.time_open_link} seconds")
+ self._init_logging()
+ if not self.swarm.query_all_values_on_connect:
+ self._init_parameters()
+ self.add_on_set_parameters_callback(self._parameters_callback)
+
+ else:
+ return
+
+
  def _fully_connected(self, link_uri):
  """
- Called when all parameters have been updated
- and the full log toc has been received of the Crazyflie
+ Called the full log toc and parameter + values
+ has been received from the Crazyflie
  """
  self.get_logger().info(f"[{self.cf_dict[link_uri]}] is fully connected!")
 
  self.swarm.fully_connected_crazyflie_cnt += 1
 
  # 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()
- self.add_on_set_parameters_callback(self._parameters_callback)
+ self.time_all_crazyflie_connected = self.get_clock().now().nanoseconds * 1e-9
+ self.get_logger().info(f"All Crazyflies are fully connected! It took {self.time_all_crazyflie_connected - self.time_open_link} seconds")
+ if self.swarm.query_all_values_on_connect:
+ self._init_parameters()
+ self.add_on_set_parameters_callback(self._parameters_callback)
+
  else:
  return
 
@@ -457,7 +499,7 @@ def _init_default_logging(self, prefix, link_uri, callback_fnc):
 
  def _log_scan_data_callback(self, timestamp, data, logconf, uri):
  """
- Once multiranger range is retrieved from the Crazyflie, 
+ Once multiranger range is retrieved from the Crazyflie,
  send out the ROS 2 topic for Scan
  """
  cf_name = self.cf_dict[uri]
@@ -489,7 +531,7 @@ def _log_scan_data_callback(self, timestamp, data, logconf, uri):
 
  def _log_pose_data_callback(self, timestamp, data, logconf, uri):
  """
- Once pose data is retrieved from the Crazyflie, 
+ Once pose data is retrieved from the Crazyflie,
  send out the ROS 2 topic for Pose
  """
 
@@ -530,7 +572,7 @@ def _log_pose_data_callback(self, timestamp, data, logconf, uri):
 
  def _log_odom_data_callback(self, timestamp, data, logconf, uri):
  """
- Once pose and velocity data is retrieved from the Crazyflie, 
+ Once pose and velocity data is retrieved from the Crazyflie,
  send out the ROS 2 topic for Odometry in 2D (no z-axis)
  """
  cf_name = self.cf_dict[uri]
@@ -599,7 +641,7 @@ def _log_status_data_callback(self, timestamp, data, logconf, uri):
 
  def _log_custom_data_callback(self, timestamp, data, logconf, uri):
  """
- Once custom log block is retrieved from the Crazyflie, 
+ Once custom log block is retrieved from the Crazyflie,
  send out the ROS 2 topic for that same type of log
  """
  msg = LogDataGeneric()
@@ -616,10 +658,10 @@ def _log_error_callback(self, logconf, msg):
 
  def _init_parameters(self):
  """
- Once custom log block is retrieved from the Crazyflie, 
+ Once custom log block is retrieved from the Crazyflie,
  send out the ROS 2 topic for that same type of log
  """
- set_param_all = False
+ set_param_to_ROS = self.swarm.query_all_values_on_connect
  for link_uri in self.uris:
  cf = self.swarm._cfs[link_uri].cf
 
@@ -633,72 +675,61 @@ def _init_parameters(self):
  elem = p_toc[group][param]
  type_cf_param = elem.ctype
  parameter_descriptor = ParameterDescriptor(
- type=cf_log_to_ros_param[type_cf_param])
+ type=type_cf_param_to_ros_param[type_cf_param])
 
  # Check ros parameters if an parameter should be set
  # Parameter sets for individual robots has priority,
  # then robot types, then all (all robots)
- set_param_value = None
+ param_value = None
  try:
- set_param_value = self._ros_parameters["all"]["firmware_params"][group][param]
+ param_value = self._ros_parameters["all"]["firmware_params"][group][param]
  except KeyError:
  pass
  try:
- set_param_value = self._ros_parameters["robot_types"][self.cf_dict[link_uri]
+ param_value = self._ros_parameters["robot_types"][self.cf_dict[link_uri]
  ]["firmware_params"][group][param]
  except KeyError:
  pass
  try:
- set_param_value = self._ros_parameters["robots"][self.cf_dict[link_uri]
+ param_value = self._ros_parameters["robots"][self.cf_dict[link_uri]
  ]["firmware_params"][group][param]
  except KeyError:
  pass
 
- if set_param_value is not None:
+ if param_value is not None:
  # If value is found in initial parameters,
  # set crazyflie firmware value and declare value in ROS 2 parameter
  # Note: currently this is not possible to get the most recent from the
  # crazyflie with get_value due to threading.
- cf.param.set_value(name, set_param_value)
+ cf.param.set_value_raw(name, type_cf_param_to_index[type_cf_param], param_value)
  self.get_logger().info(
- f"[{self.cf_dict[link_uri]}] {name} is set to {set_param_value}"
- )
- self.declare_parameter(
- self.cf_dict[link_uri] +
- ".params." + group + "." + param,
- value=set_param_value,
- descriptor=parameter_descriptor,
+ f"[{self.cf_dict[link_uri]}] {name} is set to {param_value}"
  )
+ if set_param_to_ROS:
+ self.declare_parameter(
+ self.cf_dict[link_uri] +
+ ".params." + group + "." + param,
+ value=param_value,
+ descriptor=parameter_descriptor,
+ )
 
  else:
  # If value is not found in initial parameter set
  # get crazyflie paramter value and declare that value in ROS 2 parameter
+ # Only do this if this has been indicated by the user
+ if set_param_to_ROS is True:
 
- if cf_log_to_ros_param[type_cf_param] is ParameterType.PARAMETER_INTEGER:
- cf_param_value = int(cf.param.get_value(name))
- elif cf_log_to_ros_param[type_cf_param] is ParameterType.PARAMETER_DOUBLE:
- cf_param_value = float(cf.param.get_value(name))
+  if type_cf_param_to_ros_param[type_cf_param] is ParameterType.PARAMETER_INTEGER:
+  cf_param_value = int(cf.param.get_value(name))
+  elif type_cf_param_to_ros_param[type_cf_param] is ParameterType.PARAMETER_DOUBLE:
+  cf_param_value = float(cf.param.get_value(name))
 
- self.declare_parameter(
- self.cf_dict[link_uri] +
- ".params." + group + "." + param,
- value=cf_param_value,
- descriptor=parameter_descriptor,
- )
- # Use set_param_all to set a parameter based on the toc of the first crazyflie
- if cf_log_to_ros_param[type_cf_param] is ParameterType.PARAMETER_INTEGER:
- cf_param_value = int(cf.param.get_value(name))
- elif cf_log_to_ros_param[type_cf_param] is ParameterType.PARAMETER_DOUBLE:
- cf_param_value = float(cf.param.get_value(name))
- if set_param_all is False:
- self.declare_parameter(
- "all.params." + group + "." + param,
- value=cf_param_value,
- descriptor=parameter_descriptor,
- )
-
- # Now all parameters are set
- set_param_all = True
+ self.declare_parameter(
+ self.cf_dict[link_uri] +
+ ".params." + group + "." + param,
+ value=cf_param_value,
+ descriptor=parameter_descriptor,
+ )
 
  self.get_logger().info("All Crazyflies parameters are initialized.")
 
@@ -757,7 +788,7 @@ def _emergency_callback(self, request, response, uri="all"):
 
  def _takeoff_callback(self, request, response, uri="all"):
  """
- Service callback to take the crazyflie land to 
+ Service callback to take the crazyflie land to
  a certain height in high level commander
  """
 
@@ -784,7 +815,7 @@ def _takeoff_callback(self, request, response, uri="all"):
 
  def _land_callback(self, request, response, uri="all"):
  """
- Service callback to make the crazyflie land to 
+ Service callback to make the crazyflie land to
  a certain height in high level commander
  """
  duration = float(request.duration.sec) + \
@@ -808,7 +839,7 @@ def _land_callback(self, request, response, uri="all"):
 
  def _go_to_callback(self, request, response, uri="all"):
  """
- Service callback to have the crazyflie go to 
+ Service callback to have the crazyflie go to
  a certain position in high level commander
  """
  duration = float(request.duration.sec) + \
@@ -947,7 +978,7 @@ def _poses_changed(self, msg):
  """
  Topic update callback to the motion capture lib's
  poses topic to send through the external position
- to the crazyflie 
+ to the crazyflie
  """
 
  poses = msg.poses
@@ -1004,7 +1035,7 @@ def _cmd_full_state_changed(self, msg, uri=""):
  pitch_rate = msg.twist.angular.y
  yaw_rate = msg.twist.angular.z
  self.swarm._cfs[uri].cf.commander.send_full_state_setpoint(pos, vel, acc, q, roll_rate, pitch_rate, yaw_rate)
- 
+
  def _remove_logging(self, request, response, uri="all"):
  """
  Service callback to remove logging blocks of the crazyflie