Skip to content

Commit

Permalink
Merge pull request #564 from IMRCLab/seperate_connection_status
Browse files Browse the repository at this point in the history 
Seperate parameter handling with connection status
  • Loading branch information
@knmcguire
knmcguire authored Sep 12, 2024
2 parents 8a22066 + 577fcd8 commit a872237
Showing 1 changed file with 91 additions and 60 deletions.
151 changes: 91 additions & 60 deletions crazyflie/scripts/crazyflie_server.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand All @@ -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):
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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
"""

Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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()
Expand All @@ -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

Expand All @@ -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.")

Expand Down Expand Up @@ -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
"""

Expand All @@ -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) + \
Expand All @@ -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) + \
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit a872237

Please sign in to comment.