Skip to content

Commit

Permalink
Fix some formatting issue
Browse files Browse the repository at this point in the history
  • Loading branch information
@qrthom
qrthom committed Jul 6, 2024
1 parent e55afd7 commit 3b1a2c2
Showing 1 changed file with 48 additions and 108 deletions.
156 changes: 48 additions & 108 deletions crazyflie/scripts/crazyflie_server.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,6 @@
"double": ParameterType.PARAMETER_DOUBLE,
}


class CrazyflieServer(Node):
def __init__(self):
super().__init__(
Expand All @@ -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:
Expand All @@ -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)
Expand All @@ -147,14 +121,16 @@ 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:

# Connect callbacks for different connection states of the crazyflie
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
)
Expand All @@ -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

Expand All @@ -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
Expand All @@ -235,37 +195,26 @@ 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:
self.swarm.open_links()
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:
Expand Down Expand Up @@ -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
"""
Expand All @@ -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")
Expand All @@ -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"

Expand All @@ -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, {})
Expand Down Expand Up @@ -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,"
Expand Down

0 comments on commit 3b1a2c2

Please sign in to comment.