From eca59bc25dc9528daf39e0deebdf5d702d479025 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Wed, 11 Sep 2024 13:57:12 +0200 Subject: [PATCH 1/5] cflib server add parameter to ros setting only if indicated --- crazyflie/scripts/crazyflie_server.py | 86 ++++++++++++++++----------- 1 file changed, 50 insertions(+), 36 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 1ff183f34..88d7eeb8a 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -121,14 +121,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["robots"]["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 +215,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 @@ -356,11 +362,27 @@ def _param_to_dict(self, param_ros): else: t = t.setdefault(part, {}) return tree + + def _connected(self, link_uri): + """ + Called when all parameters have been updated + and the toc 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() + 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 + and the full log toc and parameter values has been received of the Crazyflie """ self.get_logger().info(f"[{self.cf_dict[link_uri]}] is fully connected!") @@ -368,10 +390,11 @@ def _fully_connected(self, link_uri): # 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.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") self._init_parameters() - self._init_logging() self.add_on_set_parameters_callback(self._parameters_callback) + else: return @@ -619,7 +642,7 @@ 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 + set_param_to_ROS = self.swarm.query_all_values_on_connect for link_uri in self.uris: cf = self.swarm._cfs[link_uri].cf @@ -663,45 +686,36 @@ def _init_parameters(self): 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, - ) + if set_param_to_ROS: + 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 + # 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 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.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.") + + def _parameters_callback(self, params): """ Sets up all the parameters for the crazyflie and From df3deb13d9fff1c1267fa821b07a5ad7528e4064 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Wed, 11 Sep 2024 14:00:23 +0200 Subject: [PATCH 2/5] wait on fully connected only when full toc value list is wanted --- crazyflie/scripts/crazyflie_server.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 88d7eeb8a..b7fb28415 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -375,6 +375,10 @@ def _connected(self, link_uri): 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 @@ -392,8 +396,9 @@ def _fully_connected(self, link_uri): if self.swarm.fully_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 fully connected! It took {self.time_all_crazyflie_connected - self.time_open_link} seconds") - self._init_parameters() - self.add_on_set_parameters_callback(self._parameters_callback) + if self.swarm.query_all_values_on_connect: + self._init_parameters() + self.add_on_set_parameters_callback(self._parameters_callback) else: return From e076b2e1b01ca536f048f7dec59b493987144b85 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Wed, 11 Sep 2024 14:11:47 +0200 Subject: [PATCH 3/5] update text --- crazyflie/scripts/crazyflie_server.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index b7fb28415..b038cc443 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -365,8 +365,8 @@ def _param_to_dict(self, param_ros): def _connected(self, link_uri): """ - Called when all parameters have been updated - and the toc has been received of the Crazyflie + 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 @@ -385,8 +385,8 @@ def _connected(self, link_uri): def _fully_connected(self, link_uri): """ - Called when all parameters have been updated - and the full log toc and parameter values 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!") From a263dfa69b1f2c9420e9934aee5767d75b8dc41d Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 12 Sep 2024 16:46:06 +0200 Subject: [PATCH 4/5] send_param_raw and reformat --- crazyflie/scripts/crazyflie_server.py | 73 ++++++++++++++++----------- 1 file changed, 43 insertions(+), 30 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index b038cc443..dbd4f6d3b 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/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): @@ -124,7 +138,7 @@ def __init__(self): 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["robots"]["firmware_params"]["query_all_values_on_connect"] + 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: @@ -133,7 +147,7 @@ def __init__(self): self.swarm._cfs[link_uri].cf.fully_connected.add_callback( self._fully_connected) self.swarm._cfs[link_uri].cf.connected.add_callback( - self._connected) + self._connected) self.swarm._cfs[link_uri].cf.disconnected.add_callback( self._disconnected) self.swarm._cfs[link_uri].cf.connection_failed.add_callback( @@ -298,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) @@ -362,7 +376,7 @@ def _param_to_dict(self, param_ros): else: t = t.setdefault(part, {}) return tree - + def _connected(self, link_uri): """ Called when the toc of the parameters and @@ -396,7 +410,7 @@ def _fully_connected(self, link_uri): if self.swarm.fully_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 fully connected! It took {self.time_all_crazyflie_connected - self.time_open_link} seconds") - if self.swarm.query_all_values_on_connect: + if self.swarm.query_all_values_on_connect: self._init_parameters() self.add_on_set_parameters_callback(self._parameters_callback) @@ -485,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] @@ -517,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 """ @@ -558,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] @@ -627,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() @@ -644,7 +658,7 @@ 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_to_ROS = self.swarm.query_all_values_on_connect @@ -661,41 +675,42 @@ 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) + print(type_cf_param_to_index[type_cf_param]) + 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}" + 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=set_param_value, + value=param_value, descriptor=parameter_descriptor, ) @@ -705,9 +720,9 @@ def _init_parameters(self): # 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: + if type_cf_param_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: + 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( @@ -719,8 +734,6 @@ def _init_parameters(self): self.get_logger().info("All Crazyflies parameters are initialized.") - - def _parameters_callback(self, params): """ Sets up all the parameters for the crazyflie and @@ -776,7 +789,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 """ @@ -803,7 +816,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) + \ @@ -827,7 +840,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) + \ @@ -966,7 +979,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 @@ -1023,7 +1036,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 From 577fcd83064013ff3dd7609974f62a657a7a7bf3 Mon Sep 17 00:00:00 2001 From: "Kimberly N. McGuire" Date: Thu, 12 Sep 2024 20:54:56 +0200 Subject: [PATCH 5/5] remove print --- crazyflie/scripts/crazyflie_server.py | 1 - 1 file changed, 1 deletion(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index dbd4f6d3b..34b9bb505 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -701,7 +701,6 @@ def _init_parameters(self): # 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. - print(type_cf_param_to_index[type_cf_param]) 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 {param_value}"