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