From 3b1a2c2bda3058b20a899bb4033f799a79eb9ef3 Mon Sep 17 00:00:00 2001 From: qrthom Date: Sat, 6 Jul 2024 16:41:47 -0400 Subject: [PATCH] Fix some formatting issue --- crazyflie/scripts/crazyflie_server.py | 156 ++++++++------------------ 1 file changed, 48 insertions(+), 108 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index f758dbe3f..a11450500 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -62,7 +62,6 @@ "double": ParameterType.PARAMETER_DOUBLE, } - class CrazyflieServer(Node): def __init__(self): super().__init__( @@ -79,44 +78,21 @@ def __init__(self): self.cf_dict = {"all": "all"} self.uri_dict = {} self.type_dict = {} - + # Assign default topic types, variables and callbacks - self.default_log_type = { - "pose": PoseStamped, - "scan": LaserScan, - "odom": Odometry, - } - self.default_log_vars = { - "pose": [ - "stateEstimate.x", - "stateEstimate.y", - "stateEstimate.z", - "stabilizer.roll", - "stabilizer.pitch", - "stabilizer.yaw", - ], - "scan": ["range.front", "range.left", "range.back", "range.right"], - "odom": [ - "stateEstimate.x", - "stateEstimate.y", - "stateEstimate.z", - "stabilizer.yaw", - "stabilizer.roll", - "stabilizer.pitch", - "kalman.statePX", - "kalman.statePY", - "kalman.statePZ", - "gyro.z", - "gyro.x", - "gyro.y", - ], - "status": ["supervisor.info", "pm.vbatMV", "pm.state", "radio.rssi"], - } - self.default_log_fnc = { - "pose": self._log_pose_data_callback, - "scan": self._log_scan_data_callback, - "odom": self._log_odom_data_callback, - } + self.default_log_type = {"pose": PoseStamped, + "scan": LaserScan, + "odom": Odometry} + self.default_log_vars = {"pose": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z', + 'stabilizer.roll', 'stabilizer.pitch', 'stabilizer.yaw'], + "scan": ['range.front', 'range.left', 'range.back', 'range.right'], + "odom": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z', + 'stabilizer.yaw', 'stabilizer.roll', 'stabilizer.pitch', + 'kalman.statePX', 'kalman.statePY', 'kalman.statePZ', + 'gyro.z', 'gyro.x', 'gyro.y']} + self.default_log_fnc = {"pose": self._log_pose_data_callback, + "scan": self._log_scan_data_callback, + "odom": self._log_odom_data_callback} self.world_tf_name = "world" try: @@ -133,9 +109,7 @@ def __init__(self): if robot_data[crazyflie]["enabled"]: type_cf = robot_data[crazyflie]["type"] # do not include virtual objects - connection = self._ros_parameters["robot_types"][type_cf].get( - "connection", "crazyflie" - ) + connection = self._ros_parameters["robot_types"][type_cf].get("connection", "crazyflie") if connection == "crazyflie": uri = robot_data[crazyflie]["uri"] self.uris.append(uri) @@ -147,6 +121,7 @@ def __init__(self): factory = CachedCfFactory(rw_cache="./cache") self.swarm = Swarm(self.uris, factory=factory) self.swarm.fully_connected_crazyflie_cnt = 0 + # Initialize logging, services and parameters for each crazyflie for link_uri in self.uris: @@ -154,7 +129,8 @@ def __init__(self): self.swarm._cfs[link_uri].cf.fully_connected.add_callback( self._fully_connected ) - self.swarm._cfs[link_uri].cf.disconnected.add_callback(self._disconnected) + self.swarm._cfs[link_uri].cf.disconnected.add_callback( + self._disconnected) self.swarm._cfs[link_uri].cf.connection_failed.add_callback( self._connection_failed ) @@ -167,21 +143,15 @@ def __init__(self): # check if logging is enabled at startup logging_enabled = False try: - logging_enabled = self._ros_parameters["all"]["firmware_logging"][ - "enabled" - ] + logging_enabled = self._ros_parameters["all"]["firmware_logging"]["enabled"] except KeyError: pass try: - logging_enabled = self._ros_parameters["robot_types"][cf_type][ - "firmware_logging" - ]["enabled"] + logging_enabled = self._ros_parameters["robot_types"][cf_type]["firmware_logging"]["enabled"] except KeyError: pass try: - logging_enabled = self._ros_parameters["robots"][cf_name][ - "firmware_logging" - ]["enabled"] + logging_enabled = self._ros_parameters["robots"][cf_name]["firmware_logging"]["enabled"] except KeyError: pass @@ -192,36 +162,26 @@ def __init__(self): prefix = default_log_name topic_type = self.default_log_type[default_log_name] list_logvar = self.default_log_vars[default_log_name] - self._init_default_logblocks( - prefix, link_uri, list_logvar, logging_enabled, topic_type - ) + self._init_default_logblocks(prefix, link_uri, list_logvar, logging_enabled, topic_type) # Check for any custom_log topics custom_logging_enabled = False custom_log_topics = {} try: - custom_log_topics = self._ros_parameters["all"]["firmware_logging"][ - "custom_topics" - ] + custom_log_topics = self._ros_parameters['all']["firmware_logging"]["custom_topics"] custom_logging_enabled = True except KeyError: pass try: custom_log_topics.update( - self._ros_parameters["robot_types"][cf_type]["firmware_logging"][ - "custom_topics" - ] - ) + self._ros_parameters['robot_types'][cf_type]["firmware_logging"]["custom_topics"]) custom_logging_enabled = True except KeyError: pass try: custom_log_topics.update( - self._ros_parameters["robots"][cf_name]["firmware_logging"][ - "custom_topics" - ] - ) + self._ros_parameters['robots'][cf_name]["firmware_logging"]["custom_topics"]) custom_logging_enabled = True except KeyError: pass @@ -235,26 +195,17 @@ def __init__(self): for log_group_name in custom_log_topics: frequency = custom_log_topics[log_group_name]["frequency"] lg_custom = LogConfig( - name=log_group_name, period_in_ms=1000 / frequency - ) + name=log_group_name, period_in_ms=1000 / frequency) for log_name in custom_log_topics[log_group_name]["vars"]: lg_custom.add_variable(log_name) # Don't know which type this needs to be in until we get the full toc - self.swarm._cfs[link_uri].logging["custom_log_publisher"][ - log_group_name - ] = "empty publisher" - self.swarm._cfs[link_uri].logging["custom_log_groups"][ - log_group_name - ] = {} - self.swarm._cfs[link_uri].logging["custom_log_groups"][ - log_group_name - ]["log_config"] = lg_custom - self.swarm._cfs[link_uri].logging["custom_log_groups"][ - log_group_name - ]["vars"] = custom_log_topics[log_group_name]["vars"] - self.swarm._cfs[link_uri].logging["custom_log_groups"][ - log_group_name - ]["frequency"] = custom_log_topics[log_group_name]["frequency"] + self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = "empty publisher" + self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name] = { + } + self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name]["log_config"] = lg_custom + self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name]["vars"] = custom_log_topics[log_group_name]["vars"] + self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name][ + "frequency"] = custom_log_topics[log_group_name]["frequency"] # Now all crazyflies are initialized, open links! try: @@ -262,10 +213,8 @@ def __init__(self): except Exception as e: # Close node if one of the Crazyflies can not be found self.get_logger().info("Error!: One or more Crazyflies can not be found. ") - self.get_logger().info( - "Check if you got the right URIs, if they are turned on" - + " or if your script have proper access to a Crazyradio PA" - ) + self.get_logger().info("Check if you got the right URIs, if they are turned on" + + " or if your script have proper access to a Crazyradio PA") exit() for uri in self.cf_dict: @@ -347,9 +296,7 @@ def __init__(self): # 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 - ): + def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type): """ Prepare default logblocks as defined in crazyflies.yaml """ @@ -359,28 +306,26 @@ def _init_default_logblocks( logging_enabled = False logging_freq = 10 try: - logging_freq = self._ros_parameters["all"]["firmware_logging"][ - "default_topics" - ][prefix]["frequency"] + logging_freq = self._ros_parameters['all'][ + "firmware_logging"]["default_topics"][prefix]["frequency"] logging_enabled = True except KeyError: pass try: - logging_freq = self._ros_parameters["robot_types"][cf_type][ - "firmware_logging" - ]["default_topics"][prefix]["frequency"] + logging_freq = self._ros_parameters['robot_types'][cf_type][ + "firmware_logging"]["default_topics"][prefix]["frequency"] logging_enabled = True except KeyError: pass try: - logging_freq = self._ros_parameters["robots"][cf_name]["firmware_logging"][ - "default_topics" - ][prefix]["frequency"] + logging_freq = self._ros_parameters['robots'][cf_name][ + "firmware_logging"]["default_topics"][prefix]["frequency"] logging_enabled = True except KeyError: pass - lg = LogConfig(name=prefix, period_in_ms=1000 / logging_freq) + lg = LogConfig( + name=prefix, period_in_ms=1000 / logging_freq) for logvar in list_logvar: if prefix == "odom": lg.add_variable(logvar, "FP16") @@ -391,11 +336,8 @@ def _init_default_logblocks( self.swarm._cfs[link_uri].logging[prefix + "_logging_freq"] = logging_freq self.swarm._cfs[link_uri].logging[prefix + "_log_config"] = lg if logging_enabled and global_logging_enabled: - self.swarm._cfs[link_uri].logging[prefix + "_publisher"] = ( - self.create_publisher( - topic_type, self.cf_dict[link_uri] + "/" + prefix, 10 - ) - ) + self.swarm._cfs[link_uri].logging[prefix + "_publisher"] = self.create_publisher( + topic_type, self.cf_dict[link_uri] + "/" + prefix, 10) else: self.swarm._cfs[link_uri].logging[prefix + "_publisher"] = "empty" @@ -406,8 +348,8 @@ def _param_to_dict(self, param_ros): tree = {} for item in param_ros: t = tree - for part in item.split("."): - if part == item.split(".")[-1]: + for part in item.split('.'): + if part == item.split('.')[-1]: t = t.setdefault(part, param_ros[item].value) else: t = t.setdefault(part, {}) @@ -843,8 +785,6 @@ def _takeoff_callback(self, request, response, uri="all"): a certain height in high level commander """ - print("call1 ", uri) - duration = float(request.duration.sec) + float(request.duration.nanosec / 1e9) self.get_logger().info( f"[{self.cf_dict[uri]}] takeoff(height={request.height} m,"