Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Seperate parameter handling with connection status #564

Merged
merged 5 commits into from
Sep 12, 2024
Merged

Seperate parameter handling with connection status #564

Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
eca59bc
cflib server add parameter to ros setting only if indicated
knmcguire Sep 11, 2024
df3deb1
wait on fully connected only when full toc value list is wanted
knmcguire Sep 11, 2024
e076b2e
update text
knmcguire Sep 11, 2024
a263dfa
send_param_raw and reformat
knmcguire Sep 12, 2024
577fcd8
remove print
knmcguire Sep 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
152 changes: 92 additions & 60 deletions crazyflie/scripts/crazyflie_server.py
View file
Open in desktop
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,62 @@ 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])
knmcguire marked this conversation as resolved.
Show resolved Hide resolved
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 +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
"""

Expand All @@ -784,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) + \
Expand All @@ -808,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) + \
Expand Down Expand Up @@ -947,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
Expand Down Expand Up @@ -1004,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
Expand Down
Loading