Slow TOC and value retrieval with CFlib backend

[knmcguire]

With the cflib backend, the server will not fully initiate until the full TOC and content of values are downloaded.

Currently, within a fresh environment, it takes 150 seconds to connect to all Crazyflies and with cache it takes 52 seconds. That is a long time...

Two things we can consider:

• Adding a cache for the values of the TOC as well in the cflib (or investigate if that works)
• Continue the server before all the values are downloaded (so just connected).

[knmcguire]

Just to add here, that current the crazyflie python server does not react on the parameter query_all_values_on_connect in server.yaml and just grabs all values of parameters. We should implement that for sure.

[knmcguire]

So as it occurs that the parameter value retrieval is not cached, and the toc is, and that works (although it might be better to cache the toc on a fixed location). So as long as we avoid the fully connected a bit or make it more configurable it should be way faster.

Another thing that needs to be done is to separate and rewrite the param setting function _init_parameters. What it is currently doing is that it is waiting for the full value of the toc to be downloaded, such that it can use param.get_value without a callback (since this blocks if it is not fetched).

crazyswarm2/crazyflie/scripts/crazyflie_server.py

Lines 617 to 703 in 8a22066

	def _init_parameters(self):
	"""
	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
	for link_uri in self.uris:
	cf = self.swarm._cfs[link_uri].cf
	p_toc = cf.param.toc.toc
	for group in sorted(p_toc.keys()):
	for param in sorted(p_toc[group].keys()):
	name = group + "." + param
	# Check the parameter type
	elem = p_toc[group][param]
	type_cf_param = elem.ctype
	parameter_descriptor = ParameterDescriptor(
	type=cf_log_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
	try:
	set_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]
	]["firmware_params"][group][param]
	except KeyError:
	pass
	try:
	set_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 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)
	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,
	)
	else:
	# If value is not found in initial parameter set
	# get crazyflie paramter value and declare that value in ROS 2 parameter
	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))
	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.get_logger().info("All Crazyflies parameters are initialized.")

We should write this to 2 functions namely:

• Init_parameters, which only sets the value and checks it (can be in .connected)
• set_ros_parameters, which converts all values of the crazyflie to ROS parameters (can also be in .connected but better to be in fully connected). This should be able to turn completely on or off due to what is in the configs:

  crazyswarm2/crazyflie/config/server.yaml

  Line 14 in 8a22066

  query_all_values_on_connect: False

  .

[knmcguire]

Closed with #564. Seems to be working but the CFlib is a bit conservative with starting up the communication., but now it shouldn't be.